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

#pragma once
#include <UnigineComponentSystem.h>
#include <UniginePhysics.h>
class Car : public Unigine::ComponentBase
{
public:
	// конструктор компонента и список методов
	COMPONENT_DEFINE(Car, ComponentBase)
	// -------------------------------
	COMPONENT_INIT(init);
	COMPONENT_UPDATE(update);
	COMPONENT_UPDATE_PHYSICS(updatePhysics);

	enum MOVE_DIRECTION
	{
		FORWARD,
		REVERSE,
	};

	// параметры автомобиля: ускорение, максимальная скорость и поворот руля, крутящий момент
	PROP_PARAM(Float, acceleration, 50.0f);
	PROP_PARAM(Float, max_velocity, 90.0f);
	PROP_PARAM(Float, default_torque, 5.0f);

	// длина и ширина кузова
	PROP_PARAM(Float, car_base, 3.0f);
	PROP_PARAM(Float, car_width, 2.0f);

	// быстрота газа, тормоза и поворота
	PROP_PARAM(Float, throttle_speed, 2.0f);
	PROP_PARAM(Float, brake_speed, 1.2f);
	PROP_PARAM(Float, wheel_speed, 2.0f);

	// сила рабочего и стояночного тормоза
	PROP_PARAM(Float, brake_damping, 8.0f);
	PROP_PARAM(Float, hand_brake_damping, 30.0f);

	// ссылки на ноды колес
	PROP_PARAM(Node, wheel_fl, nullptr);
	PROP_PARAM(Node, wheel_fr, nullptr);
	PROP_PARAM(Node, wheel_rl, nullptr);
	PROP_PARAM(Node, wheel_rr, nullptr);

	// ссылки на ноды светотехники: стоп-сигнал и фонарь заднего хода
	PROP_PARAM(Node, brake_light, nullptr);
	PROP_PARAM(Node, reverse_light, nullptr);

	// задание основных параметров движения
	void setThrottle(float value);
	void setBrake(float value);
	void setWheelPosition(float value);
	void setHandBrake(float value);

	// метод для мгновенного перемещения автомобиля, будет использован для возвращения авто на начальную позицию
	void reset(Unigine::Math::mat4 transform);

	// метод смены режима движения, здесь также происходит управление фонарем заднего хода
	void setMoveDirection(MOVE_DIRECTION value);
	MOVE_DIRECTION getCurrentMoveDirection() { return current_move_direction; };

	// получение скорости сразу в км/ч
	float getSpeed() { return carBodyRigid->getLinearVelocity().length() * 3.6f; }

private:
	float max_turn_angle = 30.0f;

	// колесные сочленения
	Unigine::JointWheelPtr joint_wheel_fl = nullptr;
	Unigine::JointWheelPtr joint_wheel_fr = nullptr;
	Unigine::JointWheelPtr joint_wheel_rl = nullptr;
	Unigine::JointWheelPtr joint_wheel_rr = nullptr;

	// определим желаемые и текущие значения для газа, тормоза, руля и стояночного тормоза
	float target_throttle = 0.0f;
	float target_brake = 0.0f;
	float target_wheel = 0.0f;
	float target_hand_brake = 0.0f;

	float current_throttle = 0.0f;
	float current_brake = 0.0f;
	float current_wheel = 0.0f;
	float current_hand_brake = 0.0f;

	// по умолчанию автомобиль в режиме движения вперёд
	MOVE_DIRECTION current_move_direction = MOVE_DIRECTION::FORWARD;

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

	// физическое тело кузова
	Unigine::BodyRigidPtr carBodyRigid = nullptr;

protected:
	// методы основного цикла
	void init();
	void update();
	void updatePhysics();
};

#include "Car.h"

#include <UnigineGame.h>
using namespace Unigine;
using namespace Math;

REGISTER_COMPONENT(Car);

float moveTowards(float current, float target, float max_delta)
{
	if (Math::abs(target - current) <= max_delta)
		return target;
	return current + Math::sign(target - current) * max_delta;
}

void Car::init()
{
	// при инициализации получаем колесные сочленения и физическое тело кузова
	if (wheel_rl)
		joint_wheel_rl = checked_ptr_cast<JointWheel>(wheel_rl->getObjectBody()->getJoint(0));

	if (wheel_rr)
		joint_wheel_rr = checked_ptr_cast<JointWheel>(wheel_rr->getObjectBody()->getJoint(0));

	if (wheel_fl)
		joint_wheel_fl = checked_ptr_cast<JointWheel>(wheel_fl->getObjectBody()->getJoint(0));

	if (wheel_fr)
		joint_wheel_fr = checked_ptr_cast<JointWheel>(wheel_fr->getObjectBody()->getJoint(0));

	carBodyRigid = node->getObjectBodyRigid();
}

void Car::update()
{
	// используем время рендера предыдущего кадра, чтобы не зависеть от FPS
	float deltaTime = Game::getIFps();

	// плавно изменяем текущие газ, тормоз и положение руля в сторону требуемых
	current_throttle = moveTowards(current_throttle, target_throttle, throttle_speed * deltaTime);
	current_brake = moveTowards(current_brake, target_brake, brake_speed * deltaTime);
	current_wheel = moveTowards(current_wheel, target_wheel, wheel_speed * deltaTime);
	current_hand_brake = moveTowards(current_hand_brake, target_hand_brake, brake_speed * deltaTime);

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

	// при нажатии на газ
	if (current_throttle > Math::Consts::EPS)
	{
		// текущая угловая скорость колес изменяется согласно ускорению и направлению движения
		current_velocity += deltaTime * Math::lerp(0.0f, acceleration, current_throttle) * (current_move_direction == MOVE_DIRECTION::FORWARD ? 1.0f : -1.0f);
	}
	else
	{
		// в противном случае снижаем скорость экспоненциально
		current_velocity *= Math::exp(-deltaTime);
	}

	// вычисляем силу тормозов в зависимости от их текущей интенсивности
	float damping = Math::lerp(0.0f, brake_damping, current_brake);
	float rdamping = Math::lerp(0.0f, hand_brake_damping, current_hand_brake);
	// применяем торможение для всех колес, для задних колес применяется также и стояночный тормоз
	joint_wheel_fl->setAngularDamping(damping);
	joint_wheel_fr->setAngularDamping(damping);
	joint_wheel_rl->setAngularDamping(Math::max(damping, rdamping));
	joint_wheel_rr->setAngularDamping(Math::max(damping, rdamping));

	// вычисляем текущие угловую скорость и угол поворота, ограничив крайние значения
	current_velocity = Math::clamp(current_velocity, -max_velocity, max_velocity);
	current_turn_angle = Math::lerp(-max_turn_angle, max_turn_angle, Math::clamp(0.5f + current_wheel * 0.5f, 0.0f, 1.0f));

	// симуляция дифференциала для передней оси: колеса должны повернуться на различный угол
	float angle_0 = current_turn_angle;
	float angle_1 = current_turn_angle;
	if (Math::abs(current_turn_angle) > Math::Consts::EPS)
	{
		float radius = car_base / Math::tan(current_turn_angle * Math::Consts::DEG2RAD);
		float radius_0 = radius - car_width * 0.5f;
		float radius_1 = radius + car_width * 0.5f;

		angle_0 = Math::atan(car_base / radius_0) * Math::Consts::RAD2DEG;
		angle_1 = Math::atan(car_base / radius_1) * Math::Consts::RAD2DEG;
	}
	// применяем поворот для обоих передних колес при помощи матрицы поворота вдоль оси Z
	joint_wheel_fr->setAxis10(Math::rotateZ(angle_1).getColumn3(0));
	joint_wheel_fl->setAxis10(Math::rotateZ(angle_0).getColumn3(0));
}

// параметры физических объектов важно изменять в методе UpdatePhysics
void Car::updatePhysics()
{
	// применяем расчетные значения угловой скорости и крутящего момента колес
	// все 4 колеса имеют 'двигатель', т.е. автомобиль полноприводный.
	joint_wheel_fl->setAngularVelocity(current_velocity);
	joint_wheel_fr->setAngularVelocity(current_velocity);

	joint_wheel_fl->setAngularTorque(current_torque);
	joint_wheel_fr->setAngularTorque(current_torque);

	joint_wheel_rl->setAngularVelocity(current_velocity);
	joint_wheel_rr->setAngularVelocity(current_velocity);

	joint_wheel_rl->setAngularTorque(current_torque);
	joint_wheel_rr->setAngularTorque(current_torque);
}

// добавим методы для управления автомобилем: газ, тормоз, поворот руля и стояночный тормоз
void Car::setThrottle(float value)
{
	target_throttle = Math::clamp(value, 0.0f, 1.0f);
}

void Car::setBrake(float value)
{
	target_brake = Math::clamp(value, 0.0f, 1.0f);
}

void Car::setWheelPosition(float value)
{
	target_wheel = Math::clamp(value, -1.0f, 1.0f);
}

void Car::setHandBrake(float value)
{
	target_hand_brake = Math::clamp(value, -1.0f, 1.0f);
}

// метод смены режима движения, здесь также происходит управление фонарем заднего хода
void Car::setMoveDirection(Car::MOVE_DIRECTION value)
{
	if (current_move_direction == value)
		return;
	current_velocity = 0.0f;
	current_move_direction = value;
	if (reverse_light.get() != nullptr)
		reverse_light->setEnabled(current_move_direction == Car::MOVE_DIRECTION::REVERSE);
}

// метод для мгновенного перемещения автомобиля, будет использован для возвращения авто на начальную позицию
void Car::reset(Math::mat4 transform)
{
	node->setWorldTransform(transform);
	node->getObjectBodyRigid()->setLinearVelocity(Vec3_zero);
	node->getObjectBodyRigid()->setAngularVelocity(Vec3_zero);
	current_velocity = 0.0f;
}