feat(physics): wire physx sdk into build
This commit is contained in:
834
engine/third_party/physx/source/lowleveldynamics/shared/DyCpuGpu1dConstraint.h
vendored
Normal file
834
engine/third_party/physx/source/lowleveldynamics/shared/DyCpuGpu1dConstraint.h
vendored
Normal file
@@ -0,0 +1,834 @@
|
||||
// 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-2025 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 DY_1DCONSTRAINT_CPUGPU_H
|
||||
#define DY_1DCONSTRAINT_CPUGPU_H
|
||||
|
||||
#include "foundation/PxSimpleTypes.h"
|
||||
#include "foundation/PxMemory.h"
|
||||
#include "foundation/PxVecMath.h"
|
||||
#include "PxConstraintDesc.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Dy
|
||||
{
|
||||
|
||||
/**
|
||||
\brief Compute the reciprocal of the unit response and avoid divide-by-small-number numerical failures.
|
||||
\param[in] unitResponse has value J * M^-1 * J^T with J the constraint Jacobian and M the constraint mass matrix.
|
||||
\param[in] minRowResponse is the smallest value of unitResponse that the constraint is configured to support.
|
||||
\return 0 if unitResponse is smaller than the smallest supported value, 1/unitResponse otherwise.
|
||||
*/
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeRecipUnitResponse(const PxReal unitResponse, const PxReal minRowResponse)
|
||||
{
|
||||
const PxReal recipResponse = unitResponse <= minRowResponse ? 0 : 1.0f/unitResponse;
|
||||
return recipResponse;
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Compute the minimum and maximum allowed impulse from the user-specified values that were specified
|
||||
as either a force or an impulse.
|
||||
\param[in] minSpecifiedImpulseOrForce is either the minimum allowed impulse or the minimum allowed force depending on the driveLimitsAreForces flag.
|
||||
\param[in] maxSpecifiedImpulseOrForce is either the maximum allowed impulse or the maximum allowed force depending on the driveLimitsAreForces flag.
|
||||
\param[in] hasDriveLimit is true if a minimum/maximum pair have been specified and is false if the allowed impulses are unbounded.
|
||||
\param[in] driveLimitsAreForces describes whether minSpecifiedImpulseOrForce and maxSpecifiedImpulseOrForce represent forces or impulses.
|
||||
\param[in] simDt is the duration of the scene simulation step.
|
||||
\param[out] minImpulseToUseInSolver is the minimum allowed impulse.
|
||||
\param[out] maxImpulseToUseInSolver is the maximum allowed impulse.
|
||||
\note minImpulseToUseInSolver is equal to minSpecifiedImpulseOrForce*simDt if both hasDriveLimit and driveLimitsAreForces are true.
|
||||
\note maxImpulseToUseInSolver is equal to maxSpecifiedImpulseOrForce*simDt if both hasDriveLimit and driveLimitsAreForces are true.
|
||||
\note minImpulseToUseInSolver is equal to minSpecifiedImpulseOrForce if either hasDriveLimit or driveLimitsAreForces are false.
|
||||
\note maxImpulseToUseInSolver is equal to maxSpecifiedImpulseOrForce if either hasDriveLimit or driveLimitsAreForces are false.
|
||||
*/
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeMinMaxImpulseOrForceAsImpulse
|
||||
(const PxReal minSpecifiedImpulseOrForce, const PxReal maxSpecifiedImpulseOrForce,
|
||||
const bool hasDriveLimit, const bool driveLimitsAreForces, const PxReal simDt,
|
||||
PxReal& minImpulseToUseInSolver, PxReal& maxImpulseToUseInSolver)
|
||||
{
|
||||
const PxReal driveScale = (hasDriveLimit && driveLimitsAreForces) ? simDt : 1.0f;
|
||||
minImpulseToUseInSolver = minSpecifiedImpulseOrForce*driveScale;
|
||||
maxImpulseToUseInSolver = maxSpecifiedImpulseOrForce*driveScale;
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Compute the values jointSpeedForRestitutionBounce and initJointSpeed that will be used in compute1dConstraintSolverConstantsPGS().
|
||||
\param[in] normalVel0 is the projection of the velocity of body 0 projected against the corresponding Jacobian terms.
|
||||
\param[in] isRigidDynamic0 is true if body 0 is a rigid dynamic and false if it is an articulation link.
|
||||
\param[in] normalVel1 is the projection of the velocity of body 1 projected against the corresponding Jacobian terms.
|
||||
\param[in] isRigidDynamic1 is true if body 1 is a rigid dynamic and false if it is an articulation link.
|
||||
\param[out] jointSpeedForRestitutionBounce is the projection of the velocity of the constraint's body pair against the constraint Jacobian.
|
||||
param[out] initJointSpeed is the initial speed of the constraint as experienced by the solver. In the absence of rigid dynamics, this will have value 0.0.
|
||||
*/
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeJointSpeedPGS
|
||||
(const PxReal normalVel0, const bool isRigidDynamic0, const PxReal normalVel1, const bool isRigidDynamic1,
|
||||
PxReal& jointSpeedForRestitutionBounce, PxReal& initJointSpeed)
|
||||
{
|
||||
// this helper is only meant to be used for scenarios where at least one body is an articulation link
|
||||
#if PX_CUDA_COMPILER
|
||||
assert(!(isRigidDynamic0 && isRigidDynamic1)); // until PX_ASSERT works on GPU (see PX-4133)
|
||||
#else
|
||||
PX_ASSERT(!(isRigidDynamic0 && isRigidDynamic1));
|
||||
#endif
|
||||
|
||||
//
|
||||
// The solver tries to achieve a specified relative target velocity vT.
|
||||
// Comparing to the current relative velocity provides the velocity error
|
||||
// verr that has to resolved:
|
||||
// verr = vT - (v0 - v1)
|
||||
//
|
||||
// However, for rigid dynamics, the PGS solver does not work with the actual
|
||||
// velocities of the bodies but the delta velocity dv (compared to the start).
|
||||
// As such, the body velocities at the first solver step will be 0.
|
||||
//
|
||||
// v = vStart + dv
|
||||
//
|
||||
// verr = vT - ((v0Start + dv0) - (v1Start + dv1))
|
||||
// = vT - (v0Start - v1Start) - (dv0 - dv1)
|
||||
// = vT' - (dv0 - dv1)
|
||||
//
|
||||
// As shown above, this can be achieved by initially shifting the target velocity,
|
||||
// using vT' instead of vT.
|
||||
// initJointSpeed will hold this shift. Note that articulation links do use the
|
||||
// actual velocity in the solver and thus the code here only computes a non zero
|
||||
// shift if rigid dynamics are involved.
|
||||
//
|
||||
|
||||
initJointSpeed = 0.f;
|
||||
if (isRigidDynamic0)
|
||||
initJointSpeed = normalVel0;
|
||||
else if (isRigidDynamic1)
|
||||
initJointSpeed = -normalVel1;
|
||||
jointSpeedForRestitutionBounce = normalVel0 - normalVel1;
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Determine if a constraint will have a bounce response and compute the bounce velocity using the restitution parameter.
|
||||
\param[in] constraintFlags describes whether the constraint is configured to have a bounce response using the restitution parameter.
|
||||
\param[in] jointSpeedForRestitutionBounce is the velocity of the constraint's body pair projected against the constraint Jacobian.
|
||||
\param[in] bounceThreshold is the minimum speed that will trigger a bounce response.
|
||||
\param[in] restitution is the restitution parameter to apply for the bounce response.
|
||||
\param[in] geometricError is the geometric error of the constraint.
|
||||
\return The bounce velocity using the restitution parameter if a bounce should take place, 0 if no bounce response should get triggered.
|
||||
*/
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeBounceVelocity(const PxU16 constraintFlags, const PxReal jointSpeedForRestitutionBounce, const PxReal bounceThreshold,
|
||||
const PxReal restitution, const PxReal geometricError)
|
||||
{
|
||||
PxReal bounceVel = jointSpeedForRestitutionBounce * (-restitution);
|
||||
|
||||
if ((constraintFlags & Px1DConstraintFlag::eRESTITUTION) &&
|
||||
(-jointSpeedForRestitutionBounce > bounceThreshold) &&
|
||||
((bounceVel * geometricError) <= 0.0f)) // for now, no bounce in case of a speculative CCD scenario, where the geometric error points in
|
||||
// the same direction as the bounce velocity (separation, not penetration). If we solved the
|
||||
// geometric error in the bounce scenario too, then it would likely work without this check,
|
||||
// however, we are not doing that and thus would only generate undesired behavior if bounced.
|
||||
{
|
||||
return bounceVel;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Constraint1dSolverConstantsPGS contains the constant terms used by the PGS 1d constraint solver functions.
|
||||
For position iterations we have:
|
||||
newImpulse = oldImpulse*impulseMultiplier + constraintVel*velMultplier + constant.
|
||||
For velocity iterations we have:
|
||||
newImpulse = oldImpulse*impulseMultiplier + constraintVel*velMultplier + unbiasedConstant.
|
||||
\see compute1dConstraintSolverConstantsPGS()
|
||||
*/
|
||||
struct Constraint1dSolverConstantsPGS
|
||||
{
|
||||
PxReal constant;
|
||||
PxReal unbiasedConstant;
|
||||
PxReal velMultiplier;
|
||||
PxReal impulseMultiplier;
|
||||
};
|
||||
PX_COMPILE_TIME_ASSERT(sizeof(Constraint1dSolverConstantsPGS) == 16);
|
||||
|
||||
/**
|
||||
\brief Compute the constant terms that will be used by the PGS solver.
|
||||
\param[in] constraintFlags describes whether the constraint is an acceleration spring, a force spring, a bounce constraint or a hard constraint.
|
||||
\param[in] springStiffness describes the stiffness of the spring. This is ignored if Px1DConstraintFlag::eACCELERATION_SPRING is lowered in constraintFlags.
|
||||
\param[in] springDamping describes the damping of the spring. This is ignored if Px1DConstraintFlag::eACCELERATION_SPRING is lowered in constraintFlags.
|
||||
\param[in] restitution describes the bounce restitution of the spring. This is ignored if Px1DConstraintFlag::eRESTITUTION is lowered in constraintFlags.
|
||||
\param[in] bounceThreshold describes the minimum speed required to trigger a bounce response. This is ignored if Px1DConstraintFlag::eRESTITUTION is lowered in constraintFlags.
|
||||
\param[in] geometricError is the position error of the constraint.
|
||||
\param[in] velocityTarget is the target speed of the constraint.
|
||||
\param[in] jointSpeedForRestitutionBounce is the projection of the velocity of the constraint's body pair against the constraint Jacobian.
|
||||
\param[in] initJointSpeed is the initial speed of the constraint as experienced by the solver.
|
||||
\param[in] unitResponse is J * M^-1 * J^T with J the constraint Jacobian and M the constraint Mass matrix.
|
||||
\param[in] recipUnitResponse is 1/unitResponse with the caveat that very small values of unitResponse have a reciprocal of 0.0. See computeRecipUnitResponse()
|
||||
\param[in] erp is the Baumgarte multiplier used to scale the geometricError. The constraint will attempt to resolve geometricError*erp. This is ignored for spring or bounce constraints.
|
||||
\param[in] simDt is the timestep of the scene simulate step.
|
||||
\param[in] recipSimDt is equal to 1/simDt.
|
||||
\return A Constraint1dSolverConstantsPGS instance that contains the solver constant terms.
|
||||
*/
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE Constraint1dSolverConstantsPGS compute1dConstraintSolverConstantsPGS
|
||||
(const PxU16 constraintFlags,
|
||||
const PxReal springStiffness, const PxReal springDamping,
|
||||
const PxReal restitution, const PxReal bounceThreshold,
|
||||
const PxReal geometricError, const PxReal velocityTarget,
|
||||
const PxReal jointSpeedForRestitutionBounce, const PxReal initJointSpeed,
|
||||
const PxReal unitResponse, const PxReal recipUnitResponse,
|
||||
const PxReal erp,
|
||||
const PxReal simDt, const PxReal recipSimDt)
|
||||
{
|
||||
Constraint1dSolverConstantsPGS desc = {0, 0, 0, 0};
|
||||
|
||||
if(constraintFlags & Px1DConstraintFlag::eSPRING)
|
||||
{
|
||||
const PxReal a = simDt * simDt * springStiffness + simDt * springDamping;
|
||||
const PxReal b = simDt * (springDamping * velocityTarget - springStiffness * geometricError);
|
||||
|
||||
if(constraintFlags & Px1DConstraintFlag::eACCELERATION_SPRING)
|
||||
{
|
||||
const PxReal x = 1.0f/(1.0f+a);
|
||||
const PxReal constant = x * recipUnitResponse * b;
|
||||
desc.constant = constant;
|
||||
desc.unbiasedConstant = constant;
|
||||
desc.velMultiplier = -x * recipUnitResponse * a;
|
||||
desc.impulseMultiplier = 1.0f - x;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal x = 1.0f/(1.0f+a*unitResponse);
|
||||
const PxReal constant = x * b;
|
||||
desc.constant = constant;
|
||||
desc.unbiasedConstant = constant;
|
||||
desc.velMultiplier = -x * a;
|
||||
desc.impulseMultiplier = 1.0f - x;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
desc.velMultiplier = -recipUnitResponse;
|
||||
desc.impulseMultiplier = 1.0f;
|
||||
|
||||
const PxReal bounceVel = computeBounceVelocity(constraintFlags, jointSpeedForRestitutionBounce, bounceThreshold, restitution, geometricError);
|
||||
if (bounceVel != 0.0f)
|
||||
{
|
||||
const PxReal constant = recipUnitResponse * bounceVel;
|
||||
desc.constant = constant;
|
||||
desc.unbiasedConstant = constant;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal geomError = geometricError * erp;
|
||||
desc.constant = recipUnitResponse * (velocityTarget - geomError*recipSimDt);
|
||||
desc.unbiasedConstant = (!(constraintFlags & Px1DConstraintFlag::eKEEPBIAS)) ? (recipUnitResponse * velocityTarget) : desc.constant;
|
||||
}
|
||||
}
|
||||
|
||||
const PxReal velBias = initJointSpeed * desc.velMultiplier;
|
||||
desc.constant += velBias;
|
||||
desc.unbiasedConstant += velBias;
|
||||
|
||||
return desc;
|
||||
}
|
||||
|
||||
|
||||
// Computing constant, unbiasedConstant, velMultiplier, and impulseMultiplier using precomputed coefficients.
|
||||
// See also "queryReduced1dConstraintSolverConstantsPGS" and "compute1dConstraintSolverConstantsPGS."
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE void compute1dConstraintSolverConstantsPGS
|
||||
(bool isSpring, bool isAccelerationSpring, PxReal coeff0, PxReal coeff1, PxReal coeff2,
|
||||
const PxReal unitResponse, const PxReal recipUnitResponse,
|
||||
PxReal& constant, PxReal& unbiasedConstant, PxReal& velMultiplier, PxReal& impulseMultiplier)
|
||||
{
|
||||
if (isSpring)
|
||||
{
|
||||
// coeff0: a
|
||||
// coeff1: b
|
||||
|
||||
const PxReal a = coeff0;
|
||||
const PxReal b = coeff1;
|
||||
|
||||
if (isAccelerationSpring)
|
||||
{
|
||||
const PxReal x = 1.0f / (1.0f + a);
|
||||
constant = x * recipUnitResponse * b;
|
||||
unbiasedConstant = constant;
|
||||
velMultiplier = -x * recipUnitResponse * a;
|
||||
impulseMultiplier = 1.0f - x;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal x = 1.0f / (1.0f + a * unitResponse);
|
||||
constant = x * b;
|
||||
unbiasedConstant = constant;
|
||||
velMultiplier = -x * a;
|
||||
impulseMultiplier = 1.0f - x;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// coeff0: constant (to be scaled by recipUnitResponse)
|
||||
// coeff1: unbiasedConstant (to be scaled by recipUnitResponse)
|
||||
|
||||
velMultiplier = -recipUnitResponse;
|
||||
impulseMultiplier = 1.0f;
|
||||
|
||||
constant = coeff0 * recipUnitResponse;
|
||||
unbiasedConstant = coeff1 * recipUnitResponse;
|
||||
}
|
||||
|
||||
// coeff2: initJointSpeed
|
||||
const PxReal velBias = coeff2 * velMultiplier;
|
||||
constant += velBias;
|
||||
unbiasedConstant += velBias;
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Constraint1dSolverConstantsTGS contains the constant terms used by the TGS 1d constraint solver functions.
|
||||
For velocity iterations we have:
|
||||
newImpulse = oldImpulse + constraintVel*velMultplier + (1/unitResponse)*(error + biasScale*deltaGeometricError + targetVel).
|
||||
For position iterations we have:
|
||||
newImpulse = oldImpulse + constraintVel*velMultplier + (1/unitResponse)*(targetVel).
|
||||
*/
|
||||
struct Constraint1dSolverConstantsTGS
|
||||
{
|
||||
PxReal biasScale;
|
||||
PxReal error;
|
||||
PxReal velMultiplier;
|
||||
PxReal targetVel;
|
||||
};
|
||||
|
||||
/**
|
||||
\brief Compute the maximum constraint speed arising from geometric error that is allowed during solver iterations.
|
||||
The bias speed is computed as: biasScale *(geometricError + deltaGeometricError)
|
||||
\note Spring constraints have unbounded bias speed. The implicit formulation ensures stability.
|
||||
\note Bounce constraints are clamped with zero bias speed because they completetely ignore the geometric error and target only the bounce speed.
|
||||
\note Hard constraints have hard-coded maximum bias speeds based on historical testing.
|
||||
*/
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeMaxBiasVelocityTGS
|
||||
(const PxU16 constraintFlags,
|
||||
const PxReal jointSpeedForRestitutionBounce, const PxReal bounceThreshold,
|
||||
const PxReal restitution, const PxReal geometricError,
|
||||
const bool isExtendedConstraint,
|
||||
const PxReal lengthScale, const PxReal recipSimDt)
|
||||
{
|
||||
PxReal maxBiasSpeed = PX_MAX_F32;
|
||||
if (constraintFlags & Px1DConstraintFlag::eSPRING)
|
||||
{
|
||||
maxBiasSpeed = PX_MAX_F32;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal bounceVel = computeBounceVelocity(constraintFlags, jointSpeedForRestitutionBounce, bounceThreshold, restitution, geometricError);
|
||||
if (bounceVel != 0.0f)
|
||||
{
|
||||
maxBiasSpeed = 0;
|
||||
}
|
||||
else if (constraintFlags & Px1DConstraintFlag::eANGULAR_CONSTRAINT)
|
||||
{
|
||||
maxBiasSpeed = recipSimDt * 0.75f;
|
||||
}
|
||||
else
|
||||
{
|
||||
maxBiasSpeed = isExtendedConstraint ? recipSimDt * 1.5f * lengthScale : recipSimDt * 15.f * lengthScale;
|
||||
}
|
||||
}
|
||||
return maxBiasSpeed;
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Compute the values jointSpeedForRestitutionBounce and initJointSpeed that will be used in compute1dConstraintSolverConstantsTGS().
|
||||
\param[in] normalVel0 is the projection of the velocity of body 0 projected against the corresponding Jacobian terms.
|
||||
\param[in] isKinematic0 is true if body 0 is a kinematic, false otherwise.
|
||||
\param[in] normalVel1 is the projection of the velocity of body 1 projected against the corresponding Jacobian terms.
|
||||
\param[in] isKinematic1 is true if body 1 is a kinematic, false otherwise.
|
||||
\param[out] jointSpeedForRestitutionBounce is the projection of the velocity of the constraint's body pair against the constraint Jacobian.
|
||||
param[out] initJointSpeed is the initial speed of the constraint as experienced by the solver. In the absence of kinematics, this will have value 0.0.
|
||||
*/
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeJointSpeedTGS
|
||||
(const PxReal normalVel0, const bool isKinematic0, const PxReal normalVel1, const bool isKinematic1,
|
||||
PxReal& jointSpeedForRestitutionBounce, PxReal& initJointSpeed)
|
||||
{
|
||||
//
|
||||
// The solver treats kinematics more or less like static objects and
|
||||
// thus these objects will have a velocity of zero during the solver
|
||||
// iterations. This, however, requires that the target velocity gets
|
||||
// corrected initially to take this into account. initJointSpeed
|
||||
// represents this correction. See computeJointSpeedPGS() for a more
|
||||
// general explanation of shifting velocities. Kinematics have a constant
|
||||
// velocity that can not be changed by the solver, so it is fine to
|
||||
// apply the correction at the beginning and then consider the velocity
|
||||
// zero during all solver iterations. Note that unlike PGS, TGS is
|
||||
// using the full velocities of the objects during solver iterations
|
||||
// and only kinematics are the exception. Hence, the difference to
|
||||
// computeJointSpeedPGS().
|
||||
//
|
||||
|
||||
initJointSpeed = 0.f;
|
||||
if (isKinematic0)
|
||||
initJointSpeed -= normalVel0;
|
||||
if (isKinematic1)
|
||||
initJointSpeed += normalVel1;
|
||||
jointSpeedForRestitutionBounce = normalVel0 - normalVel1;
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Compute the constant terms that will be used by the TGS solver.
|
||||
\param[in] constraintFlags describes whether the constraint is an acceleration spring, a force spring, a bounce constraint or a hard constraint.
|
||||
\param[in] springStiffness describes the stiffness of the spring. This is ignored if Px1DConstraintFlag::eACCELERATION_SPRING is lowered in constraintFlags.
|
||||
\param[in] springDamping describes the damping of the spring. This is ignored if Px1DConstraintFlag::eACCELERATION_SPRING is lowered in constraintFlags.
|
||||
\param[in] restitution describes the bounce restitution of the spring. This is ignored if Px1DConstraintFlag::eRESTITUTION is lowered in constraintFlags.
|
||||
\param[in] bounceThreshold describes the minimum speed required to trigger a bounce response. This is ignored if Px1DConstraintFlag::eRESTITUTION is lowered in constraintFlags.
|
||||
\param[in] geometricError is the position error of the constraint.
|
||||
\param[in] velocityTarget is the target speed of the constraint.
|
||||
\param[in] jointSpeedForRestitutionBounce is the projection of the velocity of the constraint's body pair against the constraint Jacobian.
|
||||
\param[in] initJointSpeed is the initial speed of the constraint as experienced by the solver. In the absence of kinematics, this has value 0.0.
|
||||
\param[in] unitResponse is J * M^-1 * J^T with J the constraint Jacobian and M the constraint Mass matrix.
|
||||
\param[in] recipUnitResponse is 1/unitResponse with the caveat that very small values of unitResponse have a reciprocal of 0.0. See computeRecipUnitResponse()
|
||||
\param[in] erp is the Baumgarte multiplier used to scale the geometricError. The constraint will attempt to resolve geometricError*erp. This is ignored for spring or bounce constraints.
|
||||
\param[in] stepDt is the timestep of each TGS position iteration step.
|
||||
\param[in] recipStepDt is equal to 1/stepDt.
|
||||
\return A Constraint1dSolverConstantsTGS instance that contains the solver constant terms.
|
||||
*/
|
||||
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE Constraint1dSolverConstantsTGS compute1dConstraintSolverConstantsTGS
|
||||
(const PxU16 constraintFlags,
|
||||
const PxReal springStiffness, const PxReal springDamping,
|
||||
const PxReal restitution, const PxReal bounceThreshold,
|
||||
const PxReal geometricError, const PxReal velocityTarget,
|
||||
const PxReal jointSpeedForRestitutionBounce, const PxReal initJointSpeed,
|
||||
const PxReal unitResponse, const PxReal recipUnitResponse,
|
||||
const PxReal erp,
|
||||
const PxReal stepDt, const PxReal recipStepDt)
|
||||
{
|
||||
Constraint1dSolverConstantsTGS desc = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
if (constraintFlags & Px1DConstraintFlag::eSPRING)
|
||||
{
|
||||
const PxReal a = stepDt * (stepDt * springStiffness + springDamping);
|
||||
const PxReal b = stepDt * (springDamping * velocityTarget);
|
||||
|
||||
if (constraintFlags & Px1DConstraintFlag::eACCELERATION_SPRING)
|
||||
{
|
||||
const PxReal x = 1.0f / (1.0f + a);
|
||||
const PxReal biasScale = -x * springStiffness * stepDt * recipUnitResponse;
|
||||
const PxReal velMultiplier = -x * a * recipUnitResponse;
|
||||
|
||||
desc.biasScale = biasScale;
|
||||
desc.error = biasScale * geometricError;
|
||||
desc.velMultiplier = velMultiplier;
|
||||
desc.targetVel = x * b * recipUnitResponse - velMultiplier * initJointSpeed;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal x = 1.0f / (1.0f + a*unitResponse);
|
||||
const PxReal biasScale = -x * springStiffness * stepDt;
|
||||
const PxReal velMultiplier = -x * a;
|
||||
|
||||
desc.biasScale = biasScale;
|
||||
desc.error = biasScale * geometricError;
|
||||
desc.velMultiplier = velMultiplier;
|
||||
desc.targetVel = x * b - velMultiplier * initJointSpeed;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal bounceVel = computeBounceVelocity(constraintFlags, jointSpeedForRestitutionBounce, bounceThreshold, restitution, geometricError);
|
||||
|
||||
if (bounceVel != 0.0f)
|
||||
{
|
||||
const PxReal velMultiplier = -1.0f;
|
||||
|
||||
desc.biasScale = 0.f;
|
||||
desc.error = 0.f;
|
||||
desc.velMultiplier = velMultiplier;
|
||||
desc.targetVel = bounceVel - velMultiplier * initJointSpeed;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal velMultiplier = -1.0f;
|
||||
const PxReal biasScale = -recipStepDt*erp;
|
||||
|
||||
desc.biasScale = biasScale;
|
||||
desc.error = geometricError*biasScale;
|
||||
desc.velMultiplier = velMultiplier;
|
||||
desc.targetVel = velocityTarget - velMultiplier * initJointSpeed;
|
||||
}
|
||||
}
|
||||
|
||||
return desc;
|
||||
}
|
||||
|
||||
// Save required coefficients to compute initBias, biasScale, velMultiplier, and velTarget every sub-timestep.
|
||||
// This does not change the previous impulse formulation.
|
||||
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE void queryReduced1dConstraintSolverConstantsTGS
|
||||
(const PxU16 constraintFlags,
|
||||
const PxReal springStiffness, const PxReal springDamping,
|
||||
const PxReal restitution, const PxReal bounceThreshold,
|
||||
const PxReal geometricError, const PxReal velocityTarget,
|
||||
const PxReal jointSpeedForRestitutionBounce, const PxReal initJointSpeed,
|
||||
const PxReal erp, const PxReal stepDt, const PxReal recipStepDt,
|
||||
PxReal& coeff0, PxReal& coeff1, PxReal& coeff2, PxReal& coeff3)
|
||||
{
|
||||
// coeff0: initJointSpeed
|
||||
// coeff1: biasScale
|
||||
coeff0 = initJointSpeed;
|
||||
|
||||
if (constraintFlags & Px1DConstraintFlag::eSPRING)
|
||||
{
|
||||
// coeff2: a, coeff3: b
|
||||
const PxReal a = stepDt * (stepDt * springStiffness + springDamping);
|
||||
const PxReal b = stepDt * (springDamping * velocityTarget);
|
||||
|
||||
coeff2 = a;
|
||||
coeff3 = b;
|
||||
|
||||
if (constraintFlags & Px1DConstraintFlag::eACCELERATION_SPRING)
|
||||
{
|
||||
const PxReal x = 1.0f / (1.0f + a);
|
||||
coeff1 = -x * springStiffness * stepDt; // to recover biasScale, multiply it by recipUnitResponse
|
||||
}
|
||||
else
|
||||
{
|
||||
coeff1 = -springStiffness * stepDt; // to recover biasScale, multiply it by x
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// coeff2: bounceVel, coeff3: velocityTarget
|
||||
const PxReal bounceVel = computeBounceVelocity(constraintFlags, jointSpeedForRestitutionBounce, bounceThreshold, restitution, geometricError);
|
||||
|
||||
coeff2 = bounceVel;
|
||||
coeff3 = velocityTarget;
|
||||
|
||||
if (bounceVel != 0.0f)
|
||||
{
|
||||
coeff1 = 0.f; // biasScale is zero
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal biasScale = -recipStepDt * erp;
|
||||
coeff1 = biasScale; // biasScale
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Computing initBias, biasScale, velMultiplier, and velTarget at each sub-timestep using the coefficients saved in queryReduced1dConstraintSolverConstantsTGS.
|
||||
// This does not change the previous impulse formulation.
|
||||
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE void
|
||||
compute1dConstraintSolverConstantsTGS(bool isSpring, bool isAccelerationSpring, const PxReal geometricError,
|
||||
const PxReal unitResponse, const PxReal recipUnitResponse,
|
||||
PxReal coeff0, PxReal coeff1, PxReal coeff2, PxReal coeff3, PxReal& biasScale,
|
||||
PxReal& velMultiplier, PxReal& error, PxReal& targetVel)
|
||||
{
|
||||
PxReal initJointSpeed = coeff0;
|
||||
biasScale = coeff1;
|
||||
|
||||
if (isSpring)
|
||||
{
|
||||
// coeff2: a, coeff3: b
|
||||
const PxReal a = coeff2;
|
||||
const PxReal b = coeff3;
|
||||
|
||||
if (isAccelerationSpring)
|
||||
{
|
||||
biasScale *= recipUnitResponse;
|
||||
error = biasScale * geometricError;
|
||||
|
||||
const PxReal x = 1.0f / (1.0f + a);
|
||||
velMultiplier = -x * a * recipUnitResponse;
|
||||
targetVel = x * b * recipUnitResponse - velMultiplier * initJointSpeed;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal x = 1.0f / (1.0f + a * unitResponse);
|
||||
biasScale *= x;
|
||||
error = biasScale * geometricError;
|
||||
velMultiplier = -x * a;
|
||||
targetVel = x * b - velMultiplier * initJointSpeed;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// coeff2: bounceVel, coeff3: velocityTarget
|
||||
const PxReal bounceVel = coeff2;
|
||||
const PxReal velocityTarget = coeff3;
|
||||
|
||||
if (bounceVel != 0.0f)
|
||||
{
|
||||
biasScale = 0.f; // biasScale is zero
|
||||
error = 0.f;
|
||||
velMultiplier = -1.0f;
|
||||
targetVel = bounceVel - velMultiplier * initJointSpeed;
|
||||
}
|
||||
else
|
||||
{
|
||||
error = geometricError * biasScale;
|
||||
velMultiplier = -1.0f;
|
||||
targetVel = velocityTarget - velMultiplier * initJointSpeed;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
\brief Translate external flags and hints to internal flags.
|
||||
\param[in] externalFlags Parameter that holds Px1DConstraintFlag::Type flags to translate.
|
||||
\param[in] externalSolveHint Parameter that holds the solve hint information to translate.
|
||||
\param[out] internalFlags Location to apply the internal flags to.
|
||||
*/
|
||||
template<typename T, typename U>
|
||||
PX_CUDA_CALLABLE PX_FORCE_INLINE void raiseInternalFlagsTGS(T externalFlags, T externalSolveHint, U& internalFlags)
|
||||
{
|
||||
if (externalFlags & Px1DConstraintFlag::eSPRING)
|
||||
internalFlags |= DY_SC_FLAG_SPRING;
|
||||
|
||||
if (externalFlags & Px1DConstraintFlag::eACCELERATION_SPRING)
|
||||
internalFlags |= DY_SC_FLAG_ACCELERATION_SPRING;
|
||||
|
||||
if (externalFlags & Px1DConstraintFlag::eOUTPUT_FORCE)
|
||||
internalFlags |= DY_SC_FLAG_OUTPUT_FORCE;
|
||||
|
||||
if (externalFlags & Px1DConstraintFlag::eKEEPBIAS)
|
||||
internalFlags |= DY_SC_FLAG_KEEP_BIAS;
|
||||
|
||||
if (externalSolveHint & 1)
|
||||
internalFlags |= DY_SC_FLAG_INEQUALITY;
|
||||
}
|
||||
|
||||
/**
|
||||
\brief Compute the amount of the geometric error that has been resolved.
|
||||
|
||||
A hard constraint tries to solve the equation:
|
||||
|
||||
(vT - vJ) + geomError/simDt = 0
|
||||
|
||||
With target velocity vT and vJ = jacobian * velocity
|
||||
|
||||
To satisfy the equation, the expected velocity is:
|
||||
|
||||
vJ = vT + geomError/simDt
|
||||
|
||||
The expected total motion p_n, thus is:
|
||||
|
||||
p_n = vJ * simDt = (vT*simDt) + geomError
|
||||
|
||||
During position iterations, the transforms get integrated and as such
|
||||
part of the geometric error gets resolved. As a consequence, at each
|
||||
iteration the amount of the resolved geometric error has to be evaluated
|
||||
to know how much error remains. Assuming the expected velocity was
|
||||
reached at the first iteration, the expected accumulated relative motion
|
||||
at iteration i (starting at 0) is:
|
||||
|
||||
p_i = vJ * (i*stepDt)
|
||||
= vT * (i*stepDt) + geomError * (i*stepDt)/simDt
|
||||
= vT * (i*stepDt) + geomError * (i/posIterationCount)
|
||||
|
||||
With stepDt being the time step of a TGS position iteration, i.e.,
|
||||
stepDt = simDt/posIterationCount
|
||||
|
||||
Note that we are currently using the following definition instead:
|
||||
|
||||
p_i = vT * (i*stepDt) + geomError
|
||||
|
||||
Splitting this into two components
|
||||
|
||||
pvt_i = vT * (i*stepDt) = vT * elapsedTime_i
|
||||
pge_i = geomError
|
||||
|
||||
We get:
|
||||
|
||||
p_i = pvt_i + pge_i
|
||||
|
||||
For various reasons, the solver might not reach the expected velocity
|
||||
immediately and as such the actual amount of motion (motion_i) can differ
|
||||
from the expected motion (p_i).
|
||||
|
||||
motion_i = pvt_i + resolvedGeomError_i
|
||||
resolvedGeomError_i = motion_i - pvt_i
|
||||
|
||||
This then allows to compute the remaining error as:
|
||||
|
||||
remainingGeomError_i = pge_i+1 - resolvedGeomError_i
|
||||
|
||||
For a soft constraint, the approach described above does not apply since
|
||||
the formulation of a soft constraints represents a spring force/impulse
|
||||
that should get applied during the sim step.
|
||||
|
||||
F = stiffness * -geomError + damping * (vT - vJ)
|
||||
|
||||
Unlike hard constraints, there is no clear goal to resolve the geometric
|
||||
error within a sim step. A spring without damping just keeps oscillating.
|
||||
Thus, there is no point in trying to distinguish what part of the motion
|
||||
resulted from resolving target velocity vs. resolving geometric error.
|
||||
Soft constraints should just take the current geometric error into account
|
||||
without bringing the target velocity into the mix.
|
||||
|
||||
\param[in] deltaLin0 The accumulated translational change of the constraint anchor point of body 0 since the beginning of the solver.
|
||||
\param[in] deltaLin1 See deltaLin0 but for body 1.
|
||||
\param[in] cLinVel0 The constraint axis (jacobian) for the linear velocity of body 0.
|
||||
\param[in] cLinVel1 See cLinVel0 but for body 1.
|
||||
\param[in] deltaAngInertia0 The accumulated rotational change of the constraint anchor point of body 0 since the beginning of the solver.
|
||||
Note: if momocity is used, this is not a pure delta angle but rather: Inertia^(1/2)*deltaAngle
|
||||
\param[in] deltaAngInertia1 See deltaAngInertia0 but for body 1.
|
||||
\param[in] cAngVelInertia0 The constraint axis (jacobian) for the angular velocity of body 0.
|
||||
Note: if momocity is used, this is not the pure constraint axis but rather: Inertia1^(-1/2)*cAngVel0
|
||||
\param[in] cAngVelInertia1 See cAngVelInertia0 but for body 1.
|
||||
\param[in] angularErrorScale Multiplier for the accumulated relative anchor motion from angular velocity.
|
||||
Can be set to 0 to ignore the angular part, for example.
|
||||
\param[in] flags The internal constraint flags (see SolverConstraintFlags).
|
||||
\param[in] springFlagMask The value DY_SC_FLAG_SPRING (as VecU32V for SIMD version, see U4Load(DY_SC_FLAG_SPRING))
|
||||
\param[in] targetVel The target velocity for the constraint.
|
||||
\param[in] elapsedTime The elapsed time since start of the solver (stepDt accumulated over solver iterations).
|
||||
\return The resolved geometric error.
|
||||
*/
|
||||
#if PX_CUDA_COMPILER
|
||||
__device__ PX_FORCE_INLINE PxReal computeResolvedGeometricErrorTGS(const PxVec3& deltaLin0, const PxVec3& deltaLin1,
|
||||
const PxVec3& cLinVel0, const PxVec3& cLinVel1,
|
||||
const PxVec3& deltaAngInertia0, const PxVec3& deltaAngInertia1,
|
||||
const PxVec3& cAngVelInertia0, const PxVec3& cAngVelInertia1,
|
||||
const PxReal angularErrorScale,
|
||||
const bool isSpringConstraint, const PxReal targetVel, const PxReal elapsedTime)
|
||||
{
|
||||
const PxReal deltaAng = (cAngVelInertia0.dot(deltaAngInertia0) - cAngVelInertia1.dot(deltaAngInertia1)) * angularErrorScale;
|
||||
const PxReal deltaLin = (cLinVel0.dot(deltaLin0) - cLinVel1.dot(deltaLin1));
|
||||
|
||||
const PxReal motion = deltaLin + deltaAng;
|
||||
|
||||
if (isSpringConstraint)
|
||||
{
|
||||
return motion;
|
||||
}
|
||||
else
|
||||
{
|
||||
const PxReal resolvedError = motion - (targetVel * elapsedTime);
|
||||
return resolvedError;
|
||||
}
|
||||
}
|
||||
#else
|
||||
PX_FORCE_INLINE aos::FloatV computeResolvedGeometricErrorTGS(const aos::Vec3VArg deltaLin0, const aos::Vec3VArg deltaLin1,
|
||||
const aos::Vec3VArg cLinVel0, const aos::Vec3VArg cLinVel1,
|
||||
const aos::Vec3VArg deltaAngInertia0, const aos::Vec3VArg deltaAngInertia1,
|
||||
const aos::Vec3VArg cAngVelInertia0, const aos::Vec3VArg cAngVelInertia1,
|
||||
const aos::FloatVArg angularErrorScale,
|
||||
const aos::BoolVArg isSpringConstraint, const aos::FloatVArg targetVel, const aos::FloatVArg elapsedTime)
|
||||
{
|
||||
// motion = cLinVel0.dot(deltaLin0) + cAngVelInertia0.dot(deltaAngInertia0)
|
||||
// - cLinVel1.dot(deltaLin1) - cAngVelInertia1.dot(deltaAngInertia1)
|
||||
//
|
||||
// = cLinVel0.dot(deltaLin0) + [Inertia0^(-1/2)*cAngVel0].dot[Inertia0^(1/2)*deltaAng0]
|
||||
// - cLinVel1.dot(deltaLin1) - [Inertia1^(-1/2)*cAngVel1].dot[Inertia1^(1/2)*deltaAng1]
|
||||
//
|
||||
// = cLinVel0.dot(deltaLin0) + cAngVel0.dot(deltaAng0)
|
||||
// - cLinVel1.dot(deltaLin1) - cAngVel1.dot(deltaAng1)
|
||||
|
||||
const aos::FloatV deltaAng = aos::FMul(angularErrorScale, aos::FSub(aos::V3Dot(cAngVelInertia0, deltaAngInertia0), aos::V3Dot(cAngVelInertia1, deltaAngInertia1)));
|
||||
const aos::FloatV deltaLin = aos::FSub(aos::V3Dot(cLinVel0, deltaLin0), aos::V3Dot(cLinVel1, deltaLin1));
|
||||
|
||||
const aos::FloatV motion = aos::FAdd(deltaLin, deltaAng);
|
||||
|
||||
const aos::FloatV resolvedError = aos::FSel(isSpringConstraint, motion, aos::FNegScaleSub(targetVel, elapsedTime, motion));
|
||||
return resolvedError;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE aos::Vec4V computeResolvedGeometricErrorTGSBlock(
|
||||
const aos::Vec4VArg deltaLin0X, const aos::Vec4VArg deltaLin0Y, const aos::Vec4VArg deltaLin0Z,
|
||||
const aos::Vec4VArg deltaLin1X, const aos::Vec4VArg deltaLin1Y, const aos::Vec4VArg deltaLin1Z,
|
||||
const aos::Vec4VArg cLinVel0X, const aos::Vec4VArg cLinVel0Y, const aos::Vec4VArg cLinVel0Z,
|
||||
const aos::Vec4VArg cLinVel1X, const aos::Vec4VArg cLinVel1Y, const aos::Vec4VArg cLinVel1Z,
|
||||
const aos::Vec4VArg deltaAngInertia0X, const aos::Vec4VArg deltaAngInertia0Y, const aos::Vec4VArg deltaAngInertia0Z,
|
||||
const aos::Vec4VArg deltaAngInertia1X, const aos::Vec4VArg deltaAngInertia1Y, const aos::Vec4VArg deltaAngInertia1Z,
|
||||
const aos::Vec4VArg cAngVelInertia0X, const aos::Vec4VArg cAngVelInertia0Y, const aos::Vec4VArg cAngVelInertia0Z,
|
||||
const aos::Vec4VArg cAngVelInertia1X, const aos::Vec4VArg cAngVelInertia1Y, const aos::Vec4VArg cAngVelInertia1Z,
|
||||
const aos::Vec4VArg angularErrorScale,
|
||||
const aos::BoolVArg isSpringConstraint, const aos::Vec4VArg targetVel, const aos::FloatVArg elapsedTime)
|
||||
{
|
||||
const aos::Vec4V deltaAng0 = aos::V4MulAdd(cAngVelInertia0X, deltaAngInertia0X, aos::V4MulAdd(cAngVelInertia0Y, deltaAngInertia0Y, aos::V4Mul(cAngVelInertia0Z, deltaAngInertia0Z)));
|
||||
const aos::Vec4V deltaAng1 = aos::V4MulAdd(cAngVelInertia1X, deltaAngInertia1X, aos::V4MulAdd(cAngVelInertia1Y, deltaAngInertia1Y, aos::V4Mul(cAngVelInertia1Z, deltaAngInertia1Z)));
|
||||
const aos::Vec4V deltaAng = aos::V4Mul(angularErrorScale, aos::V4Sub(deltaAng0, deltaAng1));
|
||||
|
||||
const aos::Vec4V deltaLin0 = aos::V4MulAdd(cLinVel0X, deltaLin0X, aos::V4MulAdd(cLinVel0Y, deltaLin0Y, aos::V4Mul(cLinVel0Z, deltaLin0Z)));
|
||||
const aos::Vec4V deltaLin1 = aos::V4MulAdd(cLinVel1X, deltaLin1X, aos::V4MulAdd(cLinVel1Y, deltaLin1Y, aos::V4Mul(cLinVel1Z, deltaLin1Z)));
|
||||
const aos::Vec4V deltaLin = aos::V4Sub(deltaLin0, deltaLin1);
|
||||
|
||||
const aos::Vec4V motion = aos::V4Add(deltaLin, deltaAng);
|
||||
|
||||
const aos::Vec4V resolvedError = aos::V4Sel(isSpringConstraint, motion, aos::V4NegScaleSub(targetVel, elapsedTime, motion));
|
||||
return resolvedError;
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
\brief Compute the minimal bias given internal 1D constraint flags.
|
||||
|
||||
\param[in] flags The internal 1D constraint flags (see SolverConstraintFlags).
|
||||
\param[in] maxBias The maximum bias to use. This has the form:
|
||||
-(geometricError / dt) * erp
|
||||
(erp = geom error based impulse multiplier, i.e, Baumgarte term)
|
||||
|
||||
\return The minimum bias.
|
||||
*/
|
||||
#if PX_CUDA_COMPILER
|
||||
__device__ PX_FORCE_INLINE PxReal computeMinBiasTGS(PxU32 flags, PxReal maxBias)
|
||||
{
|
||||
const PxReal minBias = -((flags & DY_SC_FLAG_INEQUALITY) ? PX_MAX_F32 : maxBias);
|
||||
return minBias;
|
||||
}
|
||||
#else
|
||||
PX_FORCE_INLINE aos::FloatV computeMinBiasTGS(PxU32 flags, const aos::FloatVArg maxBias)
|
||||
{
|
||||
const aos::FloatV minBias = aos::FNeg((flags & DY_SC_FLAG_INEQUALITY) ? aos::FMax() : maxBias);
|
||||
return minBias;
|
||||
}
|
||||
|
||||
/*
|
||||
\brief Compute the minimal bias given internal 1D constraint flags.
|
||||
|
||||
This version is for processing blocks of 4 constraints.
|
||||
|
||||
\param[in] flags See non-block version (but for a set of 4 constraints).
|
||||
\param[in] inequalityFlagMask DY_SC_FLAG_INEQUALITY loaded into an integer SIMD
|
||||
vector (U4Load(DY_SC_FLAG_INEQUALITY)).
|
||||
\param[in] maxBias See non-block version (but for a set of 4 constraints).
|
||||
|
||||
\return The minimum bias (for a set of 4 constraints).
|
||||
*/
|
||||
PX_FORCE_INLINE aos::Vec4V computeMinBiasTGSBlock(const aos::VecU32VArg flags, const aos::VecU32VArg inequalityFlagMask, const aos::Vec4VArg maxBias)
|
||||
{
|
||||
const aos::BoolV isInequalityConstraint = aos::V4IsEqU32(aos::V4U32and(flags, inequalityFlagMask), inequalityFlagMask);
|
||||
const aos::Vec4V minBias = aos::V4Neg(aos::V4Sel(isInequalityConstraint, aos::Vec4V_From_FloatV(aos::FMax()), maxBias));
|
||||
return minBias;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
} //namespace Dy
|
||||
} //namespace physx
|
||||
#endif //DY_1DCONSTRAINT_CPUGPU_H
|
||||
|
||||
731
engine/third_party/physx/source/lowleveldynamics/shared/DyCpuGpuArticulation.h
vendored
Normal file
731
engine/third_party/physx/source/lowleveldynamics/shared/DyCpuGpuArticulation.h
vendored
Normal file
@@ -0,0 +1,731 @@
|
||||
// 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-2025 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 DY_ARTICULATION_CPUGPU_H
|
||||
#define DY_ARTICULATION_CPUGPU_H
|
||||
|
||||
#include "foundation/PxSimpleTypes.h"
|
||||
#include "foundation/PxBasicTemplates.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
|
||||
#define DY_ARTICULATION_PGS_BIAS_COEFFICIENT 0.8f
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Dy
|
||||
{
|
||||
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;
|
||||
limited = true;
|
||||
if (currErrLow < tolerance)
|
||||
{
|
||||
if (!isVelIter)
|
||||
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
|
||||
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.
|
||||
|
||||
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;
|
||||
limited = true;
|
||||
if (currErrHigh < tolerance)
|
||||
{
|
||||
if (!isVelIter)
|
||||
newJointV = currErrHigh * recipDt * erp;
|
||||
}
|
||||
else
|
||||
newJointV = currErrHigh * recipDt;
|
||||
|
||||
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.
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
} //namespace Dy
|
||||
} //namespace physx
|
||||
#endif //DY_ARTICULATION_CPUGPU_H
|
||||
Reference in New Issue
Block a user