avoid colliding with neighbors

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;
                 }
             }
         }

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();
 }