Skip to content

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.

37.7 KB
Raw Blame
// 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 "foundation/PxMemory.h"

#include "vehicle/PxVehicleParams.h"
#include "vehicle/PxVehicleFunctions.h"
#include "vehicle/PxVehicleMaths.h"

#include "vehicle/commands/PxVehicleCommandHelpers.h"

#include "vehicle/drivetrain/PxVehicleDrivetrainFunctions.h"
#include "vehicle/drivetrain/PxVehicleDrivetrainParams.h"
#include "vehicle/drivetrain/PxVehicleDrivetrainStates.h"
#include "vehicle/drivetrain/PxVehicleDrivetrainHelpers.h"

#include "vehicle/tire/PxVehicleTireStates.h"

#include "vehicle/wheel/PxVehicleWheelParams.h"
#include "vehicle/wheel/PxVehicleWheelStates.h"

namespace physx
{

void PxVehicleDirectDriveThrottleCommandResponseUpdate
(const PxReal throttle, const PxVehicleDirectDriveTransmissionCommandState& transmissionCommands, const PxReal longitudinalSpeed,
 const PxU32 wheelId, const PxVehicleDirectDriveThrottleCommandResponseParams& responseParams,
 PxReal& throttleResponse)
{
	//The gearing decides how we will multiply the response.
	PxF32 gearMultiplier = 0.0f;
	switch (transmissionCommands.gear)
	{
	case PxVehicleDirectDriveTransmissionCommandState::eREVERSE:
		gearMultiplier = -1.0f;
		break;
	case PxVehicleDirectDriveTransmissionCommandState::eNEUTRAL:
		gearMultiplier = 0.0f;
		break;
	case PxVehicleDirectDriveTransmissionCommandState::eFORWARD:
		gearMultiplier = 1.0f;
		break;
	}
	throttleResponse = gearMultiplier * PxVehicleNonLinearResponseCompute(throttle, longitudinalSpeed, wheelId, responseParams);
}

void PxVehicleDirectDriveActuationStateUpdate
(const PxReal brakeTorque, const PxReal driveTorque, 
 PxVehicleWheelActuationState& actState)
{
	PxMemZero(&actState, sizeof(PxVehicleWheelActuationState));
	actState.isBrakeApplied = (brakeTorque != 0.0f);
	actState.isDriveApplied = (driveTorque != 0.0f);
}

void PxVehicleDirectDriveUpdate
(const PxVehicleWheelParams& whlParams,
 const PxVehicleWheelActuationState& actState, 
 const PxReal brkTorque, const PxReal drvTorque, const PxVehicleTireForce& trForce,
 const PxF32 dt, 
 PxVehicleWheelRigidBody1dState& whlState)
{

	//w(t+dt) = w(t) + (1/inertia)*(brakeTorque + driveTorque + tireTorque)*dt - (1/inertia)*damping*w(t)*dt )	(1)
	//Apply implicit trick and rearrange.
	//w(t+dt)[1 + (1/inertia)*damping*dt] = w(t) + (1/inertia)*(brakeTorque + driveTorque + tireTorque)*dt		(2)
	const PxF32 wheelRotSpeed = whlState.rotationSpeed;
	const PxF32 dtOverMOI = dt/whlParams.moi;
	const PxF32 tireTorque = trForce.wheelTorque;
	const PxF32 brakeTorque = -brkTorque*PxVehicleComputeSign(wheelRotSpeed);
	const PxF32 driveTorque = drvTorque;
	const PxF32 wheelDampingRate = whlParams.dampingRate;

	//Integrate the wheel rotation speed.
	const PxF32 newRotSpeedNoBrakelock = (wheelRotSpeed + dtOverMOI*(tireTorque + driveTorque + brakeTorque)) / (1.0f + wheelDampingRate*dtOverMOI);
	//If the brake is applied and the sign flipped then lock the brake.
	const bool isBrakeApplied = actState.isBrakeApplied;
	const PxF32 newRotSpeedWithBrakelock = (isBrakeApplied && ((wheelRotSpeed*newRotSpeedNoBrakelock) <= 0)) ? 0.0f : newRotSpeedNoBrakelock;

	whlState.rotationSpeed = newRotSpeedWithBrakelock;
}

void PxVehicleDifferentialStateUpdate
(const PxVehicleFourWheelDriveDifferentialLegacyParams& diffParams,
 const PxVehicleArrayData<const PxVehicleWheelRigidBody1dState>& wheelStates,
 PxVehicleDifferentialState& diffState)
{
	diffState.setToDefault();

	//4 wheels are connected.
	diffState.connectedWheels[0] = diffParams.frontWheelIds[0];
	diffState.connectedWheels[1] = diffParams.frontWheelIds[1];
	diffState.connectedWheels[2] = diffParams.rearWheelIds[0];
	diffState.connectedWheels[3] = diffParams.rearWheelIds[1];
	diffState.nbConnectedWheels = 4;

	//Compute the contributions to average speed and ratios of available torque.
	PxVehicleLegacyDifferentialWheelSpeedContributionsCompute(diffParams, PxVehicleLimits::eMAX_NB_WHEELS,
		diffState.aveWheelSpeedContributionAllWheels);

	PxVehicleLegacyDifferentialTorqueRatiosCompute(diffParams, wheelStates, PxVehicleLimits::eMAX_NB_WHEELS,
		diffState.torqueRatiosAllWheels);
}

PX_FORCE_INLINE PxF32 computeTargetRatio
(const PxF32 target, const PxF32 dt, const PxF32 strength, const PxF32 ratio)
{
	const PxF32 targ = (PX_VEHICLE_FOUR_WHEEL_DIFFERENTIAL_MAXIMUM_STRENGTH == strength) ? target : PxMax(target, ratio - strength * dt);
	return targ;
}

void PxVehicleDifferentialStateUpdate
(const PxVehicleAxleDescription& axleDescription, const PxVehicleFourWheelDriveDifferentialParams& diffParams,
 const PxVehicleArrayData<const PxVehicleWheelRigidBody1dState>& wheelStates,
 const PxReal dt,
 PxVehicleDifferentialState& diffState, PxVehicleWheelConstraintGroupState& wheelConstraintGroupState)
{
	diffState.setToDefault();
	wheelConstraintGroupState.setToDefault();

	PxU32 nbConnectedWheels = 0;
	for (PxU32 i = 0; i < axleDescription.nbWheels; i++)
	{
		const PxU32 wheelId = axleDescription.wheelIdsInAxleOrder[i];
		if (diffParams.torqueRatios[wheelId] != 0.0f)
		{
			diffState.connectedWheels[nbConnectedWheels] = wheelId;
			diffState.aveWheelSpeedContributionAllWheels[wheelId] = diffParams.aveWheelSpeedRatios[wheelId];
			diffState.torqueRatiosAllWheels[wheelId] = diffParams.aveWheelSpeedRatios[wheelId];
			nbConnectedWheels++;
		}
	}
	diffState.nbConnectedWheels = nbConnectedWheels;

	if (0.0f == diffParams.frontBias && 0.0f == diffParams.rearBias && 0.0f == diffParams.centerBias)
		return;

	if (0.0f == diffParams.rate)
		return;

	bool frontBiasBreached = false;
	PxF32 Rf = 0.0f;
	PxF32 Tf = 0.0f;
	{
		const PxF32 bias = diffParams.frontBias;
		if (bias >= 1.0f)
		{
			const PxU32 wheel0 = diffParams.frontWheelIds[0];
			const PxU32 wheel1 = diffParams.frontWheelIds[1];
			const PxReal w0 = wheelStates[wheel0].rotationSpeed;
			const PxReal w1 = wheelStates[wheel1].rotationSpeed;
			const PxReal aw0 = PxAbs(w0);
			const PxReal aw1 = PxAbs(w1);
			const PxReal sw0 = PxVehicleComputeSign(w0);
			const PxReal sw1 = PxVehicleComputeSign(w1);
			if ((w0 != 0.0f) && (sw0 == sw1))
			{
				const PxF32 ratio = PxMax(aw0, aw1) / PxMin(aw0, aw1);
				frontBiasBreached = (ratio > bias);
				//const PxF32 target = diffParams.frontTarget + (1.0f - diffParams.strength) * (ratio - diffParams.frontTarget);
				const PxF32 target = computeTargetRatio(diffParams.frontTarget, dt, diffParams.rate, ratio);
				Tf = (aw0 > aw1) ? 1.0f / target : target;
				Rf = aw1 / aw0;
			}
		}
	}

	bool rearBiasBreached = false;
	PxF32 Rr = 0.0f;
	PxF32 Tr = 0.0f;
	{
		const PxF32 bias = diffParams.rearBias;
		if (bias >= 1.0f)
		{
			const PxU32 wheel2 = diffParams.rearWheelIds[0];
			const PxU32 wheel3 = diffParams.rearWheelIds[1];
			const PxReal w2 = wheelStates[wheel2].rotationSpeed;
			const PxReal w3 = wheelStates[wheel3].rotationSpeed;
			const PxReal aw2 = PxAbs(w2);
			const PxReal aw3 = PxAbs(w3);
			const PxReal sw2 = PxVehicleComputeSign(w2);
			const PxReal sw3 = PxVehicleComputeSign(w3);
			if ((w2 != 0.0f) && (sw2 == sw3))
			{
				const PxF32 ratio = PxMax(aw2, aw3) / PxMin(aw2, aw3);
				rearBiasBreached = (ratio > bias);
				//const PxF32 target = diffParams.rearTarget + (1.0f - diffParams.strength) * (ratio - diffParams.rearTarget);
				const PxF32 target = computeTargetRatio(diffParams.rearTarget, dt, diffParams.rate, ratio);
				Tr = (aw2 > aw3) ? 1.0f / target : target;
				Rr = aw3 / aw2;
			}
		}
	}

	bool centreBiasBrached = false;
	//PxF32 Rc = 0.0f;
	PxF32 Tc = 0.0f;
	{
		const PxF32 bias = diffParams.centerBias;
		if(bias >= 1.0f)
		{
			const PxU32 wheel0 = diffParams.frontWheelIds[0];
			const PxU32 wheel1 = diffParams.frontWheelIds[1];
			const PxU32 wheel2 = diffParams.rearWheelIds[0];
			const PxU32 wheel3 = diffParams.rearWheelIds[1];
			const PxReal w0 = wheelStates[wheel0].rotationSpeed;
			const PxReal w1 = wheelStates[wheel1].rotationSpeed;
			const PxReal w2 = wheelStates[wheel2].rotationSpeed;
			const PxReal w3 = wheelStates[wheel3].rotationSpeed;
			const PxReal aw0 = PxAbs(w0);
			const PxReal aw1 = PxAbs(w1);
			const PxReal aw2 = PxAbs(w2);
			const PxReal aw3 = PxAbs(w3);
			const PxReal sw0 = PxVehicleComputeSign(w0);
			const PxReal sw1 = PxVehicleComputeSign(w1);
			const PxReal sw2 = PxVehicleComputeSign(w2);
			const PxReal sw3 = PxVehicleComputeSign(w3);
			if ((w0 != 0.0f) && (sw0 == sw1) && (sw0 == sw2) && (sw0 == sw3))
			{
				const PxF32 ratio = PxMax(aw0 + aw1, aw2 + aw3) / PxMin(aw0 + aw1, aw2 + aw3);
				centreBiasBrached = (ratio > bias);
				//const PxF32 target = diffParams.centerTarget + (1.0f - diffParams.strength) * (ratio - diffParams.centerTarget);
				const PxF32 target = computeTargetRatio(diffParams.centerTarget, dt, diffParams.rate, ratio);
				Tc = ((aw0 + aw1) > (aw2 + aw3)) ? 1.0f / target : target;
				//Rc = (aw2 + aw3) / (aw0 + aw1);
			}
		}
	}

	if (centreBiasBrached)
	{
		PxF32 f = 0.0f;
		PxF32 r = 0.0f;
		if (frontBiasBreached && rearBiasBreached)
		{
			f = Tf;
			r = Tr;
		}
		else if (frontBiasBreached)
		{
			f = Tf;
			r = Rr;
		}
		else if (rearBiasBreached)
		{
			f = Rf;
			r = Tr;
		}
		else
		{
			f = Rf;
			r = Rr;
		}

		const PxU32  wheel0 = diffParams.frontWheelIds[0];
		const PxU32  wheel1 = diffParams.frontWheelIds[1];
		const PxU32  wheel2 = diffParams.rearWheelIds[0];
		const PxU32  wheel3 = diffParams.rearWheelIds[1];
		const PxU32 wheelIds[4] = { wheel0, wheel1, wheel2,  wheel3 };
		const PxF32 constraintMultipliers[4] = { 1.0f, f, Tc*(1 + f) / (1 + r), r*Tc*(1 + f) / (1 + r) };
		wheelConstraintGroupState.addConstraintGroup(4, wheelIds, constraintMultipliers);
	}
	else
	{
		if (frontBiasBreached)
		{
			const PxU32* wheelIds = diffParams.frontWheelIds;
			const PxF32 constraintMultipliers[2] = { 1.0f, Tf };
			wheelConstraintGroupState.addConstraintGroup(2, wheelIds, constraintMultipliers);
		}

		if (rearBiasBreached)
		{
			const PxU32* wheelIds = diffParams.rearWheelIds;
			const PxF32 constraintMultipliers[2] = { 1.0f, Tr };
			wheelConstraintGroupState.addConstraintGroup(2, wheelIds, constraintMultipliers);
		}
	}
}

void PxVehicleDifferentialStateUpdate
(const PxVehicleAxleDescription& axleDescription, const PxVehicleMultiWheelDriveDifferentialParams& diffParams,
 PxVehicleDifferentialState& diffState)
{
	diffState.setToDefault();

	PxU32 nbConnectedWheels = 0;
	for (PxU32 i = 0; i < axleDescription.nbWheels; i++)
	{
		const PxU32 wheelId = axleDescription.wheelIdsInAxleOrder[i];
		if (diffParams.torqueRatios[wheelId] != 0.0f)
		{
			diffState.connectedWheels[nbConnectedWheels] = wheelId;
			diffState.aveWheelSpeedContributionAllWheels[wheelId] = diffParams.aveWheelSpeedRatios[wheelId];
			diffState.torqueRatiosAllWheels[wheelId] = diffParams.aveWheelSpeedRatios[wheelId];
			nbConnectedWheels++;
		}
	}
	diffState.nbConnectedWheels = nbConnectedWheels;
}

void PxVehicleDifferentialStateUpdate
(const PxVehicleAxleDescription& axleDescription,
 const PxVehicleArrayData<const PxVehicleWheelParams>& wheelParams, const PxVehicleTankDriveDifferentialParams& diffParams,
 const PxReal thrust0, PxReal thrust1,
 PxVehicleDifferentialState& diffState, PxVehicleWheelConstraintGroupState& wheelConstraintGroupState)
{
	diffState.setToDefault();
	wheelConstraintGroupState.setToDefault();

	//Store the tank track id for each wheel.
	//Store the thrust controller id for each wheel.
	//Store 0xffffffff for wheels not in a tank track.
	PxU32 tankTrackIds[PxVehicleLimits::eMAX_NB_WHEELS];
	PxMemSet(tankTrackIds, 0xff, sizeof(tankTrackIds));
	PxU32 thrustControllerIds[PxVehicleLimits::eMAX_NB_WHEELS];
	PxMemSet(thrustControllerIds, 0xff, sizeof(thrustControllerIds));
	for (PxU32 i = 0; i < diffParams.getNbTracks(); i++)
	{
		const PxU32 thrustControllerIndex = diffParams.getThrustControllerIndex(i);
		for (PxU32 j = 0; j < diffParams.getNbWheelsInTrack(i); j++)
		{
			const PxU32 wheelId = diffParams.getWheelInTrack(j, i);
			tankTrackIds[wheelId] = i;
			thrustControllerIds[wheelId] = thrustControllerIndex;
		}
	}

	//Treat every wheel connected to a tank track as connected to the differential but with zero torque split.
	//If a wheel is not connected to the differential but is connected to a tank track then we treat that as a connected wheel with 
	//zero drive torque applied.
	//We do this because we need to treat these wheels as being coupled to other wheels in the linear equations that solve engine+wheels
	PxU32 nbConnectedWheels = 0;
	const PxReal thrusts[2] = { thrust0, thrust1 };
	for (PxU32 i = 0; i < axleDescription.nbWheels; i++)
	{
		const PxU32 wheelId = axleDescription.wheelIdsInAxleOrder[i];
		const PxU32 tankTrackId = tankTrackIds[wheelId];
		if (diffParams.torqueRatios[wheelId] != 0.0f && 0xffffffff != tankTrackId)
		{
			//Wheel connected to diff and in a tank track.
			//Modify the default torque split using the relevant thrust.
			const PxU32 thrustId = thrustControllerIds[wheelId];
			const PxF32 torqueSplitMultiplier = thrusts[thrustId];
			diffState.aveWheelSpeedContributionAllWheels[wheelId] = diffParams.aveWheelSpeedRatios[wheelId];
			diffState.torqueRatiosAllWheels[wheelId] = diffParams.torqueRatios[wheelId] * torqueSplitMultiplier;
			diffState.connectedWheels[nbConnectedWheels] = wheelId;
			nbConnectedWheels++;
		}
		else if (diffParams.torqueRatios[wheelId] != 0.0f)
		{
			//Wheel connected to diff but not in a tank track.
			//Use the default torque split.
			diffState.aveWheelSpeedContributionAllWheels[wheelId] = diffParams.aveWheelSpeedRatios[wheelId];
			diffState.torqueRatiosAllWheels[wheelId] = diffParams.torqueRatios[wheelId];
			diffState.connectedWheels[nbConnectedWheels] = wheelId;
			nbConnectedWheels++;
		}
		else if (0xffffffff != tankTrackId)
		{
			//Wheel not connected to diff but is in a tank track.
			//Zero torque split.
			diffState.aveWheelSpeedContributionAllWheels[wheelId] = 0.0f;
			diffState.torqueRatiosAllWheels[wheelId] = 0.0f;
			diffState.connectedWheels[nbConnectedWheels] = wheelId;
			nbConnectedWheels++;
		}
	}
	diffState.nbConnectedWheels = nbConnectedWheels;

	//Add each tank track as a constraint group.
	for (PxU32 i = 0; i < diffParams.getNbTracks(); i++)
	{
		const PxU32 nbWheelsInTrack = diffParams.getNbWheelsInTrack(i);
		if (nbWheelsInTrack >= 2)
		{
			const PxU32* wheelsInTrack = diffParams.wheelIdsInTrackOrder + diffParams.trackToWheelIds[i];

			PxF32 multipliers[PxVehicleLimits::eMAX_NB_WHEELS];

			const PxU32 wheelId0 = wheelsInTrack[0];
			const PxF32 wheelRadius0 = wheelParams[wheelId0].radius;

			//for (PxU32 j = 0; j < 1; j++)
			{
				//j = 0 is a special case with multiplier = 1.0
				multipliers[0] = 1.0f;
			}
			for (PxU32 j = 1; j < nbWheelsInTrack; j++)
			{
				const PxU32 wheelIdJ = wheelsInTrack[j];
				const PxF32 wheelRadiusJ = wheelParams[wheelIdJ].radius;
				multipliers[j] = wheelRadius0 / wheelRadiusJ;
			}

			wheelConstraintGroupState.addConstraintGroup(nbWheelsInTrack, wheelsInTrack, multipliers);
		}
	}
}

void PxVehicleEngineDriveActuationStateUpdate
(const PxVehicleAxleDescription& axleDescription,
 const PxVehicleGearboxParams& gearboxParams, 
 const PxVehicleArrayData<const PxReal>& brakeResponseStates,
 const PxVehicleEngineDriveThrottleCommandResponseState& throttleResponseState,
 const PxVehicleGearboxState& gearboxState, const PxVehicleDifferentialState& diffState, const PxVehicleClutchCommandResponseState& clutchResponseState,
 PxVehicleArrayData<PxVehicleWheelActuationState>& actuationStates)
{
	//Which wheels receive drive torque from the engine?
	const PxF32* diffTorqueRatios = diffState.torqueRatiosAllWheels;

	//Work out the clutch strength.
	//If the cutch strength is zero then none of the wheels receive drive torque from the engine.
	const PxF32 K = PxVehicleClutchStrengthCompute(clutchResponseState, gearboxParams, gearboxState);
		
	//Work out the applied throttle
	const PxF32 appliedThrottle = throttleResponseState.commandResponse;

	//Ready to set the boolean actuation state that is used to compute the tire slips. 
	//(Note: for a tire under drive or brake torque we compute the slip with a smaller denominator to accentuate the applied torque).
	//(Note: for a tire that is not under drive or brake torque we compute the sip with a larger denominator to smooth the vehicle slowing down). 
	for(PxU32 i = 0; i < axleDescription.nbWheels; i++)
	{
		const PxU32 wheelId = axleDescription.wheelIdsInAxleOrder[i];

		//Reset the actuation states.
		PxVehicleWheelActuationState& actState = actuationStates[wheelId];
		PxMemZero(&actState, sizeof(PxVehicleWheelActuationState));

		const PxF32 brakeTorque = brakeResponseStates[wheelId];
		const PxF32 diffTorqueRatio = diffTorqueRatios[wheelId];
		const bool isIntentionToAccelerate = (0.0f == brakeTorque) && (K != 0.0f) && (diffTorqueRatio != 0.0f) && (appliedThrottle != 0.0f);
		actState.isDriveApplied = isIntentionToAccelerate;
		actState.isBrakeApplied = (brakeTorque!= 0.0f);
	}
}

void PxVehicleGearCommandResponseUpdate
(const PxU32 targetGearCommand,
 const PxVehicleGearboxParams& gearboxParams,
 PxVehicleGearboxState& gearboxState)
{
	//Check that we're not halfway through a gear change and we need to execute a change of gear.
	//If we are halfway through a gear change then we cannot execute another until the first is complete.
	//Check that the command stays in a legal gear range.
	if ((gearboxState.currentGear == gearboxState.targetGear) && (targetGearCommand != gearboxState.currentGear) && (targetGearCommand < gearboxParams.nbRatios))
	{
		//We're not executing a gear change and we need to start one.
		//Start the gear change  by 
		//a)setting the target gear.
		//b)putting vehicle in neutral
		//c)set the switch time to PX_VEHICLE_GEAR_SWITCH_INITIATED to flag that we just started a gear change.
		gearboxState.currentGear = gearboxParams.neutralGear;
		gearboxState.targetGear = targetGearCommand;
		gearboxState.gearSwitchTime = PX_VEHICLE_GEAR_SWITCH_INITIATED;
	}
}

void PxVehicleAutoBoxUpdate
(const PxVehicleEngineParams& engineParams, const PxVehicleGearboxParams& gearboxParams, const PxVehicleAutoboxParams& autoboxParams, 
 const PxVehicleEngineState& engineState, const PxVehicleGearboxState& gearboxState,
 const PxReal dt,
 PxU32& targetGearCommand, PxVehicleAutoboxState& autoboxState,
 PxReal& throttle)
{
	if(targetGearCommand != PxVehicleEngineDriveTransmissionCommandState::eAUTOMATIC_GEAR)
	{
		autoboxState.activeAutoboxGearShift = false;
		autoboxState.timeSinceLastShift = PX_VEHICLE_UNSPECIFIED_TIME_SINCE_LAST_SHIFT;
		return;
	}

	//Current and target gear allow us to determine if a gear change is underway.
	const PxU32 currentGear = gearboxState.currentGear;
	const PxU32 targetGear = gearboxState.targetGear;

	//Set to current target gear in case no gear change will be initiated.
	targetGearCommand = targetGear;

	//If the autobox triggered a gear change and the gear change is complete then 
	//reset the corresponding flag.
	if(autoboxState.activeAutoboxGearShift && (currentGear == targetGear))
	{
		autoboxState.activeAutoboxGearShift = false;
	}

	//If the autobox triggered a gear change and the gear change is incomplete then 
	//turn off the throttle pedal.  This happens in autoboxes
	//to stop the driver revving the engine then damaging the 
	//clutch when the clutch re-engages at the end of the gear change.
	if(autoboxState.activeAutoboxGearShift && (currentGear != targetGear))
	{
		throttle = 0.0f;
	}

	//Only process the autobox if no gear change is underway and the time passed since 
	//the last autobox gear change is greater than the autobox latency.
	if (targetGear == currentGear)
	{
		const PxF32 autoBoxSwitchTime = autoboxState.timeSinceLastShift;
		const PxF32 autoBoxLatencyTime = autoboxParams.latency;

		if ((currentGear <= gearboxParams.neutralGear) && ((gearboxParams.neutralGear + 1) < gearboxParams.nbRatios))
		{
			// eAUTOMATIC_GEAR has been set while in neutral or one of the reverse gears
			// => switch to first
			targetGearCommand = gearboxParams.neutralGear + 1;
			throttle = 0.0f;
			autoboxState.timeSinceLastShift = 0.0f;
			autoboxState.activeAutoboxGearShift = true;
		}
		else if (autoBoxSwitchTime > autoBoxLatencyTime)
		{
			const PxF32 normalisedEngineOmega = engineState.rotationSpeed/engineParams.maxOmega;
			const PxU32 neutralGear = gearboxParams.neutralGear;
			const PxU32 nbGears = gearboxParams.nbRatios;
			const PxF32 upRatio = autoboxParams.upRatios[currentGear];
			const PxF32 downRatio = autoboxParams.downRatios[currentGear];

			//If revs too high and not in reverse/neutral and there is a higher gear then switch up. 
			//Note: never switch up from neutral to first
			//Note: never switch up from reverse to neutral
			//Note: never switch up from one reverse gear to another reverse gear.
			PX_ASSERT(currentGear > neutralGear);
			if ((normalisedEngineOmega > upRatio) && ((currentGear + 1) < nbGears))
			{
				targetGearCommand = currentGear + 1;
				throttle = 0.0f;
				autoboxState.timeSinceLastShift = 0.0f;
				autoboxState.activeAutoboxGearShift = true;
			}

			//If revs too low and in gear higher than first then switch down.
			//Note: never switch from forward to neutral
			//Note: never switch from neutral to reverse
			//Note: never switch from reverse gear to reverse gear.
			if ((normalisedEngineOmega < downRatio) && (currentGear > (neutralGear + 1)))
			{
				targetGearCommand = currentGear - 1;
				throttle = 0.0f;
				autoboxState.timeSinceLastShift = 0.0f;
				autoboxState.activeAutoboxGearShift = true;
			}
		}
		else
		{
			autoboxState.timeSinceLastShift += dt;
		}
	}
	else
	{
		autoboxState.timeSinceLastShift += dt;
	}
}

void PxVehicleGearboxUpdate(const PxVehicleGearboxParams& gearboxParams, const PxF32 dt, PxVehicleGearboxState& gearboxState)
{
	if(gearboxState.targetGear != gearboxState.currentGear)
	{
		//If we just started a gear change then set the timer to zero.
		//This replicates legacy behaviour.
		if(gearboxState.gearSwitchTime == PX_VEHICLE_GEAR_SWITCH_INITIATED)
			gearboxState.gearSwitchTime = 0.0f;
		else
			gearboxState.gearSwitchTime += dt;

		//If we've  exceed the switch time then switch to the target gear
		//and reset the timer.
		if (gearboxState.gearSwitchTime > gearboxParams.switchTime)
		{
			gearboxState.currentGear = gearboxState.targetGear;
			gearboxState.gearSwitchTime = PX_VEHICLE_NO_GEAR_SWITCH_PENDING;
		}
	}
}


void countConnectedWheelsNotInConstraintGroup
(const PxU32* connectedWheelIds, const PxU32 nbConnectedWheelIds, const PxVehicleWheelConstraintGroupState& constraintGroups,
 PxU32 connectedWheelsNotInConstraintGroup[PxVehicleLimits::eMAX_NB_WHEELS], PxU32& nbConnectedWheelsNotInConstraintGroup)
{
	//Record the constraint group for each wheel.
	//0xffffffff is reserved for a wheel not in a constraint group.
	PxU32 wheelConstraintGroupIds[PxVehicleLimits::eMAX_NB_WHEELS];
	PxMemSet(wheelConstraintGroupIds, 0xffffffff, sizeof(wheelConstraintGroupIds));
	for (PxU32 i = 0; i < constraintGroups.getNbConstraintGroups(); i++)
	{
		for (PxU32 j = 0; j < constraintGroups.getNbWheelsInConstraintGroup(i); j++)
		{
			const PxU32 wheelId = constraintGroups.getWheelInConstraintGroup(j, i);
			wheelConstraintGroupIds[wheelId] = i;
		}
	}

	//Iterate over all connected wheels and count the number not in a group.
	for (PxU32 i = 0; i < nbConnectedWheelIds; i++)
	{
		const PxU32 wheelId = connectedWheelIds[i];
		const PxU32 constraintGroupId = wheelConstraintGroupIds[wheelId];
		if (0xffffffff == constraintGroupId)
		{
			connectedWheelsNotInConstraintGroup[nbConnectedWheelsNotInConstraintGroup] = wheelId;
			nbConnectedWheelsNotInConstraintGroup++;
		}
	}
}

void PxVehicleEngineDrivetrainUpdate
(const PxVehicleAxleDescription& axleDescription,
 const PxVehicleArrayData<const PxVehicleWheelParams>& wheelParams,
 const PxVehicleEngineParams& engineParams, const PxVehicleClutchParams& clutchParams, const PxVehicleGearboxParams& gearboxParams, 
 const PxVehicleArrayData<const PxReal>& brakeResponseStates,
 const PxVehicleArrayData<const PxVehicleWheelActuationState>& actuationStates,
 const PxVehicleArrayData<const PxVehicleTireForce>& tireForces,
 const PxVehicleGearboxState& gearboxState,
 const PxVehicleEngineDriveThrottleCommandResponseState& throttleCommandResponseState, const PxVehicleClutchCommandResponseState& clutchCommandResponseState, 
 const PxVehicleDifferentialState& diffState, const PxVehicleWheelConstraintGroupState* constraintGroupState,
 const PxReal DT,
 PxVehicleArrayData<PxVehicleWheelRigidBody1dState>& wheelRigidbody1dStates,
 PxVehicleEngineState& engineState,
 PxVehicleClutchSlipState& clutchState)
{
	const PxF32 K = PxVehicleClutchStrengthCompute(clutchCommandResponseState, gearboxParams, gearboxState);
	const PxF32 G = PxVehicleGearRatioCompute(gearboxParams, gearboxState);
	const PxF32 engineDriveTorque = PxVehicleEngineDriveTorqueCompute(engineParams, engineState, throttleCommandResponseState);
	const PxF32 engineDampingRate = PxVehicleEngineDampingRateCompute(engineParams, gearboxParams, gearboxState, clutchCommandResponseState, throttleCommandResponseState);

	//Arrange wheel parameters in they order they appear in the connected wheel list.
	const PxU32* connectedWheelIds = diffState.connectedWheels;
	const PxU32 nbConnectedWheelIds = diffState.nbConnectedWheels;
	PxU32 wheelIdToConnectedWheelId[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelDiffTorqueRatios[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelGearings[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelAveWheelSpeedContributions[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelMois[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelRotSpeeds[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelBrakeTorques[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelTireTorques[PxVehicleLimits::eMAX_NB_WHEELS];
	PxF32 connectedWheelDampingRates[PxVehicleLimits::eMAX_NB_WHEELS];
	bool connectedWheelIsBrakeApplied[PxVehicleLimits::eMAX_NB_WHEELS];
	//PxF32 connectedWheelRadii[PxVehicleLimits::eMAX_NB_WHEELS];
	for(PxU32 i = 0; i < nbConnectedWheelIds; i++)
	{
		const PxU32 wheelId = connectedWheelIds[i];
		wheelIdToConnectedWheelId[wheelId] = i;
		connectedWheelDiffTorqueRatios[i] = PxAbs(diffState.torqueRatiosAllWheels[wheelId]);
		connectedWheelGearings[i] = PxVehicleComputeSign(diffState.torqueRatiosAllWheels[wheelId]);
		connectedWheelAveWheelSpeedContributions[i] = diffState.aveWheelSpeedContributionAllWheels[wheelId];
		connectedWheelMois[i] = wheelParams[wheelId].moi;
		connectedWheelRotSpeeds[i] = wheelRigidbody1dStates[wheelId].rotationSpeed;
		connectedWheelBrakeTorques[i] = -PxVehicleComputeSign(wheelRigidbody1dStates[wheelId].rotationSpeed)*brakeResponseStates[wheelId];
		connectedWheelTireTorques[i] = tireForces[wheelId].wheelTorque;
		connectedWheelDampingRates[i] = wheelParams[wheelId].dampingRate;
		connectedWheelIsBrakeApplied[i] = actuationStates[wheelId].isBrakeApplied;
		//connectedWheelRadii[i] = wheelParams[wheelId].radius;
	};

	//Compute the clutch slip
	clutchState.setToDefault();
	PxF32 clutchSlip = 0;
	{
		PxF32 averageWheelSpeed = 0;
		for (PxU32 i = 0; i < nbConnectedWheelIds; i++)
		{
			averageWheelSpeed += connectedWheelRotSpeeds[i] * connectedWheelAveWheelSpeedContributions[i];
		}
		clutchSlip = G*averageWheelSpeed - engineState.rotationSpeed;
	}
	clutchState.clutchSlip = clutchSlip;

	//
	//torque at clutch:  
	//tc = K*{G*[alpha0*w0 + alpha1*w1 + alpha2*w2 + ..... alpha(N-1)*w(N-1)] - wEng}
	//where 
	//(i)   G is the gearing ratio, 
	//(ii)  alphai is the fractional contribution of the ith wheel to the average wheel speed at the clutch (alpha(i) is zero for undriven wheels)
	//(iii) wi is the angular speed of the ith wheel
	//(iv)  K is the clutch strength 
	//(v)   wEng is the angular speed of the engine

	//torque applied to ith wheel is 
	//ti = G*gammai*tc + bt(i) + tt(i) 
	//where
	//gammai is the fractional proportion of the clutch torque that the differential delivers to the ith wheel
	//bt(i) is the brake torque applied to the ith wheel
	//tt(i) is the tire torque applied to the ith wheel

	//acceleration applied to ith wheel is 
	//ai = G*gammai*K*{G*[alpha0*w0 + alpha1*w1 alpha2*w2 + ..... alpha(N-1)*w(N-1)] - wEng}/Ii + (bt(i) + tt(i))/Ii
	//wheer Ii is the moi of the ith wheel

	//express ai as 
	//ai = [wi(t+dt) - wi(t)]/dt
	//and rearrange
	//wi(t+dt) - wi(t)] = dt*G*gammai*K*{G*[alpha0*w0(t+dt) + alpha1*w1(t+dt) + alpha2*w2(t+dt) + ..... alpha(N-1)*w(N-1)(t+dt)] - wEng(t+dt)}/Ii + dt*(bt(i) + tt(i))/Ii

	//Do the same for tEng (torque applied to engine)
	//tEng  = -tc + engineDriveTorque
	//where engineDriveTorque is the drive torque applied to the engine
	//Assuming the engine has unit mass then
	//wEng(t+dt) -wEng(t) = -dt*K*{G*[alpha0*w0(t+dt) + alpha1*w1(t+dt) + alpha2*w2(t+dt) + ..... alpha(N-1)*w(N-1(t+dt))] - wEng(t+dt)}/Ieng + dt*engineDriveTorque]/IEng

	//Introduce the vector w=(w0,w1,w2....w(N-1), wEng)
	//and re-express as a matrix after collecting all unknowns at (t+dt) and knowns at time t.
	//A*w(t+dt)=b(t);

	PxVehicleMatrixNN M(nbConnectedWheelIds + 1);
	PxVehicleVectorN b(nbConnectedWheelIds + 1);
	PxVehicleVectorN result(nbConnectedWheelIds + 1);

	const PxF32 KG = K * G;
	const PxF32 KGG = K * G*G;

	//Wheels
	{
		for (PxU32 i = 0; i < nbConnectedWheelIds; i++)
		{
			const PxF32 dt = DT / connectedWheelMois[i];
			const PxF32 R = connectedWheelDiffTorqueRatios[i];
			const PxF32 g = connectedWheelGearings[i];
			const PxF32 dtKGGRg = dt * KGG*R*g;

			for(PxU32 j = 0; j < nbConnectedWheelIds; j++)
			{
				M.set(i, j, dtKGGRg*connectedWheelAveWheelSpeedContributions[j]*connectedWheelGearings[j]);
			}
			M.set(i, i, 1.0f + dtKGGRg*connectedWheelAveWheelSpeedContributions[i]*connectedWheelGearings[i] + dt * connectedWheelDampingRates[i]);
			M.set(i, nbConnectedWheelIds, -dt*KG*R*g);
			b[i] = connectedWheelRotSpeeds[i] + dt * (connectedWheelBrakeTorques[i] + connectedWheelTireTorques[i]);
			result[i] = connectedWheelRotSpeeds[i];
		}
	}

	//Engine.
	{
		const PxF32 dt = DT / engineParams.moi;
		const PxF32 dtKG = dt*K*G;
		for(PxU32 j = 0; j < nbConnectedWheelIds; j++)
		{
			M.set(nbConnectedWheelIds, j, -dtKG * connectedWheelAveWheelSpeedContributions[j]* connectedWheelGearings[j]);
		}
		M.set(nbConnectedWheelIds, nbConnectedWheelIds, 1.0f + dt * (K + engineDampingRate));
		b[nbConnectedWheelIds] = engineState.rotationSpeed + dt * engineDriveTorque;
		result[nbConnectedWheelIds] = engineState.rotationSpeed;
	}

	if (constraintGroupState && constraintGroupState->getNbConstraintGroups() > 0)
	{
		const PxU32 nbConstraintGroups = constraintGroupState->getNbConstraintGroups();

		//Count the wheels not in a constraint group.
		PxU32 connectedWheelsNotInConstraintGroup[PxVehicleLimits::eMAX_NB_WHEELS];
		PxU32 nbConnectedWheelsNotInConstraintGroup = 0;
		countConnectedWheelsNotInConstraintGroup(
			connectedWheelIds, nbConnectedWheelIds, *constraintGroupState, 
			connectedWheelsNotInConstraintGroup, nbConnectedWheelsNotInConstraintGroup);

		//After applying constraint groups:
		//  number of columns remains nbConnectedWheelIds + 1
		//  each row will be of length nbConnectedWheelsNotInConstraintGroup + nbConstraintGroups + 1
		PxVehicleMatrixNN A(nbConnectedWheelIds + 1);
		for (PxU32 i = 0; i < nbConnectedWheelIds + 1; i++)
		{
			//1 entry for each wheel not in a constraint group.
			for (PxU32 j = 0; j < nbConnectedWheelsNotInConstraintGroup; j++)
			{
				const PxU32 wheelId = connectedWheelsNotInConstraintGroup[j];
				const PxU32 connectedWheelId = wheelIdToConnectedWheelId[wheelId];
				const PxF32 MIJ = M.get(i, connectedWheelId);
				A.set(i, j, MIJ);
			}

			//1 entry for each constraint group.
			for (PxU32 j = nbConnectedWheelsNotInConstraintGroup; j < nbConnectedWheelsNotInConstraintGroup + nbConstraintGroups; j++)
			{
				const PxU32 constraintGroupId = (j - nbConnectedWheelsNotInConstraintGroup);

				PxF32 sum = 0.0f;
				//for (PxU32 k = 0; k < 1; k++)
				{
					//k = 0 is a special case with multiplier = 1.0.
					const PxU32 wheelId = constraintGroupState->getWheelInConstraintGroup(0, constraintGroupId);
					const PxF32 multiplier = 1.0f;
					const PxU32 connectedWheelId = wheelIdToConnectedWheelId[wheelId];
					const PxF32 MIK = M.get(i, connectedWheelId);
					sum += MIK * multiplier;
				}
				for (PxU32 k = 1; k < constraintGroupState->getNbWheelsInConstraintGroup(constraintGroupId); k++)
				{
					const PxU32 wheelId = constraintGroupState->getWheelInConstraintGroup(k, constraintGroupId);
					const PxF32 multiplier = constraintGroupState->getMultiplierInConstraintGroup(k, constraintGroupId);
					const PxU32 connectedWheelId = wheelIdToConnectedWheelId[wheelId];
					const PxF32 MIK = M.get(i, connectedWheelId);
					sum += MIK*multiplier;
				}
				A.set(i, j, sum);
			}

			//1 entry for the engine.
			{
				const PxF32 MIJ = M.get(i, nbConnectedWheelIds);
				A.set(i, nbConnectedWheelsNotInConstraintGroup + nbConstraintGroups, MIJ);
			}
		}

		const PxU32 N = (nbConnectedWheelsNotInConstraintGroup + nbConstraintGroups + 1);
		//Compute A^T * A 
		PxVehicleMatrixNN ATA(N);	
		for (PxU32 i = 0; i < N; i++)
		{
			for (PxU32 j = 0; j < N; j++)
			{
				PxF32 sum = 0.0f;
				for (PxU32 k = 0; k < nbConnectedWheelIds + 1; k++)
				{
					sum += A.get(k, i)*A.get(k, j);
				}
				ATA.set(i, j, sum);
			}
		}

		//Compute A^T*b;
		PxVehicleVectorN ATb(N);
		for (PxU32 i = 0; i < N; i++)
		{
			PxF32 sum = 0;
			for (PxU32 j = 0; j < nbConnectedWheelIds + 1; j++)
			{
				sum += A.get(j, i)*b[j];
			}
			ATb[i] = sum;
		}

		//Solve it.
		PxVehicleMatrixNNLUSolver solver;
		PxVehicleVectorN result2(N);
		solver.decomposeLU(ATA);
		solver.solve(ATb, result2);

		//Map from result2 back to result.
		for (PxU32 j = 0; j < nbConnectedWheelsNotInConstraintGroup; j++)
		{
			const PxU32 wheelId = connectedWheelsNotInConstraintGroup[j];
			const PxU32 connectedWheelId = wheelIdToConnectedWheelId[wheelId];
			result[connectedWheelId] = result2[j];
		}
		for (PxU32 j = nbConnectedWheelsNotInConstraintGroup; j < nbConnectedWheelsNotInConstraintGroup + nbConstraintGroups; j++)
		{
			const PxU32 constraintGroupId = (j - nbConnectedWheelsNotInConstraintGroup);
			//for (PxU32 k = 0; k < 1; k++)
			{
				//k = 0 is a special case with multiplier = 1.0
				const PxU32 wheelId = constraintGroupState->getWheelInConstraintGroup(0, constraintGroupId);
				const PxF32 multiplier = 1.0f;
				const PxU32 connectedWheelId = wheelIdToConnectedWheelId[wheelId];
				result[connectedWheelId] = multiplier * result2[j];
			}
			for (PxU32 k = 1; k < constraintGroupState->getNbWheelsInConstraintGroup(constraintGroupId); k++)
			{
				const PxU32 wheelId = constraintGroupState->getWheelInConstraintGroup(k, constraintGroupId);
				const PxF32 multiplier = constraintGroupState->getMultiplierInConstraintGroup(k, constraintGroupId);
				const PxU32 connectedWheelId = wheelIdToConnectedWheelId[wheelId];
				result[connectedWheelId] = multiplier * result2[j];
			}
		}
		{
			result[nbConnectedWheelIds] = result2[nbConnectedWheelsNotInConstraintGroup + nbConstraintGroups];
		}
	}
	else if (PxVehicleClutchAccuracyMode::eBEST_POSSIBLE == clutchParams.accuracyMode)
	{
		//Solve Aw=b
		PxVehicleMatrixNNLUSolver solver;
		solver.decomposeLU(M);
		solver.solve(b, result);
		//PX_WARN_ONCE_IF(!isValid(A, b, result), "Unable to compute new PxVehicleDrive4W internal rotation speeds.  Please check vehicle sim data, especially clutch strength; engine moi and damping; wheel moi and damping");
	}
	else
	{
		PxVehicleMatrixNGaussSeidelSolver solver;
		solver.solve(clutchParams.estimateIterations, 1e-10f, M, b, result);
	}

	//Check for sanity in the resultant internal rotation speeds.
	//If the brakes are on and the wheels have switched direction then lock them at zero.
	//A consequence of this quick fix is that locked wheels remain locked until the brake is entirely released.
	//This isn't strictly mathematically or physically correct - a more accurate solution would either formulate the 
	//brake as a lcp problem or repeatedly solve with constraints that locked wheels remain at zero rotation speed.
	//The physically correct solution will certainly be more expensive so let's live with the restriction that 
	//locked wheels remain locked until the brake is released. 
	//newOmega=result[i], oldOmega=wheelSpeeds[i], if newOmega*oldOmega<=0 and isBrakeApplied then lock wheel.
	for(PxU32 i = 0; i < nbConnectedWheelIds; i++)
	{
		result[i] = (connectedWheelIsBrakeApplied[i] && (connectedWheelRotSpeeds[i] * result[i] <= 0)) ? 0.0f : result[i];
	}
	//Clamp the engine revs.
	//Again, this is not physically or mathematically correct but the loss in behaviour will be hard to notice.
	//The alternative would be to add constraints to the solver, which would be much more expensive.
	result[nbConnectedWheelIds] = PxClamp(result[nbConnectedWheelIds], engineParams.idleOmega, engineParams.maxOmega);

	//Copy back to the car's internal rotation speeds.
	for (PxU32 i = 0; i < nbConnectedWheelIds; i++)
	{
		const PxU32 wheelId = connectedWheelIds[i];
		wheelRigidbody1dStates[wheelId].rotationSpeed = result[i];
	}
	engineState.rotationSpeed = result[nbConnectedWheelIds];

	//Update the undriven wheels.
	bool isDrivenWheel[PxVehicleLimits::eMAX_NB_WHEELS];
	PxMemZero(isDrivenWheel, sizeof(isDrivenWheel));
	for(PxU32 i = 0; i < nbConnectedWheelIds; i++)
	{
		const PxU32 wheelId = connectedWheelIds[i];
		isDrivenWheel[wheelId] = true;
	}
	for (PxU32 i = 0; i < axleDescription.nbWheels; i++)
	{
		const PxU32 wheelId = axleDescription.wheelIdsInAxleOrder[i];
		if (!isDrivenWheel[wheelId])
		{
			PxVehicleDirectDriveUpdate(
				wheelParams[wheelId],
				actuationStates[wheelId],
				brakeResponseStates[wheelId], 0.0f,
				tireForces[wheelId], DT, wheelRigidbody1dStates[wheelId]);
		}
	}
}

void PxVehicleClutchCommandResponseLinearUpdate
(const PxReal clutchCommand,
 const PxVehicleClutchCommandResponseParams& clutchResponseParams,
 PxVehicleClutchCommandResponseState& clutchResponse)
{
	clutchResponse.normalisedCommandResponse = (1.0f - clutchCommand);
	clutchResponse.commandResponse = (1.0f - clutchCommand)*clutchResponseParams.maxResponse;
}

void PxVehicleEngineDriveThrottleCommandResponseLinearUpdate
(const PxVehicleCommandState& commands,
 PxVehicleEngineDriveThrottleCommandResponseState& throttleResponse)
{
	throttleResponse.commandResponse = commands.throttle;
}


} //namespace physx