DyFeatherstoneArticulation.h

++ // 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_H

++ #define DY_FEATHERSTONE_ARTICULATION_H

++

++ #include "DyFeatherstoneArticulationJointData.h"

++ #include "DyArticulationTendon.h"

++ #include "DyCpuGpuArticulation.h"

++ #include "PxvDynamics.h"

++

++ #ifndef FEATHERSTONE_DEBUG

++ #define FEATHERSTONE_DEBUG 0

++ #endif

++

++ #define DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER true

++

++ namespace physx

++ {

++

++ namespace Cm

++ {

++ 	template <class T> class PinnableArray;

++ }

++

++ class PxcScratchAllocator;

++ class PxsConstraintBlockManager;

++ struct PxSolverConstraintPrepDesc;

++ struct PxSolverBody;

++ struct PxSolverBodyData;

++ class PxConstraintAllocator;

++ class PxsContactManagerOutputIterator;

++ struct PxSolverConstraintDesc;

++

++ namespace Dy

++ {

++ 	struct ArticulationMimicJointCore;

++ 	class FeatherstoneArticulation;

++ 	struct SpatialMatrix;

++ 	struct Constraint;

++ 	class ThreadContext;

++ 	struct InternalConstraintSolverData;

++ 	struct ConstraintWriteback;

++

++ 	struct ArticulationInternalTendonConstraint

++ 	{

++ 		Cm::UnAlignedSpatialVector row0;			//24	24

++ 		Cm::UnAlignedSpatialVector row1;			//24	48

++

++ 		Cm::UnAlignedSpatialVector deltaVB;			//24	72

++

++ 		PxU32	linkID0;							//4		74

++ 		PxU32	linkID1;							//4		78

++ 		PxReal	accumulatedLength;					//4		82	//accumulate distance for spatial tendon, accumulate joint pose for fixed tendon

++ 		PxReal	biasCoefficient;					//4		94

++ 		PxReal	velMultiplier;						//4		98

++ 		PxReal	impulseMultiplier;					//4		102

++ 		PxReal	appliedForce;						//4		106

++ 		PxReal	recipResponse;						//4		110

++ 		PxReal	deltaVA;							//4		114

++ 		PxReal	limitBiasCoefficient;

++ 		PxReal	limitImpulseMultiplier;

++ 		PxReal	limitAppliedForce;

++ 		PxReal	restDistance;

++ 		PxReal	lowLimit;

++ 		PxReal	highLimit;

++

++ 		void setTendonImplicitSpringParams(const TendonImplicitSpringParams& springParams)

++ 		{

++ 			biasCoefficient = springParams.biasCoefficient;

++ 			velMultiplier = springParams.velMultiplier;

++ 			impulseMultiplier= springParams.impulseMultiplier;

++ 			limitBiasCoefficient = springParams.limitBiasCoefficient;

++ 			limitImpulseMultiplier = springParams.limitImpulseMultiplier;

++ 		}

++ 		TendonImplicitSpringParams getTendonImplicitSpringParams() const

++ 		{

++ 			const TendonImplicitSpringParams params(biasCoefficient, velMultiplier, impulseMultiplier, limitBiasCoefficient, limitImpulseMultiplier);

++ 			return params;

++ 		}

++ 	};

++

++ 	struct ArticulationInternalConstraintBase

++ 	{

++ 		//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

++ 	};

++

++ 	struct ArticulationInternalLimit

++ 	{

++ 		// Initial error for high and low limits. Negative means limit is violated.

++ 		PxReal errorLow;

++ 		PxReal errorHigh;

++

++ 		// Impulses are updated during solver iterations.

++ 		PxReal lowImpulse; // always >= 0 (repulsive only)

++ 		PxReal highImpulse; // always <= 0 (repulsive only)

++ 	};

++

++ 	struct ArticulationInternalConstraint : public ArticulationInternalConstraintBase

++ 	{

++ 		ArticulationImplicitDriveDesc implicitDriveDesc;

++ 		PxPerformanceEnvelope envelope;

++ 		PxReal externalJointForce;

++ 		PxReal driveImpulse;

++ 		PxReal driveMaxImpulse;

++ 		PxReal dynamicFrictionEffort;

++ 		PxReal staticFrictionEffort;

++ 		PxReal viscousFrictionCoefficient;

++ 		PxReal deprecatedFrictionMaxForce;

++ 		PxReal accumulatedFrictionImpulse;

++ 		PxReal maxJointVelocity;

++

++ 		bool isLinearConstraint;

++

++ 		void setImplicitDriveDesc(const ArticulationImplicitDriveDesc& driveDesc)

++ 		{

++ 			implicitDriveDesc = driveDesc;

++ 		}

++ 		const ArticulationImplicitDriveDesc& getImplicitDriveDesc() const

++ 		{

++ 			return implicitDriveDesc;

++ 		}

++ 		PxReal padding[3];

++ 	};

++ 	PX_COMPILE_TIME_ASSERT(0 == (sizeof(ArticulationInternalConstraint) & 0x0f));

++

++ 	struct ArticulationInternalMimicJoint

++ 	{

++ 		PxU32 linkA;

++ 		PxU32 dofA;

++

++ 		PxU32 linkB;

++ 		PxU32 dofB;

++

++ 		PxReal gearRatio;

++ 		PxReal offset;

++

++ 		PxReal naturalFrequency;

++ 		PxReal dampingRatio;

++

++ 		//Impulse = [1, gearRatio]^T * [-C*erp/dt + J*v] / [J * M^-1 * J^T + cfm]

++ 		//Cache recipEffectiveInertia = [J * M^-1 * J^T] so we can later compute:

++ 		//Impulse = [1, gearRatio]^T * [-b + J*v] /[recipEffectiveInertia + cfm];

++ 		PxReal recipEffectiveInertia;

++ 	};

++

++ 	class ArticulationData

++ 	{

++ 	public:

++

++ 		ArticulationData() :

++ 			mJointAcceleration(NULL),

++ 			mJointVelocity(NULL),

++ 			mJointNewVelocity(NULL),

++ 			mJointPosition(NULL),

++ 			mJointForce(NULL),

++ 			mJointTargetPositions(NULL),

++ 			mJointTargetVelocities(NULL),

++ 			mPosIterJointVelocities(NULL),

++ 			mDofCapacity(0),

++ 			mLinkCapacity(0),

++ 			mPosIterMotionVelocities(NULL),

++ 			mMotionVelocities(NULL),

++ 			mSolverLinkSpatialDeltaVels(NULL),

++ 			mSolverLinkSpatialImpulses(NULL),

++ 			mMotionAccelerations(NULL),

++ 			mLinkIncomingJointForces(NULL),

++ 			mMotionAccelerationsInternal(NULL),

++ 			mCoriolisVectors(NULL),

++ 			mZAInternalForces(NULL),

++ 			mZAForces(NULL),

++ 			mJointTransmittedForce(NULL),

++ 			mDeferredQstZ(NULL),

++ 			mDeltaMotionVector(NULL),

++ 			mPreTransform(NULL),

++ 			mResponseMatrixW(NULL),

++ 			mWorldSpatialArticulatedInertia(NULL),

++ 			mWorldIsolatedSpatialArticulatedInertia(NULL),

++ 			mMasses(NULL),

++ 			mInvStIs(NULL),

++ 			mIsW(NULL),

++ 			qstZIc(NULL),

++ 			qstZIntIc(NULL),

++ 			mMotionMatrix(NULL),

++ 			mWorldMotionMatrix(NULL),

++ 			mISInvStIS(NULL),

++ 			mRw(NULL),

++ 			mNbStatic1DConstraints(NULL),

++ 			mNbStaticContactConstraints(NULL),

++ 			mStatic1DConstraintStartIndex(NULL),

++ 			mStaticContactConstraintStartIndex(NULL),

++ 			mRelativeQuat(NULL),

++ 			mLinks(NULL),

++ 			mLinkCount(0),

++ 			mNumPathToRootElements(0),

++ 			mPathToRootElements(NULL),

++ 			mJointData(NULL),

++ 			mSpatialTendons(NULL),

++ 			mNumSpatialTendons(0),

++ 			mNumTotalAttachments(0),

++ 			mFixedTendons(NULL),

++ 			mNumFixedTendons(0),

++ 			mNbMimicJoints(0),

++ 			mMimicJoints(NULL),

++ 			mDt(0.0f),

++ 			mDofs(0xffffffff),

++ 			mFlags(NULL),

++ 			mExternalAcceleration(NULL),

++ 			mArticulation(NULL),

++ 			mAccumulatedPoses(NULL),

++ 			mDeltaQ(NULL),

++ 			mInvSumMass(0.0f),

++ 			mCOM(PxZero),

++ 			mDataDirty(true),

++ 			mJointDirty(false),

++ 			mIsExternalForcesEveryTgsIterationEnabled(false)

++ 		{

++ 		}

++

++ 		~ArticulationData();

++

++ 		PX_FORCE_INLINE	void						init();

++ 						void						resizeLinkData(const PxU32 linkCount);

++ 						void						resizeJointData(const PxU32 dofs);

++

++ 		PX_FORCE_INLINE PxReal*						getJointAccelerations()									{ return mJointAcceleration;		}

++ 		PX_FORCE_INLINE const PxReal*				getJointAccelerations() const							{ return mJointAcceleration;		}

++

++ 		PX_FORCE_INLINE PxReal*						getJointVelocities()									{ return mJointVelocity;			}

++ 		PX_FORCE_INLINE const PxReal*				getJointVelocities() const								{ return mJointVelocity;			}

++

++ 		PX_FORCE_INLINE PxReal*						getJointNewVelocities()									{ return mJointNewVelocity;			}

++ 		PX_FORCE_INLINE const PxReal*				getJointNewVelocities() const							{ return mJointNewVelocity;			}

++

++ 		PX_FORCE_INLINE PxReal*						getJointPositions()										{ return mJointPosition;			}

++ 		PX_FORCE_INLINE const PxReal*				getJointPositions() const								{ return mJointPosition;			}

++

++ 		PX_FORCE_INLINE PxReal*						getJointForces()										{ return mJointForce;				}

++ 		PX_FORCE_INLINE const PxReal*				getJointForces() const									{ return mJointForce;				}

++

++ 		PX_FORCE_INLINE PxReal*						getJointTargetPositions()								{ return mJointTargetPositions;		}

++ 		PX_FORCE_INLINE const PxReal*				getJointTargetPositions() const							{ return mJointTargetPositions;		}

++

++ 		PX_FORCE_INLINE PxReal*						getJointTargetVelocities()								{ return mJointTargetVelocities;	}

++ 		PX_FORCE_INLINE const PxReal*				getJointTargetVelocities() const						{ return mJointTargetVelocities;	}

++

++ 		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 Cm::SpatialVectorF*			getMotionVelocities()									{ return mMotionVelocities;			}

++ 		PX_FORCE_INLINE const Cm::SpatialVectorF*	getMotionVelocities()						const		{ return mMotionVelocities;			}

++

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getMotionAccelerations()								{ return mMotionAccelerations;		}

++ 		PX_FORCE_INLINE const Cm::SpatialVectorF*	getMotionAccelerations() const							{ return mMotionAccelerations;		}

++

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getMotionAccelerationsInternal()						{ return mMotionAccelerationsInternal;		}

++ 		PX_FORCE_INLINE	Cm::SpatialVectorF*			getLinkIncomingJointForces()							{ return mLinkIncomingJointForces;	}

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getCoriolisVectors()									{ return mCoriolisVectors;			}

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getSpatialZAVectors()									{ return mZAForces;					}

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getSpatialZAInternalVectors()							{ return mZAInternalForces;			}

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getTransmittedForces()									{ return mJointTransmittedForce;	}

++

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getPosIterMotionVelocities()							{ return mPosIterMotionVelocities;			}

++ 		PX_FORCE_INLINE const Cm::SpatialVectorF*	getPosIterMotionVelocities() const						{ return mPosIterMotionVelocities;			}

++ 		PX_FORCE_INLINE PxReal*						getPosIterJointVelocities()								{ return mPosIterJointVelocities;			}

++ 		PX_FORCE_INLINE const PxReal*				getPosIterJointVelocities()	const						{ return mPosIterJointVelocities;			}

++

++ 		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&	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&			getTransmittedForce(const PxU32 index)					{ return mJointTransmittedForce[index];		}

++

++ 		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 PxTransform*				getPreTransform()										{ return mPreTransform;						}

++ 		PX_FORCE_INLINE const PxTransform*			getPreTransform()	const								{ return mPreTransform;						}

++

++ 		PX_FORCE_INLINE const Cm::SpatialVectorF&	getDeltaMotionVector(const PxU32 index) const			{ return mDeltaMotionVector[index];			}

++ 		PX_FORCE_INLINE Cm::SpatialVectorF*			getDeltaMotionVector()									{ return mDeltaMotionVector;				}

++

++ 		PX_FORCE_INLINE ArticulationLink*			getLinks()												{ return mLinks;							}

++ 		PX_FORCE_INLINE const ArticulationLink*		getLinks()										const	{ return mLinks;							}

++ 		PX_FORCE_INLINE PxU32						getLinkCount()									const	{ return mLinkCount;						}

++ 		PX_FORCE_INLINE const ArticulationLink&		getLink(PxU32 index)							const	{ return mLinks[index];						}

++

++ 		PX_FORCE_INLINE ArticulationSpatialTendon**	getSpatialTendons()								const	{ return mSpatialTendons;					}

++ 		PX_FORCE_INLINE PxU32						getSpatialTendonCount()							const	{ return mNumSpatialTendons;				}

++ 		PX_FORCE_INLINE ArticulationSpatialTendon*	getSpatialTendon(PxU32 index)					const	{ return mSpatialTendons[index];			}

++

++ 		PX_FORCE_INLINE ArticulationFixedTendon**	getFixedTendons()								const	{ return mFixedTendons;						}

++ 		PX_FORCE_INLINE PxU32						getFixedTendonCount()							const	{ return mNumFixedTendons;					}

++ 		PX_FORCE_INLINE ArticulationFixedTendon*	getFixedTendon(PxU32 index)						const	{ return mFixedTendons[index];				}

++

++ 		PX_FORCE_INLINE	ArticulationMimicJointCore** getMimicJointCores()							const	{ return mMimicJoints;						}

++ 		PX_FORCE_INLINE PxU32						getMimicJointCount()							const	{ return mNbMimicJoints;					}

++

++ 		PX_FORCE_INLINE ArticulationJointCoreData*	getJointData()									const	{ return mJointData;						}

++ 		PX_FORCE_INLINE ArticulationJointCoreData&	getJointData(PxU32 index)						const	{ return mJointData[index];					}

++

++ 		// PT: PX-1399

++ 		PX_FORCE_INLINE PxArticulationFlags			getArticulationFlags()							const	{ return *mFlags;							}

++

++ 		PX_FORCE_INLINE const Cm::SpatialVector*	getExternalAccelerations()						const	{ return mExternalAcceleration;				}

++ 		PX_FORCE_INLINE Cm::SpatialVector*			getExternalAccelerations()								{ return mExternalAcceleration;				}

++

++ 		PX_FORCE_INLINE Cm::SpatialVector&			getExternalAcceleration(const PxU32 linkID)				{ return mExternalAcceleration[linkID];		}

++ 		PX_FORCE_INLINE const Cm::SpatialVector&	getExternalAcceleration(const PxU32 linkID)		const	{ 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 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		  SpatialMatrix&		getBaseInvSpatialArticulatedInertiaW()					{ return mBaseInvSpatialArticulatedInertiaW; }

++

++ 		PX_FORCE_INLINE PxTransform*				getAccumulatedPoses()									{ return mAccumulatedPoses;					}

++ 		PX_FORCE_INLINE const PxTransform*			getAccumulatedPoses()							const	{ return mAccumulatedPoses;					}

++

++ 		PX_FORCE_INLINE PxQuat*						getDeltaQ()												{ return mDeltaQ;					}

++ 		PX_FORCE_INLINE const PxQuat*				getDeltaQ()										const	{ return mDeltaQ;					}

++

++ 		PX_FORCE_INLINE const Cm::SpatialVectorF&	getRootDeferredZ()								const	{ return mRootDeferredZ;					}

++ 		PX_FORCE_INLINE	Cm::SpatialVectorF&			getRootDeferredZ()										{ return mRootDeferredZ;					}

++

++ 		PX_FORCE_INLINE const	SpatialMatrix*		getWorldSpatialArticulatedInertia()				const	{ return mWorldSpatialArticulatedInertia;	}

++ 		PX_FORCE_INLINE			SpatialMatrix*		getWorldSpatialArticulatedInertia()						{ return mWorldSpatialArticulatedInertia;	}

++ 		PX_FORCE_INLINE const SpatialMatrix&		getWorldSpatialArticulatedInertia(const PxU32 linkID) const { return mWorldSpatialArticulatedInertia[linkID]; }

++

++ 		PX_FORCE_INLINE const	PxMat33*		getWorldIsolatedSpatialArticulatedInertia()				const	{ return mWorldIsolatedSpatialArticulatedInertia;	}

++ 		PX_FORCE_INLINE			PxMat33*		getWorldIsolatedSpatialArticulatedInertia()						{ return mWorldIsolatedSpatialArticulatedInertia;	}

++

++ 		PX_FORCE_INLINE const	PxReal*			getMasses()											const	{ return mMasses;	}

++ 		PX_FORCE_INLINE PxReal*					getMasses()													{ return mMasses;	}

++

++ 		PX_FORCE_INLINE const	Cm::UnAlignedSpatialVector* getWorldMotionMatrix()					const	{ return mWorldMotionMatrix; }

++ 		PX_FORCE_INLINE			Cm::UnAlignedSpatialVector* getWorldMotionMatrix()							{ return mWorldMotionMatrix; }

++ 		PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& getWorldMotionMatrix(const PxU32 dofId)	const	{ return mWorldMotionMatrix[dofId]; }

++

++ 		PX_FORCE_INLINE const	Cm::UnAlignedSpatialVector* getMotionMatrix()						const	{ return mMotionMatrix;	}

++ 		PX_FORCE_INLINE			Cm::UnAlignedSpatialVector* getMotionMatrix()								{ return mMotionMatrix;	}

++ 		PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& getMotionMatrix(const PxU32 dofId)		const	{ return mMotionMatrix[dofId]; }

++

++ 		PX_FORCE_INLINE const	Cm::SpatialVectorF* getIsW()										const	{ return mIsW; }

++ 		PX_FORCE_INLINE			Cm::SpatialVectorF* getIsW()												{ return mIsW; }

++ 		PX_FORCE_INLINE const Cm::SpatialVectorF& getIsW(const PxU32 dofId)							const	{ return mIsW[dofId]; }

++

++ 		PX_FORCE_INLINE const	PxVec3* getRw()														const	{ return mRw; }

++ 		PX_FORCE_INLINE			PxVec3* getRw()																{ return mRw; }

++

++ 		PX_FORCE_INLINE const	PxQuat* getRelativeQuat()											const	{ return mRelativeQuat; }

++ 		PX_FORCE_INLINE			PxQuat* getRelativeQuat()													{ return mRelativeQuat; }

++

++ 		PX_FORCE_INLINE const	Cm::SpatialVectorF* getSolverLinkSpatialDeltaVels()					const	{ return mSolverLinkSpatialDeltaVels; }

++ 		PX_FORCE_INLINE			Cm::SpatialVectorF* getSolverLinkSpatialDeltaVels()							{ return mSolverLinkSpatialDeltaVels; }

++

++ 		PX_FORCE_INLINE const	Cm::SpatialVectorF* getSolverLinkSpatialImpulses()					const	{ return mSolverLinkSpatialImpulses; }

++ 		PX_FORCE_INLINE			Cm::SpatialVectorF* getSolverLinkSpatialImpulses()							{ return mSolverLinkSpatialImpulses; }

++

++ 		PX_FORCE_INLINE const	PxReal* getMinusStZExt()											const	{ return qstZIc;	}

++ 		PX_FORCE_INLINE			PxReal* getMinusStZExt()													{ return qstZIc;	}

++

++ 		PX_FORCE_INLINE const	PxReal* getQstZIc()													const	{ return qstZIc;	}

++ 		PX_FORCE_INLINE			PxReal* getQstZIc()															{ return qstZIc;	}

++

++ 		PX_FORCE_INLINE	const	PxReal* getQStZIntIc()												const	{ return qstZIntIc;	}

++ 		PX_FORCE_INLINE			PxReal* getQStZIntIc()														{ return qstZIntIc;	}

++

++ 		PX_FORCE_INLINE const	InvStIs* getInvStIS()												const	{ return mInvStIs;	}

++ 		PX_FORCE_INLINE			InvStIs* getInvStIS()														{ return mInvStIs;	}

++ 		PX_FORCE_INLINE const InvStIs& getInvStIs(const PxU32 linkID)								const	{ return mInvStIs[linkID]; }

++

++ 		PX_FORCE_INLINE const	Cm::SpatialVectorF* getISInvStIS()									const	{ return mISInvStIS; }

++ 		PX_FORCE_INLINE			Cm::SpatialVectorF* getISInvStIS()											{ return mISInvStIS; }

++ 		PX_FORCE_INLINE const Cm::SpatialVectorF& getWorldIsInvD(const PxU32 dofId)					const	{ return mISInvStIS[dofId]; }

++

++ 		PX_FORCE_INLINE			TestImpulseResponse* getImpulseResponseMatrixWorld()						{ return mResponseMatrixW;	}

++ 		PX_FORCE_INLINE const	TestImpulseResponse* getImpulseResponseMatrixWorld()				const	{ return mResponseMatrixW;	}

++

++ 		PX_FORCE_INLINE PxReal* getDeferredQstZ() { return mDeferredQstZ; }

++ 		PX_FORCE_INLINE const PxReal* getDeferredQstZ()	const	{ return mDeferredQstZ; }

++

++ 		PX_FORCE_INLINE const PxU32*	getPathToRootElements() const { return mPathToRootElements; }

++ 		PX_FORCE_INLINE PxU32	getPathToRootElementCount() const { return mNumPathToRootElements; }

++

++ 		PX_FORCE_INLINE void incrementSolverSpatialDeltaVel(const PxU32 linkID, const Cm::SpatialVectorF& deltaV) {mSolverLinkSpatialDeltaVels[linkID] += deltaV;}

++

++ 						void	setupDofs();

++ 	private:

++ 						PxU32	countDofs()	const;

++ 						void	configureDofs();

++

++ 		Cm::SpatialVectorF							mRootDeferredZ;

++ 		PxReal*										mJointAcceleration;			// joint acceleration

++ 		PxReal*										mJointVelocity;				// joint velocity

++ 		PxReal*										mJointNewVelocity;			// joint velocity due to contacts

++ 		PxReal*										mJointPosition;				// joint position

++ 		PxReal*										mJointForce;				// joint force

++ 		PxReal*										mJointTargetPositions;		// joint target positions

++ 		PxReal*										mJointTargetVelocities;		// joint target velocities

++ 		PxReal*										mPosIterJointVelocities;	// joint delta velocity after postion iternation before velocity iteration

++ 		PxU32										mDofCapacity;	//#### same as mDofs ?

++ 		PxU32										mLinkCapacity;	//####

++

++ 		Cm::SpatialVectorF*										mPosIterMotionVelocities;	//link motion velocites after position iteration before velocity iteration

++ 		Cm::SpatialVectorF*										mMotionVelocities;			//link motion velocites

++ 		Cm::SpatialVectorF*										mSolverLinkSpatialDeltaVels;	//link DeltaVels arising from solver

++ 		Cm::SpatialVectorF*										mSolverLinkSpatialImpulses;	//link impulses arising from solver.

++ 		Cm::SpatialVectorF*										mMotionAccelerations;	//link motion accelerations

++ 		Cm::SpatialVectorF*										mLinkIncomingJointForces;

++ 		Cm::SpatialVectorF*										mMotionAccelerationsInternal;	//link motion accelerations

++ 		Cm::SpatialVectorF*										mCoriolisVectors;		//link coriolis vector

++ 		Cm::SpatialVectorF*										mZAInternalForces;		//link internal spatial forces

++ 		Cm::SpatialVectorF*										mZAForces;				//link spatial zero acceleration force/ spatial articulated force

++ 		Cm::SpatialVectorF*										mJointTransmittedForce;

++

++ 		PxArray<ArticulationInternalConstraint>					mInternalConstraints;

++ 		PxArray<ArticulationInternalLimit>						mInternalLimits;

++ 		PxArray<ArticulationInternalTendonConstraint>			mInternalSpatialTendonConstraints;

++ 		PxArray<ArticulationInternalTendonConstraint>			mInternalFixedTendonConstraints;

++ 	    PxArray<ArticulationInternalMimicJoint>					mInternalMimicJoints;

++

++ 		PxReal*													mDeferredQstZ;

++ 		Cm::SpatialVectorF*						mDeltaMotionVector; //this is for TGS solver

++ 		PxTransform*							mPreTransform; //this is the previous transform list for links

++ 		TestImpulseResponse*					mResponseMatrixW;

++ 		SpatialMatrix*							mWorldSpatialArticulatedInertia;

++ 		PxMat33*								mWorldIsolatedSpatialArticulatedInertia;

++ 		PxReal*									mMasses;

++ 		InvStIs*								mInvStIs;

++ 		Cm::SpatialVectorF*						mIsW;

++ 		PxReal*									qstZIc;//jointForce - stZIc

++ 		PxReal*									qstZIntIc;

++

++ 		Cm::UnAlignedSpatialVector*				mMotionMatrix;

++ 		Cm::UnAlignedSpatialVector*				mWorldMotionMatrix;

++ 		Cm::SpatialVectorF*						mISInvStIS;

++

++ 		PxVec3*									mRw;

++ 		PxU32*									mNbStatic1DConstraints;

++ 		PxU32*									mNbStaticContactConstraints;

++

++ 		PxU32*									mStatic1DConstraintStartIndex;

++ 		PxU32*									mStaticContactConstraintStartIndex;

++

++ 		PxQuat*									mRelativeQuat;

++

++ 		ArticulationLink*						mLinks;

++ 		PxU32									mLinkCount;

++ 		PxU32									mNumPathToRootElements;

++ 		PxU32*									mPathToRootElements;

++ 		ArticulationJointCoreData*				mJointData;

++ 		ArticulationSpatialTendon**				mSpatialTendons;

++ 		PxU32									mNumSpatialTendons;

++ 		PxU32									mNumTotalAttachments;

++ 		ArticulationFixedTendon**				mFixedTendons;

++ 		PxU32									mNumFixedTendons;

++ 		PxU32									mNbMimicJoints;

++ 		ArticulationMimicJointCore**			mMimicJoints;

++ 		PxReal									mDt;

++ 		PxU32									mDofs;

++ 		const PxArticulationFlags*				mFlags;	// PT: PX-1399

++ 		Cm::SpatialVector*						mExternalAcceleration;

++ 		FeatherstoneArticulation*				mArticulation;

++

++ 		PxTransform*							mAccumulatedPoses;

++ 		PxQuat*									mDeltaQ;

++

++ 		SpatialMatrix							mBaseInvSpatialArticulatedInertiaW;

++

++ 		PxReal									mInvSumMass;

++ 		PxVec3									mCOM;

++

++ 		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

++ 		bool									mIsExternalForcesEveryTgsIterationEnabled;

++

++ 		friend class FeatherstoneArticulation;

++ 	};

++

++ 	void ArticulationData::init()

++ 	{

++ 		//zero delta motion vector for TGS solver

++ 		PxMemZero(getDeltaMotionVector(), sizeof(Cm::SpatialVectorF) * mLinkCount);

++ 		PxMemZero(getPosIterMotionVelocities(), sizeof(Cm::SpatialVectorF) * mLinkCount);

++ 		mJointDirty = false;

++ 	}

++

++ 	struct ScratchData

++ 	{

++ 	public:

++ 		ScratchData()

++ 		{

++ 			motionVelocities = NULL;

++ 			motionAccelerations = NULL;

++ 			coriolisVectors = NULL;

++ 			spatialZAVectors = NULL;

++ 			externalAccels = NULL;

++ 			compositeSpatialInertias = NULL;

++

++ 			jointVelocities = NULL;

++ 			jointAccelerations = NULL;

++ 			jointForces = 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*				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_FORCES = 1 << 3,

++ 			eDIRTY_ROOT_TRANSFORM = 1 << 4,

++ 			eDIRTY_ROOT_VELOCITIES = 1 << 5,

++ 			eDIRTY_LINKS = 1 << 6,

++ 			eIN_DIRTY_LIST = 1 << 7,

++ 			eDIRTY_WAKECOUNTER = 1 << 8,

++ 			eDIRTY_EXT_ACCEL = 1 << 9,

++ 			eDIRTY_LINK_FORCE = 1 << 10,

++ 			eDIRTY_LINK_TORQUE = 1 << 11,

++ 			eDIRTY_JOINT_TARGET_VEL = 1 << 12,

++ 			eDIRTY_JOINT_TARGET_POS = 1 << 13,

++ 			eDIRTY_SPATIAL_TENDON = 1 << 14,

++ 			eDIRTY_SPATIAL_TENDON_ATTACHMENT = 1 << 15,

++ 			eDIRTY_FIXED_TENDON = 1 << 16,

++ 			eDIRTY_FIXED_TENDON_JOINT = 1 << 17,

++ 			eDIRTY_MIMIC_JOINT = 1 << 18,

++ 			eDIRTY_USER_FLAGS =  1 << 19,

++ 			eNEEDS_KINEMATIC_UPDATE = 1 << 20,

++ 			eALL = (1<<21)-1

++ 		};

++ 	};

++

++ 	PX_CUDA_CALLABLE PX_FORCE_INLINE float compAng(PxReal swingYZ, PxReal swingW)

++ 	{

++ 		return 4.0f * PxAtan2(swingYZ, 1.0f + swingW);	// tan (t/2) = sin(t)/(1+cos t), so this is the quarter angle

++ 	}

++

++ 	class FeatherstoneArticulation : public PxUserAllocated

++ 	{

++ 		PX_NOCOPY(FeatherstoneArticulation)

++ 	public:

++ 		FeatherstoneArticulation();

++ 		~FeatherstoneArticulation();

++

++ 		void		assignTendons(const PxU32 /*nbTendons*/, Dy::ArticulationSpatialTendon** /*tendons*/);

++

++ 		void		assignTendons(const PxU32 /*nbTendons*/, Dy::ArticulationFixedTendon** /*tendons*/);

++

++ 		void		assignMimicJoints(const PxU32 nbMimicJoints, Dy::ArticulationMimicJointCore** mimicJoints);

++

++ 		PxU32		getDofs()	const;

++

++ 		PxU32		getDof(const PxU32 linkID);

++

++ 		bool		applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, bool& shouldWake);

++

++ 		void		copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, const bool isGpuSimEnabled);

++

++ 		static	PxU32	getCacheDataSize(PxU32 totalDofs, PxU32 linkCount);

++

++ 		static	PxArticulationCache*	createCache(PxU32 totalDofs, PxU32 linkCount);

++

++ 		void		packJointData(const PxReal* maximum, PxReal* reduced);

++

++ 		void		unpackJointData(const PxReal* reduced, PxReal* maximum);

++

++ 		void		initializeCommonData();

++

++ 		//gravity as input, joint force as output

++ 		void		getGeneralizedGravityForce(const PxVec3& gravity, PxArticulationCache& cache);

++

++ 		//joint velocity as input, generalised force(coriolis and centrigugal force) as output

++ 		void		getCoriolisAndCentrifugalForce(PxArticulationCache& cache);

++

++ 		//external force as input, joint force as output

++ 		void		getGeneralizedExternalForce(PxArticulationCache& cache);

++

++ 		//joint force as input, joint acceleration as output

++ 		void		getJointAcceleration(const PxVec3& gravity, PxArticulationCache& cache);

++

++ 		//joint acceleration as input, joint force as out

++ 		void		getJointForce(PxArticulationCache& cache);

++

++ 		void		getDenseJacobian(PxArticulationCache& cache, PxU32 & nRows, PxU32 & nCols);

++

++ 		void		getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache);

++

++ 		bool		getLambda_Deprecated(Cm::PinnableArray<Dy::ConstraintWriteback>& writeBack, ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache, PxArticulationCache& rollBackCache,

++ 			const PxReal* jointTorque, const PxVec3& gravity, const PxU32 maxIter, const PxReal invLengthScale);

++

++ 		void		getGeneralizedMassMatrix(PxArticulationCache& cache);

++

++ 		void		getGeneralizedMassMatrixCRB(PxArticulationCache& cache);

++

++ 		PxVec3		getArticulationCOM(const bool rootFrame);

++

++ 		void		getCentroidalMomentumMatrix(PxArticulationCache& cache);

++

++ 		bool		storeStaticConstraint(const PxSolverConstraintDesc& desc);

++

++ 		bool		willStoreStaticConstraint() { return DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER; }

++

++ 		void		setRootLinearVelocity(const PxVec3& velocity);

++ 		void		setRootAngularVelocity(const PxVec3& velocity);

++ 		void		teleportRootLink();

++

++ 		void		getImpulseResponse(

++ 			PxU32 linkID,

++ 			const Cm::SpatialVector& impulse,

++ 			Cm::SpatialVector& deltaV) const;

++

++ 		void	getImpulseResponse(

++ 			PxU32 linkID,

++ 			const Cm::SpatialVectorV& impulse,

++ 			Cm::SpatialVectorV& deltaV) const;

++

++ 		PX_FORCE_INLINE void getImpulseSelfResponse(

++ 			PxU32 linkID0, PxU32 linkID1,

++ 			const Cm::SpatialVector& impulse0, const Cm::SpatialVector& impulse1,

++ 			Cm::SpatialVector& deltaV0, Cm::SpatialVector& deltaV1) const

++ 		{

++ 			FeatherstoneArticulation::getImpulseSelfResponse(mArticulationData.getLinks(),

++ 				const_cast<Dy::ArticulationData&>(mArticulationData),

++ 				linkID0, impulse0, deltaV0,

++ 				linkID1, impulse1, deltaV1);

++ 		}

++

++ 		//This will return world space SpatialVectorV

++ 		PX_INLINE	Cm::SpatialVectorV	getLinkVelocity(PxU32 linkID) const

++ 		{

++ 			//This is in the world space

++ 			const Cm::SpatialVectorF& motionVelocity = mArticulationData.getMotionVelocity(linkID);

++

++ 			Cm::SpatialVectorV velocity;

++ 			velocity.linear = aos::V3LoadA(motionVelocity.bottom);

++ 			velocity.angular = aos::V3LoadA(motionVelocity.top);

++

++ 			return velocity;

++ 		}

++

++ 		PX_INLINE	Cm::SpatialVector	getLinkScalarVelocity(PxU32 linkID) const

++ 		{

++ 			//This is in the world space

++ 			const Cm::SpatialVectorF& motionVelocity = mArticulationData.getMotionVelocity(linkID);

++

++ 			return Cm::SpatialVector(motionVelocity.bottom, motionVelocity.top);

++ 		}

++

++ 		PX_INLINE	Cm::SpatialVectorV	getLinkMotionVector(PxU32 linkID) const

++ 		{

++ 			const Cm::SpatialVectorF& motionVector = mArticulationData.getDeltaMotionVector(linkID);

++

++ 			Cm::SpatialVectorV velocity;

++ 			velocity.linear = aos::V3LoadA(motionVector.bottom);

++ 			velocity.angular = aos::V3LoadA(motionVector.top);

++

++ 			return velocity;

++ 		}

++

++ 		//this is called by island gen to determine whether the articulation should be awake or sleep

++ 		PX_INLINE	Cm::SpatialVector	getMotionVelocity(PxU32 linkID) const

++ 		{

++ 			//This is in the world space

++ 			const Cm::SpatialVectorF& motionVelocity = mArticulationData.getPosIterMotionVelocities()[linkID];

++ 			return Cm::SpatialVector(motionVelocity.bottom, motionVelocity.top);

++ 		}

++

++ 		Cm::SpatialVector getMotionAcceleration(const PxU32 linkID, const bool isGpuSimEnabled) const;

++

++ 		PX_INLINE	PxReal getLinkMaxPenBias(PxU32 linkID)	const	{ return mArticulationData.getLink(linkID).bodyCore->maxPenBias;	}

++ 		PX_INLINE	PxReal getCfm(PxU32 linkID)				const	{ return mArticulationData.getLink(linkID).cfm;						}

++

++ 		static void computeUnconstrainedVelocities(FeatherstoneArticulation* articulation,

++ 			PxReal dt, const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled, bool tgs);

++

++ 		static void setupSolverConstraintsTGS(FeatherstoneArticulation* articulation,

++ 			PxReal dt, PxReal invDt, PxReal totalDt);

++

++ 		static void saveVelocity(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* deltaV);

++

++ 		static void saveVelocityTGS(FeatherstoneArticulation* articulation, PxReal invDtF32);

++

++ 		static void updateBodies(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* tempDeltaV, PxReal dt, bool integrateJointPosition);

++

++ 		static PX_FORCE_INLINE void updateBodies(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* tempDeltaV, PxReal dt)		{ updateBodies(articulation, tempDeltaV, dt, true);		}

++ 		static PX_FORCE_INLINE void updateBodiesTGS(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* tempDeltaV, PxReal dt)	{ updateBodies(articulation, tempDeltaV, dt, false);	}

++

++ 		static void recordDeltaMotionTGS(FeatherstoneArticulation* articulation, const PxReal dt, Cm::SpatialVectorF* deltaV);

++

++ 		static void applyTgsSubstepForces(FeatherstoneArticulation* articulation, PxReal stepDt, Cm::SpatialVectorF* scratchExtForcesArticulatedYW);

++

++ 		/**

++ 		\brief Apply a spatial impulse to a specified link and optionally a joint impulse to the link's inbound joint.

++ 		The impulses will be propagated from the specified link to the root.

++ 		The function will add to the accumulated impulses {Q - S^T * Z} of each encountered joint as well as to the accumulated root link rootDeferredZ.

++ 		\param[in] linkID is the index of the link that will have a spatial impulse applied to it. The joint impulse

++ 		will be applied to the inbound joint of linkID.

++ 		\param[in] linear is the linear part of the spatial impulse to apply to linkID.

++ 		\param[in] angular is the angular part of the spatial impulse to apply to linkID.

++ 		\param[in] jointImpulse is an array describing the joint impulse to apply to each dof of the inbound joint of linkID.

++ 		\note Set jointImpulse to NULL if the intention is to apply zero joint impulse.

++ 		\note The effect of the link and joint impulse on  the spatial velocity and joint speed associated with any link may be queried by

++ 		calling pxcFsGetVelocity().  This applies the latest state of {Q - S^T * Z} to the pre-solver link spatial velocities and joint speeds.

++ 		*/

++ 		void pxcFsApplyImpulse(PxU32 linkID, aos::Vec3V linear, aos::Vec3V angular, const PxReal* jointImpulse = 0);

++

++ 		/**

++ 		\brief Apply spatial impulses to two specified links and and optionally joint impulses to their inbound joints.

++ 		\note This function has the same outcome as calling pxcFsApplyImpulse(linkID, linear, angular, jointImpulse) and

++ 		pxcFsApplyImpulse(linkID2, linear2, angular2, jointImpulse2) in sequence.

++ 		\see pxcFsApplyImpulse for details.

++ 		*/

++ 		void pxcFsApplyImpulses(

++ 			PxU32 linkID, const aos::Vec3V& linear, const aos::Vec3V& angular, const PxReal* jointImpulse,

++ 			PxU32 linkID2, const aos::Vec3V& linear2, const aos::Vec3V& angular2, const PxReal* jointImpulse2);

++

++ 		/**

++ 		\brief Query the spatial velocity of a link by propagating the deferred root deltaZ from root to link

++ 		and applying the deferred QMinusSTZ for each dof encountered. The joint dof speeds of the inbound joint

++ 		of the link are optionally computed. The computed spatial impulse of the link is the pre-solver

++ 		link velocity incremented by a delta that arises from the application of the deferred impulses.

++ 		\param[in] linkID is the target link.  The propagation from root to link will terminate at the specified link.

++ 		\param[in] jointDofSpeeds are the pre-solver joint dof speeds incremented by a delta that arises from the

++ 		application of the deferred impulses.

++ 		\note jointDofSpeeds may be NULL.

++ 		\note The link and joint velocities are not changed by this query operation.

++ 		\return The spatial velocity that is equal to the pre-solver spatial velocity of the specified link incremented by

++ 		a delta that arises from the application of the deferred root and link impulses.

++ 		*/

++ 		Cm::SpatialVectorV pxcFsGetVelocity(const PxU32 linkID, PxReal* jointDofSpeeds = NULL) const;

++

++ 		void pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1) const;

++

++ 		PX_FORCE_INLINE	const PxQuat& getDeltaQ(PxU32 linkID) const	{ return mArticulationData.mDeltaQ[linkID];	}

++

++ 		/**

++ 		/brief Propagate an acceleration (or velocity) from a parent link to a child link.

++ 		This function assumes no pre-existing knowledge of Q_i - s_i^T * Z_i^A.  If this is  known

++ 		it is recommended to use propagateAccelerationW() instead.

++ 		\param[in] parentToChild is the vector from parent link to child link such that childLinkPos == parentLinkPos + parentToChild

++ 		\param[in] parentLinkDeltaV is the parent link deltaV expressed in the world frame.

++ 		\param[in] spatialInertia is the articulated spatial inertia of the child link.

++ 		\param[in] Z is the impulse to apply to the child link.

++ 		\param[in] joinDofImpulses is an optional of array joint impulses to apply to each dof of the inbound joint of the child link.

++ 		\param[in] motionMatrixW is the Mirtich equivalent of S_i of the child link's inbound joint.

++ 		\param[in] dofCount is the number of dofs on the child links' inbound joint.

++ 		\param[out] jointVelocity is incremented with the change arising from the propagated velocity and applied impulse.

++ 	    \note joinDofImpulses may be NULL if the intention is that zero joint impulse should be propagated.

++ 		\note jointVelocity may be NULL.

++ 		\return The spatial velocity of the child link.

++ 		\note See Mirtich p121 and equations for propagating forces/applying accelerations and

++ 			p141 for propagating velocities/applying impulses.

++ 		*/

++ 		static Cm::SpatialVectorF propagateVelocityW(

++ 			const PxVec3& parentToChild, const Cm::SpatialVectorF& parentLinkDeltaV,

++ 			const Dy::SpatialMatrix& spatialInertia, const Cm::SpatialVectorF& Z,

++ 			const PxReal* joinDofImpulses, const InvStIs& invStIs, const Cm::UnAlignedSpatialVector* motionMatrixW, const PxU32 dofCount,

++ 			PxReal* jointVelocity);

++

++ 		bool applyCacheToDest(ArticulationData& data, PxArticulationCache& cache,

++ 			PxReal* jVelocities, PxReal* jPosition, PxReal* jointForce,

++ 			PxReal* jTargetPositions, PxReal* jTargetVelocities,

++ 			const PxArticulationCacheFlags flag, bool& shouldWake);

++

++ 		PX_FORCE_INLINE	ArticulationData&		getArticulationData()			{ return mArticulationData;	}

++ 		PX_FORCE_INLINE	const ArticulationData&	getArticulationData()	const	{ return mArticulationData;	}

++

++ 		PX_FORCE_INLINE void setGpuDirtyFlag(ArticulationDirtyFlag::Enum flag)

++ 		{

++ 			mGPUDirtyFlags |= flag;

++ 		}

++

++ 		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;

++ 		}

++

++ 	public:

++ 		void constraintPrep(Cm::PinnableArray<Dy::ConstraintWriteback>& writeBack, ArticulationLoopConstraint* lConstraints, PxU32 nbJoints,

++ 			PxSolverConstraintPrepDesc& prepDesc, PxSolverBody& sBody,

++ 			PxSolverBodyData& sBodyData, PxSolverConstraintDesc* desc, PxConstraintAllocator& allocator,

++ 			const PxReal biasCoefficient);

++

++ 		static void updateArticulation(ArticulationData& data, const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled);

++

++ 		static void computeUnconstrainedVelocitiesInternal(ArticulationData& data,

++ 			const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled);

++

++ 		void computeUnconstrainedVelocitiesInternal(const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled)

++ 		{

++ 			// PT: TODO: unclear why the counters are in the data class but these arrays aren't

++ 			//mStaticConstraints.forceSize_Unsafe(0);

++ 			mStatic1DConstraints.forceSize_Unsafe(0);

++ 			mStaticContactConstraints.forceSize_Unsafe(0);

++

++ 			FeatherstoneArticulation::computeUnconstrainedVelocitiesInternal(mArticulationData, gravity, invLengthScale, externalForcesEveryTgsIterationEnabled);

++

++ 			// PT: the solver progress counters will be zeroed during constraint partitioning

++ 		}

++

++ 		//copy joint data from fromJointData to toJointData

++ 		static void copyJointData(const ArticulationData& data, PxReal* toJointData, const PxReal* fromJointData)

++ 		{

++ 			PxMemCopy(toJointData, fromJointData, sizeof(PxReal) * data.getDofs());

++ 		}

++

++ 		//this function calculates motion subspace matrix(s) for all tree joint

++ 		template<bool immediateMode = false>

++ 		void jcalc(ArticulationData& data);

++

++ 		static void computeSpatialInertia(ArticulationData& data);

++

++ 		//compute zero acceleration force

++ 		static void computeZ(const ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData);

++

++ 		void solveInternalConstraints(

++ 			const PxReal dt, const PxReal stepDt, const PxReal invStepDt,

++ 			bool velocityIteration, bool isTGS,

++ 			const ArticulationConstraintProcessingConfigCPU& articulationConstraintProcessingConfig,

++ 			const PxReal elapsedTime, const PxReal biasCoefficient, bool isExternalForcesEveryTgsIterationEnabled = false);

++

++ 		void solveInternalJointConstraints(

++ 			const PxReal dt, const PxReal stepDt, const PxReal invStepDt,

++ 			bool velocityIteration, bool isTGS,

++ 			bool doFrictionDrivePosLimit, ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit, bool doStaticContactAnd1dConstraint,

++ 			const PxReal elapsedTime, const PxReal biasCoefficient, bool isExternalForcesEveryTgsIterationEnabled);

++

++ 	private:

++

++ 		Cm::SpatialVectorF solveInternalJointConstraintRecursive(

++ 			const InternalConstraintSolverData& PX_RESTRICT data, PxU32 linkID,

++ 			const Cm::SpatialVectorF& PX_RESTRICT parentDeltaV, PxU32& dofId, PxU32& limitId);

++ 	public:

++ 		void solveInternalSpatialTendonConstraints(bool isTGS);

++

++ 		void solveInternalFixedTendonConstraints(bool isTGS);

++

++ 		static void setupInternalMimicJointConstraints(ArticulationData& data);

++ 		void solveInternalMimicJointConstraints(const PxReal dt, const PxReal invDt, const bool velocityIteration, const bool isTGS, const PxReal biasCoefficient);

++

++ 		void writebackInternalConstraints(bool isTGS);

++

++ 		void concludeInternalConstraints(bool isTGS);

++

++ 		//compute coriolis force

++ 		static void computeC(const ArticulationData& data, ScratchData& scratchData);

++

++ 		//compute relative transform child to parent

++ 		/**

++ 		\brief	a) copy the latest link pose to a handy array

++ 				b) update the link separation vectors using the latest link poses.

++ 				c) update the motion matrices in the world frame using the latest link poses.

++ 		\param[in] links is an array of articulation links that contain the latest link poses.

++ 		\param[in] linkCount is the number of links in the articulation

++ 		\param[in] jointCoreDatas is an array of joint descriptions

++ 		\param[in] jointDofMotionMatrices is an array of motion matrices in the joint frame.

++ 		\param[in] jointPositions is an array of joint positions (one per dof, indexed by jointOffset).

++ 		\param[out] linkAccumulatedPoses is an array used to store the latest link poses taken from ArticulationLink::PxsBodyCore.

++ 		\param[out] linkRws is an array of link separations.

++ 		\param[out] jointDofmotionMatricesW is an array of motion matrices in the world frame.

++ 		*/

++ 		static void computeRelativeTransformC2P(

++ 			const ArticulationLink* PX_RESTRICT links, PxU32 linkCount, const ArticulationJointCoreData* PX_RESTRICT jointCoreDatas,

++ 			const Cm::UnAlignedSpatialVector* PX_RESTRICT jointDofMotionMatrices, const PxReal* PX_RESTRICT jointPositions,

++ 			PxTransform* PX_RESTRICT linkAccumulatedPoses, PxVec3* PX_RESTRICT linkRws, Cm::UnAlignedSpatialVector* PX_RESTRICT jointDofmotionMatricesW);

++

++ 		static void computeLinkVelocities(ArticulationData& data, ScratchData& scratchData);

++

++ 		/**

++ 		/brief Prepare links for the next timestep.

++ 		\param[in] dt is the timestep of the current simulation step that will advance sim from t to t+dt.

++ 		\param[in] invLengthScale is the reciprocal of the lengthscale used by the simulation.

++ 		\param[in] gravity is the gravitational acceleration to apply to all links.

++ 		\param[in] fixBase determines whether the root link is to be fixed to the world (true) or will move freely (false).

++ 		\param[in] linkCount is the total number of links in the articulation

++ 		\param[in] linkAccumulatedPosesW is the pose of each link, specified in the world frame.

++ 		\param[in,out] linkExternalAccelsW is the external acceleration to apply to each link, specified in the world frame. Only modified if externalForcesEveryTgsIterationEnabled is true.

++ 		\param[in] linkRsW is the vector from each parent link to each child link, specified in the world frame.

++ 		\param[in] jointDofMotionMatricesW is the motion matrix of each dof, specified in the world frame.

++ 		\param[in] jointCoreData is the ArticulationJointCoreData instance of each link in the articulation.

++ 		\param[in] externalForcesEveryTgsIterationEnabled if true, skip the application of external forces in this function and instead prepare change linkExternalAccelsW from acceleration to impulses

++ 		\param[in,out] links is the ArticulationLink instance of each link in the articulation.

++ 		\param[in,out] linkMotionAccelerationsW is the acceleration of each link, specified in the world frame.

++ 		\param[out] linkMotionVelocitiesW is velocity of each link computed from the parent link velocity and joint velocity of the inbound joint. Specified in the world frame.

++ 		\param[out] linkZAForcesExtW is the computed spatial zero acceleration force of each link, accounting for only external forces applied to the links.  Specified in the world frame.

++ 		\param[out] linkZAForcesIntW is the computed spatial zero acceleration force of each link, accounting for only internal forces applied to the links.  Specified in the world frame.

++ 		\param[out] linkCoriolisVectorsW is the computed coriolis vector of each link.   Specified in the world frame.

++ 		\param[out] linkIsolatedArticulatedInertiasW is the inertia tensor (I) for the trivial sub-chain of each link. Specified in the world frame.

++ 		\param[out] linkMasses is the mass of each link.

++ 		\param[out] linkSpatialArticulatedInertiasW is the spatial matrix containing the inertia tensor I and the mass matrix M for the trivial sub-chain of each link.  Specified in the world frame.

++ 		\param[in,out] jointDofVelocities is the velocity of each degree of freedom. Will be updated in case joint velocity limits are violated (see note below).

++ 		\param[out] comW is the centre of mass of the assembly of links, specified in the world frame.

++ 		\param[out] invSumMass is the reciprocal of the total mass of all links.

++ 		\note invLengthScale should have value 1/100 for centimetres scale and 1/1 for metres scale.

++ 		\note If fixBase is true, the root link is assigned zero velocity.  If false, the root link inherits the velocity of the associated body core.

++ 		\note If fixBase is true, the root link is assigned zero acceleration. If false, the acceleration is propagated from the previous simulation step. The acceleration

++ 		of all other links is left unchanged.

++ 		\note If fix base is true, the root link is assigned a zero coriolis vector.

++ 		\note ArticulationLink::cfm of each link is assigned the value PxsBodyCore::cfmScale*invLengthScale, except for the root link. The root link is

++ 		assigned a value of 0 if it is fixed to the world ie fixBase == true.

++ 		\note The spatial zero acceleration force accounts for the external acceleration; the damping force arising from the velocity and from the velocity

++ 		that will accumulate from the external acceleration; the scaling force that will bring velocity back to the maximum allowed velocity if velocity exceeds the maximum allowed.

++ 		\note If the velocity of any degree of freedom exceeds the maximum velocity of the associated joint, the velocity of each degree of freedom will be scaled by a common factor so that none exceeds the maximum.

++ 		*/

++ 		static void computeLinkStates(

++ 			PxF32 dt, PxReal invLengthScale, const PxVec3& gravity, bool fixBase, PxU32 linkCount,

++ 			const PxTransform* PX_RESTRICT linkAccumulatedPosesW, Cm::SpatialVector* PX_RESTRICT linkExternalAccelsW,

++ 			const PxVec3* PX_RESTRICT linkRsW, const Cm::UnAlignedSpatialVector* PX_RESTRICT jointDofMotionMatricesW,

++ 			const Dy::ArticulationJointCoreData* PX_RESTRICT jointCoreData, bool externalForcesEveryTgsIterationEnabled,

++ 			Dy::ArticulationLink* PX_RESTRICT links,

++ 			Cm::SpatialVectorF* PX_RESTRICT linkMotionAccelerationsW, Cm::SpatialVectorF* PX_RESTRICT linkMotionVelocitiesW,

++ 			Cm::SpatialVectorF* PX_RESTRICT linkZAForcesExtW, Cm::SpatialVectorF* PX_RESTRICT linkZAForcesIntW, Cm::SpatialVectorF* PX_RESTRICT linkCoriolisVectorsW,

++ 			PxMat33* PX_RESTRICT linkIsolatedArticulatedInertiasW, PxF32* PX_RESTRICT linkMasses,

++ 			Dy::SpatialMatrix* PX_RESTRICT linkSpatialArticulatedInertiasW,

++ 			PxReal* PX_RESTRICT jointDofVelocities,

++ 			PxVec3& comW, PxF32& invSumMass);

++

++ 		/**

++ 		\brief Propagate articulated z.a. spatial force and articulated spatial inertia from parent link to child link.

++ 		Repeated calls to computePropagateSpatialInertia_ZA_ZIc allow z.a. spatial force and articulated spatial inertia

++ 		to be propagated from tip to root.

++ 		The computation proceeds by considering a link/joint pair composed of a child link and its

++ 		incoming joint.

++ 		The articulated z.a. spatial force is split into an internal and external part.  Gravity is added to the external

++ 		part, while user-applied external joint forces are added to the internal part.

++ 		\note Reference maths can be found in Eq 4.29 in Mirtich thesis.

++ 		\note Mirtich works in the joint frame while every quantity here is in the world frame.

++ 		\note linkArticulatedInertia has equivalent I_i^A in Mirtich

++ 		\note jointMotionMatrix has equivalent s_i and its transpose is s_i^T.

++ 		\param[out] dst is a term to be translated to parent link and added to the articulated inertia of the parent.

++ 		\param[in] jointType is the type of joint

++ 		\param[in] nbJointDofs is the number of dofs supported by the joint.

++ 		\param[in] jointMotionMatricesW is an array of motion matrices with one entry per dof.

++ 		\param[in] jointISW is a cached term linkArticulatedInertia*jointDofMotionMatrix with one entry per dof.

++ 		\param[in] jointTargetArmatures is an array of armature values with one entry per axis.

++ 		\param[in] dofIds is a mapping array which allows to pick out the armature for each joint dof.

++ 		\param[in] jointExternalForces is an array of user-applied external forces applied to the joint with one entry per dof. Can be NULL, in which case zero forces are assumed.

++ 		\param[in] linkArticulatedInertiaW is the articulated inertia of the link.

++ 		\param[in] linkZExtW is the external articulated z.a. force of the link.

++ 		\param[in] linkZIntIcW is the sum of the internal z.a force of the link and linkArticulatedInertia*coriolisForce

++ 		\param[out] linkInvStISW will be computed as 1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		\param[out]	jointDofISInvStISW will be computed as linkArticulatedInertia*jointMotionMatrix^T/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		\param[out] jointDofMinusStZExtW will be computed as [-jointMotionMatrix^T * ZExt]

++ 		\param[out] jointDofQStZIntIcW will be computed as [jointForce - jointMotionMatrix^T *ZIntIc]

++ 		\param[out] deltaZAExtParent is a term that is to be translated to parent link and added to the ZExt value of the parent link.

++ 		\param[out] deltaZAIntIcParent is a term that is to be translated to parent link and added to the ZInt value of the parent link.

++ 		*/

++ 		static void computePropagateSpatialInertia_ZA_ZIc(

++ 			SpatialMatrix& PX_RESTRICT dst, const PxArticulationJointType::Enum jointType, PxU8 nbJointDofs,

++ 			const Cm::UnAlignedSpatialVector* PX_RESTRICT jointMotionMatricesW, const Cm::SpatialVectorF* PX_RESTRICT jointISW,

++ 			const PxReal* PX_RESTRICT jointTargetArmatures, const PxU8* PX_RESTRICT dofIds, const PxReal* PX_RESTRICT jointExternalForces,

++ 			const SpatialMatrix& PX_RESTRICT linkArticulatedInertiaW,

++ 			const Cm::SpatialVectorF& PX_RESTRICT linkZExtW, const Cm::SpatialVectorF& PX_RESTRICT linkZIntIcW,

++ 			InvStIs& PX_RESTRICT linkInvStISW, Cm::SpatialVectorF* PX_RESTRICT jointDofISInvStISW,

++ 			PxReal* PX_RESTRICT jointDofMinusStZExtW, PxReal* PX_RESTRICT jointDofQStZIntIcW,

++ 			Cm::SpatialVectorF& PX_RESTRICT deltaZAExtParent, Cm::SpatialVectorF& PX_RESTRICT deltaZAIntIcParent);

++

++ 		/**

++ 		\brief Propagate articulated z.a. spatial force and articulated spatial inertia from child link to parent link.

++ 		Repeated calls to computePropagateSpatialInertia_ZA_ZIc allow z.a. spatial force and articulated spatial inertia

++ 		to be propagated from tip to root.

++ 		The computation proceeds by considering a link/joint pair composed of a child link and its incoming joint.

++ 		\note Reference maths can be found in Eq 4.29 in Mirtich thesis.

++ 		\note Mirtich works in the joint frame while every quantity here is in the world frame.

++ 		\note linkArticulatedInertia has equivalent I_i^A in Mirtich

++ 		\note jointMotionMatrix has equivalent s_i

++ 		\param[out] dst is a term to be translated to parent link and added to the articulated inertia of the parent.

++ 		\param[in] jointType is the type of joint

++ 		\param[in] nbJointDofs is the number of dofs supported by the joint.

++ 		\param[in] jointMotionMatrices is an array of motion matrices with one entry per dof.

++ 		\param[in] jointIs is a cached term linkArticulatedInertia*jointDofMotionMatrix with one entry per dof.

++ 		\param[in] jointTargetArmatures is an array of armature values with one entry per axis.

++ 		\param[in] dofIds is a mapping array which allows to pick out the armature for each joint dof.

++ 		\param[in] jointExternalForces is an array of user-applied external forces applied to the joint with one entry per dof.

++ 		\param[in] linkArticulatedInertia is the articulated inertia of the link.

++ 		\param[in] ZIc is the sum of the z.a force of the link and linkArticulatedInertia*coriolisForce.

++ 		\param[out] invStIs will be computed as 1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		\param[out]	isInvD will be computed as linkArticulatedInertia*jointMotionMatrix^T/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		\param[out] qstZIc will be computed as [jointForce - jointMotionMatrix^T *ZIc]/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		\param[out] deltaZParent is a term that is to be translated to parent link and added to the articulated z.a force of the parent link.

++ 		*/

++ 		static void computePropagateSpatialInertia_ZA_ZIc_NonSeparated(

++ 			SpatialMatrix& PX_RESTRICT dst, const PxArticulationJointType::Enum jointType, PxU8 nbJointDofs,

++ 			const Cm::UnAlignedSpatialVector* PX_RESTRICT jointMotionMatrices, const Cm::SpatialVectorF* PX_RESTRICT jointIs,

++ 			const PxReal* PX_RESTRICT jointTargetArmatures, const PxU8* PX_RESTRICT dofIds,

++ 			const PxReal* PX_RESTRICT jointExternalForces, const SpatialMatrix& PX_RESTRICT linkArticulatedInertia,

++  			const Cm::SpatialVectorF& PX_RESTRICT ZIc,

++ 			InvStIs& PX_RESTRICT invStIs, Cm::SpatialVectorF* PX_RESTRICT isInvD,

++ 			PxReal* PX_RESTRICT qstZIc, Cm::SpatialVectorF& PX_RESTRICT deltaZParent);

++

++ 		/*

++ 		\brief Propagate articulated spatial inertia (but not the  articulated z.a. spatial force) from child link to parent link.

++ 		Repeated calls to computePropagateSpatialInertia allow the articulated spatial inertia

++ 		to be propagated from tip to root.

++ 		The computation proceeds by considering a link/joint pair composed of a child link and its

++ 		incoming joint.

++ 		\param[out] dst is a term to be translated to parent link and added to the articulated inertia of the parent.

++ 		\param[in] jointType is the type of joint

++ 		\param[in] nbJointDofs is the number of dofs supported by the joint.

++ 		\param[in] linkArticulatedInertia is the articulated inertia of the link.

++ 		\param[in] jointMotionMatrices is an array of motion matrices with one entry per dof.

++ 		\param[in] jointIs is a cached term linkArticulatedInertia*jointDofMotionMatrix with one entry per dof.

++ 		\param[out] invStIs will be computed as 1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		\param[out]	isInvD will be computed as linkArticulatedInertia*jointMotionMatrix^T/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		*/

++ 		static void computePropagateSpatialInertia(SpatialMatrix& PX_RESTRICT dst,

++ 			const PxArticulationJointType::Enum jointType, PxU8 nbDofs,

++ 			const SpatialMatrix& PX_RESTRICT linkArticulatedInertia, const Cm::UnAlignedSpatialVector* PX_RESTRICT jointMotionMatrices,

++ 			const Cm::SpatialVectorF* PX_RESTRICT jointIs, InvStIs& PX_RESTRICT invStIs, Cm::SpatialVectorF* PX_RESTRICT isInvD);

++

++ 		static void translateInertia(const PxMat33& offset, SpatialMatrix& inertia);

++

++ 		/*

++ 		\brief Propagate articulated spatial inertia and articulated z.a. spatial force from tip to root.

++ 		\param[in] links is an array of articulation links with size denoted by linkCount.

++ 		\param[in] linkCount is the number of articulation links.

++ 		\param[in] linkRsW is an array of link separations in the world frame with one entry per link.

++ 		\param[in] jointData is an array of joint descriptions with one entry per joint.

++ 		\param[in] jointDofMotionMatricesW ins an array of motion matrices in the world frame with one entry per dof.

++ 		\param[in] linkCoriolisVectorsW is an array fo coriolis terms with one entry per link.

++ 		\param[in] jointDofForces is an array of joint-space forces/torques to be applied to joints with one entry per dof. Can be NULL, in which case zero force is assumed for all joints.

++ 		\param[out] jointDofIsW is a cached term linkArticulatedInertia*jointDofMotionMatrix to be computed with one entry per dof.

++ 		\param[out] linkInvStIsW will be computed as 1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix] with one entry per link.

++ 		\param[out] jointDofISInvStIS will be computed as linkArticulatedInertia*jointMotionMatrix^T/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix] with one entry per dof.

++ 		\param[out] joIntDofMinusStZExtW will be computed as [-jointMotionMatrix^T * ZExt] with one entry per dof.

++ 		\param[out] jointDofQStZIntIcW will be computed as [jointForce - jointMotionMatrix^T *ZIntIc] with one entry per dof.

++ 		\param[in,out] linkZAExtForcsW is the articulated z.a spatial force of each link arising from external forces.

++ 		\param[in,out] linkZAIntForcesW is the articulated z.a spatial force of each link arising from internal forces.

++ 		\param[in,out] linkSpatialArticulatedInertiaW is the articulated spatial inertia of each link.

++ 		\param[out] baseInvSpatialArticulatedInertiaW is the inverse of the articulated spatial inertia of the root link.

++ 		*/

++ 		static void computeArticulatedSpatialInertiaAndZ(

++ 			const ArticulationLink* PX_RESTRICT links, PxU32 linkCount, const PxVec3* PX_RESTRICT linkRsW,

++ 			const ArticulationJointCoreData* PX_RESTRICT jointData,

++ 			const Cm::UnAlignedSpatialVector* PX_RESTRICT jointDofMotionMatricesW,

++ 			const Cm::SpatialVectorF* PX_RESTRICT linkCoriolisVectorsW, const PxReal* PX_RESTRICT jointDofForces,

++ 			Cm::SpatialVectorF* PX_RESTRICT jointDofIsW, InvStIs* PX_RESTRICT linkInvStIsW, Cm::SpatialVectorF* PX_RESTRICT jointDofISInvStIS,

++ 			PxReal* PX_RESTRICT joIntDofMinusStZExtW, PxReal* PX_RESTRICT jointDofQStZIntIcW,

++ 			Cm::SpatialVectorF* PX_RESTRICT linkZAExtForcesW, Cm::SpatialVectorF* PX_RESTRICT linkZAIntForcesW, SpatialMatrix* PX_RESTRICT linkSpatialArticulatedInertiaW,

++ 			SpatialMatrix& baseInvSpatialArticulatedInertiaW);

++

++ 		static void computeArticulatedSpatialInertiaAndZ_NonSeparated(ArticulationData& data, ScratchData& scratchData);

++

++ 		// PT: TODO: move local to inverse dynamics file

++ 		static void computeArticulatedSpatialInertia(ArticulationData& data);

++

++ 		/*

++ 		\brief Compute the response matrix of each link of an articulation.

++ 		\param[in] articulationFlags describes whether the articulation has a fixed base.

++ 		\param[in] linkCount is the number of links in the articulation.

++ 		\param[in] jointData is an array of joint descriptions with one entry per joint.

++ 		\param[in] baseInvSpatialArticulatedInertiaW is the inverse of the articulated spatial inertia of the root link.

++ 		\param[in] linkRsW is an array of link separations (childPos - parentPos) in the world frame with one entry per link.

++ 		\param[in] jointDofMotionMatricesW is an array of motion matrices with one entry per dof.

++ 		\param[in] jointDofISW is a cached term linkArticulatedInertia*jointDofMotionMatrix to be computed with one entry per dof.

++ 		\param[in] linkInvStISW will be computed as 1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix] with one entry per link.

++ 		\param[in] jointDofIsInvDW will be computed as linkArticulatedInertia*jointMotionMatrix^T/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix] with one entry per dof.

++ 		\param[out] links is an array of articulation links with one entry per link.  The cfm value of each link will be updated.

++ 		\param[out] linkResponsesW if an array of link responses with one entry per link.

++ 		*/

++ 		static void computeArticulatedResponseMatrix(

++ 			const PxArticulationFlags articulationFlags, PxU32 linkCount,

++ 			const ArticulationJointCoreData* PX_RESTRICT jointData,

++ 			const SpatialMatrix& PX_RESTRICT baseInvArticulatedInertiaW,

++ 			const PxVec3* PX_RESTRICT linkRsW, const Cm::UnAlignedSpatialVector* PX_RESTRICT jointDofMotionMatricesW,

++ 			const Cm::SpatialVectorF* PX_RESTRICT jointDofISW, const InvStIs* PX_RESTRICT linkInvStISW, const Cm::SpatialVectorF* PX_RESTRICT jointDofIsInvDW,

++ 			ArticulationLink* PX_RESTRICT links, TestImpulseResponse* PX_RESTRICT linkResponsesW);

++

++ 		// PT: TODO: move local to inverse dynamics file

++ 		static void computeArticulatedSpatialZ(ArticulationData& data, ScratchData& scratchData);

++

++ 		/**

++ 		\brief Compute the joint acceleration

++ 		\note Reference maths found in Eq 4.27 of Mirtich thesis.

++ 		\param[in] pMotionAcceleration is the acceleration of the parent link already transformed into the (child) link frame.

++ 		\param[in] jointDofISW is an array of cached terms linkArticulatedInertia*jointDofMotionMatrix with one entry per dof.

++ 		\param[in] linkInvStISW is a cached term equivalent to 1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix]

++ 		\param[in] jointDofQStZIcW is an array of cached terms equivlaent to

++ 					[jointExternalForce - jointDofMotionMatrix^T * (zeroAccelSpatialForce + spatialInertia*coriolisForce] with one entry per dof.

++ 		\param[out] jointAcceleration is an array of output joint dof accelerations equivalent to Eq 4.27 in Mirtich thesis.

++ 		*/

++ 		static void computeJointAccelerationW(PxU8 nbJointDofs,

++ 			const Cm::SpatialVectorF& PX_RESTRICT pMotionAcceleration, const Cm::SpatialVectorF* PX_RESTRICT jointDofISW,

++ 			const InvStIs& PX_RESTRICT linkInvStISW, const PxReal* PX_RESTRICT jointDofQStZIcW, PxReal* PX_RESTRICT jointAcceleration);

++

++ 		//compute joint acceleration, joint velocity and link acceleration, velocity based

++ 		//on spatial force and spatial articulated inertia tensor

++

++ 		/**

++ 		\brief Compute joint and link accelerations.

++ 		Accelerations are computed by iterating from root to tip using the formulae in Mirtich Figure 4.8 to compute first

++ 		the joint dof acceleration and then the link acceleration.

++ 		The accelerations are used to forward integrate the link and joint velocities.

++ 		This function may be used to determine either the effect of external forces only or the effect of the external and internal forces combined.

++ 		The function may not be used to determine the effect of internal forces.  For internal forces only use computeLinkInternalAcceleration().

++ 		If external forces only are to be considered then set doIC to false to avoid adding the Coriolis vector to the link acceleration.  This is important

++ 		because Coriolis forces are accounted as part of the update arising from internal forces.

++ 		\param[in] doIC determines whether the link Coriolis force is added to the link acceleration.

++ 			Set to false if considering external forces only and true if considering the combination of internal and external forces.

++ 		\param[in] dt is the timestep used to accumulate joint/link velocities from joint/link accelerations.

++         \param[in] fixBase describes whether the root of the articulation is fixed or free to rotate and translate.

++         \param[in] links is an array of articulation links with one entry for each link.

++         \param[in] linkCount is the number of links in the articulation.

++         \param[in] jointDatas is an array of joint descriptions with one entry per joint.

++         \param[in] linkSpatialZAForcesW is an array of spatial z.a. forces arising from the forces acting on each link with one entry for each link.

++ 			linkSpatialZAForces will either be internal z.a forces or the sum of internal and external forces.

++         \param[in] linkCoriolisForcesW is an array of coriolis forces with one entry for each link.

++         \param[in] linkRsW is an array of link separations with one entry for each link.

++         \param[in] jointDofMotionMatricesW is an array of motion matrices with one entry per joint dof.

++         \param[in] baseInvSpatialArticulatedInertiaW is the inverse of the articulated spatial inertia of the root link.

++    		\param[in] linkInvStISW is  1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix] with one entry per link.

++         \param[in] jointDofISW linkArticulatedInertia*jointMotionMatrix^T/ with one entry per joint dof.

++         \param[in] jointDofQStZIcW has one of two forms:

++ 				a) [-jointDofMotionMatrix^T * linkSpatialZAForceExternal]

++ 				b) [jointDofForce - jointDofMotionMatrix^T*(linkSpatialZAForceTotal + linkSpatialInertia*linkCoriolisForce)]

++ 				with one entry for each joint dof.

++ 		\param[out] linkMotionAccelerationsW is an array of link accelerations with one entry per link. The link accelerations are computed

++ 				using the formula in Figure 4.8 of the Mirtich thesis.

++ 		\param[in,out] linkMotionVelocitiesW is an array of link velocities that are forward integrated by dt using the link accelerations.

++ 		\param[out]  jointDofAccelerations is an array of joint dof accelerations with one entry per link. The joint dof accelerations are computed

++ 				using the formula in Figure 4.8 of the Mirtich thesis.

++ 		\param[in,out] jointDofVelocities is an array of joint dof velocities that are forward integrated by dt using the joint dof accelerations.

++ 		\param[out] jointDofNewVelocities is another array of joint dof velocities that are forward integrated by dt using the joint dof accelerations.

++ 		\note If doIC is false then linkSpatialZAForces must be the external z.a. forces and jointDofQstZics must be [-jointDofMotionMatrix^T * linkSpatialZAForceExternal]

++ 		\note If doIC is true then  linkSpatialZAForces must be the internal z.a. forces and jointDofQstZics must be [jointDofForce - jointDofMotionMatrix^T*(linkSpatialZAForceTotal + linkSpatialInertia*linkCoriolisForce)]

++ 		*/

++ 		static void computeLinkAcceleration(

++ 			bool doIC, PxReal dt, bool fixBase,

++ 			const ArticulationLink* PX_RESTRICT links, PxU32 linkCount, const ArticulationJointCoreData* PX_RESTRICT jointDatas,

++ 			const Cm::SpatialVectorF* PX_RESTRICT linkSpatialZAForcesW, const Cm::SpatialVectorF* PX_RESTRICT linkCoriolisForcesW, const PxVec3* PX_RESTRICT linkRsW,

++ 			const Cm::UnAlignedSpatialVector* PX_RESTRICT jointDofMotionMatricesW,

++ 			const SpatialMatrix& PX_RESTRICT baseInvSpatialArticulatedInertiaW,

++ 			const InvStIs* PX_RESTRICT linkInvStISW,

++ 			const Cm::SpatialVectorF* PX_RESTRICT jointDofISW, const PxReal* PX_RESTRICT jointDofQStZIcW,

++ 			Cm::SpatialVectorF* PX_RESTRICT linkMotionAccelerationsW, Cm::SpatialVectorF* PX_RESTRICT linkMotionVelocitiesW,

++ 			PxReal* PX_RESTRICT jointDofAccelerations, PxReal* PX_RESTRICT jointDofVelocities, PxReal* PX_RESTRICT jointDofNewVelocities);

++

++ 		/**

++ 		\brief Compute joint and link accelerations arising from internal z.a. forces.

++ 		Accelerations are computed by iterating from root to tip using the formulae in Mirtich Figure 4.8 to compute first

++ 		the joint dof acceleration and then the link acceleration.

++ 		The accelerations are used to forward integrate the link and joint velocities.

++ 		The resulting link velocities are rescaled if any link violates the maximum allowed linear or angular velocity.

++ 		\param[in] dt is the timestep used to accumulate joint/link velocities from joint/link accelerations.

++         \param[in] fixBase describes whether the root of the articulation is fixed or free to rotate and translate.

++ 		\param[in] comW is the centre of mass of the ensemble of links in the articulation. com is used only to enforce the max linear and angular velocity.

++ 		\param[in] invSumMass is the inverse of the mass sum of the ensemble of links in the articulation. invSumMass is used only to enforce the max linear and angular velocity.

++ 		\param[in] linkIsolatedSpatialArticulatedInertiasW is an array of link inertias.  The link inertias are used only to enforce the max linear and angular velocity.

++         \param[in] baseInvSpatialArticulatedInertiaW is the inverse of the articulated spatial inertia of the root link.

++         \param[in] links is an array of articulation links with one entry for each link.

++         \param[in] linkCount is the number of links in the articulation.

++ 		\param[in] linkMasses is an array of link masses with one entry per link.

++ 		\param[in] linkRsW is an array of link separations with one entry per link.

++ 		\param[in] linkAccumulatedPosesW is an array of link poses with one entry per link.

++ 		\param[in] linkSpatialZAIntForcesW is an array of spatial z.a. forces arising from internal forces only with one netry per link.

++ 		\param[in] linkCoriolisVectorsW is an array of link Coriolis forces with one entry per link.

++ 		\param[in] jointDatas is an array of joint descriptions with one entry per joint.

++ 		\param[in] jointDofMotionMatricesW is an array of motion matrices with one entry per dof.

++    		\param[in] linkInvStISW is  1/[jointMotionMatrix^T * linkArticulatedInertia * jointMotionMatrix] with one entry per link.

++         \param[in] jointDofISW linkArticulatedInertia*jointMotionMatrix^T with one entry per joint dof.

++         \param[in] jointDoQStZIntIcW has form: [jointDofForce - jointDofMotionMatrix^T*(linkSpatialZAForceInternal + linkSpatialInertia*linkCoriolisForce)]

++ 				with one entry for each joint dof.

++ 		\param[in,out] linkMotionAccelerationsW accumulates with the computed acceleration arising from internal forces.

++ 		\param[out] linkMotionAccelerationIntsW is the computed acceleration arising from internal forces.

++

++ 		// PT: TODO: these new params appeared but were not documented

++ 		linkMotionVelocitiesW

++ 		jointDofAccelerations

++

++ 		\param[in,out] jointDofVelocities is an array of joint dof velocities that are forward integrated by dt using the joint dof accelerations arising from internal forces.

++ 		\param[out] jointDofNewVelocities is another array of joint dof velocities that are forward integrated by dt using the joint dof accelerations arising from internal forces.

++ 		\note computeLinkInternalAcceleration must be called after computeLinkAcceleration to allow the effect of internal forces to be accumulated on top of external forces.

++ 		*/

++ 		static void computeLinkInternalAcceleration

++ 			(const PxReal dt,

++ 			 const bool fixBase,

++ 			 const PxVec3& comW, const PxReal invSumMass, const PxMat33* linkIsolatedSpatialArticulatedInertiasW,

++ 			 const SpatialMatrix& baseInvSpatialArticulatedInertiaW,

++ 			 const ArticulationLink* links, const PxU32 linkCount,

++ 			 const PxReal* linkMasses, const PxVec3* linkRsW, const PxTransform* linkAccumulatedPosesW,

++ 			 const Cm::SpatialVectorF* linkSpatialZAIntForcesW, const Cm::SpatialVectorF* linkCoriolisVectorsW,

++ 			 const ArticulationJointCoreData* jointDatas, const Cm::UnAlignedSpatialVector* jointDofMotionMatricesW,

++ 			 const InvStIs* linkInvStISW, const Cm::SpatialVectorF* jointDofISW, const PxReal* jointDoQStZIntIcW,

++ 			 Cm::SpatialVectorF* linkMotionAccelerationsW, Cm::SpatialVectorF* linkMotionAccelerationIntsW, Cm::SpatialVectorF* linkMotionVelocitiesW,

++ 			 PxReal* jointDofAccelerations, PxReal* jointDofVelocities, PxReal* jointDofNewVelocities);

++

++ 		static void computeJointTransmittedFrictionForce(const ArticulationData& data, Cm::SpatialVectorF* PX_RESTRICT transmittedForce);

++

++ 		static Cm::SpatialVectorF getDeltaVWithDeltaJV(const bool fixBase, const PxU32 linkID,

++ 			const ArticulationData& data, Cm::SpatialVectorF* Z,

++ 			PxReal* jointVelocities);

++

++ 		//impulse need to be in the linkID space

++ 		static void getZ(const PxU32 linkID, const ArticulationData& data,

++ 			Cm::SpatialVectorF* Z, const Cm::SpatialVectorF& impulse);

++

++ 		//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(const 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* Z);

++

++ 		Cm::SpatialVectorF getImpulseResponseInv(const bool fixBase,

++ 			const PxU32 linkID, Cm::SpatialVectorF* Z,

++ 			const Cm::SpatialVector& impulse,

++ 			PxReal* jointVelocites);

++

++ 		static void inverseDynamic(ArticulationData& data, const PxVec3& gravity,

++ 			ScratchData& scratchData, bool computeCoriolis);

++

++ 		static void inverseDynamicFloatingBase(ArticulationData& data, const PxVec3& gravity,

++ 			ScratchData& scratchData, bool computeCoriolis);

++

++ 		//compute link body force with motion velocity and acceleration

++ 		static void computeZAForceInv(const ArticulationData& data, const Cm::SpatialVectorF* PX_RESTRICT motionAccelerations, Cm::SpatialVectorF* PX_RESTRICT biasForce);

++ 		static void initCompositeSpatialInertia(const ArticulationData& data, Dy::SpatialMatrix* compositeSpatialInertia);

++ 		static void computeCompositeSpatialInertiaAndZAForceInv(const ArticulationData& data, ScratchData& scratchData);

++

++ 		static void computeRelativeGeneralizedForceInv(const ArticulationData& data, ScratchData& scratchData, const bool coriolisFloatingBase = false);

++

++ 		//compute link acceleration due to external forces, applied external accelerations and Coriolis force

++ 		static void computeLinkAccelerationInv(const ArticulationData& data, ScratchData& scratchData);

++

++ 		static void computeGeneralizedForceInv(const ArticulationData& data, ScratchData& scratchData);

++

++ 		void calculateMassMatrixColInv(ScratchData& scratchData);

++

++ 		void calculateHFixBase(PxArticulationCache& cache);

++

++ 		void calculateHFloatingBase(PxArticulationCache& cache);

++

++ 		public:

++

++ 			PX_FORCE_INLINE	void				addBody()						{ mAcceleration.pushBack(Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f)));	}

++

++ 			PX_FORCE_INLINE PxU32				getBodyCount()			const	{ return mArticulationData.mLinkCount;	}

++ 			PX_FORCE_INLINE ArticulationCore*	getCore()						{ return mSolverDescCore;		}

++ 			PX_FORCE_INLINE PxU16				getIterationCounts()	const	{ return mSolverDescCore->solverIterationCounts;	}

++

++ 			void	setupLinks(PxU32 nbLinks, Dy::ArticulationLink* links);

++ 			void	initPathToRoot();

++

++ 		static void getImpulseSelfResponse(const ArticulationLink* links,

++ 			ArticulationData& data,

++ 			PxU32 linkID0, const Cm::SpatialVector& impulse0, Cm::SpatialVector& deltaV0,

++ 			PxU32 linkID1, const Cm::SpatialVector& impulse1, Cm::SpatialVector& deltaV1);

++

++ 		static void getImpulseResponseSlow(const Dy::ArticulationLink* links,

++ 			ArticulationData& data,

++ 			PxU32 linkID0_,

++ 			const Cm::SpatialVector& impulse0,

++ 			Cm::SpatialVector& deltaV0,

++ 			PxU32 linkID1_,

++ 			const Cm::SpatialVector& impulse1,

++ 			Cm::SpatialVector& deltaV1);

++

++ 		static void setupSolverConstraints(

++ 			ArticulationData& data);

++

++ 		static void setupInternalConstraints(

++ 			ArticulationData& data,

++ 			PxReal stepDt,

++ 			PxReal dt,

++ 			PxReal invDt,

++ 			bool isTGSSolver);

++

++ 		static void setupInternalConstraintsRecursive(

++ 			const ArticulationLink* links,

++ 			ArticulationData& data,

++ 			const PxReal stepDt,

++ 			const PxReal dt,

++ 			const bool isTGSSolver,

++ 			const PxU32 linkID,

++ 			const PxReal maxForceScale);

++

++ 		static void setupInternalSpatialTendonConstraintsRecursive(

++ 			const ArticulationLink* links,

++ 			ArticulationAttachment* attachments,

++ 			const PxU32 attachmentCount,

++ 			const PxVec3& parentAttachmentPoint,

++ 			const bool fixBase,

++ 			ArticulationData& data,

++ 			const PxReal stepDt,

++ 			const bool isTGSSolver,

++ 			const PxU32 attachmentID,

++ 			const PxReal stiffness,

++ 			const PxReal damping,

++ 			const PxReal limitStiffness,

++ 			const PxReal err,

++ 			const PxU32 startLink,

++ 			const PxVec3& startAxis,

++ 			const PxVec3& startRaXn);

++

++ 		static void setupInternalFixedTendonConstraintsRecursive(

++ 			const ArticulationLink* links,

++ 			ArticulationTendonJoint* tendonJoints,

++ 			const bool fixBase,

++ 			ArticulationData& data,

++ 			const PxReal stepDt,

++ 			const bool isTGSSolver,

++ 			const PxU32 tendonJointID,

++ 			const PxReal stiffness,

++ 			const PxReal damping,

++ 			const PxReal limitStiffness,

++ 			const PxU32 startLink,

++ 			const PxVec3& startAxis,

++ 			const PxVec3& startRaXn);

++

++ 		void updateSpatialTendonConstraintsRecursive(

++ 			const ArticulationAttachment* attachments, const PxU32 attachmentID,

++ 			const PxReal accumLength, const PxVec3& pAttachPoint,

++ 			ArticulationData& data);

++

++ 		//void updateFixedTendonConstraintsRecursive(ArticulationLink* links, ArticulationTendonJoint* tendonJoint, ArticulationData& data, const PxU32 tendonJointID, const PxReal accumErr);

++

++ 		PxVec3 calculateFixedTendonVelocityAndPositionRecursive(

++ 			const ArticulationTendonJoint* tendonJoints, const PxU32 tendonJointID,

++ 			const Cm::SpatialVectorF& parentV, const Cm::SpatialVectorF& parentDeltaV);

++

++ 		Cm::SpatialVectorF solveFixedTendonConstraintsRecursive(

++ 			const ArticulationTendonJoint* tendonJoints,const PxU32 tendonJointID,

++ 			const PxReal lengthError, const PxReal limitError, const PxReal rootVel,

++ 			 PxReal& rootImp);

++

++ 		void prepareStaticConstraints(const PxReal dt, const PxReal invDt, PxsContactManagerOutputIterator& outputs,

++ 			Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold,

++ 			PxReal ccdMaxSeparation, const PxReal rigidContactBiasCoefficient, const PxReal jointBiasCoefficient,

++ 			PxSolverBodyData* solverBodyData, PxsConstraintBlockManager& blockManager,

++ 			Dy::ConstraintWriteback* constraintWritebackPool);

++

++ 		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 PxReal rigidContactBiasCoefficient, const PxReal jointBiasCoefficient,

++ 			const PxReal lengthScale);

++

++ 		//integration

++ 		static void propagateLinksDown(ArticulationData& data, const PxReal* jointVelocities, PxReal* jointPositions,

++ 			Cm::SpatialVectorF* motionVelocities);

++

++ 		void updateJointProperties(

++ 			const PxReal* jointNewVelocities,

++ 			PxReal* jointVelocities,

++ 			PxReal* jointAccelerations);

++

++ 		static void computeAndEnforceJointPositions(ArticulationData& data);

++

++ 		//update link position based on joint position provided by the cache

++ 		static void teleportLinks(ArticulationData& data);

++

++ 		static void computeLinkVelocities(ArticulationData& data);

++

++ 		PxU8* allocateScratchSpatialData(PxcScratchAllocator* allocator,

++ 			const PxU32 linkCount, ScratchData& scratchData, bool fallBackToHeap = false);

++

++ 		//This method calculate the velocity change from parent to child using parent current motion velocity

++ 		PxTransform propagateTransform(const PxU32 linkID, const ArticulationLink* links, const ArticulationJointCoreData& jointDatum,

++ 			const Cm::SpatialVectorF* motionVelocities, const PxReal dt, const PxTransform& pBody2World, const PxTransform& currentTransform,

++ 			PxReal* jointVelocity, PxReal* jointPosition, const Cm::UnAlignedSpatialVector* motionMatrix);

++

++ 		static void updateRootBody(const Cm::SpatialVectorF& motionVelocity,

++ 			const PxTransform& preTransform, ArticulationData& data, const PxReal dt);

++

++ 		//These variables are used in the constraint partition

++ 		PxU16							mMaxSolverFrictionProgress;

++ 		PxU16							mMaxSolverNormalProgress;

++ 		PxU32							mSolverProgress;

++ 		PxU16							mArticulationIndex;

++

++ 		ArticulationCore*				mSolverDescCore;

++ 		const PxArticulationFlags*		mSolverDescFlags;	// PT: PX-1399

++ 		void	initData(ArticulationCore* core_, const PxArticulationFlags* flags_)

++ 		{

++ 			mSolverDescCore		= core_;

++ 			mSolverDescFlags	= flags_;

++ 		}

++

++ 		PxArray<Cm::SpatialVector>		mAcceleration;		// supplied by Sc-layer to feed into articulations

++

++ 		ArticulationData				mArticulationData;

++

++ 		PxArray<PxSolverConstraintDesc> mStaticContactConstraints;

++ 		PxArray<PxSolverConstraintDesc> mStatic1DConstraints;

++ 		PxU32							mGPUDirtyFlags;

++ 		bool							mJcalcDirty;

++ };

++

++ #if PX_VC

++ #pragma warning(pop)

++ #endif

++

++ } //namespace Dy

++

++ }

++

++ #endif

Revision control