From 9309d5387033eae5e2841a490b8af64fa76646bf Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Sun, 26 Nov 2023 14:23:33 -0800
Subject: [PATCH] avoid colliding with neighbors

---
 src/lib.rs  | 57 ++++++++++++++++++++++++++++++++++++-----------------
 src/main.rs | 11 ++---------
 2 files changed, 41 insertions(+), 27 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index b3758eb..4047db5 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -5,17 +5,14 @@ use bevy_spatial::{kdtree::KDTree3, SpatialAccess};
 pub type NNTree = KDTree3<Toid>;
 
 // toid
-const SPEED: f32 = 1.0;
-const SPEED_DIFF_RANGE: f32 = 0.08; // +/- 8%
+const SPEED: f32 = 5.0;
+const SPEED_DIFF_RANGE: f32 = 0.1; // +/- 8%
 
 // how far from origin before really wanting to come back
-const RADIUS: f32 = 800.0;
+const RADIUS: f32 = 400.0;
 
 // how close to try to stay to your buddies
-const BUDDY_RADIUS: f32 = 15.0;
-
-// how much room to try to maintain between toids
-const PERSONAL_SPACE: f32 = 1.5;
+const BUDDY_RADIUS: f32 = 50.0;
 
 const MIN_ALTITUDE: f32 = 1.5;
 
@@ -75,22 +72,15 @@ pub fn turkey_time(
 }
 
 pub fn update_vel(
-    mut toids: Query<(&Transform, &mut Velocity, &Toid, &Buddies)>,
+    mut toids: Query<(&Transform, &mut Velocity, &Toid, &Buddies, Entity)>,
     positions: Res<Positions>,
+    index: Res<NNTree>,
 ) {
-    for (xform, mut vel, toid, buddies) in toids.iter_mut() {
+    for (xform, mut vel, toid, buddies, entity) in toids.iter_mut() {
         let speed = toid.speed;
         let mut dir = vel.normalize();
         let pos = xform.translation;
-
-        // nudge up if too low
-        if pos.y < MIN_ALTITUDE {
-            let dh = MIN_ALTITUDE - pos.y;
-            let pct = (dh / MIN_ALTITUDE).min(1.0).powi(2);
-            let rot = Quat::from_rotation_arc(dir, Vec3::Y);
-            let rot = Quat::IDENTITY.slerp(rot, pct);
-            dir = rot.mul_vec3(dir).normalize();
-        }
+        let original_dir = dir.clone();
 
         // nudge toward origin if too far
         {
@@ -113,6 +103,35 @@ pub fn update_vel(
             dir = rot.mul_vec3(dir).normalize();
         }
 
+        // avoid flying into neighbors
+        for neighbor in index
+            .within_distance(pos, speed)
+            .iter()
+            .filter(|n| n.1.is_some() && n.1.unwrap() != entity)
+        {
+            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::IDENTITY.slerp(rot, s);
+            dir = rot.mul_vec3(dir).normalize();
+        }
+
+        // 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 rot = Quat::from_rotation_arc(dir, Vec3::Y);
+            let rot = Quat::IDENTITY.slerp(rot, pct);
+            dir = rot.mul_vec3(dir).normalize();
+        }
+
+        // make sure velocity doesn't change too suddenly
+        {
+            //
+        }
+
         **vel = dir * speed;
     }
 }
@@ -157,6 +176,8 @@ pub fn update_buddies(
             for _ in 0..diff {
                 if let Some(neighbor) = neighbors.next() {
                     buddies.0.insert(neighbor.1.unwrap());
+                } else {
+                    break;
                 }
             }
         }
diff --git a/src/main.rs b/src/main.rs
index 2ed7d5e..ba6575d 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -11,17 +11,10 @@ fn main() {
         .insert_resource(config)
         .insert_resource(Positions::default())
         .add_systems(Startup, setup)
+        .add_systems(Update, (update_pos, update_buddies, update_vel).chain())
         .add_systems(
             Update,
-            (
-                update_vel,
-                update_pos,
-                update_buddies,
-                rotate_camera,
-                update_config,
-                bevy::window::close_on_esc,
-            )
-                .chain(),
+            (rotate_camera, update_config, bevy::window::close_on_esc),
         )
         .run();
 }