Implementing Vehicle Physics

using System;
using System.Collections;
using System.Collections.Generic;
using Unigine;
[Component(PropertyGuid = "AUTOGENERATED_GUID")] // <-- identifier is generated automatically for a new component
public class Car : Component
{
    // define two modes of movement: forward and reverse
    protected enum MoveDirection
    {
        Forward,
        Reverse,
    }
   
    // vehicle parameters: acceleration, maximum speed and wheel turning angle, torque
    public float acceleration = 50.0f;
    public float max_velocity = 90.0f;
    private float max_turn_angle = 30.0f;
    public float default_torque = 5.0f;
    // car body length and width
    public float car_base = 3.0f;
    public float car_width = 2.0f;
    // speed of accelerating, braking and turning
    public float throttle_speed = 2.0f;
    public float brake_speed = 1.2f;
    public float wheel_speed = 2.0f;
    // service and hand brake force
    public float brake_damping = 8.0f;
    public float hand_brake_damping = 30.0f;
    // references to wheel nodes
    public Node wheel_fl = null;
    public Node wheel_fr = null;
    public Node wheel_rl = null;
    public Node wheel_rr = null;
   
    // references to light nodes: brake and reverse light
    public Node brake_light = null;
    public Node reverse_light = null;
    // wheel joints
    private JointWheel joint_wheel_fl = null;
    private JointWheel joint_wheel_fr = null;
    private JointWheel joint_wheel_rl = null;
    private JointWheel joint_wheel_rr = null;
   
    // define the desired and current values for throttle, brake, steering wheel and hand brake
    private float target_throttle = 0.0f;
    private float target_brake = 0.0f;
    private float target_wheel = 0.0f;
    private float target_hand_brake = 0.0f;
   
    private float current_throttle = 0.0f;
    private float current_brake = 0.0f;
    private float current_wheel = 0.0f;
    private float current_hand_brake = 0.0f;
   
    // by default, the car moves in the Forward direction
    private MoveDirection current_move_direction = MoveDirection.Forward;
   
    // variables for current rotation speed, torque and turn angle
    private float current_velocity = 0.0f;
    private float current_torque = 0.0f;
    private float current_turn_angle = 0.0f;
   
    // car physical body
    private BodyRigid CarBodyRigid = null;
   
    private void Init()
    {
        // at initialization, get wheel joints and car physical body
        if (wheel_rl)
            joint_wheel_rl = wheel_rl.ObjectBody.GetJoint(0) as JointWheel;
       
        if (wheel_rr)
            joint_wheel_rr = wheel_rr.ObjectBody.GetJoint(0) as JointWheel;
       
        if (wheel_fl)
            joint_wheel_fl = wheel_fl.ObjectBody.GetJoint(0) as JointWheel;
       
        if (wheel_fr)
            joint_wheel_fr = wheel_fr.ObjectBody.GetJoint(0) as JointWheel;
       
        CarBodyRigid = node.ObjectBodyRigid;
    }
    protected virtual void Update()
    {
        // get the time it took to render the previous frame in order to be independent from FPS
        float deltaTime = Game.IFps;
       
        // smoothly change the current throttle, brake, and steering position towards the required values
        current_throttle = MathLib.MoveTowards(current_throttle, target_throttle, throttle_speed * deltaTime);
        current_brake = MathLib.MoveTowards(current_brake, target_brake, brake_speed * deltaTime);
        current_wheel = MathLib.MoveTowards(current_wheel, target_wheel, wheel_speed * deltaTime);
        current_hand_brake = MathLib.MoveTowards(current_hand_brake, target_hand_brake, brake_speed * deltaTime);
       
        // enable the brake light node if the brake is activated (value greater than ~zero)
        if (brake_light != null)
            brake_light.Enabled = target_brake > MathLib.EPSILON;
        // the current torque value is calculated as the product of the throttle position and the standard multiplier
        current_torque = default_torque * current_throttle;
        // when the throttle is pressed
        if (current_throttle > MathLib.EPSILON)
        {
            // current angular velocity of wheels changes according to acceleration and motion direction
            current_velocity += deltaTime * MathLib.Lerp(0.0f, acceleration, current_throttle) 
                                          * (current_move_direction == MoveDirection.Forward ? 1.0f : -1.0f);
        }
        else
        {
            // otherwise decrease the speed exponentially
            current_velocity *= MathLib.Exp(-deltaTime);
        }
        // calculate the brake force depending on the current brake intensity
        float damping = MathLib.Lerp(0.0f, brake_damping, current_brake);
        float rdamping = MathLib.Lerp(0.0f, hand_brake_damping, current_hand_brake);
        // apply braking for all wheels, hand brake is also applied for the rear wheels
        joint_wheel_fl.AngularDamping = damping;
        joint_wheel_fr.AngularDamping = damping;
        joint_wheel_rl.AngularDamping = MathLib.Max(damping, rdamping);
        joint_wheel_rr.AngularDamping = MathLib.Max(damping, rdamping);
        // calculate the current angular velocity and angle of rotation, limited by the extreme values
        current_velocity = MathLib.Clamp(current_velocity, -max_velocity, max_velocity);
        current_turn_angle = MathLib.Lerp(-max_turn_angle, max_turn_angle, MathLib.Clamp(0.5f + current_wheel * 0.5f, 0.0f,1.0f));
        // simulate differential for the front axle: the wheels should turn by different angles
        float angle_0 = current_turn_angle;
        float angle_1 = current_turn_angle;
        if (MathLib.Abs(current_turn_angle) > MathLib.EPSILON)
        {
            float radius = car_base / MathLib.Tan(current_turn_angle * MathLib.DEG2RAD);
            float radius_0 = radius - car_width * 0.5f;
            float radius_1 = radius + car_width * 0.5f;
            angle_0 = MathLib.Atan(car_base / radius_0) * MathLib.RAD2DEG;
            angle_1 = MathLib.Atan(car_base / radius_1) * MathLib.RAD2DEG;
        }
        // apply rotation for both front wheels using the rotation matrix along the Z axis
        joint_wheel_fr.Axis10 = MathLib.RotateZ(angle_1).GetColumn3(0);
        joint_wheel_fl.Axis10 = MathLib.RotateZ(angle_0).GetColumn3(0);
    }
    // it is important to change the parameters of physical objects in the UpdatePhysics method
    private void UpdatePhysics()
    {
        // apply the calculated values of wheels angular velocity and torque
        // all 4 wheels have a "motor", i.e. the car is all-wheel drive
        joint_wheel_fl.AngularVelocity = current_velocity;
        joint_wheel_fr.AngularVelocity = current_velocity;
        joint_wheel_fl.AngularTorque = current_torque;
        joint_wheel_fr.AngularTorque = current_torque;
       
        joint_wheel_rl.AngularVelocity = current_velocity;
        joint_wheel_rr.AngularVelocity = current_velocity;
        joint_wheel_rl.AngularTorque = current_torque;
        joint_wheel_rr.AngularTorque = current_torque;
    }
   
    // add methods to control the car: throttle, brake, steering wheel turning and hand brake
    protected void SetThrottle(float value)
    {
        target_throttle = MathLib.Clamp(value, 0.0f, 1.0f);
    }
   
    protected void SetBrake(float value)
    {
        target_brake = MathLib.Clamp(value, 0.0f, 1.0f);
    }
   
    protected void SetWheelPosition(float value)
    {
        target_wheel = MathLib.Clamp(value, -1.0f, 1.0f);
    }
   
    protected void SetHandBrake(float value)
    {
        target_hand_brake = MathLib.Clamp(value, -1.0f, 1.0f);
    }
   
    // method for changing the driving mode, it also controls the reverse light
    protected void SetMoveDirection(MoveDirection value)
    {
        if (current_move_direction == value)
            return;
        current_velocity = 0.0f;
        current_move_direction = value;
        if (reverse_light != null)
            reverse_light.Enabled = current_move_direction == MoveDirection.Reverse;
    }
   
    protected MoveDirection CurrentMoveDirection { get { return current_move_direction; } }
   
    // method for the instant car relocation, returns the car to the initial position
    public void Reset(mat4 transform)
    {
        node.WorldTransform = transform;
        node.ObjectBodyRigid.LinearVelocity = vec3.ZERO;
        node.ObjectBodyRigid.AngularVelocity = vec3.ZERO;
        current_velocity = 0.0f;
    }
   
    // get speed immediately in km/h
    public float Speed { get { return CarBodyRigid.LinearVelocity.Length * 3.6f; } }
}