initial commit, 4.5 stable
Some checks failed
🔗 GHA / 📊 Static checks (push) Has been cancelled
🔗 GHA / 🤖 Android (push) Has been cancelled
🔗 GHA / 🍏 iOS (push) Has been cancelled
🔗 GHA / 🐧 Linux (push) Has been cancelled
🔗 GHA / 🍎 macOS (push) Has been cancelled
🔗 GHA / 🏁 Windows (push) Has been cancelled
🔗 GHA / 🌐 Web (push) Has been cancelled

This commit is contained in:
2025-09-16 20:46:46 -04:00
commit 9d30169a8d
13378 changed files with 7050105 additions and 0 deletions

View File

@@ -0,0 +1,293 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/MotorcycleController.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MotorcycleControllerSettings)
{
JPH_ADD_BASE_CLASS(MotorcycleControllerSettings, VehicleControllerSettings)
JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mMaxLeanAngle)
JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringConstant)
JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringDamping)
JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficient)
JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficientDecay)
JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSmoothingFactor)
}
VehicleController *MotorcycleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const
{
return new MotorcycleController(*this, inConstraint);
}
void MotorcycleControllerSettings::SaveBinaryState(StreamOut &inStream) const
{
WheeledVehicleControllerSettings::SaveBinaryState(inStream);
inStream.Write(mMaxLeanAngle);
inStream.Write(mLeanSpringConstant);
inStream.Write(mLeanSpringDamping);
inStream.Write(mLeanSpringIntegrationCoefficient);
inStream.Write(mLeanSpringIntegrationCoefficientDecay);
inStream.Write(mLeanSmoothingFactor);
}
void MotorcycleControllerSettings::RestoreBinaryState(StreamIn &inStream)
{
WheeledVehicleControllerSettings::RestoreBinaryState(inStream);
inStream.Read(mMaxLeanAngle);
inStream.Read(mLeanSpringConstant);
inStream.Read(mLeanSpringDamping);
inStream.Read(mLeanSpringIntegrationCoefficient);
inStream.Read(mLeanSpringIntegrationCoefficientDecay);
inStream.Read(mLeanSmoothingFactor);
}
MotorcycleController::MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint) :
WheeledVehicleController(inSettings, inConstraint),
mMaxLeanAngle(inSettings.mMaxLeanAngle),
mLeanSpringConstant(inSettings.mLeanSpringConstant),
mLeanSpringDamping(inSettings.mLeanSpringDamping),
mLeanSpringIntegrationCoefficient(inSettings.mLeanSpringIntegrationCoefficient),
mLeanSpringIntegrationCoefficientDecay(inSettings.mLeanSpringIntegrationCoefficientDecay),
mLeanSmoothingFactor(inSettings.mLeanSmoothingFactor)
{
}
float MotorcycleController::GetWheelBase() const
{
float low = FLT_MAX, high = -FLT_MAX;
for (const Wheel *w : mConstraint.GetWheels())
{
const WheelSettings *s = w->GetSettings();
// Measure distance along the forward axis by looking at the fully extended suspension.
// If the suspension force point is active, use that instead.
Vec3 force_point = s->mEnableSuspensionForcePoint? s->mSuspensionForcePoint : s->mPosition + s->mSuspensionDirection * s->mSuspensionMaxLength;
float value = force_point.Dot(mConstraint.GetLocalForward());
// Update min and max
low = min(low, value);
high = max(high, value);
}
return high - low;
}
void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
{
WheeledVehicleController::PreCollide(inDeltaTime, inPhysicsSystem);
const Body *body = mConstraint.GetVehicleBody();
Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward();
float wheel_base = GetWheelBase();
Vec3 world_up = mConstraint.GetWorldUp();
if (mEnableLeanController)
{
// Calculate the target lean vector, this is in the direction of the total applied impulse by the ground on the wheels
Vec3 target_lean = Vec3::sZero();
for (const Wheel *w : mConstraint.GetWheels())
if (w->HasContact())
target_lean += w->GetContactNormal() * w->GetSuspensionLambda() + w->GetContactLateral() * w->GetLateralLambda();
// Normalize the impulse
target_lean = target_lean.NormalizedOr(world_up);
// Smooth the impulse to avoid jittery behavior
mTargetLean = mLeanSmoothingFactor * mTargetLean + (1.0f - mLeanSmoothingFactor) * target_lean;
// Remove forward component, we can only lean sideways
mTargetLean -= forward * mTargetLean.Dot(forward);
mTargetLean = mTargetLean.NormalizedOr(world_up);
// Clamp the target lean against the max lean angle
Vec3 adjusted_world_up = world_up - forward * world_up.Dot(forward);
adjusted_world_up = adjusted_world_up.NormalizedOr(world_up);
float w_angle = -Sign(mTargetLean.Cross(adjusted_world_up).Dot(forward)) * ACos(mTargetLean.Dot(adjusted_world_up));
if (abs(w_angle) > mMaxLeanAngle)
mTargetLean = Quat::sRotation(forward, Sign(w_angle) * mMaxLeanAngle) * adjusted_world_up;
// Integrate the delta angle
Vec3 up = body->GetRotation() * mConstraint.GetLocalUp();
float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up));
mLeanSpringIntegratedDeltaAngle += d_angle * inDeltaTime;
}
else
{
// Controller not enabled, reset target lean
mTargetLean = world_up;
// Reset integrated delta angle
mLeanSpringIntegratedDeltaAngle = 0;
}
JPH_DET_LOG("WheeledVehicleController::PreCollide: mTargetLean: " << mTargetLean);
// Calculate max steering angle based on the max lean angle we're willing to take
// See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning
// LeanAngle = Atan(Velocity^2 / (Gravity * TurnRadius))
// And: https://en.wikipedia.org/wiki/Turning_radius (we're ignoring the tire width)
// The CasterAngle is the added according to https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Turning (this is the same formula but without small angle approximation)
// TurnRadius = WheelBase / (Sin(SteerAngle) * Cos(CasterAngle))
// => SteerAngle = ASin(WheelBase * Tan(LeanAngle) * Gravity / (Velocity^2 * Cos(CasterAngle))
// The caster angle is different for each wheel so we can only calculate part of the equation here
float max_steer_angle_factor = wheel_base * Tan(mMaxLeanAngle) * (mConstraint.IsGravityOverridden()? mConstraint.GetGravityOverride() : inPhysicsSystem.GetGravity()).Length();
// Calculate forward velocity
float velocity = body->GetLinearVelocity().Dot(forward);
float velocity_sq = Square(velocity);
// Decompose steering into sign and direction
float steer_strength = abs(mRightInput);
float steer_sign = -Sign(mRightInput);
for (Wheel *w_base : mConstraint.GetWheels())
{
WheelWV *w = static_cast<WheelWV *>(w_base);
const WheelSettingsWV *s = w->GetSettings();
// Check if this wheel can steer
if (s->mMaxSteerAngle != 0.0f)
{
// Calculate cos(caster angle), the angle between the steering axis and the up vector
float cos_caster_angle = s->mSteeringAxis.Dot(mConstraint.GetLocalUp());
// Calculate steer angle
float steer_angle = steer_strength * w->GetSettings()->mMaxSteerAngle;
// Clamp to max steering angle
if (mEnableLeanSteeringLimit
&& velocity_sq > 1.0e-6f && cos_caster_angle > 1.0e-6f)
{
float max_steer_angle = ASin(max_steer_angle_factor / (velocity_sq * cos_caster_angle));
steer_angle = min(steer_angle, max_steer_angle);
}
// Set steering angle
w->SetSteerAngle(steer_sign * steer_angle);
}
}
// Reset applied impulse
mAppliedImpulse = 0;
}
bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime)
{
bool impulse = WheeledVehicleController::SolveLongitudinalAndLateralConstraints(inDeltaTime);
if (mEnableLeanController)
{
// Only apply a lean impulse if all wheels are in contact, otherwise we can easily spin out
bool all_in_contact = true;
for (const Wheel *w : mConstraint.GetWheels())
if (!w->HasContact() || w->GetSuspensionLambda() <= 0.0f)
{
all_in_contact = false;
break;
}
if (all_in_contact)
{
Body *body = mConstraint.GetVehicleBody();
const MotionProperties *mp = body->GetMotionProperties();
Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward();
Vec3 up = body->GetRotation() * mConstraint.GetLocalUp();
// Calculate delta to target angle and derivative
float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up));
float ddt_angle = body->GetAngularVelocity().Dot(forward);
// Calculate impulse to apply to get to target lean angle
float total_impulse = (mLeanSpringConstant * d_angle - mLeanSpringDamping * ddt_angle + mLeanSpringIntegrationCoefficient * mLeanSpringIntegratedDeltaAngle) * inDeltaTime;
// Remember angular velocity pre angular impulse
Vec3 old_w = mp->GetAngularVelocity();
// Apply impulse taking into account the impulse we've applied earlier
float delta_impulse = total_impulse - mAppliedImpulse;
body->AddAngularImpulse(delta_impulse * forward);
mAppliedImpulse = total_impulse;
// Calculate delta angular velocity due to angular impulse
Vec3 dw = mp->GetAngularVelocity() - old_w;
Vec3 linear_acceleration = Vec3::sZero();
float total_lambda = 0.0f;
for (Wheel *w_base : mConstraint.GetWheels())
{
const WheelWV *w = static_cast<WheelWV *>(w_base);
// We weigh the importance of each contact point according to the contact force
float lambda = w->GetSuspensionLambda();
total_lambda += lambda;
// Linear acceleration of contact point is dw x com_to_contact
Vec3 r = Vec3(w->GetContactPosition() - body->GetCenterOfMassPosition());
linear_acceleration += lambda * dw.Cross(r);
}
// Apply linear impulse to COM to cancel the average velocity change on the wheels due to the angular impulse
Vec3 linear_impulse = -linear_acceleration / (total_lambda * mp->GetInverseMass());
body->AddImpulse(linear_impulse);
// Return true if we applied an impulse
impulse |= delta_impulse != 0.0f;
}
else
{
// Decay the integrated angle because we won't be applying a torque this frame
// Uses 1st order Taylor approximation of e^(-decay * dt) = 1 - decay * dt
mLeanSpringIntegratedDeltaAngle *= max(0.0f, 1.0f - mLeanSpringIntegrationCoefficientDecay * inDeltaTime);
}
}
return impulse;
}
void MotorcycleController::SaveState(StateRecorder &inStream) const
{
WheeledVehicleController::SaveState(inStream);
inStream.Write(mTargetLean);
}
void MotorcycleController::RestoreState(StateRecorder &inStream)
{
WheeledVehicleController::RestoreState(inStream);
inStream.Read(mTargetLean);
}
#ifdef JPH_DEBUG_RENDERER
void MotorcycleController::Draw(DebugRenderer *inRenderer) const
{
WheeledVehicleController::Draw(inRenderer);
// Draw current and desired lean angle
Body *body = mConstraint.GetVehicleBody();
RVec3 center_of_mass = body->GetCenterOfMassPosition();
Vec3 up = body->GetRotation() * mConstraint.GetLocalUp();
inRenderer->DrawArrow(center_of_mass, center_of_mass + up, Color::sYellow, 0.1f);
inRenderer->DrawArrow(center_of_mass, center_of_mass + mTargetLean, Color::sRed, 0.1f);
}
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_END

View File

@@ -0,0 +1,116 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
JPH_NAMESPACE_BEGIN
/// Settings of a two wheeled motorcycle (adds a spring to balance the motorcycle)
/// Note: The motor cycle controller is still in development and may need a lot of tweaks/hacks to work properly!
class JPH_EXPORT MotorcycleControllerSettings : public WheeledVehicleControllerSettings
{
JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MotorcycleControllerSettings)
public:
// See: VehicleControllerSettings
virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override;
virtual void SaveBinaryState(StreamOut &inStream) const override;
virtual void RestoreBinaryState(StreamIn &inStream) override;
/// How far we're willing to make the bike lean over in turns (in radians)
float mMaxLeanAngle = DegreesToRadians(45.0f);
/// Spring constant for the lean spring
float mLeanSpringConstant = 5000.0f;
/// Spring damping constant for the lean spring
float mLeanSpringDamping = 1000.0f;
/// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller
float mLeanSpringIntegrationCoefficient = 0.0f;
/// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value
float mLeanSpringIntegrationCoefficientDecay = 4.0f;
/// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes)
/// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current
float mLeanSmoothingFactor = 0.8f;
};
/// Runtime controller class
class JPH_EXPORT MotorcycleController : public WheeledVehicleController
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint);
/// Get the distance between the front and back wheels
float GetWheelBase() const;
/// Enable or disable the lean spring. This allows you to temporarily disable the lean spring to allow the motorcycle to fall over.
void EnableLeanController(bool inEnable) { mEnableLeanController = inEnable; }
/// Check if the lean spring is enabled.
bool IsLeanControllerEnabled() const { return mEnableLeanController; }
/// Enable or disable the lean steering limit. When enabled (default) the steering angle is limited based on the vehicle speed to prevent steering that would cause an inertial force that causes the motorcycle to topple over.
void EnableLeanSteeringLimit(bool inEnable) { mEnableLeanSteeringLimit = inEnable; }
bool IsLeanSteeringLimitEnabled() const { return mEnableLeanSteeringLimit; }
/// Spring constant for the lean spring
void SetLeanSpringConstant(float inConstant) { mLeanSpringConstant = inConstant; }
float GetLeanSpringConstant() const { return mLeanSpringConstant; }
/// Spring damping constant for the lean spring
void SetLeanSpringDamping(float inDamping) { mLeanSpringDamping = inDamping; }
float GetLeanSpringDamping() const { return mLeanSpringDamping; }
/// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller
void SetLeanSpringIntegrationCoefficient(float inCoefficient) { mLeanSpringIntegrationCoefficient = inCoefficient; }
float GetLeanSpringIntegrationCoefficient() const { return mLeanSpringIntegrationCoefficient; }
/// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value
void SetLeanSpringIntegrationCoefficientDecay(float inDecay) { mLeanSpringIntegrationCoefficientDecay = inDecay; }
float GetLeanSpringIntegrationCoefficientDecay() const { return mLeanSpringIntegrationCoefficientDecay; }
/// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes)
/// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current
void SetLeanSmoothingFactor(float inFactor) { mLeanSmoothingFactor = inFactor; }
float GetLeanSmoothingFactor() const { return mLeanSmoothingFactor; }
protected:
// See: VehicleController
virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
virtual void SaveState(StateRecorder &inStream) const override;
virtual void RestoreState(StateRecorder &inStream) override;
#ifdef JPH_DEBUG_RENDERER
virtual void Draw(DebugRenderer *inRenderer) const override;
#endif // JPH_DEBUG_RENDERER
// Configuration properties
bool mEnableLeanController = true;
bool mEnableLeanSteeringLimit = true;
float mMaxLeanAngle;
float mLeanSpringConstant;
float mLeanSpringDamping;
float mLeanSpringIntegrationCoefficient;
float mLeanSpringIntegrationCoefficientDecay;
float mLeanSmoothingFactor;
// Run-time calculated target lean vector
Vec3 mTargetLean = Vec3::sZero();
// Integrated error for the lean spring
float mLeanSpringIntegratedDeltaAngle = 0.0f;
// Run-time total angular impulse applied to turn the cycle towards the target lean angle
float mAppliedImpulse = 0.0f;
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,531 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/TrackedVehicleController.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TrackedVehicleControllerSettings)
{
JPH_ADD_BASE_CLASS(TrackedVehicleControllerSettings, VehicleControllerSettings)
JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mEngine)
JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTransmission)
JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTracks)
}
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsTV)
{
JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLongitudinalFriction)
JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLateralFriction)
}
void WheelSettingsTV::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mLongitudinalFriction);
inStream.Write(mLateralFriction);
}
void WheelSettingsTV::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mLongitudinalFriction);
inStream.Read(mLateralFriction);
}
WheelTV::WheelTV(const WheelSettingsTV &inSettings) :
Wheel(inSettings)
{
}
void WheelTV::CalculateAngularVelocity(const VehicleConstraint &inConstraint)
{
const WheelSettingsTV *settings = GetSettings();
const Wheels &wheels = inConstraint.GetWheels();
const VehicleTrack &track = static_cast<const TrackedVehicleController *>(inConstraint.GetController())->GetTracks()[mTrackIndex];
// Calculate angular velocity of this wheel
mAngularVelocity = track.mAngularVelocity * wheels[track.mDrivenWheel]->GetSettings()->mRadius / settings->mRadius;
}
void WheelTV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint)
{
CalculateAngularVelocity(inConstraint);
// Update rotation of wheel
mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI);
// Reset brake impulse, will be set during post collision again
mBrakeImpulse = 0.0f;
if (mContactBody != nullptr)
{
// Friction at the point of this wheel between track and floor
const WheelSettingsTV *settings = GetSettings();
VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction();
mCombinedLongitudinalFriction = settings->mLongitudinalFriction;
mCombinedLateralFriction = settings->mLateralFriction;
combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID);
}
else
{
// No collision
mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f;
}
}
VehicleController *TrackedVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const
{
return new TrackedVehicleController(*this, inConstraint);
}
TrackedVehicleControllerSettings::TrackedVehicleControllerSettings()
{
// Numbers guestimated from: https://en.wikipedia.org/wiki/M1_Abrams
mEngine.mMinRPM = 500.0f;
mEngine.mMaxRPM = 4000.0f;
mEngine.mMaxTorque = 500.0f; // Note actual torque for M1 is around 5000 but we need a reduced mass in order to keep the simulation sane
mTransmission.mShiftDownRPM = 1000.0f;
mTransmission.mShiftUpRPM = 3500.0f;
mTransmission.mGearRatios = { 4.0f, 3.0f, 2.0f, 1.0f };
mTransmission.mReverseGearRatios = { -4.0f, -3.0f };
}
void TrackedVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const
{
mEngine.SaveBinaryState(inStream);
mTransmission.SaveBinaryState(inStream);
for (const VehicleTrackSettings &t : mTracks)
t.SaveBinaryState(inStream);
}
void TrackedVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream)
{
mEngine.RestoreBinaryState(inStream);
mTransmission.RestoreBinaryState(inStream);
for (VehicleTrackSettings &t : mTracks)
t.RestoreBinaryState(inStream);
}
TrackedVehicleController::TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) :
VehicleController(inConstraint)
{
// Copy engine settings
static_cast<VehicleEngineSettings &>(mEngine) = inSettings.mEngine;
JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f);
JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM);
mEngine.SetCurrentRPM(mEngine.mMinRPM);
// Copy transmission settings
static_cast<VehicleTransmissionSettings &>(mTransmission) = inSettings.mTransmission;
#ifdef JPH_ENABLE_ASSERTS
for (float r : inSettings.mTransmission.mGearRatios)
JPH_ASSERT(r > 0.0f);
for (float r : inSettings.mTransmission.mReverseGearRatios)
JPH_ASSERT(r < 0.0f);
#endif // JPH_ENABLE_ASSERTS
JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f);
JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f);
JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM);
JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM);
// Copy track settings
for (uint i = 0; i < std::size(mTracks); ++i)
{
const VehicleTrackSettings &d = inSettings.mTracks[i];
static_cast<VehicleTrackSettings &>(mTracks[i]) = d;
JPH_ASSERT(d.mInertia >= 0.0f);
JPH_ASSERT(d.mAngularDamping >= 0.0f);
JPH_ASSERT(d.mMaxBrakeTorque >= 0.0f);
JPH_ASSERT(d.mDifferentialRatio > 0.0f);
}
}
bool TrackedVehicleController::AllowSleep() const
{
return mForwardInput == 0.0f // No user input
&& mTransmission.AllowSleep() // Transmission is not shifting
&& mEngine.AllowSleep(); // Engine is idling
}
void TrackedVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
{
Wheels &wheels = mConstraint.GetWheels();
// Fill in track index
for (size_t t = 0; t < std::size(mTracks); ++t)
for (uint w : mTracks[t].mWheels)
static_cast<WheelTV *>(wheels[w])->mTrackIndex = (uint)t;
// Angular damping: dw/dt = -c * w
// Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt)
// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
for (VehicleTrack &t : mTracks)
t.mAngularVelocity *= max(0.0f, 1.0f - t.mAngularDamping * inDeltaTime);
}
void TrackedVehicleController::SyncLeftRightTracks()
{
// Apply left to right ratio according to track inertias
VehicleTrack &tl = mTracks[(int)ETrackSide::Left];
VehicleTrack &tr = mTracks[(int)ETrackSide::Right];
if (mLeftRatio * mRightRatio > 0.0f)
{
// Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = -dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks
float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mLeftRatio * tr.mInertia + mRightRatio * tl.mInertia);
tl.mAngularVelocity += impulse * tl.mInertia;
tr.mAngularVelocity -= impulse * tr.mInertia;
}
else
{
// Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks
float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mRightRatio * tl.mInertia - mLeftRatio * tr.mInertia);
tl.mAngularVelocity += impulse * tl.mInertia;
tr.mAngularVelocity += impulse * tr.mInertia;
}
}
void TrackedVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
{
JPH_PROFILE_FUNCTION();
Wheels &wheels = mConstraint.GetWheels();
// Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again)
for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index)
{
WheelTV *w = static_cast<WheelTV *>(wheels[wheel_index]);
w->Update(wheel_index, inDeltaTime, mConstraint);
}
// First calculate engine speed based on speed of all wheels
bool can_engine_apply_torque = false;
if (mTransmission.GetCurrentGear() != 0 && mTransmission.GetClutchFriction() > 1.0e-3f)
{
float transmission_ratio = mTransmission.GetCurrentRatio();
bool forward = transmission_ratio >= 0.0f;
float fastest_wheel_speed = forward? -FLT_MAX : FLT_MAX;
for (const VehicleTrack &t : mTracks)
{
if (forward)
fastest_wheel_speed = max(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio);
else
fastest_wheel_speed = min(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio);
for (uint w : t.mWheels)
if (wheels[w]->HasContact())
{
can_engine_apply_torque = true;
break;
}
}
// Update RPM only if the tracks are connected to the engine
if (fastest_wheel_speed > -FLT_MAX && fastest_wheel_speed < FLT_MAX)
mEngine.SetCurrentRPM(fastest_wheel_speed * mTransmission.GetCurrentRatio() * VehicleEngine::cAngularVelocityToRPM);
}
else
{
// Update engine with damping
mEngine.ApplyDamping(inDeltaTime);
// In auto transmission mode, don't accelerate the engine when switching gears
float forward_input = mTransmission.mMode == ETransmissionMode::Manual? abs(mForwardInput) : 0.0f;
// Engine not connected to wheels, update RPM based on engine inertia alone
mEngine.ApplyTorque(mEngine.GetTorque(forward_input), inDeltaTime);
}
// Update transmission
// Note: only allow switching gears up when the tracks are rolling in the same direction
mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, mLeftRatio * mRightRatio > 0.0f && can_engine_apply_torque);
// Calculate the amount of torque the transmission gives to the differentials
float transmission_ratio = mTransmission.GetCurrentRatio();
float transmission_torque = mTransmission.GetClutchFriction() * transmission_ratio * mEngine.GetTorque(abs(mForwardInput));
if (transmission_torque != 0.0f)
{
// Apply the transmission torque to the wheels
for (uint i = 0; i < std::size(mTracks); ++i)
{
VehicleTrack &t = mTracks[i];
// Get wheel rotation ratio for this track
float ratio = i == 0? mLeftRatio : mRightRatio;
// Calculate the max angular velocity of the driven wheel of the track given current engine RPM
// Note this adds 0.1% slop to avoid numerical accuracy issues
float track_max_angular_velocity = mEngine.GetCurrentRPM() / (transmission_ratio * t.mDifferentialRatio * ratio * VehicleEngine::cAngularVelocityToRPM) * 1.001f;
// Calculate torque on the driven wheel
float differential_torque = t.mDifferentialRatio * ratio * transmission_torque;
// Apply torque to driven wheel
if (t.mAngularVelocity * track_max_angular_velocity < 0.0f || abs(t.mAngularVelocity) < abs(track_max_angular_velocity))
t.mAngularVelocity += differential_torque * inDeltaTime / t.mInertia;
}
}
// Ensure that we have the correct ratio between the two tracks
SyncLeftRightTracks();
// Braking
for (VehicleTrack &t : mTracks)
{
// Calculate brake torque
float brake_torque = mBrakeInput * t.mMaxBrakeTorque;
if (brake_torque > 0.0f)
{
// Calculate how much torque is needed to stop the track from rotating in this time step
float brake_torque_to_lock_track = abs(t.mAngularVelocity) * t.mInertia / inDeltaTime;
if (brake_torque > brake_torque_to_lock_track)
{
// Wheels are locked
t.mAngularVelocity = 0.0f;
brake_torque -= brake_torque_to_lock_track;
}
else
{
// Slow down the track
t.mAngularVelocity -= Sign(t.mAngularVelocity) * brake_torque * inDeltaTime / t.mInertia;
}
}
if (brake_torque > 0.0f)
{
// Sum the radius of all wheels touching the floor
float total_radius = 0.0f;
for (uint wheel_index : t.mWheels)
{
const WheelTV *w = static_cast<WheelTV *>(wheels[wheel_index]);
if (w->HasContact())
total_radius += w->GetSettings()->mRadius;
}
if (total_radius > 0.0f)
{
brake_torque /= total_radius;
for (uint wheel_index : t.mWheels)
{
WheelTV *w = static_cast<WheelTV *>(wheels[wheel_index]);
if (w->HasContact())
{
// Impulse: p = F * dt = Torque / Wheel_Radius * dt, Torque = Total_Torque * Wheel_Radius / Summed_Radius => p = Total_Torque * dt / Summed_Radius
w->mBrakeImpulse = brake_torque * inDeltaTime;
}
}
}
}
}
// Update wheel angular velocity based on that of the track
for (Wheel *w_base : wheels)
{
WheelTV *w = static_cast<WheelTV *>(w_base);
w->CalculateAngularVelocity(mConstraint);
}
}
bool TrackedVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime)
{
bool impulse = false;
for (Wheel *w_base : mConstraint.GetWheels())
if (w_base->HasContact())
{
WheelTV *w = static_cast<WheelTV *>(w_base);
const WheelSettingsTV *settings = w->GetSettings();
VehicleTrack &track = mTracks[w->mTrackIndex];
// Calculate max impulse that we can apply on the ground
float max_longitudinal_friction_impulse = w->mCombinedLongitudinalFriction * w->GetSuspensionLambda();
// Calculate relative velocity between wheel contact point and floor in longitudinal direction
Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity();
float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal());
// Calculate brake force to apply
float min_longitudinal_impulse, max_longitudinal_impulse;
if (w->mBrakeImpulse != 0.0f)
{
// Limit brake force by max tire friction
float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse);
// Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle)
if (relative_longitudinal_velocity >= 0.0f)
{
min_longitudinal_impulse = -brake_impulse;
max_longitudinal_impulse = 0.0f;
}
else
{
min_longitudinal_impulse = 0.0f;
max_longitudinal_impulse = brake_impulse;
}
// Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas
impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
}
else
{
// Assume we want to apply an angular impulse that makes the delta velocity between track and ground zero in one time step, calculate the amount of linear impulse needed to do that
float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius;
float linear_impulse = (track.mAngularVelocity - desired_angular_velocity) * track.mInertia / settings->mRadius;
// Limit the impulse by max track friction
float prev_lambda = w->GetLongitudinalLambda();
min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse);
// Longitudinal impulse
impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
// Update the angular velocity of the track according to the lambda that was applied
track.mAngularVelocity -= (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / track.mInertia;
SyncLeftRightTracks();
}
}
for (Wheel *w_base : mConstraint.GetWheels())
if (w_base->HasContact())
{
WheelTV *w = static_cast<WheelTV *>(w_base);
// Update angular velocity of wheel for the next iteration
w->CalculateAngularVelocity(mConstraint);
// Lateral friction
float max_lateral_friction_impulse = w->mCombinedLateralFriction * w->GetSuspensionLambda();
impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_friction_impulse, max_lateral_friction_impulse);
}
return impulse;
}
#ifdef JPH_DEBUG_RENDERER
void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const
{
float constraint_size = mConstraint.GetDrawConstraintSize();
// Draw RPM
Body *body = mConstraint.GetVehicleBody();
Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp();
RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition;
Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward();
mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM);
// Draw current vehicle state
String status = StringFormat("Forward: %.1f, LRatio: %.1f, RRatio: %.1f, Brake: %.1f\n"
"Gear: %d, Clutch: %.1f, EngineRPM: %.0f, V: %.1f km/h",
(double)mForwardInput, (double)mLeftRatio, (double)mRightRatio, (double)mBrakeInput,
mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6);
inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size);
for (const VehicleTrack &t : mTracks)
{
const WheelTV *w = static_cast<const WheelTV *>(mConstraint.GetWheels()[t.mDrivenWheel]);
const WheelSettings *settings = w->GetSettings();
// Calculate where the suspension attaches to the body in world space
RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass());
DebugRenderer::sInstance->DrawText3D(ws_position, StringFormat("W: %.1f", (double)t.mAngularVelocity), Color::sWhite, constraint_size);
}
RMat44 body_transform = body->GetWorldTransform();
for (const Wheel *w_base : mConstraint.GetWheels())
{
const WheelTV *w = static_cast<const WheelTV *>(w_base);
const WheelSettings *settings = w->GetSettings();
// Calculate where the suspension attaches to the body in world space
RVec3 ws_position = body_transform * settings->mPosition;
Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
// Draw suspension
RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength;
RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength;
inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed);
inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen);
// Draw current length
RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength();
inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size);
// Draw wheel basis
Vec3 wheel_forward, wheel_up, wheel_right;
mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right);
wheel_forward = body_transform.Multiply3x3(wheel_forward);
wheel_up = body_transform.Multiply3x3(wheel_up);
wheel_right = body_transform.Multiply3x3(wheel_right);
Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis);
inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed);
inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen);
inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue);
inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow);
// Draw wheel
RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos);
wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle()));
inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
if (w->HasContact())
{
// Draw contact
inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow);
inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed);
inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue);
DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("S: %.2f", (double)w->GetSuspensionLength()), Color::sWhite, constraint_size);
}
}
}
#endif // JPH_DEBUG_RENDERER
void TrackedVehicleController::SaveState(StateRecorder &inStream) const
{
inStream.Write(mForwardInput);
inStream.Write(mLeftRatio);
inStream.Write(mRightRatio);
inStream.Write(mBrakeInput);
mEngine.SaveState(inStream);
mTransmission.SaveState(inStream);
for (const VehicleTrack &t : mTracks)
t.SaveState(inStream);
}
void TrackedVehicleController::RestoreState(StateRecorder &inStream)
{
inStream.Read(mForwardInput);
inStream.Read(mLeftRatio);
inStream.Read(mRightRatio);
inStream.Read(mBrakeInput);
mEngine.RestoreState(inStream);
mTransmission.RestoreState(inStream);
for (VehicleTrack &t : mTracks)
t.RestoreState(inStream);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,166 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
#include <Jolt/Physics/Vehicle/VehicleController.h>
#include <Jolt/Physics/Vehicle/VehicleEngine.h>
#include <Jolt/Physics/Vehicle/VehicleTransmission.h>
#include <Jolt/Physics/Vehicle/VehicleTrack.h>
JPH_NAMESPACE_BEGIN
class PhysicsSystem;
/// WheelSettings object specifically for TrackedVehicleController
class JPH_EXPORT WheelSettingsTV : public WheelSettings
{
JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsTV)
public:
// See: WheelSettings
virtual void SaveBinaryState(StreamOut &inStream) const override;
virtual void RestoreBinaryState(StreamIn &inStream) override;
float mLongitudinalFriction = 4.0f; ///< Friction in forward direction of tire
float mLateralFriction = 2.0f; ///< Friction in sideways direction of tire
};
/// Wheel object specifically for TrackedVehicleController
class JPH_EXPORT WheelTV : public Wheel
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
explicit WheelTV(const WheelSettingsTV &inWheel);
/// Override GetSettings and cast to the correct class
const WheelSettingsTV * GetSettings() const { return StaticCast<WheelSettingsTV>(mSettings); }
/// Update the angular velocity of the wheel based on the angular velocity of the track
void CalculateAngularVelocity(const VehicleConstraint &inConstraint);
/// Update the wheel rotation based on the current angular velocity
void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint);
int mTrackIndex = -1; ///< Index in mTracks to which this wheel is attached (calculated on initialization)
float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and track)
float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and track)
float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction), spread out from brake impulse applied on track
};
/// Settings of a vehicle with tank tracks
///
/// Default settings are based around what I could find about the M1 Abrams tank.
/// Note to avoid issues with very heavy objects vs very light objects the mass of the tank should be a lot lower (say 10x) than that of a real tank. That means that the engine/brake torque is also 10x less.
class JPH_EXPORT TrackedVehicleControllerSettings : public VehicleControllerSettings
{
JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TrackedVehicleControllerSettings)
public:
// Constructor
TrackedVehicleControllerSettings();
// See: VehicleControllerSettings
virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override;
virtual void SaveBinaryState(StreamOut &inStream) const override;
virtual void RestoreBinaryState(StreamIn &inStream) override;
VehicleEngineSettings mEngine; ///< The properties of the engine
VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box)
VehicleTrackSettings mTracks[(int)ETrackSide::Num]; ///< List of tracks and their properties
};
/// Runtime controller class for vehicle with tank tracks
class JPH_EXPORT TrackedVehicleController : public VehicleController
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint);
/// Set input from driver
/// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
/// @param inLeftRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering)
/// @param inRightRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering)
/// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed
void SetDriverInput(float inForward, float inLeftRatio, float inRightRatio, float inBrake) { JPH_ASSERT(inLeftRatio != 0.0f && inRightRatio != 0.0f); mForwardInput = inForward; mLeftRatio = inLeftRatio; mRightRatio = inRightRatio; mBrakeInput = inBrake; }
/// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
void SetForwardInput(float inForward) { mForwardInput = inForward; }
float GetForwardInput() const { return mForwardInput; }
/// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering)
void SetLeftRatio(float inLeftRatio) { JPH_ASSERT(inLeftRatio != 0.0f); mLeftRatio = inLeftRatio; }
float GetLeftRatio() const { return mLeftRatio; }
/// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering)
void SetRightRatio(float inRightRatio) { JPH_ASSERT(inRightRatio != 0.0f); mRightRatio = inRightRatio; }
float GetRightRatio() const { return mRightRatio; }
/// Value between 0 and 1 indicating how strong the brake pedal is pressed
void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; }
float GetBrakeInput() const { return mBrakeInput; }
/// Get current engine state
const VehicleEngine & GetEngine() const { return mEngine; }
/// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
VehicleEngine & GetEngine() { return mEngine; }
/// Get current transmission state
const VehicleTransmission & GetTransmission() const { return mTransmission; }
/// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
VehicleTransmission & GetTransmission() { return mTransmission; }
/// Get the tracks this vehicle has
const VehicleTracks & GetTracks() const { return mTracks; }
/// Get the tracks this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step)
VehicleTracks & GetTracks() { return mTracks; }
#ifdef JPH_DEBUG_RENDERER
/// Debug drawing of RPM meter
void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; }
#endif // JPH_DEBUG_RENDERER
protected:
/// Synchronize angular velocities of left and right tracks according to their ratios
void SyncLeftRightTracks();
// See: VehicleController
virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsTV))); return new WheelTV(static_cast<const WheelSettingsTV &>(inWheel)); }
virtual bool AllowSleep() const override;
virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
virtual void SaveState(StateRecorder &inStream) const override;
virtual void RestoreState(StateRecorder &inStream) override;
#ifdef JPH_DEBUG_RENDERER
virtual void Draw(DebugRenderer *inRenderer) const override;
#endif // JPH_DEBUG_RENDERER
// Control information
float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
float mLeftRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering)
float mRightRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering)
float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed
// Simulation information
VehicleEngine mEngine; ///< Engine state of the vehicle
VehicleTransmission mTransmission; ///< Transmission state of the vehicle
VehicleTracks mTracks; ///< Tracks of the vehicle
#ifdef JPH_DEBUG_RENDERER
// Debug settings
Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint
float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint
#endif // JPH_DEBUG_RENDERER
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,33 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleAntiRollBar.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleAntiRollBar)
{
JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mLeftWheel)
JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mRightWheel)
JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mStiffness)
}
void VehicleAntiRollBar::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mLeftWheel);
inStream.Write(mRightWheel);
inStream.Write(mStiffness);
}
void VehicleAntiRollBar::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mLeftWheel);
inStream.Read(mRightWheel);
inStream.Read(mStiffness);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,33 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
JPH_NAMESPACE_BEGIN
/// An anti rollbar is a stiff spring that connects two wheels to reduce the amount of roll the vehicle makes in sharp corners
/// See: https://en.wikipedia.org/wiki/Anti-roll_bar
class JPH_EXPORT VehicleAntiRollBar
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleAntiRollBar)
public:
/// Saves the contents in binary form to inStream.
void SaveBinaryState(StreamOut &inStream) const;
/// Restores the contents in binary form to inStream.
void RestoreBinaryState(StreamIn &inStream);
int mLeftWheel = 0; ///< Index (in mWheels) that represents the left wheel of this anti-rollbar
int mRightWheel = 1; ///< Index (in mWheels) that represents the right wheel of this anti-rollbar
float mStiffness = 1000.0f; ///< Stiffness (spring constant in N/m) of anti rollbar, can be 0 to disable the anti-rollbar
};
using VehicleAntiRollBars = Array<VehicleAntiRollBar>;
JPH_NAMESPACE_END

View File

@@ -0,0 +1,376 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleCollisionTester.h>
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
#include <Jolt/Physics/Collision/RayCast.h>
#include <Jolt/Physics/Collision/ShapeCast.h>
#include <Jolt/Physics/Collision/CastResult.h>
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
#include <Jolt/Physics/PhysicsSystem.h>
JPH_NAMESPACE_BEGIN
bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
{
const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
float wheel_radius = wheel_settings->mRadius;
float ray_length = wheel_settings->mSuspensionMaxLength + wheel_radius;
RRayCast ray { inOrigin, ray_length * inDirection };
class MyCollector : public CastRayCollector
{
public:
MyCollector(PhysicsSystem &inPhysicsSystem, const RRayCast &inRay, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
mPhysicsSystem(inPhysicsSystem),
mRay(inRay),
mUpDirection(inUpDirection),
mCosMaxSlopeAngle(inCosMaxSlopeAngle)
{
}
virtual void AddHit(const RayCastResult &inResult) override
{
// Test if this collision is closer than the previous one
if (inResult.mFraction < GetEarlyOutFraction())
{
// Lock the body
BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID);
JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
const Body *body = &lock.GetBody();
if (body->IsSensor())
return;
// Test that we're not hitting a vertical wall
RVec3 contact_pos = mRay.GetPointOnRay(inResult.mFraction);
Vec3 normal = body->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, contact_pos);
if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
{
// Update early out fraction to this hit
UpdateEarlyOutFraction(inResult.mFraction);
// Get the contact properties
mBody = body;
mSubShapeID2 = inResult.mSubShapeID2;
mContactPosition = contact_pos;
mContactNormal = normal;
}
}
}
// Configuration
PhysicsSystem & mPhysicsSystem;
RRayCast mRay;
Vec3 mUpDirection;
float mCosMaxSlopeAngle;
// Resulting closest collision
const Body * mBody = nullptr;
SubShapeID mSubShapeID2;
RVec3 mContactPosition;
Vec3 mContactNormal;
};
RayCastSettings settings;
MyCollector collector(inPhysicsSystem, ray, mUp, mCosMaxSlopeAngle);
inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphase_layer_filter, object_layer_filter, body_filter);
if (collector.mBody == nullptr)
return false;
outBody = const_cast<Body *>(collector.mBody);
outSubShapeID = collector.mSubShapeID2;
outContactPosition = collector.mContactPosition;
outContactNormal = collector.mContactNormal;
outSuspensionLength = max(0.0f, ray_length * collector.GetEarlyOutFraction() - wheel_radius);
return true;
}
void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
{
// Recalculate the contact points assuming the contact point is on an infinite plane
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
float d_dot_n = inDirection.Dot(ioContactNormal);
if (d_dot_n < -1.0e-6f)
{
// Reproject the contact position using the suspension ray and the plane formed by the contact position and normal
ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection;
// The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius
ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
}
else
{
// If the normal is pointing away we assume there's no collision anymore
ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
}
}
bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
{
const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
SphereShape sphere(mRadius);
sphere.SetEmbedded();
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
float wheel_radius = wheel_settings->mRadius;
float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius;
RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length);
ShapeCastSettings settings;
settings.mUseShrunkenShapeAndConvexRadius = true;
settings.mReturnDeepestPoint = true;
class MyCollector : public CastShapeCollector
{
public:
MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
mPhysicsSystem(inPhysicsSystem),
mShapeCast(inShapeCast),
mUpDirection(inUpDirection),
mCosMaxSlopeAngle(inCosMaxSlopeAngle)
{
}
virtual void AddHit(const ShapeCastResult &inResult) override
{
// Test if this collision is closer/deeper than the previous one
float early_out = inResult.GetEarlyOutFraction();
if (early_out < GetEarlyOutFraction())
{
// Lock the body
BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
const Body *body = &lock.GetBody();
if (body->IsSensor())
return;
// Test that we're not hitting a vertical wall
Vec3 normal = -inResult.mPenetrationAxis.Normalized();
if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
{
// Update early out fraction to this hit
UpdateEarlyOutFraction(early_out);
// Get the contact properties
mBody = body;
mSubShapeID2 = inResult.mSubShapeID2;
mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
mContactNormal = normal;
mFraction = inResult.mFraction;
}
}
}
// Configuration
PhysicsSystem & mPhysicsSystem;
const RShapeCast & mShapeCast;
Vec3 mUpDirection;
float mCosMaxSlopeAngle;
// Resulting closest collision
const Body * mBody = nullptr;
SubShapeID mSubShapeID2;
RVec3 mContactPosition;
Vec3 mContactNormal;
float mFraction;
};
MyCollector collector(inPhysicsSystem, shape_cast, mUp, mCosMaxSlopeAngle);
inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
if (collector.mBody == nullptr)
return false;
outBody = const_cast<Body *>(collector.mBody);
outSubShapeID = collector.mSubShapeID2;
outContactPosition = collector.mContactPosition;
outContactNormal = collector.mContactNormal;
outSuspensionLength = max(0.0f, shape_cast_length * collector.mFraction + mRadius - wheel_radius);
return true;
}
void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
{
// Recalculate the contact points assuming the contact point is on an infinite plane
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
float d_dot_n = inDirection.Dot(ioContactNormal);
if (d_dot_n < -1.0e-6f)
{
// Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal
// This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction
float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal);
float fraction = (mRadius + oc_dot_n) / d_dot_n;
ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal;
// Calculate the new suspension length in the same way as the cast sphere normally does
ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
}
else
{
// If the normal is pointing away we assume there's no collision anymore
ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
}
}
bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
{
const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
float max_suspension_length = wheel_settings->mSuspensionMaxLength;
// Get the wheel transform given that the cylinder rotates around the Y axis
RMat44 shape_cast_start = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
shape_cast_start.SetTranslation(inOrigin);
// Construct a cylinder with the dimensions of the wheel
float wheel_half_width = 0.5f * wheel_settings->mWidth;
CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction);
cylinder.SetEmbedded();
RShapeCast shape_cast(&cylinder, Vec3::sOne(), shape_cast_start, inDirection * max_suspension_length);
ShapeCastSettings settings;
settings.mUseShrunkenShapeAndConvexRadius = true;
settings.mReturnDeepestPoint = true;
class MyCollector : public CastShapeCollector
{
public:
MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast) :
mPhysicsSystem(inPhysicsSystem),
mShapeCast(inShapeCast)
{
}
virtual void AddHit(const ShapeCastResult &inResult) override
{
// Test if this collision is closer/deeper than the previous one
float early_out = inResult.GetEarlyOutFraction();
if (early_out < GetEarlyOutFraction())
{
// Lock the body
BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
const Body *body = &lock.GetBody();
if (body->IsSensor())
return;
// Update early out fraction to this hit
UpdateEarlyOutFraction(early_out);
// Get the contact properties
mBody = body;
mSubShapeID2 = inResult.mSubShapeID2;
mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
mContactNormal = -inResult.mPenetrationAxis.Normalized();
mFraction = inResult.mFraction;
}
}
// Configuration
PhysicsSystem & mPhysicsSystem;
const RShapeCast & mShapeCast;
// Resulting closest collision
const Body * mBody = nullptr;
SubShapeID mSubShapeID2;
RVec3 mContactPosition;
Vec3 mContactNormal;
float mFraction;
};
MyCollector collector(inPhysicsSystem, shape_cast);
inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
if (collector.mBody == nullptr)
return false;
outBody = const_cast<Body *>(collector.mBody);
outSubShapeID = collector.mSubShapeID2;
outContactPosition = collector.mContactPosition;
outContactNormal = collector.mContactNormal;
outSuspensionLength = max_suspension_length * collector.mFraction;
return true;
}
void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
{
// Recalculate the contact points assuming the contact point is on an infinite plane
const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
float d_dot_n = inDirection.Dot(ioContactNormal);
if (d_dot_n < -1.0e-6f)
{
// Wheel size
float half_width = 0.5f * wheel_settings->mWidth;
float radius = wheel_settings->mRadius;
// Get the inverse local space contact normal for a cylinder pointing along Y
RMat44 wheel_transform = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
Vec3 inverse_local_normal = -wheel_transform.Multiply3x3Transposed(ioContactNormal);
// Get the support point of this normal in local space of the cylinder
// See CylinderShape::Cylinder::GetSupport
float x = inverse_local_normal.GetX(), y = inverse_local_normal.GetY(), z = inverse_local_normal.GetZ();
float o = sqrt(Square(x) + Square(z));
Vec3 support_point;
if (o > 0.0f)
support_point = Vec3((radius * x) / o, Sign(y) * half_width, (radius * z) / o);
else
support_point = Vec3(0, Sign(y) * half_width, 0);
// Rotate back to world space
support_point = wheel_transform.Multiply3x3(support_point);
// Now we can use inOrigin + support_point as the start of a ray of our suspension to the contact plane
// as know that it is the first point on the wheel that will hit the plane
RVec3 origin = inOrigin + support_point;
// Calculate contact position and suspension length, the is the same as VehicleCollisionTesterRay
// but we don't need to take the radius into account anymore
Vec3 oc(ioContactPosition - origin);
ioContactPosition = origin + oc.Dot(ioContactNormal) / d_dot_n * inDirection;
ioSuspensionLength = Clamp(oc.Dot(inDirection), 0.0f, wheel_settings->mSuspensionMaxLength);
}
else
{
// If the normal is pointing away we assume there's no collision anymore
ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
}
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,146 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Core/NonCopyable.h>
JPH_NAMESPACE_BEGIN
class PhysicsSystem;
class VehicleConstraint;
class BroadPhaseLayerFilter;
class ObjectLayerFilter;
class BodyFilter;
/// Class that does collision detection between wheels and ground
class JPH_EXPORT VehicleCollisionTester : public RefTarget<VehicleCollisionTester>, public NonCopyable
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructors
VehicleCollisionTester() = default;
explicit VehicleCollisionTester(ObjectLayer inObjectLayer) : mObjectLayer(inObjectLayer) { }
/// Virtual destructor
virtual ~VehicleCollisionTester() = default;
/// Object layer to use for collision detection, this is used when the filters are not overridden
ObjectLayer GetObjectLayer() const { return mObjectLayer; }
void SetObjectLayer(ObjectLayer inObjectLayer) { mObjectLayer = inObjectLayer; }
/// Access to the broad phase layer filter, when set this overrides the object layer supplied in the constructor
void SetBroadPhaseLayerFilter(const BroadPhaseLayerFilter *inFilter) { mBroadPhaseLayerFilter = inFilter; }
const BroadPhaseLayerFilter * GetBroadPhaseLayerFilter() const { return mBroadPhaseLayerFilter; }
/// Access to the object layer filter, when set this overrides the object layer supplied in the constructor
void SetObjectLayerFilter(const ObjectLayerFilter *inFilter) { mObjectLayerFilter = inFilter; }
const ObjectLayerFilter * GetObjectLayerFilter() const { return mObjectLayerFilter; }
/// Access to the body filter, when set this overrides the default filter that filters out the vehicle body
void SetBodyFilter(const BodyFilter *inFilter) { mBodyFilter = inFilter; }
const BodyFilter * GetBodyFilter() const { return mBodyFilter; }
/// Do a collision test with the world
/// @param inPhysicsSystem The physics system that should be tested against
/// @param inVehicleConstraint The vehicle constraint
/// @param inWheelIndex Index of the wheel that we're testing collision for
/// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point
/// @param inDirection Direction for the test (unit vector, world space)
/// @param inVehicleBodyID This body should be filtered out during collision detection to avoid self collisions
/// @param outBody Body that the wheel collided with
/// @param outSubShapeID Sub shape ID that the wheel collided with
/// @param outContactPosition Contact point between wheel and floor, in world space
/// @param outContactNormal Contact normal between wheel and floor, pointing away from the floor
/// @param outSuspensionLength New length of the suspension [0, inSuspensionMaxLength]
/// @return True when collision found, false if not
virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const = 0;
/// Do a cheap contact properties prediction based on the contact properties from the last collision test (provided as input parameters)
/// @param inPhysicsSystem The physics system that should be tested against
/// @param inVehicleConstraint The vehicle constraint
/// @param inWheelIndex Index of the wheel that we're testing collision for
/// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point
/// @param inDirection Direction for the test (unit vector, world space)
/// @param inVehicleBodyID The body ID for the vehicle itself
/// @param ioBody Body that the wheel previously collided with
/// @param ioSubShapeID Sub shape ID that the wheel collided with during the last check
/// @param ioContactPosition Contact point between wheel and floor during the last check, in world space
/// @param ioContactNormal Contact normal between wheel and floor during the last check, pointing away from the floor
/// @param ioSuspensionLength New length of the suspension [0, inSuspensionMaxLength]
virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const = 0;
protected:
const BroadPhaseLayerFilter * mBroadPhaseLayerFilter = nullptr;
const ObjectLayerFilter * mObjectLayerFilter = nullptr;
const BodyFilter * mBodyFilter = nullptr;
ObjectLayer mObjectLayer = cObjectLayerInvalid;
};
/// Collision tester that tests collision using a raycast
class JPH_EXPORT VehicleCollisionTesterRay : public VehicleCollisionTester
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
/// @param inObjectLayer Object layer to test collision with
/// @param inUp World space up vector, used to avoid colliding with vertical walls.
/// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls.
VehicleCollisionTesterRay(ObjectLayer inObjectLayer, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { }
// See: VehicleCollisionTester
virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
private:
Vec3 mUp;
float mCosMaxSlopeAngle;
};
/// Collision tester that tests collision using a sphere cast
class JPH_EXPORT VehicleCollisionTesterCastSphere : public VehicleCollisionTester
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
/// @param inObjectLayer Object layer to test collision with
/// @param inUp World space up vector, used to avoid colliding with vertical walls.
/// @param inRadius Radius of sphere
/// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls.
VehicleCollisionTesterCastSphere(ObjectLayer inObjectLayer, float inRadius, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mRadius(inRadius), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { }
// See: VehicleCollisionTester
virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
private:
float mRadius;
Vec3 mUp;
float mCosMaxSlopeAngle;
};
/// Collision tester that tests collision using a cylinder shape
class JPH_EXPORT VehicleCollisionTesterCastCylinder : public VehicleCollisionTester
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
/// @param inObjectLayer Object layer to test collision with
/// @param inConvexRadiusFraction Fraction of half the wheel width (or wheel radius if it is smaller) that is used as the convex radius
VehicleCollisionTesterCastCylinder(ObjectLayer inObjectLayer, float inConvexRadiusFraction = 0.1f) : VehicleCollisionTester(inObjectLayer), mConvexRadiusFraction(inConvexRadiusFraction) { JPH_ASSERT(mConvexRadiusFraction >= 0.0f && mConvexRadiusFraction <= 1.0f); }
// See: VehicleCollisionTester
virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
private:
float mConvexRadiusFraction;
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,694 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
#include <Jolt/Physics/Vehicle/VehicleController.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#include <Jolt/Core/Factory.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(VehicleConstraintSettings)
{
JPH_ADD_BASE_CLASS(VehicleConstraintSettings, ConstraintSettings)
JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mUp)
JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mForward)
JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mMaxPitchRollAngle)
JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mWheels)
JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mAntiRollBars)
JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mController)
}
void VehicleConstraintSettings::SaveBinaryState(StreamOut &inStream) const
{
ConstraintSettings::SaveBinaryState(inStream);
inStream.Write(mUp);
inStream.Write(mForward);
inStream.Write(mMaxPitchRollAngle);
uint32 num_anti_rollbars = (uint32)mAntiRollBars.size();
inStream.Write(num_anti_rollbars);
for (const VehicleAntiRollBar &r : mAntiRollBars)
r.SaveBinaryState(inStream);
uint32 num_wheels = (uint32)mWheels.size();
inStream.Write(num_wheels);
for (const WheelSettings *w : mWheels)
w->SaveBinaryState(inStream);
inStream.Write(mController->GetRTTI()->GetHash());
mController->SaveBinaryState(inStream);
}
void VehicleConstraintSettings::RestoreBinaryState(StreamIn &inStream)
{
ConstraintSettings::RestoreBinaryState(inStream);
inStream.Read(mUp);
inStream.Read(mForward);
inStream.Read(mMaxPitchRollAngle);
uint32 num_anti_rollbars = 0;
inStream.Read(num_anti_rollbars);
mAntiRollBars.resize(num_anti_rollbars);
for (VehicleAntiRollBar &r : mAntiRollBars)
r.RestoreBinaryState(inStream);
uint32 num_wheels = 0;
inStream.Read(num_wheels);
mWheels.resize(num_wheels);
for (WheelSettings *w : mWheels)
w->RestoreBinaryState(inStream);
uint32 hash = 0;
inStream.Read(hash);
const RTTI *rtti = Factory::sInstance->Find(hash);
mController = reinterpret_cast<VehicleControllerSettings *>(rtti->CreateObject());
mController->RestoreBinaryState(inStream);
}
VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings) :
Constraint(inSettings),
mBody(&inVehicleBody),
mForward(inSettings.mForward),
mUp(inSettings.mUp),
mWorldUp(inSettings.mUp),
mAntiRollBars(inSettings.mAntiRollBars)
{
// Check sanity of incoming settings
JPH_ASSERT(inSettings.mUp.IsNormalized());
JPH_ASSERT(inSettings.mForward.IsNormalized());
JPH_ASSERT(!inSettings.mWheels.empty());
// Store max pitch/roll angle
SetMaxPitchRollAngle(inSettings.mMaxPitchRollAngle);
// Construct our controller class
mController = inSettings.mController->ConstructController(*this);
// Create wheels
mWheels.resize(inSettings.mWheels.size());
for (uint i = 0; i < mWheels.size(); ++i)
mWheels[i] = mController->ConstructWheel(*inSettings.mWheels[i]);
// Use the body ID as a seed for the step counter so that not all vehicles will update at the same time
mCurrentStep = uint32(Hash64(inVehicleBody.GetID().GetIndex()));
}
VehicleConstraint::~VehicleConstraint()
{
// Destroy controller
delete mController;
// Destroy our wheels
for (Wheel *w : mWheels)
delete w;
}
void VehicleConstraint::GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const
{
const WheelSettings *settings = inWheel->mSettings;
Quat steer_rotation = Quat::sRotation(settings->mSteeringAxis, inWheel->mSteerAngle);
outUp = steer_rotation * settings->mWheelUp;
outForward = steer_rotation * settings->mWheelForward;
outRight = outForward.Cross(outUp).Normalized();
outForward = outUp.Cross(outRight).Normalized();
}
Mat44 VehicleConstraint::GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const
{
JPH_ASSERT(inWheelIndex < mWheels.size());
const Wheel *wheel = mWheels[inWheelIndex];
const WheelSettings *settings = wheel->mSettings;
// Use the two vectors provided to calculate a matrix that takes us from wheel model space to X = right, Y = up, Z = forward (the space where we will rotate the wheel)
Mat44 wheel_to_rotational = Mat44(Vec4(inWheelRight, 0), Vec4(inWheelUp, 0), Vec4(inWheelUp.Cross(inWheelRight), 0), Vec4(0, 0, 0, 1)).Transposed();
// Calculate the matrix that takes us from the rotational space to vehicle local space
Vec3 local_forward, local_up, local_right;
GetWheelLocalBasis(wheel, local_forward, local_up, local_right);
Vec3 local_wheel_pos = settings->mPosition + settings->mSuspensionDirection * wheel->mSuspensionLength;
Mat44 rotational_to_local(Vec4(local_right, 0), Vec4(local_up, 0), Vec4(local_forward, 0), Vec4(local_wheel_pos, 1));
// Calculate transform of rotated wheel
return rotational_to_local * Mat44::sRotationX(wheel->mAngle) * wheel_to_rotational;
}
RMat44 VehicleConstraint::GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const
{
return mBody->GetWorldTransform() * GetWheelLocalTransform(inWheelIndex, inWheelRight, inWheelUp);
}
void VehicleConstraint::OnStep(const PhysicsStepListenerContext &inContext)
{
JPH_PROFILE_FUNCTION();
// Callback to higher-level systems. We do it before PreCollide, in case steering changes.
if (mPreStepCallback != nullptr)
mPreStepCallback(*this, inContext);
if (mIsGravityOverridden)
{
// If gravity is overridden, we replace the normal gravity calculations
if (mBody->IsActive())
{
MotionProperties *mp = mBody->GetMotionProperties();
mp->SetGravityFactor(0.0f);
mBody->AddForce(mGravityOverride / mp->GetInverseMass());
}
// And we calculate the world up using the custom gravity
mWorldUp = (-mGravityOverride).NormalizedOr(mWorldUp);
}
else
{
// Calculate new world up vector by inverting gravity
mWorldUp = (-inContext.mPhysicsSystem->GetGravity()).NormalizedOr(mWorldUp);
}
// Callback on our controller
mController->PreCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem);
// Calculate if this constraint is active by checking if our main vehicle body is active or any of the bodies we touch are active
mIsActive = mBody->IsActive();
// Test how often we need to update the wheels
uint num_steps_between_collisions = mIsActive? mNumStepsBetweenCollisionTestActive : mNumStepsBetweenCollisionTestInactive;
RMat44 body_transform = mBody->GetWorldTransform();
// Test collision for wheels
for (uint wheel_index = 0; wheel_index < mWheels.size(); ++wheel_index)
{
Wheel *w = mWheels[wheel_index];
const WheelSettings *settings = w->mSettings;
// Calculate suspension origin and direction
RVec3 ws_origin = body_transform * settings->mPosition;
Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
// Test if we need to update this wheel
if (num_steps_between_collisions == 0
|| (mCurrentStep + wheel_index) % num_steps_between_collisions != 0)
{
// Simplified wheel contact test
if (!w->mContactBodyID.IsInvalid())
{
// Test if the body is still valid
w->mContactBody = inContext.mPhysicsSystem->GetBodyLockInterfaceNoLock().TryGetBody(w->mContactBodyID);
if (w->mContactBody == nullptr)
{
// It's not, forget the contact
w->mContactBodyID = BodyID();
w->mContactSubShapeID = SubShapeID();
w->mSuspensionLength = settings->mSuspensionMaxLength;
}
else
{
// Extrapolate the wheel contact properties
mVehicleCollisionTester->PredictContactProperties(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength);
}
}
}
else
{
// Full wheel contact test, start by resetting the contact data
w->mContactBodyID = BodyID();
w->mContactBody = nullptr;
w->mContactSubShapeID = SubShapeID();
w->mSuspensionLength = settings->mSuspensionMaxLength;
// Test collision to find the floor
if (mVehicleCollisionTester->Collide(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength))
{
// Store ID (pointer is not valid outside of the simulation step)
w->mContactBodyID = w->mContactBody->GetID();
}
}
if (w->mContactBody != nullptr)
{
// Store contact velocity, cache this as the contact body may be removed
w->mContactPointVelocity = w->mContactBody->GetPointVelocity(w->mContactPosition);
// Determine plane constant for axle contact plane
w->mAxlePlaneConstant = RVec3(w->mContactNormal).Dot(ws_origin + w->mSuspensionLength * ws_direction);
// Check if body is active, if so the entire vehicle should be active
mIsActive |= w->mContactBody->IsActive();
// Determine world space forward using steering angle and body rotation
Vec3 forward, up, right;
GetWheelLocalBasis(w, forward, up, right);
forward = body_transform.Multiply3x3(forward);
right = body_transform.Multiply3x3(right);
// The longitudinal axis is in the up/forward plane
w->mContactLongitudinal = w->mContactNormal.Cross(right);
// Make sure that the longitudinal axis is aligned with the forward axis
if (w->mContactLongitudinal.Dot(forward) < 0.0f)
w->mContactLongitudinal = -w->mContactLongitudinal;
// Normalize it
w->mContactLongitudinal = w->mContactLongitudinal.NormalizedOr(w->mContactNormal.GetNormalizedPerpendicular());
// The lateral axis is perpendicular to contact normal and longitudinal axis
w->mContactLateral = w->mContactLongitudinal.Cross(w->mContactNormal).Normalized();
}
}
// Callback to higher-level systems. We do it immediately after wheel collision.
if (mPostCollideCallback != nullptr)
mPostCollideCallback(*this, inContext);
// Calculate anti-rollbar impulses
for (const VehicleAntiRollBar &r : mAntiRollBars)
{
JPH_ASSERT(r.mStiffness >= 0.0f);
Wheel *lw = mWheels[r.mLeftWheel];
Wheel *rw = mWheels[r.mRightWheel];
if (lw->mContactBody != nullptr && rw->mContactBody != nullptr)
{
// Calculate the impulse to apply based on the difference in suspension length
float difference = rw->mSuspensionLength - lw->mSuspensionLength;
float impulse = difference * r.mStiffness * inContext.mDeltaTime;
lw->mAntiRollBarImpulse = -impulse;
rw->mAntiRollBarImpulse = impulse;
}
else
{
// When one of the wheels is not on the ground we don't apply any impulses
lw->mAntiRollBarImpulse = rw->mAntiRollBarImpulse = 0.0f;
}
}
// Callback on our controller
mController->PostCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem);
// Callback to higher-level systems. We do it before the sleep section, in case velocities change.
if (mPostStepCallback != nullptr)
mPostStepCallback(*this, inContext);
// If the wheels are rotating, we don't want to go to sleep yet
if (mBody->GetAllowSleeping())
{
bool allow_sleep = mController->AllowSleep();
if (allow_sleep)
for (const Wheel *w : mWheels)
if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f))
{
allow_sleep = false;
break;
}
if (!allow_sleep)
mBody->ResetSleepTimer();
}
// Increment step counter
++mCurrentStep;
}
void VehicleConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager)
{
// Find dynamic bodies that our wheels are touching
BodyID *body_ids = (BodyID *)JPH_STACK_ALLOC((mWheels.size() + 1) * sizeof(BodyID));
int num_bodies = 0;
bool needs_to_activate = false;
for (const Wheel *w : mWheels)
if (w->mContactBody != nullptr)
{
// Avoid adding duplicates
bool duplicate = false;
BodyID id = w->mContactBody->GetID();
for (int i = 0; i < num_bodies; ++i)
if (body_ids[i] == id)
{
duplicate = true;
break;
}
if (duplicate)
continue;
if (w->mContactBody->IsDynamic())
{
body_ids[num_bodies++] = id;
needs_to_activate |= !w->mContactBody->IsActive();
}
}
// Activate bodies, note that if we get here we have already told the system that we're active so that means our main body needs to be active too
if (!mBody->IsActive())
{
// Our main body is not active, activate it too
body_ids[num_bodies] = mBody->GetID();
inBodyManager.ActivateBodies(body_ids, num_bodies + 1);
}
else if (needs_to_activate)
{
// Only activate bodies the wheels are touching
inBodyManager.ActivateBodies(body_ids, num_bodies);
}
// Link the bodies into the same island
uint32 min_active_index = Body::cInactiveIndex;
for (int i = 0; i < num_bodies; ++i)
{
const Body &body = inBodyManager.GetBody(body_ids[i]);
min_active_index = min(min_active_index, body.GetIndexInActiveBodiesInternal());
ioBuilder.LinkBodies(mBody->GetIndexInActiveBodiesInternal(), body.GetIndexInActiveBodiesInternal());
}
// Link the constraint in the island
ioBuilder.LinkConstraint(inConstraintIndex, mBody->GetIndexInActiveBodiesInternal(), min_active_index);
}
uint VehicleConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const
{
return ioSplitter.AssignToNonParallelSplit(mBody);
}
void VehicleConstraint::CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const
{
// Determine point to apply force to
RVec3 force_point;
if (inWheel.mSettings->mEnableSuspensionForcePoint)
force_point = mBody->GetWorldTransform() * inWheel.mSettings->mSuspensionForcePoint;
else
force_point = inWheel.mContactPosition;
// Calculate r1 + u and r2
outR1PlusU = Vec3(force_point - mBody->GetCenterOfMassPosition());
outR2 = Vec3(force_point - inWheel.mContactBody->GetCenterOfMassPosition());
}
void VehicleConstraint::CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform)
{
// Check if a limit was specified
if (mCosMaxPitchRollAngle > -1.0f)
{
// Calculate cos of angle between world up vector and vehicle up vector
Vec3 vehicle_up = inBodyTransform.Multiply3x3(mUp);
mCosPitchRollAngle = mWorldUp.Dot(vehicle_up);
if (mCosPitchRollAngle < mCosMaxPitchRollAngle)
{
// Calculate rotation axis to rotate vehicle towards up
Vec3 rotation_axis = mWorldUp.Cross(vehicle_up);
float len = rotation_axis.Length();
if (len > 0.0f)
mPitchRollRotationAxis = rotation_axis / len;
mPitchRollPart.CalculateConstraintProperties(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis);
}
else
mPitchRollPart.Deactivate();
}
else
mPitchRollPart.Deactivate();
}
void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime)
{
RMat44 body_transform = mBody->GetWorldTransform();
for (Wheel *w : mWheels)
if (w->mContactBody != nullptr)
{
const WheelSettings *settings = w->mSettings;
Vec3 neg_contact_normal = -w->mContactNormal;
Vec3 r1_plus_u, r2;
CalculateSuspensionForcePoint(*w, r1_plus_u, r2);
// Suspension spring
if (settings->mSuspensionMaxLength > settings->mSuspensionMinLength)
{
float stiffness, damping;
if (settings->mSuspensionSpring.mMode == ESpringMode::FrequencyAndDamping)
{
// Calculate effective mass based on vehicle configuration (the stiffness of the spring should not be affected by the dynamics of the vehicle): K = 1 / (J M^-1 J^T)
// Note that if no suspension force point is supplied we don't know where the force is applied so we assume it is applied at average suspension length
Vec3 force_point = settings->mEnableSuspensionForcePoint? settings->mSuspensionForcePoint : settings->mPosition + 0.5f * (settings->mSuspensionMinLength + settings->mSuspensionMaxLength) * settings->mSuspensionDirection;
Vec3 force_point_x_neg_up = force_point.Cross(-mUp);
const MotionProperties *mp = mBody->GetMotionProperties();
float effective_mass = 1.0f / (mp->GetInverseMass() + force_point_x_neg_up.Dot(mp->GetLocalSpaceInverseInertia().Multiply3x3(force_point_x_neg_up)));
// Convert frequency and damping to stiffness and damping
float omega = 2.0f * JPH_PI * settings->mSuspensionSpring.mFrequency;
stiffness = effective_mass * Square(omega);
damping = 2.0f * effective_mass * settings->mSuspensionSpring.mDamping * omega;
}
else
{
// In this case we can simply copy the properties
stiffness = settings->mSuspensionSpring.mStiffness;
damping = settings->mSuspensionSpring.mDamping;
}
// Calculate the damping and frequency of the suspension spring given the angle between the suspension direction and the contact normal
// If the angle between the suspension direction and the inverse of the contact normal is alpha then the force on the spring relates to the force along the contact normal as:
//
// Fspring = Fnormal * cos(alpha)
//
// The spring force is:
//
// Fspring = -k * x
//
// where k is the spring constant and x is the displacement of the spring. So we have:
//
// Fnormal * cos(alpha) = -k * x <=> Fnormal = -k / cos(alpha) * x
//
// So we can see this as a spring with spring constant:
//
// k' = k / cos(alpha)
//
// In the same way the velocity relates like:
//
// Vspring = Vnormal * cos(alpha)
//
// Which results in the modified damping constant c:
//
// c' = c / cos(alpha)
//
// Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much.
Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
float cos_angle = max(0.1f, ws_direction.Dot(neg_contact_normal));
stiffness /= cos_angle;
damping /= cos_angle;
// Get the value of the constraint equation
float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength;
w->mSuspensionPart.CalculateConstraintPropertiesWithStiffnessAndDamping(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, stiffness, damping);
}
else
w->mSuspensionPart.Deactivate();
// Check if we reached the 'max up' position and if so add a hard velocity constraint that stops any further movement in the normal direction
if (w->mSuspensionLength < settings->mSuspensionMinLength)
w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal);
else
w->mSuspensionMaxUpPart.Deactivate();
// Friction and propulsion
w->mLongitudinalPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLongitudinal);
w->mLateralPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLateral);
}
else
{
// No contact -> disable everything
w->mSuspensionPart.Deactivate();
w->mSuspensionMaxUpPart.Deactivate();
w->mLongitudinalPart.Deactivate();
w->mLateralPart.Deactivate();
}
CalculatePitchRollConstraintProperties(body_transform);
}
void VehicleConstraint::ResetWarmStart()
{
for (Wheel *w : mWheels)
{
w->mSuspensionPart.Deactivate();
w->mSuspensionMaxUpPart.Deactivate();
w->mLongitudinalPart.Deactivate();
w->mLateralPart.Deactivate();
}
mPitchRollPart.Deactivate();
}
void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
{
for (Wheel *w : mWheels)
if (w->mContactBody != nullptr)
{
Vec3 neg_contact_normal = -w->mContactNormal;
w->mSuspensionPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio);
w->mSuspensionMaxUpPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio);
w->mLongitudinalPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLongitudinal, 0.0f); // Don't warm start the longitudinal part (the engine/brake force, we don't want to preserve anything from the last frame)
w->mLateralPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLateral, inWarmStartImpulseRatio);
}
mPitchRollPart.WarmStart(*mBody, Body::sFixedToWorld, inWarmStartImpulseRatio);
}
bool VehicleConstraint::SolveVelocityConstraint(float inDeltaTime)
{
bool impulse = false;
// Solve suspension
for (Wheel *w : mWheels)
if (w->mContactBody != nullptr)
{
Vec3 neg_contact_normal = -w->mContactNormal;
// Suspension spring, note that it can only push and not pull
if (w->mSuspensionPart.IsActive())
impulse |= w->mSuspensionPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX);
// When reaching the minimal suspension length only allow forces pushing the bodies away
if (w->mSuspensionMaxUpPart.IsActive())
impulse |= w->mSuspensionMaxUpPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX);
}
// Solve the horizontal movement of the vehicle
impulse |= mController->SolveLongitudinalAndLateralConstraints(inDeltaTime);
// Apply the pitch / roll constraint to avoid the vehicle from toppling over
if (mPitchRollPart.IsActive())
impulse |= mPitchRollPart.SolveVelocityConstraint(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis, 0, FLT_MAX);
return impulse;
}
bool VehicleConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
{
bool impulse = false;
RMat44 body_transform = mBody->GetWorldTransform();
for (Wheel *w : mWheels)
if (w->mContactBody != nullptr)
{
const WheelSettings *settings = w->mSettings;
// Check if we reached the 'max up' position now that the body has possibly moved
// We do this by calculating the axle position at minimum suspension length and making sure it does not go through the
// plane defined by the contact normal and the axle position when the contact happened
// TODO: This assumes that only the vehicle moved and not the ground as we kept the axle contact plane in world space
Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
RVec3 ws_position = body_transform * settings->mPosition;
RVec3 min_suspension_pos = ws_position + settings->mSuspensionMinLength * ws_direction;
float max_up_error = float(RVec3(w->mContactNormal).Dot(min_suspension_pos) - w->mAxlePlaneConstant);
if (max_up_error < 0.0f)
{
Vec3 neg_contact_normal = -w->mContactNormal;
// Recalculate constraint properties since the body may have moved
Vec3 r1_plus_u, r2;
CalculateSuspensionForcePoint(*w, r1_plus_u, r2);
w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal);
impulse |= w->mSuspensionMaxUpPart.SolvePositionConstraint(*mBody, *w->mContactBody, neg_contact_normal, max_up_error, inBaumgarte);
}
}
// Apply the pitch / roll constraint to avoid the vehicle from toppling over
CalculatePitchRollConstraintProperties(body_transform);
if (mPitchRollPart.IsActive())
impulse |= mPitchRollPart.SolvePositionConstraint(*mBody, Body::sFixedToWorld, mCosPitchRollAngle - mCosMaxPitchRollAngle, inBaumgarte);
return impulse;
}
#ifdef JPH_DEBUG_RENDERER
void VehicleConstraint::DrawConstraint(DebugRenderer *inRenderer) const
{
mController->Draw(inRenderer);
}
void VehicleConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
{
}
#endif // JPH_DEBUG_RENDERER
void VehicleConstraint::SaveState(StateRecorder &inStream) const
{
Constraint::SaveState(inStream);
mController->SaveState(inStream);
for (const Wheel *w : mWheels)
{
inStream.Write(w->mAngularVelocity);
inStream.Write(w->mAngle);
inStream.Write(w->mContactBodyID); // Used by MotorcycleController::PreCollide
inStream.Write(w->mContactPosition); // Used by VehicleCollisionTester::PredictContactProperties
inStream.Write(w->mContactNormal); // Used by MotorcycleController::PreCollide
inStream.Write(w->mContactLateral); // Used by MotorcycleController::PreCollide
inStream.Write(w->mSuspensionLength); // Used by VehicleCollisionTester::PredictContactProperties
w->mSuspensionPart.SaveState(inStream);
w->mSuspensionMaxUpPart.SaveState(inStream);
w->mLongitudinalPart.SaveState(inStream);
w->mLateralPart.SaveState(inStream);
}
inStream.Write(mPitchRollRotationAxis); // When rotation is too small we use last frame so we need to store it
mPitchRollPart.SaveState(inStream);
inStream.Write(mCurrentStep);
}
void VehicleConstraint::RestoreState(StateRecorder &inStream)
{
Constraint::RestoreState(inStream);
mController->RestoreState(inStream);
for (Wheel *w : mWheels)
{
inStream.Read(w->mAngularVelocity);
inStream.Read(w->mAngle);
inStream.Read(w->mContactBodyID);
inStream.Read(w->mContactPosition);
inStream.Read(w->mContactNormal);
inStream.Read(w->mContactLateral);
inStream.Read(w->mSuspensionLength);
w->mContactBody = nullptr; // No longer valid
w->mSuspensionPart.RestoreState(inStream);
w->mSuspensionMaxUpPart.RestoreState(inStream);
w->mLongitudinalPart.RestoreState(inStream);
w->mLateralPart.RestoreState(inStream);
}
inStream.Read(mPitchRollRotationAxis);
mPitchRollPart.RestoreState(inStream);
inStream.Read(mCurrentStep);
}
Ref<ConstraintSettings> VehicleConstraint::GetConstraintSettings() const
{
JPH_ASSERT(false); // Not implemented yet
return nullptr;
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,250 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Constraints/Constraint.h>
#include <Jolt/Physics/PhysicsStepListener.h>
#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
#include <Jolt/Physics/Vehicle/VehicleCollisionTester.h>
#include <Jolt/Physics/Vehicle/VehicleAntiRollBar.h>
#include <Jolt/Physics/Vehicle/Wheel.h>
#include <Jolt/Physics/Vehicle/VehicleController.h>
JPH_NAMESPACE_BEGIN
class PhysicsSystem;
/// Configuration for constraint that simulates a wheeled vehicle.
///
/// The properties in this constraint are largely based on "Car Physics for Games" by Marco Monster.
/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html
class JPH_EXPORT VehicleConstraintSettings : public ConstraintSettings
{
JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, VehicleConstraintSettings)
public:
/// Saves the contents of the constraint settings in binary form to inStream.
virtual void SaveBinaryState(StreamOut &inStream) const override;
Vec3 mUp { 0, 1, 0 }; ///< Vector indicating the up direction of the vehicle (in local space to the body)
Vec3 mForward { 0, 0, 1 }; ///< Vector indicating forward direction of the vehicle (in local space to the body)
float mMaxPitchRollAngle = JPH_PI; ///< Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off.
Array<Ref<WheelSettings>> mWheels; ///< List of wheels and their properties
VehicleAntiRollBars mAntiRollBars; ///< List of anti rollbars and their properties
Ref<VehicleControllerSettings> mController; ///< Defines how the vehicle can accelerate / decelerate
protected:
/// This function should not be called directly, it is used by sRestoreFromBinaryState.
virtual void RestoreBinaryState(StreamIn &inStream) override;
};
/// Constraint that simulates a vehicle
/// Note: Don't forget to register the constraint as a StepListener with the PhysicsSystem!
///
/// When the vehicle drives over very light objects (rubble) you may see the car body dip down. This is a known issue and is an artifact of the iterative solver that Jolt is using.
/// Basically if a light object is sandwiched between two heavy objects (the static floor and the car body), the light object is not able to transfer enough force from the ground to
/// the car body to keep the car body up. You can see this effect in the HeavyOnLightTest sample, the boxes on the right have a lot of penetration because they're on top of light objects.
///
/// There are a couple of ways to improve this:
///
/// 1. You can increase the number of velocity steps (global settings PhysicsSettings::mNumVelocitySteps or if you only want to increase it on
/// the vehicle you can use VehicleConstraintSettings::mNumVelocityStepsOverride). E.g. going from 10 to 30 steps in the HeavyOnLightTest sample makes the penetration a lot less.
/// The number of position steps can also be increased (the first prevents the body from going down, the second corrects it if the problem did
/// occur which inevitably happens due to numerical drift). This solution costs CPU cycles.
///
/// 2. You can reduce the mass difference between the vehicle body and the rubble on the floor (by making the rubble heavier or the car lighter).
///
/// 3. You could filter out collisions between the vehicle collision test and the rubble completely. This would make the wheels ignore the rubble but would cause the vehicle to drive
/// through it as if nothing happened. You could create fake wheels (keyframed bodies) that move along with the vehicle and that only collide with rubble (and not the vehicle or the ground).
/// This would cause the vehicle to push away the rubble without the rubble being able to affect the vehicle (unless it hits the main body of course).
///
/// Note that when driving over rubble, you may see the wheel jump up and down quite quickly because one frame a collision is found and the next frame not.
/// To alleviate this, it may be needed to smooth the motion of the visual mesh for the wheel.
class JPH_EXPORT VehicleConstraint : public Constraint, public PhysicsStepListener
{
public:
/// Constructor / destructor
VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings);
virtual ~VehicleConstraint() override;
/// Get the type of a constraint
virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Vehicle; }
/// Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off.
void SetMaxPitchRollAngle(float inMaxPitchRollAngle) { mCosMaxPitchRollAngle = Cos(inMaxPitchRollAngle); }
/// Set the interface that tests collision between wheel and ground
void SetVehicleCollisionTester(const VehicleCollisionTester *inTester) { mVehicleCollisionTester = inTester; }
/// Callback function to combine the friction of a tire with the friction of the body it is colliding with.
/// On input ioLongitudinalFriction and ioLateralFriction contain the friction of the tire, on output they should contain the combined friction with inBody2.
using CombineFunction = function<void(uint inWheelIndex, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &inSubShapeID2)>;
/// Set the function that combines the friction of two bodies and returns it
/// Default method is the geometric mean: sqrt(friction1 * friction2).
void SetCombineFriction(const CombineFunction &inCombineFriction) { mCombineFriction = inCombineFriction; }
const CombineFunction & GetCombineFriction() const { return mCombineFriction; }
/// Callback function to notify of current stage in PhysicsStepListener::OnStep.
using StepCallback = function<void(VehicleConstraint &inVehicle, const PhysicsStepListenerContext &inContext)>;
/// Callback function to notify that PhysicsStepListener::OnStep has started for this vehicle. Default is to do nothing.
/// Can be used to allow higher-level code to e.g. control steering. This is the last moment that the position/orientation of the vehicle can be changed.
/// Wheel collision checks have not been performed yet.
const StepCallback & GetPreStepCallback() const { return mPreStepCallback; }
void SetPreStepCallback(const StepCallback &inPreStepCallback) { mPreStepCallback = inPreStepCallback; }
/// Callback function to notify that PhysicsStepListener::OnStep has just completed wheel collision checks. Default is to do nothing.
/// Can be used to allow higher-level code to e.g. detect tire contact or to modify the velocity of the vehicle based on the wheel contacts.
/// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed.
const StepCallback & GetPostCollideCallback() const { return mPostCollideCallback; }
void SetPostCollideCallback(const StepCallback &inPostCollideCallback) { mPostCollideCallback = inPostCollideCallback; }
/// Callback function to notify that PhysicsStepListener::OnStep has completed for this vehicle. Default is to do nothing.
/// Can be used to allow higher-level code to e.g. control the vehicle in the air.
/// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed.
const StepCallback & GetPostStepCallback() const { return mPostStepCallback; }
void SetPostStepCallback(const StepCallback &inPostStepCallback) { mPostStepCallback = inPostStepCallback; }
/// Override gravity for this vehicle. Note that overriding gravity will set the gravity factor of the vehicle body to 0 and apply gravity in the PhysicsStepListener instead.
void OverrideGravity(Vec3Arg inGravity) { mGravityOverride = inGravity; mIsGravityOverridden = true; }
bool IsGravityOverridden() const { return mIsGravityOverridden; }
Vec3 GetGravityOverride() const { return mGravityOverride; }
void ResetGravityOverride() { mIsGravityOverridden = false; mBody->GetMotionProperties()->SetGravityFactor(1.0f); } ///< Note that resetting the gravity override will restore the gravity factor of the vehicle body to 1.
/// Get the local space forward vector of the vehicle
Vec3 GetLocalForward() const { return mForward; }
/// Get the local space up vector of the vehicle
Vec3 GetLocalUp() const { return mUp; }
/// Vector indicating the world space up direction (used to limit vehicle pitch/roll), calculated every frame by inverting gravity
Vec3 GetWorldUp() const { return mWorldUp; }
/// Access to the vehicle body
Body * GetVehicleBody() const { return mBody; }
/// Access to the vehicle controller interface (determines acceleration / deceleration)
const VehicleController * GetController() const { return mController; }
/// Access to the vehicle controller interface (determines acceleration / deceleration)
VehicleController * GetController() { return mController; }
/// Get the state of the wheels
const Wheels & GetWheels() const { return mWheels; }
/// Get the state of a wheels (writable interface, allows you to make changes to the configuration which will take effect the next time step)
Wheels & GetWheels() { return mWheels; }
/// Get the state of a wheel
Wheel * GetWheel(uint inIdx) { return mWheels[inIdx]; }
const Wheel * GetWheel(uint inIdx) const { return mWheels[inIdx]; }
/// Get the basis vectors for the wheel in local space to the vehicle body (note: basis does not rotate when the wheel rotates around its axis)
/// @param inWheel Wheel to fetch basis for
/// @param outForward Forward vector for the wheel
/// @param outUp Up vector for the wheel
/// @param outRight Right vector for the wheel
void GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const;
/// Get the transform of a wheel in local space to the vehicle body, returns a matrix that transforms a cylinder aligned with the Y axis in body space (not COM space)
/// @param inWheelIndex Index of the wheel to fetch
/// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels)
/// @param inWheelUp Unit vector that indicates up in model space of the wheel
Mat44 GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
/// Get the transform of a wheel in world space, returns a matrix that transforms a cylinder aligned with the Y axis in world space
/// @param inWheelIndex Index of the wheel to fetch
/// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels)
/// @param inWheelUp Unit vector that indicates up in model space of the wheel
RMat44 GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
/// Access to the vehicle's anti roll bars
const VehicleAntiRollBars & GetAntiRollBars() const { return mAntiRollBars; }
VehicleAntiRollBars & GetAntiRollBars() { return mAntiRollBars; }
/// Number of simulation steps between wheel collision tests when the vehicle is active. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc.
/// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion.
/// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle.
/// If you set this to test less than every step, you may see simulation artifacts. This setting can be used to reduce the cost of simulating vehicles in the distance.
void SetNumStepsBetweenCollisionTestActive(uint inSteps) { mNumStepsBetweenCollisionTestActive = inSteps; }
uint GetNumStepsBetweenCollisionTestActive() const { return mNumStepsBetweenCollisionTestActive; }
/// Number of simulation steps between wheel collision tests when the vehicle is inactive. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc.
/// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion.
/// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle.
/// This number can be lower than the number of steps when the vehicle is active as the only purpose of this test is
/// to allow the vehicle to wake up in response to bodies moving into the wheels but not touching the body of the vehicle.
void SetNumStepsBetweenCollisionTestInactive(uint inSteps) { mNumStepsBetweenCollisionTestInactive = inSteps; }
uint GetNumStepsBetweenCollisionTestInactive() const { return mNumStepsBetweenCollisionTestInactive; }
// Generic interface of a constraint
virtual bool IsActive() const override { return mIsActive && Constraint::IsActive(); }
virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ }
virtual void SetupVelocityConstraint(float inDeltaTime) override;
virtual void ResetWarmStart() override;
virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
virtual bool SolveVelocityConstraint(float inDeltaTime) override;
virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override;
virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override;
#ifdef JPH_DEBUG_RENDERER
virtual void DrawConstraint(DebugRenderer *inRenderer) const override;
virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override;
#endif // JPH_DEBUG_RENDERER
virtual void SaveState(StateRecorder &inStream) const override;
virtual void RestoreState(StateRecorder &inStream) override;
virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
private:
// See: PhysicsStepListener
virtual void OnStep(const PhysicsStepListenerContext &inContext) override;
// Calculate the position where the suspension and traction forces should be applied in world space, relative to the center of mass of both bodies
void CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const;
// Calculate the constraint properties for mPitchRollPart
void CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform);
// Gravity override
bool mIsGravityOverridden = false; ///< If the gravity is currently overridden
Vec3 mGravityOverride = Vec3::sZero(); ///< Gravity override value, replaces PhysicsSystem::GetGravity() when mIsGravityOverridden is true
// Simulation information
Body * mBody; ///< Body of the vehicle
Vec3 mForward; ///< Local space forward vector for the vehicle
Vec3 mUp; ///< Local space up vector for the vehicle
Vec3 mWorldUp; ///< Vector indicating the world space up direction (used to limit vehicle pitch/roll)
Wheels mWheels; ///< Wheel states of the vehicle
VehicleAntiRollBars mAntiRollBars; ///< Anti rollbars of the vehicle
VehicleController * mController; ///< Controls the acceleration / deceleration of the vehicle
bool mIsActive = false; ///< If this constraint is active
uint mNumStepsBetweenCollisionTestActive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is active
uint mNumStepsBetweenCollisionTestInactive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is inactive
uint mCurrentStep = 0; ///< Current step number, used to determine when to test a wheel
// Prevent vehicle from toppling over
float mCosMaxPitchRollAngle; ///< Cos of the max pitch/roll angle
float mCosPitchRollAngle; ///< Cos of the current pitch/roll angle
Vec3 mPitchRollRotationAxis { 0, 1, 0 }; ///< Current axis along which to apply torque to prevent the car from toppling over
AngleConstraintPart mPitchRollPart; ///< Constraint part that prevents the car from toppling over
// Interfaces
RefConst<VehicleCollisionTester> mVehicleCollisionTester; ///< Class that performs testing of collision for the wheels
CombineFunction mCombineFriction = [](uint, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &)
{
float body_friction = inBody2.GetFriction();
ioLongitudinalFriction = sqrt(ioLongitudinalFriction * body_friction);
ioLateralFriction = sqrt(ioLateralFriction * body_friction);
};
// Callbacks
StepCallback mPreStepCallback;
StepCallback mPostCollideCallback;
StepCallback mPostStepCallback;
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,17 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleController.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(VehicleControllerSettings)
{
JPH_ADD_BASE_CLASS(VehicleControllerSettings, SerializableObject)
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,84 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
class PhysicsSystem;
class VehicleController;
class VehicleConstraint;
class WheelSettings;
class Wheel;
class StateRecorder;
/// Basic settings object for interface that controls acceleration / deceleration of the vehicle
class JPH_EXPORT VehicleControllerSettings : public SerializableObject, public RefTarget<VehicleControllerSettings>
{
JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, VehicleControllerSettings)
public:
/// Saves the contents of the controller settings in binary form to inStream.
virtual void SaveBinaryState(StreamOut &inStream) const = 0;
/// Restore the contents of the controller settings in binary form from inStream.
virtual void RestoreBinaryState(StreamIn &inStream) = 0;
/// Create an instance of the vehicle controller class
virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const = 0;
};
/// Runtime data for interface that controls acceleration / deceleration of the vehicle
class JPH_EXPORT VehicleController : public NonCopyable
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor / destructor
explicit VehicleController(VehicleConstraint &inConstraint) : mConstraint(inConstraint) { }
virtual ~VehicleController() = default;
/// Access the vehicle constraint that this controller is part of
VehicleConstraint & GetConstraint() { return mConstraint; }
const VehicleConstraint & GetConstraint() const { return mConstraint; }
protected:
// The functions below are only for the VehicleConstraint
friend class VehicleConstraint;
// Create a new instance of wheel
virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const = 0;
// If the vehicle is allowed to go to sleep
virtual bool AllowSleep() const = 0;
// Called before the wheel probes have been done
virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0;
// Called after the wheel probes have been done
virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0;
// Solve longitudinal and lateral constraint parts for all of the wheels
virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) = 0;
// Saving state for replay
virtual void SaveState(StateRecorder &inStream) const = 0;
virtual void RestoreState(StateRecorder &inStream) = 0;
#ifdef JPH_DEBUG_RENDERER
// Drawing interface
virtual void Draw(DebugRenderer *inRenderer) const = 0;
#endif // JPH_DEBUG_RENDERER
VehicleConstraint & mConstraint; ///< The vehicle constraint we belong to
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,81 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleDifferential.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleDifferentialSettings)
{
JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftWheel)
JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mRightWheel)
JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mDifferentialRatio)
JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftRightSplit)
JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLimitedSlipRatio)
JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mEngineTorqueRatio)
}
void VehicleDifferentialSettings::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mLeftWheel);
inStream.Write(mRightWheel);
inStream.Write(mDifferentialRatio);
inStream.Write(mLeftRightSplit);
inStream.Write(mLimitedSlipRatio);
inStream.Write(mEngineTorqueRatio);
}
void VehicleDifferentialSettings::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mLeftWheel);
inStream.Read(mRightWheel);
inStream.Read(mDifferentialRatio);
inStream.Read(mLeftRightSplit);
inStream.Read(mLimitedSlipRatio);
inStream.Read(mEngineTorqueRatio);
}
void VehicleDifferentialSettings::CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const
{
// Start with the default torque ratio
outLeftTorqueFraction = 1.0f - mLeftRightSplit;
outRightTorqueFraction = mLeftRightSplit;
if (mLimitedSlipRatio < FLT_MAX)
{
JPH_ASSERT(mLimitedSlipRatio > 1.0f);
// This is a limited slip differential, adjust torque ratios according to wheel speeds
float omega_l = max(1.0e-3f, abs(inLeftAngularVelocity)); // prevent div by zero by setting a minimum velocity and ignoring that the wheels may be rotating in different directions
float omega_r = max(1.0e-3f, abs(inRightAngularVelocity));
float omega_min = min(omega_l, omega_r);
float omega_max = max(omega_l, omega_r);
// Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mLimitedSlipRotationRatio
float alpha = min((omega_max / omega_min - 1.0f) / (mLimitedSlipRatio - 1.0f), 1.0f);
JPH_ASSERT(alpha >= 0.0f);
float one_min_alpha = 1.0f - alpha;
if (omega_l < omega_r)
{
// Redirect more power to the left wheel
outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha + alpha;
outRightTorqueFraction = outRightTorqueFraction * one_min_alpha;
}
else
{
// Redirect more power to the right wheel
outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha;
outRightTorqueFraction = outRightTorqueFraction * one_min_alpha + alpha;
}
}
// Assert the values add up to 1
JPH_ASSERT(abs(outLeftTorqueFraction + outRightTorqueFraction - 1.0f) < 1.0e-6f);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,39 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
JPH_NAMESPACE_BEGIN
class JPH_EXPORT VehicleDifferentialSettings
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleDifferentialSettings)
public:
/// Saves the contents in binary form to inStream.
void SaveBinaryState(StreamOut &inStream) const;
/// Restores the contents in binary form to inStream.
void RestoreBinaryState(StreamIn &inStream);
/// Calculate the torque ratio between left and right wheel
/// @param inLeftAngularVelocity Angular velocity of left wheel (rad / s)
/// @param inRightAngularVelocity Angular velocity of right wheel (rad / s)
/// @param outLeftTorqueFraction Fraction of torque that should go to the left wheel
/// @param outRightTorqueFraction Fraction of torque that should go to the right wheel
void CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const;
int mLeftWheel = -1; ///< Index (in mWheels) that represents the left wheel of this differential (can be -1 to indicate no wheel)
int mRightWheel = -1; ///< Index (in mWheels) that represents the right wheel of this differential (can be -1 to indicate no wheel)
float mDifferentialRatio = 3.42f; ///< Ratio between rotation speed of gear box and wheels
float mLeftRightSplit = 0.5f; ///< Defines how the engine torque is split across the left and right wheel (0 = left, 0.5 = center, 1 = right)
float mLimitedSlipRatio = 1.4f; ///< Ratio max / min wheel speed. When this ratio is exceeded, all torque gets distributed to the slowest moving wheel. This allows implementing a limited slip differential. Set to FLT_MAX for an open differential. Value should be > 1.
float mEngineTorqueRatio = 1.0f; ///< How much of the engines torque is applied to this differential (0 = none, 1 = full), make sure the sum of all differentials is 1.
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,122 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleEngine.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleEngineSettings)
{
JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxTorque)
JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMinRPM)
JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxRPM)
JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mNormalizedTorque)
}
VehicleEngineSettings::VehicleEngineSettings()
{
mNormalizedTorque.Reserve(3);
mNormalizedTorque.AddPoint(0.0f, 0.8f);
mNormalizedTorque.AddPoint(0.66f, 1.0f);
mNormalizedTorque.AddPoint(1.0f, 0.8f);
}
void VehicleEngineSettings::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mMaxTorque);
inStream.Write(mMinRPM);
inStream.Write(mMaxRPM);
mNormalizedTorque.SaveBinaryState(inStream);
}
void VehicleEngineSettings::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mMaxTorque);
inStream.Read(mMinRPM);
inStream.Read(mMaxRPM);
mNormalizedTorque.RestoreBinaryState(inStream);
}
void VehicleEngine::ApplyTorque(float inTorque, float inDeltaTime)
{
// Accelerate engine using torque
mCurrentRPM += cAngularVelocityToRPM * inTorque * inDeltaTime / mInertia;
ClampRPM();
}
void VehicleEngine::ApplyDamping(float inDeltaTime)
{
// Angular damping: dw/dt = -c * w
// Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt)
// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
mCurrentRPM *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime);
ClampRPM();
}
#ifdef JPH_DEBUG_RENDERER
void VehicleEngine::DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const
{
// Function to draw part of a pie
auto draw_pie = [this, inRenderer, inSize, inPosition, inForward, inUp](float inMinRPM, float inMaxRPM, Color inColor) {
inRenderer->DrawPie(inPosition, inSize, inForward, inUp, ConvertRPMToAngle(inMinRPM), ConvertRPMToAngle(inMaxRPM), inColor, DebugRenderer::ECastShadow::Off);
};
// Draw segment under min RPM
draw_pie(0, mMinRPM, Color::sGrey);
// Draw segment until inShiftDownRPM
if (mCurrentRPM < inShiftDownRPM)
{
draw_pie(mMinRPM, mCurrentRPM, Color::sRed);
draw_pie(mCurrentRPM, inShiftDownRPM, Color::sDarkRed);
}
else
{
draw_pie(mMinRPM, inShiftDownRPM, Color::sRed);
}
// Draw segment between inShiftDownRPM and inShiftUpRPM
if (mCurrentRPM > inShiftDownRPM && mCurrentRPM < inShiftUpRPM)
{
draw_pie(inShiftDownRPM, mCurrentRPM, Color::sOrange);
draw_pie(mCurrentRPM, inShiftUpRPM, Color::sDarkOrange);
}
else
{
draw_pie(inShiftDownRPM, inShiftUpRPM, mCurrentRPM <= inShiftDownRPM? Color::sDarkOrange : Color::sOrange);
}
// Draw segment above inShiftUpRPM
if (mCurrentRPM > inShiftUpRPM)
{
draw_pie(inShiftUpRPM, mCurrentRPM, Color::sGreen);
draw_pie(mCurrentRPM, mMaxRPM, Color::sDarkGreen);
}
else
{
draw_pie(inShiftUpRPM, mMaxRPM, Color::sDarkGreen);
}
}
#endif // JPH_DEBUG_RENDERER
void VehicleEngine::SaveState(StateRecorder &inStream) const
{
inStream.Write(mCurrentRPM);
}
void VehicleEngine::RestoreState(StateRecorder &inStream)
{
inStream.Read(mCurrentRPM);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,93 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/LinearCurve.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#include <Jolt/Physics/StateRecorder.h>
JPH_NAMESPACE_BEGIN
#ifdef JPH_DEBUG_RENDERER
class DebugRenderer;
#endif // JPH_DEBUG_RENDERER
/// Generic properties for a vehicle engine
class JPH_EXPORT VehicleEngineSettings
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleEngineSettings)
public:
/// Constructor
VehicleEngineSettings();
/// Saves the contents in binary form to inStream.
void SaveBinaryState(StreamOut &inStream) const;
/// Restores the contents in binary form to inStream.
void RestoreBinaryState(StreamIn &inStream);
float mMaxTorque = 500.0f; ///< Max amount of torque (Nm) that the engine can deliver
float mMinRPM = 1000.0f; ///< Min amount of revolutions per minute (rpm) the engine can produce without stalling
float mMaxRPM = 6000.0f; ///< Max amount of revolutions per minute (rpm) the engine can generate
LinearCurve mNormalizedTorque; ///< Y-axis: Curve that describes a ratio of the max torque the engine can produce (0 = 0, 1 = mMaxTorque). X-axis: the fraction of the RPM of the engine (0 = mMinRPM, 1 = mMaxRPM)
float mInertia = 0.5f; ///< Moment of inertia (kg m^2) of the engine
float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w
};
/// Runtime data for engine
class JPH_EXPORT VehicleEngine : public VehicleEngineSettings
{
public:
/// Multiply an angular velocity (rad/s) with this value to get rounds per minute (RPM)
static constexpr float cAngularVelocityToRPM = 60.0f / (2.0f * JPH_PI);
/// Clamp the RPM between min and max RPM
inline void ClampRPM() { mCurrentRPM = Clamp(mCurrentRPM, mMinRPM, mMaxRPM); }
/// Current rotation speed of engine in rounds per minute
float GetCurrentRPM() const { return mCurrentRPM; }
/// Update rotation speed of engine in rounds per minute
void SetCurrentRPM(float inRPM) { mCurrentRPM = inRPM; ClampRPM(); }
/// Get current angular velocity of the engine in radians / second
inline float GetAngularVelocity() const { return mCurrentRPM / cAngularVelocityToRPM; }
/// Get the amount of torque (N m) that the engine can supply
/// @param inAcceleration How much the gas pedal is pressed [0, 1]
float GetTorque(float inAcceleration) const { return inAcceleration * mMaxTorque * mNormalizedTorque.GetValue(mCurrentRPM / mMaxRPM); }
/// Apply a torque to the engine rotation speed
/// @param inTorque Torque in N m
/// @param inDeltaTime Delta time in seconds
void ApplyTorque(float inTorque, float inDeltaTime);
/// Update the engine RPM for damping
/// @param inDeltaTime Delta time in seconds
void ApplyDamping(float inDeltaTime);
#ifdef JPH_DEBUG_RENDERER
// Function that converts RPM to an angle in radians for debugging purposes
float ConvertRPMToAngle(float inRPM) const { return (-0.75f + 1.5f * inRPM / mMaxRPM) * JPH_PI; }
/// Debug draw a RPM meter
void DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const;
#endif // JPH_DEBUG_RENDERER
/// If the engine is idle we allow the vehicle to sleep
bool AllowSleep() const { return mCurrentRPM <= 1.01f * mMinRPM; }
/// Saving state for replay
void SaveState(StateRecorder &inStream) const;
void RestoreState(StateRecorder &inStream);
private:
float mCurrentRPM = mMinRPM; ///< Current rotation speed of engine in rounds per minute
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,52 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleTrack.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTrackSettings)
{
JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDrivenWheel)
JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mWheels)
JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mInertia)
JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mAngularDamping)
JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mMaxBrakeTorque)
JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDifferentialRatio)
}
void VehicleTrackSettings::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mDrivenWheel);
inStream.Write(mWheels);
inStream.Write(mInertia);
inStream.Write(mAngularDamping);
inStream.Write(mMaxBrakeTorque);
inStream.Write(mDifferentialRatio);
}
void VehicleTrackSettings::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mDrivenWheel);
inStream.Read(mWheels);
inStream.Read(mInertia);
inStream.Read(mAngularDamping);
inStream.Read(mMaxBrakeTorque);
inStream.Read(mDifferentialRatio);
}
void VehicleTrack::SaveState(StateRecorder &inStream) const
{
inStream.Write(mAngularVelocity);
}
void VehicleTrack::RestoreState(StateRecorder &inStream)
{
inStream.Read(mAngularVelocity);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,56 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/LinearCurve.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#include <Jolt/Physics/StateRecorder.h>
JPH_NAMESPACE_BEGIN
/// On which side of the vehicle the track is located (for steering)
enum class ETrackSide : uint
{
Left = 0,
Right = 1,
Num = 2
};
/// Generic properties for tank tracks
class JPH_EXPORT VehicleTrackSettings
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTrackSettings)
public:
/// Saves the contents in binary form to inStream.
void SaveBinaryState(StreamOut &inStream) const;
/// Restores the contents in binary form to inStream.
void RestoreBinaryState(StreamIn &inStream);
uint mDrivenWheel; ///< Which wheel on the track is connected to the engine
Array<uint> mWheels; ///< Indices of wheels that are inside this track, should include the driven wheel too
float mInertia = 10.0f; ///< Moment of inertia (kg m^2) of the track and its wheels as seen on the driven wheel
float mAngularDamping = 0.5f; ///< Damping factor of track and its wheels: dw/dt = -c * w as seen on the driven wheel
float mMaxBrakeTorque = 15000.0f; ///< How much torque (Nm) the brakes can apply on the driven wheel
float mDifferentialRatio = 6.0f; ///< Ratio between rotation speed of gear box and driven wheel of track
};
/// Runtime data for tank tracks
class JPH_EXPORT VehicleTrack : public VehicleTrackSettings
{
public:
/// Saving state for replay
void SaveState(StateRecorder &inStream) const;
void RestoreState(StateRecorder &inStream);
float mAngularVelocity = 0.0f; ///< Angular velocity of the driven wheel, will determine the speed of the entire track
};
using VehicleTracks = VehicleTrack[(int)ETrackSide::Num];
JPH_NAMESPACE_END

View File

@@ -0,0 +1,159 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/VehicleTransmission.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTransmissionSettings)
{
JPH_ADD_ENUM_ATTRIBUTE(VehicleTransmissionSettings, mMode)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mGearRatios)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mReverseGearRatios)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchTime)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchReleaseTime)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchLatency)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftUpRPM)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftDownRPM)
JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchStrength)
}
void VehicleTransmissionSettings::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mMode);
inStream.Write(mGearRatios);
inStream.Write(mReverseGearRatios);
inStream.Write(mSwitchTime);
inStream.Write(mClutchReleaseTime);
inStream.Write(mSwitchLatency);
inStream.Write(mShiftUpRPM);
inStream.Write(mShiftDownRPM);
inStream.Write(mClutchStrength);
}
void VehicleTransmissionSettings::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mMode);
inStream.Read(mGearRatios);
inStream.Read(mReverseGearRatios);
inStream.Read(mSwitchTime);
inStream.Read(mClutchReleaseTime);
inStream.Read(mSwitchLatency);
inStream.Read(mShiftUpRPM);
inStream.Read(mShiftDownRPM);
inStream.Read(mClutchStrength);
}
void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp)
{
// Update current gear and calculate clutch friction
if (mMode == ETransmissionMode::Auto)
{
// Switch gears based on rpm
int old_gear = mCurrentGear;
if (mCurrentGear == 0 // In neutral
|| inForwardInput * float(mCurrentGear) < 0.0f) // Changing between forward / reverse
{
// Switch to first gear or reverse depending on input
mCurrentGear = inForwardInput > 0.0f? 1 : (inForwardInput < 0.0f? -1 : 0);
}
else if (mGearSwitchLatencyTimeLeft == 0.0f) // If not in the timout after switching gears
{
if (inCanShiftUp && inCurrentRPM > mShiftUpRPM)
{
if (mCurrentGear < 0)
{
// Shift up, reverse
if (mCurrentGear > -(int)mReverseGearRatios.size())
mCurrentGear--;
}
else
{
// Shift up, forward
if (mCurrentGear < (int)mGearRatios.size())
mCurrentGear++;
}
}
else if (inCurrentRPM < mShiftDownRPM)
{
if (mCurrentGear < 0)
{
// Shift down, reverse
int max_gear = inForwardInput != 0.0f? -1 : 0;
if (mCurrentGear < max_gear)
mCurrentGear++;
}
else
{
// Shift down, forward
int min_gear = inForwardInput != 0.0f? 1 : 0;
if (mCurrentGear > min_gear)
mCurrentGear--;
}
}
}
if (old_gear != mCurrentGear)
{
// We've shifted gear, start switch countdown
mGearSwitchTimeLeft = old_gear != 0? mSwitchTime : 0.0f;
mClutchReleaseTimeLeft = mClutchReleaseTime;
mGearSwitchLatencyTimeLeft = mSwitchLatency;
mClutchFriction = 0.0f;
}
else if (mGearSwitchTimeLeft > 0.0f)
{
// If still switching gears, count down
mGearSwitchTimeLeft = max(0.0f, mGearSwitchTimeLeft - inDeltaTime);
mClutchFriction = 0.0f;
}
else if (mClutchReleaseTimeLeft > 0.0f)
{
// After switching the gears we slowly release the clutch
mClutchReleaseTimeLeft = max(0.0f, mClutchReleaseTimeLeft - inDeltaTime);
mClutchFriction = 1.0f - mClutchReleaseTimeLeft / mClutchReleaseTime;
}
else
{
// Clutch has full friction
mClutchFriction = 1.0f;
// Count down switch latency
mGearSwitchLatencyTimeLeft = max(0.0f, mGearSwitchLatencyTimeLeft - inDeltaTime);
}
}
}
float VehicleTransmission::GetCurrentRatio() const
{
if (mCurrentGear < 0)
return mReverseGearRatios[-mCurrentGear - 1];
else if (mCurrentGear == 0)
return 0.0f;
else
return mGearRatios[mCurrentGear - 1];
}
void VehicleTransmission::SaveState(StateRecorder &inStream) const
{
inStream.Write(mCurrentGear);
inStream.Write(mClutchFriction);
inStream.Write(mGearSwitchTimeLeft);
inStream.Write(mClutchReleaseTimeLeft);
inStream.Write(mGearSwitchLatencyTimeLeft);
}
void VehicleTransmission::RestoreState(StateRecorder &inStream)
{
inStream.Read(mCurrentGear);
inStream.Read(mClutchFriction);
inStream.Read(mGearSwitchTimeLeft);
inStream.Read(mClutchReleaseTimeLeft);
inStream.Read(mGearSwitchLatencyTimeLeft);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,87 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#include <Jolt/Physics/StateRecorder.h>
JPH_NAMESPACE_BEGIN
/// How gears are shifted
enum class ETransmissionMode : uint8
{
Auto, ///< Automatically shift gear up and down
Manual, ///< Manual gear shift (call SetTransmissionInput)
};
/// Configuration for the transmission of a vehicle (gear box)
class JPH_EXPORT VehicleTransmissionSettings
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTransmissionSettings)
public:
/// Saves the contents in binary form to inStream.
void SaveBinaryState(StreamOut &inStream) const;
/// Restores the contents in binary form to inStream.
void RestoreBinaryState(StreamIn &inStream);
ETransmissionMode mMode = ETransmissionMode::Auto; ///< How to switch gears
Array<float> mGearRatios { 2.66f, 1.78f, 1.3f, 1.0f, 0.74f }; ///< Ratio in rotation rate between engine and gear box, first element is 1st gear, 2nd element 2nd gear etc.
Array<float> mReverseGearRatios { -2.90f }; ///< Ratio in rotation rate between engine and gear box when driving in reverse
float mSwitchTime = 0.5f; ///< How long it takes to switch gears (s), only used in auto mode
float mClutchReleaseTime = 0.3f; ///< How long it takes to release the clutch (go to full friction), only used in auto mode
float mSwitchLatency = 0.5f; ///< How long to wait after releasing the clutch before another switch is attempted (s), only used in auto mode
float mShiftUpRPM = 4000.0f; ///< If RPM of engine is bigger then this we will shift a gear up, only used in auto mode
float mShiftDownRPM = 2000.0f; ///< If RPM of engine is smaller then this we will shift a gear down, only used in auto mode
float mClutchStrength = 10.0f; ///< Strength of the clutch when fully engaged. Total torque a clutch applies is Torque = ClutchStrength * (Velocity Engine - Avg Velocity Wheels At Clutch) (units: k m^2 s^-1)
};
/// Runtime data for transmission
class JPH_EXPORT VehicleTransmission : public VehicleTransmissionSettings
{
public:
/// Set input from driver regarding the transmission (only relevant when transmission is set to manual mode)
/// @param inCurrentGear Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc.
/// @param inClutchFriction Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction)
void Set(int inCurrentGear, float inClutchFriction) { mCurrentGear = inCurrentGear; mClutchFriction = inClutchFriction; }
/// Update the current gear and clutch friction if the transmission is in auto mode
/// @param inDeltaTime Time step delta time in s
/// @param inCurrentRPM Current RPM for engine
/// @param inForwardInput Hint if the user wants to drive forward (> 0) or backwards (< 0)
/// @param inCanShiftUp Indicates if we want to allow the transmission to shift up (e.g. pass false if wheels are slipping)
void Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp);
/// Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc.
int GetCurrentGear() const { return mCurrentGear; }
/// Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction)
float GetClutchFriction() const { return mClutchFriction; }
/// If the auto box is currently switching gears
bool IsSwitchingGear() const { return mGearSwitchTimeLeft > 0.0f; }
/// Return the transmission ratio based on the current gear (ratio between engine and differential)
float GetCurrentRatio() const;
/// Only allow sleeping when the transmission is idle
bool AllowSleep() const { return mGearSwitchTimeLeft <= 0.0f && mClutchReleaseTimeLeft <= 0.0f && mGearSwitchLatencyTimeLeft <= 0.0f; }
/// Saving state for replay
void SaveState(StateRecorder &inStream) const;
void RestoreState(StateRecorder &inStream);
private:
int mCurrentGear = 0; ///< Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc.
float mClutchFriction = 1.0f; ///< Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction)
float mGearSwitchTimeLeft = 0.0f; ///< When switching gears this will be > 0 and will cause the engine to not provide any torque to the wheels for a short time (used for automatic gear switching only)
float mClutchReleaseTimeLeft = 0.0f; ///< After switching gears this will be > 0 and will cause the clutch friction to go from 0 to 1 (used for automatic gear switching only)
float mGearSwitchLatencyTimeLeft = 0.0f; ///< After releasing the clutch this will be > 0 and will prevent another gear switch (used for automatic gear switching only)
};
JPH_NAMESPACE_END

View File

@@ -0,0 +1,93 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/Wheel.h>
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettings)
{
JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionForcePoint)
JPH_ADD_ATTRIBUTE(WheelSettings, mPosition)
JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionDirection)
JPH_ADD_ATTRIBUTE(WheelSettings, mSteeringAxis)
JPH_ADD_ATTRIBUTE(WheelSettings, mWheelForward)
JPH_ADD_ATTRIBUTE(WheelSettings, mWheelUp)
JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMinLength)
JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMaxLength)
JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionPreloadLength)
JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mMode, "mSuspensionSpringMode")
JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mFrequency, "mSuspensionFrequency") // Renaming attributes to stay compatible with old versions of the library
JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mDamping, "mSuspensionDamping")
JPH_ADD_ATTRIBUTE(WheelSettings, mRadius)
JPH_ADD_ATTRIBUTE(WheelSettings, mWidth)
JPH_ADD_ATTRIBUTE(WheelSettings, mEnableSuspensionForcePoint)
}
void WheelSettings::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mSuspensionForcePoint);
inStream.Write(mPosition);
inStream.Write(mSuspensionDirection);
inStream.Write(mSteeringAxis);
inStream.Write(mWheelForward);
inStream.Write(mWheelUp);
inStream.Write(mSuspensionMinLength);
inStream.Write(mSuspensionMaxLength);
inStream.Write(mSuspensionPreloadLength);
mSuspensionSpring.SaveBinaryState(inStream);
inStream.Write(mRadius);
inStream.Write(mWidth);
inStream.Write(mEnableSuspensionForcePoint);
}
void WheelSettings::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mSuspensionForcePoint);
inStream.Read(mPosition);
inStream.Read(mSuspensionDirection);
inStream.Read(mSteeringAxis);
inStream.Read(mWheelForward);
inStream.Read(mWheelUp);
inStream.Read(mSuspensionMinLength);
inStream.Read(mSuspensionMaxLength);
inStream.Read(mSuspensionPreloadLength);
mSuspensionSpring.RestoreBinaryState(inStream);
inStream.Read(mRadius);
inStream.Read(mWidth);
inStream.Read(mEnableSuspensionForcePoint);
}
Wheel::Wheel(const WheelSettings &inSettings) :
mSettings(&inSettings),
mSuspensionLength(inSettings.mSuspensionMaxLength)
{
JPH_ASSERT(inSettings.mSuspensionDirection.IsNormalized());
JPH_ASSERT(inSettings.mSteeringAxis.IsNormalized());
JPH_ASSERT(inSettings.mWheelForward.IsNormalized());
JPH_ASSERT(inSettings.mWheelUp.IsNormalized());
JPH_ASSERT(inSettings.mSuspensionMinLength >= 0.0f);
JPH_ASSERT(inSettings.mSuspensionMaxLength >= inSettings.mSuspensionMinLength);
JPH_ASSERT(inSettings.mSuspensionPreloadLength >= 0.0f);
JPH_ASSERT(inSettings.mSuspensionSpring.mFrequency > 0.0f);
JPH_ASSERT(inSettings.mSuspensionSpring.mDamping >= 0.0f);
JPH_ASSERT(inSettings.mRadius > 0.0f);
JPH_ASSERT(inSettings.mWidth >= 0.0f);
}
bool Wheel::SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse)
{
return mLongitudinalPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLongitudinal, inMinImpulse, inMaxImpulse);
}
bool Wheel::SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse)
{
return mLateralPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLateral, inMinImpulse, inMaxImpulse);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,148 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
#include <Jolt/ObjectStream/SerializableObject.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
JPH_NAMESPACE_BEGIN
class VehicleConstraint;
/// Base class for wheel settings, each VehicleController can implement a derived class of this
class JPH_EXPORT WheelSettings : public SerializableObject, public RefTarget<WheelSettings>
{
JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettings)
public:
/// Saves the contents in binary form to inStream.
virtual void SaveBinaryState(StreamOut &inStream) const;
/// Restores the contents in binary form to inStream.
virtual void RestoreBinaryState(StreamIn &inStream);
Vec3 mPosition { 0, 0, 0 }; ///< Attachment point of wheel suspension in local space of the body
Vec3 mSuspensionForcePoint { 0, 0, 0 }; ///< Where tire forces (suspension and traction) are applied, in local space of the body. A good default is the center of the wheel in its neutral pose. See mEnableSuspensionForcePoint.
Vec3 mSuspensionDirection { 0, -1, 0 }; ///< Direction of the suspension in local space of the body, should point down
Vec3 mSteeringAxis { 0, 1, 0 }; ///< Direction of the steering axis in local space of the body, should point up (e.g. for a bike would be -mSuspensionDirection)
Vec3 mWheelUp { 0, 1, 0 }; ///< Up direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mUp but can be used to give the wheel camber or for a bike would be -mSuspensionDirection)
Vec3 mWheelForward { 0, 0, 1 }; ///< Forward direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mForward but can be used to give the wheel toe, does not need to be perpendicular to mWheelUp)
float mSuspensionMinLength = 0.3f; ///< How long the suspension is in max raised position relative to the attachment point (m)
float mSuspensionMaxLength = 0.5f; ///< How long the suspension is in max droop position relative to the attachment point (m)
float mSuspensionPreloadLength = 0.0f; ///< The natural length (m) of the suspension spring is defined as mSuspensionMaxLength + mSuspensionPreloadLength. Can be used to preload the suspension as the spring is compressed by mSuspensionPreloadLength when the suspension is in max droop position. Note that this means when the vehicle touches the ground there is a discontinuity so it will also make the vehicle more bouncy as we're updating with discrete time steps.
SpringSettings mSuspensionSpring { ESpringMode::FrequencyAndDamping, 1.5f, 0.5f }; ///< Settings for the suspension spring
float mRadius = 0.3f; ///< Radius of the wheel (m)
float mWidth = 0.1f; ///< Width of the wheel (m)
bool mEnableSuspensionForcePoint = false; ///< Enables mSuspensionForcePoint, if disabled, the forces are applied at the collision contact point. This leads to a more accurate simulation when interacting with dynamic objects but makes the vehicle less stable. When setting this to true, all forces will be applied to a fixed point on the vehicle body.
};
/// Base class for runtime data for a wheel, each VehicleController can implement a derived class of this
class JPH_EXPORT Wheel : public NonCopyable
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor / destructor
explicit Wheel(const WheelSettings &inSettings);
virtual ~Wheel() = default;
/// Get settings for the wheel
const WheelSettings * GetSettings() const { return mSettings; }
/// Get the angular velocity (rad/s) for this wheel, note that positive means the wheel is rotating such that the car moves forward
float GetAngularVelocity() const { return mAngularVelocity; }
/// Update the angular velocity (rad/s)
void SetAngularVelocity(float inVel) { mAngularVelocity = inVel; }
/// Get the current rotation angle of the wheel in radians [0, 2 pi]
float GetRotationAngle() const { return mAngle; }
/// Set the current rotation angle of the wheel in radians [0, 2 pi]
void SetRotationAngle(float inAngle) { mAngle = inAngle; }
/// Get the current steer angle of the wheel in radians [-pi, pi], positive is to the left
float GetSteerAngle() const { return mSteerAngle; }
/// Set the current steer angle of the wheel in radians [-pi, pi]
void SetSteerAngle(float inAngle) { mSteerAngle = inAngle; }
/// Returns true if the wheel is touching an object
inline bool HasContact() const { return !mContactBodyID.IsInvalid(); }
/// Returns the body ID of the body that this wheel is touching
BodyID GetContactBodyID() const { return mContactBodyID; }
/// Returns the sub shape ID where we're contacting the body
SubShapeID GetContactSubShapeID() const { return mContactSubShapeID; }
/// Returns the current contact position in world space (note by the time you call this the vehicle has moved)
RVec3 GetContactPosition() const { JPH_ASSERT(HasContact()); return mContactPosition; }
/// Velocity of the contact point (m / s, not relative to the wheel but in world space)
Vec3 GetContactPointVelocity() const { JPH_ASSERT(HasContact()); return mContactPointVelocity; }
/// Returns the current contact normal in world space (note by the time you call this the vehicle has moved)
Vec3 GetContactNormal() const { JPH_ASSERT(HasContact()); return mContactNormal; }
/// Returns longitudinal direction (direction along the wheel relative to floor) in world space (note by the time you call this the vehicle has moved)
Vec3 GetContactLongitudinal() const { JPH_ASSERT(HasContact()); return mContactLongitudinal; }
/// Returns lateral direction (sideways direction) in world space (note by the time you call this the vehicle has moved)
Vec3 GetContactLateral() const { JPH_ASSERT(HasContact()); return mContactLateral; }
/// Get the length of the suspension for a wheel (m) relative to the suspension attachment point (hard point)
float GetSuspensionLength() const { return mSuspensionLength; }
/// Check if the suspension hit its upper limit
bool HasHitHardPoint() const { return mSuspensionMaxUpPart.IsActive(); }
/// Get the total impulse (N s) that was applied by the suspension
float GetSuspensionLambda() const { return mSuspensionPart.GetTotalLambda() + mSuspensionMaxUpPart.GetTotalLambda(); }
/// Get total impulse (N s) applied along the forward direction of the wheel
float GetLongitudinalLambda() const { return mLongitudinalPart.GetTotalLambda(); }
/// Get total impulse (N s) applied along the sideways direction of the wheel
float GetLateralLambda() const { return mLateralPart.GetTotalLambda(); }
/// Internal function that should only be called by the controller. Used to apply impulses in the forward direction of the vehicle.
bool SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse);
/// Internal function that should only be called by the controller. Used to apply impulses in the sideways direction of the vehicle.
bool SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse);
protected:
friend class VehicleConstraint;
RefConst<WheelSettings> mSettings; ///< Configuration settings for this wheel
BodyID mContactBodyID; ///< ID of body for ground
SubShapeID mContactSubShapeID; ///< Sub shape ID for ground
Body * mContactBody = nullptr; ///< Body for ground
float mSuspensionLength; ///< Current length of the suspension
RVec3 mContactPosition; ///< Position of the contact point between wheel and ground
Vec3 mContactPointVelocity; ///< Velocity of the contact point (m / s, not relative to the wheel but in world space)
Vec3 mContactNormal; ///< Normal of the contact point between wheel and ground
Vec3 mContactLongitudinal; ///< Vector perpendicular to normal in the forward direction
Vec3 mContactLateral; ///< Vector perpendicular to normal and longitudinal direction in the right direction
Real mAxlePlaneConstant; ///< Constant for the contact plane of the axle, defined as ContactNormal . (WorldSpaceSuspensionPoint + SuspensionLength * WorldSpaceSuspensionDirection)
float mAntiRollBarImpulse = 0.0f; ///< Amount of impulse applied to the suspension from the anti-rollbars
float mSteerAngle = 0.0f; ///< Rotation around the suspension direction, positive is to the left
float mAngularVelocity = 0.0f; ///< Rotation speed of wheel, positive when the wheels cause the vehicle to move forwards (rad/s)
float mAngle = 0.0f; ///< Current rotation of the wheel (rad, [0, 2 pi])
AxisConstraintPart mSuspensionPart; ///< Controls movement up/down along the contact normal
AxisConstraintPart mSuspensionMaxUpPart; ///< Adds a hard limit when reaching the minimal suspension length
AxisConstraintPart mLongitudinalPart; ///< Controls movement forward/backward
AxisConstraintPart mLateralPart; ///< Controls movement sideways (slip)
};
using Wheels = Array<Wheel *>;
JPH_NAMESPACE_END

View File

@@ -0,0 +1,845 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#include <Jolt/Math/DynMatrix.h>
#include <Jolt/Math/GaussianElimination.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
//#define JPH_TRACE_VEHICLE_STATS
JPH_NAMESPACE_BEGIN
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheeledVehicleControllerSettings)
{
JPH_ADD_BASE_CLASS(WheeledVehicleControllerSettings, VehicleControllerSettings)
JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mEngine)
JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mTransmission)
JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentials)
JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentialLimitedSlipRatio)
}
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsWV)
{
JPH_ADD_ATTRIBUTE(WheelSettingsWV, mInertia)
JPH_ADD_ATTRIBUTE(WheelSettingsWV, mAngularDamping)
JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxSteerAngle)
JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLongitudinalFriction)
JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLateralFriction)
JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxBrakeTorque)
JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxHandBrakeTorque)
}
WheelSettingsWV::WheelSettingsWV()
{
mLongitudinalFriction.Reserve(3);
mLongitudinalFriction.AddPoint(0.0f, 0.0f);
mLongitudinalFriction.AddPoint(0.06f, 1.2f);
mLongitudinalFriction.AddPoint(0.2f, 1.0f);
mLateralFriction.Reserve(3);
mLateralFriction.AddPoint(0.0f, 0.0f);
mLateralFriction.AddPoint(3.0f, 1.2f);
mLateralFriction.AddPoint(20.0f, 1.0f);
}
void WheelSettingsWV::SaveBinaryState(StreamOut &inStream) const
{
inStream.Write(mInertia);
inStream.Write(mAngularDamping);
inStream.Write(mMaxSteerAngle);
mLongitudinalFriction.SaveBinaryState(inStream);
mLateralFriction.SaveBinaryState(inStream);
inStream.Write(mMaxBrakeTorque);
inStream.Write(mMaxHandBrakeTorque);
}
void WheelSettingsWV::RestoreBinaryState(StreamIn &inStream)
{
inStream.Read(mInertia);
inStream.Read(mAngularDamping);
inStream.Read(mMaxSteerAngle);
mLongitudinalFriction.RestoreBinaryState(inStream);
mLateralFriction.RestoreBinaryState(inStream);
inStream.Read(mMaxBrakeTorque);
inStream.Read(mMaxHandBrakeTorque);
}
WheelWV::WheelWV(const WheelSettingsWV &inSettings) :
Wheel(inSettings)
{
JPH_ASSERT(inSettings.mInertia >= 0.0f);
JPH_ASSERT(inSettings.mAngularDamping >= 0.0f);
JPH_ASSERT(abs(inSettings.mMaxSteerAngle) <= 0.5f * JPH_PI);
JPH_ASSERT(inSettings.mMaxBrakeTorque >= 0.0f);
JPH_ASSERT(inSettings.mMaxHandBrakeTorque >= 0.0f);
}
void WheelWV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint)
{
const WheelSettingsWV *settings = GetSettings();
// Angular damping: dw/dt = -c * w
// Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt)
// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
mAngularVelocity *= max(0.0f, 1.0f - settings->mAngularDamping * inDeltaTime);
// Update rotation of wheel
mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI);
if (mContactBody != nullptr)
{
const Body *body = inConstraint.GetVehicleBody();
// Calculate relative velocity between wheel contact point and floor
Vec3 relative_velocity = body->GetPointVelocity(mContactPosition) - mContactPointVelocity;
// Cancel relative velocity in the normal plane
relative_velocity -= mContactNormal.Dot(relative_velocity) * mContactNormal;
float relative_longitudinal_velocity = relative_velocity.Dot(mContactLongitudinal);
// Calculate longitudinal friction based on difference between velocity of rolling wheel and drive surface
float relative_longitudinal_velocity_denom = Sign(relative_longitudinal_velocity) * max(1.0e-3f, abs(relative_longitudinal_velocity)); // Ensure we don't divide by zero
mLongitudinalSlip = abs((mAngularVelocity * settings->mRadius - relative_longitudinal_velocity) / relative_longitudinal_velocity_denom);
float longitudinal_slip_friction = settings->mLongitudinalFriction.GetValue(mLongitudinalSlip);
// Calculate lateral friction based on slip angle
float relative_velocity_len = relative_velocity.Length();
mLateralSlip = relative_velocity_len < 1.0e-3f ? 0.0f : ACos(abs(relative_longitudinal_velocity) / relative_velocity_len);
float lateral_slip_angle = RadiansToDegrees(mLateralSlip);
float lateral_slip_friction = settings->mLateralFriction.GetValue(lateral_slip_angle);
// Tire friction
VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction();
mCombinedLongitudinalFriction = longitudinal_slip_friction;
mCombinedLateralFriction = lateral_slip_friction;
combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID);
}
else
{
// No collision
mLongitudinalSlip = 0.0f;
mLateralSlip = 0.0f;
mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f;
}
}
VehicleController *WheeledVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const
{
return new WheeledVehicleController(*this, inConstraint);
}
void WheeledVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const
{
mEngine.SaveBinaryState(inStream);
mTransmission.SaveBinaryState(inStream);
uint32 num_differentials = (uint32)mDifferentials.size();
inStream.Write(num_differentials);
for (const VehicleDifferentialSettings &d : mDifferentials)
d.SaveBinaryState(inStream);
inStream.Write(mDifferentialLimitedSlipRatio);
}
void WheeledVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream)
{
mEngine.RestoreBinaryState(inStream);
mTransmission.RestoreBinaryState(inStream);
uint32 num_differentials = 0;
inStream.Read(num_differentials);
mDifferentials.resize(num_differentials);
for (VehicleDifferentialSettings &d : mDifferentials)
d.RestoreBinaryState(inStream);
inStream.Read(mDifferentialLimitedSlipRatio);
}
WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) :
VehicleController(inConstraint)
{
// Copy engine settings
static_cast<VehicleEngineSettings &>(mEngine) = inSettings.mEngine;
JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f);
JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM);
mEngine.SetCurrentRPM(mEngine.mMinRPM);
// Copy transmission settings
static_cast<VehicleTransmissionSettings &>(mTransmission) = inSettings.mTransmission;
#ifdef JPH_ENABLE_ASSERTS
for (float r : inSettings.mTransmission.mGearRatios)
JPH_ASSERT(r > 0.0f);
for (float r : inSettings.mTransmission.mReverseGearRatios)
JPH_ASSERT(r < 0.0f);
#endif // JPH_ENABLE_ASSERTS
JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f);
JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f);
JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM);
JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM);
JPH_ASSERT(inSettings.mTransmission.mClutchStrength > 0.0f);
// Copy differential settings
mDifferentials.resize(inSettings.mDifferentials.size());
for (uint i = 0; i < mDifferentials.size(); ++i)
{
const VehicleDifferentialSettings &d = inSettings.mDifferentials[i];
mDifferentials[i] = d;
JPH_ASSERT(d.mDifferentialRatio > 0.0f);
JPH_ASSERT(d.mLeftRightSplit >= 0.0f && d.mLeftRightSplit <= 1.0f);
JPH_ASSERT(d.mEngineTorqueRatio >= 0.0f);
JPH_ASSERT(d.mLimitedSlipRatio > 1.0f);
}
mDifferentialLimitedSlipRatio = inSettings.mDifferentialLimitedSlipRatio;
JPH_ASSERT(mDifferentialLimitedSlipRatio > 1.0f);
}
float WheeledVehicleController::GetWheelSpeedAtClutch() const
{
float wheel_speed_at_clutch = 0.0f;
int num_driven_wheels = 0;
for (const VehicleDifferentialSettings &d : mDifferentials)
{
int wheels[] = { d.mLeftWheel, d.mRightWheel };
for (int w : wheels)
if (w >= 0)
{
wheel_speed_at_clutch += mConstraint.GetWheel(w)->GetAngularVelocity() * d.mDifferentialRatio;
num_driven_wheels++;
}
}
return wheel_speed_at_clutch / float(num_driven_wheels) * VehicleEngine::cAngularVelocityToRPM * mTransmission.GetCurrentRatio();
}
bool WheeledVehicleController::AllowSleep() const
{
return mForwardInput == 0.0f // No user input
&& mTransmission.AllowSleep() // Transmission is not shifting
&& mEngine.AllowSleep(); // Engine is idling
}
void WheeledVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
{
JPH_PROFILE_FUNCTION();
#ifdef JPH_TRACE_VEHICLE_STATS
static bool sTracedHeader = false;
if (!sTracedHeader)
{
Trace("Time, ForwardInput, Gear, ClutchFriction, EngineRPM, WheelRPM, Velocity (km/h)");
sTracedHeader = true;
}
static float sTime = 0.0f;
sTime += inDeltaTime;
Trace("%.3f, %.1f, %d, %.1f, %.1f, %.1f, %.1f", sTime, mForwardInput, mTransmission.GetCurrentGear(), mTransmission.GetClutchFriction(), mEngine.GetCurrentRPM(), GetWheelSpeedAtClutch(), mConstraint.GetVehicleBody()->GetLinearVelocity().Length() * 3.6f);
#endif // JPH_TRACE_VEHICLE_STATS
for (Wheel *w_base : mConstraint.GetWheels())
{
WheelWV *w = static_cast<WheelWV *>(w_base);
// Set steering angle
w->SetSteerAngle(-mRightInput * w->GetSettings()->mMaxSteerAngle);
}
}
void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
{
JPH_PROFILE_FUNCTION();
// Remember old RPM so we can detect if we're increasing or decreasing
float old_engine_rpm = mEngine.GetCurrentRPM();
Wheels &wheels = mConstraint.GetWheels();
// Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again)
for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index)
{
WheelWV *w = static_cast<WheelWV *>(wheels[wheel_index]);
w->Update(wheel_index, inDeltaTime, mConstraint);
}
// In auto transmission mode, don't accelerate the engine when switching gears
float forward_input = abs(mForwardInput);
if (mTransmission.mMode == ETransmissionMode::Auto)
forward_input *= mTransmission.GetClutchFriction();
// Apply engine damping
mEngine.ApplyDamping(inDeltaTime);
// Calculate engine torque
float engine_torque = mEngine.GetTorque(forward_input);
// Define a struct that contains information about driven differentials (i.e. that have wheels connected)
struct DrivenDifferential
{
const VehicleDifferentialSettings * mDifferential;
float mAngularVelocity;
float mClutchToDifferentialTorqueRatio;
float mTempTorqueFactor;
};
// Collect driven differentials and their speeds
Array<DrivenDifferential> driven_differentials;
driven_differentials.reserve(mDifferentials.size());
float differential_omega_min = FLT_MAX, differential_omega_max = 0.0f;
for (const VehicleDifferentialSettings &d : mDifferentials)
{
float avg_omega = 0.0f;
int avg_omega_denom = 0;
int indices[] = { d.mLeftWheel, d.mRightWheel };
for (int idx : indices)
if (idx != -1)
{
avg_omega += wheels[idx]->GetAngularVelocity();
avg_omega_denom++;
}
if (avg_omega_denom > 0)
{
avg_omega = abs(avg_omega * d.mDifferentialRatio / float(avg_omega_denom)); // ignoring that the differentials may be rotating in different directions
driven_differentials.push_back({ &d, avg_omega, d.mEngineTorqueRatio, 0 });
// Remember min and max velocity
differential_omega_min = min(differential_omega_min, avg_omega);
differential_omega_max = max(differential_omega_max, avg_omega);
}
}
if (mDifferentialLimitedSlipRatio < FLT_MAX // Limited slip differential needs to be turned on
&& differential_omega_max > differential_omega_min) // There needs to be a velocity difference
{
// Calculate factor based on relative speed of a differential
float sum_factor = 0.0f;
for (DrivenDifferential &d : driven_differentials)
{
// Differential with max velocity gets factor 0, differential with min velocity 1
d.mTempTorqueFactor = (differential_omega_max - d.mAngularVelocity) / (differential_omega_max - differential_omega_min);
sum_factor += d.mTempTorqueFactor;
}
// Normalize the result
for (DrivenDifferential &d : driven_differentials)
d.mTempTorqueFactor /= sum_factor;
// Prevent div by zero
differential_omega_min = max(1.0e-3f, differential_omega_min);
differential_omega_max = max(1.0e-3f, differential_omega_max);
// Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mDifferentialLimitedSlipRatio
float alpha = min((differential_omega_max / differential_omega_min - 1.0f) / (mDifferentialLimitedSlipRatio - 1.0f), 1.0f);
JPH_ASSERT(alpha >= 0.0f);
float one_min_alpha = 1.0f - alpha;
// Update torque ratio for all differentials
for (DrivenDifferential &d : driven_differentials)
d.mClutchToDifferentialTorqueRatio = one_min_alpha * d.mClutchToDifferentialTorqueRatio + alpha * d.mTempTorqueFactor;
}
#ifdef JPH_ENABLE_ASSERTS
// Assert the values add up to 1
float sum_torque_factors = 0.0f;
for (DrivenDifferential &d : driven_differentials)
sum_torque_factors += d.mClutchToDifferentialTorqueRatio;
JPH_ASSERT(abs(sum_torque_factors - 1.0f) < 1.0e-6f);
#endif // JPH_ENABLE_ASSERTS
// Define a struct that collects information about the wheels that connect to the engine
struct DrivenWheel
{
WheelWV * mWheel;
float mClutchToWheelRatio;
float mClutchToWheelTorqueRatio;
float mEstimatedAngularImpulse;
};
Array<DrivenWheel> driven_wheels;
driven_wheels.reserve(wheels.size());
// Collect driven wheels
float transmission_ratio = mTransmission.GetCurrentRatio();
for (const DrivenDifferential &dd : driven_differentials)
{
VehicleDifferentialSettings d = *dd.mDifferential;
WheelWV *wl = d.mLeftWheel != -1? static_cast<WheelWV *>(wheels[d.mLeftWheel]) : nullptr;
WheelWV *wr = d.mRightWheel != -1? static_cast<WheelWV *>(wheels[d.mRightWheel]) : nullptr;
float clutch_to_wheel_ratio = transmission_ratio * d.mDifferentialRatio;
if (wl != nullptr && wr != nullptr)
{
// Calculate torque ratio
float ratio_l, ratio_r;
d.CalculateTorqueRatio(wl->GetAngularVelocity(), wr->GetAngularVelocity(), ratio_l, ratio_r);
// Add both wheels
driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_l, 0.0f });
driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_r, 0.0f });
}
else if (wl != nullptr)
{
// Only left wheel, all power to left
driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f });
}
else if (wr != nullptr)
{
// Only right wheel, all power to right
driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f });
}
}
bool solved = false;
if (!driven_wheels.empty())
{
// Define the torque at the clutch at time t as:
//
// tc(t):=S*(we(t)-sum(R(j)*ww(j,t),j,1,N)/N)
//
// Where:
// S is the total strength of clutch (= friction * strength)
// we(t) is the engine angular velocity at time t
// R(j) is the total gear ratio of clutch to wheel for wheel j
// ww(j,t) is the angular velocity of wheel j at time t
// N is the amount of wheels
//
// The torque that increases the engine angular velocity at time t is:
//
// te(t):=TE-tc(t)
//
// Where:
// TE is the torque delivered by the engine
//
// The torque that increases the wheel angular velocity for wheel i at time t is:
//
// tw(i,t):=TW(i)+R(i)*F(i)*tc(t)
//
// Where:
// TW(i) is the torque applied to the wheel outside of the engine (brake + torque due to friction with the ground)
// F(i) is the fraction of the engine torque applied from engine to wheel i
//
// Because the angular acceleration and torque are connected through: Torque = I * dw/dt
//
// We have the angular acceleration of the engine at time t:
//
// ddt_we(t):=te(t)/Ie
//
// Where:
// Ie is the inertia of the engine
//
// We have the angular acceleration of wheel i at time t:
//
// ddt_ww(i,t):=tw(i,t)/Iw(i)
//
// Where:
// Iw(i) is the inertia of wheel i
//
// We could take a simple Euler step to calculate the resulting accelerations but because the system is very stiff this turns out to be unstable, so we need to use implicit Euler instead:
//
// we(t+dt)=we(t)+dt*ddt_we(t+dt)
//
// and:
//
// ww(i,t+dt)=ww(i,t)+dt*ddt_ww(i,t+dt)
//
// Expanding both equations (the equations above are in wxMaxima format and this can easily be done by expand(%)):
//
// For wheel:
//
// ww(i,t+dt) + (S*dt*F(i)*R(i)*sum(R(j)*ww(j,t+dt),j,1,N))/(N*Iw(i)) - (S*dt*F(i)*R(i)*we(t+dt))/Iw(i) = ww(i,t)+(dt*TW(i))/Iw(i)
//
// For engine:
//
// we(t+dt) + (S*dt*we(t+dt))/Ie - (S*dt*sum(R(j)*ww(j,t+dt),j,1,N))/(Ie*N) = we(t)+(TE*dt)/Ie
//
// Defining a vector w(t) = (ww(1, t), ww(2, t), ..., ww(N, t), we(t)) we can write both equations as a matrix multiplication:
//
// a * w(t + dt) = b
//
// We then invert the matrix to get the new angular velocities.
// Dimension of matrix is N + 1
int n = (int)driven_wheels.size() + 1;
// Last column of w is for the engine angular velocity
int engine = n - 1;
// Define a and b
DynMatrix a(n, n);
DynMatrix b(n, 1);
// Get number of driven wheels as a float
float num_driven_wheels_float = float(driven_wheels.size());
// Angular velocity of engine
float w_engine = mEngine.GetAngularVelocity();
// Calculate the total strength of the clutch
float clutch_strength = transmission_ratio != 0.0f? mTransmission.GetClutchFriction() * mTransmission.mClutchStrength : 0.0f;
// dt / Ie
float dt_div_ie = inDeltaTime / mEngine.mInertia;
// Calculate scale factor for impulses based on previous delta time
float impulse_scale = mPreviousDeltaTime > 0.0f? inDeltaTime / mPreviousDeltaTime : 0.0f;
// Iterate the rows for the wheels
for (int i = 0; i < (int)driven_wheels.size(); ++i)
{
DrivenWheel &w_i = driven_wheels[i];
const WheelSettingsWV *settings = w_i.mWheel->GetSettings();
// Get wheel inertia
float inertia = settings->mInertia;
// S * R(i)
float s_r = clutch_strength * w_i.mClutchToWheelRatio;
// dt * S * R(i) * F(i) / Iw
float dt_s_r_f_div_iw = inDeltaTime * s_r * w_i.mClutchToWheelTorqueRatio / inertia;
// Fill in the columns of a for wheel j
for (int j = 0; j < (int)driven_wheels.size(); ++j)
{
const DrivenWheel &w_j = driven_wheels[j];
a(i, j) = dt_s_r_f_div_iw * w_j.mClutchToWheelRatio / num_driven_wheels_float;
}
// Add ww(i, t+dt)
a(i, i) += 1.0f;
// Add the column for the engine
a(i, engine) = -dt_s_r_f_div_iw;
// Calculate external angular impulse operating on the wheel: TW(i) * dt
float dt_tw = 0.0f;
// Combine brake with hand brake torque
float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque;
if (brake_torque > 0.0f)
{
// We're braking
// Calculate brake angular impulse
float sign;
if (w_i.mWheel->GetAngularVelocity() != 0.0f)
sign = Sign(w_i.mWheel->GetAngularVelocity());
else
sign = Sign(mTransmission.GetCurrentRatio()); // When wheels have locked up use the transmission ratio to determine the sign
dt_tw = sign * inDeltaTime * brake_torque;
}
if (w_i.mWheel->HasContact())
{
// We have wheel contact with the floor
// Note that we don't know the torque due to the ground contact yet, so we use the impulse applied from the last frame to estimate it
// Wheel torque TW = force * radius = lambda / dt * radius
dt_tw += impulse_scale * w_i.mWheel->GetLongitudinalLambda() * settings->mRadius;
}
w_i.mEstimatedAngularImpulse = dt_tw;
// Fill in the constant b = ww(i,t)+(dt*TW(i))/Iw(i)
b(i, 0) = w_i.mWheel->GetAngularVelocity() - dt_tw / inertia;
// To avoid looping over the wheels again, we also fill in the wheel columns of the engine row here
a(engine, i) = -dt_div_ie * s_r / num_driven_wheels_float;
}
// Finalize the engine row
a(engine, engine) = (1.0f + dt_div_ie * clutch_strength);
b(engine, 0) = w_engine + dt_div_ie * engine_torque;
// Solve the linear equation
if (GaussianElimination(a, b))
{
// Update the angular velocities for the wheels
for (int i = 0; i < (int)driven_wheels.size(); ++i)
{
DrivenWheel &w_i = driven_wheels[i];
const WheelSettingsWV *settings = w_i.mWheel->GetSettings();
// Get solved wheel angular velocity
float angular_velocity = b(i, 0);
// We estimated TW and applied it in the equation above, but we haven't actually applied this torque yet so we undo it here.
// It will be applied when we solve the actual braking / the constraints with the floor.
angular_velocity += w_i.mEstimatedAngularImpulse / settings->mInertia;
// Update angular velocity
w_i.mWheel->SetAngularVelocity(angular_velocity);
}
// Update the engine RPM
mEngine.SetCurrentRPM(b(engine, 0) * VehicleEngine::cAngularVelocityToRPM);
// The speeds have been solved
solved = true;
}
else
{
JPH_ASSERT(false, "New engine/wheel speeds could not be calculated!");
}
}
if (!solved)
{
// Engine not connected to wheels, apply all torque to engine rotation
mEngine.ApplyTorque(engine_torque, inDeltaTime);
}
// Calculate if any of the wheels are slipping, this is used to prevent gear switching
bool wheels_slipping = false;
for (const DrivenWheel &w : driven_wheels)
wheels_slipping |= w.mClutchToWheelTorqueRatio > 0.0f && (!w.mWheel->HasContact() || w.mWheel->mLongitudinalSlip > 0.1f);
// Only allow shifting up when we're not slipping and we're increasing our RPM.
// After a jump, we have a very high engine RPM but once we hit the ground the RPM should be decreasing and we don't want to shift up
// during that time.
bool can_shift_up = !wheels_slipping && mEngine.GetCurrentRPM() >= old_engine_rpm;
// Update transmission
mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, can_shift_up);
// Braking
for (Wheel *w_base : wheels)
{
WheelWV *w = static_cast<WheelWV *>(w_base);
const WheelSettingsWV *settings = w->GetSettings();
// Combine brake with hand brake torque
float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque;
if (brake_torque > 0.0f)
{
// Calculate how much torque is needed to stop the wheels from rotating in this time step
float brake_torque_to_lock_wheels = abs(w->GetAngularVelocity()) * settings->mInertia / inDeltaTime;
if (brake_torque > brake_torque_to_lock_wheels)
{
// Wheels are locked
w->SetAngularVelocity(0.0f);
w->mBrakeImpulse = (brake_torque - brake_torque_to_lock_wheels) * inDeltaTime / settings->mRadius;
}
else
{
// Slow down the wheels
w->ApplyTorque(-Sign(w->GetAngularVelocity()) * brake_torque, inDeltaTime);
w->mBrakeImpulse = 0.0f;
}
}
else
{
// Not braking
w->mBrakeImpulse = 0.0f;
}
}
// Remember previous delta time so we can scale the impulses correctly
mPreviousDeltaTime = inDeltaTime;
}
bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime)
{
bool impulse = false;
float *max_lateral_friction_impulse = (float *)JPH_STACK_ALLOC(mConstraint.GetWheels().size() * sizeof(float));
uint wheel_index = 0;
for (Wheel *w_base : mConstraint.GetWheels())
{
if (w_base->HasContact())
{
WheelWV *w = static_cast<WheelWV *>(w_base);
const WheelSettingsWV *settings = w->GetSettings();
// Calculate max impulse that we can apply on the ground
float max_longitudinal_friction_impulse;
mTireMaxImpulseCallback(wheel_index,
max_longitudinal_friction_impulse, max_lateral_friction_impulse[wheel_index], w->GetSuspensionLambda(),
w->mCombinedLongitudinalFriction, w->mCombinedLateralFriction, w->mLongitudinalSlip, w->mLateralSlip, inDeltaTime);
// Calculate relative velocity between wheel contact point and floor in longitudinal direction
Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity();
float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal());
// Calculate brake force to apply
float min_longitudinal_impulse, max_longitudinal_impulse;
if (w->mBrakeImpulse != 0.0f)
{
// Limit brake force by max tire friction
float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse);
// Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle)
if (relative_longitudinal_velocity >= 0.0f)
{
min_longitudinal_impulse = -brake_impulse;
max_longitudinal_impulse = 0.0f;
}
else
{
min_longitudinal_impulse = 0.0f;
max_longitudinal_impulse = brake_impulse;
}
// Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas
impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
}
else
{
// Assume we want to apply an angular impulse that makes the delta velocity between wheel and ground zero in one time step, calculate the amount of linear impulse needed to do that
float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius;
float linear_impulse = (w->GetAngularVelocity() - desired_angular_velocity) * settings->mInertia / settings->mRadius;
// Limit the impulse by max tire friction
float prev_lambda = w->GetLongitudinalLambda();
min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse);
// Longitudinal impulse
impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
// Update the angular velocity of the wheels according to the lambda that was applied
w->SetAngularVelocity(w->GetAngularVelocity() - (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / settings->mInertia);
}
}
++wheel_index;
}
wheel_index = 0;
for (Wheel *w_base : mConstraint.GetWheels())
{
if (w_base->HasContact())
{
WheelWV *w = static_cast<WheelWV *>(w_base);
// Lateral friction
float max_lateral_impulse = max_lateral_friction_impulse[wheel_index];
impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_impulse, max_lateral_impulse);
}
++wheel_index;
}
return impulse;
}
#ifdef JPH_DEBUG_RENDERER
void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
{
float constraint_size = mConstraint.GetDrawConstraintSize();
// Draw RPM
Body *body = mConstraint.GetVehicleBody();
Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp();
RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition;
Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward();
mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM);
if (mTransmission.GetCurrentRatio() != 0.0f)
{
// Calculate average wheel speed at clutch
float wheel_speed_at_clutch = GetWheelSpeedAtClutch();
// Draw the average wheel speed measured at clutch to compare engine RPM with wheel RPM
inRenderer->DrawLine(rpm_meter_pos, rpm_meter_pos + Quat::sRotation(rpm_meter_fwd, mEngine.ConvertRPMToAngle(wheel_speed_at_clutch)) * (rpm_meter_up * 1.1f * mRPMMeterSize), Color::sYellow);
}
// Draw current vehicle state
String status = StringFormat("Forward: %.1f, Right: %.1f\nBrake: %.1f, HandBrake: %.1f\n"
"Gear: %d, Clutch: %.1f\nEngineRPM: %.0f, V: %.1f km/h",
(double)mForwardInput, (double)mRightInput, (double)mBrakeInput, (double)mHandBrakeInput,
mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6);
inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size);
RMat44 body_transform = body->GetWorldTransform();
for (const Wheel *w_base : mConstraint.GetWheels())
{
const WheelWV *w = static_cast<const WheelWV *>(w_base);
const WheelSettings *settings = w->GetSettings();
// Calculate where the suspension attaches to the body in world space
RVec3 ws_position = body_transform * settings->mPosition;
Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
// Draw suspension
RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength;
RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength;
inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed);
inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen);
// Draw current length
RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength();
inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size);
// Draw wheel basis
Vec3 wheel_forward, wheel_up, wheel_right;
mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right);
wheel_forward = body_transform.Multiply3x3(wheel_forward);
wheel_up = body_transform.Multiply3x3(wheel_up);
wheel_right = body_transform.Multiply3x3(wheel_right);
Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis);
inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed);
inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen);
inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue);
inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow);
// Draw wheel
RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos);
wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle()));
inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
if (w->HasContact())
{
// Draw contact
inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow);
inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed);
inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue);
DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f, S: %.2f\nSlipLateral: %.1f, SlipLong: %.2f\nFrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)RadiansToDegrees(w->mLateralSlip), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, constraint_size);
}
else
{
// Draw 'no hit'
DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f", (double)w->GetAngularVelocity()), Color::sRed, constraint_size);
}
}
}
#endif // JPH_DEBUG_RENDERER
void WheeledVehicleController::SaveState(StateRecorder &inStream) const
{
inStream.Write(mForwardInput);
inStream.Write(mRightInput);
inStream.Write(mBrakeInput);
inStream.Write(mHandBrakeInput);
inStream.Write(mPreviousDeltaTime);
mEngine.SaveState(inStream);
mTransmission.SaveState(inStream);
}
void WheeledVehicleController::RestoreState(StateRecorder &inStream)
{
inStream.Read(mForwardInput);
inStream.Read(mRightInput);
inStream.Read(mBrakeInput);
inStream.Read(mHandBrakeInput);
inStream.Read(mPreviousDeltaTime);
mEngine.RestoreState(inStream);
mTransmission.RestoreState(inStream);
}
JPH_NAMESPACE_END

View File

@@ -0,0 +1,199 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
#include <Jolt/Physics/Vehicle/VehicleController.h>
#include <Jolt/Physics/Vehicle/VehicleEngine.h>
#include <Jolt/Physics/Vehicle/VehicleTransmission.h>
#include <Jolt/Physics/Vehicle/VehicleDifferential.h>
#include <Jolt/Core/LinearCurve.h>
JPH_NAMESPACE_BEGIN
class PhysicsSystem;
/// WheelSettings object specifically for WheeledVehicleController
class JPH_EXPORT WheelSettingsWV : public WheelSettings
{
JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsWV)
public:
/// Constructor
WheelSettingsWV();
// See: WheelSettings
virtual void SaveBinaryState(StreamOut &inStream) const override;
virtual void RestoreBinaryState(StreamIn &inStream) override;
float mInertia = 0.9f; ///< Moment of inertia (kg m^2), for a cylinder this would be 0.5 * M * R^2 which is 0.9 for a wheel with a mass of 20 kg and radius 0.3 m
float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w
float mMaxSteerAngle = DegreesToRadians(70.0f); ///< How much this wheel can steer (radians)
LinearCurve mLongitudinalFriction; ///< On the Y-axis: friction in the forward direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip ratio (fraction) defined as (omega_wheel * r_wheel - v_longitudinal) / |v_longitudinal|. You can see slip ratio as the amount the wheel is spinning relative to the floor: 0 means the wheel has full traction and is rolling perfectly in sync with the ground, 1 is for example when the wheel is locked and sliding over the ground.
LinearCurve mLateralFriction; ///< On the Y-axis: friction in the sideways direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip angle (degrees) defined as angle between relative contact velocity and tire direction.
float mMaxBrakeTorque = 1500.0f; ///< How much torque (Nm) the brakes can apply to this wheel
float mMaxHandBrakeTorque = 4000.0f; ///< How much torque (Nm) the hand brake can apply to this wheel (usually only applied to the rear wheels)
};
/// Wheel object specifically for WheeledVehicleController
class JPH_EXPORT WheelWV : public Wheel
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
explicit WheelWV(const WheelSettingsWV &inWheel);
/// Override GetSettings and cast to the correct class
const WheelSettingsWV * GetSettings() const { return StaticCast<WheelSettingsWV>(mSettings); }
/// Apply a torque (N m) to the wheel for a particular delta time
void ApplyTorque(float inTorque, float inDeltaTime)
{
mAngularVelocity += inTorque * inDeltaTime / GetSettings()->mInertia;
}
/// Update the wheel rotation based on the current angular velocity
void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint);
float mLongitudinalSlip = 0.0f; ///< Velocity difference between ground and wheel relative to ground velocity
float mLateralSlip = 0.0f; ///< Angular difference (in radians) between ground and wheel relative to ground velocity
float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and tires)
float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and tires)
float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction)
};
/// Settings of a vehicle with regular wheels
///
/// The properties in this controller are largely based on "Car Physics for Games" by Marco Monster.
/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html
class JPH_EXPORT WheeledVehicleControllerSettings : public VehicleControllerSettings
{
JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheeledVehicleControllerSettings)
public:
// See: VehicleControllerSettings
virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override;
virtual void SaveBinaryState(StreamOut &inStream) const override;
virtual void RestoreBinaryState(StreamIn &inStream) override;
VehicleEngineSettings mEngine; ///< The properties of the engine
VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box)
Array<VehicleDifferentialSettings> mDifferentials; ///< List of differentials and their properties
float mDifferentialLimitedSlipRatio = 1.4f; ///< Ratio max / min average wheel speed of each differential (measured at the clutch). When the ratio is exceeded all torque gets distributed to the differential with the minimal average velocity. This allows implementing a limited slip differential between differentials. Set to FLT_MAX for an open differential. Value should be > 1.
};
/// Runtime controller class
class JPH_EXPORT WheeledVehicleController : public VehicleController
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint);
/// Typedefs
using Differentials = Array<VehicleDifferentialSettings>;
/// Set input from driver
/// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
/// @param inRight Value between -1 and 1 indicating desired steering angle (1 = right)
/// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed
/// @param inHandBrake Value between 0 and 1 indicating how strong the hand brake is pulled
void SetDriverInput(float inForward, float inRight, float inBrake, float inHandBrake) { mForwardInput = inForward; mRightInput = inRight; mBrakeInput = inBrake; mHandBrakeInput = inHandBrake; }
/// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
void SetForwardInput(float inForward) { mForwardInput = inForward; }
float GetForwardInput() const { return mForwardInput; }
/// Value between -1 and 1 indicating desired steering angle (1 = right)
void SetRightInput(float inRight) { mRightInput = inRight; }
float GetRightInput() const { return mRightInput; }
/// Value between 0 and 1 indicating how strong the brake pedal is pressed
void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; }
float GetBrakeInput() const { return mBrakeInput; }
/// Value between 0 and 1 indicating how strong the hand brake is pulled
void SetHandBrakeInput(float inHandBrake) { mHandBrakeInput = inHandBrake; }
float GetHandBrakeInput() const { return mHandBrakeInput; }
/// Get current engine state
const VehicleEngine & GetEngine() const { return mEngine; }
/// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
VehicleEngine & GetEngine() { return mEngine; }
/// Get current transmission state
const VehicleTransmission & GetTransmission() const { return mTransmission; }
/// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
VehicleTransmission & GetTransmission() { return mTransmission; }
/// Get the differentials this vehicle has
const Differentials & GetDifferentials() const { return mDifferentials; }
/// Get the differentials this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step)
Differentials & GetDifferentials() { return mDifferentials; }
/// Ratio max / min average wheel speed of each differential (measured at the clutch).
float GetDifferentialLimitedSlipRatio() const { return mDifferentialLimitedSlipRatio; }
void SetDifferentialLimitedSlipRatio(float inV) { mDifferentialLimitedSlipRatio = inV; }
/// Get the average wheel speed of all driven wheels (measured at the clutch)
float GetWheelSpeedAtClutch() const;
/// Calculate max tire impulses by combining friction, slip, and suspension impulse. Note that the actual applied impulse may be lower (e.g. when the vehicle is stationary on a horizontal surface the actual impulse applied will be 0).
using TireMaxImpulseCallback = function<void(uint inWheelIndex, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float inLongitudinalSlip, float inLateralSlip, float inDeltaTime)>;
const TireMaxImpulseCallback&GetTireMaxImpulseCallback() const { return mTireMaxImpulseCallback; }
void SetTireMaxImpulseCallback(const TireMaxImpulseCallback &inTireMaxImpulseCallback) { mTireMaxImpulseCallback = inTireMaxImpulseCallback; }
#ifdef JPH_DEBUG_RENDERER
/// Debug drawing of RPM meter
void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; }
#endif // JPH_DEBUG_RENDERER
protected:
// See: VehicleController
virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsWV))); return new WheelWV(static_cast<const WheelSettingsWV &>(inWheel)); }
virtual bool AllowSleep() const override;
virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
virtual void SaveState(StateRecorder &inStream) const override;
virtual void RestoreState(StateRecorder &inStream) override;
#ifdef JPH_DEBUG_RENDERER
virtual void Draw(DebugRenderer *inRenderer) const override;
#endif // JPH_DEBUG_RENDERER
// Control information
float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
float mRightInput = 0.0f; ///< Value between -1 and 1 indicating desired steering angle
float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed
float mHandBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the hand brake is pulled
// Simulation information
VehicleEngine mEngine; ///< Engine state of the vehicle
VehicleTransmission mTransmission; ///< Transmission state of the vehicle
Differentials mDifferentials; ///< Differential states of the vehicle
float mDifferentialLimitedSlipRatio; ///< Ratio max / min average wheel speed of each differential (measured at the clutch).
float mPreviousDeltaTime = 0.0f; ///< Delta time of the last step
// Callback that calculates the max impulse that the tire can apply to the ground
TireMaxImpulseCallback mTireMaxImpulseCallback =
[](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float)
{
outLongitudinalImpulse = inLongitudinalFriction * inSuspensionImpulse;
outLateralImpulse = inLateralFriction * inSuspensionImpulse;
};
#ifdef JPH_DEBUG_RENDERER
// Debug settings
Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint
float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint
#endif // JPH_DEBUG_RENDERER
};
JPH_NAMESPACE_END