include/solver/PxSolverDefs.h
File members: include/solver/PxSolverDefs.h
// 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-2024 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 PX_SOLVER_DEFS_H
#define PX_SOLVER_DEFS_H
#include "PxPhysXConfig.h"
#include "foundation/PxVec3.h"
#include "foundation/PxMat33.h"
#include "foundation/PxTransform.h"
#include "PxConstraintDesc.h"
#include "geomutils/PxContactPoint.h"
#if PX_VC
#pragma warning(push)
#pragma warning(disable : 4324) // structure was padded due to alignment
#endif
#if !PX_DOXYGEN
namespace physx
{
#endif
struct PxTGSSolverBodyVel;
struct PxSolverBody
{
PX_ALIGN(16, PxVec3) linearVelocity;
PxU16 maxSolverNormalProgress;
PxU16 maxSolverFrictionProgress;
PxVec3 angularState;
PxU32 solverProgress;
PxSolverBody() : linearVelocity(0.f), maxSolverNormalProgress(0), maxSolverFrictionProgress(0), angularState(0), solverProgress(0)
{
}
};
PX_COMPILE_TIME_ASSERT(sizeof(PxSolverBody) == 32);
PX_COMPILE_TIME_ASSERT((PX_OFFSET_OF(PxSolverBody, angularState) & 15) == 0);
struct PxSolverBodyData
{
PX_ALIGN(16, PxVec3 linearVelocity);
PxReal invMass;
PxVec3 angularVelocity;
PxReal reportThreshold;
PxMat33 sqrtInvInertia;
PxReal penBiasClamp;
PxU32 nodeIndex;
PxReal maxContactImpulse;
PxTransform body2World;
PxU16 pad;
PX_FORCE_INLINE PxReal projectVelocity(const PxVec3& lin, const PxVec3& ang) const
{
return linearVelocity.dot(lin) + angularVelocity.dot(ang);
}
};
PX_COMPILE_TIME_ASSERT(0 == (sizeof(PxSolverBodyData) & 15));
// PT: ensure that sqrtInvInertia is not the last member of PxSolverBodyData, i.e. it is safe to load 4 bytes after sqrtInvInertia
PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxSolverBodyData, sqrtInvInertia)+sizeof(PxSolverBodyData::sqrtInvInertia) + 4 <= sizeof(PxSolverBodyData));
//----------------------------------
struct PxConstraintBatchHeader
{
PxU32 startIndex;
PxU16 stride;
PxU16 constraintType;
};
PX_ALIGN_PREFIX(16)
struct PxSolverConstraintDesc
{
static const PxU16 RIGID_BODY = 0xffff;
enum ConstraintType
{
eCONTACT_CONSTRAINT,
eJOINT_CONSTRAINT
};
union
{
PxSolverBody* bodyA;
PxTGSSolverBodyVel* tgsBodyA;
void* articulationA;
};
union
{
PxSolverBody* bodyB;
PxTGSSolverBodyVel* tgsBodyB;
void* articulationB;
};
PxU32 bodyADataIndex;
PxU32 bodyBDataIndex;
PxU32 linkIndexA;
PxU32 linkIndexB;
PxU8* constraint;
void* writeBack;
void* writeBackFriction;
PxU16 progressA;
PxU16 progressB;
union
{
PxU16 constraintType;
PxU16 constraintLengthOver16;
};
}PX_ALIGN_SUFFIX(16);
PX_COMPILE_TIME_ASSERT(sizeof(PxSolverConstraintDesc) % 16 == 0);
struct PxSolverConstraintPrepDescBase
{
enum BodyState
{
eDYNAMIC_BODY = 1 << 0,
eSTATIC_BODY = 1 << 1,
eKINEMATIC_BODY = 1 << 2,
eARTICULATION = 1 << 3
};
PxConstraintInvMassScale invMassScales;
PxSolverConstraintDesc* desc;
const PxSolverBody* body0;
const PxSolverBody* body1;
const PxSolverBodyData* data0;
const PxSolverBodyData* data1;
PxTransform bodyFrame0;
PxTransform bodyFrame1;
// PT: these two use 4 bytes each but we only need 4 bits (or even just 2 bits) for a type
BodyState bodyState0;
BodyState bodyState1;
// PT: the following pointers have been moved here from derived structures to fill padding bytes
union
{
// PT: moved from PxSolverConstraintPrepDesc
void* writeback;
// PT: moved from PxSolverContactDesc
void* shapeInteraction;
};
};
PX_COMPILE_TIME_ASSERT(sizeof(PxSolverConstraintPrepDescBase)<=128);
// PT: ensure that we can safely read 4 bytes after the bodyFrames
PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxSolverConstraintPrepDescBase, bodyFrame0)+sizeof(PxSolverConstraintPrepDescBase::bodyFrame0) + 4 <= sizeof(PxSolverConstraintPrepDescBase));
PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxSolverConstraintPrepDescBase, bodyFrame1)+sizeof(PxSolverConstraintPrepDescBase::bodyFrame1) + 4 <= sizeof(PxSolverConstraintPrepDescBase));
struct PxSolverConstraintPrepDesc : public PxSolverConstraintPrepDescBase
{
PX_ALIGN(16, Px1DConstraint* rows);
PxU32 numRows;
PxReal linBreakForce, angBreakForce;
PxReal minResponseThreshold;
bool disablePreprocessing;
bool improvedSlerp;
bool driveLimitsAreForces;
bool extendedLimits;
bool disableConstraint;
PxVec3p body0WorldOffset;
};
struct PxSolverContactDesc : public PxSolverConstraintPrepDescBase
{
PxU8* frictionPtr;
const PxContactPoint* contacts;
PxU32 numContacts;
PxU8 frictionCount;
bool hasMaxImpulse;
bool disableStrongFriction;
bool hasForceThresholds;
PxReal restDistance;
PxReal maxCCDSeparation;
PxReal* contactForces;
PxU32 startFrictionPatchIndex;
PxU32 numFrictionPatches;
PxU32 startContactPatchIndex;
PxU16 numContactPatches;
PxU16 axisConstraintCount;
PxReal offsetSlop;
};
class PxConstraintAllocator
{
public:
virtual PxU8* reserveConstraintData(const PxU32 byteSize) = 0;
virtual PxU8* reserveFrictionData(const PxU32 byteSize) = 0;
virtual ~PxConstraintAllocator() {}
};
struct PxArticulationAxis
{
enum Enum
{
eTWIST = 0,
eSWING1 = 1,
eSWING2 = 2,
eX = 3,
eY = 4,
eZ = 5,
eCOUNT = 6
};
};
PX_FLAGS_OPERATORS(PxArticulationAxis::Enum, PxU8)
struct PxArticulationMotion
{
enum Enum
{
eLOCKED = 0,
eLIMITED = 1,
eFREE = 2
};
};
typedef PxFlags<PxArticulationMotion::Enum, PxU8> PxArticulationMotions;
PX_FLAGS_OPERATORS(PxArticulationMotion::Enum, PxU8)
struct PxArticulationJointType
{
enum Enum
{
eFIX = 0,
ePRISMATIC = 1,
eREVOLUTE = 2,
eREVOLUTE_UNWRAPPED = 3,
eSPHERICAL = 4,
eUNDEFINED = 5
};
};
struct PxArticulationFlag
{
enum Enum
{
eFIX_BASE = (1 << 0),
eDRIVE_LIMITS_ARE_FORCES = (1<<1),
eDISABLE_SELF_COLLISION = (1<<2)
};
};
typedef PxFlags<PxArticulationFlag::Enum, PxU8> PxArticulationFlags;
PX_FLAGS_OPERATORS(PxArticulationFlag::Enum, PxU8)
struct PxArticulationDriveType
{
enum Enum
{
eFORCE = 0,
eACCELERATION = 1,
eTARGET = 2,
eVELOCITY = 3,
eNONE = 4
};
};
struct PxArticulationLimit
{
PxArticulationLimit(){}
PxArticulationLimit(const PxReal low_, const PxReal high_)
{
low = low_;
high = high_;
}
PxReal low;
PxReal high;
};
struct PxArticulationDrive
{
PxArticulationDrive(){}
PxArticulationDrive(const PxReal stiffness_, const PxReal damping_, const PxReal maxForce_, PxArticulationDriveType::Enum driveType_=PxArticulationDriveType::eFORCE)
{
stiffness = stiffness_;
damping = damping_;
maxForce = maxForce_;
driveType = driveType_;
}
PxReal stiffness;
PxReal damping;
PxReal maxForce;
PxArticulationDriveType::Enum driveType;
};
struct PxTGSSolverBodyVel
{
PX_ALIGN(16, PxVec3) linearVelocity; //12
PxU16 maxDynamicPartition; //14 Used to accumulate the max partition of dynamic interactions
PxU16 nbStaticInteractions; //16 Used to accumulate the number of static interactions
PxVec3 angularVelocity; //28
PxU32 partitionMask; //32 Used in partitioning as a bit-field
PxVec3 deltaAngDt; //44
PxReal maxAngVel; //48
PxVec3 deltaLinDt; //60
PxU16 lockFlags; //62
bool isKinematic; //63
PxU8 pad; //64
PX_FORCE_INLINE PxReal projectVelocity(const PxVec3& lin, const PxVec3& ang) const
{
return linearVelocity.dot(lin) + angularVelocity.dot(ang);
}
};
//Needed only by prep, integration and 1D constraints
//Solver body state at time = simulationTime + simStepDt*posIter/nbPosIters
struct PxTGSSolverBodyTxInertia
{
//Accumulated change to quaternion that has accumulated since solver started.
PxQuat deltaBody2WorldQ;
//Absolute body position at t
PxVec3 body2WorldP;
//RotMatrix * I0^(-1/2) * RotMatrixTranspose
PxMat33 sqrtInvInertia;
};
PX_COMPILE_TIME_ASSERT(0 == (sizeof(PxTGSSolverBodyTxInertia) & 15));
struct PxTGSSolverBodyData
{
PX_ALIGN(16, PxVec3) originalLinearVelocity;
PxReal maxContactImpulse;
PxVec3 originalAngularVelocity;
PxReal penBiasClamp;
PxReal invMass;
PxU32 nodeIndex;
PxReal reportThreshold;
PxU32 pad;
PxReal projectVelocity(const PxVec3& linear, const PxVec3& angular) const
{
return originalLinearVelocity.dot(linear) + originalAngularVelocity.dot(angular);
}
};
struct PxTGSSolverConstraintPrepDescBase
{
PxConstraintInvMassScale invMassScales;
PxSolverConstraintDesc* desc;
const PxTGSSolverBodyVel* body0;
const PxTGSSolverBodyVel* body1;
const PxTGSSolverBodyTxInertia* body0TxI;
const PxTGSSolverBodyTxInertia* body1TxI;
const PxTGSSolverBodyData* bodyData0;
const PxTGSSolverBodyData* bodyData1;
PxTransform bodyFrame0;
PxTransform bodyFrame1;
// PT: these two use 4 bytes each but we only need 4 bits (or even just 2 bits) for a type
PxSolverContactDesc::BodyState bodyState0;
PxSolverContactDesc::BodyState bodyState1;
// PT: the following pointers have been moved here from derived structures to fill padding bytes
union
{
// PT: moved from PxTGSSolverConstraintPrepDesc
void* writeback;
// PT: moved from PxTGSSolverContactDesc
void* shapeInteraction;
};
};
// PT: ensure that we can safely read 4 bytes after the bodyFrames
PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxTGSSolverConstraintPrepDescBase, bodyFrame0)+sizeof(PxTGSSolverConstraintPrepDescBase::bodyFrame0) + 4 <= sizeof(PxTGSSolverConstraintPrepDescBase));
PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxTGSSolverConstraintPrepDescBase, bodyFrame1)+sizeof(PxTGSSolverConstraintPrepDescBase::bodyFrame1) + 4 <= sizeof(PxTGSSolverConstraintPrepDescBase));
struct PxTGSSolverConstraintPrepDesc : public PxTGSSolverConstraintPrepDescBase
{
Px1DConstraint* rows;
PxU32 numRows;
PxReal linBreakForce, angBreakForce;
PxReal minResponseThreshold;
bool disablePreprocessing;
bool improvedSlerp;
bool driveLimitsAreForces;
bool extendedLimits;
bool disableConstraint;
PxVec3p body0WorldOffset;
PxVec3p cA2w;
PxVec3p cB2w;
};
struct PxTGSSolverContactDesc : public PxTGSSolverConstraintPrepDescBase
{
PxU8* frictionPtr;
const PxContactPoint* contacts;
PxU32 numContacts;
PxU8 frictionCount;
bool hasMaxImpulse;
bool disableStrongFriction;
bool hasForceThresholds;
PxReal restDistance;
PxReal maxCCDSeparation;
PxReal* contactForces;
PxU32 startFrictionPatchIndex;
PxU32 numFrictionPatches;
PxU32 startContactPatchIndex;
PxU16 numContactPatches;
PxU16 axisConstraintCount;
PxReal maxImpulse;
PxReal torsionalPatchRadius;
PxReal minTorsionalPatchRadius;
PxReal offsetSlop;
};
#if !PX_DOXYGEN
}
#endif
#if PX_VC
#pragma warning(pop)
#endif
#endif