Revision control
++ // 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_CPUGPU_H
++ #define DY_ARTICULATION_CPUGPU_H
++
++ #include "foundation/PxSimpleTypes.h"
++ #include "foundation/PxBasicTemplates.h"
++ #include "foundation/PxVec3.h"
++ #include "foundation/PxQuat.h"
++ #include "PxArticulationJointReducedCoordinate.h"
++ #include "CmSpatialVector.h"
++ #include "DyFeatherstoneArticulationUtils.h"
++
++ #define DY_MIN_RESPONSE 1e-12f
++ #define DY_ARTICULATION_MIN_RESPONSE 1e-12f
++ #define DY_ARTICULATION_CFM 2e-4f
++
++ namespace physx
++ {
++ namespace Dy
++ {
++
++ /**
++ * Compute the parent-to-child world translation Rw from the stored world
++ * orientations and link-local anchor offsets. Precision-clean at any world
++ * coordinate: uses only unit world quaternions and small link-local offsets;
++ * no world-scale position subtraction.
++ *
++ * The joint anchor coincides on both bodies, so
++ * Rw = child.world.p - parent.world.p
++ * = parentQW.rotate(parentOffset) + childQW.rotate(slide - childOffset)
++ * where parentOffset = joint.parentPose.p (unchanged)
++ * childOffset = joint.childPose.p (NOT negated -- caller passes raw)
++ * slide = prismatic translation (zero for all other joint types)
++ */
++ template<typename T>
++ PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3T<T> computeLinkRw(
++ const PxQuatT<T>& parentQW,
++ const PxQuatT<T>& childQW,
++ const PxVec3T<T>& parentOffset,
++ const PxVec3T<T>& childOffset,
++ const PxVec3T<T>& slide)
++ {
++ return parentQW.rotate(parentOffset) + childQW.rotate(slide - childOffset);
++ }
++
++ struct ArticulationImplicitDriveDesc
++ {
++ PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc(PxZERO)
++ : driveTargetVelPlusInitialBias(0.0f),
++ driveBiasCoefficient(0.0f),
++ driveVelMultiplier(0.0f),
++ driveImpulseMultiplier(0.0f),
++ driveTargetPosBias(0.0f)
++ {
++ }
++ PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc
++ (const PxReal targetVelPlusInitialBias, const PxReal biasCoefficient, const PxReal velMultiplier,
++ const PxReal impulseMultiplier, const PxReal targetPosBias)
++ : driveTargetVelPlusInitialBias(targetVelPlusInitialBias),
++ driveBiasCoefficient(biasCoefficient),
++ driveVelMultiplier(velMultiplier),
++ driveImpulseMultiplier(impulseMultiplier),
++ driveTargetPosBias(targetPosBias)
++ {
++ }
++ PxReal driveTargetVelPlusInitialBias;
++ PxReal driveBiasCoefficient;
++ PxReal driveVelMultiplier;
++ PxReal driveImpulseMultiplier;
++ PxReal driveTargetPosBias;
++ };
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc computeImplicitDriveParamsForceDrive
++ (const PxReal stiffness, const PxReal damping, const PxReal dt, const PxReal simDt,
++ const PxReal unitResponse, const PxReal geomError, const PxReal targetVelocity, const bool isTGSSolver)
++ {
++ ArticulationImplicitDriveDesc driveDesc(PxZero);
++ const PxReal a = dt * (dt * stiffness + damping);
++ const PxReal b = dt * (damping * targetVelocity);
++ const PxReal x = unitResponse > 0.f ? 1.0f / (1.0f + a * unitResponse) : 0.f;
++ const PxReal initialBias = geomError - (simDt - dt) * targetVelocity; //equal to geomError for PGS as simDt = dt
++ const PxReal driveBiasCoefficient = stiffness * x * dt;
++ driveDesc.driveTargetVelPlusInitialBias = (x * b) + driveBiasCoefficient * initialBias;
++ driveDesc.driveVelMultiplier = -x * a;
++ driveDesc.driveBiasCoefficient = driveBiasCoefficient;
++ driveDesc.driveImpulseMultiplier = isTGSSolver ? 1.f : 1.0f - x;
++ driveDesc.driveTargetPosBias = isTGSSolver ? driveBiasCoefficient * targetVelocity : 0.f;
++ return driveDesc;
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc computeImplicitDriveParamsAccelerationDrive
++ (const PxReal stiffness, const PxReal damping, const PxReal dt, const PxReal simDt,
++ const PxReal recipUnitResponse, const PxReal geomError, const PxReal targetVelocity, const bool isTGSSolver)
++ {
++ ArticulationImplicitDriveDesc driveDesc(PxZero);
++ const PxReal a = dt * (dt * stiffness + damping);
++ const PxReal b = dt * (damping * targetVelocity);
++ const PxReal x = 1.0f / (1.0f + a);
++ const PxReal initialBias = geomError - (simDt - dt) * targetVelocity; //equal to geomError for PGS as simDt = dt
++ const PxReal driveBiasCoefficient = stiffness * x * recipUnitResponse * dt;
++ driveDesc.driveTargetVelPlusInitialBias = (x * b * recipUnitResponse) + driveBiasCoefficient * initialBias;
++ driveDesc.driveVelMultiplier = -x * a * recipUnitResponse;
++ driveDesc.driveBiasCoefficient = driveBiasCoefficient;
++ driveDesc.driveImpulseMultiplier = isTGSSolver ? 1.f : 1.0f - x;
++ driveDesc.driveTargetPosBias = isTGSSolver ? driveBiasCoefficient * targetVelocity : 0.f;
++ return driveDesc;
++ }
++
++
++ /**
++ \brief Compute the parameters for an implicitly integrated spring.
++ \param[in] driveType is the type of drive.
++ \param[in] stiffness is the drive stiffness (force per unit position bias)
++ \param[in] damping is the drive damping (force per unit velocity bias)
++ \param[in] dt is the timestep that will be used to forward integrate the spring position bias.
++ \param[in] simDt is the simulation timestep.
++ \param[in] unitResponse is the multiplier that converts impulse to velocity change.
++ \param[in] recipUnitResponse is the reciprocal of unitResponse
++ \param[in] geomError is the position bias with value (targetPos - currentPos)
++ \param[in] targetVelocity is the target velocity of the drive.
++ \param[in] isTGSSolver should be set true when computing implicit spring params for TGS and false for PGS.
++ \return The implicit spring parameters.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc computeImplicitDriveParams
++ (const PxArticulationDriveType::Enum driveType, const PxReal stiffness, const PxReal damping, const PxReal dt, const PxReal simDt,
++ const PxReal unitResponse, const PxReal recipUnitResponse, const PxReal geomError, const PxReal targetVelocity, const bool isTGSSolver)
++ {
++ ArticulationImplicitDriveDesc driveDesc(PxZero);
++ switch (driveType)
++ {
++ case PxArticulationDriveType::eFORCE:
++ {
++ driveDesc = computeImplicitDriveParamsForceDrive(stiffness, damping, dt, simDt, unitResponse, geomError, targetVelocity, isTGSSolver);
++ }
++ break;
++ case PxArticulationDriveType::eACCELERATION:
++ {
++ driveDesc = computeImplicitDriveParamsAccelerationDrive(stiffness, damping, dt, simDt, recipUnitResponse, geomError, targetVelocity, isTGSSolver);
++ }
++ break;
++ case PxArticulationDriveType::eNONE:
++ {
++ PX_ASSERT(false);
++ }
++ break;
++ }
++ return driveDesc;
++ }
++
++ /**
++ \brief Compute the friction impulse.
++ \param[in] frictionImpulse is the accumulated frictiom impulse on the current simulation step.
++ \param[in] staticFrictionImpulse is threshold to prevent motion of the joint.
++ \param[in] dynamicFrictionImpulse is constant friction applied to moving joints .
++ \param[in] viscousFrictionCoefficient is the coefficient of velocity dependent friction term.
++ \param[in] jointVel is the current velocity of the joint.
++ \return The friction impulse.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeFrictionImpulse
++ (PxReal frictionImpulse, const PxReal staticFrictionImpulse, const PxReal dynamicFrictionImpulse, const PxReal viscousFrictionCoefficient, const PxReal jointVel)
++ {
++ if (PxAbs(frictionImpulse) > staticFrictionImpulse)
++ frictionImpulse = PxClamp(frictionImpulse, -dynamicFrictionImpulse - viscousFrictionCoefficient * PxAbs(jointVel), dynamicFrictionImpulse + viscousFrictionCoefficient * PxAbs(jointVel));
++
++ return frictionImpulse;
++ }
++
++ /**
++ \brief Compute the drive impulse for an implicitly integrated spring.
++ \param[in] accumulatedDriveImpulse is the drive impulse that has accumulated since the solver started on the current simulation step.
++ \param[in] jointVel is the current velocity of the joint.
++ \param[in] jointDeltaPos is the change in joint position that has accumulated since the solver started on the current simulation step.
++ \param[in] elapsedTime is the time elapsed on the current simulation step (used only for the TGS solver).
++ \param[in] driveDesc is the implicit spring params.
++ \return The impulse for the implicitly integrated spring.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeDriveImpulse
++ (const PxReal accumulatedDriveImpulse, const PxReal jointVel, const PxReal jointDeltaPos, const PxReal elapsedTime, const ArticulationImplicitDriveDesc& driveDesc)
++ {
++ const PxReal unclampedForce =
++ accumulatedDriveImpulse * driveDesc.driveImpulseMultiplier
++ + jointVel * driveDesc.driveVelMultiplier
++ + driveDesc.driveTargetVelPlusInitialBias
++ - jointDeltaPos * driveDesc.driveBiasCoefficient
++ + elapsedTime * driveDesc.driveTargetPosBias;
++ return unclampedForce;
++ }
++
++
++
++ PX_CUDA_CALLABLE PX_INLINE PxPair<PxReal, PxReal> computeBoundsForNegativeImpulse(
++ PxReal velDeviation,
++ PxReal max,
++ PxReal denom1,
++ PxReal denom2,
++ PxReal impulse)
++ {
++ PxReal bound1;
++ PxReal bound2;
++
++ // invalid case
++ if(PxAbs(denom1) < PX_EPS_F32)
++ {
++ return PxPair<PxReal, PxReal>(-PX_MAX_F32, PX_MAX_F32);
++ }
++ if (PxAbs(denom2) < PX_EPS_F32)
++ {
++ bound1 = (velDeviation - max) / denom1;
++ if (PxAbs(velDeviation + max) < PX_EPS_F32)
++ bound2 = PX_MAX_F32;
++ else
++ bound2 = (velDeviation + max) > 0.0f ? PX_MAX_F32 : -PX_MAX_F32;
++
++ return PxPair<PxReal, PxReal>(bound1, PxMin(0.0f, bound2));
++ }
++
++ bound1 = (velDeviation - max) / denom1;
++ bound2 = (velDeviation + max) / denom2;
++
++
++ if (denom2 >= 0.0f) {
++ return PxPair<PxReal, PxReal>(PxMax(bound1, impulse), PxMin(0.0f, bound2));
++ }
++ else {
++ return PxPair<PxReal, PxReal>( PxMax(PxMax(bound1, bound2), impulse), 0.0f);
++ }
++ }
++
++
++ PX_CUDA_CALLABLE PX_INLINE PxPair<PxReal, PxReal> computeBoundsForPositiveImpulse(
++ PxReal velDeviation,
++ PxReal max,
++ PxReal denom1,
++ PxReal denom2,
++ PxReal impulse)
++ {
++ PxReal bound1;
++ PxReal bound2;
++
++ // invalid case
++ if(PxAbs(denom2) < PX_EPS_F32)
++ {
++ return PxPair<PxReal, PxReal>(-PX_MAX_F32, PX_MAX_F32);
++ }
++
++ if (PxAbs(denom1) < PX_EPS_F32)
++ {
++ bound2 = (velDeviation + max) / denom2;
++ if (PxAbs(velDeviation - max) < PX_EPS_F32)
++ bound1 = -PX_MAX_F32;
++ else
++ bound1 = (velDeviation - max) > 0.0f ? PX_MAX_F32 : -PX_MAX_F32;
++ return PxPair<PxReal, PxReal>(PxMax(0.0f, bound1), bound2);
++ }
++ bound1 = (velDeviation - max) / denom1;
++ bound2 = (velDeviation + max) / denom2;
++
++ if (denom1 >= 0.0f)
++ return PxPair<PxReal, PxReal>(PxMax(0.0f, bound1), PxMin(bound2, impulse));
++ else
++ return PxPair<PxReal, PxReal>(0.0f, PxMin(PxMin(bound1, bound2), impulse));
++ }
++
++ /**
++ \brief Compute the drive impulse implicitly.
++ \param[in] jointVel0 is the joint velocity before drive impulse is applied.
++ \param[in] driveImpulse0 accumulated drive effort until the current iteration.
++ \param[in] driveImpulse drive impulse to be clamped.
++ \param[in] response response.
++ \param[in] maxJointVel max actuator velocity.
++ \param[in] maxImpulse max impulse.
++ \param[in] speedImpulseGradient speed impulse gradient of the performance envelope.
++ \return The clamped drive impulse.
++ */
++ PX_CUDA_CALLABLE PX_INLINE PxReal clampDriveImpulse(
++ PxReal jointVel0,
++ PxReal driveImpulse0,
++ PxReal driveImpulse,
++ PxReal response,
++ PxReal maxJointVel,
++ PxReal maxImpulse,
++ PxReal speedImpulseGradient,
++ PxReal velocityDependentResistance)
++ {
++
++ PxReal velDeviation = driveImpulse0 * response - jointVel0;
++ PxPair<PxReal, PxReal> bounds1;
++ PxPair<PxReal, PxReal> bounds2;
++
++ if (driveImpulse > 0.0f) {
++ PxReal denom1 = response - speedImpulseGradient;
++ PxReal denom2 = response + speedImpulseGradient;
++ bounds1 = computeBoundsForPositiveImpulse(velDeviation, maxJointVel, denom1, denom2, driveImpulse);
++ if (velocityDependentResistance == 0.0f) {
++ bounds2 = PxPair<PxReal, PxReal>(-maxImpulse, maxImpulse);
++ } else {
++ denom1 = response - 1.0f / velocityDependentResistance;
++ denom2 = response + 1.0f / velocityDependentResistance;
++ bounds2 = computeBoundsForPositiveImpulse(velDeviation, maxImpulse / velocityDependentResistance, denom1, denom2, driveImpulse);
++ }
++
++ }
++
++ else
++ {
++ PxReal denom1 = response + speedImpulseGradient;
++ PxReal denom2 = response - speedImpulseGradient;
++ bounds1 = computeBoundsForNegativeImpulse(velDeviation, maxJointVel, denom1, denom2, driveImpulse);
++ if (velocityDependentResistance == 0.0f) {
++ bounds2 = PxPair<PxReal, PxReal>(-maxImpulse, maxImpulse);
++ } else {
++ denom1 = response + 1.0f / velocityDependentResistance;
++ denom2 = response - 1.0f / velocityDependentResistance;
++ bounds2 = computeBoundsForNegativeImpulse(velDeviation, maxImpulse / velocityDependentResistance, denom1, denom2, driveImpulse);
++ }
++ }
++
++ // Combine bounds
++ PxReal lowerBound = PxMax(bounds1.first, bounds2.first);
++ PxReal upperBound = PxMin(bounds1.second, bounds2.second);
++
++ // Check for invalid bounds
++ if (lowerBound > upperBound) {
++ return 0.0f;
++ }
++ return PxClamp(driveImpulse, lowerBound, upperBound);
++ }
++
++ /**
++ \brief Apply limit constraints to an articulation joint dof.
++ 1) Compute the delta impulse required to maintain limit constraints.
++ 2) Individually accumulate the impulses that have been applied to maintain both the lower and upper limit.
++ 3) Compute the updated joint speed after applying the delta impulse.
++ \param[in] dt is the timestep of the simulation
++ \param[in] recipDt has value 1/dt
++ \param[in] isVelIter is true if we are performing a velocity iteration, false if performing a position iteration.
++ \param[in] response is the deltaSpeed response of the joint dof to a unit impulse.
++ \param[in] recipResponse has value 1/response.
++ \param[in] erp is the Baumgarte multiplier used to resolve a fraction of the limit error.
++ \param[in] errorLow is the lower bound of the limit.
++ \param[in] errorHigh is the upper bound of the limit.
++ \param[in] jointPDelta is the change to the joint position that has accumulated over solver iterations.
++ \param[in,out] lowImpulse_ is the accumulated impulse that has been applied to maintain the limit's lower bound.
++ \param[in,out] highImpulse_ is the accumulated impulse that has applied to maintain the limit's upper bound.
++ \param[in,out] jointV_ is the joint speed before and after applying the limit impulses.
++ \return deltaImpulse required to enforce the upper and lower limits.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeLimitImpulse
++ (const PxReal dt, const PxReal recipDt, const bool isVelIter,
++ const PxReal response, const PxReal recipResponse, const PxReal erp,
++ const PxReal errorLow, const PxReal errorHigh,
++ const PxReal jointPDelta,
++ PxReal& lowImpulse_, PxReal& highImpulse_, PxReal& jointV_)
++ {
++ // PT: avoid aliasing
++ PxReal jointV = jointV_;
++ PxReal lowImpulse = lowImpulse_;
++ PxReal highImpulse = highImpulse_;
++
++ const PxReal futureDeltaJointP = jointPDelta + jointV * dt;
++
++ // for all errors: Negative means violated
++ const PxReal currErrLow = errorLow + jointPDelta;
++ const PxReal nextErrLow = errorLow + futureDeltaJointP;
++ const PxReal currErrHigh = errorHigh - jointPDelta;
++ const PxReal nextErrHigh = errorHigh - futureDeltaJointP;
++
++ bool limited = false;
++ bool newLow = false;
++ bool newHigh = false;
++
++ const PxReal tolerance = 0.f;
++
++ PxReal deltaF = 0.f;
++ if (currErrLow < tolerance || nextErrLow < tolerance)
++ {
++ PxReal newJointV = jointV;
++ if (currErrLow < tolerance)
++ {
++ //Only apply an impulse to resolve the bias on pos iters.
++ //Any subsequent vel iter will attempt to bring the dof vel to zero in the
++ //code block that follows (if !limited) if this dof had already received an
++ //impulse to resolve the limit bias.
++ if (!isVelIter)
++ {
++ limited = true;
++ newJointV = -currErrLow * recipDt * erp;
++ }
++ }
++ else
++ {
++ // Currently we're not in violation of the limit but would be after this time step given the current velocity.
++ // To prevent that future violation, we want the current velocity to only take us right to the limit, not across it
++ limited = true;
++ newJointV = -currErrLow * recipDt;
++ }
++
++ // In position iterations, the newJointV is now such that we end up exactly on the limit after this time step (ignoring erp)
++ // However, we ignored the current velocity, which may already take us further away from the limit than the newJointV.
++ // Therefore, we additionally have to check now that the impulse we're applying is only repulsive overall.
++
++ if(limited)
++ {
++ const PxReal deltaV = newJointV - jointV;
++ deltaF = PxMax(lowImpulse + deltaV * recipResponse, 0.f) - lowImpulse; // accumulated limit impulse must be repulsive
++ lowImpulse += deltaF;
++ newLow = true;
++ }
++ }
++ else if (currErrHigh < tolerance || nextErrHigh < tolerance)
++ {
++ PxReal newJointV = jointV;
++ if (currErrHigh < tolerance)
++ {
++ if (!isVelIter)
++ {
++ limited = true;
++ newJointV = currErrHigh * recipDt * erp;
++ }
++ }
++ else
++ {
++ limited = true;
++ newJointV = currErrHigh * recipDt;
++ }
++
++ if(limited)
++ {
++ const PxReal deltaV = newJointV - jointV;
++ deltaF = PxMin(highImpulse + deltaV * recipResponse, 0.f) - highImpulse;
++ highImpulse += deltaF;
++ newHigh = true;
++ }
++ }
++
++ if (!limited)
++ {
++ // If no limit is violated right now, it could still be that a limit was active in an earlier iteration and
++ // overshot. Therefore, we give that limit from which the joint position is currently moving away a chance to
++ // pull back and correct the overshoot.
++ // The pull-back impulse is the smaller of
++ // a) The impulse needed to bring the joint velocity to zero.
++ // b) The opposite impulse of the already applied joint limit impulse, thereby cancelling out the accumulated effect of the limit.
++ //Note: if the limit was not breached, lowImpulse and highImpulse will both be 0 and deltaF will also be 0.
++
++ const PxReal impulseForZeroVel = -jointV * recipResponse;
++ if (jointV > 0.f) // moving away from the lower limit
++ {
++ deltaF = PxMax(impulseForZeroVel, -lowImpulse);
++ lowImpulse += deltaF;
++ newLow = true;
++ }
++ else // moving away from the higher limit
++ {
++ deltaF = PxMin(impulseForZeroVel, -highImpulse);
++ highImpulse += deltaF;
++ newHigh = true;
++ }
++ }
++
++ jointV += deltaF * response;
++
++ if(newLow)
++ lowImpulse_ = lowImpulse;
++ if(newHigh)
++ highImpulse_ = highImpulse;
++ jointV_ = jointV;
++
++ return deltaF;
++ }
++
++
++ /**
++ \brief Translate a spatial vector from the frame of one link (the source link) to the frame of another link (the target link).
++ \param[in] offset is the vector from the source link to the target link (== posTargetLink - posSourceLlink)
++ \param[in] s is the spatial vector in the frame of the source link with s.top representing the angular part of the
++ spatial vector and s.bottom representing the linear part of the spatial vector.
++ \return The spatial vector translated into the frame of the target link with top representing the angular part of the spatial vector
++ and bottom representing the linear part of the spatial vector.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF translateSpatialVector(const PxVec3& offset, const Cm::SpatialVectorF& s)
++ {
++ return Cm::SpatialVectorF(s.top, s.bottom + offset.cross(s.top));
++ }
++
++ /**
++ \brief Propagate to the parent link
++ a) a spatial impulse applied to a child link
++ b) a joint impulse applied to the child link's inbound joint.
++ The Mirtich equivalent is the equation for Y in Figure 5.7, page 141 but with a modification
++ to account for a joint impulse applied to the child link's inbound joint.
++ If the joint impulse is Q and the child link impulse is YChildW then the parent link impulse has
++ the form:
++ YParentW = translateChildToParent{ YChildW + (I * s) *(Q - s^T * YChildW)/(s * I * s^T) }
++ Optionally accumulate [Q - S^T * YChildW] because this can be useful to reuse when propagating
++ delta spatial velocity from parent link to child link.
++ \param[in] parentToChild is the vector from parent link to child link such that childLinkPos == parentLinkPos + parentToChild
++ \param[in] YChildW is the link impulse to apply to the child link expressed in the world frame.
++ \param[in] jointDofImpulse is an optional of array joint impulses ({Q}) to apply to each dof of the inbound joint of the child link.
++ \param[in] jointDofISInvStISW is (I * s) / (s^T * I * s) with one entry for each dof of the child link's inbound joint.
++ \param[in] jointDofMotionMatrixW is the motion matrix s with one entry for each dof of the child link's inbound joint.
++ \param[in] dofCount is the number of dofs of the child link's incoming joint.
++ \param[in,out] jointDofQMinusStY accumulates [Q - s^T * YChildW] for each dof of the child link's inbound joint.
++ \note jointDofQMinusStY may be NULL if there is no need to accumulate [Q - s^T * YChildW] for each dof of the child link's inbound joint.
++ \note jointDofImpulse may be NULL if the intention is that zero joint impulse should be propagated.
++ \note jointDofImpulse, jointDofISInvStISW, jointDofMotionMatrixW and jointDofQMinusStY have dofCount entries ie one entry for each dof of the joint.
++ \return The propagated spatial impulse in the world frame.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF propagateImpulseW
++ (const PxVec3& parentToChild,
++ const Cm::SpatialVectorF& YChildW,
++ const PxReal* jointDofImpulse, const Cm::SpatialVectorF* jointDofISInvStISW, const Cm::UnAlignedSpatialVector* jointDofMotionMatrixW, const PxU8 dofCount,
++ PxReal* jointDofQMinusStY)
++ {
++ //See Mirtich Figure 5.7 page 141
++ //Mirtich equivalent after accounting for joint impulse:
++ // childToParentTranform{ Y + (I * s) * (Q - s^T* Y)]/ (s^T * I * s) }
++
++ Cm::SpatialVectorF YParentW(PxVec3(0, 0, 0), PxVec3(0, 0, 0));
++ for (PxU8 ind = 0; ind < dofCount; ++ind)
++ {
++ //(Q - s^T* Y)
++ const Cm::UnAlignedSpatialVector& sa = jointDofMotionMatrixW[ind];
++ const PxReal Q = jointDofImpulse ? jointDofImpulse[ind] : 0.0f;
++ const PxReal QMinusStY = Q - (sa.innerProduct(YChildW));
++ PX_ASSERT(PxIsFinite(QMinusStY));
++
++ //(I * s) * (Q - s^T* Y)]/ (s^T * I * s)
++ YParentW += jointDofISInvStISW[ind] * QMinusStY;
++
++ //Accumulate (Q - s^T * Y)
++ PX_ASSERT(!jointDofQMinusStY || PxIsFinite(jointDofQMinusStY[ind]));
++ if(jointDofQMinusStY)
++ jointDofQMinusStY[ind] += QMinusStY;
++ }
++ //Y + [(I * s) * (Q - s^T* Y)]/ (s^T * I * s)]
++ YParentW += YChildW;
++
++ //parent space's spatial zero acceleration impulse
++ //parentToChild satisfies parent = child + parentToChild.
++ return translateSpatialVector(parentToChild, YParentW);
++ }
++
++ /**
++ /brief Propagate an acceleration (or velocity) from a parent link to a child link.
++ This function exploits existing knowledge of Q_i - s_i^T * Z_i^A. If this is not known it is recommended to use
++ propagateVelocityW().
++ \param[in] parentToChild is the vector from parent link to child link such that childLinkPos == parentLinkPos + parentToChild
++ \param[in] parentLinkAccelerationW is the parent link acceleration (or velocity) expressed in the world frame.
++ \param[in] invStISW is the Mirtich equivalent of 1/[S_i^T * I_i^A * S_i]
++ \param[in] motionMatrixW is the Mirth equivalent of S_i of the child link's inbound joint.
++ \param[in] IsW is the Mirtich equvialent of I_i^A * S_i (== S_i^T * I_i^A)
++ \param[in] QMinusSTZ is the equivalent of Q_i - S_i^T * ZA_i in Mirtich notiation with
++ Q_i the joint force (or impulse) of the child link's inbound joint and ZA_i the child link
++ zero acceleration force (or impulse)
++ \param[in] dofCount is the number of dofs on the child links' inbound joint.
++ \param[out] jointAcceleration (or joint velocity) is incremented with the change arising
++ from the propagated acceleration (or velocity).
++ \note jointAcceleration (or velocity) may be NULL.
++ \return The spatial acceleration (or velocity) of the child link.
++ \note See Mirtich p121 and equations for propagating forces/applying accelerations and
++ p141 for propagating velocities/applying impulses.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF propagateAccelerationW(
++ const PxVec3& parentToChild, const Cm::SpatialVectorF& parentLinkAccelerationW,
++ const InvStIs& invStISW, const Cm::UnAlignedSpatialVector* motionMatrixW, const Cm::SpatialVectorF* IsW, const PxReal* QMinusSTZ, const PxU32 dofCount,
++ PxReal* jointAcceleration)
++ {
++ //parentToChild satisfies parent = child + parentToChild.
++ Cm::SpatialVectorF motionAccelerationW = translateSpatialVector(-parentToChild, parentLinkAccelerationW); //parent velocity change
++
++ //[Q_i - (s^T * Z_i^A + I_i^A * c_i)] - s^T * I_i^A * translated(vParent)
++ //Alternatively we compute I_i^A * s == s^T * I_i^A
++ //[Q_i - (s^T * vParent + I_i^A * c_i)] - I_i^A *s * translated(vParent)
++ PxReal tJAccel[3];
++ for (PxU32 ind = 0; ind < dofCount; ++ind)
++ {
++ const PxReal temp = IsW[ind].innerProduct(motionAccelerationW);
++ tJAccel[ind] = (QMinusSTZ[ind] - temp);
++ }
++
++ //calculate jointAcceleration
++ //qdot = [1 / s^T * I_i^A * s] *{ [Q_i - (s^T * vParent + I_i^A * c_i)] - I_i^A *s * translated(vParent) }
++ //linkVel = translated(vParent) + qdot * s_i
++ for (PxU32 ind = 0; ind < dofCount; ++ind)
++ {
++ PxReal jVel = 0.f;
++ for (PxU32 ind2 = 0; ind2 < dofCount; ++ind2)
++ {
++ jVel += invStISW.invStIs[ind2][ind] * tJAccel[ind2];
++ }
++
++ motionAccelerationW.top += motionMatrixW[ind].top * jVel;
++ motionAccelerationW.bottom += motionMatrixW[ind].bottom * jVel;
++
++ if(jointAcceleration)
++ jointAcceleration[ind] += jVel;
++ }
++
++ return motionAccelerationW;
++ }
++
++ /**
++ \brief Compute the equivalent of (J * M^-1 * J^T) for a mimic joint.
++ \param[in] rAA is the deltaQDot arising at joint A/dof A as a consequence of a unit joint impulse applied to joint A/dofA.
++ \param[in] rAB is the deltaQDot arising at joint A/dof A as a consequence of a unit joint impulse applied to joint B/dofB.
++ \param[in] rBB is the deltaQDot arising at joint B/dof B as a consequence of a unit joint impulse applied to joint B/dofB.
++ \param[in] rBA is the deltaQDot arising at joint B/dof B as a consequence of a unit joint impulse applied to joint A/dofA.
++ \param[in] gearRatio is the gear ratio of the mimic joint [qA + gearRatio * qB + offset = 0]
++ \note deltaQDotA = rAA * jointImpulseA + rAB * jointImpulseB
++ deltaQDotB = rBA * jointImpulseA + rBB * jointImpulseB
++ rAA, rAB, rBA, rBB play the role of an inverse mass matrix as follows:
++ M^-1 = [rAA rAB]
++ [rBA rBB]
++ \return (J * M^-1 * J^T) for a mimic joint.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeRecipMimicJointEffectiveInertia
++ (const PxReal rAA, const PxReal rAB, const PxReal rBB, const PxReal rBA, const PxReal gearRatio)
++ {
++ return (rAA + gearRatio*(rAB + rBA) + gearRatio*gearRatio*rBB);
++ }
++
++ /*
++ \brief Compute the impulses to apply to jointA/dofA and jointB dofB that
++ satisfy the requirements of a mimimic joint.
++ \param[in] biasCoefficient is the Baumarte constant.
++ \param[in] dt is the timestep of the simulation.
++ \param[in] invDt is the reciprocal of dt used to forward integrate the joint positions.
++ \param[in] qA is the position of jointA/dofA
++ \param[in] qB is the position of jointB/dofB
++ \param[in] qADot is speed of jointA/dofA
++ \param[in] qBDot is speed of jointB/dofB
++ \param[in] gearRatio is a constant of the mimic joint constraint: qA + gearRatio*qB + offset = 0
++ \param[in] offset is a constant of the mimic joint constraint: qA + gearRatio*qB + offset = 0
++ \param[in] naturalFrequency is the oscillation frequency of the mimic joint's compliance (s^-1)
++ \param[in] dampingRatio is the damping ratio of the mimic joint's compliance.
++ \param[in] r is a constant derived from the mass matrix of the mimic joint.
++ \param[in] isVelocityIteration is true if we are computing the impulse during a velocity iteration
++ and false if we are computing the impulse during a position iteration.
++ \param[out] jointImpA is the joint impulse to apply to jointA/dofA to enforce the mimic constraint.
++ \param[out] jointImpB is the joint impulse to apply to jointB/dofB to enforce the mimic constraint.
++ \note r has value [J * M^-1 * J^T] with J the jacobian [1, beta] and
++ M^-1 = 2x2 mass matrix describing deltaQDot effect of unit impulse applied to jointA/dofA and jointB/dofB
++ \note The impulses are computed with a zero constraint bias during velocity iterations.
++ \note r is pre-computed with computeRecipMimicJointEffectiveInertia()
++ \note If naturalFrequency < 0 the mimic joint is treated as a hard constraint with no compliance.
++ \note If dampingRatio < 0 the mimic joint is treated as a hard constraint with no compliance.
++ \note biasCoefficient is ignored if the mimic joint is compliant (when naturalFrequency > 0 and dampingRatio > 0).
++ When this is the case, biasCoefficient is replaced with a value that reflects the compliance of the mimic joint.
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void computeMimicJointImpulses
++ (const PxReal biasCoefficient, const PxReal dt, const PxReal invDt,
++ const PxReal qA, const PxReal qB, const PxReal qADot, const PxReal qBDot,
++ const PxReal gearRatio, const PxReal offset,
++ const PxReal naturalFrequency, const PxReal dampingRatio,
++ const PxReal r,
++ const bool isVelocityIteration,
++ PxReal& jointImpA, PxReal& jointImpB)
++ {
++ //For velocity iterations we have erp = 0.
++ //But erp = dt*kp/(dt*kp + kd). If it is 0
++ //then kd >> dt*kp. If this is true then
++ //cfm = 1/[dt*(dt*kp + kd)] = 1/(dt*kd) = 0.
++
++ PxReal erp = 0.0f;
++ PxReal cfm = 0.0f;
++ if(naturalFrequency <= 0 || dampingRatio <= 0 || isVelocityIteration)
++ {
++ erp = isVelocityIteration ? 0.0f : biasCoefficient;
++ cfm = 0.0f;
++ }
++ else
++ {
++ //Note:
++ //cfm has units velocity per unit impulse = kg^-1 for linear constraints
++ //erp is dimensionless
++
++ //Governing equation is:
++ //Jv + (r + cfm)*lambda + (erp/dt)* C = 0
++ //with
++ //C = qA + G*qB + offset
++ //r = rAA + G*(rAB + rBA) + rBB
++ //and dt the timestep
++
++ //Solve for lambda:
++ //lambda = -(1/(cfm + r)] * (erp*C/dt + Jv)
++ // = -[1/(1 + r/cfm)] * [ C*(erp/(dt*cfm)) + v0*(1/cfm) ]
++ //with v0 = Jv
++
++ //Now compare with spring
++ //lambda = -[1/(1 + r*dt*(dt*kp + kd)][ C*dt*kp + v0*dt*(dt*kp + kd) ]
++
++ //Solve for cfm by comparing terms.
++ //1/cfm = dt*(dt*kp + kd)
++ //cfm = 1/[dt*(dt*kp + kd)]
++
++ //Solve for erp by comparing terms.
++ //erp/(dt*cfm) = dt*kp
++ //erp = dt*dt*kp*cfm = dt*kp/(dt*kp + kd)
++
++ //Summarise results for cfm, erp.
++ // cfm = 1/[dt*(dt*kp + kd)]
++ // erp = dt*kp/(dt*kp + kd)
++
++ //Now express cfm and erp in terms of natural frequency mu and damping ratio zeta
++ //Start with computing kp from mu.
++ //Remember that r plays role of reciprocal mass
++ //kp = mu^2 / r
++ //kd = 2 * (mu / r) * zeta
++
++ //Summarise:
++ //Given mu and zeta and r we have
++ //kp = mu^2 / r
++ //kd = 2 * (mu / r) * zeta
++ //cfm = 1/[dt*(dt*kp + kd)]
++ //erp = dt*kp/(dt*kp + kd)
++ //lambda = -(1/(cfm + r)] * (erp*C/dt + Jv)
++
++ //Compute cfm, erp from mu, zeta, r.
++ const PxReal mu = naturalFrequency;
++ const PxReal zeta = dampingRatio;
++ const PxReal kp = mu * mu / r;
++ const PxReal kd = 2.0f * mu * zeta / r;
++ cfm = 1.0f / (dt * (dt * kp + kd));
++ erp = (dt * kp) / (dt * kp + kd);
++ }
++
++ //Comute lambda
++ //lambda = -[ (erp * C/ dt) + Jv ] / [r + cfm]
++ //C = qA + gearRatio* qB + offset
++ //b = erp*C/dt
++ //JV = qADot + gearRatio*qBDot
++ const PxReal C = qA + gearRatio * qB + offset;
++ const PxReal b = erp * C * invDt;
++ const PxReal Jv = qADot + gearRatio * qBDot;
++ const PxReal effectiveInertia = 1.0f / (r + cfm);
++ const PxReal lambda = -(b + Jv) * effectiveInertia;
++ jointImpA = lambda;
++ jointImpB = gearRatio * lambda;
++ }
++
++ struct TendonImplicitSpringParams
++ {
++ PX_CUDA_CALLABLE PX_FORCE_INLINE TendonImplicitSpringParams(const PxReal biasCoef, const PxReal velMult, const PxReal impMult, const PxReal limitBiasCoef, const PxReal limitIMpMult)
++ : biasCoefficient(biasCoef),
++ velMultiplier(velMult),
++ impulseMultiplier(impMult),
++ limitBiasCoefficient(limitBiasCoef),
++ limitImpulseMultiplier(limitIMpMult)
++ {
++ }
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE TendonImplicitSpringParams()
++ : biasCoefficient(0.0f), velMultiplier(0.0f), impulseMultiplier(0.0f), limitBiasCoefficient(0.0f), limitImpulseMultiplier(0.0f)
++ {
++ }
++
++ PxReal biasCoefficient;
++ PxReal velMultiplier;
++ PxReal impulseMultiplier;
++ PxReal limitBiasCoefficient;
++ PxReal limitImpulseMultiplier;
++ };
++
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void computeSpringParams
++ (const PxReal stepDt, const bool isTGSSolver,
++ const PxReal unitResponse,
++ const PxReal stiffness, const PxReal damping,
++ PxReal& biasCoefficient, PxReal& velMultiplier, PxReal& impulseMultiplier)
++ {
++ const PxReal a = stepDt * (stepDt*stiffness + damping);
++ const PxReal x = unitResponse > 0.f ? 1.0f / (1.0f + a * unitResponse) : 0.f;
++ biasCoefficient = (-stiffness * x * stepDt);
++ velMultiplier = -x * a;
++ impulseMultiplier = isTGSSolver ? 1.f : 1.f - x;
++ }
++
++ /**
++ \brief Compute the parameters for implicit tendon springs (1 spring for rest length, 1 spring for limit).
++ \param[in] dt is the timestep that will be used to forward integrate the spring position bias.
++ \param[in] isTGSSolver is true if TGS solver is active, false if PGS solver is active.
++ \param[in] unitResponse is the multiplier that converts impulse to velocity change.
++ \param[in] stiffness is the stiffness of the rest length spring (force per unit position bias)
++ \param[in] damping is the damping of the rest length spring (force per unit velocity bias)
++ \param[in] limitStiffness is the stiffness of the limit spring (force per unit position bias)
++ \return The params used to compute the impulse arising from current tendon state.
++ \see computeTendonImpulse
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE TendonImplicitSpringParams computeTendonSpringParams
++ (const PxReal dt, const bool isTGSSolver,
++ const PxReal unitResponse,
++ const PxReal stiffness, const PxReal damping,
++ const PxReal limitStiffness)
++ {
++ TendonImplicitSpringParams params;
++
++ computeSpringParams(
++ dt, isTGSSolver,
++ unitResponse,
++ stiffness, damping,
++ params.biasCoefficient, params.velMultiplier, params.impulseMultiplier);
++
++ PxReal limitVelMultiplier = 0.0f;
++ computeSpringParams(
++ dt, isTGSSolver,
++ unitResponse,
++ limitStiffness, damping,
++ params.limitBiasCoefficient, limitVelMultiplier, params.limitImpulseMultiplier);
++
++ return params;
++ }
++
++ /**
++ \brief Compute the tendon impulse for the rest length spring and for the limit spring and compute the change in total impulse.
++ \param[in] recipCoefficient is a multiplier used to scale (restLengthError * biasCoefficient + tendonSpeed * velMultiplier)
++ \param[in] tendonImplicitSpringParams are the params for the implicit springs of the tendon.
++ \param[in] restLengthError is the bias of the rest length spring
++ \param[in] tendonSpeed is the speed of the tendon
++ \param[in] limitError is the bias of the limit spring
++ \param[in] accumulatedRestLengthImpulse is the accumualted impulse applied to the rest length spring
++ \param[in] accumulatedLimitImpulse is the accumualted impulse applied to the limit spring
++ \param[out] restLengthImpulse is the new impulse to apply to the rest length spring
++ \param[out] limitImpulse is the new impulse to apply to the limit spring
++ \param[out] deltaF is the change in total impulse
++ \see computeTendonSpringParams
++ \note restLengthImpulse = ((restLengthError*biasCoefficient*) + (tendonSpeed*velMultiplier))*recipCoefficient + accumulatedRestLengthImpulse*impulseMultiplier
++ \note accumulatedLimitImpulse = ((limitError*limitBiasCoefficient*)*recipCoefficient + accumulatedLimitImpulse*limitImpulseMultiplier
++ */
++ PX_CUDA_CALLABLE PX_FORCE_INLINE void computeTendonImpulse
++ (const PxReal recipCoefficient,
++ const TendonImplicitSpringParams& tendonImplicitSpringParams,
++ const PxReal restLengthError, const PxReal tendonSpeed, const PxReal limitError,
++ const PxReal accumulatedRestLengthImpulse, const PxReal accumulatedLimitImpulse,
++ PxReal& restLengthImpulse, PxReal& limitImpulse, PxReal& deltaF)
++ {
++ restLengthImpulse =
++ (restLengthError * tendonImplicitSpringParams.biasCoefficient + tendonSpeed * tendonImplicitSpringParams.velMultiplier) * recipCoefficient
++ + accumulatedRestLengthImpulse * tendonImplicitSpringParams.impulseMultiplier;
++
++ limitImpulse =
++ limitError * tendonImplicitSpringParams.limitBiasCoefficient * recipCoefficient
++ + accumulatedLimitImpulse * tendonImplicitSpringParams.limitImpulseMultiplier;
++
++ deltaF = (restLengthImpulse - accumulatedRestLengthImpulse) + (limitImpulse - accumulatedLimitImpulse);
++ }
++
++ /**
++ \brief The default constraint resolution order is as follows:
++
++ dynamic 1d constraints + dynamic contact constraints
++ spatial tendons + fixed tendons + mimic joints
++ friction (per dof)
++ drive (per dof)
++ pos limit (per dof)
++ vel limit (per dof)
++ static 1d constraints + static contact constraints
++
++ For grabbing scenarios, the following constraint resolution order can bring behaviour advantages:
++
++ spatial tendons + fixed tendons + mimic joints
++ friction (per dof)
++ drive (per dof)
++ pos limit (per dof)
++ dynamic 1d constraints + dynamic contact constraints
++ static 1d constraints + static contact constraints
++ vel limit (per dof)
++
++ The default constraint order requires a single pass through the articulations
++ after completing dynamic contact.
++
++ The grabbing constraint order requires two passes through the articulations,
++ one before dynamic contact and one after.
++
++ ArticulationConstraintProcessingConfig controls the work that will be performed on articulations
++ during each pass (either one pass or two passes) through the articulations.
++ */
++ class ArticulationConstraintProcessingConfig
++ {
++ public:
++
++ struct VelLimit
++ {
++ enum Enum
++ {
++ eNONE,
++ eBEFORE_STATIC_CONSTRAINTS,
++ eAFTER_STATIC_CONSTRAINTS
++ };
++ };
++
++ protected:
++
++ ArticulationConstraintProcessingConfig(
++ bool doSpatialTendonsFixedTendonsMimicJoints,
++ bool doFrictionDrivePosLimit,
++ VelLimit::Enum doVelLimit,
++ bool doStaticContactsAnd1dConstraints)
++ : mDoSpatialTendonsFixedTendonsMimicJoints(doSpatialTendonsFixedTendonsMimicJoints),
++ mDoFrictionDrivePosLimit(doFrictionDrivePosLimit),
++ mDoVelLimit(doVelLimit),
++ mDoStaticContactsAnd1dConstraints(doStaticContactsAnd1dConstraints){}
++
++ public:
++
++ bool mDoSpatialTendonsFixedTendonsMimicJoints;
++ bool mDoFrictionDrivePosLimit;
++ VelLimit::Enum mDoVelLimit;
++ bool mDoStaticContactsAnd1dConstraints;
++ };
++
++ class ArticulationConstraintProcessingConfigCPU : public ArticulationConstraintProcessingConfig
++ {
++ private:
++
++ ArticulationConstraintProcessingConfigCPU(
++ bool doSpatialTendonsFixedTendonsMimicJoints,
++ bool doFrictionDrivePosLimit,
++ ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit,
++ bool doStaticContactsAnd1dConstraints)
++ : ArticulationConstraintProcessingConfig(doSpatialTendonsFixedTendonsMimicJoints, doFrictionDrivePosLimit, doVelLimit, doStaticContactsAnd1dConstraints)
++ {}
++
++ public:
++
++ //There are 3 distinct regimes on CPU:
++ //1) default contact order
++ //2) swapped contact order with no dynamic contact
++ //3) swapped contact order with dynamic contact
++
++ //1) sequence with default contact order:
++ //note that default contact order processes everything in a single pass.
++ //*dynamic contact*
++ //spatial tendons + fixed tendons + mimic joints
++ //friction + drive + pos limit
++ //vel limit
++ //static contact/1dconstraint
++
++ //2) sequence with swapped contact order and no dynamic contact
++ //note that in absence of dynamic contact we can process everything in a single pass
++ //spatial tendons + fixed tendons + mimic joints
++ //friction + drive + pos limit
++ //static contact/1dconstraint
++ //vel limit
++
++ //3) sequence with swapped contact order and dynamic contact
++ //Note that inclusion of dynamic contact requires 2 passes through the tree.
++ //spatial tendons + fixed tendons + mimic joints (1st pass)
++ //friction + drive + pos limit (1st pass)
++ //*dynamic contact*
++ //static contact/1d constraint (2nd pass)
++ //vel limit (2nd pass)
++
++ //Case 1) with false==solveArticulationContactLast and Case 2) with true==solveArticulationContactLast
++ static PX_FORCE_INLINE ArticulationConstraintProcessingConfigCPU getSinglePassConfig(const bool solveArticulationContactLast)
++ {
++ if(!solveArticulationContactLast)
++ return ArticulationConstraintProcessingConfigCPU(true, true, VelLimit::eBEFORE_STATIC_CONSTRAINTS, true);
++ else
++ return ArticulationConstraintProcessingConfigCPU(true, true, VelLimit::eAFTER_STATIC_CONSTRAINTS, true);
++ }
++
++ //Case 3)
++ static PX_FORCE_INLINE ArticulationConstraintProcessingConfigCPU getFirstPassConfig()
++ {
++ return ArticulationConstraintProcessingConfigCPU(true, true, VelLimit::eNONE, false);
++ }
++
++ //Case 3)
++ static PX_FORCE_INLINE ArticulationConstraintProcessingConfigCPU getSecondPassConfig()
++ {
++ return ArticulationConstraintProcessingConfigCPU(false, false, VelLimit::eAFTER_STATIC_CONSTRAINTS, true);
++ }
++ };
++
++ struct ArticulationConstraintProcessingConfigGPU : public ArticulationConstraintProcessingConfig
++ {
++ private:
++
++ ArticulationConstraintProcessingConfigGPU(
++ bool propagateRigidBodyImpulsesAndSolveSelfConstraints,
++ bool doSpatialTendonsFixedTendonsMimicJoints,
++ bool doFrictionDrivePosLimit,
++ ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit,
++ bool doStaticContactsAnd1dConstraints)
++ : ArticulationConstraintProcessingConfig(doSpatialTendonsFixedTendonsMimicJoints, doFrictionDrivePosLimit, doVelLimit, doStaticContactsAnd1dConstraints),
++ mPropagateRigidBodyImpulsesAndSolveSelfConstraints(propagateRigidBodyImpulsesAndSolveSelfConstraints)
++ {}
++
++ public:
++
++ //There are 2 distinct regimes on GPU because we do not have a specific path in the absence of dynamic contact.
++ //1) default contact order
++ //2) swapped contact order with/without dynamic contact
++
++ //1) sequence with default contact order:
++ //note that default contact order processes everything in a single pass.
++ //*dynamic contact*
++ //propagate impulses after dynamic contact
++ //spatial tendons + fixed tendons + mimic joints
++ //friction + drive + pos limit
++ //vel limit
++ //static contact/1dconstraint
++
++ //2) sequence with swapped contact order and with/without dynamic contact
++ //spatial tendons + fixed tendons + mimic joints (1st pass)
++ //friction + drive + pos limit (1st pass)
++ //*dynamic contact*
++ //propagate impulses after dynamic contact (2nd pass)
++ //static contact/1d constraint (2nd pass)
++ //vel limit (2nd pass)
++
++
++ //Case 1)
++ static PX_FORCE_INLINE ArticulationConstraintProcessingConfigGPU getSinglePassConfig()
++ {
++ ArticulationConstraintProcessingConfigGPU items(true, true, true, VelLimit::eBEFORE_STATIC_CONSTRAINTS, true);
++ return items;
++ }
++
++ //Case 2)
++ static PX_FORCE_INLINE ArticulationConstraintProcessingConfigGPU getFirstPassConfig()
++ {
++ ArticulationConstraintProcessingConfigGPU items(false, true, true, VelLimit::eNONE, false);
++ return items;
++ }
++
++ //Case 2)
++ static PX_FORCE_INLINE ArticulationConstraintProcessingConfigGPU getSecondPassConfig()
++ {
++ ArticulationConstraintProcessingConfigGPU items(true, false, false, VelLimit::eAFTER_STATIC_CONSTRAINTS, true);
++ return items;
++ }
++
++ bool mPropagateRigidBodyImpulsesAndSolveSelfConstraints;
++ };
++
++
++
++ } //namespace Dy
++ } //namespace physx
++ #endif //DY_ARTICULATION_CPUGPU_H