Revision control

- // 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-2026 NVIDIA Corporation. All rights reserved.
+ // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
- // Copyright (c) 2001-2004 NovodeX AG. 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 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
+ 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
+ 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
+ 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
+ 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);
- void updateArticulation(const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled);
+ static void updateArticulation(ArticulationData& data, const PxVec3& gravity, PxReal invLengthScale, bool externalForcesEveryTgsIterationEnabled);
- void computeUnconstrainedVelocitiesInternal(
+ 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[out] linkAccumulatedPoses is an array used to store the latest link poses taken from ArticulationLink::PxsBodyCore.
+ \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* links, const PxU32 linkCount, const ArticulationJointCoreData* jointCoreDatas,
- const Cm::UnAlignedSpatialVector* jointDofMotionMatrices,
- PxTransform* linkAccumulatedPoses, PxVec3* linkRws, Cm::UnAlignedSpatialVector* jointDofmotionMatricesW);
+ 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(
- 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::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,
+ 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, 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);
+ 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, 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);
+ 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, const PxU8 nbDofs,
- const SpatialMatrix& linkArticulatedInertia, const Cm::UnAlignedSpatialVector* jointMotionMatrices,
- const Cm::SpatialVectorF* jointIs,
- InvStIs& invStIs, Cm::SpatialVectorF* isInvD);
+ 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* 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);
+ 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, 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);
+ 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(const PxU8 nbJointDofs,
- const Cm::SpatialVectorF& pMotionAcceleration, const Cm::SpatialVectorF* jointDofISW, const InvStIs& linkInvStISW,
- const PxReal* jointDofQStZIcW, PxReal* jointAcceleration);
+ 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
- (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);
+ 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 maxSolverFrictionProgress;
- PxU16 maxSolverNormalProgress;
- PxU32 solverProgress;
+ 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