From 81bec0b2b40f9d8a03a97df1494c683777b799e1 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Mon, 28 Apr 2025 16:52:05 -0700
Subject: [PATCH] use computed center of mass

---
 src/bike.rs    |  2 +-
 src/physics.rs | 39 +++++++++++++++++++++++++++------------
 2 files changed, 28 insertions(+), 13 deletions(-)

diff --git a/src/bike.rs b/src/bike.rs
index 6abf8a4..03e3136 100644
--- a/src/bike.rs
+++ b/src/bike.rs
@@ -18,7 +18,7 @@ use bevy::{
 
 use crate::physics::CatControllerState;
 
-pub const SPRING_CONSTANT: Scalar = 50.0;
+pub const SPRING_CONSTANT: Scalar = 40.0;
 pub const DAMPING_CONSTANT: Scalar = 10.0;
 pub const WHEEL_RADIUS: Scalar = 0.4;
 pub const REST_DISTANCE: Scalar = 1.5 + WHEEL_RADIUS;
diff --git a/src/physics.rs b/src/physics.rs
index b4012bd..481d0d0 100644
--- a/src/physics.rs
+++ b/src/physics.rs
@@ -22,7 +22,7 @@ impl Default for CatControllerSettings {
         Self {
             kp: 45.0,
             kd: 15.0,
-            ki: 1.0,
+            ki: 2.5,
         }
     }
 }
@@ -57,7 +57,10 @@ impl CatControllerState {
 mod systems {
     use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
-    use avian3d::prelude::*;
+    use avian3d::prelude::{
+        ComputedCenterOfMass, ExternalForce, Gravity, LinearVelocity, RayCaster, RayHits,
+        RigidBodyQueryReadOnly,
+    };
     use bevy::{
         ecs::error::BevyError,
         prelude::{
@@ -183,7 +186,10 @@ mod systems {
     }
 
     pub(super) fn suspension(
-        mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
+        mut bike_body_query: Query<
+            (&Transform, &ComputedCenterOfMass, &mut ExternalForce),
+            With<CyberBikeBody>,
+        >,
         mut wheel_mesh_query: Query<
             (&mut Transform, &mut WheelState, &WheelConfig, &CyberWheel),
             Without<CyberBikeBody>,
@@ -213,7 +219,7 @@ mod systems {
             }
         }
 
-        let (bike_xform, mut bike_forces) = bike_body_query.single_mut()?;
+        let (bike_xform, com, mut bike_forces) = bike_body_query.single_mut()?;
 
         for (mut xform, mut state, config, wheel) in wheel_mesh_query.iter_mut() {
             let (caster, hits) = match wheel {
@@ -248,7 +254,7 @@ mod systems {
                 bike_forces.apply_force_at_point(
                     force,
                     caster.global_origin(),
-                    bike_xform.translation,
+                    bike_xform.translation + com.0,
                 );
             } else {
                 xform.translation = config.attach + (caster.direction.as_vec3() * config.rest_dist);
@@ -277,7 +283,7 @@ mod systems {
         let bike_vel = bike_vel.0;
 
         let dt = time.delta().as_secs_f32();
-        let max_thrust = 5000.0;
+        let max_thrust = 1000.0;
         let yaw_angle = -yaw_to_angle(input.yaw);
 
         for (mut state, config, wheel) in wheels.iter_mut() {
@@ -290,7 +296,7 @@ mod systems {
                 let forward = normal.cross(*bike_xform.right());
                 let thrust_mag = input.throttle * max_thrust * dt;
                 let (thrust_dir, thrust_force) = match wheel {
-                    CyberWheel::Rear => (forward, thrust_mag * 0.1),
+                    CyberWheel::Rear => (forward, thrust_mag * 0.5),
                     CyberWheel::Front => (rot * forward, thrust_mag),
                 };
 
@@ -318,7 +324,11 @@ mod systems {
                     state.sliding = false;
                 }
 
-                bike_force.apply_force_at_point(force, contact_point, bike_xform.translation);
+                bike_force.apply_force_at_point(
+                    force,
+                    contact_point,
+                    bike_xform.translation + bike_body.center_of_mass.0,
+                );
                 gizmos.arrow(
                     contact_point,
                     contact_point + friction,
@@ -337,13 +347,18 @@ mod systems {
 
     pub(super) fn drag(
         mut bike_query: Query<
-            (&Transform, &LinearVelocity, &mut ExternalForce),
+            (
+                &Transform,
+                &LinearVelocity,
+                &ComputedCenterOfMass,
+                &mut ExternalForce,
+            ),
             With<CyberBikeBody>,
         >,
         mut gizmos: Gizmos,
         time: Res<Time>,
     ) -> Result {
-        let (xform, vel, mut force) = bike_query.single_mut()?;
+        let (xform, vel, com, mut force) = bike_query.single_mut()?;
 
         let dt = time.delta_secs();
 
@@ -354,8 +369,8 @@ mod systems {
         if speed > 1.0 {
             force.apply_force_at_point(
                 drag,
-                xform.translation - (0.1 * *xform.down()),
-                xform.translation,
+                xform.translation - (0.2 * *xform.down()),
+                xform.translation + com.0,
             );
         }