VehicleConstraint.h

Go to the documentation of this file.

// 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;
 
class JPH_EXPORT VehicleConstraintSettings : public ConstraintSettings
{
public:
    JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, VehicleConstraintSettings)
 
    
    virtual void                SaveBinaryState(StreamOut &inStream) const override;
 
    Vec3                        mUp { 0, 1, 0 };                            
    Vec3                        mForward { 0, 0, 1 };                       
    float                       mMaxPitchRollAngle = JPH_PI;                
    Array<Ref<WheelSettings>>   mWheels;                                    
    Array<VehicleAntiRollBar>   mAntiRollBars;                              
    Ref<VehicleControllerSettings> mController;                             
 
protected:
    virtual void                RestoreBinaryState(StreamIn &inStream) override;
};
 
class JPH_EXPORT VehicleConstraint : public Constraint, public PhysicsStepListener
{
public:
                                VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings);
    virtual                     ~VehicleConstraint() override;
 
    virtual EConstraintSubType  GetSubType() const override                 { return EConstraintSubType::Vehicle; }
 
    void                        SetMaxPitchRollAngle(float inMaxPitchRollAngle) { mCosMaxPitchRollAngle = Cos(inMaxPitchRollAngle); }
 
    void                        SetVehicleCollisionTester(const VehicleCollisionTester *inTester) { mVehicleCollisionTester = inTester; }
 
    using CombineFunction = function<void(uint inWheelIndex, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &inSubShapeID2)>;
 
    void                        SetCombineFriction(const CombineFunction &inCombineFriction) { mCombineFriction = inCombineFriction; }
    const CombineFunction &     GetCombineFriction() const                  { return mCombineFriction; }
 
    using StepCallback = function<void(VehicleConstraint &inVehicle, float inDeltaTime, PhysicsSystem &inPhysicsSystem)>;
 
    const StepCallback &        GetPreStepCallback() const                  { return mPreStepCallback; }
    void                        SetPreStepCallback(const StepCallback &inPreStepCallback) { mPreStepCallback = inPreStepCallback; }
 
    const StepCallback &        GetPostCollideCallback() const              { return mPostCollideCallback; }
    void                        SetPostCollideCallback(const StepCallback &inPostCollideCallback) { mPostCollideCallback = inPostCollideCallback; }
 
    const StepCallback &        GetPostStepCallback() const                 { return mPostStepCallback; }
    void                        SetPostStepCallback(const StepCallback &inPostStepCallback) { mPostStepCallback = inPostStepCallback; }
 
    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); } 
 
    Vec3                        GetLocalForward() const                     { return mForward; }
 
    Vec3                        GetLocalUp() const                          { return mUp; }
 
    Vec3                        GetWorldUp() const                          { return mWorldUp; }
 
    Body *                      GetVehicleBody() const                      { return mBody; }
 
    const VehicleController *   GetController() const                       { return mController; }
 
    VehicleController *         GetController()                             { return mController; }
 
    const Wheels &              GetWheels() const                           { return mWheels; }
 
    Wheels &                    GetWheels()                                 { return mWheels; }
 
    Wheel *                     GetWheel(uint inIdx)                        { return mWheels[inIdx]; }
    const Wheel *               GetWheel(uint inIdx) const                  { return mWheels[inIdx]; }
 
    void                        GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const;
 
    Mat44                       GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
 
    RMat44                      GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
 
    void                        SetNumStepsBetweenCollisionTestActive(uint inSteps) { mNumStepsBetweenCollisionTestActive = inSteps; }
    uint                        GetNumStepsBetweenCollisionTestActive() const { return mNumStepsBetweenCollisionTestActive; }
 
    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(float inDeltaTime, PhysicsSystem &inPhysicsSystem) 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;               
    Vec3                        mGravityOverride = Vec3::sZero();           
 
    // Simulation information
    Body *                      mBody;                                      
    Vec3                        mForward;                                   
    Vec3                        mUp;                                        
    Vec3                        mWorldUp;                                   
    Wheels                      mWheels;                                    
    Array<VehicleAntiRollBar>   mAntiRollBars;                              
    VehicleController *         mController;                                
    bool                        mIsActive = false;                          
    uint                        mNumStepsBetweenCollisionTestActive = 1;    
    uint                        mNumStepsBetweenCollisionTestInactive = 1;  
    uint                        mCurrentStep = 0;                           
 
    // Prevent vehicle from toppling over
    float                       mCosMaxPitchRollAngle;                      
    float                       mCosPitchRollAngle;                         
    Vec3                        mPitchRollRotationAxis { 0, 1, 0 };         
    AngleConstraintPart         mPitchRollPart;                             
 
    // Interfaces
    RefConst<VehicleCollisionTester> mVehicleCollisionTester;               
    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