Implementing Vehicle Physics

#pragma once
#include <UnigineComponentSystem.h>
#include <UniginePhysics.h>
class Car : public Unigine::ComponentBase
{
public:
	// component constructor and the list of methods
	COMPONENT_DEFINE(Car, ComponentBase)
	// -------------------------------
	COMPONENT_INIT(init);
	COMPONENT_UPDATE(update);
	COMPONENT_UPDATE_PHYSICS(updatePhysics);

	enum MOVE_DIRECTION
	{
		FORWARD,
		REVERSE,
	};

	// vehicle parameters: acceleration, maximum speed, and wheel turning angle, torque
	PROP_PARAM(Float, acceleration, 50.0f);
	PROP_PARAM(Float, max_velocity, 90.0f);
	PROP_PARAM(Float, default_torque, 5.0f);

	// car body length and width
	PROP_PARAM(Float, car_base, 3.0f);
	PROP_PARAM(Float, car_width, 2.0f);

	// speed of accelerating, braking, and turning
	PROP_PARAM(Float, throttle_speed, 2.0f);
	PROP_PARAM(Float, brake_speed, 1.2f);
	PROP_PARAM(Float, wheel_speed, 2.0f);

	// service and hand brake force
	PROP_PARAM(Float, brake_damping, 8.0f);
	PROP_PARAM(Float, hand_brake_damping, 30.0f);

	// wheel joints
	PROP_PARAM(Node, wheel_fl, nullptr);
	PROP_PARAM(Node, wheel_fr, nullptr);
	PROP_PARAM(Node, wheel_rl, nullptr);
	PROP_PARAM(Node, wheel_rr, nullptr);

	// references to light nodes: brake and reverse light
	PROP_PARAM(Node, brake_light, nullptr);
	PROP_PARAM(Node, reverse_light, nullptr);

	// define the desired and current values for throttle, brake, steering wheel and hand brake
	void setThrottle(float value);
	void setBrake(float value);
	void setWheelPosition(float value);
	void setHandBrake(float value);

	// method for the instant car relocation, returns the car to the initial position
	void reset(Unigine::Math::mat4 transform);

	// method for changing the driving mode, it also controls the reverse light
	void setMoveDirection(MOVE_DIRECTION value);
	MOVE_DIRECTION getCurrentMoveDirection() { return current_move_direction; };

	// get speed immediately in km/h
	float getSpeed() { return carBodyRigid->getLinearVelocity().length() * 3.6f; }

private:
	float max_turn_angle = 30.0f;

	// wheel joints
	Unigine::JointWheelPtr joint_wheel_fl = nullptr;
	Unigine::JointWheelPtr joint_wheel_fr = nullptr;
	Unigine::JointWheelPtr joint_wheel_rl = nullptr;
	Unigine::JointWheelPtr joint_wheel_rr = nullptr;

	// define the desired and current values for throttle, brake, steering wheel, and hand brake
	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;

	// by default, the car moves in the Forward direction
	MOVE_DIRECTION current_move_direction = MOVE_DIRECTION::FORWARD;

	// variables for current rotation speed, torque and turn angle
	float current_velocity = 0.0f;
	float current_torque = 0.0f;
	float current_turn_angle = 0.0f;

	// car physical body
	Unigine::BodyRigidPtr carBodyRigid = nullptr;

protected:
	// main loop overrides
	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()
{
	// at initialization, we get wheel joints and car physical body
	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()
{
	// get the time it took to render the previous frame in order to be independent from FPS
	float deltaTime = Game::getIFps();

	// smoothly change the current throttle, brake, and steering position towards the required values
	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);

	// enable the brake light node if the brake is activated (value greater than ~zero)
	if (brake_light.get() != nullptr)
		brake_light->setEnabled(target_brake > Math::Consts::EPS);
	// 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 > Math::Consts::EPS)
	{
		// current angular velocity of wheels changes according to acceleration and motion direction
		current_velocity += deltaTime * Math::lerp(0.0f, acceleration, current_throttle) * (current_move_direction == MOVE_DIRECTION::FORWARD ? 1.0f : -1.0f);
	}
	else
	{
		// otherwise decrease the speed exponentially
		current_velocity *= Math::exp(-deltaTime);
	}

	// calculate the brake force depending on the current brake intensity
	float damping = Math::lerp(0.0f, brake_damping, current_brake);
	float rdamping = Math::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->setAngularDamping(damping);
	joint_wheel_fr->setAngularDamping(damping);
	joint_wheel_rl->setAngularDamping(Math::max(damping, rdamping));
	joint_wheel_rr->setAngularDamping(Math::max(damping, rdamping));

	// calculate the current angular velocity and angle of rotation, limited by the extreme values
	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));

	// 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 (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;
	}
	// apply rotation for both front wheels using the rotation matrix along the Z axis
	joint_wheel_fr->setAxis10(Math::rotateZ(angle_1).getColumn3(0));
	joint_wheel_fl->setAxis10(Math::rotateZ(angle_0).getColumn3(0));
}

// it is important to change the parameters of physical objects in the UpdatePhysics method
void Car::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->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);
}

// add methods to control the car: throttle, brake, steering wheel turning, and hand brake
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);
}

// method for changing the driving mode, it also controls the reverse light
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);
}

// method for the instant car relocation, returns the car to the initial position
void Car::reset(Math::mat4 transform)
{
	node->setWorldTransform(transform);
	node->getObjectBodyRigid()->setLinearVelocity(Vec3_zero);
	node->getObjectBodyRigid()->setAngularVelocity(Vec3_zero);
	current_velocity = 0.0f;
}