Revision control
Copy as Markdown
// 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