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