dirt-racer/src/car_physics.cpp

#include "car_physics.hpp"
#include "godot_cpp/classes/collision_object3d.hpp"
#include "godot_cpp/variant/utility_functions.hpp"
#include "utils/godot_macros.h"
#include <godot_cpp/classes/curve.hpp>

namespace godot {
void CarPhysics::_bind_methods() {
#define CLASSNAME CarPhysics
    GDPROPERTY_HINTED(oversteer_curve, Variant::OBJECT, PROPERTY_HINT_RESOURCE_TYPE, "Curve");
    GDPROPERTY(oversteer_curve_x_scale, Variant::FLOAT);
    GDPROPERTY_HINTED(understeer_curve, Variant::OBJECT, PROPERTY_HINT_RESOURCE_TYPE, "Curve");
    GDPROPERTY(understeer_curve_x_scale, Variant::FLOAT);
    GDPROPERTY(acceleration, Variant::FLOAT);
    GDPROPERTY(engine_brake_force, Variant::FLOAT);
    GDPROPERTY(handbrake_force, Variant::FLOAT);
    GDPROPERTY(handbrake_oversteer, Variant::FLOAT);
    GDPROPERTY(traction_recovery_speed, Variant::FLOAT);
    GDPROPERTY(max_slide_speed, Variant::FLOAT);
    GDPROPERTY(slide_speed_acceleration, Variant::FLOAT);
    GDPROPERTY(oversteer_speed_penalty, Variant::FLOAT);
    GDPROPERTY(oversteer_brake_penalty, Variant::FLOAT);
    GDPROPERTY(oversteer_steering_speed, Variant::FLOAT);
    GDPROPERTY(slide_resistance, Variant::FLOAT);
    GDPROPERTY(steering_inward_speed, Variant::FLOAT);
}

void CarPhysics::_enter_tree() {
    this->set_use_custom_integrator(true);
    this->front_wheels = this->get_node<CollisionShape3D>("FrontWheels");
    this->back_wheels = this->get_node<CollisionShape3D>("BackWheels");
    this->set_contact_monitor(true);
    this->set_max_contacts_reported(128);

    GDGAMEONLY();
    this->connect("body_shape_entered", callable_mp(this, &CarPhysics::on_body_shape_entered));
    this->connect("body_shape_exited", callable_mp(this, &CarPhysics::on_body_shape_exited));
}

void CarPhysics::_physics_process(double delta_time) { GDGAMEONLY();
    this->process_oversteer(delta_time);
    this->process_understeer(delta_time);
    UtilityFunctions::print("true target speed: ", this->get_true_target_speed());
    UtilityFunctions::print("speed: ", this->local_velocity.z);
    UtilityFunctions::print("grounded: ", this->grounded_objects.size());
    UtilityFunctions::print("brakes: ", this->brake);
    UtilityFunctions::print("acceleration: ", this->get_current_acceleration());
    UtilityFunctions::print("-----------------------------------------------------------");
}

void CarPhysics::process_oversteer(double delta_time) {
    float const target = this->current_steering == 0.f
        ? 0.f
        : (this->evaluate_oversteer_curve(this->local_velocity.z) + (this->brake ? this->handbrake_oversteer : 0.f)) * Math::sign(this->current_steering);
    if(Math::abs(target) >=  Math::abs(this->current_oversteer) && Math::sign(target) == Math::sign(this->current_oversteer))
        this->current_oversteer = target;
    else
        this->current_oversteer = Math::move_toward(this->current_oversteer, target, float(delta_time * this->traction_recovery_speed));
    UtilityFunctions::print("current oversteer: ", this->current_oversteer, " target: ", target);
}

void CarPhysics::process_understeer(double delta_time) {
    float const target = this->evaluate_understeer_curve(this->local_velocity.z);
    if(target >= this->current_understeer)
        this->current_understeer = target;
    else
        this->current_understeer = Math::move_toward(this->current_understeer, target, float(delta_time * this->traction_recovery_speed));
    UtilityFunctions::print("current understeer: ", this->current_understeer, " target: ", target);
}

void CarPhysics::_integrate_forces(PhysicsDirectBodyState3D *state) {
    if(this->is_grounded()) {
        this->integrate_engine_acceleration(state);
        this->integrate_steering(state);
        this->integrate_oversteer(state);
    }
    this->last_velocity = state->get_linear_velocity();
    state->set_linear_velocity(
            (this->is_grounded()
                ? (this->local_to_world_velocity() + Vector3{0, float(-9.8f * state->get_step()), 0})
                : (this->get_linear_velocity() + Vector3{0, float(-9.8f * state->get_step()), 0})));
}

void CarPhysics::integrate_steering(PhysicsDirectBodyState3D *state) {
    float target_x_velocity = 0.f;
    if(this->local_velocity.z != 0.f) {
        Vector3 const steering_angle_vel = this->get_global_basis().get_column(1) * (
                this->current_steering * Math::sign(this->local_velocity.z + this->current_oversteer * this->oversteer_steering_speed));
        state->set_angular_velocity(steering_angle_vel * (1.0f-this->current_understeer));
        target_x_velocity = this->current_steering * this->steering_inward_speed;
    } else {
        state->set_angular_velocity(Vector3(0,0,0));
    }
    this->local_velocity.x = Math::move_toward(this->local_velocity.x,
                                               target_x_velocity,
                                               float(this->slide_resistance * (1.f-this->current_oversteer) * state->get_step()));
}

void CarPhysics::integrate_engine_acceleration(PhysicsDirectBodyState3D *state) {
    Vector3 const jolt = state->get_linear_velocity() - this->last_velocity;
    float const z_jolt = jolt.dot(this->get_global_basis().get_column(2));

    if(Math::abs(z_jolt) >= Math::abs(this->local_velocity.z) / 2.f && Math::sign(z_jolt) != Math::sign(this->local_velocity.z)) {
        this->local_velocity.z = 0.f;
    }

    this->local_velocity.z = Math::move_toward(
            this->local_velocity.z,
            this->get_true_target_speed(),
            float(this->get_current_acceleration() * state->get_step()));
}

void CarPhysics::integrate_oversteer(PhysicsDirectBodyState3D *state) {
    this->local_velocity.x = Math::clamp(
        this->local_velocity.x - this->local_velocity.z * this->current_oversteer * this->slide_speed_acceleration,
        -this->max_slide_speed,
        this->max_slide_speed);
}

void CarPhysics::on_body_shape_entered(RID body_rid, Node *body, int body_shape_index, int local_shape_index) {
    CollisionShape3D *shape = Object::cast_to<CollisionShape3D>(this->shape_owner_get_owner(this->shape_find_owner(local_shape_index)));
    bool const was_grounded = this->is_grounded();
    if(shape == this->front_wheels || shape == this->back_wheels)
        this->grounded_objects.insert(body);
    if(!was_grounded && this->is_grounded()) {
        this->local_velocity = this->world_to_local_velocity();
    }
}

void CarPhysics::on_body_shape_exited(RID body_rid, Node *body, int body_shape_index, int local_shape_index) {
    if(this->grounded_objects.has(body))
        this->grounded_objects.erase(body);
}

Vector3 CarPhysics::local_to_world_velocity() const {
    Basis const basis = this->get_global_basis();
    Vector3 world_velocity{
        this->local_velocity.x * basis.get_column(0) +
        this->local_velocity.y * basis.get_column(1) +
        this->local_velocity.z * basis.get_column(2)
    };
    return world_velocity;
}

Vector3 CarPhysics::world_to_local_velocity() const {
    Basis const basis = this->get_global_basis();
    Vector3 const velocity = this->get_linear_velocity();
    return Vector3{
        basis.get_column(0).dot(velocity),
        0.f,
        basis.get_column(2).dot(velocity)
    };
}

float CarPhysics::evaluate_oversteer_curve(float speed) const {
    return this->current_steering != 0.f
        ? this->oversteer_curve->sample(speed / this->oversteer_curve_x_scale)
        : 0.f;
}

float CarPhysics::evaluate_understeer_curve(float speed) const {
    return this->current_steering != 0.f
        ? this->understeer_curve->sample(speed / this->understeer_curve_x_scale)
        : 0.f;
}

float CarPhysics::get_true_target_speed() const {
        return (this->brake ? 0.f
                : (this->target_speed > 0
                    ? this->target_speed - (Math::abs(this->current_oversteer) * this->oversteer_speed_penalty
                        + this->current_understeer * this->understeer_speed_penalty)
                    : this->target_speed));
}

bool CarPhysics::is_grounded() const {
    return !this->grounded_objects.is_empty();
}

float CarPhysics::get_current_acceleration() const {
 return Math::abs(this->brake
         ? this->handbrake_force
         : ((Math::abs(this->get_true_target_speed()) >= Math::abs(this->local_velocity.z))
            ? (this->acceleration * (1.f-Math::abs(this->current_oversteer)))
            : (this->engine_brake_force * (1.f-Math::abs(this->current_oversteer * this->oversteer_brake_penalty)))));
}

void CarPhysics::set_target_speed(float target) {
    this->target_speed = target;
}

float CarPhysics::get_target_speed() const {
    return this->target_speed;
}

void CarPhysics::set_current_steering(float steering) {
    this->current_steering = steering;
}

float CarPhysics::get_current_steering() const {
    return this->current_steering;
}

void CarPhysics::set_brake(bool brake) {
    this->brake = brake;
}

bool CarPhysics::get_brake() const {
    return this->brake;
}

void CarPhysics::set_oversteer_curve(Ref<Curve> curve) {
    this->oversteer_curve = curve;
}

Ref<Curve> CarPhysics::get_oversteer_curve() const {
    return this->oversteer_curve;
}

void CarPhysics::set_oversteer_curve_x_scale(float scale) {
    this->oversteer_curve_x_scale = scale;
}

float CarPhysics::get_oversteer_curve_x_scale() const {
    return this->oversteer_curve_x_scale;
}

void CarPhysics::set_understeer_curve(Ref<Curve> curve) {
    this->understeer_curve = curve;
}

Ref<Curve> CarPhysics::get_understeer_curve() const {
    return this->understeer_curve;
}

void CarPhysics::set_understeer_curve_x_scale(float scale) {
    this->understeer_curve_x_scale = scale;
}

float CarPhysics::get_understeer_curve_x_scale() const {
    return this->understeer_curve_x_scale;
}

void CarPhysics::set_acceleration(float value) {
    this->acceleration = value;
}

float CarPhysics::get_acceleration() const {
    return this->acceleration;
}

void CarPhysics::set_engine_brake_force(float value) {
    this->engine_brake_force = value;
}

float CarPhysics::get_engine_brake_force() const {
    return this->engine_brake_force;
}

void CarPhysics::set_handbrake_force(float value) {
    this->handbrake_force = value;
}

float CarPhysics::get_handbrake_force() const {
    return this->handbrake_force;
}

void CarPhysics::set_handbrake_oversteer(float value) {
    this->handbrake_oversteer = value;
}

float CarPhysics::get_handbrake_oversteer() const {
    return this->handbrake_oversteer;
}

void CarPhysics::set_traction_recovery_speed(float value) {
    this->traction_recovery_speed = value;
}

float CarPhysics::get_traction_recovery_speed() const {
    return this->traction_recovery_speed;
}

void CarPhysics::set_max_slide_speed(float value) {
    this->max_slide_speed = value;
}

float CarPhysics::get_max_slide_speed() const {
    return this->max_slide_speed;
}

void CarPhysics::set_slide_speed_acceleration(float value) {
    this->slide_speed_acceleration = value;
}

float CarPhysics::get_slide_speed_acceleration() const {
    return this->slide_speed_acceleration;
}

void CarPhysics::set_oversteer_speed_penalty(float value) {
    this->oversteer_speed_penalty = value;
}

float CarPhysics::get_oversteer_speed_penalty() const {
    return this->oversteer_speed_penalty;
}

void CarPhysics::set_oversteer_brake_penalty(float value) {
    this->oversteer_brake_penalty = value;
}

float CarPhysics::get_oversteer_brake_penalty() const {
    return this->oversteer_brake_penalty;
}

void CarPhysics::set_oversteer_steering_speed(float value) {
    this->oversteer_steering_speed = value;
}

float CarPhysics::get_oversteer_steering_speed() const {
    return this->oversteer_steering_speed;
}

void CarPhysics::set_slide_resistance(float value) {
    this->slide_resistance = value;
}

float CarPhysics::get_slide_resistance() const {
    return this->slide_resistance;
}

void CarPhysics::set_steering_inward_speed(float value) {
    this->steering_inward_speed = value;
}

float CarPhysics::get_steering_inward_speed() const {
    return this->steering_inward_speed;
}
}