Revision control
Copy as Markdown
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0
#ifndef DY_ARTICULATION_JOINT_CORE_H
#define DY_ARTICULATION_JOINT_CORE_H
#include "DyArticulationCore.h"
#include "solver/PxSolverDefs.h"
#include "PxArticulationJointReducedCoordinate.h"
#include "CmSpatialVector.h"
namespace physx
{
namespace Dy
{
// PT: avoid some multiplies when immediately normalizing a rotated vector
PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 rotateAndNormalize(const PxQuat& q, const PxVec3& v)
{
const float vx = v.x;
const float vy = v.y;
const float vz = v.z;
const float x = q.x;
const float y = q.y;
const float z = q.z;
const float w = q.w;
const float w2 = w * w - 0.5f;
const float dot2 = (x * vx + y * vy + z * vz);
const PxVec3 rotated( (vx * w2 + (y * vz - z * vy) * w + x * dot2),
(vy * w2 + (z * vx - x * vz) * w + y * dot2),
(vz * w2 + (x * vy - y * vx) * w + z * dot2));
return rotated.getNormalized();
}
class ArticulationJointCoreData;
PX_ALIGN_PREFIX(16)
struct ArticulationJointCore
{
public:
// PX_SERIALIZATION
ArticulationJointCore(const PxEMPTY&) : drives{ PxArticulationDrive(PxEmpty),
PxArticulationDrive(PxEmpty),
PxArticulationDrive(PxEmpty),
PxArticulationDrive(PxEmpty),
PxArticulationDrive(PxEmpty),
PxArticulationDrive(PxEmpty) }, jCalcUpdateFrames(false)
{
PX_COMPILE_TIME_ASSERT(sizeof(PxArticulationMotions) == sizeof(PxU8));
}
//~PX_SERIALIZATION
ArticulationJointCore(const PxTransform& parentFrame, const PxTransform& childFrame)
{
//PxMarkSerializedMemory(this, sizeof(ArticulationJointCore));
init(parentFrame, childFrame);
}
// PT: these ones don't update the dirty flags
PX_FORCE_INLINE void setLimit(PxArticulationAxis::Enum axis, const PxArticulationLimit& limit) { limits[axis] = limit; }
PX_FORCE_INLINE void setDrive(PxArticulationAxis::Enum axis, const PxArticulationDrive& drive) { drives[axis] = drive; }
PX_FORCE_INLINE void setJointType(PxArticulationJointType::Enum type) { jointType = PxU8(type); }
PX_FORCE_INLINE void setMaxJointVelocity(const PxReal maxJointV)
{
for(PxU32 i = 0; i < PxArticulationAxis::eCOUNT; i++)
{
maxJointVelocity[i] = maxJointV;
}
}
PX_FORCE_INLINE void setMaxJointVelocity(PxArticulationAxis::Enum axis, const PxReal maxJointV)
{
maxJointVelocity[axis] = maxJointV;
}
PX_FORCE_INLINE void setFrictionCoefficient(const PxReal coefficient) { frictionCoefficient = coefficient; }
PX_FORCE_INLINE void setFrictionParams(PxArticulationAxis::Enum axis, const PxJointFrictionParams& jointFrictionParams)
{
frictionParams[axis] = jointFrictionParams;
}
void init(const PxTransform& parentFrame, const PxTransform& childFrame)
{
PX_ASSERT(parentFrame.isValid());
PX_ASSERT(childFrame.isValid());
parentPose = parentFrame;
childPose = childFrame;
jCalcUpdateFrames = true;
setFrictionCoefficient(0.05f);
setMaxJointVelocity(100.0f);
setJointType(PxArticulationJointType::eUNDEFINED);
for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
{
setLimit(PxArticulationAxis::Enum(i), PxArticulationLimit(0.0f, 0.0f));
setDrive(PxArticulationAxis::Enum(i), PxArticulationDrive(0.0f, 0.0f, 0.0f, PxArticulationDriveType::eNONE));
setFrictionParams(PxArticulationAxis::Enum(i), PxJointFrictionParams(0.0f, 0.0f, 0.0f));
setFrictionParams(PxArticulationAxis::Enum(i), PxJointFrictionParams(0.0f, 0.0f, 0.0f));
targetP[i] = 0.0f;
targetV[i] = 0.0f;
armature[i] = 0.0f;
jointPos[i] = 0.0f;
jointVel[i] = 0.0f;
dofIds[i] = 0xff;
invDofIds[i] = 0xff;
motion[i] = PxArticulationMotion::eLOCKED;
}
}
PX_CUDA_CALLABLE void setJointFrame(Cm::UnAlignedSpatialVector* motionMatrix,
const Cm::UnAlignedSpatialVector* jointAxis,
PxQuat& relativeQuat,
const PxU32 dofs)
{
if (jCalcUpdateFrames)
{
relativeQuat = (childPose.q * (parentPose.q.getConjugate())).getNormalized();
computeMotionMatrix(motionMatrix, jointAxis, dofs);
jCalcUpdateFrames = false;
}
}
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeMotionMatrix(Cm::UnAlignedSpatialVector* motionMatrix,
const Cm::UnAlignedSpatialVector* jointAxis,
const PxU32 dofs)
{
// PT: the joint axes are computed like this:
// Cm::UnAlignedSpatialVector axis = Cm::UnAlignedSpatialVector::Zero();
// axis[i] = 1.f;
// So we could store and pass a simple index in [0;6] instead of the full spatial vector, and then
// use specialized rotateAndNormalize() functions that take advantage of zeros.
const PxVec3 childOffset = -childPose.p;
switch (jointType)
{
case PxArticulationJointType::ePRISMATIC:
{
const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[0];
const PxVec3 u = rotateAndNormalize(childPose.q, jJointAxis.bottom);
motionMatrix[0] = Cm::UnAlignedSpatialVector(PxVec3(0.0f), u);
PX_ASSERT(dofs == 1);
break;
}
case PxArticulationJointType::eREVOLUTE:
case PxArticulationJointType::eREVOLUTE_UNWRAPPED:
{
const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[0];
const PxVec3 u = rotateAndNormalize(childPose.q, jJointAxis.top);
const PxVec3 uXd = u.cross(childOffset);
motionMatrix[0] = Cm::UnAlignedSpatialVector(u, uXd);
break;
}
case PxArticulationJointType::eSPHERICAL:
{
for (PxU32 ind = 0; ind < dofs; ++ind)
{
const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[ind];
const PxVec3 u = rotateAndNormalize(childPose.q, jJointAxis.top);
const PxVec3 uXd = u.cross(childOffset);
motionMatrix[ind] = Cm::UnAlignedSpatialVector(u, uXd);
}
break;
}
case PxArticulationJointType::eFIX:
{
PX_ASSERT(dofs == 0);
break;
}
default:
break;
}
}
PX_CUDA_CALLABLE PX_FORCE_INLINE void operator=(ArticulationJointCore& other)
{
parentPose = other.parentPose;
childPose = other.childPose;
//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];
armature[i] = other.armature[i];
frictionParams[i] = other.frictionParams[i];
maxJointVelocity[i] = other.maxJointVelocity[i];
jointPos[i] = other.jointPos[i];
jointVel[i] = other.jointVel[i];
dofIds[i] = other.dofIds[i];
invDofIds[i] = other.invDofIds[i];
motion[i] = other.motion[i];
}
frictionCoefficient = other.frictionCoefficient;
jCalcUpdateFrames = other.jCalcUpdateFrames;
jointType = other.jointType;
}
PX_FORCE_INLINE void setParentPose(const PxTransform& t) { parentPose = t; jCalcUpdateFrames = true; }
PX_FORCE_INLINE void setChildPose(const PxTransform& t) { childPose = t; jCalcUpdateFrames = true; }
PX_FORCE_INLINE void setMotion(PxArticulationAxis::Enum axis, PxArticulationMotion::Enum m) { motion[axis] = PxU8(m); }
PX_FORCE_INLINE void setTargetP(PxArticulationAxis::Enum axis, PxReal value) { targetP[axis] = value; }
PX_FORCE_INLINE void setTargetV(PxArticulationAxis::Enum axis, PxReal value) { targetV[axis] = value; }
PX_FORCE_INLINE void setArmature(PxArticulationAxis::Enum axis, PxReal value) { armature[axis] = value; }
// 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
PxReal armature[PxArticulationAxis::eCOUNT]; //24 272
PxReal jointPos[PxArticulationAxis::eCOUNT]; //24 296
PxReal jointVel[PxArticulationAxis::eCOUNT]; //24 320
PxReal frictionCoefficient; //4 324
PxJointFrictionParams frictionParams[PxArticulationAxis::eCOUNT]; //72 396
PxReal maxJointVelocity[PxArticulationAxis::eCOUNT]; // 24 420
PxU8 dofIds[PxArticulationAxis::eCOUNT]; //6 426
PxU8 motion[PxArticulationAxis::eCOUNT]; //6 432
PxU8 invDofIds[PxArticulationAxis::eCOUNT]; //6 438
bool jCalcUpdateFrames; //1 439
PxU8 jointType; //1 440
PxReal padding[2]; //8 448
}PX_ALIGN_SUFFIX(16);
}
}
#endif