835 lines
37 KiB
C++
835 lines
37 KiB
C++
// 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
|
|
|