From 5c728b1b3991515f2b119c495d84b2a22f39b29b Mon Sep 17 00:00:00 2001
From: Sara <sara@saragerretsen.nl>
Date: Thu, 16 May 2024 09:35:28 +0200
Subject: [PATCH] feat: first attempt at car physics integrator

---
 src/car_physics.cpp | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/car_physics.cpp b/src/car_physics.cpp
index be63566..5421f7b 100644
--- a/src/car_physics.cpp
+++ b/src/car_physics.cpp
@@ -14,6 +14,28 @@ void CarPhysics::_enter_tree() {
 }
 
 void CarPhysics::_integrate_forces(PhysicsDirectBodyState3D *state) {
+    Basis const basis = this->get_global_basis();
+
+    this->local_velocity.z = Math::move_toward(this->local_velocity.z, this->target_speed, float(ACCELERATION * state->get_step()));
+    if(this->local_velocity.z != 0.f) {
+        state->set_angular_velocity(basis.get_column(1) * -this->current_steering * Math::sign(this->local_velocity.z));
+        this->local_velocity.x = -this->current_steering * 0.1f;
+    } else {
+        state->set_angular_velocity(Vector3(0,0,0));
+        this->local_velocity.x = 0;
+    }
+    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)
+    };
+    world_velocity.y = 0;
+    if(state->get_linear_velocity().y > this->gravity_velocity && state->get_linear_velocity().y <= 0.f)
+        this->gravity_velocity = 0.f;
+    else
+        this->gravity_velocity -= 15.f * state->get_step();
+    state->set_linear_velocity(world_velocity + Vector3{0, this->gravity_velocity, 0});
+}
 
 void CarPhysics::set_target_speed(float target) {
     this->target_speed = target;