dev0/PhysX4.1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init

Browse Source
This commit is contained in:
dev0
2025-11-28 23:13:44 +05:30
commit a3a8e79709
7360 changed files with 1156074 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

298
physx/source/lowleveldynamics/include/DyArticulation.h Normal file
View File

@ -0,0 +1,298 @@
//
// 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-2021 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_ARTICULATION_H
#define PXD_ARTICULATION_H
#include "foundation/PxVec3.h"
#include "foundation/PxQuat.h"
#include "foundation/PxTransform.h"
#include "PsVecMath.h"
#include "CmUtils.h"
#include "CmSpatialVector.h"
#include "PxArticulationJoint.h"
#include "DyVArticulation.h"
namespace physx
{
class PxConstraintAllocator;
class PxcConstraintBlockStream;
struct PxSolverConstraintDesc;
class PxsConstraintBlockManager;
#define DY_DEBUG_ARTICULATION 0
namespace Dy
{
struct ArticulationJointTransforms;
struct FsInertia;
struct FsData;
#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 Articulation : public ArticulationV
{
public:
// public interface
Articulation(void*);
~Articulation();
virtual bool resize(const PxU32 linkCount);
virtual void onUpdateSolverDesc()
{
PxMemZero(mExternalLoads.begin(), sizeof(Ps::aos::Mat33V) * mExternalLoads.size());
PxMemZero(mInternalLoads.begin(), sizeof(Ps::aos::Mat33V) * mExternalLoads.size());
}
FsData* getFsDataPtr() const { return reinterpret_cast<FsData *>(mFsDataBytes.begin()); }
//void setFsDataPtr(FsData* data) { mFsData = data; }
// get data sizes for allocation at higher levels
virtual void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize);
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 recordDeltaMotion(const ArticulationSolverDesc &desc, const PxReal dt, Cm::SpatialVectorF* deltaV, PxReal);
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 solveInternalConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
bool velIteration, bool isTGS, const PxReal elapsedTime);
virtual void writebackInternalConstraints(bool /*isTGS*/) {}
virtual void concludeInternalConstraints(bool /*isTGS*/) {}
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) { return Articulation::pxcFsGetVelocity(linkID); }
virtual const PxTransform& getCurrentTransform(PxU32 linkID)const
{
return mPose[linkID];
}
virtual const PxQuat& getDeltaQ(PxU32 linkID) const
{
return mDeltaQ[linkID];
}
static void prepareDataBlock(FsData& fsData,
const ArticulationLink* links,
PxU16 linkCount,
PxTransform* poses,
PxQuat* deltaQ,
FsInertia* baseInertia,
ArticulationJointTransforms* jointTransforms,
PxU32 expectedSize);
static void prepareFsData(FsData& fsData,
const ArticulationLink* links);
static PxReal getResistance(PxReal compliance);
static PxU32 getFsDataSize(PxU32 linkCount);
static PxU32 getLtbDataSize(PxU32 linkCount);
static void setInertia(FsInertia& inertia,
const PxsBodyCore& body,
const PxTransform& pose);
static void setJointTransforms(ArticulationJointTransforms& transforms,
const PxTransform& parentPose,
const PxTransform& childPose,
const ArticulationJointCore& joint);
static void applyImpulses(const FsData& matrix,
Cm::SpatialVectorV* Z,
Cm::SpatialVectorV* V);
private:
#if DY_DEBUG_ARTICULATION
// debug quantities
Cm::SpatialVector computeMomentum(const FsInertia *inertia) const;
void computeResiduals(const Cm::SpatialVector *,
const ArticulationJointTransforms* jointTransforms,
bool dump = false) const;
void checkLimits() const;
#endif
//PX_FORCE_INLINE Cm::SpatialVectorV* getVelocity(FsData& matrix);
void computeUnconstrainedVelocitiesInternal(const ArticulationSolverDesc& desc,
PxReal dt,
const PxVec3& gravity, PxU64 contextID,
FsInertia* PX_RESTRICT baseInertia,
ArticulationJointTransforms* PX_RESTRICT jointTransforms,
PxcFsScratchAllocator& allocator);
void prepareLtbMatrix(FsData& fsData,
const FsInertia* baseInertia,
const PxTransform* poses,
const ArticulationJointTransforms* jointTransforms,
PxReal recipDt);
void computeJointDrives(FsData& fsData,
Ps::aos::Vec3V* drives,
const ArticulationLink* links,
const PxTransform* poses,
const ArticulationJointTransforms* transforms,
const Ps::aos::Mat33V* loads,
PxReal dt);
mutable Ps::Array<char> mFsDataBytes; // drive cache creation (which is const) can force a resize
// persistent state of the articulation for warm-starting joint load computation
Ps::Array<Ps::aos::Mat33V> mInternalLoads;
Ps::Array<Ps::aos::Mat33V> mExternalLoads;
Ps::Array<char> mScratchMemory; // drive cache creation (which is const) can force a resize
Ps::Array<PxTransform> mPose;
Ps::Array<PxQuat> mDeltaQ;
Ps::Array<Cm::SpatialVectorV> mMotionVelocity; // saved here in solver to communicate back to island management/sleeping
} PX_ALIGN_SUFFIX(64);
#if PX_VC
#pragma warning(pop)
#endif
class PxvArticulationDriveCache
{
public:
// drive cache stuff
static void initialize(
FsData &cache,
PxU16 linkCount,
const ArticulationLink* links,
PxReal compliance,
PxU32 iterations,
char* scratchMemory,
PxU32 scratchMemorySize);
static PxU32 getLinkCount(const FsData& cache);
static void applyImpulses(const FsData& cache,
Cm::SpatialVectorV* Z,
Cm::SpatialVectorV* V);
static void getImpulseResponse(const FsData& cache,
PxU32 linkID,
const Cm::SpatialVectorV& impulse,
Cm::SpatialVectorV& deltaV);
};
void PxvRegisterArticulations();
}
}
#endif

81
physx/source/lowleveldynamics/include/DyArticulationCore.h Normal file
View File

@ -0,0 +1,81 @@
//
// 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-2021 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_CORE_H
#define PXDV_ARTICULATION_CORE_H
#include "PxArticulationReducedCoordinate.h"
namespace physx
{
namespace Dy
{
struct ArticulationCore
{
//= 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.
//==================================================================================================
PxU32 internalDriveIterations;
PxU32 externalDriveIterations;
PxU32 maxProjectionIterations;
PxU16 solverIterationCounts; //KS - made a U16 so that it matches PxsRigidCore
PxReal separationTolerance;
PxReal sleepThreshold;
PxReal freezeThreshold;
PxReal wakeCounter;
PxArticulationFlags flags;
};
struct ArticulationJointCoreDirtyFlag
{
enum Enum
{
eNONE = 0,
eMOTION = 1 << 0,
ePOSE = 1 << 1,
eTARGETPOSE = 1 << 2,
eTARGETVELOCITY = 1 << 3,
eLIMIT = 1 << 4,
eDRIVE = 1 << 5,
eALL = eMOTION | ePOSE | eTARGETPOSE | eTARGETVELOCITY
};
};
typedef PxFlags<ArticulationJointCoreDirtyFlag::Enum, PxU8> ArticulationJointCoreDirtyFlags;
PX_FLAGS_OPERATORS(ArticulationJointCoreDirtyFlag::Enum, PxU8)
}
}
#endif

129
physx/source/lowleveldynamics/include/DyArticulationJointCore.h Normal file
View File

@ -0,0 +1,129 @@
//
// 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-2021 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 DV_ARTICULATION_JOINT_CORE_H
#define DV_ARTICULATION_JOINT_CORE_H
#include "DyArticulationCore.h"
#include "PxArticulationJoint.h"
#include "solver/PxSolverDefs.h"
#include "PxArticulationJointReducedCoordinate.h"
namespace physx
{
namespace Dy
{
struct 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.
//==================================================================================================
public:
PX_CUDA_CALLABLE bool setJointPose(PxQuat& relativeQuat)
{
if (dirtyFlag & ArticulationJointCoreDirtyFlag::ePOSE)
{
relativeQuat = (childPose.q * (parentPose.q.getConjugate())).getNormalized();
//ML: this way work in GPU
PxU8 flag = PxU8(ArticulationJointCoreDirtyFlag::ePOSE);
dirtyFlag &= ArticulationJointCoreDirtyFlags(~flag);
return true;
}
return false;
}
PX_CUDA_CALLABLE PX_FORCE_INLINE void operator=(ArticulationJointCoreBase& other)
{
parentPose = other.parentPose;
childPose = other.childPose;
dirtyFlag = other.dirtyFlag;
//KS - temp place to put reduced coordinate limit and drive values
for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
{
limits[i] = other.limits[i];
drives[i] = other.drives[i];
targetP[i] = other.targetP[i];
targetV[i] = other.targetV[i];
dofIds[i] = other.dofIds[i];
motion[i] = other.motion[i];
}
frictionCoefficient = other.frictionCoefficient;
maxJointVelocity = other.maxJointVelocity;
//relativeQuat = other.relativeQuat;
jointType = other.jointType;
jointOffset = other.jointOffset; //this is the dof offset for the joint in the cache
}
// attachment points, don't change the order, otherwise it will break GPU code
PxTransform parentPose; //28 28
PxTransform childPose; //28 56
//KS - temp place to put reduced coordinate limit and drive values
PxArticulationLimit limits[PxArticulationAxis::eCOUNT]; //48 104
PxArticulationDrive drives[PxArticulationAxis::eCOUNT]; //96 200
PxReal targetP[PxArticulationAxis::eCOUNT]; //24 224
PxReal targetV[PxArticulationAxis::eCOUNT]; //24 248
// initial parent to child rotation. Could be
//PxQuat relativeQuat; //16 264
PxReal frictionCoefficient; //4 268
PxU8 dofIds[PxArticulationAxis::eCOUNT]; //6 274
PxU8 motion[PxArticulationAxis::eCOUNT]; //6 280
PxReal maxJointVelocity; //4 284
//Currently, jointOffset can't exceed 64*3 so we can use a PxU8 here! This brings mem footprint to exactly a multiple of 16 bytes
//this is the dof offset for the joint in the cache.
PxU8 jointOffset; //1 285
ArticulationJointCoreDirtyFlags dirtyFlag; //1 286
PxU8 jointType; //1 287
PxU8 pad[1];
ArticulationJointCoreBase() { maxJointVelocity = 100.f; }
// PX_SERIALIZATION
ArticulationJointCoreBase(const PxEMPTY&) : dirtyFlag(PxEmpty) { PX_COMPILE_TIME_ASSERT(sizeof(PxArticulationMotions) == sizeof(PxU8)); }
//~PX_SERIALIZATION
};
}
}
#endif

88
physx/source/lowleveldynamics/include/DyConstraint.h Normal file
View File

@ -0,0 +1,88 @@
//
// 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-2021 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_SHADER_H
#define PXD_SHADER_H
#include "foundation/PxVec3.h"
#include "foundation/PxTransform.h"
#include "PxvConfig.h"
#include "PxvDynamics.h"
#include "PxConstraint.h"
#include "DyConstraintWriteBack.h"
namespace physx
{
class PxsRigidBody;
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
PX_ALIGN_PREFIX(16)
struct Constraint
{
public:
PxReal linBreakForce; //0
PxReal angBreakForce; //4
PxU16 constantBlockSize; //6
PxU16 flags; //8
PxConstraintSolverPrep solverPrep; //12
PxConstraintProject project; //16
void* constantBlock; //20
PxsRigidBody* body0; //24
PxsRigidBody* body1; //28
PxsBodyCore* bodyCore0; //32
PxsBodyCore* bodyCore1; //36
PxU32 index; //40 //this is also a constraint write back index
PxReal minResponseThreshold; //44
}
PX_ALIGN_SUFFIX(16);
#if PX_VC
#pragma warning(pop)
#endif
#if !PX_P64_FAMILY
PX_COMPILE_TIME_ASSERT(48==sizeof(Constraint));
#endif
}
}
#endif

65
physx/source/lowleveldynamics/include/DyConstraintWriteBack.h Normal file
View File

@ -0,0 +1,65 @@
//
// 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-2021 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_CONSTRAINT_WRITE_BACK_H
#define PXD_CONSTRAINT_WRITE_BACK_H
#include "foundation/PxVec3.h"
#include "PxvConfig.h"
#include "PxvDynamics.h"
namespace physx
{
namespace Dy
{
PX_ALIGN_PREFIX(16)
struct ConstraintWriteback
{
public:
void initialize()
{
linearImpulse = PxVec3(0);
angularImpulse = PxVec3(0);
broken = false;
}
PxVec3 linearImpulse;
PxU32 broken;
PxVec3 angularImpulse;
PxU32 pad;
}
PX_ALIGN_SUFFIX(16);
}
}
#endif

419
physx/source/lowleveldynamics/include/DyContext.h Normal file
View File

@ -0,0 +1,419 @@
//
// 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-2021 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 PXV_DYNAMICS_CONTEXT_H
#define PXV_DYNAMICS_CONTEXT_H
#include "CmPhysXCommon.h"
#include "PxSceneDesc.h"
#include "DyThresholdTable.h"
#include "PxcNpThreadContext.h"
#include "PxsSimulationController.h"
#include "DyConstraintWriteBack.h"
#include "PsAllocator.h"
#define DY_MAX_VELOCITY_COUNT 4
namespace physx
{
class PxsIslandManager;
class PxcNpMemBlockPool;
namespace Cm
{
class EventProfiler;
class FlushPool;
}
namespace IG
{
class SimpleIslandManager;
class IslandSim;
}
template<typename T, typename P> class PxcThreadCoherentCache;
class PxcScratchAllocator;
struct PxvSimStats;
class PxTaskManager;
class PxcNpMemBlockPool;
struct PxgDynamicsMemoryConfig;
class PxsContactManagerOutputIterator;
struct PxsContactManagerOutput;
class PxsKernelWranglerManager;
class PxsHeapMemoryAllocator;
class PxsMemoryManager;
class PxsContactManager;
namespace Dy
{
class Context
{
PX_NOCOPY(Context)
public:
/**
\brief Returns the bounce threshold
\return The bounce threshold.
*/
PX_FORCE_INLINE PxReal getBounceThreshold() const { return mBounceThreshold; }
/**
\brief Returns the friction offset threshold
\return The friction offset threshold.
*/
PX_FORCE_INLINE PxReal getFrictionOffsetThreshold() const { return mFrictionOffsetThreshold; }
/**
\brief Returns the friction offset threshold
\return The friction offset threshold.
*/
PX_FORCE_INLINE PxReal getSolverOffsetSlop() const { return mSolverOffsetSlop; }
/**
\brief Returns the correlation distance
\return The correlation distance.
*/
PX_FORCE_INLINE PxReal getCorrelationDistance() const { return mCorrelationDistance; }
/**
\brief Returns the CCD separation threshold
\return The CCD separation threshold.
*/
PX_FORCE_INLINE PxReal getCCDSeparationThreshold() const { return mCCDSeparationThreshold; }
/**
\brief Sets the bounce threshold
\param[in] f The bounce threshold
*/
PX_FORCE_INLINE void setBounceThreshold(PxReal f) { mBounceThreshold = f; }
/**
\brief Sets the correlation distance
\param[in] f The correlation distance
*/
PX_FORCE_INLINE void setCorrelationDistance(PxReal f) { mCorrelationDistance = f; }
/**
\brief Sets the friction offset threshold
\param[in] offset The friction offset threshold
*/
PX_FORCE_INLINE void setFrictionOffsetThreshold(PxReal offset) { mFrictionOffsetThreshold = offset; }
/**
\brief Sets the solver offset slop
\param[in] offset The solver offset slop
*/
PX_FORCE_INLINE void setSolverOffsetSlop(PxReal offset) { mSolverOffsetSlop = offset; }
/**
\brief Sets the friction offset threshold
\param[in] offset The friction offset threshold
*/
PX_FORCE_INLINE void setCCDSeparationThreshold(PxReal offset) { mCCDSeparationThreshold = offset; }
/**
\brief Returns the solver batch size
\return The solver batch size.
*/
PX_FORCE_INLINE PxU32 getSolverBatchSize() const { return mSolverBatchSize; }
/**
\brief Sets the solver batch size
\param[in] f The solver batch size
*/
PX_FORCE_INLINE void setSolverBatchSize(PxU32 f) { mSolverBatchSize = f; }
/**
\brief Returns the solver batch size
\return The solver batch size.
*/
PX_FORCE_INLINE PxU32 getSolverArticBatchSize() const { return mSolverArticBatchSize; }
/**
\brief Sets the solver batch size
\param[in] f The solver batch size
*/
PX_FORCE_INLINE void setSolverArticBatchSize(PxU32 f) { mSolverArticBatchSize = f; }
/**
\brief Returns the maximum solver constraint size
\return The maximum solver constraint size in this island in bytes.
*/
PX_FORCE_INLINE PxU32 getMaxSolverConstraintSize() const { return mMaxSolverConstraintSize; }
/**
\brief Returns the friction model being used.
\return The friction model being used.
*/
PX_FORCE_INLINE PxFrictionType::Enum getFrictionType() const { return mFrictionType; }
/**
\brief Returns the threshold stream
\return The threshold stream
*/
PX_FORCE_INLINE ThresholdStream& getThresholdStream() { return *mThresholdStream; }
PX_FORCE_INLINE ThresholdStream& getForceChangedThresholdStream() { return *mForceChangedThresholdStream; }
/**
\brief Returns the threshold table
\return The threshold table
*/
PX_FORCE_INLINE ThresholdTable& getThresholdTable() { return mThresholdTable; }
/**
\brief Sets the friction model to be used.
\param[in] f The friction model to be used.
*/
PX_FORCE_INLINE void setFrictionType(PxFrictionType::Enum f) { mFrictionType = f; }
/**
\brief Destroys this dynamics context
*/
virtual void destroy() = 0;
PX_FORCE_INLINE PxcDataStreamPool& getContactStreamPool() { return mContactStreamPool; }
PX_FORCE_INLINE PxcDataStreamPool& getPatchStreamPool() { return mPatchStreamPool; }
PX_FORCE_INLINE PxcDataStreamPool& getForceStreamPool() { return mForceStreamPool; }
PX_FORCE_INLINE Ps::Array<Dy::ConstraintWriteback, Ps::VirtualAllocator>& getConstraintWriteBackPool() { return mConstraintWriteBackPool; }
/**
\brief Returns the current frame's timestep
\return The current frame's timestep.
*/
PX_FORCE_INLINE PxReal getDt() const { return mDt; }
/**
\brief Returns 1/(current frame's timestep)
\return 1/(current frame's timestep).
*/
PX_FORCE_INLINE PxReal getInvDt() const { return mInvDt; }
PX_FORCE_INLINE PxReal getMaxBiasCoefficient() const { return mMaxBiasCoefficient; }
PX_FORCE_INLINE PxVec3 getGravity() const { return mGravity; }
/**
\brief The entry point for the constraint solver.
\param[in] dt The simulation time-step
\param[in] continuation The continuation task for the solver
\param[in] processLostTouchTask The task that processes lost touches
This method is called after the island generation has completed. Its main responsibilities are:
(1) Reserving the solver body pools
(2) Initializing the static and kinematic solver bodies, which are shared resources between islands.
(3) Construct the solver task chains for each island
Each island is solved as an independent solver task chain. In addition, large islands may be solved using multiple parallel tasks.
Island solving is asynchronous. Once all islands have been solved, the continuation task will be called.
*/
virtual void update(IG::SimpleIslandManager& simpleIslandManager, PxBaseTask* continuation, PxBaseTask* processLostTouchTask,
PxsContactManager** foundPatchManagers, PxU32 nbFoundPatchManagers, PxsContactManager** lostPatchManagers, PxU32 nbLostPatchManagers, PxU32 maxPatchesPerCM,
PxsContactManagerOutputIterator& iterator, PxsContactManagerOutput* gpuOutputs, const PxReal dt, const PxVec3& gravity, const PxU32 bitMapWordCounts) = 0;
virtual void processLostPatches(IG::SimpleIslandManager& simpleIslandManager, PxsContactManager** lostPatchManagers, PxU32 nbLostPatchManagers, PxsContactManagerOutputIterator& iterator) = 0;
/**
\brief This method copy gpu solver body data to cpu body core
*/
virtual void updateBodyCore(PxBaseTask* continuation) = 0;
/**
\brief Called after update's task chain has completed. This collects the results of the solver together
*/
virtual void mergeResults() = 0;
virtual void setSimulationController(PxsSimulationController* simulationController) = 0;
virtual void getDataStreamBase(void*& contactStreamBase, void*& patchStreamBase, void*& forceAndIndiceStreamBase) = 0;
void createThresholdStream(Ps::VirtualAllocatorCallback& callback) { PX_ASSERT(mThresholdStream == NULL); mThresholdStream = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(ThresholdStream), PX_DEBUG_EXP("ThresholdStream")), ThresholdStream(callback));}
void createForceChangeThresholdStream(Ps::VirtualAllocatorCallback& callback) { PX_ASSERT(mForceChangedThresholdStream == NULL); mForceChangedThresholdStream = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(ThresholdStream), PX_DEBUG_EXP("ThresholdStream")), ThresholdStream(callback));}
protected:
Context(IG::IslandSim* accurateIslandSim, Ps::VirtualAllocatorCallback* allocatorCallback,
PxvSimStats& simStats, bool enableStabilization, bool useEnhancedDeterminism, bool useAdaptiveForce,
const PxReal maxBiasCoefficient) :
mThresholdStream(NULL),
mForceChangedThresholdStream(NULL),
mAccurateIslandSim(accurateIslandSim),
mDt (1.0f),
mInvDt (1.0f),
mMaxBiasCoefficient (maxBiasCoefficient),
mEnableStabilization (enableStabilization),
mUseEnhancedDeterminism (useEnhancedDeterminism),
mUseAdaptiveForce (useAdaptiveForce),
mBounceThreshold(-2.0f),
mSolverBatchSize(32),
mConstraintWriteBackPool(Ps::VirtualAllocator(allocatorCallback)),
mSimStats(simStats)
{
}
virtual ~Context()
{
if(mThresholdStream)
{
mThresholdStream->~ThresholdStream();
PX_FREE(mThresholdStream);
}
mThresholdStream = NULL;
if(mForceChangedThresholdStream)
{
mForceChangedThresholdStream->~ThresholdStream();
PX_FREE(mForceChangedThresholdStream);
}
mForceChangedThresholdStream = NULL;
}
ThresholdStream* mThresholdStream;
ThresholdStream* mForceChangedThresholdStream;
ThresholdTable mThresholdTable;
IG::IslandSim* mAccurateIslandSim;
PxsSimulationController* mSimulationController;
/**
\brief Time-step.
*/
PxReal mDt;
/**
\brief 1/time-step.
*/
PxReal mInvDt;
PxReal mMaxBiasCoefficient;
const bool mEnableStabilization;
const bool mUseEnhancedDeterminism;
const bool mUseAdaptiveForce;
PxVec3 mGravity;
/**
\brief max solver constraint size
*/
PxU32 mMaxSolverConstraintSize;
/**
\brief Threshold controlling the relative velocity at which the solver transitions between restitution and bias for solving normal contact constraint.
*/
PxReal mBounceThreshold;
/**
\brief Threshold controlling whether friction anchors are constructed or not. If the separation is above mFrictionOffsetThreshold, the contact will not be considered to become a friction anchor
*/
PxReal mFrictionOffsetThreshold;
/**
\brief Tolerance used to zero offsets along an axis if it is below this threshold. Used to compensate for small numerical divergence inside contact gen.
*/
PxReal mSolverOffsetSlop;
/**
\brief Threshold controlling whether distant contacts are processed using bias, restitution or a combination of the two. This only has effect on pairs involving bodies that have enabled speculative CCD simulation mode.
*/
PxReal mCCDSeparationThreshold;
/**
\brief Threshold for controlling friction correlation
*/
PxReal mCorrelationDistance;
/**
\brief The minimum size of an island to generate a solver task chain.
*/
PxU32 mSolverBatchSize;
/**
\brief The minimum number of articulations required to generate a solver task chain.
*/
PxU32 mSolverArticBatchSize;
/**
\brief The current friction model being used
*/
PxFrictionType::Enum mFrictionType;
/**
\brief Structure to encapsulate contact stream allocations. Used by GPU solver to reference pre-allocated pinned host memory
*/
PxcDataStreamPool mContactStreamPool;
/**
\brief Struct to encapsulate the contact patch stream allocations. Used by GPU solver to reference pre-allocated pinned host memory
*/
PxcDataStreamPool mPatchStreamPool;
/**
\brief Structure to encapsulate force stream allocations. Used by GPU solver to reference pre-allocated pinned host memory for force reports.
*/
PxcDataStreamPool mForceStreamPool;
/**
\brief Structure to encapsulate constraint write back allocations. Used by GPU/CPU solver to reference pre-allocated pinned host memory for breakable joint reports.
*/
Ps::Array<Dy::ConstraintWriteback, Ps::VirtualAllocator> mConstraintWriteBackPool;
PxvSimStats& mSimStats;
};
Context* createDynamicsContext( PxcNpMemBlockPool* memBlockPool,
PxcScratchAllocator& scratchAllocator, Cm::FlushPool& taskPool,
PxvSimStats& simStats, PxTaskManager* taskManager, Ps::VirtualAllocatorCallback* allocatorCallback, PxsMaterialManager* materialManager,
IG::IslandSim* accurateIslandSim, PxU64 contextID,
const bool enableStabilization, const bool useEnhancedDeterminism, const bool useAdaptiveForce, const PxReal maxBiasCoefficient,
const bool frictionEveryIteration
);
Context* createTGSDynamicsContext(PxcNpMemBlockPool* memBlockPool,
PxcScratchAllocator& scratchAllocator, Cm::FlushPool& taskPool,
PxvSimStats& simStats, PxTaskManager* taskManager, Ps::VirtualAllocatorCallback* allocatorCallback, PxsMaterialManager* materialManager,
IG::IslandSim* accurateIslandSim, PxU64 contextID,
const bool enableStabilization, const bool useEnhancedDeterminism, const bool useAdaptiveForce, const PxReal lengthScale
);
}
}
#endif

1033
physx/source/lowleveldynamics/include/DyFeatherstoneArticulation.h Normal file
View File

File diff suppressed because it is too large Load Diff

261
physx/source/lowleveldynamics/include/DyFeatherstoneArticulationJointData.h Normal file
View File

@ -0,0 +1,261 @@
//
// 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-2021 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_JOINTCORE_H
#define PXD_FEATHERSTONE_ARTICULATION_JOINTCORE_H
#include "foundation/PxVec3.h"
#include "foundation/PxQuat.h"
#include "foundation/PxTransform.h"
#include "PsVecMath.h"
#include "CmUtils.h"
#include "CmSpatialVector.h"
#include "DyVArticulation.h"
#include "DyFeatherstoneArticulationUtils.h"
#include "DyArticulationJointCore.h"
#include <stdio.h>
namespace physx
{
namespace Dy
{
class PX_ALIGN_PREFIX(16) ArticulationJointCoreData
{
public:
ArticulationJointCoreData() : jointOffset(0xffffffff), dofInternalConstraintMask(0)
{
}
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeMotionMatrix(ArticulationJointCoreBase* joint,
SpatialSubspaceMatrix& motionMatrix)
{
const PxVec3 childOffset = -joint->childPose.p;
//transpose(Tc)*S = 0
//transpose(Ta)*S = 1
switch (joint->jointType)
{
case PxArticulationJointType::ePRISMATIC:
{
const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[0];
const PxVec3 u = (joint->childPose.rotate(jJointAxis.bottom)).getNormalized();
motionMatrix.setNumColumns(1);
motionMatrix.setColumn(0, PxVec3(0.f), u);
PX_ASSERT(dof == 1);
break;
}
case PxArticulationJointType::eREVOLUTE:
{
const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[0];
const PxVec3 u = (joint->childPose.rotate(jJointAxis.top)).getNormalized();
const PxVec3 uXd = u.cross(childOffset);
motionMatrix.setNumColumns(1);
motionMatrix.setColumn(0, u, uXd);
break;
}
case PxArticulationJointType::eSPHERICAL:
{
motionMatrix.setNumColumns(dof);
for (PxU32 ind = 0; ind <dof; ++ind)
{
const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[ind];
const PxVec3 u = (joint->childPose.rotate(jJointAxis.top)).getNormalized();
const PxVec3 uXd = u.cross(childOffset);
motionMatrix.setColumn(ind, u, uXd);
}
break;
}
case PxArticulationJointType::eFIX:
{
motionMatrix.setNumColumns(0);
PX_ASSERT(dof == 0);
break;
}
default:
break;
}
}
PX_CUDA_CALLABLE PX_FORCE_INLINE PxU8 computeJointDofs(ArticulationJointCoreBase* joint) const
{
PxU8 tDof = 0;
for (PxU32 i = 0; i < DY_MAX_DOF; ++i)
{
if (joint->motion[i] != PxArticulationMotion::eLOCKED)
{
tDof++;
}
}
return tDof;
}
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeJointDof(ArticulationJointCoreBase* joint, const bool forceRecompute)
{
if (joint->dirtyFlag & ArticulationJointCoreDirtyFlag::eMOTION || forceRecompute)
{
dof = 0;
lockedAxes = 0;
limitedAxes = 0;
//KS - no need to zero memory here.
//PxMemZero(jointAxis, sizeof(jointAxis));
for (PxU8 i = 0; i < DY_MAX_DOF; ++i)
{
if (joint->motion[i] != PxArticulationMotion::eLOCKED)
{
//axis is in the local space of joint
jointAxis[dof][i] = 1.f;
if (joint->motion[i] == PxArticulationMotion::eLIMITED)
{
limitedAxes++;
}
joint->dofIds[dof++] = i;
}
}
lockedAxes = 0;
#if 1
//Spherical joints treat locked axes as free axes with a constraint. This produces better
//results for spherical joints with 2 dofs free, where keeping the 3rd axis locked can lead to
//an over-consrtained behaviour that is undesirable. However, the drawback is that there will be
//some drift and error on the joint axes
if (joint->jointType == PxArticulationJointType::eSPHERICAL && dof == 2)
{
for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i)
{
if (joint->motion[i] == PxArticulationMotion::eLOCKED)
{
//axis is in the local space of joint
jointAxis[dof][i] = 1.f;
joint->dofIds[dof++] = PxU8(i);
lockedAxes++;
}
}
}
#endif
joint->dirtyFlag &= (~ArticulationJointCoreDirtyFlag::eMOTION);
}
}
//in the joint space
Cm::UnAlignedSpatialVector jointAxis[3]; //72
//this is the dof offset for the joint in the cache
PxU32 jointOffset; //76
//degree of freedom
PxU8 dof; //77
PxU8 limitedAxes; //78
PxU8 dofInternalConstraintMask; //79
PxU8 lockedAxes; //80
} PX_ALIGN_SUFFIX(16);
struct PX_ALIGN_PREFIX(16) ArticulationJointTargetData
{
PxReal targetJointVelocity[3]; //12
PxReal targetJointPosition[3]; //24
Cm::UnAlignedSpatialVector worldJointAxis[3]; //96
//PxU32 pad[2];
ArticulationJointTargetData()
{
for (PxU32 i = 0; i < 3; ++i)
{
targetJointPosition[i] = 0.f;
targetJointVelocity[i] = 0.f;
}
}
PX_CUDA_CALLABLE PX_FORCE_INLINE void setJointVelocityDrive(ArticulationJointCoreBase* joint)
{
if (joint->dirtyFlag & ArticulationJointCoreDirtyFlag::eTARGETVELOCITY)
{
PxU32 count = 0;
for (PxU32 i = 0; i < DY_MAX_DOF; ++i)
{
if (joint->motion[i] != PxArticulationMotion::eLOCKED)
{
targetJointVelocity[count] = joint->targetV[i];
count++;
}
}
joint->dirtyFlag &= ~ArticulationJointCoreDirtyFlag::eTARGETVELOCITY;
}
}
PX_CUDA_CALLABLE PX_FORCE_INLINE void setJointPoseDrive(ArticulationJointCoreBase* joint)
{
if (joint->dirtyFlag & ArticulationJointCoreDirtyFlag::eTARGETPOSE)
{
PxU32 count = 0;
for (PxU32 i = 0; i < DY_MAX_DOF; ++i)
{
if (joint->motion[i] != PxArticulationMotion::eLOCKED)
{
targetJointPosition[count] = joint->targetP[i];
count++;
}
}
joint->dirtyFlag &= ~ArticulationJointCoreDirtyFlag::eTARGETPOSE;
}
}
} PX_ALIGN_SUFFIX(16);
}//namespace Dy
}
#endif

1021
physx/source/lowleveldynamics/include/DyFeatherstoneArticulationUtils.h Normal file
View File

File diff suppressed because it is too large Load Diff

42
physx/source/lowleveldynamics/include/DySleepingConfigulation.h Normal file
View File

@ -0,0 +1,42 @@
//
// 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-2021 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_SLEEPING_CONFIGULATION_H
#define PXD_SLEEPING_CONFIGULATION_H
#define PXD_FREEZE_INTERVAL 1.5f
#define PXD_FREE_EXIT_THRESHOLD 4.f
#define PXD_FREEZE_TOLERANCE 0.25f
#define PXD_SLEEP_DAMPING 0.5f
#define PXD_ACCEL_LOSS 0.9f
#define PXD_FREEZE_SCALE 0.1f
#endif

280
physx/source/lowleveldynamics/include/DyThresholdTable.h Normal file
View File

@ -0,0 +1,280 @@
//
// 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-2021 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_THRESHOLDTABLE_H
#define PXD_THRESHOLDTABLE_H
#include "Ps.h"
#include "PsArray.h"
#include "CmPhysXCommon.h"
#include "PsAllocator.h"
#include "PsHash.h"
#include "foundation/PxMemory.h"
#include "PxsIslandNodeIndex.h"
namespace physx
{
class PxsRigidBody;
namespace Sc
{
class ShapeInteraction;
}
namespace Dy
{
struct ThresholdStreamElement
{
Sc::ShapeInteraction* shapeInteraction; //4 8
PxReal normalForce; //8 12
PxReal threshold; //12 16
IG::NodeIndex nodeIndexA; //this is the unique node index in island gen which corresonding to that body and it is persistent 16 20
IG::NodeIndex nodeIndexB; //This is the unique node index in island gen which corresonding to that body and it is persistent 20 24
PxReal accumulatedForce; //24 28
PxU32 pad; //28 32
#if !PX_P64_FAMILY
PxU32 pad1; //32
#endif // !PX_X64
PX_CUDA_CALLABLE bool operator <= (const ThresholdStreamElement& otherPair) const
{
return ((nodeIndexA < otherPair.nodeIndexA) ||(nodeIndexA == otherPair.nodeIndexA && nodeIndexB <= otherPair.nodeIndexB));
}
};
typedef Ps::Array<ThresholdStreamElement, Ps::VirtualAllocator> ThresholdArray;
class ThresholdStream : public ThresholdArray
{
public:
ThresholdStream(Ps::VirtualAllocatorCallback& allocatorCallback) : ThresholdArray(Ps::VirtualAllocator(&allocatorCallback))
{
}
};
class ThresholdTable
{
public:
ThresholdTable()
: mBuffer(NULL),
mHash(NULL),
mHashSize(0),
mHashCapactiy(0),
mPairs(NULL),
mNexts(NULL),
mPairsSize(0),
mPairsCapacity(0)
{
}
~ThresholdTable()
{
if(mBuffer) PX_FREE(mBuffer);
}
void build(const ThresholdStream& stream);
bool check(const ThresholdStream& stream, const PxU32 nodexIndexA, const PxU32 nodexIndexB, PxReal dt);
bool check(const ThresholdStream& stream, const ThresholdStreamElement& elem, PxU32& thresholdIndex);
//private:
static const PxU32 NO_INDEX = 0xffffffff;
struct Pair
{
PxU32 thresholdStreamIndex;
PxReal accumulatedForce;
//PxU32 next; // hash key & next ptr
};
PxU8* mBuffer;
PxU32* mHash;
PxU32 mHashSize;
PxU32 mHashCapactiy;
Pair* mPairs;
PxU32* mNexts;
PxU32 mPairsSize;
PxU32 mPairsCapacity;
};
namespace
{
static PX_FORCE_INLINE PxU32 computeHashKey(const PxU32 nodeIndexA, const PxU32 nodeIndexB, const PxU32 hashCapacity)
{
return (Ps::hash(PxU64(nodeIndexA)<<32 | PxU64(nodeIndexB)) % hashCapacity);
}
}
inline bool ThresholdTable::check(const ThresholdStream& stream, const ThresholdStreamElement& elem, PxU32& thresholdIndex)
{
PxU32* PX_RESTRICT hashes = mHash;
PxU32* PX_RESTRICT nextIndices = mNexts;
Pair* PX_RESTRICT pairs = mPairs;
PX_ASSERT(elem.nodeIndexA < elem.nodeIndexB);
PxU32 hashKey = computeHashKey(elem.nodeIndexA.index(), elem.nodeIndexB.index(), mHashSize);
PxU32 pairIndex = hashes[hashKey];
while(NO_INDEX != pairIndex)
{
Pair& pair = pairs[pairIndex];
const PxU32 thresholdStreamIndex = pair.thresholdStreamIndex;
PX_ASSERT(thresholdStreamIndex < stream.size());
const ThresholdStreamElement& otherElement = stream[thresholdStreamIndex];
if(otherElement.nodeIndexA==elem.nodeIndexA && otherElement.nodeIndexB==elem.nodeIndexB && otherElement.shapeInteraction == elem.shapeInteraction)
{
thresholdIndex = thresholdStreamIndex;
return true;
}
pairIndex = nextIndices[pairIndex];
}
thresholdIndex = NO_INDEX;
return false;
}
inline void ThresholdTable::build(const ThresholdStream& stream)
{
//Handle the case of an empty stream.
if(0==stream.size())
{
mPairsSize=0;
mPairsCapacity=0;
mHashSize=0;
mHashCapactiy=0;
if(mBuffer) PX_FREE(mBuffer);
mBuffer = NULL;
return;
}
//Realloc/resize if necessary.
const PxU32 pairsCapacity = stream.size();
const PxU32 hashCapacity = pairsCapacity*2+1;
if((pairsCapacity > mPairsCapacity) || (pairsCapacity < (mPairsCapacity >> 2)))
{
if(mBuffer) PX_FREE(mBuffer);
const PxU32 pairsByteSize = sizeof(Pair)*pairsCapacity;
const PxU32 nextsByteSize = sizeof(PxU32)*pairsCapacity;
const PxU32 hashByteSize = sizeof(PxU32)*hashCapacity;
const PxU32 totalByteSize = pairsByteSize + nextsByteSize + hashByteSize;
mBuffer = reinterpret_cast<PxU8*>(PX_ALLOC(totalByteSize, "PxThresholdStream"));
PxU32 offset = 0;
mPairs = reinterpret_cast<Pair*>(mBuffer + offset);
offset += pairsByteSize;
mNexts = reinterpret_cast<PxU32*>(mBuffer + offset);
offset += nextsByteSize;
mHash = reinterpret_cast<PxU32*>(mBuffer + offset);
offset += hashByteSize;
PX_ASSERT(totalByteSize == offset);
mPairsCapacity = pairsCapacity;
mHashCapactiy = hashCapacity;
}
//Set each entry of the hash table to 0xffffffff
PxMemSet(mHash, 0xff, sizeof(PxU32)*hashCapacity);
//Init the sizes of the pairs array and hash array.
mPairsSize = 0;
mHashSize = hashCapacity;
PxU32* PX_RESTRICT hashes = mHash;
PxU32* PX_RESTRICT nextIndices = mNexts;
Pair* PX_RESTRICT pairs = mPairs;
//Add all the pairs from the stream.
PxU32 pairsSize = 0;
for(PxU32 i = 0; i < pairsCapacity; i++)
{
const ThresholdStreamElement& element = stream[i];
const IG::NodeIndex nodeIndexA = element.nodeIndexA;
const IG::NodeIndex nodeIndexB = element.nodeIndexB;
const PxF32 force = element.normalForce;
PX_ASSERT(nodeIndexA < nodeIndexB);
const PxU32 hashKey = computeHashKey(nodeIndexA.index(), nodeIndexB.index(), hashCapacity);
//Get the index of the first pair found that resulted in a hash that matched hashKey.
PxU32 prevPairIndex = hashKey;
PxU32 pairIndex = hashes[hashKey];
//Search through all pairs found that resulted in a hash that matched hashKey.
//Search until the exact same body pair is found.
//Increment the accumulated force if the exact same body pair is found.
while(NO_INDEX != pairIndex)
{
Pair& pair = pairs[pairIndex];
const PxU32 thresholdStreamIndex = pair.thresholdStreamIndex;
PX_ASSERT(thresholdStreamIndex < stream.size());
const ThresholdStreamElement& otherElement = stream[thresholdStreamIndex];
if(nodeIndexA == otherElement.nodeIndexA && nodeIndexB==otherElement.nodeIndexB)
{
pair.accumulatedForce += force;
prevPairIndex = NO_INDEX;
pairIndex = NO_INDEX;
break;
}
prevPairIndex = pairIndex;
pairIndex = nextIndices[pairIndex];
}
if(NO_INDEX != prevPairIndex)
{
nextIndices[pairsSize] = hashes[hashKey];
hashes[hashKey] = pairsSize;
Pair& newPair = pairs[pairsSize];
newPair.thresholdStreamIndex = i;
newPair.accumulatedForce = force;
pairsSize++;
}
}
mPairsSize = pairsSize;
}
}
}
#endif //DY_THRESHOLDTABLE_H

523
physx/source/lowleveldynamics/include/DyVArticulation.h Normal file
View File

@ -0,0 +1,523 @@
//
// 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-2021 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 concludeInternalConstraints(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