A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.
TypeScript 2d-game-engine3d3d-game-engineandroidcppecsgame-editorgame-enginegamedeviostypescriptwebassemblywebglwebgpuwechat-minigame
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0
#include "vehicle/PxVehicleParams.h"
#include "vehicle/PxVehicleFunctions.h"
#include "vehicle/PxVehicleMaths.h"
#include "vehicle/drivetrain/PxVehicleDrivetrainHelpers.h"
#include "vehicle/drivetrain/PxVehicleDrivetrainParams.h"
#include "vehicle/wheel/PxVehicleWheelStates.h"
namespace physx
{
void PxVehicleMatrixNNLUSolver::decomposeLU(const PxVehicleMatrixNN& A)
{
const PxU32 D = A.mSize;
mLU = A;
mDetM = 1.0f;
for (PxU32 k = 0; k < D - 1; ++k)
{
PxU32 pivot_row = k;
PxU32 pivot_col = k;
float abs_pivot_elem = 0.0f;
for (PxU32 c = k; c < D; ++c)
{
for (PxU32 r = k; r < D; ++r)
{
const PxF32 abs_elem = PxAbs(mLU.get(r, c));
if (abs_elem > abs_pivot_elem)
{
abs_pivot_elem = abs_elem;
pivot_row = r;
pivot_col = c;
}
}
}
mP[k] = pivot_row;
if (pivot_row != k)
{
mDetM = -mDetM;
for (PxU32 c = 0; c < D; ++c)
{
//swap(m_LU(k,c), m_LU(pivot_row,c));
const PxF32 pivotrowc = mLU.get(pivot_row, c);
mLU.set(pivot_row, c, mLU.get(k, c));
mLU.set(k, c, pivotrowc);
}
}
mQ[k] = pivot_col;
if (pivot_col != k)
{
mDetM = -mDetM;
for (PxU32 r = 0; r < D; ++r)
{
//swap(m_LU(r,k), m_LU(r,pivot_col));
const PxF32 rpivotcol = mLU.get(r, pivot_col);
mLU.set(r, pivot_col, mLU.get(r, k));
mLU.set(r, k, rpivotcol);
}
}
mDetM *= mLU.get(k, k);
if (mLU.get(k, k) != 0.0f)
{
for (PxU32 r = k + 1; r < D; ++r)
{
mLU.set(r, k, mLU.get(r, k) / mLU.get(k, k));
for (PxU32 c = k + 1; c < D; ++c)
{
//m_LU(r,c) -= m_LU(r,k)*m_LU(k,c);
const PxF32 rc = mLU.get(r, c);
const PxF32 rk = mLU.get(r, k);
const PxF32 kc = mLU.get(k, c);
mLU.set(r, c, rc - rk * kc);
}
}
}
}
mDetM *= mLU.get(D - 1, D - 1);
}
bool PxVehicleMatrixNNLUSolver::solve(const PxVehicleVectorN& b, PxVehicleVectorN& x) const
{
const PxU32 D = x.getSize();
if ((b.getSize() != x.getSize()) || (b.getSize() != mLU.getSize()) || (0.0f == mDetM))
{
for (PxU32 i = 0; i < D; i++)
{
x[i] = 0.0f;
}
return false;
}
x = b;
// Perform row permutation to get Pb
for (PxU32 i = 0; i < D - 1; ++i)
{
//swap(x(i), x(m_P[i]));
const PxF32 xp = x[mP[i]];
x[mP[i]] = x[i];
x[i] = xp;
}
// Forward substitute to get (L^-1)Pb
for (PxU32 r = 1; r < D; ++r)
{
for (PxU32 i = 0; i < r; ++i)
{
x[r] -= mLU.get(r, i)*x[i];
}
}
// Back substitute to get (U^-1)(L^-1)Pb
for (PxU32 r = D; r-- > 0;)
{
for (PxU32 i = r + 1; i < D; ++i)
{
x[r] -= mLU.get(r, i)*x[i];
}
x[r] /= mLU.get(r, r);
}
// Perform column permutation to get the solution (Q^T)(U^-1)(L^-1)Pb
for (PxU32 i = D - 1; i-- > 0;)
{
//swap(x(i), x(m_Q[i]));
const PxF32 xq = x[mQ[i]];
x[mQ[i]] = x[i];
x[i] = xq;
}
return true;
}
void PxVehicleMatrixNGaussSeidelSolver::solve(const PxU32 maxIterations, const PxF32 tolerance, const PxVehicleMatrixNN& A, const PxVehicleVectorN& b, PxVehicleVectorN& result) const
{
const PxU32 N = A.getSize();
PxVehicleVectorN DInv(N);
PxF32 bLength2 = 0.0f;
for (PxU32 i = 0; i < N; i++)
{
DInv[i] = 1.0f / A.get(i, i);
bLength2 += (b[i] * b[i]);
}
PxU32 iteration = 0;
PxF32 error = PX_MAX_F32;
while (iteration < maxIterations && tolerance < error)
{
for (PxU32 i = 0; i < N; i++)
{
PxF32 l = 0.0f;
for (PxU32 j = 0; j < i; j++)
{
l += A.get(i, j) * result[j];
}
PxF32 u = 0.0f;
for (PxU32 j = i + 1; j < N; j++)
{
u += A.get(i, j) * result[j];
}
result[i] = DInv[i] * (b[i] - l - u);
}
//Compute the error.
PxF32 rLength2 = 0;
for (PxU32 i = 0; i < N; i++)
{
PxF32 e = -b[i];
for (PxU32 j = 0; j < N; j++)
{
e += A.get(i, j) * result[j];
}
rLength2 += e * e;
}
error = (rLength2 / (bLength2 + 1e-10f));
iteration++;
}
}
bool PxVehicleMatrix33Solver::solve(const PxVehicleMatrixNN& A_, const PxVehicleVectorN& b_, PxVehicleVectorN& result) const
{
const PxF32 a = A_.get(0, 0);
const PxF32 b = A_.get(0, 1);
const PxF32 c = A_.get(0, 2);
const PxF32 d = A_.get(1, 0);
const PxF32 e = A_.get(1, 1);
const PxF32 f = A_.get(1, 2);
const PxF32 g = A_.get(2, 0);
const PxF32 h = A_.get(2, 1);
const PxF32 k = A_.get(2, 2);
const PxF32 detA = a * (e*k - f * h) - b * (k*d - f * g) + c * (d*h - e * g);
if (0.0f == detA)
{
return false;
}
const PxF32 detAInv = 1.0f / detA;
const PxF32 A = (e*k - f * h);
const PxF32 D = -(b*k - c * h);
const PxF32 G = (b*f - c * e);
const PxF32 B = -(d*k - f * g);
const PxF32 E = (a*k - c * g);
const PxF32 H = -(a*f - c * d);
const PxF32 C = (d*h - e * g);
const PxF32 F = -(a*h - b * g);
const PxF32 K = (a*e - b * d);
result[0] = detAInv * (A*b_[0] + D * b_[1] + G * b_[2]);
result[1] = detAInv * (B*b_[0] + E * b_[1] + H * b_[2]);
result[2] = detAInv * (C*b_[0] + F * b_[1] + K * b_[2]);
return true;
}
void PxVehicleLegacyDifferentialWheelSpeedContributionsCompute
(const PxVehicleFourWheelDriveDifferentialLegacyParams& diffParams,
const PxU32 nbWheels, PxReal* diffAveWheelSpeedContributions)
{
PxMemZero(diffAveWheelSpeedContributions, sizeof(PxReal) * nbWheels);
const PxU32 wheelIds[4] =
{
diffParams.frontWheelIds[0],
diffParams.frontWheelIds[1],
diffParams.rearWheelIds[0],
diffParams.rearWheelIds[1],
};
const PxF32 frontRearSplit = diffParams.frontRearSplit;
const PxF32 frontNegPosSplit = diffParams.frontNegPosSplit;
const PxF32 rearNegPosSplit = diffParams.rearNegPosSplit;
const PxF32 oneMinusFrontRearSplit = 1.0f - diffParams.frontRearSplit;
const PxF32 oneMinusFrontNegPosSplit = 1.0f - diffParams.frontNegPosSplit;
const PxF32 oneMinusRearNegPosSplit = 1.0f - diffParams.rearNegPosSplit;
switch (diffParams.type)
{
case PxVehicleFourWheelDriveDifferentialLegacyParams::eDIFF_TYPE_LS_4WD:
diffAveWheelSpeedContributions[wheelIds[0]] = frontRearSplit * frontNegPosSplit;
diffAveWheelSpeedContributions[wheelIds[1]] = frontRearSplit * oneMinusFrontNegPosSplit;
diffAveWheelSpeedContributions[wheelIds[2]] = oneMinusFrontRearSplit * rearNegPosSplit;
diffAveWheelSpeedContributions[wheelIds[3]] = oneMinusFrontRearSplit * oneMinusRearNegPosSplit;
break;
case PxVehicleFourWheelDriveDifferentialLegacyParams::eDIFF_TYPE_LS_FRONTWD:
diffAveWheelSpeedContributions[wheelIds[0]] = frontNegPosSplit;
diffAveWheelSpeedContributions[wheelIds[1]] = oneMinusFrontNegPosSplit;
diffAveWheelSpeedContributions[wheelIds[2]] = 0.0f;
diffAveWheelSpeedContributions[wheelIds[3]] = 0.0f;
break;
case PxVehicleFourWheelDriveDifferentialLegacyParams::eDIFF_TYPE_LS_REARWD:
diffAveWheelSpeedContributions[wheelIds[0]] = 0.0f;
diffAveWheelSpeedContributions[wheelIds[1]] = 0.0f;
diffAveWheelSpeedContributions[wheelIds[2]] = rearNegPosSplit;
diffAveWheelSpeedContributions[wheelIds[3]] = oneMinusRearNegPosSplit;
break;
default:
PX_ASSERT(false);
break;
}
PX_ASSERT(
((diffAveWheelSpeedContributions[wheelIds[0]] + diffAveWheelSpeedContributions[wheelIds[1]] + diffAveWheelSpeedContributions[wheelIds[2]] + diffAveWheelSpeedContributions[wheelIds[3]]) >= 0.999f) &&
((diffAveWheelSpeedContributions[wheelIds[0]] + diffAveWheelSpeedContributions[wheelIds[1]] + diffAveWheelSpeedContributions[wheelIds[2]] + diffAveWheelSpeedContributions[wheelIds[3]]) <= 1.001f));
}
PX_FORCE_INLINE void splitTorque
(const PxF32 w1, const PxF32 w2, const PxF32 diffBias, const PxF32 defaultSplitRatio,
PxF32* t1, PxF32* t2)
{
PX_ASSERT(PxVehicleComputeSign(w1) == PxVehicleComputeSign(w2) && 0.0f != PxVehicleComputeSign(w1));
const PxF32 w1Abs = PxAbs(w1);
const PxF32 w2Abs = PxAbs(w2);
const PxF32 omegaMax = PxMax(w1Abs, w2Abs);
const PxF32 omegaMin = PxMin(w1Abs, w2Abs);
const PxF32 delta = omegaMax - diffBias * omegaMin;
const PxF32 deltaTorque = physx::intrinsics::fsel(delta, delta / omegaMax, 0.0f);
const PxF32 f1 = physx::intrinsics::fsel(w1Abs - w2Abs, defaultSplitRatio*(1.0f - deltaTorque), defaultSplitRatio*(1.0f + deltaTorque));
const PxF32 f2 = physx::intrinsics::fsel(w1Abs - w2Abs, (1.0f - defaultSplitRatio)*(1.0f + deltaTorque), (1.0f - defaultSplitRatio)*(1.0f - deltaTorque));
const PxF32 denom = 1.0f / (f1 + f2);
*t1 = f1 * denom;
*t2 = f2 * denom;
PX_ASSERT((*t1 + *t2) >= 0.999f && (*t1 + *t2) <= 1.001f);
}
void PxVehicleLegacyDifferentialTorqueRatiosCompute
(const PxVehicleFourWheelDriveDifferentialLegacyParams& diffParams,
const PxVehicleArrayData<const PxVehicleWheelRigidBody1dState>& wheelOmegas,
const PxU32 nbWheels, PxReal* diffTorqueRatios)
{
PxMemZero(diffTorqueRatios, sizeof(PxReal) * nbWheels);
const PxU32 wheelIds[4] =
{
diffParams.frontWheelIds[0],
diffParams.frontWheelIds[1],
diffParams.rearWheelIds[0],
diffParams.rearWheelIds[1],
};
const PxF32 wfl = wheelOmegas[wheelIds[0]].rotationSpeed;
const PxF32 wfr = wheelOmegas[wheelIds[1]].rotationSpeed;
const PxF32 wrl = wheelOmegas[wheelIds[2]].rotationSpeed;
const PxF32 wrr = wheelOmegas[wheelIds[3]].rotationSpeed;
const PxF32 centreBias = diffParams.centerBias;
const PxF32 frontBias = diffParams.frontBias;
const PxF32 rearBias = diffParams.rearBias;
const PxF32 frontRearSplit = diffParams.frontRearSplit;
const PxF32 frontLeftRightSplit = diffParams.frontNegPosSplit;
const PxF32 rearLeftRightSplit = diffParams.rearNegPosSplit;
const PxF32 oneMinusFrontRearSplit = 1.0f - diffParams.frontRearSplit;
const PxF32 oneMinusFrontLeftRightSplit = 1.0f - diffParams.frontNegPosSplit;
const PxF32 oneMinusRearLeftRightSplit = 1.0f - diffParams.rearNegPosSplit;
const PxF32 swfl = PxVehicleComputeSign(wfl);
//Split a torque of 1 between front and rear.
//Then split that torque between left and right.
PxF32 torqueFrontLeft = 0;
PxF32 torqueFrontRight = 0;
PxF32 torqueRearLeft = 0;
PxF32 torqueRearRight = 0;
switch (diffParams.type)
{
case PxVehicleFourWheelDriveDifferentialLegacyParams::eDIFF_TYPE_LS_4WD:
if (0.0f != swfl && swfl == PxVehicleComputeSign(wfr) && swfl == PxVehicleComputeSign(wrl) && swfl == PxVehicleComputeSign(wrr))
{
PxF32 torqueFront, torqueRear;
const PxF32 omegaFront = PxAbs(wfl + wfr);
const PxF32 omegaRear = PxAbs(wrl + wrr);
splitTorque(omegaFront, omegaRear, centreBias, frontRearSplit, &torqueFront, &torqueRear);
splitTorque(wfl, wfr, frontBias, frontLeftRightSplit, &torqueFrontLeft, &torqueFrontRight);
splitTorque(wrl, wrr, rearBias, rearLeftRightSplit, &torqueRearLeft, &torqueRearRight);
torqueFrontLeft *= torqueFront;
torqueFrontRight *= torqueFront;
torqueRearLeft *= torqueRear;
torqueRearRight *= torqueRear;
}
else
{
torqueFrontLeft = frontRearSplit * frontLeftRightSplit;
torqueFrontRight = frontRearSplit * oneMinusFrontLeftRightSplit;
torqueRearLeft = oneMinusFrontRearSplit * rearLeftRightSplit;
torqueRearRight = oneMinusFrontRearSplit * oneMinusRearLeftRightSplit;
}
break;
case PxVehicleFourWheelDriveDifferentialLegacyParams::eDIFF_TYPE_LS_FRONTWD:
if (0.0f != swfl && swfl == PxVehicleComputeSign(wfr))
{
splitTorque(wfl, wfr, frontBias, frontLeftRightSplit, &torqueFrontLeft, &torqueFrontRight);
}
else
{
torqueFrontLeft = frontLeftRightSplit;
torqueFrontRight = oneMinusFrontLeftRightSplit;
}
break;
case PxVehicleFourWheelDriveDifferentialLegacyParams::eDIFF_TYPE_LS_REARWD:
if (0.0f != PxVehicleComputeSign(wrl) && PxVehicleComputeSign(wrl) == PxVehicleComputeSign(wrr))
{
splitTorque(wrl, wrr, rearBias, rearLeftRightSplit, &torqueRearLeft, &torqueRearRight);
}
else
{
torqueRearLeft = rearLeftRightSplit;
torqueRearRight = oneMinusRearLeftRightSplit;
}
break;
default:
PX_ASSERT(false);
break;
}
diffTorqueRatios[wheelIds[0]] = torqueFrontLeft;
diffTorqueRatios[wheelIds[1]] = torqueFrontRight;
diffTorqueRatios[wheelIds[2]] = torqueRearLeft;
diffTorqueRatios[wheelIds[3]] = torqueRearRight;
PX_ASSERT(((torqueFrontLeft + torqueFrontRight + torqueRearLeft + torqueRearRight) >= 0.999f) && ((torqueFrontLeft + torqueFrontRight + torqueRearLeft + torqueRearRight) <= 1.001f));
}
} //namespace physx