From 2049fbcc005c161250a94c9b0b73566a54a6323a Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Sun, 26 Nov 2023 15:12:57 -0800
Subject: [PATCH] don't change velocity too suddenly

---
 src/lib.rs | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index 4047db5..aace608 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -7,6 +7,7 @@ pub type NNTree = KDTree3<Toid>;
 // toid
 const SPEED: f32 = 5.0;
 const SPEED_DIFF_RANGE: f32 = 0.1; // +/- 8%
+const MAX_DELTA_V: f32 = std::f32::consts::PI * 2.0; // basically 360 degrees/sec
 
 // how far from origin before really wanting to come back
 const RADIUS: f32 = 400.0;
@@ -73,9 +74,12 @@ pub fn turkey_time(
 
 pub fn update_vel(
     mut toids: Query<(&Transform, &mut Velocity, &Toid, &Buddies, Entity)>,
+    time: Res<Time>,
     positions: Res<Positions>,
     index: Res<NNTree>,
 ) {
+    let dt = time.delta_seconds();
+    let max_delta = MAX_DELTA_V * dt;
     for (xform, mut vel, toid, buddies, entity) in toids.iter_mut() {
         let speed = toid.speed;
         let mut dir = vel.normalize();
@@ -112,8 +116,8 @@ pub fn update_vel(
             let bp = neighbor.0;
             let bdir = pos - bp;
             let dist = bdir.length();
-            let rot = Quat::from_rotation_arc(dir, bdir.normalize());
             let s = 1.0 - (dist / speed).min(1.0);
+            let rot = Quat::from_rotation_arc(dir, bdir.normalize());
             let rot = Quat::IDENTITY.slerp(rot, s);
             dir = rot.mul_vec3(dir).normalize();
         }
@@ -121,15 +125,19 @@ pub fn update_vel(
         // nudge up if too low
         if pos.y < MIN_ALTITUDE {
             let dh = MIN_ALTITUDE - pos.y;
-            let pct = (dh / MIN_ALTITUDE).min(1.0);
+            let s = (dh / MIN_ALTITUDE).min(1.0);
             let rot = Quat::from_rotation_arc(dir, Vec3::Y);
-            let rot = Quat::IDENTITY.slerp(rot, pct);
+            let rot = Quat::IDENTITY.slerp(rot, s);
             dir = rot.mul_vec3(dir).normalize();
         }
 
         // make sure velocity doesn't change too suddenly
-        {
-            //
+        let delta = dir.dot(original_dir).acos();
+        if delta > max_delta {
+            let s = max_delta / delta;
+            let rot = Quat::from_rotation_arc(original_dir, dir);
+            let rot = Quat::IDENTITY.slerp(rot, s);
+            dir = rot.mul_vec3(original_dir).normalize();
         }
 
         **vel = dir * speed;