Implementing Vehicle Physics

using System;
using System.Collections;
using System.Collections.Generic;
using Unigine;

#region Math Variables
#if UNIGINE_DOUBLE
using Vec3 = Unigine.dvec3;
using Mat4 = Unigine.dmat4;
#else
using Vec3 = Unigine.vec3;
using Mat4 = Unigine.mat4;
#endif
#endregion

[Component(PropertyGuid = "AUTOGENERATED_GUID")] // <-- this line 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; } }
}