522 lines
18 KiB
C
522 lines
18 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 PXDV_ARTICULATION_H
|
||
|
|
#define PXDV_ARTICULATION_H
|
||
|
|
|
||
|
|
#include "foundation/PxVec3.h"
|
||
|
|
#include "foundation/PxQuat.h"
|
||
|
|
#include "foundation/PxTransform.h"
|
||
|
|
#include "PsVecMath.h"
|
||
|
|
#include "PsUtilities.h"
|
||
|
|
#include "CmUtils.h"
|
||
|
|
#include "CmSpatialVector.h"
|
||
|
|
#include "PxArticulationJoint.h"
|
||
|
|
#include "PxArticulation.h"
|
||
|
|
#include "foundation/PxMemory.h"
|
||
|
|
#include "DyArticulationCore.h"
|
||
|
|
#include "DyArticulationJointCore.h"
|
||
|
|
|
||
|
|
namespace physx
|
||
|
|
{
|
||
|
|
struct PxsBodyCore;
|
||
|
|
class PxsConstraintBlockManager;
|
||
|
|
class PxsContactManagerOutputIterator;
|
||
|
|
struct PxSolverConstraintDesc;
|
||
|
|
struct PxSolverBodyData;
|
||
|
|
class PxContactJoint;
|
||
|
|
struct PxTGSSolverBodyData;
|
||
|
|
struct PxTGSSolverBodyTxInertia;
|
||
|
|
struct PxSolverConstraintDesc;
|
||
|
|
|
||
|
|
namespace Dy
|
||
|
|
{
|
||
|
|
|
||
|
|
struct SpatialSubspaceMatrix;
|
||
|
|
|
||
|
|
struct ConstraintWriteback;
|
||
|
|
class ThreadContext;
|
||
|
|
|
||
|
|
static const size_t DY_ARTICULATION_MAX_SIZE = 64;
|
||
|
|
class ArticulationJointCoreData;
|
||
|
|
struct Constraint;
|
||
|
|
class Context;
|
||
|
|
|
||
|
|
struct ArticulationJointCore : public ArticulationJointCoreBase
|
||
|
|
{
|
||
|
|
//= ATTENTION! =====================================================================================
|
||
|
|
// Changing the data layout of this class breaks the binary serialization format. See comments for
|
||
|
|
// PX_BINARY_SERIAL_VERSION. If a modification is required, please adjust the getBinaryMetaData
|
||
|
|
// function. If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
|
||
|
|
// accordingly.
|
||
|
|
//==================================================================================================
|
||
|
|
|
||
|
|
// drive model
|
||
|
|
PxQuat targetPosition;
|
||
|
|
PxVec3 targetVelocity;
|
||
|
|
|
||
|
|
PxReal spring;//old
|
||
|
|
PxReal damping;//old
|
||
|
|
|
||
|
|
PxReal internalCompliance;//old
|
||
|
|
PxReal externalCompliance;//old
|
||
|
|
|
||
|
|
// limit model
|
||
|
|
|
||
|
|
PxReal swingLimitContactDistance;//old
|
||
|
|
|
||
|
|
PxReal tangentialStiffness;//old
|
||
|
|
PxReal tangentialDamping;//old
|
||
|
|
|
||
|
|
bool swingLimited;//old
|
||
|
|
bool twistLimited;//old
|
||
|
|
|
||
|
|
PxU8 driveType; //both
|
||
|
|
|
||
|
|
PxReal twistLimitContactDistance; //old
|
||
|
|
|
||
|
|
PxReal tanQSwingY;//old
|
||
|
|
PxReal tanQSwingZ;//old
|
||
|
|
PxReal tanQSwingPad;//old
|
||
|
|
PxReal tanQTwistHigh;//old
|
||
|
|
PxReal tanQTwistLow;//old
|
||
|
|
PxReal tanQTwistPad;//old
|
||
|
|
|
||
|
|
ArticulationJointCore()
|
||
|
|
{
|
||
|
|
//Cm::markSerializedMem(this, sizeof(ArticulationJointCore));
|
||
|
|
parentPose = PxTransform(PxIdentity);
|
||
|
|
childPose = PxTransform(PxIdentity);
|
||
|
|
internalCompliance = 0.0f;
|
||
|
|
externalCompliance = 0.0f;
|
||
|
|
swingLimitContactDistance = 0.05f;
|
||
|
|
twistLimitContactDistance = 0.05f;
|
||
|
|
|
||
|
|
driveType = PxArticulationJointDriveType::eTARGET;
|
||
|
|
|
||
|
|
jointType = PxArticulationJointType::eFIX;
|
||
|
|
|
||
|
|
for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
|
||
|
|
motion[i] = PxArticulationMotion::eLOCKED;
|
||
|
|
|
||
|
|
dirtyFlag = ArticulationJointCoreDirtyFlag::eMOTION;
|
||
|
|
|
||
|
|
jointOffset = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
ArticulationJointCore(const PxTransform& parentFrame, const PxTransform& childFrame)
|
||
|
|
{
|
||
|
|
parentPose = parentFrame;
|
||
|
|
childPose = childFrame;
|
||
|
|
|
||
|
|
//the old articulation is eTARGET
|
||
|
|
driveType = PxArticulationJointDriveType::eTARGET;
|
||
|
|
|
||
|
|
spring = 0.0f;
|
||
|
|
damping = 0.0f;
|
||
|
|
|
||
|
|
internalCompliance = 1.0f;
|
||
|
|
externalCompliance = 1.0f;
|
||
|
|
|
||
|
|
for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
|
||
|
|
{
|
||
|
|
limits[i].low = 0.f;
|
||
|
|
limits[i].high = 0.f;
|
||
|
|
drives[i].stiffness = 0.f;
|
||
|
|
drives[i].damping = 0.f;
|
||
|
|
drives[i].maxForce = 0.f;
|
||
|
|
targetP[i] = 0.f;
|
||
|
|
targetV[i] = 0.f;
|
||
|
|
motion[i] = PxArticulationMotion::eLOCKED;
|
||
|
|
}
|
||
|
|
|
||
|
|
const PxReal swingYLimit = PxPi / 4.0f;
|
||
|
|
const PxReal swingZLimit = PxPi / 4.0f;
|
||
|
|
|
||
|
|
limits[PxArticulationAxis::eSWING1].low = swingYLimit;
|
||
|
|
limits[PxArticulationAxis::eSWING2].low = swingZLimit;
|
||
|
|
|
||
|
|
swingLimitContactDistance = 0.05f;
|
||
|
|
swingLimited = false;
|
||
|
|
tangentialStiffness = 0.0f;
|
||
|
|
tangentialDamping = 0.0f;
|
||
|
|
|
||
|
|
const PxReal twistLimitLow = -PxPi / 4.0f;
|
||
|
|
const PxReal twistLimitHigh = PxPi / 4.0f;
|
||
|
|
limits[PxArticulationAxis::eTWIST].low = twistLimitLow;
|
||
|
|
limits[PxArticulationAxis::eTWIST].high = twistLimitHigh;
|
||
|
|
twistLimitContactDistance = 0.05f;
|
||
|
|
twistLimited = false;
|
||
|
|
|
||
|
|
tanQSwingY = PxTan(swingYLimit / 4.0f);
|
||
|
|
tanQSwingZ = PxTan(swingZLimit / 4.0f);
|
||
|
|
tanQSwingPad = PxTan(swingLimitContactDistance / 4.0f);
|
||
|
|
|
||
|
|
tanQTwistHigh = PxTan(twistLimitHigh / 4.0f);
|
||
|
|
tanQTwistLow = PxTan(twistLimitLow / 4.0f);
|
||
|
|
tanQTwistPad = PxTan(twistLimitContactDistance / 4.0f);
|
||
|
|
|
||
|
|
frictionCoefficient = 0.f;
|
||
|
|
|
||
|
|
dirtyFlag = ArticulationJointCoreDirtyFlag::eMOTION;
|
||
|
|
|
||
|
|
jointOffset = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void setJointPose(ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& motionMatrix, bool forceUpdate,
|
||
|
|
PxQuat& relativeRot);
|
||
|
|
|
||
|
|
// PX_SERIALIZATION
|
||
|
|
ArticulationJointCore(const PxEMPTY&) : ArticulationJointCoreBase(PxEmpty) {}
|
||
|
|
//~PX_SERIALIZATION
|
||
|
|
};
|
||
|
|
|
||
|
|
struct ArticulationLoopConstraint
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
PxU32 linkIndex0;
|
||
|
|
PxU32 linkIndex1;
|
||
|
|
Dy::Constraint* constraint;
|
||
|
|
};
|
||
|
|
|
||
|
|
#define DY_ARTICULATION_LINK_NONE 0xffffffff
|
||
|
|
|
||
|
|
typedef PxU64 ArticulationBitField;
|
||
|
|
|
||
|
|
struct ArticulationLink
|
||
|
|
{
|
||
|
|
ArticulationBitField children; // child bitmap
|
||
|
|
ArticulationBitField pathToRoot; // path to root, including link and root
|
||
|
|
PxsBodyCore* bodyCore;
|
||
|
|
ArticulationJointCore* inboundJoint;
|
||
|
|
PxU32 parent;
|
||
|
|
};
|
||
|
|
|
||
|
|
typedef size_t ArticulationLinkHandle;
|
||
|
|
|
||
|
|
class ArticulationV;
|
||
|
|
|
||
|
|
struct ArticulationSolverDesc
|
||
|
|
{
|
||
|
|
void initData(const ArticulationCore* core_, const PxArticulationFlags* flags_)
|
||
|
|
{
|
||
|
|
articulation = NULL;
|
||
|
|
links = NULL;
|
||
|
|
motionVelocity = NULL;
|
||
|
|
acceleration = NULL;
|
||
|
|
poses = NULL;
|
||
|
|
deltaQ = NULL;
|
||
|
|
externalLoads = NULL;
|
||
|
|
internalLoads = NULL;
|
||
|
|
core = core_;
|
||
|
|
flags = flags_;
|
||
|
|
scratchMemory = NULL;
|
||
|
|
totalDataSize = 0;
|
||
|
|
solverDataSize = 0;
|
||
|
|
linkCount = 0;
|
||
|
|
numInternalConstraints = 0;
|
||
|
|
scratchMemorySize = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
ArticulationV* articulation;
|
||
|
|
ArticulationLink* links;
|
||
|
|
Cm::SpatialVectorV* motionVelocity;
|
||
|
|
Cm::SpatialVector* acceleration;
|
||
|
|
PxTransform* poses;
|
||
|
|
PxQuat* deltaQ;
|
||
|
|
physx::shdfnd::aos::Mat33V* externalLoads;
|
||
|
|
physx::shdfnd::aos::Mat33V* internalLoads;
|
||
|
|
const ArticulationCore* core;
|
||
|
|
const PxArticulationFlags* flags; // PT: PX-1399
|
||
|
|
char* scratchMemory;
|
||
|
|
PxU16 totalDataSize;
|
||
|
|
PxU16 solverDataSize;
|
||
|
|
PxU8 linkCount;
|
||
|
|
PxU8 numInternalConstraints;
|
||
|
|
PxU16 scratchMemorySize;
|
||
|
|
};
|
||
|
|
|
||
|
|
struct PxcFsScratchAllocator
|
||
|
|
{
|
||
|
|
char* base;
|
||
|
|
size_t size;
|
||
|
|
size_t taken;
|
||
|
|
PxcFsScratchAllocator(char* p, size_t s) : base(p), size(s), taken(0) {}
|
||
|
|
|
||
|
|
template<typename T>
|
||
|
|
static size_t sizeof16()
|
||
|
|
{
|
||
|
|
return (sizeof(T) + 15)&~15;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<class T> T* alloc(PxU32 count)
|
||
|
|
{
|
||
|
|
size_t s = sizeof16<T>();
|
||
|
|
PX_ASSERT(taken + s*count <= size);
|
||
|
|
T* result = reinterpret_cast<T*>(base + taken);
|
||
|
|
taken += s*count;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
static const size_t DY_ARTICULATION_IDMASK = DY_ARTICULATION_MAX_SIZE - 1;
|
||
|
|
|
||
|
|
#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
|
||
|
|
PX_ALIGN_PREFIX(64)
|
||
|
|
class ArticulationV
|
||
|
|
{
|
||
|
|
PX_NOCOPY(ArticulationV)
|
||
|
|
public:
|
||
|
|
|
||
|
|
enum Enum
|
||
|
|
{
|
||
|
|
eReducedCoordinate = 0,
|
||
|
|
eMaximumCoordinate = 1
|
||
|
|
};
|
||
|
|
|
||
|
|
ArticulationV(void* userData, Enum type) :
|
||
|
|
mUserData (userData),
|
||
|
|
mContext (NULL),
|
||
|
|
mType (type),
|
||
|
|
mUpdateSolverData (true),
|
||
|
|
mDirty (false),
|
||
|
|
mMaxDepth (0)
|
||
|
|
{}
|
||
|
|
|
||
|
|
virtual ~ArticulationV() {}
|
||
|
|
|
||
|
|
virtual void onUpdateSolverDesc() {}
|
||
|
|
|
||
|
|
virtual bool resize(const PxU32 linkCount);
|
||
|
|
|
||
|
|
virtual void addBody()
|
||
|
|
{
|
||
|
|
mAcceleration.pushBack(Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f)));
|
||
|
|
mUpdateSolverData = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual void removeBody()
|
||
|
|
{
|
||
|
|
mUpdateSolverData = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
PX_FORCE_INLINE bool updateSolverData() { return mUpdateSolverData; }
|
||
|
|
|
||
|
|
PX_FORCE_INLINE void setDirty(const bool dirty) { mDirty = dirty; }
|
||
|
|
PX_FORCE_INLINE bool getDirty() const { return mDirty; }
|
||
|
|
|
||
|
|
PX_FORCE_INLINE PxU32 getMaxDepth() const { return mMaxDepth; }
|
||
|
|
PX_FORCE_INLINE void setMaxDepth(const PxU32 depth) { mMaxDepth = depth; }
|
||
|
|
|
||
|
|
// solver methods
|
||
|
|
PX_FORCE_INLINE PxU32 getBodyCount() const { return mSolverDesc.linkCount; }
|
||
|
|
PX_FORCE_INLINE PxU32 getSolverDataSize() const { return mSolverDesc.solverDataSize; }
|
||
|
|
PX_FORCE_INLINE PxU32 getTotalDataSize() const { return mSolverDesc.totalDataSize; }
|
||
|
|
PX_FORCE_INLINE void getSolverDesc(ArticulationSolverDesc& d) const { d = mSolverDesc; }
|
||
|
|
PX_FORCE_INLINE ArticulationSolverDesc& getSolverDesc() { return mSolverDesc; }
|
||
|
|
|
||
|
|
PX_FORCE_INLINE const ArticulationCore* getCore() const { return mSolverDesc.core; }
|
||
|
|
PX_FORCE_INLINE PxU16 getIterationCounts() const { return mSolverDesc.core->solverIterationCounts; }
|
||
|
|
|
||
|
|
PX_FORCE_INLINE void* getUserData() const { return mUserData; }
|
||
|
|
|
||
|
|
PX_FORCE_INLINE PxU32 getType() const { return mType; }
|
||
|
|
|
||
|
|
PX_FORCE_INLINE void setDyContext(Dy::Context* context) { mContext = context; }
|
||
|
|
|
||
|
|
// get data sizes for allocation at higher levels
|
||
|
|
virtual void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize) = 0;
|
||
|
|
|
||
|
|
virtual PxU32 getDofs() { return 0; }
|
||
|
|
|
||
|
|
virtual PxU32 getDof(const PxU32 /*linkID*/) { return 0; }
|
||
|
|
|
||
|
|
virtual bool applyCache(PxArticulationCache& /*cache*/, const PxArticulationCacheFlags /*flag*/) {return false;}
|
||
|
|
|
||
|
|
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() {}
|
||
|
|
|
||
|
|
virtual void getGeneralizedGravityForce(const PxVec3& /*gravity*/, PxArticulationCache& /*cache*/) {}
|
||
|
|
|
||
|
|
virtual void getCoriolisAndCentrifugalForce(PxArticulationCache& /*cache*/) {}
|
||
|
|
|
||
|
|
virtual void getGeneralizedExternalForce(PxArticulationCache& /*cache*/) {}
|
||
|
|
|
||
|
|
virtual void getJointAcceleration(const PxVec3& /*gravity*/, PxArticulationCache& /*cache*/){}
|
||
|
|
|
||
|
|
virtual void getJointForce(PxArticulationCache& /*cache*/){}
|
||
|
|
|
||
|
|
virtual void getCoefficientMatrix(const PxReal /*dt*/, const PxU32 /*linkID*/, const PxContactJoint* /*joints*/, const PxU32 /*nbContacts*/, PxArticulationCache& /*cache*/){}
|
||
|
|
|
||
|
|
virtual void getDenseJacobian(PxArticulationCache& /*cache*/, PxU32&, PxU32&) {}
|
||
|
|
|
||
|
|
virtual void getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* /*lConstraints*/, const PxU32 /*nbJoints*/, PxArticulationCache& /*cache*/) {}
|
||
|
|
|
||
|
|
virtual bool getLambda( ArticulationLoopConstraint* /*lConstraints*/, const PxU32 /*nbJoints*/,
|
||
|
|
PxArticulationCache& /*cache*/, PxArticulationCache& /*initialState*/, const PxReal* /*jointTorque*/,
|
||
|
|
const PxVec3& /*gravity*/, const PxU32 /*maxIter*/) { return false; }
|
||
|
|
|
||
|
|
virtual void getGeneralizedMassMatrix(PxArticulationCache& /*cache*/){}
|
||
|
|
virtual void getGeneralizedMassMatrixCRB(PxArticulationCache& /*cache*/){}
|
||
|
|
|
||
|
|
virtual void teleportRootLink(){}
|
||
|
|
|
||
|
|
virtual void getImpulseResponse( PxU32 linkID,
|
||
|
|
Cm::SpatialVectorF* Z,
|
||
|
|
const Cm::SpatialVector& impulse,
|
||
|
|
Cm::SpatialVector& deltaV) const = 0;
|
||
|
|
|
||
|
|
virtual void getImpulseResponse(
|
||
|
|
PxU32 linkID,
|
||
|
|
Cm::SpatialVectorV* Z,
|
||
|
|
const Cm::SpatialVectorV& impulse,
|
||
|
|
Cm::SpatialVectorV& deltaV) const = 0;
|
||
|
|
|
||
|
|
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 = 0;
|
||
|
|
|
||
|
|
virtual Cm::SpatialVectorV getLinkVelocity(const PxU32 linkID) const = 0;
|
||
|
|
|
||
|
|
virtual Cm::SpatialVectorV getLinkMotionVector(const PxU32 linkID) const = 0;
|
||
|
|
|
||
|
|
virtual PxReal getLinkMaxPenBias(const PxU32 linkID) const = 0;
|
||
|
|
|
||
|
|
virtual void pxcFsApplyImpulse( PxU32 linkID, Ps::aos::Vec3V linear,
|
||
|
|
Ps::aos::Vec3V angular, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV) = 0;
|
||
|
|
|
||
|
|
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) = 0;
|
||
|
|
|
||
|
|
virtual void solveInternalConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
|
||
|
|
bool velIteration, bool isTGS, const PxReal elapsedTime) = 0;
|
||
|
|
|
||
|
|
virtual void writebackInternalConstraints(bool isTGS) = 0;
|
||
|
|
|
||
|
|
virtual void prepareStaticConstraints(const PxReal /*dt*/, const PxReal /*invDt*/, PxsContactManagerOutputIterator& /*outputs*/,
|
||
|
|
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*/, ThreadContext& /*threadContext*/, PxReal /*correlationDist*/, PxReal /*bounceThreshold*/,
|
||
|
|
PxReal /*frictionOffsetThreshold*/, PxTGSSolverBodyData* /*solverBodyData*/,
|
||
|
|
PxTGSSolverBodyTxInertia* /*txInertia*/, PxsConstraintBlockManager& /*blockManager*/, Dy::ConstraintWriteback* /*constraintWritebackPool*/,
|
||
|
|
const PxU32 /*nbSubsteps*/, const PxReal /*lengthScale*/) {}
|
||
|
|
|
||
|
|
virtual void pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1) = 0;
|
||
|
|
|
||
|
|
virtual Cm::SpatialVectorV pxcFsGetVelocity(PxU32 linkID) = 0;
|
||
|
|
|
||
|
|
virtual Cm::SpatialVectorV pxcFsGetVelocityTGS(PxU32 linkID) = 0;
|
||
|
|
|
||
|
|
virtual const PxTransform& getCurrentTransform(PxU32 linkID) const= 0;
|
||
|
|
|
||
|
|
virtual const PxQuat& getDeltaQ(PxU32 linkID) const = 0;
|
||
|
|
|
||
|
|
virtual bool storeStaticConstraint(const PxSolverConstraintDesc& /*desc*/) { return false; }
|
||
|
|
|
||
|
|
virtual bool willStoreStaticConstraint() { return false; }
|
||
|
|
|
||
|
|
//this is called by island gen to determine whether the articulation should be awake or sleep
|
||
|
|
virtual Cm::SpatialVector getMotionVelocity(const PxU32 linkID) const = 0;
|
||
|
|
|
||
|
|
virtual Cm::SpatialVector getMotionAcceleration(const PxU32 linkID) const = 0;
|
||
|
|
|
||
|
|
void setupLinks(PxU32 nbLinks, Dy::ArticulationLink* links)
|
||
|
|
{
|
||
|
|
//if this is needed, we need to re-allocated the link data
|
||
|
|
resize(nbLinks);
|
||
|
|
|
||
|
|
getSolverDesc().links = links;
|
||
|
|
getSolverDesc().linkCount = Ps::to8(nbLinks);
|
||
|
|
|
||
|
|
//if this is needed, we need to re-allocated the joint data
|
||
|
|
onUpdateSolverDesc();
|
||
|
|
}
|
||
|
|
virtual void fillIndexedManager(const PxU32 linkId, Dy::ArticulationLinkHandle& handle, PxU8& indexType) = 0;
|
||
|
|
|
||
|
|
//These variables are used in the constraint partition
|
||
|
|
PxU16 maxSolverFrictionProgress;
|
||
|
|
PxU16 maxSolverNormalProgress;
|
||
|
|
PxU32 solverProgress;
|
||
|
|
PxU8 numTotalConstraints;
|
||
|
|
protected:
|
||
|
|
void* mUserData;
|
||
|
|
Dy::Context* mContext;
|
||
|
|
PxU32 mType;
|
||
|
|
ArticulationSolverDesc mSolverDesc;
|
||
|
|
|
||
|
|
Ps::Array<Cm::SpatialVector> mAcceleration; // supplied by Sc-layer to feed into articulations
|
||
|
|
|
||
|
|
bool mUpdateSolverData;
|
||
|
|
bool mDirty; //any of links update configulations, the boolean will be set to true
|
||
|
|
PxU32 mMaxDepth;
|
||
|
|
|
||
|
|
} PX_ALIGN_SUFFIX(64);
|
||
|
|
|
||
|
|
#if PX_VC
|
||
|
|
#pragma warning(pop)
|
||
|
|
#endif
|
||
|
|
|
||
|
|
PX_FORCE_INLINE ArticulationV* getArticulation(ArticulationLinkHandle handle)
|
||
|
|
{
|
||
|
|
return reinterpret_cast<ArticulationV*>(handle & ~DY_ARTICULATION_IDMASK);
|
||
|
|
}
|
||
|
|
|
||
|
|
PX_FORCE_INLINE bool isArticulationRootLink(ArticulationLinkHandle handle)
|
||
|
|
{
|
||
|
|
return !(handle & DY_ARTICULATION_IDMASK);
|
||
|
|
}
|
||
|
|
|
||
|
|
PX_FORCE_INLINE PxU32 getLinkIndex(ArticulationLinkHandle handle)
|
||
|
|
{
|
||
|
|
return PxU32(handle&DY_ARTICULATION_IDMASK);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|