Реализация физики движения автомобиля

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")] // <-- идентификатор генерируется автоматически для нового компонента
public class Car : Component
{
	// определяем два режима движения: вперед и назад
	protected enum MoveDirection
	{
		Forward,
		Reverse,
	}
	
	// параметры автомобиля: ускорение, максимальная скорость и поворот руля, крутящий момент
	public float acceleration = 50.0f;
	public float max_velocity = 90.0f;
	private float max_turn_angle = 30.0f;
	public float default_torque = 5.0f;
	// длина и ширина кузова
	public float car_base = 3.0f;
	public float car_width = 2.0f;
	// быстрота газа, тормоза и поворота
	public float throttle_speed = 2.0f;
	public float brake_speed = 1.2f;
	public float wheel_speed = 2.0f;
	// сила рабочего и стояночного тормоза
	public float brake_damping = 8.0f;
	public float hand_brake_damping = 30.0f;

	// ссылки на ноды колес
	public Node wheel_fl = null;
	public Node wheel_fr = null;
	public Node wheel_rl = null;
	public Node wheel_rr = null;

	// ссылки на ноды светотехники: стоп-сигнал и фонарь заднего хода
	public Node brake_light = null;
	public Node reverse_light = null;

	// колесные сочленения
	private JointWheel joint_wheel_fl = null;
	private JointWheel joint_wheel_fr = null;
	private JointWheel joint_wheel_rl = null;
	private JointWheel joint_wheel_rr = null;

	// определим желаемые и текущие значения для газа, тормоза, руля и стояночного тормоза
	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;

	// по умолчанию автомобиль в режиме движения вперёд
	private MoveDirection current_move_direction = MoveDirection.Forward;

	// переменные для текущей скорости вращения, крутящего момента и угла поворота
	private float current_velocity = 0.0f;
	private float current_torque = 0.0f;
	private float current_turn_angle = 0.0f;

	// физическое тело кузова
	private BodyRigid CarBodyRigid = null;

	private void Init()
	{
		// при инициализации получаем колесные сочленения и физическое тело кузова
		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()
	{
		// используем время рендера предыдущего кадра, чтобы не зависеть от FPS
		float deltaTime = Game.IFps;

		// плавно изменяем текущие газ, тормоз и положение руля в сторону требуемых
		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);

		// включаем ноду стоп-сигнала, если тормоз активен (значение больше ~нуля)
		if (brake_light != null)
			brake_light.Enabled = target_brake > MathLib.EPSILON;
		// текущее значение крутящего момента вычисляется как произведение положения газа и стандартного множителя
		current_torque = default_torque * current_throttle;

		// при нажатии на газ
		if (current_throttle > MathLib.EPSILON)
		{
			// текущая угловая скорость колес изменяется согласно ускорению и направлению движения
			current_velocity += deltaTime * MathLib.Lerp(0.0f, acceleration, current_throttle) * (current_move_direction == MoveDirection.Forward ? 1.0f : -1.0f);
		}
		else
		{
			// в противном случае снижаем скорость экспоненциально
			current_velocity *= MathLib.Exp(-deltaTime);
		}

		// вычисляем силу тормозов в зависимости от их текущей интенсивности
		float damping = MathLib.Lerp(0.0f, brake_damping, current_brake);
		float rdamping = MathLib.Lerp(0.0f, hand_brake_damping, current_hand_brake);
		// применяем торможение для всех колес, для задних колес применяется также и стояночный тормоз
		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);

		// вычисляем текущие угловую скорость и угол поворота, ограничив крайние значения
		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));

		// симуляция дифференциала для передней оси: колеса должны повернуться на различный угол
		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;
		}
		// применяем поворот для обоих передних колес при помощи матрицы поворота вдоль оси Z
		joint_wheel_fr.Axis10 = MathLib.RotateZ(angle_1).GetColumn3(0);
		joint_wheel_fl.Axis10 = MathLib.RotateZ(angle_0).GetColumn3(0);
	}

	// параметры физических объектов важно изменять в методе UpdatePhysics
	private void UpdatePhysics()
	{
		// применяем расчетные значения угловой скорости и крутящего момента колес
		// все 4 колеса имеют 'двигатель', т.е. автомобиль полноприводный.
		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;
	}

	// добавим методы для управления автомобилем: газ, тормоз, поворот руля и стояночный тормоз
	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);
	}

	// метод смены режима движения, здесь также происходит управление фонарем заднего хода
	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; } }

	// метод для мгновенного перемещения автомобиля, будет использован для возвращения авто на начальную позицию
	public void Reset(Mat4 transform)
	{
		node.WorldTransform = transform;
		node.ObjectBodyRigid.LinearVelocity = vec3.ZERO;
		node.ObjectBodyRigid.AngularVelocity = vec3.ZERO;
		current_velocity = 0.0f;
	}

	// получение скорости сразу в км/ч
	public float Speed { get { return CarBodyRigid.LinearVelocity.Length * 3.6f; } }
}