use parallel iter to update velocities

.rustfmt.toml

@@ -0,0 +1,4 @@
+imports_granularity = "Crate"
+group_imports = "StdExternalCrate"
+wrap_comments = true
+edition = "2021"

Cargo.toml

@@ -6,7 +6,7 @@ edition = "2021"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
-argh = "0.1.12"
-bevy = "0.12.0"
-rand = "0.8.5"
-rstar = "0.11.0"
+argh = "0.1"
+bevy = "0.15.0-rc.3"
+bevy_spatial = { git = "https://github.com/laundmo/bevy-spatial.git", rev = "9ac313ac6173b440f50954b696dc2063513b4f37" } #"0.10.0"
+rand = "0.8"

src/lib.rs

@@ -1,31 +1,39 @@
 use argh::FromArgs;
-use bevy::prelude::*;
+use bevy::{
+    prelude::*,
+    tasks::ParallelIterator,
+    utils::{HashMap, HashSet},
+};
+use bevy_spatial::{kdtree::KDTree3, SpatialAccess};
 
-// toid
-const SPEED: f32 = 1.0;
-const SPEED_DIFF_RANGE: f32 = 0.08; // +/- 8%
+pub type NNTree = KDTree3<Toid>;
+
+// toid stuff
+const SPEED: f32 = 15.0;
+const SPEED_DIFF_RANGE: f32 = 0.1; // +/- 10%
+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 = 100.0;
+const RADIUS: f32 = 50.0;
 
-const MIN_ALTITUDE: f32 = 1.5;
+// how close to try to stay to your buddies
+const BUDDY_RADIUS: f32 = SPEED;
 
-pub type Point = [f32; 3];
-pub type Toint = rstar::primitives::GeomWithData<Point, Entity>;
+const MIN_ALTITUDE: f32 = 13.5;
 
 #[derive(Debug, FromArgs, Resource)]
 /// Toid Watching
 pub struct Config {
     /// how many Toids to spawn
-    #[argh(option, short = 't', default = "100")]
+    #[argh(option, short = 't', default = "5_000")]
     pub toids: usize,
 }
 
-#[derive(Resource, Deref, DerefMut, Default)]
-pub struct Index(pub rstar::RTree<Toint>);
+#[derive(Clone, Debug, Default, Deref, DerefMut, Resource)]
+pub struct Positions(pub HashMap<Entity, Vec3>);
 
-#[derive(Component, Debug, Clone, Deref, DerefMut, Default)]
-pub struct Buddies(Vec<Toint>);
+#[derive(Component, Debug, Deref, DerefMut, Clone, Default)]
+pub struct Buddies(HashSet<Entity>);
 
 #[derive(Component, Debug, Clone, Deref, DerefMut, Default)]
 pub struct Velocity(Vec3);
@@ -36,6 +44,12 @@ pub struct Toid {
     pub buddies: usize,
 }
 
+#[derive(Resource, Debug, Deref, DerefMut, Clone, Default)]
+pub struct Paused(bool);
+
+#[derive(Debug, Default, Clone, Copy, Deref, DerefMut, Resource)]
+pub struct LookAt(Vec3);
+
 pub fn turkey_time(
     commands: &mut Commands,
     scene: &Handle<Scene>,
@@ -45,79 +59,207 @@ pub fn turkey_time(
     let speed = SPEED + (SPEED * speed_diff);
     let vel = unit_vec(r) * speed;
     let buddies = r.gen_range(6..=8);
+    let x = r.gen_range(-10.0..=10.0);
+    let z = r.gen_range(-10.0..=10.0);
+    let y = r.gen_range(0.1..=5.5);
+    let pos = Vec3::new(x, MIN_ALTITUDE + y, z);
+    let xform = Transform::from_translation(pos);
+    let spatial_bundle = (xform, Visibility::Visible);
     commands
-        .spawn(SpatialBundle::default())
+        .spawn(spatial_bundle)
         .insert((Velocity(vel), Buddies::default(), Toid { speed, buddies }))
         .with_children(|t| {
-            t.spawn(SceneBundle {
-                scene: scene.to_owned(),
-                ..Default::default()
-            })
-            .insert(Transform::from_rotation(Quat::from_axis_angle(
-                Vec3::Y,
-                -std::f32::consts::FRAC_PI_2,
-            )));
+            t.spawn(SceneRoot(scene.to_owned()))
+                .insert(Transform::default());
         })
         .id()
 }
 
-pub fn add_gizmos(mut gizmos: Gizmos, toids: Query<(&Transform, Entity), With<Toid>>) {
-    let gizmos = &mut gizmos;
-    for (pos, _entity) in toids.iter() {
-        let nudge = pos.up() * 0.15;
-        let rpos = pos.translation + nudge;
-        gizmos.ray(rpos, pos.forward(), Color::RED);
-    }
-}
-
-pub fn update_pos(
-    mut toids: Query<(&mut Transform, &Velocity, Entity)>,
-    time: Res<Time>,
-    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() {
-        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(
-    mut toids: Query<(&Transform, &mut Velocity, &Toid), With<Toid>>,
+    mut toids: Query<(&Transform, &mut Velocity, &Toid, &Buddies, Entity)>,
     time: Res<Time>,
+    positions: Res<Positions>,
+    index: Res<NNTree>,
+    paused: Res<Paused>,
 ) {
-    let dt = time.delta_seconds();
-    for (xform, mut vel, toid) in toids.iter_mut() {
+    if **paused {
+        return;
+    }
+
+    let dt = time.delta_secs();
+    let max_delta = MAX_DELTA_V * dt;
+    toids
+        .par_iter_mut()
+        .for_each(|(xform, mut vel, toid, buddies, entity)| {
             let speed = toid.speed;
-        let mut dir = xform.forward();
+            let mut dir = vel.normalize();
             let pos = xform.translation;
+            let original_dir = dir;
+
+            // find buddies and orient; point more towards further-away buddies
+            for buddy in buddies.iter() {
+                let bp = positions.get(buddy).unwrap();
+                let bdir = *bp - pos;
+                let dist = bdir.length();
+                let rot = Quat::from_rotation_arc(dir, bdir.normalize());
+                let s = (dist / (BUDDY_RADIUS * 1.2)).min(1.0);
+                let rot = Quat::IDENTITY.slerp(rot, s);
+                dir = rot.mul_vec3(dir).normalize();
+            }
+
+            // avoid flying into neighbors
+            let min_dist = speed * 20.0;
+            for neighbor in index
+                .within_distance(pos, min_dist)
+                .iter()
+                .filter(|n| n.1.is_some() && n.1.unwrap() != entity)
+            {
+                let bp = neighbor.0;
+                let bdir = pos - bp;
+                let dist = bdir.length();
+                let s = 1.0 - (dist / min_dist).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();
+            }
+
+            // nudge toward origin if too far
+            {
+                let dist = pos.length();
+                let toward_origin = -pos.normalize();
+                let s = (dist / RADIUS).min(1.0);
+                let rot = Quat::from_rotation_arc(dir, toward_origin);
+                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 up = Quat::from_rotation_arc(dir, Vec3::Y);
-            let rot = xform.rotation.slerp(up, pct);
-            dir = rot.mul_vec3(dir);
+                let s = (dh / MIN_ALTITUDE).min(1.0);
+                let rot = Quat::from_rotation_arc(dir, Vec3::Y);
+                let rot = Quat::IDENTITY.slerp(rot, s);
+                dir = rot.mul_vec3(dir).normalize();
             }
 
-        // nudge toward origin if too far
-        let dist = pos.length();
-
-        // find buddies and orient: point more towards further-away buddies
+            // 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;
+        });
+}
+
+pub fn update_pos(
+    mut toids: Query<(&mut Transform, &Velocity, Entity), With<Toid>>,
+    mut positions: ResMut<Positions>,
+    mut lookat: ResMut<LookAt>,
+    time: Res<Time>,
+    paused: Res<Paused>,
+) {
+    if **paused {
+        return;
+    }
+
+    let mut new_look = Vec3::ZERO;
+    let dt = time.delta_secs();
+    for (mut xform, vel, entity) in toids.iter_mut() {
+        xform.translation += **vel * dt;
+        let look_at = vel.normalize();
+        xform.look_to(look_at, Vec3::Y);
+        *positions.entry(entity).or_insert(Vec3::ZERO) = xform.translation;
+        new_look += xform.translation;
+    }
+    **lookat = new_look / positions.len() as f32;
+}
+
+pub fn update_buddies(
+    mut toids: Query<(&Transform, Entity, &Toid, &mut Buddies)>,
+    index: Res<NNTree>,
+    positions: Res<Positions>,
+) {
+    let d2 = (BUDDY_RADIUS * 1.5).powi(2);
+    for (xform, entity, toid, mut buddies) in toids.iter_mut() {
+        let pos = xform.translation;
+        for buddy in buddies.clone().iter() {
+            let bp = positions.get(buddy).unwrap();
+            let bd2 = (*bp - pos).length_squared();
+            if bd2 > d2 {
+                buddies.remove(buddy);
+            }
+        }
+
+        if buddies.len() < toid.buddies {
+            let diff = toid.buddies - buddies.len();
+            for (_, neighbor) in index
+                .k_nearest_neighbour(pos, diff + 1)
+                .into_iter()
+                .filter(|n| n.1.is_some() && n.1.unwrap() != entity)
+            {
+                buddies.insert(neighbor.unwrap());
+            }
+        }
     }
 }
 
-pub fn update_buddies(mut toids: Query<(&Transform, Entity, &mut Buddies)>, index: Res<Index>) {}
+pub fn update_gizmos(toids: Query<&Transform, With<Toid>>, mut gizmos: Gizmos) {
+    for toid in toids.iter() {
+        let pos = toid.translation;
+        //dbg!(toid);
+        let up = toid.up().as_vec3();
+        let forward = toid.forward().as_vec3();
+        let right = toid.right().as_vec3();
+        gizmos.ray(pos, pos + forward, Color::srgb_u8(255, 0, 0));
+        gizmos.ray(pos, pos + up, Color::srgb_u8(0, 0, 255));
+        gizmos.ray(pos, pos + right, Color::srgb_u8(0, 255, 0));
+    }
+}
+
+pub fn rotate_camera(
+    mut query: Query<&mut Transform, With<Camera>>,
+    mut paused: ResMut<Paused>,
+    keys: Res<ButtonInput<KeyCode>>,
+    lookat: Res<LookAt>,
+) {
+    let mut xform = query.single_mut();
+
+    let forward = xform.forward() * 0.7;
+    let right = xform.right().as_vec3();
+
+    if keys.just_pressed(KeyCode::Space) {
+        let pause = **paused;
+        **paused = !pause;
+    }
+
+    if keys.pressed(KeyCode::ArrowRight) {
+        xform.translation += right;
+    }
+    if keys.pressed(KeyCode::ArrowLeft) {
+        xform.translation -= right;
+    }
+
+    if keys.pressed(KeyCode::ArrowUp) {
+        if keys.pressed(KeyCode::ShiftLeft) || keys.pressed(KeyCode::ShiftRight) {
+            xform.translation += Vec3::Y;
+        } else {
+            xform.translation += forward;
+        }
+    }
+    if keys.pressed(KeyCode::ArrowDown) {
+        if keys.pressed(KeyCode::ShiftLeft) || keys.pressed(KeyCode::ShiftRight) {
+            xform.translation -= Vec3::Y;
+        } else {
+            xform.translation -= forward;
+        }
+    }
+
+    xform.look_at(**lookat, Vec3::Y);
+}
 
 //-************************************************************************
 // util
@@ -126,7 +268,7 @@ pub fn update_buddies(mut toids: Query<(&Transform, Entity, &mut Buddies)>, inde
 pub fn unit_vec(r: &mut impl rand::Rng) -> Vec3 {
     let mut x1: f32 = 0.0;
     let mut x2: f32 = 0.0;
-    let mut ssum = std::f32::MAX;
+    let mut ssum = f32::MAX;
     while ssum >= 1.0 {
         x1 = r.gen_range(-1.0..=1.0);
         x2 = r.gen_range(-1.0..=1.0);

src/main.rs

@@ -1,29 +1,36 @@
-//use std::f32::consts::PI;
-
-use bevy::prelude::*;
+use std::time::Duration;
 
 use audubon::*;
+use bevy::{
+    diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin},
+    prelude::*,
+};
+use bevy_spatial::{AutomaticUpdate, TransformMode};
 
 fn main() {
     let config: audubon::Config = argh::from_env();
     App::new()
         .add_plugins(DefaultPlugins)
-        .insert_resource(Index(rstar::RTree::new()))
+        .add_plugins((
+            AutomaticUpdate::<Toid>::new()
+                .with_transform(TransformMode::GlobalTransform)
+                .with_frequency(Duration::from_millis(150)),
+            FrameTimeDiagnosticsPlugin,
+            LogDiagnosticsPlugin::default(),
+        ))
         .insert_resource(config)
-        .add_systems(Startup, setup)
-        .add_systems(
-            Update,
-            (
-                update_vel,
-                add_gizmos,
-                update_pos,
-                update_buddies,
-                rotate_camera,
-                update_config,
-                bevy::window::close_on_esc,
-            )
-                .chain(),
-        )
+        .insert_resource(Positions::default())
+        .insert_resource(ClearColor(Color::srgb(0.64, 0.745, 0.937))) // a nice light blue
+        .insert_resource(AmbientLight {
+            color: Color::WHITE,
+            brightness: 1.0,
+        })
+        .insert_resource(LookAt::default())
+        .insert_resource(Paused::default())
+        .add_systems(Startup, (setup, setup_ambient_light))
+        //.add_systems(Update, update_gizmos)
+        .add_systems(Update, (update_pos, update_buddies, update_vel))
+        .add_systems(Update, (rotate_camera, close_on_esc))
         .run();
 }
 
@@ -35,76 +42,61 @@ fn setup(
     models: Res<AssetServer>,
 ) {
     let rand = &mut rand::thread_rng();
+    let cam = Camera3d::default();
 
-    commands.spawn(Camera3dBundle {
-        transform: Transform::from_xyz(0., 3.5, 10.).looking_at(Vec3::ZERO, Vec3::Y),
-        ..default()
-    });
+    let camera = commands
+        .spawn((
+            Transform::from_xyz(0., 5.0, 25.).looking_at(Vec3::new(0.0, 5.0, 0.0), Vec3::Y),
+            cam,
+            Visibility::Hidden,
+        ))
+        .id();
     // plane
-    commands.spawn(PbrBundle {
-        mesh: meshes.add(Mesh::from(shape::Plane::from_size(50.0))),
-        material: materials.add(Color::rgb(0.3, 0.5, 0.3).into()),
-        ..default()
-    });
+    commands
+        .spawn(Mesh3d(
+            meshes.add(Mesh::from(Plane3d::default().mesh().size(500.0, 500.0))),
+        ))
+        .insert(MeshMaterial3d(materials.add(Color::srgb(0.3, 1.0, 0.3))));
 
-    let toid_model = models.load("models/boid.glb#Scene0");
+    let toid_model = models.load("models/toid3.glb#Scene0");
 
     for _ in 0..config.toids {
         let _ = turkey_time(&mut commands, &toid_model, rand);
     }
 
-    // light
-    commands.spawn(PointLightBundle {
-        point_light: PointLight {
-            intensity: 1500.0,
-            shadows_enabled: true,
-            ..default()
+    // instructions
+    commands.spawn((
+        Text::new(
+            "Up and down for camera forward and back; hold shift to change height.\nLeft or right to move left or right.\nPress 'ESC' to quit."), TextFont {
+        font_size: 20.0,
+        ..Default::default()
         },
-        transform: Transform::from_xyz(4.0, 8.0, 4.0),
-        ..default()
-    });
-
-    // example instructions
-    commands.spawn(
-        TextBundle::from_section(
-            "Press 'D' to toggle drawing gizmos on top of everything else in the scene\n\
-            Press 'P' to toggle perspective for line gizmos\n\
-            Hold 'Left' or 'Right' to change the line width",
-            TextStyle {
-                font_size: 20.,
-                ..default()
-            },
-        )
-        .with_style(Style {
+        Node {
             position_type: PositionType::Absolute,
                 top: Val::Px(12.0),
                 left: Val::Px(12.0),
                 ..default()
-        }),
-    );
+        },
+        TargetCamera(camera),
+        TextColor::WHITE));
 }
 
-fn rotate_camera(mut query: Query<&mut Transform, With<Camera>>, time: Res<Time>) {
-    let mut transform = query.single_mut();
+fn close_on_esc(
+    mut commands: Commands,
+    focused_windows: Query<(Entity, &Window)>,
+    input: Res<ButtonInput<KeyCode>>,
+) {
+    for (window, focus) in focused_windows.iter() {
+        if !focus.focused {
+            continue;
+        }
 
-    transform.rotate_around(Vec3::ZERO, Quat::from_rotation_y(time.delta_seconds() / 2.));
+        if input.just_pressed(KeyCode::Escape) {
+            commands.entity(window).despawn();
+        }
+    }
 }
 
-fn update_config(mut config: ResMut<GizmoConfig>, keyboard: Res<Input<KeyCode>>, time: Res<Time>) {
-    if keyboard.just_pressed(KeyCode::D) {
-        config.depth_bias = if config.depth_bias == 0. { -1. } else { 0. };
-    }
-    if keyboard.just_pressed(KeyCode::P) {
-        // Toggle line_perspective
-        config.line_perspective ^= true;
-        // Increase the line width when line_perspective is on
-        config.line_width *= if config.line_perspective { 5. } else { 1. / 5. };
-    }
-
-    if keyboard.pressed(KeyCode::Right) {
-        config.line_width += 5. * time.delta_seconds();
-    }
-    if keyboard.pressed(KeyCode::Left) {
-        config.line_width -= 5. * time.delta_seconds();
-    }
+fn setup_ambient_light(mut ambient_light: ResMut<AmbientLight>) {
+    ambient_light.brightness = 100.0;
 }