Revision control
++ // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
++ // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
++ // SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
++ // SPDX-License-Identifier: Apache-2.0
++
++ #ifndef DY_FEATHERSTONE_ARTICULATION_UTIL_H
++ #define DY_FEATHERSTONE_ARTICULATION_UTIL_H
++
++ #include "foundation/PxVecMath.h"
++ #include "CmSpatialVector.h"
++ #include "foundation/PxBitUtils.h"
++ #include "foundation/PxMemory.h"
++
++ #if PX_CUDA_COMPILER
++ #define PX_SPATIAL_MATRIX_SIMD 0
++ #else
++ #define PX_SPATIAL_MATRIX_SIMD 1
++ #endif
++
++ namespace physx
++ {
++
++ namespace Dy
++ {
++ static const size_t DY_MAX_DOF = 6;
++
++ #if PX_SUPPORT_GPU_PHYSX
++ struct SpatialSubspaceMatrix
++ {
++ static const PxU32 MaxColumns = 3;
++ public:
++
++ #if !PX_CUDA_COMPILER
++ PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialSubspaceMatrix() :numColumns(0)
++ {
++ //PxMemZero(columns, sizeof(Cm::SpatialVectorF) * 6);
++ PxMemSet(columns, 0, sizeof(Cm::UnAlignedSpatialVector) * MaxColumns);
++ }
++ #endif
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void setNumColumns(const PxU32 nc)
++ {
++ numColumns = nc;
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 getNumColumns() const
++ {
++ return numColumns;
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF transposeMultiply(Cm::SpatialVectorF& v) const
++ {
++ PxReal result[6];
++ for (PxU32 i = 0; i < numColumns; ++i)
++ {
++ const Cm::UnAlignedSpatialVector& row = columns[i];
++ result[i] = row.dot(v);
++ }
++
++ Cm::SpatialVectorF res;
++ res.top.x = result[0]; res.top.y = result[1]; res.top.z = result[2];
++ res.bottom.x = result[3]; res.bottom.y = result[4]; res.bottom.z = result[5];
++
++ return res;
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void setColumn(const PxU32 index, const PxVec3& top, const PxVec3& bottom)
++ {
++ PX_ASSERT(index < MaxColumns);
++ columns[index] = Cm::SpatialVectorF(top, bottom);
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector& operator[](unsigned int num)
++ {
++ PX_ASSERT(num < MaxColumns);
++ return columns[num];
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& operator[](unsigned int num) const
++ {
++ PX_ASSERT(num < MaxColumns);
++ return columns[num];
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE const Cm::UnAlignedSpatialVector* getColumns() const
++ {
++ return columns;
++ }
++
++ //private:
++ Cm::UnAlignedSpatialVector columns[MaxColumns]; //3x24 = 72
++ PxU32 numColumns; //76
++ PxU32 padding; //80
++ };
++ #endif
++
++ struct InvStIs
++ {
++ PxReal invStIs[3][3];
++ };
++
++ //this should be 6x6 matrix and initialize to
++ //|0, M|
++ //|I, 0|
++ //this should be 6x6 matrix but bottomRight is the transpose of topLeft
++ //so we can get rid of bottomRight
++ struct SpatialMatrix
++ {
++ PxMat33 topLeft; // intialize to 0
++ PxMat33 topRight; // initialize to mass matrix
++ PxMat33 bottomLeft; // initialize to inertia
++ PxU32 padding; //4 112
++
++ public:
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix()
++ {
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix(PxZERO) : topLeft(PxZero), topRight(PxZero),
++ bottomLeft(PxZero)
++ {
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix(const PxMat33& topLeft_, const PxMat33& topRight_, const PxMat33& bottomLeft_)
++ {
++ topLeft = topLeft_;
++ topRight = topRight_;
++ bottomLeft = bottomLeft_;
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 getBottomRight() const
++ {
++ return topLeft.getTranspose();
++ }
++
++ PX_FORCE_INLINE PX_CUDA_CALLABLE void setZero()
++ {
++ topLeft = PxMat33(0.f);
++ topRight = PxMat33(0.f);
++ bottomLeft = PxMat33(0.f);
++ }
++
++ //This assume angular is the top vector and linear is the bottom vector
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVector operator *(const Cm::SpatialVector& s) const
++ {
++ const PxVec3 angular = topLeft * s.angular + topRight * s.linear;
++ const PxVec3 linear = bottomLeft * s.angular + topLeft.transformTranspose(s.linear);
++ return Cm::SpatialVector(linear, angular);
++ }
++
++ //This assume angular is the top vector and linear is the bottom vector
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF operator *(const Cm::SpatialVectorF& s) const
++ {
++ const PxVec3 top = topLeft * s.top + topRight * s.bottom;
++ const PxVec3 bottom = bottomLeft * s.top + topLeft.transformTranspose(s.bottom);
++
++ return Cm::SpatialVectorF(top, bottom);
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector operator *(const Cm::UnAlignedSpatialVector& s) const
++ {
++ const PxVec3 top = topLeft * s.top + topRight * s.bottom;
++ const PxVec3 bottom = bottomLeft * s.top + topLeft.transformTranspose(s.bottom);
++
++ return Cm::UnAlignedSpatialVector(top, bottom);
++ }
++
++ // PT: this is unused
++ /*PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator *(const PxReal& s) const
++ {
++ const PxMat33 newTopLeft = topLeft * s;
++ const PxMat33 newTopRight = topRight * s;
++ const PxMat33 newBottomLeft = bottomLeft * s;
++
++ return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
++ }*/
++
++ static PX_FORCE_INLINE void sub(SpatialMatrix& dst, const SpatialMatrix& a, const SpatialMatrix& b)
++ {
++ #if PX_SPATIAL_MATRIX_SIMD
++ using namespace aos;
++
++ const float* src0 = &a.topLeft.column0.x;
++ const float* src1 = &b.topLeft.column0.x;
++ float* d = &dst.topLeft.column0.x;
++
++ // PT: 3*3*3 floats = 27 floats of data + 1 float of padding = 28 floats to copy = 7 SIMD copies
++ V4StoreU(V4Sub(V4LoadU(src0+4*0), V4LoadU(src1+4*0)), d+4*0);
++ V4StoreU(V4Sub(V4LoadU(src0+4*1), V4LoadU(src1+4*1)), d+4*1);
++ V4StoreU(V4Sub(V4LoadU(src0+4*2), V4LoadU(src1+4*2)), d+4*2);
++ V4StoreU(V4Sub(V4LoadU(src0+4*3), V4LoadU(src1+4*3)), d+4*3);
++ V4StoreU(V4Sub(V4LoadU(src0+4*4), V4LoadU(src1+4*4)), d+4*4);
++ V4StoreU(V4Sub(V4LoadU(src0+4*5), V4LoadU(src1+4*5)), d+4*5);
++ V4StoreU(V4Sub(V4LoadU(src0+4*6), V4LoadU(src1+4*6)), d+4*6);
++ #else
++ dst.topLeft = a.topLeft - b.topLeft;
++ dst.topRight = a.topRight - b.topRight;
++ dst.bottomLeft = a.bottomLeft - b.bottomLeft;
++ #endif
++ }
++
++ // PT: this is unused on the CPU
++ #if PX_CUDA_COMPILER
++ PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator -(const SpatialMatrix& s) const
++ {
++ const PxMat33 newTopLeft = topLeft - s.topLeft;
++ const PxMat33 newTopRight = topRight - s.topRight;
++ const PxMat33 newBottomLeft = bottomLeft - s.bottomLeft;
++
++ return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator +(const SpatialMatrix& s) const
++ {
++ const PxMat33 newTopLeft = topLeft + s.topLeft;
++ const PxMat33 newTopRight = topRight + s.topRight;
++ const PxMat33 newBottomLeft = bottomLeft + s.bottomLeft;
++
++ return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
++ }
++ #endif
++
++ // PT: this is unused
++ /*PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator-()
++ {
++ const PxMat33 newTopLeft = -topLeft;
++ const PxMat33 newTopRight = -topRight;
++ const PxMat33 newBottomLeft = -bottomLeft;
++
++ return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
++ }*/
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void operator +=(const SpatialMatrix& s)
++ {
++ #if PX_SPATIAL_MATRIX_SIMD
++ using namespace aos;
++
++ const float* src1 = &s.topLeft.column0.x;
++ float* d = &topLeft.column0.x;
++
++ // PT: 3*3*3 floats = 27 floats of data + 1 float of padding = 28 floats to copy = 7 SIMD copies
++ V4StoreU(V4Add(V4LoadU(d+4*0), V4LoadU(src1+4*0)), d+4*0);
++ V4StoreU(V4Add(V4LoadU(d+4*1), V4LoadU(src1+4*1)), d+4*1);
++ V4StoreU(V4Add(V4LoadU(d+4*2), V4LoadU(src1+4*2)), d+4*2);
++ V4StoreU(V4Add(V4LoadU(d+4*3), V4LoadU(src1+4*3)), d+4*3);
++ V4StoreU(V4Add(V4LoadU(d+4*4), V4LoadU(src1+4*4)), d+4*4);
++ V4StoreU(V4Add(V4LoadU(d+4*5), V4LoadU(src1+4*5)), d+4*5);
++ V4StoreU(V4Add(V4LoadU(d+4*6), V4LoadU(src1+4*6)), d+4*6);
++ #else
++ topLeft += s.topLeft;
++ topRight += s.topRight;
++ bottomLeft += s.bottomLeft;
++ #endif
++ }
++
++ // PT: this is unused
++ /* PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator *(const SpatialMatrix& s)
++ {
++ const PxMat33 sBottomRight = s.topLeft.getTranspose();
++ const PxMat33 bottomRight = topLeft.getTranspose();
++
++ const PxMat33 newTopLeft = topLeft * s.topLeft + topRight * s.bottomLeft;
++ const PxMat33 newTopRight = topLeft * s.topRight + topRight * sBottomRight;
++ const PxMat33 newBottomLeft = bottomLeft * s.topLeft + bottomRight * s.bottomLeft;
++
++ return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
++ }*/
++
++ #if PX_CUDA_COMPILER
++ static SpatialMatrix constructSpatialMatrix(const Cm::SpatialVector& Is, const Cm::SpatialVector& stI)
++ {
++ //construct top left
++ const PxVec3 tLeftC0 = Is.angular * stI.angular.x;
++ const PxVec3 tLeftC1 = Is.angular * stI.angular.y;
++ const PxVec3 tLeftC2 = Is.angular * stI.angular.z;
++ const PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);
++
++ //construct top right
++ const PxVec3 tRightC0 = Is.angular * stI.linear.x;
++ const PxVec3 tRightC1 = Is.angular * stI.linear.y;
++ const PxVec3 tRightC2 = Is.angular * stI.linear.z;
++ const PxMat33 topRight(tRightC0, tRightC1, tRightC2);
++
++ //construct bottom left
++ const PxVec3 bLeftC0 = Is.linear * stI.angular.x;
++ const PxVec3 bLeftC1 = Is.linear * stI.angular.y;
++ const PxVec3 bLeftC2 = Is.linear * stI.angular.z;
++ const PxMat33 bottomLeft(bLeftC0, bLeftC1, bLeftC2);
++
++ return SpatialMatrix(topLeft, topRight, bottomLeft);
++ }
++
++ static PX_CUDA_CALLABLE SpatialMatrix constructSpatialMatrix(const Cm::SpatialVectorF& Is, const Cm::SpatialVectorF& stI)
++ {
++ //construct top left
++ const PxVec3 tLeftC0 = Is.top * stI.top.x;
++ const PxVec3 tLeftC1 = Is.top * stI.top.y;
++ const PxVec3 tLeftC2 = Is.top * stI.top.z;
++ const PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);
++
++ //construct top right
++ const PxVec3 tRightC0 = Is.top * stI.bottom.x;
++ const PxVec3 tRightC1 = Is.top * stI.bottom.y;
++ const PxVec3 tRightC2 = Is.top * stI.bottom.z;
++ const PxMat33 topRight(tRightC0, tRightC1, tRightC2);
++
++ //construct bottom left
++ const PxVec3 bLeftC0 = Is.bottom * stI.top.x;
++ const PxVec3 bLeftC1 = Is.bottom * stI.top.y;
++ const PxVec3 bLeftC2 = Is.bottom * stI.top.z;
++ const PxMat33 bottomLeft(bLeftC0, bLeftC1, bLeftC2);
++
++ return SpatialMatrix(topLeft, topRight, bottomLeft);
++ }
++ #else
++ static PX_CUDA_CALLABLE void constructSpatialMatrix(SpatialMatrix& dst, const Cm::SpatialVectorF& Is, const Cm::SpatialVectorF& stI)
++ {
++ using namespace aos;
++
++ const Vec4V IsTopV = V4LoadA(&Is.top.x);
++ const Vec4V IsBottomV = V4LoadA(&Is.bottom.x);
++ const Vec4V stITopV = V4LoadA(&stI.top.x);
++ const Vec4V stIBottomV = V4LoadA(&stI.bottom.x);
++
++ //construct top left
++ const Vec4V tLeftC0V = V4Scale(IsTopV, V4GetX(stITopV));
++ const Vec4V tLeftC1V = V4Scale(IsTopV, V4GetY(stITopV));
++ const Vec4V tLeftC2V = V4Scale(IsTopV, V4GetZ(stITopV));
++ V4StoreU(tLeftC0V, &dst.topLeft.column0.x);
++ V4StoreU(tLeftC1V, &dst.topLeft.column1.x);
++ V4StoreU(tLeftC2V, &dst.topLeft.column2.x);
++
++ //construct top right
++ const Vec4V tRightC0V = V4Scale(IsTopV, V4GetX(stIBottomV));
++ const Vec4V tRightC1V = V4Scale(IsTopV, V4GetY(stIBottomV));
++ const Vec4V tRightC2V = V4Scale(IsTopV, V4GetZ(stIBottomV));
++ V4StoreU(tRightC0V, &dst.topRight.column0.x);
++ V4StoreU(tRightC1V, &dst.topRight.column1.x);
++ V4StoreU(tRightC2V, &dst.topRight.column2.x);
++
++ //construct bottom left
++ const Vec4V bLeftC0V = V4Scale(IsBottomV, V4GetX(stITopV));
++ const Vec4V bLeftC1V = V4Scale(IsBottomV, V4GetY(stITopV));
++ const Vec4V bLeftC2V = V4Scale(IsBottomV, V4GetZ(stITopV));
++ V4StoreU(bLeftC0V, &dst.bottomLeft.column0.x);
++ V4StoreU(bLeftC1V, &dst.bottomLeft.column1.x);
++ V4StoreU(bLeftC2V, &dst.bottomLeft.column2.x);
++ }
++ #endif
++
++ #if PX_CUDA_COMPILER
++ template <typename SpatialVector>
++ static PX_CUDA_CALLABLE SpatialMatrix constructSpatialMatrix(const SpatialVector* columns)
++ {
++ const PxMat33 topLeft(columns[0].top, columns[1].top, columns[2].top);
++ const PxMat33 bottomLeft(columns[0].bottom, columns[1].bottom, columns[2].bottom);
++ const PxMat33 topRight(columns[3].top, columns[4].top, columns[5].top);
++
++ return SpatialMatrix(topLeft, topRight, bottomLeft);
++ }
++ #else
++ static PX_CUDA_CALLABLE void constructSpatialMatrix(SpatialMatrix& dst, const Cm::SpatialVectorF* columns)
++ {
++ //dst.topLeft = PxMat33(columns[0].top, columns[1].top, columns[2].top);
++ //dst.bottomLeft = PxMat33(columns[0].bottom, columns[1].bottom, columns[2].bottom);
++ //dst.topRight = PxMat33(columns[3].top, columns[4].top, columns[5].top);
++
++ using namespace aos;
++
++ const Vec4V column0TopV = V4LoadA(&columns[0].top.x);
++ const Vec4V column1TopV = V4LoadA(&columns[1].top.x);
++ const Vec4V column2TopV = V4LoadA(&columns[2].top.x);
++ V4StoreU(column0TopV, &dst.topLeft.column0.x);
++ V4StoreU(column1TopV, &dst.topLeft.column1.x);
++ V4StoreU(column2TopV, &dst.topLeft.column2.x);
++
++ const Vec4V column3TopV = V4LoadA(&columns[3].top.x);
++ const Vec4V column4TopV = V4LoadA(&columns[4].top.x);
++ const Vec4V column5TopV = V4LoadA(&columns[5].top.x);
++ V4StoreU(column3TopV, &dst.topRight.column0.x);
++ V4StoreU(column4TopV, &dst.topRight.column1.x);
++ V4StoreU(column5TopV, &dst.topRight.column2.x);
++
++ const Vec4V column0BottomV = V4LoadA(&columns[0].bottom.x);
++ const Vec4V column1BottomV = V4LoadA(&columns[1].bottom.x);
++ const Vec4V column2BottomV = V4LoadA(&columns[2].bottom.x);
++ V4StoreU(column0BottomV, &dst.bottomLeft.column0.x);
++ V4StoreU(column1BottomV, &dst.bottomLeft.column1.x);
++ V4StoreU(column2BottomV, &dst.bottomLeft.column2.x);
++ // PT: TODO: if the bottom 3/4/5 data aren't used, why did we compute them in the calling code?
++ }
++ #endif
++
++ // PT: this is unused
++ /*PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix getTranspose()
++ {
++ const PxMat33 newTopLeft = topLeft.getTranspose();
++ const PxMat33 newTopRight = bottomLeft.getTranspose();
++ const PxMat33 newBottomLeft = topRight.getTranspose();
++ //const PxMat33 newBottomRight = bottomRight.getTranspose();
++
++ return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);// , newBottomRight);
++ }*/
++
++ //static bool isTranspose(const PxMat33& a, const PxMat33& b)
++ //{
++ // PxReal eps = 0.01f;
++ // //test bottomRight is the transpose of topLeft
++ // for (PxU32 i = 0; i <3; ++i)
++ // {
++ // for (PxU32 j = 0; j <3; ++j)
++ // {
++ // if (PxAbs(a[i][j] - b[j][i]) > eps)
++ // return false;
++ // }
++ // }
++
++ // return true;
++ //}
++
++ PX_FORCE_INLINE bool isIdentity(const PxMat33& matrix)
++ {
++ const PxReal eps = 0.00001f;
++ const float x = PxAbs(1.f - matrix.column0.x);
++ const float y = PxAbs(1.f - matrix.column1.y);
++ const float z = PxAbs(1.f - matrix.column2.z);
++ const bool identity = ((x < eps) && PxAbs(matrix.column0.y - 0.f) < eps && PxAbs(matrix.column0.z - 0.f) < eps) &&
++ (PxAbs(matrix.column1.x - 0.f) < eps && (y < eps) && PxAbs(matrix.column1.z - 0.f) < eps) &&
++ (PxAbs(matrix.column2.x - 0.f) < eps && PxAbs(matrix.column2.y - 0.f) < eps && (z < eps));
++
++ return identity;
++ }
++
++ PX_FORCE_INLINE bool isZero(const PxMat33& matrix)
++ {
++ const PxReal eps = 0.0001f;
++ for (PxU32 i = 0; i < 3; ++i)
++ {
++ for (PxU32 j = 0; j < 3; ++j)
++ {
++ if (PxAbs(matrix[i][j]) > eps)
++ return false;
++ }
++ }
++
++ return true;
++ }
++
++ PX_FORCE_INLINE bool isIdentity()
++ {
++ const bool topLeftIsIdentity = isIdentity(topLeft);
++
++ const bool topRightIsZero = isZero(topRight);
++
++ const bool bottomLeftIsZero = isZero(bottomLeft);
++
++ return topLeftIsIdentity && topRightIsZero && bottomLeftIsZero;
++ }
++
++ static bool isEqual(const PxMat33& s0, const PxMat33& s1)
++ {
++ const PxReal eps = 0.00001f;
++ for (PxU32 i = 0; i < 3; ++i)
++ {
++ for (PxU32 j = 0; j < 3; ++j)
++ {
++ const PxReal t = s0[i][j] - s1[i][j];
++ if (PxAbs(t) > eps)
++ return false;
++ }
++ }
++
++ return true;
++ }
++
++ PX_FORCE_INLINE bool isEqual(const SpatialMatrix& s)
++ {
++ const bool topLeftEqual = isEqual(topLeft, s.topLeft);
++ const bool topRightEqual = isEqual(topRight, s.topRight);
++ const bool bottomLeftEqual = isEqual(bottomLeft, s.bottomLeft);
++
++ return topLeftEqual && topRightEqual && bottomLeftEqual;
++ }
++
++ static PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 invertSym33(const PxMat33& in)
++ {
++ const PxVec3 v0 = in[1].cross(in[2]);
++ const PxVec3 v1 = in[2].cross(in[0]);
++ const PxVec3 v2 = in[0].cross(in[1]);
++
++ const PxReal det = v0.dot(in[0]);
++
++ if (det != 0)
++ {
++ const PxReal recipDet = 1.0f / det;
++
++ return PxMat33(v0 * recipDet,
++ PxVec3(v0.y, v1.y, v1.z) * recipDet,
++ PxVec3(v0.z, v1.z, v2.z) * recipDet);
++ }
++ else
++ {
++ return PxMat33(PxIdentity);
++ }
++ }
++
++ static PX_FORCE_INLINE aos::Mat33V invertSym33(const aos::Mat33V& in)
++ {
++ using namespace aos;
++ const Vec3V v0 = V3Cross(in.col1, in.col2);
++ const Vec3V v1 = V3Cross(in.col2, in.col0);
++ const Vec3V v2 = V3Cross(in.col0, in.col1);
++
++ const FloatV det = V3Dot(v0, in.col0);
++
++ const FloatV recipDet = FRecip(det);
++
++ if (!FAllEq(det, FZero()))
++ {
++ return Mat33V(V3Scale(v0, recipDet),
++ V3Scale(V3Merge(V3GetY(v0), V3GetY(v1), V3GetZ(v1)), recipDet),
++ V3Scale(V3Merge(V3GetZ(v0), V3GetZ(v1), V3GetZ(v2)), recipDet));
++ }
++ else
++ {
++ return Mat33V(V3UnitX(), V3UnitY(), V3UnitZ());
++ }
++
++ //return M33Inverse(in);
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix invertInertia()
++ {
++ PxMat33 aa = bottomLeft, ll = topRight, la = topLeft;
++
++ aa = (aa + aa.getTranspose())*0.5f;
++ ll = (ll + ll.getTranspose())*0.5f;
++
++ const PxMat33 AAInv = invertSym33(aa);
++
++ const PxMat33 z = -la * AAInv;
++ const PxMat33 S = ll + z * la.getTranspose(); // Schur complement of mAA
++
++ const PxMat33 LL = invertSym33(S);
++
++ const PxMat33 LA = LL * z;
++ const PxMat33 AA = AAInv + z.getTranspose() * LA;
++
++ const SpatialMatrix result(LA.getTranspose(), AA, LL);// , LA);
++
++ return result;
++ }
++
++ PX_FORCE_INLINE void M33Store(const aos::Mat33V& src, PxMat33& dest)
++ {
++ aos::V3StoreU(src.col0, dest.column0);
++ aos::V3StoreU(src.col1, dest.column1);
++ aos::V3StoreU(src.col2, dest.column2);
++ }
++
++ PX_FORCE_INLINE void invertInertiaV(SpatialMatrix& result)
++ {
++ using namespace aos;
++ Mat33V aa = M33Load(bottomLeft), ll = M33Load(topRight), la = M33Load(topLeft);
++
++ aa = M33Scale(M33Add(aa, M33Trnsps(aa)), FHalf());
++ ll = M33Scale(M33Add(ll, M33Trnsps(ll)), FHalf());
++
++ const Mat33V AAInv = invertSym33(aa);
++
++ const Mat33V z = M33MulM33(M33Neg(la), AAInv);
++ const Mat33V S = M33Add(ll, M33MulM33(z, M33Trnsps(la))); // Schur complement of mAA
++
++ const Mat33V LL = invertSym33(S);
++
++ const Mat33V LA = M33MulM33(LL, z);
++ const Mat33V AA = M33Add(AAInv, M33MulM33(M33Trnsps(z), LA));
++
++ M33Store(M33Trnsps(LA), result.topLeft);
++ M33Store(AA, result.topRight);
++ M33Store(LL, result.bottomLeft);
++ }
++
++ SpatialMatrix getInverse() const
++ {
++ const PxMat33 bottomRight = topLeft.getTranspose();
++
++ const PxMat33 blInverse = bottomLeft.getInverse();
++ const PxMat33 lComp0 = blInverse * (-bottomRight);
++ const PxMat33 lComp1 = topLeft * lComp0 + topRight;
++
++ //This can be simplified
++ const PxMat33 newBottomLeft = lComp1.getInverse();
++ const PxMat33 newTopLeft = lComp0 * newBottomLeft;
++
++ const PxMat33 trInverse = topRight.getInverse();
++ const PxMat33 rComp0 = trInverse * (-topLeft);
++ const PxMat33 rComp1 = bottomLeft + bottomRight * rComp0;
++
++ const PxMat33 newTopRight = rComp1.getInverse();
++
++ return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
++ }
++
++ void zero()
++ {
++ topLeft = PxMat33(PxZero);
++ topRight = PxMat33(PxZero);
++ bottomLeft = PxMat33(PxZero);
++ }
++ };
++
++ #if PX_SPATIAL_MATRIX_SIMD
++ // PT: some SIMD code above relies on this size
++ PX_COMPILE_TIME_ASSERT(sizeof(SpatialMatrix) == 7 * 16);
++ #endif
++
++ struct TestImpulseResponse
++ {
++ //Link deltaV responses to 6 test link impulses { [(1,0,0),(0,0,0)], [(0,1,0),(0,0,0)] ......[(0,0,0),(0,1,0)], [(0,0,0),(0,0,1)] }
++ Cm::SpatialVectorF linkDeltaVTestImpulseResponses[6];
++
++ Cm::SpatialVectorF getLinkDeltaVImpulseResponse(const Cm::SpatialVectorF& impulse) const
++ {
++ /*return rows[0] * impulse.top.x + rows[1] * impulse.top.y + rows[2] * impulse.top.z
++ + rows[3] * impulse.bottom.x + rows[4] * impulse.bottom.y + rows[5] * impulse.bottom.z;*/
++
++ using namespace aos;
++ const Cm::SpatialVectorV row0(V3LoadA(&linkDeltaVTestImpulseResponses[0].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[0].bottom.x));
++ const Cm::SpatialVectorV row1(V3LoadA(&linkDeltaVTestImpulseResponses[1].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[1].bottom.x));
++ const Cm::SpatialVectorV row2(V3LoadA(&linkDeltaVTestImpulseResponses[2].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[2].bottom.x));
++ const Cm::SpatialVectorV row3(V3LoadA(&linkDeltaVTestImpulseResponses[3].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[3].bottom.x));
++ const Cm::SpatialVectorV row4(V3LoadA(&linkDeltaVTestImpulseResponses[4].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[4].bottom.x));
++ const Cm::SpatialVectorV row5(V3LoadA(&linkDeltaVTestImpulseResponses[5].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[5].bottom.x));
++
++ const Vec4V top = V4LoadA(&impulse.top.x);
++ const Vec4V bottom = V4LoadA(&impulse.bottom.x);
++
++ const FloatV ix = V4GetX(top);
++ const FloatV iy = V4GetY(top);
++ const FloatV iz = V4GetZ(top);
++ const FloatV ia = V4GetX(bottom);
++ const FloatV ib = V4GetY(bottom);
++ const FloatV ic = V4GetZ(bottom);
++
++ Cm::SpatialVectorV res = row0 * ix + row1 * iy + row2 * iz + row3 * ia + row4 * ib + row5 * ic;
++
++ Cm::SpatialVectorF returnVal;
++ V4StoreA(Vec4V_From_Vec3V(res.linear), &returnVal.top.x);
++ V4StoreA(Vec4V_From_Vec3V(res.angular), &returnVal.bottom.x);
++
++ return returnVal;
++ }
++
++ Cm::SpatialVectorV getLinkDeltaVImpulseResponse(const Cm::SpatialVectorV& impulse) const
++ {
++ using namespace aos;
++ const Cm::SpatialVectorV row0(V3LoadA(&linkDeltaVTestImpulseResponses[0].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[0].bottom.x));
++ const Cm::SpatialVectorV row1(V3LoadA(&linkDeltaVTestImpulseResponses[1].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[1].bottom.x));
++ const Cm::SpatialVectorV row2(V3LoadA(&linkDeltaVTestImpulseResponses[2].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[2].bottom.x));
++ const Cm::SpatialVectorV row3(V3LoadA(&linkDeltaVTestImpulseResponses[3].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[3].bottom.x));
++ const Cm::SpatialVectorV row4(V3LoadA(&linkDeltaVTestImpulseResponses[4].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[4].bottom.x));
++ const Cm::SpatialVectorV row5(V3LoadA(&linkDeltaVTestImpulseResponses[5].top.x), V3LoadA(&linkDeltaVTestImpulseResponses[5].bottom.x));
++
++ const Vec3V top = impulse.linear;
++ const Vec3V bottom = impulse.angular;
++
++ const FloatV ix = V3GetX(top);
++ const FloatV iy = V3GetY(top);
++ const FloatV iz = V3GetZ(top);
++ const FloatV ia = V3GetX(bottom);
++ const FloatV ib = V3GetY(bottom);
++ const FloatV ic = V3GetZ(bottom);
++
++ Cm::SpatialVectorV res = row0 * ix + row1 * iy + row2 * iz + row3 * ia + row4 * ib + row5 * ic;
++ return res;
++ }
++ };
++
++ struct Temp6x6Matrix;
++
++ struct Temp6x3Matrix
++ {
++ PxReal column[3][6];
++ public:
++
++ Temp6x3Matrix()
++ {
++ }
++
++ Temp6x3Matrix(const Cm::SpatialVectorF* spatialAxis)
++ {
++ constructColumn(column[0], spatialAxis[0]);
++ constructColumn(column[1], spatialAxis[1]);
++ constructColumn(column[2], spatialAxis[2]);
++ }
++
++ void constructColumn(PxReal* dest, const Cm::SpatialVectorF& v)
++ {
++ dest[0] = v.top.x;
++ dest[1] = v.top.y;
++ dest[2] = v.top.z;
++
++ dest[3] = v.bottom.x;
++ dest[4] = v.bottom.y;
++ dest[5] = v.bottom.z;
++ }
++
++ Temp6x6Matrix operator * (PxReal s[6][3]);
++
++ ////s is 3x6 matrix
++ //PX_FORCE_INLINE Temp6x6Matrix operator * (PxReal s[6][3])
++ //{
++ // Temp6x6Matrix temp;
++
++ // for (PxU32 i = 0; i < 6; ++i)
++ // {
++ // PxReal* tc = temp.column[i];
++
++ // for (PxU32 j = 0; j < 6; ++j)
++ // {
++ // tc[j] = 0.f;
++ // for (PxU32 k = 0; k < 3; ++k)
++ // {
++ // tc[j] += column[k][j] * s[i][k];
++ // }
++ // }
++ // }
++
++ // return temp;
++ //}
++
++ PX_FORCE_INLINE Temp6x3Matrix operator * (const PxMat33& s)
++ {
++ Temp6x3Matrix temp;
++
++ for (PxU32 i = 0; i < 3; ++i)
++ {
++ PxReal* tc = temp.column[i];
++ const PxVec3 sc = s[i];
++
++ for (PxU32 j = 0; j < 6; ++j)
++ {
++ tc[j] = 0.f;
++ for (PxU32 k = 0; k < 3; ++k)
++ {
++ tc[j] += column[k][j] * sc[k];
++ }
++ }
++ }
++
++ return temp;
++ }
++
++ PX_FORCE_INLINE bool isColumnEqual(const PxU32 ind, const Cm::SpatialVectorF& col)
++ {
++ PxReal temp[6];
++ constructColumn(temp, col);
++ const PxReal eps = 0.00001f;
++ for (PxU32 i = 0; i < 6; ++i)
++ {
++ const PxReal dif = column[ind][i] - temp[i];
++ if (PxAbs(dif) > eps)
++ return false;
++ }
++ return true;
++ }
++ };
++
++ struct Temp6x6Matrix
++ {
++ PxReal column[6][6];
++ public:
++ Temp6x6Matrix()
++ {
++ }
++
++ Temp6x6Matrix(const SpatialMatrix& spatialMatrix)
++ {
++ constructColumn(column[0], spatialMatrix.topLeft.column0, spatialMatrix.bottomLeft.column0);
++ constructColumn(column[1], spatialMatrix.topLeft.column1, spatialMatrix.bottomLeft.column1);
++ constructColumn(column[2], spatialMatrix.topLeft.column2, spatialMatrix.bottomLeft.column2);
++
++ const PxMat33 bottomRight = spatialMatrix.getBottomRight();
++ constructColumn(column[3], spatialMatrix.topRight.column0, bottomRight.column0);
++ constructColumn(column[4], spatialMatrix.topRight.column1, bottomRight.column1);
++ constructColumn(column[5], spatialMatrix.topRight.column2, bottomRight.column2);
++ }
++
++ void constructColumn(const PxU32 ind, const PxReal* const values)
++ {
++ for (PxU32 i = 0; i < 6; ++i)
++ {
++ column[ind][i] = values[i];
++ }
++ }
++
++ void constructColumn(PxReal* dest, const PxVec3& top, const PxVec3& bottom)
++ {
++ dest[0] = top.x;
++ dest[1] = top.y;
++ dest[2] = top.z;
++
++ dest[3] = bottom.x;
++ dest[4] = bottom.y;
++ dest[5] = bottom.z;
++ }
++
++ Temp6x6Matrix getTranspose() const
++ {
++ Temp6x6Matrix temp;
++ for (PxU32 i = 0; i < 6; ++i)
++ {
++ for (PxU32 j = 0; j < 6; ++j)
++ {
++ temp.column[i][j] = column[j][i];
++ }
++ }
++ return temp;
++ }
++
++ PX_FORCE_INLINE Cm::SpatialVector operator * (const Cm::SpatialVector& s) const
++ {
++ Temp6x6Matrix tempMatrix = getTranspose();
++ PxReal st[6];
++ st[0] = s.angular.x; st[1] = s.angular.y; st[2] = s.angular.z;
++ st[3] = s.linear.x; st[4] = s.linear.y; st[5] = s.linear.z;
++
++ PxReal result[6];
++ for (PxU32 i = 0; i < 6; i++)
++ {
++ result[i] = 0;
++ for (PxU32 j = 0; j < 6; ++j)
++ {
++ result[i] += tempMatrix.column[i][j] * st[j];
++ }
++ }
++
++ Cm::SpatialVector temp;
++ temp.angular.x = result[0]; temp.angular.y = result[1]; temp.angular.z = result[2];
++ temp.linear.x = result[3]; temp.linear.y = result[4]; temp.linear.z = result[5];
++ return temp;
++ }
++
++ PX_FORCE_INLINE Cm::SpatialVectorF operator * (const Cm::SpatialVectorF& s) const
++ {
++ PxReal st[6];
++ st[0] = s.top.x; st[1] = s.top.y; st[2] = s.top.z;
++ st[3] = s.bottom.x; st[4] = s.bottom.y; st[5] = s.bottom.z;
++
++ PxReal result[6];
++ for (PxU32 i = 0; i < 6; ++i)
++ {
++ result[i] = 0.f;
++ for (PxU32 j = 0; j < 6; ++j)
++ {
++ result[i] += column[j][i] * st[j];
++ }
++ }
++
++ Cm::SpatialVectorF temp;
++ temp.top.x = result[0]; temp.top.y = result[1]; temp.top.z = result[2];
++ temp.bottom.x = result[3]; temp.bottom.y = result[4]; temp.bottom.z = result[5];
++ return temp;
++ }
++
++ PX_FORCE_INLINE Temp6x3Matrix operator * (const Temp6x3Matrix& s) const
++ {
++ Temp6x3Matrix temp;
++ for (PxU32 i = 0; i < 3; ++i)
++ {
++ PxReal* result = temp.column[i];
++
++ const PxReal* input = s.column[i];
++
++ for (PxU32 j = 0; j < 6; ++j)
++ {
++ result[j] = 0.f;
++ for (PxU32 k = 0; k < 6; ++k)
++ {
++ result[j] += column[k][j] * input[k];
++ }
++ }
++ }
++
++ return temp;
++ }
++
++ PX_FORCE_INLINE Cm::SpatialVector spatialVectorMul(const Cm::SpatialVector& s)
++ {
++ PxReal st[6];
++ st[0] = s.angular.x; st[1] = s.angular.y; st[2] = s.angular.z;
++ st[3] = s.linear.x; st[4] = s.linear.y; st[5] = s.linear.z;
++
++ PxReal result[6];
++ for (PxU32 i = 0; i < 6; ++i)
++ {
++ result[i] = 0.f;
++ for (PxU32 j = 0; j < 6; j++)
++ {
++ result[i] += column[i][j] * st[j];
++ }
++ }
++
++ Cm::SpatialVector temp;
++ temp.angular.x = result[0]; temp.angular.y = result[1]; temp.angular.z = result[2];
++ temp.linear.x = result[3]; temp.linear.y = result[4]; temp.linear.z = result[5];
++ return temp;
++ }
++
++ PX_FORCE_INLINE bool isEqual(const Cm::SpatialVectorF* m)
++ {
++ PxReal temp[6];
++ const PxReal eps = 0.00001f;
++ for (PxU32 i = 0; i < 6; ++i)
++ {
++ temp[0] = m[i].top.x; temp[1] = m[i].top.y; temp[2] = m[i].top.z;
++ temp[3] = m[i].bottom.x; temp[4] = m[i].bottom.y; temp[5] = m[i].bottom.z;
++
++ for (PxU32 j = 0; j < 6; ++j)
++ {
++ const PxReal dif = column[i][j] - temp[j];
++ if (PxAbs(dif) > eps)
++ return false;
++ }
++ }
++
++ return true;
++ }
++ };
++
++ //s is 3x6 matrix
++ PX_FORCE_INLINE Temp6x6Matrix Temp6x3Matrix::operator * (PxReal s[6][3])
++ {
++ Temp6x6Matrix temp;
++
++ for (PxU32 i = 0; i < 6; ++i)
++ {
++ PxReal* tc = temp.column[i];
++
++ for (PxU32 j = 0; j < 6; ++j)
++ {
++ tc[j] = 0.f;
++ for (PxU32 k = 0; k < 3; ++k)
++ {
++ tc[j] += column[k][j] * s[i][k];
++ }
++ }
++ }
++
++ return temp;
++ }
++
++ PX_FORCE_INLINE void calculateNewVelocity(const PxTransform& newTransform, const PxTransform& oldTransform,
++ const PxReal dt, PxVec3& linear, PxVec3& angular)
++ {
++ //calculate the new velocity
++ linear = (newTransform.p - oldTransform.p) / dt;
++ PxQuat quat = newTransform.q * oldTransform.q.getConjugate();
++
++ if (quat.w < 0) //shortest angle.
++ quat = -quat;
++
++ PxReal angle;
++ PxVec3 axis;
++ quat.toRadiansAndUnitAxis(angle, axis);
++ angular = (axis * angle) / dt;
++ }
++
++ // generates a pair of quaternions (swing, twist) such that in = swing * twist, with
++ // swing.x = 0
++ // twist.y = twist.z = 0, and twist is a unit quat
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void separateSwingTwist(const PxQuat& q, PxQuat& twist, PxQuat& swing1, PxQuat& swing2)
++ {
++ twist = q.x != 0.0f ? PxQuat(q.x, 0, 0, q.w).getNormalized() : PxQuat(PxIdentity);
++ PxQuat swing = q * twist.getConjugate();
++ swing1 = swing.y != 0.f ? PxQuat(0.f, swing.y, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
++ swing = swing * swing1.getConjugate();
++ swing2 = swing.z != 0.f ? PxQuat(0.f, 0.f, swing.z, swing.w).getNormalized() : PxQuat(PxIdentity);
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void separateSwingTwist2(const PxQuat& q, PxQuat& twist, PxQuat& swing1, PxQuat& swing2)
++ {
++ swing2 = q.z != 0.0f ? PxQuat(0.f, 0.f, q.z, q.w).getNormalized() : PxQuat(PxIdentity);
++ PxQuat swing = q * swing2.getConjugate();
++ swing1 = swing.y != 0.f ? PxQuat(0.f, swing.y, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
++ swing = swing * swing1.getConjugate();
++ twist = swing.x != 0.f ? PxQuat(swing.x, 0.f, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
++ }
++
++ } //namespace Dy
++
++ }
++
++ #endif