feat(physics): wire physx sdk into build
This commit is contained in:
720
engine/third_party/physx/source/physxvehicle/src/tire/VhTireFunctions.cpp
vendored
Normal file
720
engine/third_party/physx/source/physxvehicle/src/tire/VhTireFunctions.cpp
vendored
Normal file
@@ -0,0 +1,720 @@
|
||||
// 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.
|
||||
|
||||
#include "vehicle2/PxVehicleParams.h"
|
||||
|
||||
#include "vehicle2/drivetrain/PxVehicleDrivetrainStates.h"
|
||||
|
||||
#include "vehicle2/rigidBody/PxVehicleRigidBodyStates.h"
|
||||
|
||||
#include "vehicle2/roadGeometry/PxVehicleRoadGeometryState.h"
|
||||
|
||||
#include "vehicle2/suspension/PxVehicleSuspensionHelpers.h"
|
||||
|
||||
#include "vehicle2/tire/PxVehicleTireFunctions.h"
|
||||
#include "vehicle2/tire/PxVehicleTireParams.h"
|
||||
|
||||
#include "vehicle2/wheel/PxVehicleWheelHelpers.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace vehicle2
|
||||
{
|
||||
|
||||
PX_FORCE_INLINE PxF32 computeFilteredNormalisedTireLoad
|
||||
(const PxF32 xmin, const PxF32 ymin, const PxF32 xmax, const PxF32 ymax, const PxF32 x)
|
||||
{
|
||||
if (x <= xmin)
|
||||
{
|
||||
return ymin;
|
||||
}
|
||||
else if (x >= xmax)
|
||||
{
|
||||
return ymax;
|
||||
}
|
||||
else
|
||||
{
|
||||
return (ymin + (x - xmin)*(ymax - ymin)/ (xmax - xmin));
|
||||
}
|
||||
}
|
||||
|
||||
void PxVehicleTireDirsLegacyUpdate
|
||||
(const PxVehicleSuspensionParams& suspParams,
|
||||
const PxReal steerAngle, const PxVehicleRoadGeometryState& rdGeomState, const PxVehicleRigidBodyState& rigidBodyState,
|
||||
const PxVehicleFrame& frame,
|
||||
PxVehicleTireDirectionState& trSlipDirs)
|
||||
{
|
||||
trSlipDirs.setToDefault();
|
||||
|
||||
//If there are no hits we'll have no ground plane.
|
||||
if (!rdGeomState.hitState)
|
||||
return;
|
||||
|
||||
|
||||
//Compute wheel orientation with zero compliance, zero steer and zero
|
||||
//rotation. Ignore rotation because it plays no role due to radial
|
||||
//symmetry of wheel. Steer will be applied to the pose so we can
|
||||
//ignore when computing the orientation. Compliance ought to be applied but
|
||||
//we're not doing this in legacy code.
|
||||
const PxQuat wheelOrientation = PxVehicleComputeWheelOrientation(frame, suspParams, 0.0f, 0.0f, 0.0f,
|
||||
rigidBodyState.pose.q, 0.0f);
|
||||
|
||||
//We need lateral dir and hit norm to project wheel into ground plane.
|
||||
const PxVec3 latDir = wheelOrientation.rotate(frame.getLatAxis());
|
||||
const PxVec3& hitNorm = rdGeomState.plane.n;
|
||||
|
||||
//Compute the tire axes in the ground plane.
|
||||
PxVec3 tLongRaw = latDir.cross(hitNorm);
|
||||
PxVec3 tLatRaw = hitNorm.cross(tLongRaw);
|
||||
tLongRaw.normalize();
|
||||
tLatRaw.normalize();
|
||||
|
||||
//Rotate the tire using the steer angle.
|
||||
const PxF32 yawAngle = steerAngle;
|
||||
const PxF32 cosWheelSteer = PxCos(yawAngle);
|
||||
const PxF32 sinWheelSteer = PxSin(yawAngle);
|
||||
const PxVec3 tLong = tLongRaw * cosWheelSteer + tLatRaw * sinWheelSteer;
|
||||
const PxVec3 tLat = tLatRaw * cosWheelSteer - tLongRaw * sinWheelSteer;
|
||||
|
||||
trSlipDirs.directions[PxVehicleTireDirectionModes::eLATERAL] = tLat;
|
||||
trSlipDirs.directions[PxVehicleTireDirectionModes::eLONGITUDINAL] = tLong;
|
||||
}
|
||||
|
||||
void PxVehicleTireDirsUpdate
|
||||
(const PxVehicleSuspensionParams& suspParams,
|
||||
const PxReal steerAngle, const PxVec3& groundNormal, bool isWheelOnGround,
|
||||
const PxVehicleSuspensionComplianceState& compState,
|
||||
const PxVehicleRigidBodyState& rigidBodyState,
|
||||
const PxVehicleFrame& frame,
|
||||
PxVehicleTireDirectionState& trSlipDirs)
|
||||
{
|
||||
trSlipDirs.setToDefault();
|
||||
|
||||
//Skip if the suspension could not push the wheel to the ground.
|
||||
if (!isWheelOnGround)
|
||||
return;
|
||||
|
||||
//Compute the wheel quaternion in the world frame.
|
||||
//Ignore rotation because it plays no role due to radial symmetry of wheel.
|
||||
const PxQuat wheelOrientation = PxVehicleComputeWheelOrientation(frame, suspParams, compState.camber, compState.toe, steerAngle,
|
||||
rigidBodyState.pose.q, 0.0f);
|
||||
|
||||
//We need lateral dir and hit norm to project wheel into ground plane.
|
||||
const PxVec3 latDir = wheelOrientation.rotate(frame.getLatAxis());
|
||||
|
||||
//Compute the tire axes in the ground plane.
|
||||
PxVec3 lng = latDir.cross(groundNormal);
|
||||
PxVec3 lat = groundNormal.cross(lng);
|
||||
lng.normalize();
|
||||
lat.normalize();
|
||||
|
||||
//Set the direction vectors.
|
||||
trSlipDirs.directions[PxVehicleTireDirectionModes::eLATERAL] = lat;
|
||||
trSlipDirs.directions[PxVehicleTireDirectionModes::eLONGITUDINAL] = lng;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxF32 computeLateralSlip
|
||||
(const PxF32 lngSpeed, const PxF32 latSpeed, const PxF32 minLatSlipDenominator)
|
||||
{
|
||||
const PxF32 latSlip = PxAtan(latSpeed / (PxAbs(lngSpeed) + minLatSlipDenominator));
|
||||
return latSlip;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxF32 computeLongitudionalSlip
|
||||
(const bool useLegacyLongSlipCalculation,
|
||||
const PxF32 longSpeed,
|
||||
const PxF32 wheelOmega, const PxF32 wheelRadius,
|
||||
const PxF32 minPassiveLongSlipDenominator, const PxF32 minActiveLongSlipDenominator,
|
||||
const bool isAccelApplied, const bool isBrakeApplied)
|
||||
{
|
||||
const PxF32 longSpeedAbs = PxAbs(longSpeed);
|
||||
const PxF32 wheelSpeed = wheelOmega * wheelRadius;
|
||||
const PxF32 wheelSpeedAbs = PxAbs(wheelSpeed);
|
||||
|
||||
PxF32 lngSlip = 0.0f;
|
||||
|
||||
//If nothing is moving just avoid a divide by zero and set the long slip to zero.
|
||||
if (longSpeed == 0 && wheelOmega == 0)
|
||||
{
|
||||
lngSlip = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (isBrakeApplied || isAccelApplied)
|
||||
{
|
||||
//Wheel experiencing an applied torque.
|
||||
//Use the raw denominator value plus an offset to avoid anything approaching a divide by zero.
|
||||
//When accelerating from rest the small denominator will generate really quite large
|
||||
//slip values, which will, in turn, generate large longitudinal forces. With large
|
||||
//time-steps this might lead to a temporary oscillation in longSlip direction and an
|
||||
//oscillation in wheel speed direction. The amplitude of the oscillation should be low
|
||||
//unless the timestep is really large.
|
||||
//There's not really an obvious solution to this without setting the denominator offset higher
|
||||
//(or decreasing the timestep). Setting the denominator higher affects handling everywhere so
|
||||
//settling for a potential temporary oscillation is probably the least worst compromise.
|
||||
if (useLegacyLongSlipCalculation)
|
||||
{
|
||||
lngSlip = (wheelSpeed - longSpeed) / (PxMax(longSpeedAbs, wheelSpeedAbs) + minActiveLongSlipDenominator);
|
||||
}
|
||||
else
|
||||
{
|
||||
lngSlip = (wheelSpeed - longSpeed) / (longSpeedAbs + minActiveLongSlipDenominator);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//Wheel not experiencing an applied torque.
|
||||
//If the denominator becomes too small then the longSlip becomes large and the longitudinal force
|
||||
//can overshoot zero at large timesteps. This can be really noticeable so it's harder to justify
|
||||
//not taking action. Further, the car isn't being actually driven so there is a strong case to fiddle
|
||||
//with the denominator because it doesn't really affect active handling.
|
||||
//Don't let the denominator fall below a user-specified value. This can be tuned upwards until the
|
||||
//oscillation in the sign of longSlip disappears.
|
||||
if (useLegacyLongSlipCalculation)
|
||||
{
|
||||
lngSlip = (wheelSpeed - longSpeed) / (PxMax(minPassiveLongSlipDenominator, PxMax(longSpeedAbs, wheelSpeedAbs)));
|
||||
}
|
||||
else
|
||||
{
|
||||
lngSlip = (wheelSpeed - longSpeed) / (longSpeedAbs + minPassiveLongSlipDenominator);
|
||||
}
|
||||
}
|
||||
}
|
||||
return lngSlip;
|
||||
}
|
||||
|
||||
void PxVehicleTireSlipSpeedsUpdate
|
||||
(const PxVehicleWheelParams& whlParams, const PxVehicleSuspensionParams& suspParams,
|
||||
const PxF32 steerAngle, const PxVehicleSuspensionState& suspStates, const PxVehicleTireDirectionState& trSlipDirs,
|
||||
const PxVehicleRigidBodyState& rigidBodyState, const PxVehicleRoadGeometryState& roadGeometryState,
|
||||
const PxVehicleFrame& frame,
|
||||
PxVehicleTireSpeedState& trSpeedState)
|
||||
{
|
||||
trSpeedState.setToDefault();
|
||||
|
||||
//Compute the position of the bottom of the wheel (placed on the ground plane).
|
||||
PxVec3 wheelBottomPos;
|
||||
{
|
||||
PxVec3 v,w;
|
||||
PxF32 dist;
|
||||
PxVehicleComputeSuspensionRaycast(frame, whlParams, suspParams, steerAngle, rigidBodyState.pose, v, w, dist);
|
||||
wheelBottomPos = v + w*(dist - suspStates.jounce);
|
||||
}
|
||||
|
||||
//Compute the rigid body velocity at the bottom of the wheel (placed on the ground plane).
|
||||
PxVec3 wheelBottomVel;
|
||||
{
|
||||
const PxVec3 r = wheelBottomPos - rigidBodyState.pose.p;
|
||||
wheelBottomVel = rigidBodyState.linearVelocity + rigidBodyState.angularVelocity.cross(r) - roadGeometryState.velocity;
|
||||
}
|
||||
|
||||
//Comput the velocities along lateral and longitudinal tire directions.
|
||||
trSpeedState.speedStates[PxVehicleTireDirectionModes::eLONGITUDINAL] = wheelBottomVel.dot(trSlipDirs.directions[PxVehicleTireDirectionModes::eLONGITUDINAL]);
|
||||
trSpeedState.speedStates[PxVehicleTireDirectionModes::eLATERAL] = wheelBottomVel.dot(trSlipDirs.directions[PxVehicleTireDirectionModes::eLATERAL]);
|
||||
}
|
||||
|
||||
|
||||
void vehicleTireSlipsUpdate
|
||||
(const PxVehicleWheelParams& whlParams,
|
||||
const PxVehicleTireSlipParams& trSlipParams, const bool useLegacyLongSlipCalculation,
|
||||
const PxVehicleWheelActuationState& actState, PxVehicleTireSpeedState& trSpeedState, const PxVehicleWheelRigidBody1dState& whlRigidBody1dState,
|
||||
PxVehicleTireSlipState& trSlipState)
|
||||
{
|
||||
trSlipState.setToDefault();
|
||||
|
||||
PxF32 latSlip = 0.0f;
|
||||
{
|
||||
const PxF32 lngSpeed = trSpeedState.speedStates[PxVehicleTireDirectionModes::eLONGITUDINAL];
|
||||
const PxF32 latSpeed = trSpeedState.speedStates[PxVehicleTireDirectionModes::eLATERAL];
|
||||
const PxF32 minLatSlipDenominator = trSlipParams.minLatSlipDenominator;
|
||||
latSlip = computeLateralSlip(lngSpeed, latSpeed, minLatSlipDenominator);
|
||||
}
|
||||
|
||||
PxF32 lngSlip = 0.0f;
|
||||
{
|
||||
const PxF32 lngSpeed = trSpeedState.speedStates[PxVehicleTireDirectionModes::eLONGITUDINAL];
|
||||
const PxF32 wheelRotSpeed = whlRigidBody1dState.rotationSpeed;
|
||||
const PxF32 wheelRadius = whlParams.radius;
|
||||
const PxF32 minPassiveLngSlipDenominator = trSlipParams.minPassiveLongSlipDenominator;
|
||||
const PxF32 minActiveLngSlipDenominator = trSlipParams.minActiveLongSlipDenominator;
|
||||
const bool isBrakeApplied = actState.isBrakeApplied;
|
||||
const bool isAccelApplied = actState.isDriveApplied;
|
||||
lngSlip = computeLongitudionalSlip(
|
||||
useLegacyLongSlipCalculation,
|
||||
lngSpeed, wheelRotSpeed, wheelRadius,
|
||||
minPassiveLngSlipDenominator, minActiveLngSlipDenominator,
|
||||
isAccelApplied, isBrakeApplied);
|
||||
}
|
||||
|
||||
trSlipState.slips[PxVehicleTireDirectionModes::eLATERAL] = latSlip;
|
||||
trSlipState.slips[PxVehicleTireDirectionModes::eLONGITUDINAL] = lngSlip;
|
||||
}
|
||||
|
||||
void PxVehicleTireSlipsUpdate
|
||||
(const PxVehicleWheelParams& whlParams,
|
||||
const PxVehicleTireSlipParams& trSlipParams,
|
||||
const PxVehicleWheelActuationState& actState, PxVehicleTireSpeedState& trSpeedState, const PxVehicleWheelRigidBody1dState& whlRigidBody1dState,
|
||||
PxVehicleTireSlipState& trSlipState)
|
||||
{
|
||||
vehicleTireSlipsUpdate(
|
||||
whlParams, trSlipParams, false,
|
||||
actState, trSpeedState, whlRigidBody1dState,
|
||||
trSlipState);
|
||||
}
|
||||
|
||||
void PxVehicleTireSlipsLegacyUpdate
|
||||
(const PxVehicleWheelParams& whlParams,
|
||||
const PxVehicleTireSlipParams& trSlipParams,
|
||||
const PxVehicleWheelActuationState& actState, PxVehicleTireSpeedState& trSpeedState, const PxVehicleWheelRigidBody1dState& whlRigidBody1dState,
|
||||
PxVehicleTireSlipState& trSlipState)
|
||||
{
|
||||
vehicleTireSlipsUpdate(
|
||||
whlParams, trSlipParams, true,
|
||||
actState, trSpeedState, whlRigidBody1dState,
|
||||
trSlipState);
|
||||
|
||||
}
|
||||
|
||||
void PxVehicleTireCamberAnglesUpdate
|
||||
(const PxVehicleSuspensionParams& suspParams,
|
||||
const PxReal steerAngle, const PxVec3& groundNormal, bool isWheelOnGround,
|
||||
const PxVehicleSuspensionComplianceState& compState,
|
||||
const PxVehicleRigidBodyState& rigidBodyState,
|
||||
const PxVehicleFrame& frame,
|
||||
PxVehicleTireCamberAngleState& trCamberAngleState)
|
||||
{
|
||||
trCamberAngleState.setToDefault();
|
||||
|
||||
//Use zero camber if the suspension could not push the wheel to the ground.
|
||||
if (!isWheelOnGround)
|
||||
return;
|
||||
|
||||
//Compute the wheel quaternion in the world frame.
|
||||
//Ignore rotation due to radial symmetry.
|
||||
const PxQuat wheelOrientation = PxVehicleComputeWheelOrientation(frame, suspParams, compState.camber, compState.toe, steerAngle,
|
||||
rigidBodyState.pose.q, 0.0f);
|
||||
|
||||
//Compute the axes of the wheel.
|
||||
const PxVec3 latDir = wheelOrientation.rotate(frame.getLatAxis());
|
||||
const PxVec3 lngDir = wheelOrientation.rotate(frame.getLngAxis());
|
||||
|
||||
//Project normal into lateral/vertical plane.
|
||||
//Start with:
|
||||
//n = lat*alpha + lng*beta + vrt*delta
|
||||
//Want to work out
|
||||
//T = n - lng*beta
|
||||
// = n - lng*(n.dot(lng))
|
||||
//Don't forget to normalise T.
|
||||
//For the angle theta to look for we have:
|
||||
//T.vrtDir = cos(theta)
|
||||
//However, the cosine destroys the sign of the angle, thus we
|
||||
//use:
|
||||
//T.latDir = cos(pi/2 - theta) = sin(theta) (latDir and vrtDir are perpendicular)
|
||||
const PxF32 beta = groundNormal.dot(lngDir);
|
||||
PxVec3 T = groundNormal - lngDir * beta;
|
||||
T.normalize();
|
||||
const PxF32 sinTheta = T.dot(latDir);
|
||||
const PxF32 theta = PxAsin(sinTheta);
|
||||
trCamberAngleState.camberAngle = theta;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxF32 updateLowLngSpeedTimer
|
||||
(const PxF32 lngSpeed, const PxF32 wheelOmega, const PxF32 wheelRadius, const PxF32 lngThresholdSpeed,
|
||||
const bool isIntentionToAccelerate, const PxF32 dt, const PxF32 lowLngSpeedTime)
|
||||
{
|
||||
//If the tire is rotating slowly and the longitudinal speed is slow then increment the slow longitudinal speed timer.
|
||||
//If the intention of the driver is to accelerate the vehicle then reset the timer because the intention has been signalled NOT to bring
|
||||
//the wheel to rest.
|
||||
PxF32 newLowSpeedTime = 0.0f;
|
||||
if ((PxAbs(lngSpeed) < lngThresholdSpeed) && (PxAbs(wheelOmega*wheelRadius) < lngThresholdSpeed) && !isIntentionToAccelerate)
|
||||
{
|
||||
newLowSpeedTime = lowLngSpeedTime + dt;
|
||||
}
|
||||
else
|
||||
{
|
||||
newLowSpeedTime = 0;
|
||||
}
|
||||
return newLowSpeedTime;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void activateStickyFrictionLngConstraint
|
||||
(const PxF32 longSpeed, const PxF32 wheelOmega, const PxF32 lowLngSpeedTime, const bool isIntentionToBrake,
|
||||
const PxF32 thresholdSpeed, const PxF32 thresholdTime,
|
||||
bool& stickyTireActiveFlag)
|
||||
{
|
||||
//Setup the sticky friction constraint to bring the vehicle to rest at the tire contact point.
|
||||
//The idea here is to resolve the singularity of the tire long slip at low vz by replacing the long force with a velocity constraint.
|
||||
//Only do this if we can guarantee that the intention is to bring the car to rest (no accel pedal applied).
|
||||
//We're going to replace the longitudinal tire force with the sticky friction so set the long slip to zero to ensure zero long force.
|
||||
//Apply sticky friction to this tire if
|
||||
//(1) the wheel is locked (this means the brake/handbrake must be on) and the longitudinal speed at the tire contact point is vanishingly small.
|
||||
//(2) the accumulated time of low longitudinal speed is greater than a threshold.
|
||||
stickyTireActiveFlag = false;
|
||||
if (((PxAbs(longSpeed) < thresholdSpeed) && (0.0f == wheelOmega) && isIntentionToBrake) || (lowLngSpeedTime > thresholdTime))
|
||||
{
|
||||
stickyTireActiveFlag = true;
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxF32 updateLowLatSpeedTimer
|
||||
(const PxF32 latSpeed, const bool isIntentionToAccelerate, const PxF32 timestep, const PxF32 thresholdSpeed, const PxF32 lowSpeedTime)
|
||||
{
|
||||
//If the lateral speed is slow then increment the slow lateral speed timer.
|
||||
//If the intention of the driver is to accelerate the vehicle then reset the timer because the intention has been signalled NOT to bring
|
||||
//the wheel to rest.
|
||||
PxF32 newLowSpeedTime = lowSpeedTime;
|
||||
if ((PxAbs(latSpeed) < thresholdSpeed) && !isIntentionToAccelerate)
|
||||
{
|
||||
newLowSpeedTime += timestep;
|
||||
}
|
||||
else
|
||||
{
|
||||
newLowSpeedTime = 0;
|
||||
}
|
||||
return newLowSpeedTime;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void activateStickyFrictionLatConstraint
|
||||
(const bool lowSpeedLngTimerActive,
|
||||
const PxF32 lowLatSpeedTimer, const PxF32 thresholdTime,
|
||||
bool& stickyTireActiveFlag)
|
||||
{
|
||||
//Setup the sticky friction constraint to bring the vehicle to rest at the tire contact point.
|
||||
//Only do this if we can guarantee that the intention is to bring the car to rest (no accel pedal applied).
|
||||
//We're going to replace the lateral tire force with the sticky friction so set the lat slip to zero to ensure zero lat force.
|
||||
//Apply sticky friction to this tire if
|
||||
//(1) the low longitudinal speed timer is > 0.
|
||||
//(2) the accumulated time of low longitudinal speed is greater than a threshold.
|
||||
stickyTireActiveFlag = false;
|
||||
if (lowSpeedLngTimerActive && (lowLatSpeedTimer > thresholdTime))
|
||||
{
|
||||
stickyTireActiveFlag = true;
|
||||
}
|
||||
}
|
||||
|
||||
void PxVehicleTireStickyStateUpdate
|
||||
(const PxVehicleAxleDescription& axleDescription, const PxVehicleWheelParams& whlParams,
|
||||
const PxVehicleTireStickyParams& trStickyParams,
|
||||
const PxVehicleArrayData<const PxVehicleWheelActuationState>& actuationStates,
|
||||
const PxVehicleTireGripState& trGripState, const PxVehicleTireSpeedState& trSpeedState, const PxVehicleWheelRigidBody1dState& whlState,
|
||||
const PxReal dt,
|
||||
PxVehicleTireStickyState& trStickyState)
|
||||
{
|
||||
trStickyState.activeStatus[PxVehicleTireDirectionModes::eLONGITUDINAL] = false;
|
||||
trStickyState.activeStatus[PxVehicleTireDirectionModes::eLATERAL] = false;
|
||||
|
||||
//Only process sticky state if tire can generate force.
|
||||
const PxF32 load = trGripState.load;
|
||||
const PxF32 friction = trGripState.friction;
|
||||
if(0 == load*friction)
|
||||
{
|
||||
trStickyState.lowSpeedTime[PxVehicleTireDirectionModes::eLONGITUDINAL] = 0.0f;
|
||||
trStickyState.lowSpeedTime[PxVehicleTireDirectionModes::eLATERAL] = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
//Work out if any wheel is to have a drive applied to it.
|
||||
bool isIntentionToAccelerate = false;
|
||||
bool isIntentionToBrake = false;
|
||||
for(PxU32 i = 0; i < axleDescription.nbWheels; i++)
|
||||
{
|
||||
const PxU32 wheelId = axleDescription.wheelIdsInAxleOrder[i];
|
||||
if(actuationStates[wheelId].isDriveApplied)
|
||||
isIntentionToAccelerate = true;
|
||||
if (actuationStates[wheelId].isBrakeApplied)
|
||||
isIntentionToBrake = true;
|
||||
}
|
||||
|
||||
//Long sticky state.
|
||||
bool lngTimerActive = false;
|
||||
{
|
||||
const PxF32 lngSpeed = trSpeedState.speedStates[PxVehicleTireDirectionModes::eLONGITUDINAL];
|
||||
const PxF32 wheelOmega = whlState.rotationSpeed;
|
||||
const PxF32 wheelRadius = whlParams.radius;
|
||||
const PxF32 lngThresholdSpeed = trStickyParams.stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL].thresholdSpeed;
|
||||
const PxF32 lngLowSpeedTime = trStickyState.lowSpeedTime[PxVehicleTireDirectionModes::eLONGITUDINAL];
|
||||
|
||||
const PxF32 newLowLngSpeedTime = updateLowLngSpeedTimer(
|
||||
lngSpeed, wheelOmega, wheelRadius, lngThresholdSpeed,
|
||||
isIntentionToAccelerate,
|
||||
dt, lngLowSpeedTime);
|
||||
lngTimerActive = (newLowLngSpeedTime > 0);
|
||||
|
||||
bool lngActiveState = false;
|
||||
const PxF32 lngThresholdTime = trStickyParams.stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL].thresholdTime;
|
||||
activateStickyFrictionLngConstraint(
|
||||
lngSpeed, wheelOmega, newLowLngSpeedTime, isIntentionToBrake,
|
||||
lngThresholdSpeed, lngThresholdTime,
|
||||
lngActiveState);
|
||||
|
||||
trStickyState.lowSpeedTime[PxVehicleTireDirectionModes::eLONGITUDINAL] = newLowLngSpeedTime;
|
||||
trStickyState.activeStatus[PxVehicleTireDirectionModes::eLONGITUDINAL] = lngActiveState;
|
||||
}
|
||||
|
||||
//Lateral sticky state
|
||||
{
|
||||
const PxF32 latSpeed = trSpeedState.speedStates[PxVehicleTireDirectionModes::eLATERAL];
|
||||
const PxF32 latThresholdSpeed = trStickyParams.stickyParams[PxVehicleTireDirectionModes::eLATERAL].thresholdSpeed;
|
||||
const PxF32 latLowSpeedTime = trStickyState.lowSpeedTime[PxVehicleTireDirectionModes::eLATERAL];
|
||||
const PxF32 latNewLowSpeedTime = updateLowLatSpeedTimer(latSpeed, isIntentionToAccelerate, dt, latThresholdSpeed, latLowSpeedTime);
|
||||
|
||||
bool latActiveState = false;
|
||||
const PxF32 latThresholdTime = trStickyParams.stickyParams[PxVehicleTireDirectionModes::eLATERAL].thresholdTime;
|
||||
activateStickyFrictionLatConstraint(lngTimerActive, latNewLowSpeedTime, latThresholdTime,
|
||||
latActiveState);
|
||||
|
||||
trStickyState.lowSpeedTime[PxVehicleTireDirectionModes::eLATERAL] = latNewLowSpeedTime;
|
||||
trStickyState.activeStatus[PxVehicleTireDirectionModes::eLATERAL] = latActiveState;
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxF32 computeTireLoad
|
||||
(const PxVehicleTireForceParams& trForceParams, const PxVehicleSuspensionForce& suspForce)
|
||||
{
|
||||
//Compute the normalised load.
|
||||
const PxF32 rawLoad = suspForce.normalForce;
|
||||
const PxF32 restLoad = trForceParams.restLoad;
|
||||
const PxF32 normalisedLoad = rawLoad / restLoad;
|
||||
|
||||
//Get the load filter params.
|
||||
const PxF32 minNormalisedLoad = trForceParams.loadFilter[0][0];
|
||||
const PxF32 minFilteredNormalisedLoad = trForceParams.loadFilter[0][1];
|
||||
const PxF32 maxNormalisedLoad = trForceParams.loadFilter[1][0];
|
||||
const PxF32 maxFilteredNormalisedLoad = trForceParams.loadFilter[1][1];
|
||||
|
||||
//Compute the filtered load.
|
||||
const PxF32 filteredNormalisedLoad = computeFilteredNormalisedTireLoad(minNormalisedLoad, minFilteredNormalisedLoad, maxNormalisedLoad, maxFilteredNormalisedLoad, normalisedLoad);
|
||||
const PxF32 filteredLoad = restLoad * filteredNormalisedLoad;
|
||||
|
||||
//Set the load after applying the filter.
|
||||
return filteredLoad;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxF32 computeTireFriction
|
||||
(const PxVehicleTireForceParams& trForceParams,
|
||||
const PxReal frictionCoefficient, const PxVehicleTireSlipState& tireSlipState)
|
||||
{
|
||||
//Interpolate the friction using the long slip value.
|
||||
const PxF32 x0 = trForceParams.frictionVsSlip[0][0];
|
||||
const PxF32 y0 = trForceParams.frictionVsSlip[0][1];
|
||||
const PxF32 x1 = trForceParams.frictionVsSlip[1][0];
|
||||
const PxF32 y1 = trForceParams.frictionVsSlip[1][1];
|
||||
const PxF32 x2 = trForceParams.frictionVsSlip[2][0];
|
||||
const PxF32 y2 = trForceParams.frictionVsSlip[2][1];
|
||||
const PxF32 longSlipAbs = PxAbs(tireSlipState.slips[PxVehicleTireDirectionModes::eLONGITUDINAL]);
|
||||
PxF32 mu;
|
||||
if (longSlipAbs < x1)
|
||||
{
|
||||
mu = y0 + (y1 - y0)*(longSlipAbs - x0) / (x1 - x0);
|
||||
}
|
||||
else if (longSlipAbs < x2)
|
||||
{
|
||||
mu = y1 + (y2 - y1)*(longSlipAbs - x1) / (x2 - x1);
|
||||
}
|
||||
else
|
||||
{
|
||||
mu = y2;
|
||||
}
|
||||
PX_ASSERT(mu >= 0);
|
||||
|
||||
const PxF32 tireFriction = frictionCoefficient * mu;
|
||||
return tireFriction;
|
||||
}
|
||||
|
||||
void PxVehicleTireGripUpdate
|
||||
(const PxVehicleTireForceParams& trForceParams,
|
||||
const PxReal frictionCoefficient, bool isWheelOnGround, const PxVehicleSuspensionForce& suspForce,
|
||||
const PxVehicleTireSlipState& trSlipState,
|
||||
PxVehicleTireGripState& trGripState)
|
||||
{
|
||||
trGripState.setToDefault();
|
||||
|
||||
//If the wheel is not touching the ground then carry on with zero grip state.
|
||||
if (!isWheelOnGround)
|
||||
return;
|
||||
|
||||
//Compute load and friction.
|
||||
trGripState.load = computeTireLoad(trForceParams, suspForce);
|
||||
trGripState.friction = computeTireFriction(trForceParams, frictionCoefficient, trSlipState);
|
||||
}
|
||||
|
||||
void PxVehicleTireSlipsAccountingForStickyStatesUpdate
|
||||
(const PxVehicleTireStickyState& trStickyState,
|
||||
PxVehicleTireSlipState& trSlipState)
|
||||
{
|
||||
if(trStickyState.activeStatus[PxVehicleTireDirectionModes::eLATERAL])
|
||||
trSlipState.slips[PxVehicleTireDirectionModes::eLATERAL] = 0.f;
|
||||
if (trStickyState.activeStatus[PxVehicleTireDirectionModes::eLONGITUDINAL])
|
||||
trSlipState.slips[PxVehicleTireDirectionModes::eLONGITUDINAL] = 0.f;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
//Default tire force shader function.
|
||||
//Taken from Michigan tire model.
|
||||
//Computes tire long and lat forces plus the aligning moment arising from
|
||||
//the lat force and the torque to apply back to the wheel arising from the
|
||||
//long force (application of Newton's 3rd law).
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
#define ONE_TWENTYSEVENTH 0.037037f
|
||||
#define ONE_THIRD 0.33333f
|
||||
PX_FORCE_INLINE PxF32 smoothingFunction1(const PxF32 K)
|
||||
{
|
||||
//Equation 20 in CarSimEd manual Appendix F.
|
||||
//Looks a bit like a curve of sqrt(x) for 0<x<1 but reaching 1.0 on y-axis at K=3.
|
||||
PX_ASSERT(K >= 0.0f);
|
||||
return PxMin(1.0f, K - ONE_THIRD * K*K + ONE_TWENTYSEVENTH * K*K*K);
|
||||
}
|
||||
PX_FORCE_INLINE PxF32 smoothingFunction2(const PxF32 K)
|
||||
{
|
||||
//Equation 21 in CarSimEd manual Appendix F.
|
||||
//Rises to a peak at K=0.75 and falls back to zero by K=3
|
||||
PX_ASSERT(K >= 0.0f);
|
||||
return (K - K * K + ONE_THIRD * K*K*K - ONE_TWENTYSEVENTH * K*K*K*K);
|
||||
}
|
||||
|
||||
void computeTireForceMichiganModel
|
||||
(const PxVehicleTireForceParams& tireData,
|
||||
const PxF32 tireFriction,
|
||||
const PxF32 longSlipUnClamped, const PxF32 latSlipUnClamped, const PxF32 camberUnclamped,
|
||||
const PxF32 wheelRadius,
|
||||
const PxF32 tireLoad,
|
||||
PxF32& wheelTorque, PxF32& tireLongForceMag, PxF32& tireLatForceMag, PxF32& tireAlignMoment)
|
||||
{
|
||||
PX_ASSERT(tireFriction > 0);
|
||||
PX_ASSERT(tireLoad > 0);
|
||||
|
||||
wheelTorque = 0.0f;
|
||||
tireLongForceMag = 0.0f;
|
||||
tireLatForceMag = 0.0f;
|
||||
tireAlignMoment = 0.0f;
|
||||
|
||||
//Clamp the slips to a minimum value.
|
||||
const PxF32 minimumSlipThreshold = 1e-5f;
|
||||
const PxF32 latSlip = PxAbs(latSlipUnClamped) >= minimumSlipThreshold ? latSlipUnClamped : 0.0f;
|
||||
const PxF32 longSlip = PxAbs(longSlipUnClamped) >= minimumSlipThreshold ? longSlipUnClamped : 0.0f;
|
||||
const PxF32 camber = PxAbs(camberUnclamped) >= minimumSlipThreshold ? camberUnclamped : 0.0f;
|
||||
|
||||
//Normalise the tire load.
|
||||
const PxF32 restTireLoad = tireData.restLoad;
|
||||
const PxF32 normalisedTireLoad = tireLoad/ restTireLoad;
|
||||
|
||||
//Compute the lateral stiffness
|
||||
const PxF32 latStiff = (0.0f == tireData.latStiffX) ? tireData.latStiffY : tireData.latStiffY*smoothingFunction1(normalisedTireLoad*3.0f / tireData.latStiffX);
|
||||
|
||||
//Get the longitudinal stiffness
|
||||
const PxF32 longStiff = tireData.longStiff;
|
||||
|
||||
//Get the camber stiffness.
|
||||
const PxF32 camberStiff = tireData.camberStiff;
|
||||
|
||||
//If long slip/lat slip/camber are all zero than there will be zero tire force.
|
||||
if ((0 == latSlip*latStiff) && (0 == longSlip*longStiff) && (0 == camber*camberStiff))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
//Carry on and compute the forces.
|
||||
const PxF32 TEff = PxTan(latSlip + (camber * camberStiff / latStiff)); // "+" because we define camber stiffness as a positive value
|
||||
const PxF32 K = PxSqrt(latStiff*TEff*latStiff*TEff + longStiff * longSlip*longStiff*longSlip) / (tireFriction*tireLoad);
|
||||
//const PxF32 KAbs=PxAbs(K);
|
||||
PxF32 FBar = smoothingFunction1(K);//K - ONE_THIRD*PxAbs(K)*K + ONE_TWENTYSEVENTH*K*K*K;
|
||||
PxF32 MBar = smoothingFunction2(K); //K - KAbs*K + ONE_THIRD*K*K*K - ONE_TWENTYSEVENTH*KAbs*K*K*K;
|
||||
//Mbar = PxMin(Mbar, 1.0f);
|
||||
PxF32 nu = 1;
|
||||
if (K <= 2.0f*PxPi)
|
||||
{
|
||||
const PxF32 latOverlLong = latStiff/longStiff;
|
||||
nu = 0.5f*(1.0f + latOverlLong - (1.0f - latOverlLong)*PxCos(K*0.5f));
|
||||
}
|
||||
const PxF32 FZero = tireFriction * tireLoad / (PxSqrt(longSlip*longSlip + nu * TEff*nu*TEff));
|
||||
const PxF32 fz = longSlip * FBar*FZero;
|
||||
const PxF32 fx = -nu * TEff*FBar*FZero;
|
||||
//TODO: pneumatic trail.
|
||||
const PxF32 pneumaticTrail = 1.0f;
|
||||
const PxF32 fMy = nu * pneumaticTrail * TEff * MBar * FZero;
|
||||
|
||||
//We can add the torque to the wheel.
|
||||
wheelTorque = -fz * wheelRadius;
|
||||
tireLongForceMag = fz;
|
||||
tireLatForceMag = fx;
|
||||
tireAlignMoment = fMy;
|
||||
}
|
||||
|
||||
void PxVehicleTireForcesUpdate
|
||||
(const PxVehicleWheelParams& whlParams, const PxVehicleSuspensionParams& suspParams,
|
||||
const PxVehicleTireForceParams& trForceParams,
|
||||
const PxVehicleSuspensionComplianceState& compState,
|
||||
const PxVehicleTireGripState& trGripState, const PxVehicleTireDirectionState& trDirectionState,
|
||||
const PxVehicleTireSlipState& trSlipState, const PxVehicleTireCamberAngleState& cmbAngleState,
|
||||
const PxVehicleRigidBodyState& rigidBodyState,
|
||||
PxVehicleTireForce& trForce)
|
||||
{
|
||||
trForce.setToDefault();
|
||||
|
||||
//If the tire can generate no force then carry on with zero force.
|
||||
if(0 == trGripState.friction*trGripState.load)
|
||||
return;
|
||||
|
||||
PxF32 wheelTorque = 0;
|
||||
PxF32 tireLongForceMag = 0;
|
||||
PxF32 tireLatForceMag = 0;
|
||||
PxF32 tireAlignMoment = 0;
|
||||
|
||||
const PxF32 friction = trGripState.friction;
|
||||
const PxF32 tireLoad = trGripState.load;
|
||||
const PxF32 lngSlip = trSlipState.slips[PxVehicleTireDirectionModes::eLONGITUDINAL];
|
||||
const PxF32 latSlip = trSlipState.slips[PxVehicleTireDirectionModes::eLATERAL];
|
||||
const PxF32 camber = cmbAngleState.camberAngle;
|
||||
const PxF32 wheelRadius = whlParams.radius;
|
||||
computeTireForceMichiganModel(trForceParams, friction, lngSlip, latSlip, camber, wheelRadius, tireLoad,
|
||||
wheelTorque, tireLongForceMag, tireLatForceMag, tireAlignMoment);
|
||||
|
||||
//Compute the forces.
|
||||
const PxVec3 tireLongForce = trDirectionState.directions[PxVehicleTireDirectionModes::eLONGITUDINAL]*tireLongForceMag;
|
||||
const PxVec3 tireLatForce = trDirectionState.directions[PxVehicleTireDirectionModes::eLATERAL]*tireLatForceMag;
|
||||
|
||||
//Compute the torques.
|
||||
const PxVec3 r = rigidBodyState.pose.rotate(suspParams.suspensionAttachment.transform(compState.tireForceAppPoint));
|
||||
const PxVec3 tireLongTorque = r.cross(tireLongForce);
|
||||
const PxVec3 tireLatTorque = r.cross(tireLatForce);
|
||||
|
||||
//Set the torques.
|
||||
trForce.forces[PxVehicleTireDirectionModes::eLONGITUDINAL] = tireLongForce;
|
||||
trForce.torques[PxVehicleTireDirectionModes::eLONGITUDINAL] = tireLongTorque;
|
||||
trForce.forces[PxVehicleTireDirectionModes::eLATERAL] = tireLatForce;
|
||||
trForce.torques[PxVehicleTireDirectionModes::eLATERAL] = tireLatTorque;
|
||||
trForce.aligningMoment = tireAlignMoment;
|
||||
trForce.wheelTorque = wheelTorque;
|
||||
}
|
||||
|
||||
} //namespace vehicle2
|
||||
} //namespace physx
|
||||
|
||||
65
engine/third_party/physx/source/physxvehicle/src/tire/VhTireHelpers.cpp
vendored
Normal file
65
engine/third_party/physx/source/physxvehicle/src/tire/VhTireHelpers.cpp
vendored
Normal file
@@ -0,0 +1,65 @@
|
||||
// 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.
|
||||
|
||||
#include "vehicle2/tire/PxVehicleTireHelpers.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace vehicle2
|
||||
{
|
||||
|
||||
bool PxVehicleAccelerationIntentCompute
|
||||
(const PxVehicleAxleDescription& driveVehicleAxleDesc, const PxVehicleArrayData<const PxVehicleWheelActuationState>& driveVehicleActuationStates)
|
||||
{
|
||||
bool isIntentionToAccelerate = false;
|
||||
for (PxU32 i = 0; i < driveVehicleAxleDesc.nbWheels; i++)
|
||||
{
|
||||
const PxU32 wheelId = driveVehicleAxleDesc.wheelIdsInAxleOrder[i];
|
||||
isIntentionToAccelerate = !isIntentionToAccelerate ? driveVehicleActuationStates[wheelId].isDriveApplied : isIntentionToAccelerate;
|
||||
}
|
||||
return isIntentionToAccelerate;
|
||||
}
|
||||
|
||||
void PxVehicleTireStickyStateReset
|
||||
(const bool intentionToAccelerate,
|
||||
const PxVehicleAxleDescription& towedVehicleAxleDesc,
|
||||
PxVehicleArrayData<PxVehicleTireStickyState>& towedVehicleStickyState)
|
||||
{
|
||||
if (!intentionToAccelerate)
|
||||
return;
|
||||
|
||||
for (PxU32 i = 0; i < towedVehicleAxleDesc.nbWheels; i++)
|
||||
{
|
||||
const PxU32 wheelId = towedVehicleAxleDesc.wheelIdsInAxleOrder[i];
|
||||
towedVehicleStickyState[wheelId].setToDefault();
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace vehicle2
|
||||
} //namespace physx
|
||||
|
||||
Reference in New Issue
Block a user