GRK/dependencies/physx-4.1/source/lowleveldynamics/include/DyVArticulation.h

522 lines
18 KiB
C
Raw Normal View History

2022-01-12 16:07:16 +01:00
//
// 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