s452751/GRK
1
2
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
c260a57621
GRK/dependencies/physx-4.1/source/lowleveldynamics/include/DyFeatherstoneArticulation.h

1034 lines
42 KiB
C
Raw Normal View History

master
2022-01-12 16:07:16 +01:00
//
// 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 PXD_FEATHERSTONE_ARTICULATION_H
#define PXD_FEATHERSTONE_ARTICULATION_H
#include "foundation/PxVec3.h"
#include "foundation/PxQuat.h"
#include "foundation/PxTransform.h"
#include "PsVecMath.h"
#include "CmUtils.h"
#include "PxArticulationJoint.h"
#include "DyVArticulation.h"
#include "DyFeatherstoneArticulationUtils.h"
#include "DyFeatherstoneArticulationJointData.h"
#include "solver/PxSolverDefs.h"
#ifndef FEATHERSTONE_DEBUG
#define FEATHERSTONE_DEBUG 0
#endif
namespace physx
{
class PxContactJoint;
//struct PxSolverConstraintDesc;
class PxcConstraintBlockStream;
class PxcScratchAllocator;
class PxsConstraintBlockManager;
struct SolverConstraint1DExtStep;
struct PxSolverConstraintPrepDesc;
struct PxSolverBody;
struct PxSolverBodyData;
class PxConstraintAllocator;
class PxsContactManagerOutputIterator;
struct PxSolverConstraintDesc;
namespace Dy
{
//#if PX_VC
//#pragma warning(push)
//#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
//#endif
//class ArticulationJointCoreData;
class ArticulationLinkData;
struct SpatialSubspaceMatrix;
struct SolverConstraint1DExt;
struct SolverConstraint1DStep;
class FeatherstoneArticulation;
struct SpatialMatrix;
struct SpatialTransform;
struct Constraint;
class ThreadContext;
/* struct DyScratchAllocator
{
char* base;
size_t size;
size_t taken;
DyScratchAllocator(char* p, size_t s) : base(p), size(s), taken(0) {}
PxU32 round16(PxU32 size_) { return (size_ + 15)&(~15); }
template<class T> T* alloc(PxU32 count)
{
const PxU32 sizeReq = round16(sizeof(T)*count);
PX_ASSERT((taken+ sizeReq) < size);
T* result = reinterpret_cast<T*>(base + taken);
taken += sizeReq;
return result;
}
};*/
//This stuct is used in TGS
/* class ArticulationTempData
{
public:
Cm::SpatialVectorF mBaseLinkMotionVelocite;
Ps::Array<Cm::SpatialVectorF> mZAForce;
Ps::Array<PxReal> mJointPosition; // joint position
Ps::Array<PxReal> mJointVelocity; // joint velocity
Ps::Array<PxTransform> mLinkTransform; // this is the transform list for links
};*/
struct ArticulationInternalConstraint
{
//Common/shared directional info between, frictions and drives
Cm::UnAlignedSpatialVector row0; //24 24
Cm::UnAlignedSpatialVector row1; //24 48
Cm::UnAlignedSpatialVector deltaVA; //24 72
Cm::UnAlignedSpatialVector deltaVB; //24 96
//Response information
PxReal recipResponse; //4 100
PxReal response; //4 104
//Joint limit values
PxReal lowLimit; //4 108
PxReal highLimit; //4 112
PxReal lowImpulse; //4 116 changed
PxReal highImpulse; //4 120 changed
PxReal erp; //4 124
//Joint spring drive info
PxReal driveTargetVel; //4 128
PxReal driveBiasCoefficient; //4 132
PxReal driveTarget; //4 136
PxReal driveVelMultiplier; //4 140
PxReal driveBackPropagateScale; //4 144
PxReal driveImpulseMultiplier; //4 148
PxReal maxDriveForce; //4 152
PxReal driveForce; //4 156
PxReal maxFrictionForce; //4 160
PxReal frictionForce; //4 164
PxReal frictionForceCoefficient; //4 168
bool isLinearConstraint; //1 169
PxU8 padding[7]; //11 176
};
struct ArticulationInternalLockedAxis
{
//How much an impulse will effect angular velocity
Cm::UnAlignedSpatialVector deltaVA; //24 24
Cm::UnAlignedSpatialVector deltaVB; //24 48
//Jacobian axis that is locked
PxVec3 axis; //12 60
//Response information
PxReal recipResponse; //4 64
//Initial error
PxReal error; //4 68
//Bias scale
PxReal biasScale; //4 72
PxU32 pad[2]; //4 80
};
class ArticulationData
{
public:
ArticulationData() :
mLinksData(NULL), mJointData(NULL), mJointTranData(NULL),
mDt(0.f), mDofs(0xffffffff), mLocks(0), mDataDirty(true)
{
mRootPreMotionVelocity = Cm::SpatialVectorF::Zero();
}
~ArticulationData();
PX_FORCE_INLINE void init();
void resizeLinkData(const PxU32 linkCount);
void resizeJointData(const PxU32 dofs);
PX_FORCE_INLINE PxReal* getJointAccelerations() { return mJointAcceleration.begin(); }
PX_FORCE_INLINE const PxReal* getJointAccelerations() const { return mJointAcceleration.begin(); }
PX_FORCE_INLINE PxReal* getJointVelocities() { return mJointVelocity.begin(); }
PX_FORCE_INLINE const PxReal* getJointVelocities() const { return mJointVelocity.begin(); }
PX_FORCE_INLINE PxReal* getJointDeltaVelocities() { return mJointDeltaVelocity.begin(); }
PX_FORCE_INLINE const PxReal* getJointDeltaVelocities() const { return mJointDeltaVelocity.begin(); }
PX_FORCE_INLINE PxReal* getJointPositions() { return mJointPosition.begin(); }
PX_FORCE_INLINE const PxReal* getJointPositions() const { return mJointPosition.begin(); }
PX_FORCE_INLINE PxReal* getJointForces() { return mJointForce.begin(); }
PX_FORCE_INLINE const PxReal* getJointForces() const { return mJointForce.begin(); }
//PX_FORCE_INLINE PxReal* getJointFrictionForces() { return mJointFrictionForce.begin(); }
PX_FORCE_INLINE ArticulationInternalConstraint& getInternalConstraint(const PxU32 dofId) { return mInternalConstraints[dofId]; }
PX_FORCE_INLINE const ArticulationInternalConstraint& getInternalConstraint(const PxU32 dofId) const { return mInternalConstraints[dofId]; }
PX_FORCE_INLINE ArticulationInternalLockedAxis& getInternalLocks(const PxU32 dofId) { return mInternalLockedAxes[dofId]; }
PX_FORCE_INLINE const ArticulationInternalLockedAxis& getInternalLocks(const PxU32 dofId) const { return mInternalLockedAxes[dofId]; }
PX_FORCE_INLINE Cm::SpatialVectorF* getMotionVelocities() { return mMotionVelocities.begin(); }
PX_FORCE_INLINE Cm::SpatialVectorF* getMotionAccelerations() { return mMotionAccelerations.begin(); }
PX_FORCE_INLINE const Cm::SpatialVectorF* getMotionAccelerations() const { return mMotionAccelerations.begin(); }
PX_FORCE_INLINE Cm::SpatialVectorF* getCorioliseVectors() { return mCorioliseVectors.begin(); }
PX_FORCE_INLINE Cm::SpatialVectorF* getSpatialZAVectors() { return mZAForces.begin(); }
PX_FORCE_INLINE Cm::SpatialVectorF* getTransmittedForces() { return mJointTransmittedForce.begin(); }
PX_FORCE_INLINE Cm::SpatialVectorF* getPosIterMotionVelocities() { return mPosIterMotionVelocities.begin(); }
PX_FORCE_INLINE PxReal* getPosIterJointDeltaVelocities() { return mPosIterJointDeltaVelocities.begin(); }
PX_FORCE_INLINE Cm::SpatialVectorF& getPosIterMotionVelocity(const PxU32 index) { return mPosIterMotionVelocities[index]; }
PX_FORCE_INLINE const Cm::SpatialVectorF& getMotionVelocity(const PxU32 index) const { return mMotionVelocities[index]; }
PX_FORCE_INLINE const Cm::SpatialVectorF& getMotionAcceleration(const PxU32 index) const { return mMotionAccelerations[index]; }
PX_FORCE_INLINE const Cm::SpatialVectorF& getCorioliseVector(const PxU32 index) const { return mCorioliseVectors[index]; }
PX_FORCE_INLINE const Cm::SpatialVectorF& getSpatialZAVector(const PxU32 index) const { return mZAForces[index]; }
PX_FORCE_INLINE const Cm::SpatialVectorF& getTransmittedForce(const PxU32 index) const { return mJointTransmittedForce[index]; }
PX_FORCE_INLINE Cm::SpatialVectorF& getMotionVelocity(const PxU32 index) { return mMotionVelocities[index]; }
PX_FORCE_INLINE Cm::SpatialVectorF& getMotionAcceleration(const PxU32 index) { return mMotionAccelerations[index]; }
PX_FORCE_INLINE Cm::SpatialVectorF& getCorioliseVector(const PxU32 index) { return mCorioliseVectors[index]; }
PX_FORCE_INLINE Cm::SpatialVectorF& getSpatialZAVector(const PxU32 index) { return mZAForces[index]; }
PX_FORCE_INLINE Cm::SpatialVectorF& getTransmittedForce(const PxU32 index) { return mJointTransmittedForce[index]; }
//PX_FORCE_INLINE Dy::SpatialMatrix* getTempSpatialMatrix() { mTempSpatialMatrix.begin(); }
PX_FORCE_INLINE PxTransform& getPreTransform(const PxU32 index) { return mPreTransform[index]; }
PX_FORCE_INLINE const PxTransform& getPreTransform(const PxU32 index) const { return mPreTransform[index]; }
// PX_FORCE_INLINE void setPreTransform(const PxU32 index, const PxTransform& t){ mPreTransform[index] = t; }
PX_FORCE_INLINE PxTransform* getPreTransform() { return mPreTransform.begin(); }
PX_FORCE_INLINE const Cm::SpatialVectorF& getDeltaMotionVector(const PxU32 index) const { return mDeltaMotionVector[index]; }
PX_FORCE_INLINE void setDeltaMotionVector(const PxU32 index, const Cm::SpatialVectorF& vec) { mDeltaMotionVector[index] = vec; }
PX_FORCE_INLINE Cm::SpatialVectorF* getDeltaMotionVector() { return mDeltaMotionVector.begin(); }
PX_FORCE_INLINE ArticulationLink* getLinks() const { return mLinks; }
PX_FORCE_INLINE PxU32 getLinkCount() const { return mLinkCount; }
PX_FORCE_INLINE ArticulationLink& getLink(PxU32 index) const { return mLinks[index]; }
PX_FORCE_INLINE ArticulationLinkData* getLinkData() const { return mLinksData; }
ArticulationLinkData& getLinkData(PxU32 index) const;
PX_FORCE_INLINE ArticulationJointCoreData* getJointData() const { return mJointData; }
PX_FORCE_INLINE ArticulationJointCoreData& getJointData(PxU32 index) const { return mJointData[index]; }
PX_FORCE_INLINE ArticulationJointTargetData* getJointTranData() const { return mJointTranData; }
PX_FORCE_INLINE ArticulationJointTargetData& getJointTranData(PxU32 index) const { return mJointTranData[index]; }
// PT: PX-1399
PX_FORCE_INLINE PxArticulationFlags getArticulationFlags() const { return *mFlags; }
PX_FORCE_INLINE Cm::SpatialVector* getExternalAccelerations() { return mExternalAcceleration; }
PX_FORCE_INLINE PxU32 getSolverDataSize() const { return mSolverDataSize; }
PX_FORCE_INLINE Cm::SpatialVector& getExternalAcceleration(const PxU32 linkID) { return mExternalAcceleration[linkID]; }
PX_FORCE_INLINE PxReal getDt() const { return mDt; }
PX_FORCE_INLINE void setDt(const PxReal dt) { mDt = dt; }
PX_FORCE_INLINE bool getDataDirty() const { return mDataDirty; }
PX_FORCE_INLINE void setDataDirty(const bool dirty) { mDataDirty = dirty; }
PX_FORCE_INLINE PxU32 getDofs() const { return mDofs; }
PX_FORCE_INLINE void setDofs(const PxU32 dof) { mDofs = dof; }
PX_FORCE_INLINE PxU32 getLocks() const { return mLocks; }
PX_FORCE_INLINE void setLocks(const PxU32 locks) { mLocks = locks; }
PX_FORCE_INLINE FeatherstoneArticulation* getArticulation() { return mArticulation; }
PX_FORCE_INLINE void setArticulation(FeatherstoneArticulation* articulation) { mArticulation = articulation; }
PX_FORCE_INLINE const SpatialMatrix& getBaseInvSpatialArticulatedInertiaW() const { return mBaseInvSpatialArticulatedInertiaW; }
PX_FORCE_INLINE PxTransform* getAccumulatedPoses() { return mAccumulatedPoses.begin(); }
PX_FORCE_INLINE const PxTransform* getAccumulatedPoses() const { return mAccumulatedPoses.begin(); }
PX_FORCE_INLINE SpatialImpulseResponseMatrix* getImpulseResponseMatrixWorld() { return mResponseMatrixW.begin(); }
PX_FORCE_INLINE const SpatialImpulseResponseMatrix* getImpulseResponseMatrixWorld() const { return mResponseMatrixW.begin(); }
PX_FORCE_INLINE const SpatialMatrix& getWorldSpatialArticulatedInertia(const PxU32 linkID) const { return mWorldSpatialArticulatedInertia[linkID]; }
PX_FORCE_INLINE const InvStIs& getInvStIs(const PxU32 linkID) const { return mInvStIs[linkID]; }
PX_FORCE_INLINE const SpatialSubspaceMatrix& getMotionMatrix(const PxU32 linkID) const { return mMotionMatrix[linkID]; }
PX_FORCE_INLINE const SpatialSubspaceMatrix& getWorldMotionMatrix(const PxU32 linkID) const { return mWorldMotionMatrix[linkID]; }
PX_FORCE_INLINE const IsInvD& getWorldIsInvD(const PxU32 linkID) const { return mIsInvDW[linkID]; }
private:
Cm::SpatialVectorF mRootPreMotionVelocity;
Ps::Array<PxReal> mJointAcceleration; // joint acceleration
Ps::Array<PxReal> mJointVelocity; // joint velocity
Ps::Array<PxReal> mJointDeltaVelocity; // joint velocity change due to contacts
Ps::Array<PxReal> mJointPosition; // joint position
Ps::Array<PxReal> mJointForce; // joint force
//Ps::Array<PxReal> mJointFrictionForce; // joint friction force
Ps::Array<PxReal> mPosIterJointDeltaVelocities; //joint delta velocity after postion iternation before velocity iteration
Ps::Array<Cm::SpatialVectorF> mPosIterMotionVelocities; //link motion velocites after position iteration before velocity iteration
Ps::Array<Cm::SpatialVectorF> mMotionVelocities; //link motion velocites
Ps::Array<Cm::SpatialVectorF> mMotionAccelerations; //link motion accelerations
Ps::Array<Cm::SpatialVectorF> mCorioliseVectors; //link coriolise vector
Ps::Array<Cm::SpatialVectorF> mZAForces; //link spatial zero acceleration force/ spatial articulated force
Ps::Array<Cm::SpatialVectorF> mJointTransmittedForce;
Ps::Array<ArticulationInternalConstraint> mInternalConstraints;
Ps::Array<ArticulationInternalLockedAxis> mInternalLockedAxes;
Ps::Array<Cm::SpatialVectorF> mDeltaMotionVector; //this is for TGS solver
Ps::Array<PxTransform> mPreTransform; //this is the previous transform list for links
Ps::Array<SpatialImpulseResponseMatrix> mResponseMatrixW;
Ps::Array<SpatialMatrix> mWorldSpatialArticulatedInertia;
Ps::Array<InvStIs> mInvStIs;
Ps::Array<SpatialSubspaceMatrix> mMotionMatrix;
Ps::Array<SpatialSubspaceMatrix> mWorldMotionMatrix;
Ps::Array<IsInvD> mIsInvDW;
Ps::Array<PxU32> mNbStaticConstraints;
Ps::Array<PxU32> mStaticConstraintStartIndex;
Ps::Array<PxQuat> mRelativeQuat;
//Ps::Array<SpatialMatrix> mTempSpatialMatrix;
ArticulationLink* mLinks;
PxU32 mLinkCount;
ArticulationLinkData* mLinksData;
ArticulationJointCoreData* mJointData;
ArticulationJointTargetData* mJointTranData;
PxReal mDt;
PxU32 mDofs;
PxU32 mLocks;
const PxArticulationFlags* mFlags; // PT: PX-1399
Cm::SpatialVector* mExternalAcceleration;
PxU32 mSolverDataSize;
bool mDataDirty; //this means we need to call commonInit()
bool mJointDirty; //this means joint delta velocity has been changed by contacts so we need to update joint velocity/joint acceleration
FeatherstoneArticulation* mArticulation;
Ps::Array<PxTransform> mAccumulatedPoses;
Ps::Array<PxQuat> mDeltaQ;
PxReal mAccumulatedDt;
SpatialMatrix mBaseInvSpatialArticulatedInertiaW;
friend class FeatherstoneArticulation;
};
void ArticulationData::init()
{
//zero delta motion vector for TGS solver
PxMemZero(getDeltaMotionVector(), sizeof(Cm::SpatialVectorF) * mLinkCount);
//zero joint velocity delta, which will be changed in pxcFsApplyImpulse if there are any contact with links
PxMemZero(getJointDeltaVelocities(), sizeof(PxReal) * mDofs);
mJointDirty = false;
}
struct ScratchData
{
public:
ScratchData()
{
motionVelocities = NULL;
motionAccelerations = NULL;
coriolisVectors = NULL;
spatialZAVectors = NULL;
externalAccels = NULL;
compositeSpatialInertias = NULL;
jointVelocities = NULL;
jointAccelerations = NULL;
jointForces = NULL;
jointPositions = NULL;
jointFrictionForces = NULL;
}
Cm::SpatialVectorF* motionVelocities;
Cm::SpatialVectorF* motionAccelerations;
Cm::SpatialVectorF* coriolisVectors;
Cm::SpatialVectorF* spatialZAVectors;
Cm::SpatialVector* externalAccels;
Dy::SpatialMatrix* compositeSpatialInertias;
PxReal* jointVelocities;
PxReal* jointAccelerations;
PxReal* jointForces;
PxReal* jointPositions;
PxReal* jointFrictionForces;
};
#if PX_VC
#pragma warning(push)
#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
#endif
//Articulation dirty flag - used to tag which properties of the articulation are dirty. Used only to transfer selected data to the GPU...
struct ArticulationDirtyFlag
{
enum Enum
{
eDIRTY_JOINTS = 1<<0,
eDIRTY_POSITIONS = 1<<1,
eDIRTY_VELOCITIES = 1<<2,
eDIRTY_ACCELERATIONS = 1<<3,
eDIRTY_FORCES = 1<<4,
eDIRTY_ROOT = 1<<5,
eDIRTY_LINKS = 1<<6,
eIN_DIRTY_LIST = 1<<7,
eDIRTY_DOFS = (eDIRTY_POSITIONS | eDIRTY_VELOCITIES | eDIRTY_ACCELERATIONS | eDIRTY_FORCES)
};
};
PX_INLINE void computeArticJacobianAxes(PxVec3 row[3], const PxQuat& qa, const PxQuat& qb)
{
// Compute jacobian matrix for (qa* qb) [[* means conjugate in this expr]]
// d/dt (qa* qb) = 1/2 L(qa*) R(qb) (omega_b - omega_a)
// result is L(qa*) R(qb), where L(q) and R(q) are left/right q multiply matrix
const PxReal wa = qa.w, wb = qb.w;
const PxVec3 va(qa.x, qa.y, qa.z), vb(qb.x, qb.y, qb.z);
const PxVec3 c = vb*wa + va*wb;
const PxReal d0 = wa*wb;
const PxReal d1 = va.dot(vb);
const PxReal d = d0 - d1;
row[0] = (va * vb.x + vb * va.x + PxVec3(d, c.z, -c.y)) * 0.5f;
row[1] = (va * vb.y + vb * va.y + PxVec3(-c.z, d, c.x)) * 0.5f;
row[2] = (va * vb.z + vb * va.z + PxVec3(c.y, -c.x, d)) * 0.5f;
if ((d0 + d1) != 0.0f) // check if relative rotation is 180 degrees which can lead to singular matrix
return;
else
{
row[0].x += PX_EPS_F32;
row[1].y += PX_EPS_F32;
row[2].z += PX_EPS_F32;
}
}
PX_ALIGN_PREFIX(64)
class FeatherstoneArticulation : public ArticulationV
{
PX_NOCOPY(FeatherstoneArticulation)
public:
// public interface
explicit FeatherstoneArticulation(void*);
~FeatherstoneArticulation();
// get data sizes for allocation at higher levels
virtual void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize);
virtual bool resize(const PxU32 linkCount);
virtual void onUpdateSolverDesc();
virtual PxU32 getDofs();
virtual PxU32 getDof(const PxU32 linkID);
virtual bool applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag);
virtual void copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag);
virtual void packJointData(const PxReal* maximum, PxReal* reduced);
virtual void unpackJointData(const PxReal* reduced, PxReal* maximum);
virtual void initializeCommonData();
//gravity as input, joint force as output
virtual void getGeneralizedGravityForce(const PxVec3& gravity, PxArticulationCache& cache);
//joint velocity as input, generalised force(coriolis and centrigugal force) as output
virtual void getCoriolisAndCentrifugalForce(PxArticulationCache& cache);
//external force as input, joint force as output
virtual void getGeneralizedExternalForce(PxArticulationCache& /*cache*/);
//joint force as input, joint acceleration as output
virtual void getJointAcceleration(const PxVec3& gravity, PxArticulationCache& cache);
//joint acceleration as input, joint force as out
virtual void getJointForce(PxArticulationCache& cache);
virtual void getDenseJacobian(PxArticulationCache& cache, PxU32 & nRows, PxU32 & nCols);
//These two functions are for closed loop system
void getKMatrix(ArticulationJointCore* loopJoint, const PxU32 parentIndex, const PxU32 childIndex, PxArticulationCache& cache);
virtual void getCoefficientMatrix(const PxReal dt, const PxU32 linkID, const PxContactJoint* contactJoints, const PxU32 nbContacts, PxArticulationCache& cache);
virtual void getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache);
virtual bool getLambda(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache, PxArticulationCache& rollBackCache,
const PxReal* jointTorque, const PxVec3& gravity, const PxU32 maxIter);
virtual void getGeneralizedMassMatrix(PxArticulationCache& cache);
virtual void getGeneralizedMassMatrixCRB(PxArticulationCache& cache);
virtual bool storeStaticConstraint(const PxSolverConstraintDesc& desc);
virtual bool willStoreStaticConstraint() { return true; }
virtual void teleportRootLink();
virtual void getImpulseResponse(
PxU32 linkID,
Cm::SpatialVectorF* Z,
const Cm::SpatialVector& impulse,
Cm::SpatialVector& deltaV) const;
virtual void getImpulseResponse(
PxU32 linkID,
Cm::SpatialVectorV* /*Z*/,
const Cm::SpatialVectorV& impulse,
Cm::SpatialVectorV& deltaV) const;
virtual void getImpulseSelfResponse(
PxU32 linkID0,
PxU32 linkID1,
Cm::SpatialVectorF* Z,
const Cm::SpatialVector& impulse0,
const Cm::SpatialVector& impulse1,
Cm::SpatialVector& deltaV0,
Cm::SpatialVector& deltaV1) const;
virtual Cm::SpatialVectorV getLinkVelocity(const PxU32 linkID) const;
virtual Cm::SpatialVectorV getLinkMotionVector(const PxU32 linkID) const;
//this is called by island gen to determine whether the articulation should be awake or sleep
virtual Cm::SpatialVector getMotionVelocity(const PxU32 linkID) const;
virtual Cm::SpatialVector getMotionAcceleration(const PxU32 linkID) const;
virtual void fillIndexedManager(const PxU32 linkId, Dy::ArticulationLinkHandle& handle, PxU8& indexType);
virtual PxReal getLinkMaxPenBias(const PxU32 linkID) const;
static PxU32 computeUnconstrainedVelocities(
const ArticulationSolverDesc& desc,
PxReal dt,
PxConstraintAllocator& allocator,
PxSolverConstraintDesc* constraintDesc,
PxU32& acCount,
const PxVec3& gravity, PxU64 contextID,
Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
static void computeUnconstrainedVelocitiesTGS(
const ArticulationSolverDesc& desc,
PxReal dt, const PxVec3& gravity,
PxU64 contextID, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
static PxU32 setupSolverConstraintsTGS(const ArticulationSolverDesc& articDesc,
PxcConstraintBlockStream& stream,
PxSolverConstraintDesc* constraintDesc,
PxReal dt,
PxReal invDt,
PxReal totalDt,
PxU32& acCount,
PxsConstraintBlockManager& constraintBlockManager,
Cm::SpatialVectorF* Z);
static void saveVelocity(const ArticulationSolverDesc& d, Cm::SpatialVectorF* deltaV);
static void saveVelocityTGS(const ArticulationSolverDesc& d, PxReal invDtF32);
static void updateBodies(const ArticulationSolverDesc& desc, PxReal dt);
static void updateBodiesTGS(const ArticulationSolverDesc& desc, PxReal dt);
static void updateBodies(FeatherstoneArticulation* articulation, PxReal dt, bool integrateJointPosition);
static void recordDeltaMotion(const ArticulationSolverDesc& desc, const PxReal dt, Cm::SpatialVectorF* deltaV);
static void deltaMotionToMotionVelocity(const ArticulationSolverDesc& desc, PxReal invDt);
virtual void pxcFsApplyImpulse(PxU32 linkID, Ps::aos::Vec3V linear,
Ps::aos::Vec3V angular, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
virtual void pxcFsApplyImpulses(PxU32 linkID, const Ps::aos::Vec3V& linear,
const Ps::aos::Vec3V& angular, PxU32 linkID2, const Ps::aos::Vec3V& linear2,
const Ps::aos::Vec3V& angular2, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
virtual void pxcFsApplyImpulses(Cm::SpatialVectorF* Z);
virtual Cm::SpatialVectorV pxcFsGetVelocity(PxU32 linkID);
virtual void pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1);
virtual Cm::SpatialVectorV pxcFsGetVelocityTGS(PxU32 linkID);
virtual const PxTransform& getCurrentTransform(PxU32 linkID) const;
virtual const PxQuat& getDeltaQ(PxU32 linkID) const;
//Applies a set of N impulses, all in local space and updates the links' motion and joint velocities
void applyImpulses(Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
void getDeltaV(Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
//This method calculate the velocity change due to collision/constraint impulse, record joint velocity and acceleration
static Cm::SpatialVectorF propagateVelocityW(const PxVec3& c2p, const Dy::SpatialMatrix& spatialInertia,
const InvStIs& invStIs, const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z,
PxReal* jointVelocity, const Cm::SpatialVectorF& hDeltaV);
//This method calculate the velocity change due to collision/constraint impulse
static Cm::SpatialVectorF propagateVelocityTestImpulseW(const PxVec3& c2p, const Dy::SpatialMatrix& spatialInertia, const InvStIs& invStIs,
const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z, const Cm::SpatialVectorF& hDeltaV);
////This method calculate zero acceration impulse due to test/actual impluse
//static Cm::SpatialVectorF propagateImpulse(const IsInvD& isInvD, const SpatialTransform& childToParent,
// const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z);
static Cm::SpatialVectorF propagateImpulseW(const IsInvD& isInvD, const PxVec3& childToParent,
const SpatialSubspaceMatrix& motionMatrix, const Cm::SpatialVectorF& Z);
bool applyCacheToDest(ArticulationData& data, PxArticulationCache& cache,
PxReal* jVelocities, PxReal* jAcceleration, PxReal* jPosition, PxReal* jointForce,
const PxArticulationCacheFlags flag);
PX_FORCE_INLINE ArticulationData& getArticulationData() { return mArticulationData; }
PX_FORCE_INLINE const ArticulationData& getArticulationData() const { return mArticulationData; }
//void setGpuRemapId(const PxU32 id) { mGpuRemapId = id; }
//PxU32 getGpuRemapId() { return mGpuRemapId; }
static PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF translateSpatialVector(const PxVec3& offset, const Cm::SpatialVectorF& vec)
{
return Cm::SpatialVectorF(vec.top, vec.bottom + offset.cross(vec.top));
}
static PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector translateSpatialVector(const PxVec3& offset, const Cm::UnAlignedSpatialVector& vec)
{
return Cm::UnAlignedSpatialVector(vec.top, vec.bottom + offset.cross(vec.top));
}
static PX_FORCE_INLINE PxMat33 constructSkewSymmetricMatrix(const PxVec3 r)
{
return PxMat33(PxVec3(0.0f, r.z, -r.y),
PxVec3(-r.z, 0.0f, r.x),
PxVec3(r.y, -r.x, 0.0f));
}
bool raiseGPUDirtyFlag(ArticulationDirtyFlag::Enum flag)
{
bool nothingRaised = !(mGPUDirtyFlags);
mGPUDirtyFlags |= flag;
return nothingRaised;
}
void clearGPUDirtyFlags()
{
mGPUDirtyFlags = 0;
}
protected:
void constraintPrep(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints,
Cm::SpatialVectorF* Z, PxSolverConstraintPrepDesc& prepDesc, PxSolverBody& sBody,
PxSolverBodyData& sBodyData, PxSolverConstraintDesc* desc, PxConstraintAllocator& allocator);
void updateArticulation(ScratchData& scratchData,
const PxVec3& gravity,
Cm::SpatialVectorF* Z,
Cm::SpatialVectorF* DeltaV);
void computeUnconstrainedVelocitiesInternal(
const PxVec3& gravity,
Cm::SpatialVectorF* Z, Cm::SpatialVectorF* DeltaV);
//copy joint data from fromJointData to toJointData
void copyJointData(ArticulationData& data, PxReal* toJointData, const PxReal* fromJointData);
void computeDofs();
//this function calculates motion subspace matrix(s) for all tree joint
void jcalc(ArticulationData& data, bool forceUpdate = false);
//this function calculates loop joint constraint subspace matrix(s) and active force
//subspace matrix
void jcalcLoopJointSubspace(ArticulationJointCore* joint, ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& T);
void computeSpatialInertia(ArticulationData& data);
//compute zero acceleration force
void computeZ(ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData);
//compute drag force
void computeD(ArticulationData& data, ScratchData& scratchData,
Cm::SpatialVectorF* tZ, Cm::SpatialVectorF* tDeltaV);
void solveInternalConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
bool velocityIteration, bool isTGS, const PxReal elapsedTime);
Cm::SpatialVectorF solveInternalConstraintRecursive(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
bool velocityIteration, bool isTGS, const PxReal elapsedTime, const PxU32 linkID, const Cm::SpatialVectorF& parentDeltaV,
PxU32& dofId, PxU32& lockId);
void writebackInternalConstraints(bool isTGS);
void concludeInternalConstraints(bool isTGS);
//compute coriolis force
void computeC(ArticulationData& data, ScratchData& scratchData);
//compute relative transform child to parent
void computeRelativeTransformC2P(ArticulationData& data);
//compute relative transform child to base
void computeRelativeTransformC2B(ArticulationData& data);
void computeLinkVelocities(ArticulationData& data, ScratchData& scratchData);
void initLinks(ArticulationData& data, const PxVec3& gravity,
ScratchData& scratchData, Cm::SpatialVectorF* tZ, Cm::SpatialVectorF* tDeltaV);
void computeIs(ArticulationLinkData& linkDatum, ArticulationJointCoreData& jointDatum, const PxU32 linkID);
static SpatialMatrix computePropagateSpatialInertia(const PxU8 jointType, ArticulationJointCoreData& jointDatum,
const SpatialMatrix& articulatedInertia, const Cm::SpatialVectorF* linkIs, InvStIs& invStIs, IsInvD& isInvD,
const SpatialSubspaceMatrix& motionMatrix);
static void transformInertia(const SpatialTransform& sTod, SpatialMatrix& inertia);
static void translateInertia(const PxMat33& offset, SpatialMatrix& inertia);
void computeArticulatedSpatialInertia(ArticulationData& data);
void computeArticulatedResponseMatrix(ArticulationData& data);
void computeArticulatedSpatialZ(ArticulationData& data, ScratchData& scratchData);
/*void computeJointAcceleration(ArticulationLinkData& linkDatum, ArticulationJointCoreData& jointDatum,
const Cm::SpatialVectorF& pMotionAcceleration, PxReal* jointAcceleration, const PxU32 linkID);*/
void computeJointAccelerationW(ArticulationLinkData& linkDatum, ArticulationJointCoreData& jointDatum,
const Cm::SpatialVectorF& pMotionAcceleration, PxReal* jointAcceleration, const PxU32 linkID);
//compute joint acceleration, joint velocity and link acceleration, velocity based
//on spatial force and spatial articulated inertia tensor
void computeLinkAcceleration(ArticulationData& data, ScratchData& scratchData);
//void computeTempLinkAcceleration(ArticulationData& data, ScratchData& scratchData);
void computeJointTransmittedFrictionForce(ArticulationData& data, ScratchData& scratchData,
Cm::SpatialVectorF* Z, Cm::SpatialVectorF* DeltaV);
//void computeJointFriction(ArticulationData& data, ScratchData& scratchData);
//void copyFromBodyCore();
//void copyToBodyCore();
void updateBodies();
void applyExternalImpulse(ArticulationLink* links, const PxU32 linkCount, const bool fixBase,
ArticulationData& data, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV, const PxReal dt, const PxVec3& gravity, Cm::SpatialVector* acceleration);
static Cm::SpatialVectorF getDeltaVWithDeltaJV(const bool fixBase, const PxU32 linkID,
const ArticulationData& data, Cm::SpatialVectorF* Z,
PxReal* jointVelocities);
static Cm::SpatialVectorF getDeltaV(const bool fixBase, const PxU32 linkID,
const ArticulationData& data, Cm::SpatialVectorF* Z);
//impulse need to be in the linkID space
static void getZ(const PxU32 linkID, const ArticulationData& data,
Cm::SpatialVectorF* Z, const Cm::SpatialVectorF& impulse);
////impulse0 and impulse1 are in linkID0 and linkID1 space respectively
//static void getZ(
// const ArticulationData& data,
// Cm::SpatialVectorF* Z,
// PxU32 linkID0_,
// const Cm::SpatialVector& impulse0,
// PxU32 linkID1_,
// const Cm::SpatialVector& impulse1);
//This method use in impulse self response. The input impulse is in the link space
static Cm::SpatialVectorF getImpulseResponseW(
const PxU32 linkID,
const ArticulationData& data,
const Cm::SpatialVectorF& impulse);
//This method use in impulse self response. The input impulse is in the link space
static Cm::SpatialVectorF getImpulseResponseWithJ(
const PxU32 linkID,
const bool fixBase,
const ArticulationData& data,
Cm::SpatialVectorF* Z,
const Cm::SpatialVectorF& impulse,
PxReal* jointVelocities);
void getImpulseSelfResponseInv(const bool fixBase,
PxU32 linkID0,
PxU32 linkID1,
Cm::SpatialVectorF* Z,
const Cm::SpatialVector& impulse0,
const Cm::SpatialVector& impulse1,
Cm::SpatialVector& deltaV0,
Cm::SpatialVector& deltaV1,
PxReal* jointVelocities);
void getImpulseResponseSlowInv(Dy::ArticulationLink* links,
const ArticulationData& data,
PxU32 linkID0_,
const Cm::SpatialVector& impulse0,
Cm::SpatialVector& deltaV0,
PxU32 linkID1_,
const Cm::SpatialVector& impulse1,
Cm::SpatialVector& deltaV1,
PxReal* jointVelocities);
Cm::SpatialVectorF getImpulseResponseInv(const bool fixBase,
const PxU32 linkID, Cm::SpatialVectorF* Z,
const Cm::SpatialVector& impulse,
PxReal* jointVelocites);
void inverseDynamic(ArticulationData& data, const PxVec3& gravity,
ScratchData& scratchData, bool computeCoriolis);
void inverseDynamicFloatingBase(ArticulationData& data, const PxVec3& gravity,
ScratchData& scratchData, bool computeCoriolis);
//compute link body force with motion velocity and acceleration
void computeZAForceInv(ArticulationData& data, ScratchData& scratchData);
void initCompositeSpatialInertia(ArticulationData& data, Dy::SpatialMatrix* compositeSpatialInertia);
void computeCompositeSpatialInertiaAndZAForceInv(ArticulationData& data, ScratchData& scratchData);
void computeRelativeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData);
//provided joint velocity and joint acceleartion, compute link acceleration
void computeLinkAccelerationInv(ArticulationData& data, ScratchData& scratchData);
void computeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData);
void calculateMassMatrixColInv(ScratchData& scratchData);
void calculateHFixBase(PxArticulationCache& cache);
void calculateHFloatingBase(PxArticulationCache& cache);
//joint limits
void enforcePrismaticLimits(PxReal* jPosition, ArticulationJointCore* joint);
public:
static void getImpulseSelfResponse(ArticulationLink* links,
const bool fixBase,
Cm::SpatialVectorF* Z,
const ArticulationData& data,
PxU32 linkID0,
const Cm::SpatialVectorV& impulse0,
Cm::SpatialVectorV& deltaV0,
PxU32 linkID1,
const Cm::SpatialVectorV& impulse1,
Cm::SpatialVectorV& deltaV1);
static void getImpulseResponseSlow(Dy::ArticulationLink* links,
const ArticulationData& data,
PxU32 linkID0_,
const Cm::SpatialVector& impulse0,
Cm::SpatialVector& deltaV0,
PxU32 linkID1_,
const Cm::SpatialVector& impulse1,
Cm::SpatialVector& deltaV1,
Cm::SpatialVectorF* Z);
void createHardLimit(
ArticulationLink* links,
const bool fixBase,
Cm::SpatialVectorF* Z,
ArticulationData& data,
PxU32 linkIndex,
SolverConstraint1DExt& s,
const PxVec3& axis,
PxReal err,
PxReal recipDt);
void createHardLimits(
SolverConstraint1DExt& s0,
SolverConstraint1DExt& s1,
const PxVec3& axis,
PxReal err0,
PxReal err1,
PxReal recipDt,
const Cm::SpatialVectorV& deltaVA,
const Cm::SpatialVectorV& deltaVB,
PxReal recipResponse);
void createTangentialSpring(
ArticulationLink* links,
const bool fixBase,
Cm::SpatialVectorF* Z,
ArticulationData& data,
PxU32 linkIndex,
SolverConstraint1DExt& s,
const PxVec3& axis,
PxReal stiffness,
PxReal damping,
PxReal dt);
PxU32 setupSolverConstraints(
ArticulationLink* links,
const PxU32 linkCount,
const bool fixBase,
ArticulationData& data,
Cm::SpatialVectorF* Z,
PxU32& acCount);
void setupInternalConstraints(
ArticulationLink* links,
const PxU32 linkCount,
const bool fixBase,
ArticulationData& data,
Cm::SpatialVectorF* Z,
PxReal stepDt,
PxReal dt,
PxReal invDt,
PxReal erp,
bool isTGSSolver);
void setupInternalConstraintsRecursive(
ArticulationLink* links,
const PxU32 linkCount,
const bool fixBase,
ArticulationData& data,
Cm::SpatialVectorF* Z,
const PxReal stepDt,
const PxReal dt,
const PxReal invDt,
const PxReal erp,
const PxReal cfm,
const bool isTGSSolver,
const PxU32 linkID,
const PxReal maxForceScale);
virtual void prepareStaticConstraints(const PxReal dt, const PxReal invDt, PxsContactManagerOutputIterator& outputs,
Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold, PxReal solverOffsetSlop,
PxReal ccdMaxSeparation, PxSolverBodyData* solverBodyData, PxsConstraintBlockManager& blockManager,
Dy::ConstraintWriteback* constraintWritebackPool);
virtual void prepareStaticConstraintsTGS(const PxReal stepDt, const PxReal totalDt, const PxReal invStepDt, const PxReal invTotalDt,
PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold,
PxReal frictionOffsetThreshold, PxTGSSolverBodyData* solverBodyData,
PxTGSSolverBodyTxInertia* txInertia, PxsConstraintBlockManager& blockManager, Dy::ConstraintWriteback* constraintWritebackPool,
const PxU32 nbSubsteps, const PxReal lengthScale);
void createHardLimitTGS(
SolverConstraint1DExtStep& s,
const PxVec3& axis,
PxReal err,
PxReal recipDt,
const Cm::SpatialVectorV& deltaVA,
const Cm::SpatialVectorV& deltaVB,
PxReal recipResponse);
void createTangentialSpringTGS(
ArticulationLink* links,
const bool fixBase,
Cm::SpatialVectorF* Z,
ArticulationData& data,
PxU32 linkIndex,
SolverConstraint1DExtStep& s,
const PxVec3& axis,
PxReal stiffness,
PxReal damping,
PxReal dt);
//integration
void propagateLinksDown(ArticulationData& data, PxReal* jointVelocities, PxReal* jointPositions,
Cm::SpatialVectorF* motionVelocities);
void updateJointProperties(
const PxReal* jointDeltaVelocities,
const Cm::SpatialVectorF* motionVelocities,
PxReal* jointVelocities,
PxReal* jointAccelerations);
void recomputeAccelerations(const PxReal dt);
Cm::SpatialVector recomputeAcceleration(const PxU32 linkID, const PxReal dt) const;
void computeAndEnforceJointPositions(ArticulationData& data, PxReal* jointPositions);
//update link position based on joint position provided by the cache
void teleportLinks(ArticulationData& data);
void computeLinkVelocities(ArticulationData& data);
PxU8* allocateScratchSpatialData(PxcScratchAllocator* allocator,
const PxU32 linkCount, ScratchData& scratchData, bool fallBackToHeap = false);
// void allocateScratchSpatialData(DyScratchAllocator& allocator,
// const PxU32 linkCount, ScratchData& scratchData);
//This method calculate the velocity change from parent to child using parent current motion velocity
PxTransform propagateTransform(const PxU32 linkID, ArticulationLink* links, ArticulationJointCoreData& jointDatum,
Cm::SpatialVectorF* motionVelocities, const PxReal dt, const PxTransform& pBody2World, const PxTransform& currentTransform,
PxReal* jointVelocity, PxReal* jointDeltaVelocities, PxReal* jointPosition, const SpatialSubspaceMatrix& motionMatrix,
const SpatialSubspaceMatrix& worldMotionMatrix);
static void updateRootBody(const Cm::SpatialVectorF& motionVelocity,
const PxTransform& preTransform, ArticulationData& data, const PxReal dt);
ArticulationData mArticulationData;
Ps::Array<char> mScratchMemory;
bool mHasSphericalJoint;
Ps::Array<PxSolverConstraintDesc> mStaticConstraints;
//this is the id remap pxgbodysim and pxgariculation. if application delete the articulation, we need to
//put back this id to the id pool
//PxU32 mGpuRemapId;
PxU32 mGPUDirtyFlags;
} PX_ALIGN_SUFFIX(64);
#if PX_VC
#pragma warning(pop)
#endif
void PxvRegisterArticulationsReducedCoordinate();
} //namespace Dy
}
#endif
Reference in New Issue Copy Permalink