1554 lines
81 KiB
C++
1554 lines
81 KiB
C++
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#ifndef DY_FEATHERSTONE_ARTICULATION_H
|
|
#define DY_FEATHERSTONE_ARTICULATION_H
|
|
|
|
#include "foundation/PxVec3.h"
|
|
#include "foundation/PxQuat.h"
|
|
#include "foundation/PxTransform.h"
|
|
#include "foundation/PxVecMath.h"
|
|
#include "CmUtils.h"
|
|
#include "DyVArticulation.h"
|
|
#include "DyFeatherstoneArticulationUtils.h"
|
|
#include "DyFeatherstoneArticulationJointData.h"
|
|
#include "solver/PxSolverDefs.h"
|
|
#include "DyArticulationTendon.h"
|
|
#include "DyCpuGpuArticulation.h"
|
|
#include "CmSpatialVector.h"
|
|
#include "DyResidualAccumulator.h"
|
|
|
|
#ifndef FEATHERSTONE_DEBUG
|
|
#define FEATHERSTONE_DEBUG 0
|
|
#endif
|
|
|
|
#define DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER true
|
|
|
|
namespace physx
|
|
{
|
|
|
|
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 ArticulationLinkData;
|
|
struct ArticulationMimicJointCore;
|
|
struct SpatialSubspaceMatrix;
|
|
struct SolverConstraint1DExt;
|
|
struct SolverConstraint1DStep;
|
|
|
|
class FeatherstoneArticulation;
|
|
struct SpatialMatrix;
|
|
struct SpatialTransform;
|
|
struct Constraint;
|
|
class ThreadContext;
|
|
struct InternalConstraintSolverData;
|
|
|
|
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;
|
|
PxReal velImpulseMultiplier;
|
|
PxReal limitVelImpulseMultiplier;
|
|
};
|
|
|
|
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 frictionMaxForce;
|
|
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;
|
|
};
|
|
|
|
|
|
struct PX_ALIGN_PREFIX(16) JointSpaceSpatialZ
|
|
{
|
|
PxReal mVals [6][4];
|
|
|
|
PxReal dot(Cm::SpatialVectorF& v, PxU32 id)
|
|
{
|
|
return v.top.x * mVals[0][id] + v.top.y * mVals[1][id] + v.top.z * mVals[2][id]
|
|
+ v.bottom.x * mVals[3][id] + v.bottom.y * mVals[4][id] + v.bottom.z * mVals[5][id];
|
|
}
|
|
}
|
|
PX_ALIGN_SUFFIX(16);
|
|
|
|
class ArticulationData
|
|
{
|
|
public:
|
|
|
|
ArticulationData() :
|
|
mPathToRootElements(NULL), mNumPathToRootElements(0), mLinksData(NULL), mJointData(NULL),
|
|
mSpatialTendons(NULL), mNumSpatialTendons(0), mNumTotalAttachments(0),
|
|
mFixedTendons(NULL), mNumFixedTendons(0),
|
|
mMimicJoints(NULL), mNbMimicJoints(0), mDt(0.f),mDofs(0xffffffff),
|
|
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* getJointNewVelocities() { return mJointNewVelocity.begin(); }
|
|
PX_FORCE_INLINE const PxReal* getJointNewVelocities() const { return mJointNewVelocity.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* getJointTargetPositions() { return mJointTargetPositions.begin(); }
|
|
PX_FORCE_INLINE const PxReal* getJointTargetPositions() const { return mJointTargetPositions.begin(); }
|
|
PX_FORCE_INLINE PxReal* getJointTargetVelocities() { return mJointTargetVelocities.begin(); }
|
|
PX_FORCE_INLINE const PxReal* getJointTargetVelocities() const { return mJointTargetVelocities.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 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* getLinkIncomingJointForces() { return mLinkIncomingJointForces.begin(); }
|
|
PX_FORCE_INLINE Cm::SpatialVectorF* getCorioliseVectors() { return mCorioliseVectors.begin(); }
|
|
PX_FORCE_INLINE Cm::SpatialVectorF* getSpatialZAVectors() { return mZAForces.begin(); }
|
|
PX_FORCE_INLINE Cm::SpatialVectorF* getSpatialZAInternalVectors() { return mZAInternalForces.begin(); }
|
|
PX_FORCE_INLINE Cm::SpatialVectorF* getTransmittedForces() { return mJointTransmittedForce.begin(); }
|
|
|
|
PX_FORCE_INLINE Cm::SpatialVectorF* getPosIterMotionVelocities() { return mPosIterMotionVelocities.begin(); }
|
|
PX_FORCE_INLINE const Cm::SpatialVectorF* getPosIterMotionVelocities() const { return mPosIterMotionVelocities.begin(); }
|
|
PX_FORCE_INLINE PxReal* getPosIterJointVelocities() { return mPosIterJointVelocities.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 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 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]; }
|
|
|
|
// PT: PX-1399
|
|
PX_FORCE_INLINE PxArticulationFlags getArticulationFlags() const { return *mFlags; }
|
|
|
|
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.begin(); }
|
|
PX_FORCE_INLINE const PxTransform* getAccumulatedPoses() const { return mAccumulatedPoses.begin(); }
|
|
|
|
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.begin(); }
|
|
PX_FORCE_INLINE SpatialMatrix* getWorldSpatialArticulatedInertia() { return mWorldSpatialArticulatedInertia.begin(); }
|
|
|
|
PX_FORCE_INLINE const Cm::UnAlignedSpatialVector* getWorldMotionMatrix() const { return mWorldMotionMatrix.begin(); }
|
|
PX_FORCE_INLINE Cm::UnAlignedSpatialVector* getWorldMotionMatrix() { return mWorldMotionMatrix.begin(); }
|
|
|
|
PX_FORCE_INLINE const Cm::UnAlignedSpatialVector* getMotionMatrix() const { return mMotionMatrix.begin(); }
|
|
PX_FORCE_INLINE Cm::UnAlignedSpatialVector* getMotionMatrix() { return mMotionMatrix.begin(); }
|
|
|
|
PX_FORCE_INLINE const Cm::SpatialVectorF* getIsW() const { return mIsW.begin(); }
|
|
PX_FORCE_INLINE Cm::SpatialVectorF* getIsW() { return mIsW.begin(); }
|
|
|
|
PX_FORCE_INLINE const PxVec3* getRw() const { return mRw.begin(); }
|
|
PX_FORCE_INLINE PxVec3* getRw() { return mRw.begin(); }
|
|
|
|
PX_FORCE_INLINE const PxReal* getMinusStZExt() const { return qstZIc.begin(); }
|
|
PX_FORCE_INLINE PxReal* getMinusStZExt() { return qstZIc.begin(); }
|
|
|
|
PX_FORCE_INLINE const PxReal* getQstZIc() const { return qstZIc.begin(); }
|
|
PX_FORCE_INLINE PxReal* getQstZIc() { return qstZIc.begin(); }
|
|
|
|
PX_FORCE_INLINE const PxReal* getQStZIntIc() const { return qstZIntIc.begin();}
|
|
PX_FORCE_INLINE PxReal* getQStZIntIc() { return qstZIntIc.begin();}
|
|
|
|
PX_FORCE_INLINE const InvStIs* getInvStIS() const { return mInvStIs.begin(); }
|
|
PX_FORCE_INLINE InvStIs* getInvStIS() { return mInvStIs.begin(); }
|
|
|
|
PX_FORCE_INLINE const Cm::SpatialVectorF* getISInvStIS() const { return mISInvStIS.begin(); }
|
|
PX_FORCE_INLINE Cm::SpatialVectorF* getISInvStIS() { return mISInvStIS.begin(); }
|
|
|
|
PX_FORCE_INLINE TestImpulseResponse* getImpulseResponseMatrixWorld() { return mResponseMatrixW.begin(); }
|
|
|
|
PX_FORCE_INLINE const TestImpulseResponse* 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 Cm::UnAlignedSpatialVector& getMotionMatrix(const PxU32 dofId) const { return mMotionMatrix[dofId]; }
|
|
PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& getWorldMotionMatrix(const PxU32 dofId) const { return mWorldMotionMatrix[dofId]; }
|
|
|
|
PX_FORCE_INLINE Cm::UnAlignedSpatialVector& getJointAxis(const PxU32 dofId) { return mJointAxis[dofId]; }
|
|
PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& getJointAxis(const PxU32 dofId) const { return mJointAxis[dofId]; }
|
|
|
|
PX_FORCE_INLINE const PxVec3& getRw(const PxU32 linkID) const { return mRw[linkID]; }
|
|
|
|
PX_FORCE_INLINE const Cm::SpatialVectorF& getIsW(const PxU32 dofId) const { return mIsW[dofId]; }
|
|
|
|
PX_FORCE_INLINE const Cm::SpatialVectorF& getWorldIsInvD(const PxU32 dofId) const { return mISInvStIS[dofId]; }
|
|
PX_FORCE_INLINE PxReal* getDeferredQstZ() { return mDeferredQstZ.begin(); }
|
|
|
|
PX_FORCE_INLINE void setRootPreMotionVelocity(const Cm::UnAlignedSpatialVector& vel) { mRootPreMotionVelocity.top = vel.top; mRootPreMotionVelocity.bottom = vel.bottom; }
|
|
|
|
PX_FORCE_INLINE 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;}
|
|
|
|
private:
|
|
Cm::SpatialVectorF mRootPreMotionVelocity;
|
|
Cm::SpatialVectorF mRootDeferredZ;
|
|
PxArray<PxReal> mJointAcceleration; // joint acceleration
|
|
PxArray<PxReal> mJointInternalAcceleration; //joint internal force acceleration
|
|
PxArray<PxReal> mJointVelocity; // joint velocity
|
|
PxArray<PxReal> mJointNewVelocity; // joint velocity due to contacts
|
|
PxArray<PxReal> mJointPosition; // joint position
|
|
PxArray<PxReal> mJointForce; // joint force
|
|
PxArray<PxReal> mJointTargetPositions; // joint target positions
|
|
PxArray<PxReal> mJointTargetVelocities; // joint target velocities
|
|
|
|
PxArray<PxReal> mPosIterJointVelocities; //joint delta velocity after postion iternation before velocity iteration
|
|
PxArray<Cm::SpatialVectorF> mPosIterMotionVelocities; //link motion velocites after position iteration before velocity iteration
|
|
PxArray<Cm::SpatialVectorF> mMotionVelocities; //link motion velocites
|
|
PxArray<Cm::SpatialVectorF> mSolverLinkSpatialDeltaVels; //link DeltaVels arising from solver
|
|
PxArray<Cm::SpatialVectorF> mSolverLinkSpatialImpulses; //link impulses arising from solver.
|
|
PxArray<Cm::SpatialVectorF> mMotionAccelerations; //link motion accelerations
|
|
PxArray<Cm::SpatialVectorF> mLinkIncomingJointForces;
|
|
PxArray<Cm::SpatialVectorF> mMotionAccelerationsInternal; //link motion accelerations
|
|
PxArray<Cm::SpatialVectorF> mCorioliseVectors; //link coriolise vector
|
|
PxArray<Cm::SpatialVectorF> mZAInternalForces; //link internal spatial forces
|
|
PxArray<Cm::SpatialVectorF> mZAForces; //link spatial zero acceleration force/ spatial articulated force
|
|
PxArray<Cm::SpatialVectorF> mJointTransmittedForce;
|
|
PxArray<ArticulationInternalConstraint> mInternalConstraints;
|
|
PxArray<ArticulationInternalLimit> mInternalLimits;
|
|
PxArray<ArticulationInternalTendonConstraint> mInternalSpatialTendonConstraints;
|
|
PxArray<ArticulationInternalTendonConstraint> mInternalFixedTendonConstraints;
|
|
PxArray<ArticulationInternalMimicJoint> mInternalMimicJoints;
|
|
|
|
PxArray<PxReal> mDeferredQstZ;
|
|
|
|
PxArray<Cm::SpatialVectorF> mDeltaMotionVector; //this is for TGS solver
|
|
PxArray<PxTransform> mPreTransform; //this is the previous transform list for links
|
|
PxArray<TestImpulseResponse> mResponseMatrixW;
|
|
PxArray<SpatialMatrix> mWorldSpatialArticulatedInertia;
|
|
PxArray<PxMat33> mWorldIsolatedSpatialArticulatedInertia;
|
|
PxArray<PxReal> mMasses;
|
|
PxArray<InvStIs> mInvStIs;
|
|
PxArray<Cm::SpatialVectorF> mIsW;
|
|
PxArray<PxReal> qstZIc;//jointForce - stZIc
|
|
PxArray<PxReal> qstZIntIc;
|
|
PxArray<Cm::UnAlignedSpatialVector> mJointAxis;
|
|
PxArray<Cm::UnAlignedSpatialVector> mMotionMatrix;
|
|
PxArray<Cm::UnAlignedSpatialVector> mWorldMotionMatrix;
|
|
PxArray<Cm::SpatialVectorF> mISInvStIS;
|
|
PxArray<PxVec3> mRw;
|
|
|
|
PxArray<PxU32> mNbStatic1DConstraints;
|
|
PxArray<PxU32> mNbStaticContactConstraints;
|
|
|
|
PxArray<PxU32> mStatic1DConstraintStartIndex;
|
|
PxArray<PxU32> mStaticContactConstraintStartIndex;
|
|
|
|
PxArray<PxQuat> mRelativeQuat;
|
|
|
|
ArticulationLink* mLinks;
|
|
PxU32 mLinkCount;
|
|
PxU32* mPathToRootElements;
|
|
PxU32 mNumPathToRootElements;
|
|
ArticulationLinkData* mLinksData;
|
|
ArticulationJointCoreData* mJointData;
|
|
ArticulationSpatialTendon** mSpatialTendons;
|
|
PxU32 mNumSpatialTendons;
|
|
PxU32 mNumTotalAttachments;
|
|
ArticulationFixedTendon** mFixedTendons;
|
|
PxU32 mNumFixedTendons;
|
|
ArticulationMimicJointCore** mMimicJoints;
|
|
PxU32 mNbMimicJoints;
|
|
PxReal mDt;
|
|
PxU32 mDofs;
|
|
const PxArticulationFlags* mFlags; // PT: PX-1399
|
|
Cm::SpatialVector* mExternalAcceleration;
|
|
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;
|
|
|
|
PxArray<PxTransform> mAccumulatedPoses;
|
|
PxArray<PxQuat> mDeltaQ;
|
|
|
|
SpatialMatrix mBaseInvSpatialArticulatedInertiaW;
|
|
|
|
PxReal mInvSumMass;
|
|
PxVec3 mCOM;
|
|
|
|
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;
|
|
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;
|
|
};
|
|
|
|
struct FixedTendonSolveData
|
|
{
|
|
ArticulationLink* links;
|
|
ArticulationTendonJoint* tendonJoints;
|
|
PxReal rootVel;
|
|
PxReal rootImp;
|
|
PxReal erp;
|
|
PxReal error;
|
|
PxReal limitError;
|
|
};
|
|
|
|
#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_INLINE PX_CUDA_CALLABLE 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_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
|
|
}
|
|
|
|
|
|
PX_ALIGN_PREFIX(64)
|
|
class FeatherstoneArticulation
|
|
{
|
|
PX_NOCOPY(FeatherstoneArticulation)
|
|
public:
|
|
// public interface
|
|
|
|
explicit FeatherstoneArticulation(void*);
|
|
~FeatherstoneArticulation();
|
|
|
|
// get data sizes for allocation at higher levels
|
|
void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize);
|
|
|
|
bool resize(const PxU32 linkCount);
|
|
|
|
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, const bool rootMotion);
|
|
|
|
//joint velocity as input, generalised force(coriolis and centrigugal force) as output
|
|
void getCoriolisAndCentrifugalForce(PxArticulationCache& cache, const bool rootMotion);
|
|
|
|
//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);
|
|
|
|
//These two functions are for closed loop system
|
|
void getKMatrix(ArticulationJointCore* loopJoint, const PxU32 parentIndex, const PxU32 childIndex, PxArticulationCache& cache);
|
|
|
|
void getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, PxArticulationCache& cache);
|
|
|
|
bool getLambda(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, const bool rootMotion);
|
|
|
|
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;
|
|
|
|
void getImpulseSelfResponse(
|
|
PxU32 linkID0,
|
|
PxU32 linkID1,
|
|
const Cm::SpatialVector& impulse0,
|
|
const Cm::SpatialVector& impulse1,
|
|
Cm::SpatialVector& deltaV0,
|
|
Cm::SpatialVector& deltaV1) const;
|
|
|
|
Cm::SpatialVectorV getLinkVelocity(const PxU32 linkID) const;
|
|
|
|
Cm::SpatialVector getLinkScalarVelocity(const PxU32 linkID) const;
|
|
|
|
Cm::SpatialVectorV getLinkMotionVector(const PxU32 linkID) const;
|
|
|
|
//this is called by island gen to determine whether the articulation should be awake or sleep
|
|
Cm::SpatialVector getMotionVelocity(const PxU32 linkID) const;
|
|
|
|
Cm::SpatialVector getMotionAcceleration(const PxU32 linkID, const bool isGpuSimEnabled) const;
|
|
|
|
PxReal getLinkMaxPenBias(const PxU32 linkID) const;
|
|
|
|
PxReal getCfm(const PxU32 linkID) const;
|
|
|
|
static PxU32 computeUnconstrainedVelocities(
|
|
const ArticulationSolverDesc& desc,
|
|
PxReal dt,
|
|
PxU32& acCount,
|
|
const PxVec3& gravity,
|
|
PxReal invLengthScale);
|
|
|
|
static void computeUnconstrainedVelocitiesTGS(
|
|
const ArticulationSolverDesc& desc,
|
|
PxReal dt, const PxVec3& gravity,
|
|
PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled);
|
|
|
|
static PxU32 setupSolverConstraintsTGS(const ArticulationSolverDesc& articDesc,
|
|
PxReal dt,
|
|
PxReal invDt, PxReal totalDt);
|
|
|
|
static void saveVelocity(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* deltaV);
|
|
|
|
static void saveVelocityTGS(FeatherstoneArticulation* articulation, PxReal invDtF32);
|
|
|
|
static void updateBodies(const ArticulationSolverDesc& desc, Cm::SpatialVectorF* tempDeltaV, PxReal dt);
|
|
|
|
static void updateBodiesTGS(const ArticulationSolverDesc& desc, Cm::SpatialVectorF* tempDeltaV, PxReal dt);
|
|
|
|
static void updateBodies(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* tempDeltaV, PxReal dt, bool integrateJointPosition);
|
|
|
|
static void recordDeltaMotion(const ArticulationSolverDesc& desc, const PxReal dt, Cm::SpatialVectorF* deltaV, const PxReal totalInvDt);
|
|
|
|
static void deltaMotionToMotionVelocity(const ArticulationSolverDesc& desc, PxReal invDt);
|
|
|
|
static void applyTgsSubstepForces(const ArticulationSolverDesc& desc, const 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;
|
|
|
|
Cm::SpatialVectorV pxcFsGetVelocityTGS(PxU32 linkID);
|
|
|
|
const PxQuat& getDeltaQ(PxU32 linkID) const;
|
|
|
|
/**
|
|
/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;
|
|
}
|
|
//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;
|
|
}
|
|
|
|
public:
|
|
void constraintPrep(ArticulationLoopConstraint* lConstraints, PxU32 nbJoints,
|
|
PxSolverConstraintPrepDesc& prepDesc, PxSolverBody& sBody,
|
|
PxSolverBodyData& sBodyData, PxSolverConstraintDesc* desc, PxConstraintAllocator& allocator);
|
|
|
|
void updateArticulation(const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled);
|
|
|
|
void computeUnconstrainedVelocitiesInternal(
|
|
const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled = false);
|
|
|
|
//copy joint data from fromJointData to toJointData
|
|
void copyJointData(const ArticulationData& data, PxReal* toJointData, const PxReal* fromJointData);
|
|
|
|
PxU32 countDofs();
|
|
void configureDofs();
|
|
|
|
//this function calculates motion subspace matrix(s) for all tree joint
|
|
template<bool immediateMode = false>
|
|
void jcalc(ArticulationData& data);
|
|
|
|
//this function calculates loop joint constraint subspace matrix(s) and active force
|
|
//subspace matrix
|
|
void jcalcLoopJointSubspace(ArticulationJointCore* joint, ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& T,
|
|
const Cm::UnAlignedSpatialVector* jointAxis);
|
|
|
|
void computeSpatialInertia(ArticulationData& data);
|
|
|
|
//compute zero acceleration force
|
|
void computeZ(const ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData);
|
|
void computeZD(const ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData);
|
|
|
|
void solveInternalConstraints(const PxReal dt, const PxReal stepDt, const PxReal invStepDt,
|
|
bool velocityIteration, bool isTGS, const PxReal elapsedTime, const PxReal biasCoefficient, bool residualReportingActive, bool isExternalForcesEveryTgsIterationEnabled = false);
|
|
|
|
void solveInternalJointConstraints(const PxReal dt, const PxReal stepDt, const PxReal invStepDt,
|
|
bool velocityIteration, bool isTGS, const PxReal elapsedTime, const PxReal biasCoefficient, bool residualReportingActive, bool isExternalForcesEveryTgsIterationEnabled);
|
|
|
|
private:
|
|
Cm::SpatialVectorF solveInternalJointConstraintRecursive(const InternalConstraintSolverData& data, const PxU32 linkID,
|
|
const Cm::SpatialVectorF& parentDeltaV, PxU32& dofId, PxU32& limitId);
|
|
public:
|
|
void solveInternalSpatialTendonConstraints(bool isTGS);
|
|
|
|
void solveInternalFixedTendonConstraints(bool isTGS);
|
|
|
|
void setupInternalMimicJointConstraints();
|
|
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
|
|
void computeC(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[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* links, const PxU32 linkCount, const ArticulationJointCoreData* jointCoreDatas,
|
|
const Cm::UnAlignedSpatialVector* jointDofMotionMatrices,
|
|
PxTransform* linkAccumulatedPoses, PxVec3* linkRws, Cm::UnAlignedSpatialVector* jointDofmotionMatricesW);
|
|
|
|
//compute relative transform child to base
|
|
void computeRelativeTransformC2B(ArticulationData& data);
|
|
|
|
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] linkData is the ArticulationLinkData instance of each link in the articulation.
|
|
\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] rootPreMotionVelocityW is assigned the spatial velocity of the root link.
|
|
\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 ArticulationLinkData::maxPenBias of each link inherits the value of the associated PxsBodyCore::maxPenBias.
|
|
\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(
|
|
const PxF32 dt, const PxReal invLengthScale, const PxVec3& gravity,
|
|
const bool fixBase,
|
|
const PxU32 linkCount,
|
|
const PxTransform* linkAccumulatedPosesW, Cm::SpatialVector* linkExternalAccelsW, const PxVec3* linkRsW, const Cm::UnAlignedSpatialVector* jointDofMotionMatricesW,
|
|
const Dy::ArticulationJointCoreData* jointCoreData, bool externalForcesEveryTgsIterationEnabled,
|
|
Dy::ArticulationLinkData *linkData, Dy::ArticulationLink* links,
|
|
Cm::SpatialVectorF* linkMotionAccelerationsW, Cm::SpatialVectorF* linkMotionVelocitiesW,
|
|
Cm::SpatialVectorF* linkZAForcesExtW, Cm::SpatialVectorF* linkZAForcesIntW, Cm::SpatialVectorF* linkCoriolisVectorsW,
|
|
PxMat33* linkIsolatedArticulatedInertiasW, PxF32* linkMasses, Dy::SpatialMatrix* linkSpatialArticulatedInertiasW,
|
|
PxReal* jointDofVelocities,
|
|
Cm::SpatialVectorF& rootPreMotionVelocityW, 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[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.
|
|
\return A term to be translated to parent link and added to the articulated inertia of the parent.
|
|
*/
|
|
static SpatialMatrix computePropagateSpatialInertia_ZA_ZIc
|
|
(const PxArticulationJointType::Enum jointType, const PxU8 nbJointDofs,
|
|
const Cm::UnAlignedSpatialVector* jointMotionMatricesW, const Cm::SpatialVectorF* jointISW,
|
|
const PxReal* jointTargetArmatures, const PxU8* dofIds, const PxReal* jointExternalForces,
|
|
const SpatialMatrix& linkArticulatedInertiaW,
|
|
const Cm::SpatialVectorF& linkZExtW, const Cm::SpatialVectorF& linkZIntIcW,
|
|
InvStIs& linkInvStISW, Cm::SpatialVectorF* jointDofISInvStISW,
|
|
PxReal* jointDofMinusStZExtW, PxReal* jointDofQStZIntIcW,
|
|
Cm::SpatialVectorF& deltaZAExtParent, Cm::SpatialVectorF& 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[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.
|
|
\return A term to be translated to parent link and added to the articulated inertia of the parent.
|
|
*/
|
|
static SpatialMatrix computePropagateSpatialInertia_ZA_ZIc_NonSeparated
|
|
(const PxArticulationJointType::Enum jointType, const PxU8 nbJointDofs,
|
|
const Cm::UnAlignedSpatialVector* jointMotionMatrices, const Cm::SpatialVectorF* jointIs,
|
|
const PxReal* jointTargetArmatures, const PxU8* dofIds,
|
|
const PxReal* jointExternalForces,
|
|
const SpatialMatrix& linkArticulatedInertia,
|
|
const Cm::SpatialVectorF& ZIc,
|
|
InvStIs& invStIs, Cm::SpatialVectorF* isInvD,
|
|
PxReal* qstZIc,
|
|
Cm::SpatialVectorF& 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[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]
|
|
\return A term to be translated to parent link and added to the articulated inertia of the parent.
|
|
*/
|
|
static SpatialMatrix computePropagateSpatialInertia(
|
|
const PxArticulationJointType::Enum jointType, const PxU8 nbDofs,
|
|
const SpatialMatrix& linkArticulatedInertia, const Cm::UnAlignedSpatialVector* jointMotionMatrices,
|
|
const Cm::SpatialVectorF* jointIs,
|
|
InvStIs& invStIs, Cm::SpatialVectorF* isInvD);
|
|
|
|
static void transformInertia(const SpatialTransform& sTod, SpatialMatrix& inertia);
|
|
|
|
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* links, const PxU32 linkCount, const PxVec3* linkRsW,
|
|
const ArticulationJointCoreData* jointData,
|
|
const Cm::UnAlignedSpatialVector* jointDofMotionMatricesW,
|
|
const Cm::SpatialVectorF* linkCoriolisVectorsW, const PxReal* jointDofForces,
|
|
Cm::SpatialVectorF* jointDofIsW, InvStIs* linkInvStIsW, Cm::SpatialVectorF* jointDofISInvStIS, PxReal* joIntDofMinusStZExtW, PxReal* jointDofQStZIntIcW,
|
|
Cm::SpatialVectorF* linkZAExtForcesW, Cm::SpatialVectorF* linkZAIntForcesW, SpatialMatrix* linkSpatialArticulatedInertiaW,
|
|
SpatialMatrix& baseInvSpatialArticulatedInertiaW);
|
|
|
|
void computeArticulatedSpatialInertiaAndZ_NonSeparated(ArticulationData& data, ScratchData& scratchData);
|
|
|
|
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, const PxU32 linkCount,
|
|
const ArticulationJointCoreData* jointData,
|
|
const SpatialMatrix& baseInvArticulatedInertiaW,
|
|
const PxVec3* linkRsW, const Cm::UnAlignedSpatialVector* jointDofMotionMatricesW,
|
|
const Cm::SpatialVectorF* jointDofISW, const InvStIs* linkInvStISW, const Cm::SpatialVectorF* jointDofIsInvDW,
|
|
ArticulationLink* links, TestImpulseResponse* linkResponsesW);
|
|
|
|
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(const PxU8 nbJointDofs,
|
|
const Cm::SpatialVectorF& pMotionAcceleration, const Cm::SpatialVectorF* jointDofISW, const InvStIs& linkInvStISW,
|
|
const PxReal* jointDofQStZIcW, PxReal* 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
|
|
(const bool doIC, const PxReal dt,
|
|
const bool fixBase,
|
|
const ArticulationLink* links, const PxU32 linkCount, const ArticulationJointCoreData* jointDatas,
|
|
const Cm::SpatialVectorF* linkSpatialZAForcesW, const Cm::SpatialVectorF* linkCoriolisForcesW, const PxVec3* linkRsW,
|
|
const Cm::UnAlignedSpatialVector* jointDofMotionMatricesW,
|
|
const SpatialMatrix& baseInvSpatialArticulatedInertiaW,
|
|
const InvStIs* linkInvStISW,
|
|
const Cm::SpatialVectorF* jointDofISW, const PxReal* jointDofQStZIcW,
|
|
Cm::SpatialVectorF* linkMotionAccelerationsW, Cm::SpatialVectorF* linkMotionVelocitiesW,
|
|
PxReal* jointDofAccelerations, PxReal* jointDofVelocities, PxReal* 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.
|
|
\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* jointDofInternalAccelerations, PxReal* jointDofVelocities, PxReal* jointDofNewVelocities);
|
|
|
|
/**
|
|
\brief For each link compute the incoming joint force in the joint frame.
|
|
\param[in] linkCount is the number of links in the articulation
|
|
\param[in] linkZAForcesExtW are the external spatial zero acceleration forces in the world frame with one entry per link.
|
|
\param[in] linkZAForcesIntW are the internal spatial zero acceleration forces in the world frame with one entry per link.
|
|
\param[in] linkMotionAccelerationsW are the spatial accelerations ion the world framewith one entry per link.
|
|
\param[in] linkSpatialInertiasW are the spatial articulated inertias in the world frame with one entry per link.
|
|
\param[out] linkIncomingJointForces are the incoming joint forces specified in the joint frame that are applied to each link.
|
|
*/
|
|
static void computeLinkIncomingJointForce(
|
|
const PxU32 linkCount,
|
|
const Cm::SpatialVectorF* linkZAForcesExtW, const Cm::SpatialVectorF* linkZAForcesIntW,
|
|
const Cm::SpatialVectorF* linkMotionAccelerationsW, const SpatialMatrix* linkSpatialInertiasW,
|
|
Cm::SpatialVectorF* linkIncomingJointForces);
|
|
|
|
//void computeTempLinkAcceleration(ArticulationData& data, ScratchData& scratchData);
|
|
void computeJointTransmittedFrictionForce(ArticulationData& data, ScratchData& scratchData);
|
|
|
|
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(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);
|
|
|
|
void inverseDynamic(ArticulationData& data, const PxVec3& gravity,
|
|
ScratchData& scratchData, bool computeCoriolis);
|
|
|
|
void inverseDynamicFloatingBase(ArticulationData& data, const PxVec3& gravity,
|
|
ScratchData& scratchData, bool computeCoriolis, const bool rootMotion = false);
|
|
|
|
//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, const bool coriolisFloatingBase = false);
|
|
|
|
//compute link acceleration due to external forces, applied external accelerations and Coriolis force
|
|
void computeLinkAccelerationInv(ArticulationData& data, ScratchData& scratchData);
|
|
|
|
void computeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData);
|
|
|
|
void calculateMassMatrixColInv(ScratchData& scratchData);
|
|
|
|
void calculateHFixBase(PxArticulationCache& cache);
|
|
|
|
void calculateHFloatingBase(PxArticulationCache& cache, const bool rootMotion);
|
|
|
|
//joint limits
|
|
static void enforcePrismaticLimits(PxReal& jPosition, ArticulationJointCore* joint);
|
|
|
|
|
|
public:
|
|
|
|
PX_FORCE_INLINE void addBody()
|
|
{
|
|
mAcceleration.pushBack(Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f)));
|
|
mUpdateSolverData = true;
|
|
}
|
|
|
|
PX_FORCE_INLINE void removeBody()
|
|
{
|
|
mUpdateSolverData = true;
|
|
}
|
|
|
|
PX_FORCE_INLINE bool updateSolverData() { return mUpdateSolverData; }
|
|
|
|
PX_FORCE_INLINE PxU32 getMaxDepth() const { return mMaxDepth; }
|
|
PX_FORCE_INLINE void setMaxDepth(const PxU32 depth) { mMaxDepth = depth; }
|
|
|
|
// solver methods
|
|
PX_FORCE_INLINE PxU32 getBodyCount() const { return mSolverDesc.linkCount; }
|
|
PX_FORCE_INLINE void getSolverDesc(ArticulationSolverDesc& d) const { d = mSolverDesc; }
|
|
PX_FORCE_INLINE ArticulationSolverDesc& getSolverDesc() { return mSolverDesc; }
|
|
|
|
PX_FORCE_INLINE ArticulationCore* getCore() { return mSolverDesc.core; }
|
|
PX_FORCE_INLINE PxU16 getIterationCounts() const { return mSolverDesc.core->solverIterationCounts; }
|
|
|
|
PX_FORCE_INLINE void* getUserData() const { return mUserData; }
|
|
|
|
PX_FORCE_INLINE void setDyContext(Dy::Context* context) { mContext = context; }
|
|
|
|
void setupLinks(PxU32 nbLinks, Dy::ArticulationLink* links);
|
|
void setupDofs();
|
|
void allocatePathToRootElements(const PxU32 totalPathToRootElements);
|
|
void initPathToRoot();
|
|
|
|
static void getImpulseSelfResponse(ArticulationLink* links,
|
|
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,
|
|
ArticulationData& data,
|
|
PxU32 linkID0_,
|
|
const Cm::SpatialVector& impulse0,
|
|
Cm::SpatialVector& deltaV0,
|
|
PxU32 linkID1_,
|
|
const Cm::SpatialVector& impulse1,
|
|
Cm::SpatialVector& deltaV1);
|
|
|
|
PxU32 setupSolverConstraints(
|
|
ArticulationLink* links,
|
|
const PxU32 linkCount,
|
|
const bool fixBase,
|
|
ArticulationData& data,
|
|
PxU32& acCount);
|
|
|
|
void setupInternalConstraints(
|
|
ArticulationLink* links,
|
|
const PxU32 linkCount,
|
|
const bool fixBase,
|
|
ArticulationData& data,
|
|
PxReal stepDt,
|
|
PxReal dt,
|
|
PxReal invDt,
|
|
bool isTGSSolver);
|
|
|
|
void setupInternalConstraintsRecursive(
|
|
ArticulationLink* links,
|
|
const PxU32 linkCount,
|
|
const bool fixBase,
|
|
ArticulationData& data,
|
|
const PxReal stepDt,
|
|
const PxReal dt,
|
|
const PxReal invDt,
|
|
const bool isTGSSolver,
|
|
const PxU32 linkID,
|
|
const PxReal maxForceScale);
|
|
|
|
void setupInternalSpatialTendonConstraintsRecursive(
|
|
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);
|
|
|
|
|
|
void setupInternalFixedTendonConstraintsRecursive(
|
|
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(ArticulationAttachment* attachments, ArticulationData& data, const PxU32 attachmentID, const PxReal accumErr,
|
|
const PxVec3& parentAttachmentPoint);
|
|
|
|
//void updateFixedTendonConstraintsRecursive(ArticulationLink* links, ArticulationTendonJoint* tendonJoint, ArticulationData& data, const PxU32 tendonJointID, const PxReal accumErr);
|
|
|
|
PxVec3 calculateFixedTendonVelocityAndPositionRecursive(FixedTendonSolveData& solveData,
|
|
const Cm::SpatialVectorF& parentV, const Cm::SpatialVectorF& parentDeltaV, const PxU32 tendonJointID);
|
|
|
|
Cm::SpatialVectorF solveFixedTendonConstraintsRecursive(FixedTendonSolveData& solveData,
|
|
const PxU32 tendonJointID);
|
|
|
|
void prepareStaticConstraints(const PxReal dt, const PxReal invDt, PxsContactManagerOutputIterator& outputs,
|
|
Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold,
|
|
PxReal ccdMaxSeparation, 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 biasCoefficient, const PxReal lengthScale);
|
|
|
|
|
|
//integration
|
|
void propagateLinksDown(ArticulationData& data, const PxReal* jointVelocities, PxReal* jointPositions,
|
|
Cm::SpatialVectorF* motionVelocities);
|
|
|
|
void updateJointProperties(
|
|
const PxReal* jointNewVelocities,
|
|
PxReal* jointVelocities,
|
|
PxReal* jointAccelerations);
|
|
|
|
void computeAndEnforceJointPositions(ArticulationData& data);
|
|
|
|
//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);
|
|
|
|
//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* jointPosition, const Cm::UnAlignedSpatialVector* motionMatrix,
|
|
const Cm::UnAlignedSpatialVector* worldMotionMatrix);
|
|
|
|
static void updateRootBody(const Cm::SpatialVectorF& motionVelocity,
|
|
const PxTransform& preTransform, ArticulationData& data, const PxReal dt);
|
|
|
|
//These variables are used in the constraint partition
|
|
PxU16 maxSolverFrictionProgress;
|
|
PxU16 maxSolverNormalProgress;
|
|
PxU32 solverProgress;
|
|
PxU16 mArticulationIndex;
|
|
PxU8 numTotalConstraints;
|
|
|
|
void* mUserData;
|
|
Dy::Context* mContext;
|
|
ArticulationSolverDesc mSolverDesc;
|
|
|
|
PxArray<Cm::SpatialVector> mAcceleration; // supplied by Sc-layer to feed into articulations
|
|
|
|
bool mUpdateSolverData;
|
|
PxU32 mMaxDepth;
|
|
|
|
ArticulationData mArticulationData;
|
|
|
|
PxArray<PxSolverConstraintDesc> mStaticContactConstraints;
|
|
PxArray<PxSolverConstraintDesc> mStatic1DConstraints;
|
|
PxU32 mGPUDirtyFlags;
|
|
bool mJcalcDirty;
|
|
|
|
Dy::ErrorAccumulator mInternalErrorAccumulatorVelIter;
|
|
Dy::ErrorAccumulator mContactErrorAccumulatorVelIter;
|
|
|
|
Dy::ErrorAccumulator mInternalErrorAccumulatorPosIter;
|
|
Dy::ErrorAccumulator mContactErrorAccumulatorPosIter;
|
|
|
|
} PX_ALIGN_SUFFIX(64);
|
|
|
|
#if PX_VC
|
|
#pragma warning(pop)
|
|
#endif
|
|
|
|
} //namespace Dy
|
|
|
|
}
|
|
|
|
#endif
|