实现载具物理

#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()
{
	// 应用计算出的轮子角速度和扭矩
	// 所有四个轮子都有“马达”，即四驱
	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;
}