1022 lines
28 KiB
C++
1022 lines
28 KiB
C++
//
|
|
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2019 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#ifndef DY_FEATHERSTONE_ARTICULATION_UTIL_H
|
|
#define DY_FEATHERSTONE_ARTICULATION_UTIL_H
|
|
|
|
#include "PsVecMath.h"
|
|
#include "CmSpatialVector.h"
|
|
#include "PsBitUtils.h"
|
|
#include "foundation/PxMemory.h"
|
|
|
|
namespace physx
|
|
{
|
|
|
|
namespace Dy
|
|
{
|
|
static const size_t DY_MAX_DOF = 6;
|
|
|
|
struct SpatialSubspaceMatrix
|
|
{
|
|
static const PxU32 MaxColumns = 3;
|
|
public:
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialSubspaceMatrix() :numColumns(0)
|
|
{
|
|
//PxMemZero(columns, sizeof(Cm::SpatialVectorF) * 6);
|
|
memset(columns, 0, sizeof(Cm::UnAlignedSpatialVector) * MaxColumns);
|
|
}
|
|
|
|
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]; //192 192
|
|
PxU32 numColumns; //4 208 (12 bytes padding)
|
|
|
|
};
|
|
|
|
//this should be 6x6 matrix
|
|
//|R, 0|
|
|
//|-R*rX, R|
|
|
struct SpatialTransform
|
|
{
|
|
PxMat33 R;
|
|
PxQuat q;
|
|
PxMat33 T;
|
|
|
|
public:
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform() : R(PxZero), T(PxZero)
|
|
{
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform(const PxMat33& R_, const PxMat33& T_) : R(R_), T(T_)
|
|
{
|
|
q = PxQuat(R_);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform(const PxQuat& q_, const PxMat33& T_) : q(q_), T(T_)
|
|
{
|
|
R = PxMat33(q_);
|
|
}
|
|
|
|
//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 = R * s.angular;
|
|
const PxVec3 linear = T * s.angular + R * s.linear;
|
|
return Cm::SpatialVector(linear, angular);
|
|
}*/
|
|
|
|
|
|
////This assume angular is the top vector and linear is the bottom vector
|
|
//PX_FORCE_INLINE Cm::SpatialVectorF operator *(Cm::SpatialVectorF& s) const
|
|
//{
|
|
// const PxVec3 top = R * s.top;
|
|
// const PxVec3 bottom = T * s.top + R * s.bottom;
|
|
|
|
// const PxVec3 top1 = q.rotate(s.top);
|
|
// const PxVec3 bottom1 = T * s.top + q.rotate(s.bottom);
|
|
|
|
///* const PxVec3 tDif = (top - top1).abs();
|
|
// const PxVec3 bDif = (bottom - bottom1).abs();
|
|
// const PxReal eps = 0.001f;
|
|
// PX_ASSERT(tDif.x < eps && tDif.y < eps && tDif.z < eps);
|
|
// PX_ASSERT(bDif.x < eps && bDif.y < eps && bDif.z < eps);*/
|
|
// return Cm::SpatialVectorF(top1, bottom1);
|
|
//}
|
|
|
|
//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 = R * s.top;
|
|
//const PxVec3 bottom = T * s.top + R * s.bottom;
|
|
|
|
const PxVec3 top1 = q.rotate(s.top);
|
|
const PxVec3 bottom1 = T * s.top + q.rotate(s.bottom);
|
|
|
|
return Cm::SpatialVectorF(top1, bottom1);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector operator *(const Cm::UnAlignedSpatialVector& s) const
|
|
{
|
|
//const PxVec3 top = R * s.top;
|
|
//const PxVec3 bottom = T * s.top + R * s.bottom;
|
|
|
|
const PxVec3 top1 = q.rotate(s.top);
|
|
const PxVec3 bottom1 = T * s.top + q.rotate(s.bottom);
|
|
|
|
return Cm::UnAlignedSpatialVector(top1, bottom1);
|
|
}
|
|
|
|
//transpose is the same as inverse, R(inverse) = R(transpose)
|
|
//|R(t), 0 |
|
|
//|rXR(t), R(t)|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform getTranspose() const
|
|
{
|
|
SpatialTransform ret;
|
|
ret.q = q.getConjugate();
|
|
ret.R = R.getTranspose();
|
|
ret.T = T.getTranspose();
|
|
return ret;
|
|
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF transposeTransform(const Cm::SpatialVectorF& s) const
|
|
{
|
|
const PxVec3 top1 = q.rotateInv(s.top);
|
|
const PxVec3 bottom1 = T.transformTranspose(s.top) + q.rotateInv(s.bottom);
|
|
|
|
return Cm::SpatialVectorF(top1, bottom1);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector transposeTransform(const Cm::UnAlignedSpatialVector& s) const
|
|
{
|
|
const PxVec3 top1 = q.rotateInv(s.top);
|
|
const PxVec3 bottom1 = T.transformTranspose(s.top) + q.rotateInv(s.bottom);
|
|
|
|
return Cm::UnAlignedSpatialVector(top1, bottom1);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE void operator =(SpatialTransform& other)
|
|
{
|
|
R = other.R;
|
|
q = other.q;
|
|
T = other.T;
|
|
}
|
|
|
|
};
|
|
|
|
struct InvStIs
|
|
{
|
|
PxReal invStIs[3][3];
|
|
};
|
|
|
|
struct IsInvD
|
|
{
|
|
Cm::SpatialVectorF isInvD[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 r) : topLeft(PxZero), topRight(PxZero),
|
|
bottomLeft(PxZero)
|
|
{
|
|
PX_UNUSED(r);
|
|
}
|
|
|
|
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 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.getTranspose() * 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);
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator -(const SpatialMatrix& s) const
|
|
{
|
|
PxMat33 newTopLeft = topLeft - s.topLeft;
|
|
PxMat33 newTopRight = topRight - s.topRight;
|
|
PxMat33 newBottomLeft = bottomLeft - s.bottomLeft;
|
|
|
|
return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator +(const SpatialMatrix& s) const
|
|
{
|
|
PxMat33 newTopLeft = topLeft + s.topLeft;
|
|
PxMat33 newTopRight = topRight + s.topRight;
|
|
PxMat33 newBottomLeft = bottomLeft + s.bottomLeft;
|
|
|
|
return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator-()
|
|
{
|
|
PxMat33 newTopLeft = -topLeft;
|
|
PxMat33 newTopRight = -topRight;
|
|
PxMat33 newBottomLeft = -bottomLeft;
|
|
|
|
return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE void operator +=(const SpatialMatrix& s)
|
|
{
|
|
topLeft += s.topLeft;
|
|
topRight += s.topRight;
|
|
bottomLeft += s.bottomLeft;
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator *(const SpatialMatrix& s)
|
|
{
|
|
PxMat33 sBottomRight = s.topLeft.getTranspose();
|
|
PxMat33 bottomRight = topLeft.getTranspose();
|
|
|
|
PxMat33 newTopLeft = topLeft * s.topLeft + topRight * s.bottomLeft;
|
|
PxMat33 newTopRight = topLeft * s.topRight + topRight * sBottomRight;
|
|
PxMat33 newBottomLeft = bottomLeft * s.topLeft + bottomRight * s.bottomLeft;
|
|
|
|
return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
|
|
}
|
|
|
|
static SpatialMatrix constructSpatialMatrix(const Cm::SpatialVector& Is, const Cm::SpatialVector& stI)
|
|
{
|
|
//construct top left
|
|
PxVec3 tLeftC0 = Is.angular * stI.angular.x;
|
|
PxVec3 tLeftC1 = Is.angular * stI.angular.y;
|
|
PxVec3 tLeftC2 = Is.angular * stI.angular.z;
|
|
|
|
PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);
|
|
|
|
//construct top right
|
|
PxVec3 tRightC0 = Is.angular * stI.linear.x;
|
|
PxVec3 tRightC1 = Is.angular * stI.linear.y;
|
|
PxVec3 tRightC2 = Is.angular * stI.linear.z;
|
|
PxMat33 topRight(tRightC0, tRightC1, tRightC2);
|
|
|
|
//construct bottom left
|
|
PxVec3 bLeftC0 = Is.linear * stI.angular.x;
|
|
PxVec3 bLeftC1 = Is.linear * stI.angular.y;
|
|
PxVec3 bLeftC2 = Is.linear * stI.angular.z;
|
|
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
|
|
PxVec3 tLeftC0 = Is.top * stI.top.x;
|
|
PxVec3 tLeftC1 = Is.top * stI.top.y;
|
|
PxVec3 tLeftC2 = Is.top * stI.top.z;
|
|
|
|
PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);
|
|
|
|
//construct top right
|
|
PxVec3 tRightC0 = Is.top * stI.bottom.x;
|
|
PxVec3 tRightC1 = Is.top * stI.bottom.y;
|
|
PxVec3 tRightC2 = Is.top * stI.bottom.z;
|
|
PxMat33 topRight(tRightC0, tRightC1, tRightC2);
|
|
|
|
//construct bottom left
|
|
PxVec3 bLeftC0 = Is.bottom * stI.top.x;
|
|
PxVec3 bLeftC1 = Is.bottom * stI.top.y;
|
|
PxVec3 bLeftC2 = Is.bottom * stI.top.z;
|
|
PxMat33 bottomLeft(bLeftC0, bLeftC1, bLeftC2);
|
|
|
|
return SpatialMatrix(topLeft, topRight, bottomLeft);
|
|
}
|
|
|
|
static PX_CUDA_CALLABLE SpatialMatrix constructSpatialMatrix(const Cm::SpatialVectorF* columns)
|
|
{
|
|
PxMat33 topLeft(columns[0].top, columns[1].top, columns[2].top);
|
|
PxMat33 bottomLeft(columns[0].bottom, columns[1].bottom, columns[2].bottom);
|
|
PxMat33 topRight(columns[3].top, columns[4].top, columns[5].top);
|
|
|
|
return SpatialMatrix(topLeft, topRight, bottomLeft);
|
|
}
|
|
|
|
PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix getTranspose()
|
|
{
|
|
PxMat33 newTopLeft = topLeft.getTranspose();
|
|
PxMat33 newTopRight = bottomLeft.getTranspose();
|
|
PxMat33 newBottomLeft = topRight.getTranspose();
|
|
//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)
|
|
{
|
|
PxReal eps = 0.00001f;
|
|
float x = PxAbs(1.f - matrix.column0.x);
|
|
float y = PxAbs(1.f - matrix.column1.y);
|
|
float z = PxAbs(1.f - matrix.column2.z);
|
|
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)
|
|
{
|
|
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()
|
|
{
|
|
bool topLeftIsIdentity = isIdentity(topLeft);
|
|
|
|
bool topRightIsZero = isZero(topRight);
|
|
|
|
bool bottomLeftIsZero = isZero(bottomLeft);
|
|
|
|
return topLeftIsIdentity && topRightIsZero && bottomLeftIsZero;
|
|
}
|
|
|
|
static bool isEqual(const PxMat33& s0, const PxMat33& s1)
|
|
{
|
|
PxReal eps = 0.00001f;
|
|
for (PxU32 i = 0; i < 3; ++i)
|
|
{
|
|
for (PxU32 j = 0; j < 3; ++j)
|
|
{
|
|
PxReal t = s0[i][j] - s1[i][j];
|
|
if (PxAbs(t) > eps)
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
PX_FORCE_INLINE bool isEqual(const SpatialMatrix& s)
|
|
{
|
|
bool topLeftEqual = isEqual(topLeft, s.topLeft);
|
|
bool topRightEqual = isEqual(topRight, s.topRight);
|
|
bool bottomLeftEqual = isEqual(bottomLeft, s.bottomLeft);
|
|
|
|
return topLeftEqual && topRightEqual && bottomLeftEqual;
|
|
}
|
|
|
|
static PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 invertSym33(const PxMat33& in)
|
|
{
|
|
PxVec3 v0 = in[1].cross(in[2]),
|
|
v1 = in[2].cross(in[0]),
|
|
v2 = in[0].cross(in[1]);
|
|
|
|
PxReal det = v0.dot(in[0]);
|
|
|
|
if (det != 0)
|
|
{
|
|
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 Ps::aos::Mat33V invertSym33(const Ps::aos::Mat33V& in)
|
|
{
|
|
using namespace Ps::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);
|
|
|
|
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;
|
|
|
|
PxMat33 AAInv = invertSym33(aa);
|
|
|
|
PxMat33 z = -la * AAInv;
|
|
PxMat33 S = ll + z * la.getTranspose(); // Schur complement of mAA
|
|
|
|
PxMat33 LL = invertSym33(S);
|
|
|
|
PxMat33 LA = LL * z;
|
|
PxMat33 AA = AAInv + z.getTranspose() * LA;
|
|
|
|
SpatialMatrix result(LA.getTranspose(), AA, LL);// , LA);
|
|
|
|
return result;
|
|
}
|
|
|
|
PX_FORCE_INLINE void M33Store(const Ps::aos::Mat33V& src, PxMat33& dest)
|
|
{
|
|
Ps::aos::V3StoreU(src.col0, dest.column0);
|
|
Ps::aos::V3StoreU(src.col1, dest.column1);
|
|
Ps::aos::V3StoreU(src.col2, dest.column2);
|
|
}
|
|
|
|
PX_FORCE_INLINE void invertInertiaV(SpatialMatrix& result)
|
|
{
|
|
using namespace Ps::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());
|
|
|
|
Mat33V AAInv = invertSym33(aa);
|
|
|
|
Mat33V z = M33MulM33(M33Neg(la), AAInv);
|
|
Mat33V S = M33Add(ll, M33MulM33(z, M33Trnsps(la))); // Schur complement of mAA
|
|
|
|
Mat33V LL = invertSym33(S);
|
|
|
|
Mat33V LA = M33MulM33(LL, z);
|
|
Mat33V AA = M33Add(AAInv, M33MulM33(M33Trnsps(z), LA));
|
|
|
|
M33Store(M33Trnsps(LA), result.topLeft);
|
|
M33Store(AA, result.topRight);
|
|
M33Store(LL, result.bottomLeft);
|
|
}
|
|
|
|
SpatialMatrix getInverse()
|
|
{
|
|
PxMat33 bottomRight = topLeft.getTranspose();
|
|
|
|
PxMat33 blInverse = bottomLeft.getInverse();
|
|
PxMat33 lComp0 = blInverse * (-bottomRight);
|
|
PxMat33 lComp1 = topLeft * lComp0 + topRight;
|
|
|
|
//This can be simplified
|
|
PxMat33 newBottomLeft = lComp1.getInverse();
|
|
PxMat33 newTopLeft = lComp0 * newBottomLeft;
|
|
|
|
PxMat33 trInverse = topRight.getInverse();
|
|
PxMat33 rComp0 = trInverse * (-topLeft);
|
|
PxMat33 rComp1 = bottomLeft + bottomRight * rComp0;
|
|
|
|
PxMat33 newTopRight = rComp1.getInverse();
|
|
|
|
return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
|
|
}
|
|
|
|
void zero()
|
|
{
|
|
topLeft = PxMat33(PxZero);
|
|
topRight = PxMat33(PxZero);
|
|
bottomLeft = PxMat33(PxZero);
|
|
}
|
|
|
|
};
|
|
|
|
struct SpatialImpulseResponseMatrix
|
|
{
|
|
Cm::SpatialVectorF rows[6];
|
|
|
|
Cm::SpatialVectorF getResponse(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 Ps::aos;
|
|
Cm::SpatialVectorV row0(V3LoadA(&rows[0].top.x), V3LoadA(&rows[0].bottom.x));
|
|
Cm::SpatialVectorV row1(V3LoadA(&rows[1].top.x), V3LoadA(&rows[1].bottom.x));
|
|
Cm::SpatialVectorV row2(V3LoadA(&rows[2].top.x), V3LoadA(&rows[2].bottom.x));
|
|
Cm::SpatialVectorV row3(V3LoadA(&rows[3].top.x), V3LoadA(&rows[3].bottom.x));
|
|
Cm::SpatialVectorV row4(V3LoadA(&rows[4].top.x), V3LoadA(&rows[4].bottom.x));
|
|
Cm::SpatialVectorV row5(V3LoadA(&rows[5].top.x), V3LoadA(&rows[5].bottom.x));
|
|
|
|
Vec4V top = V4LoadA(&impulse.top.x);
|
|
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 getResponse(const Cm::SpatialVectorV& impulse) const
|
|
{
|
|
using namespace Ps::aos;
|
|
Cm::SpatialVectorV row0(V3LoadA(&rows[0].top.x), V3LoadA(&rows[0].bottom.x));
|
|
Cm::SpatialVectorV row1(V3LoadA(&rows[1].top.x), V3LoadA(&rows[1].bottom.x));
|
|
Cm::SpatialVectorV row2(V3LoadA(&rows[2].top.x), V3LoadA(&rows[2].bottom.x));
|
|
Cm::SpatialVectorV row3(V3LoadA(&rows[3].top.x), V3LoadA(&rows[3].bottom.x));
|
|
Cm::SpatialVectorV row4(V3LoadA(&rows[4].top.x), V3LoadA(&rows[4].bottom.x));
|
|
Cm::SpatialVectorV row5(V3LoadA(&rows[5].top.x), V3LoadA(&rows[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];
|
|
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];
|
|
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)
|
|
{
|
|
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
|