From ed7b947f1268a6603c5a25225009c156906b2842 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Sat, 25 Nov 2023 13:41:58 -0800
Subject: [PATCH] avoid the ground

---
 src/lib.rs | 34 ++++++++++++++++++++++++++--------
 1 file changed, 26 insertions(+), 8 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index 76032ef..b583547 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,10 +1,14 @@
 use argh::FromArgs;
 use bevy::prelude::*;
 
+// toid
 const SPEED: f32 = 1.0;
 const SPEED_DIFF_RANGE: f32 = 0.08; // +/- 8%
 
-const RADIUS: f32 = 1_000.0;
+// how far from origin before really wanting to come back
+const RADIUS: f32 = 100.0;
+
+const MIN_ALTITUDE: f32 = 1.5;
 
 pub type Point = [f32; 3];
 pub type Toint = rstar::primitives::GeomWithData<Point, Entity>;
@@ -20,11 +24,8 @@ pub struct Config {
 #[derive(Resource, Deref, DerefMut, Default)]
 pub struct Index(pub rstar::RTree<Toint>);
 
-#[derive(Resource, Deref, DerefMut, Default)]
-pub struct Positions(pub std::collections::HashMap<Entity, Vec3>);
-
 #[derive(Component, Debug, Clone, Deref, DerefMut, Default)]
-pub struct Buddies(Vec<Entity>);
+pub struct Buddies(Vec<Toint>);
 
 #[derive(Component, Debug, Clone, Deref, DerefMut, Default)]
 pub struct Velocity(Vec3);
@@ -72,14 +73,20 @@ pub fn add_gizmos(mut gizmos: Gizmos, toids: Query<(&Transform, Entity), With<To
 pub fn update_pos(
     mut toids: Query<(&mut Transform, &Velocity, Entity)>,
     time: Res<Time>,
-    mut index: Res<Index>,
+    mut index: ResMut<Index>,
 ) {
+    let mut positions = Vec::with_capacity(toids.iter().len());
+
     let dt = time.delta_seconds();
-    for (mut xform, vel, _entity) in toids.iter_mut() {
-        xform.translation += vel.0 * dt;
+    for (mut xform, vel, entity) in toids.iter_mut() {
         let look_at = xform.translation + vel.0;
+        xform.translation += vel.0 * dt;
         xform.look_at(look_at, Vec3::Y);
+        let toint = Toint::new(xform.translation.to_array(), entity);
+        positions.push(toint);
     }
+    let tree = rstar::RTree::bulk_load(positions);
+    **index = tree;
 }
 
 pub fn update_vel(
@@ -90,12 +97,23 @@ pub fn update_vel(
     for (xform, mut vel, toid) in toids.iter_mut() {
         let speed = toid.speed;
         let mut dir = xform.forward();
+        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);
+            let up = Quat::from_rotation_arc(dir, Vec3::Y);
+            let rot = xform.rotation.slerp(up, pct);
+            dir = rot.mul_vec3(dir);
+        }
 
         // nudge toward origin if too far
+        let dist = pos.length();
 
         // find buddies and orient: point more towards further-away buddies
+
+        **vel = dir * speed;
     }
 }