From 18766a3d5be572d51e716cce83b4f0103aa79860 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Sun, 16 Jun 2024 09:21:42 -0700
Subject: [PATCH] checkpoint, no normal force yet

---
 src/action/systems.rs | 34 +++++++++++++++++++++++-----------
 1 file changed, 23 insertions(+), 11 deletions(-)

diff --git a/src/action/systems.rs b/src/action/systems.rs
index 069876d..5461388 100644
--- a/src/action/systems.rs
+++ b/src/action/systems.rs
@@ -1,7 +1,7 @@
 use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
 use bevy::prelude::{
-    Commands, Entity, Local, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
+    info, Commands, Entity, Local, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
 };
 use bevy_rapier3d::{
     dynamics::MassProperties,
@@ -135,6 +135,7 @@ pub(super) fn wheel_forces(
             &Transform,
             &Velocity,
             &mut PreviousVelocity,
+            &mut ExternalForce,
             Option<&CyberSteering>,
         ),
         (With<CyberWheel>, Without<CyberBikeBody>),
@@ -168,31 +169,42 @@ pub(super) fn wheel_forces(
     // we're just projecting onto the ground, so nuke the Y.
     body_forward.y = 0.0;
     let body_half_mass = body_mass.mass * 0.5;
-    let g = gvec * settings.gravity * body_half_mass;
+    let half_weight = settings.gravity * body_half_mass;
 
     // inputs
-    let steering_angle = yaw_to_angle(input.yaw);
+    let steering = input.yaw;
     let throttle = input.throttle * settings.accel;
 
-    for (xform, vel, mut pvel, steering) in wheels_query.iter_mut() {
+    for (xform, vel, mut pvel, mut force, steering) in wheels_query.iter_mut() {
         if let Some((_, projected)) =
             context.project_point(xform.translation, true, QueryFilter::only_fixed())
         {
-            let normal = -(projected.point - xform.translation);
+            info!("projected point: {:?}", projected.point);
+            //dbg!("{:?}", projected);
+            let normal = (projected.point - xform.translation);
             let len = normal.length();
             let normal = normal.normalize();
             if len < wheel_config.radius {
-                // we're in contact
-
-                // do gravity's share
+                let grav_norm = gvec.normalize().dot(normal);
+                // we're in contact, first gravity
+                let inside = if projected.is_inside { -1.0 } else { 1.0 };
+                let mut force = inside * 10.0 * grav_norm * half_weight * normal;
 
                 // do linear acceleration's share
+                // f = ma
+                let lin_accel_norm = normal.dot(body_lin_accel.normalize());
+                force += body_mass.mass * lin_accel_norm * body_lin_accel;
 
-                // do friction's share
+                // do angular accelerations's share?
 
-                // do thrust's share
+                // do friction's share (depends on previous three)
 
-                // do steering's share
+                // do thrust's share (depends on friction):
+                //
+                // let ef = ExternalForce::at_point(force, point,
+                // center_of_mass);
+
+                // do steering's share (also depends on friction)
             }
         }
     }