Setup spatial grid for entity versus entity collisions

Cargo.lock

@@ -5640,6 +5640,7 @@ dependencies = [
  "bincode",
  "hashbrown",
  "indexmap",
+ "inline_tweak",
  "rand 0.8.3",
  "rayon",
  "scopeguard",

client/src/lib.rs

@@ -246,6 +246,7 @@ impl Client {
 
         ping_stream.send(PingMsg::Ping)?;
 
+        let mut ping_interval = tokio::time::interval(core::time::Duration::from_secs(1));
         // Wait for initial sync
         let mut ping_interval = tokio::time::interval(core::time::Duration::from_secs(1));
         let (

common/base/src/lib.rs

@@ -6,6 +6,23 @@ pub use userdata_dir::userdata_dir;
 
 #[cfg(feature = "tracy")] pub use tracy_client;
 
+#[macro_export]
+macro_rules! plot {
+    ($name:expr, $value:expr) => {
+        #[cfg(feature = "tracy")]
+        {
+            use $crate::tracy_client::{create_plot, Plot};
+            static PLOT: Plot = create_plot!($name);
+            PLOT.point($value);
+        }
+        #[cfg(not(feature = "tracy"))]
+        {
+            // type check
+            let _: f64 = $value;
+        }
+    };
+}
+
 // https://discordapp.com/channels/676678179678715904/676685797524766720/723358438943621151
 #[macro_export]
 macro_rules! span {

common/sys/Cargo.toml

@@ -40,3 +40,6 @@ tar = { version = "0.4.30", optional = true }
 wasmer = { version = "1.0.0", optional = true, default-features = false, features = ["wat", "default-cranelift", "default-jit"] }
 bincode = { version = "1.3.1", optional = true }
 plugin-api = { package = "veloren-plugin-api", path = "../../plugin/api", optional = true }
+
+# Tweak running code
+inline_tweak = { version = "1.0.8", features = ["release_tweak"] }

common/sys/src/phys.rs

@@ -1,3 +1,7 @@
+mod spatial_grid;
+
+use spatial_grid::SpatialGrid;
+
 use common::{
     comp::{
         body::ship::figuredata::VOXEL_COLLIDER_MANIFEST, BeamSegment, Body, CharacterState,
@@ -189,8 +193,67 @@ impl<'a> PhysicsData<'a> {
         }
     }
 
-    fn apply_pushback(&mut self, job: &mut Job<Sys>) {
+    fn construct_spatial_grid(&mut self) -> SpatialGrid {
+        span!(_guard, "Construct spatial grid");
+        let PhysicsData {
+            ref read,
+            ref write,
+        } = self;
+        // NOTE: assumes that entity max radius * 2 + max velocity per tick is less than
+        // half a chunk (16 blocks)
+        // NOTE: i32 places certain constraints on how far out collision works
+        // NOTE: uses the radius of the entity and their current position rather than
+        // the radius of their bounding sphere for the current frame of movement
+        // because the nonmoving entity is what is collided against in the inner
+        // loop of the pushback collision code
+        // TODO: maintain frame to frame? (requires handling deletion)
+        // TODO: if not maintaining frame to frame consider counting entities to
+        // preallocate?
+        // TODO: assess parallelizing (overhead might dominate here? would need to merge
+        // the vecs in each hashmap)
+        let lg2_cell_size = inline_tweak::release_tweak!(5);
+        let lg2_large_cell_size = 6;
+        let radius_cutoff = 8;
+        common_base::plot!("spatial grid cell size", (1 << lg2_cell_size) as f64);
+        // let mut radius_list = Vec::new();
+        let mut spatial_grid = SpatialGrid::new(lg2_cell_size, lg2_large_cell_size, radius_cutoff);
+        for (entity, pos, phys_cache, _, _, _, _, _) in (
+            &read.entities,
+            &write.positions,
+            &write.previous_phys_cache,
+            write.velocities.mask(),
+            !&read.projectiles, // Not needed because they are skipped in the inner loop below
+            !&read.mountings,
+            !&read.beams,
+            !&read.shockwaves,
+        )
+            .join()
+        {
+            // Note: to not get too fine grained we use a 2D grid for now
+            let radius_2d = phys_cache.scaled_radius.ceil() as u32;
+            let pos_2d = pos.0.xy().map(|e| e as i32);
+            const POS_TRUNCATION_ERROR: u32 = 1;
+            spatial_grid.insert(pos_2d, radius_2d + POS_TRUNCATION_ERROR, entity);
+            // radius_list.push(phys_cache.scaled_radius.ceil() as u32);
+        }
+        /* if !radius_list.is_empty() {
+            radius_list.sort();
+            common_base::plot!("radius:min", *radius_list.first().unwrap() as f64);
+            common_base::plot!("radius:max", *radius_list.last().unwrap() as f64);
+            common_base::plot!(
+                "radius:mean",
+                radius_list.iter().sum::<u32>() as f64 / radius_list.len() as f64
+            );
+            common_base::plot!("radius:mode", radius_list[radius_list.len() / 2] as f64);
+        } */
+
+        spatial_grid
+    }
+
+    fn apply_pushback(&mut self, job: &mut Job<Sys>, spatial_grid: &SpatialGrid) {
+        // TODO: make sure to check git stash show -p  to make sure nothing was missed
         span!(_guard, "Apply pushback");
+        let use_grid = inline_tweak::release_tweak!(true);
         job.cpu_stats.measure(ParMode::Rayon);
         let PhysicsData {
             ref read,
@@ -247,6 +310,139 @@ impl<'a> PhysicsData<'a> {
                     let mut entity_entity_collision_checks = 0;
                     let mut entity_entity_collisions = 0;
 
+                    if use_grid {
+                        let aabr = {
+                            let center = previous_cache.center.xy().map(|e| e as i32);
+                            let radius = previous_cache.collision_boundary.ceil() as i32;
+                            // From conversion of center above
+                            const CENTER_TRUNCATION_ERROR: i32 = 1;
+                            let max_dist = radius + CENTER_TRUNCATION_ERROR;
+
+                            Aabr {
+                                min: center - max_dist,
+                                max: center + max_dist,
+                            }
+                        };
+
+                        spatial_grid
+                            .in_aabr(aabr)
+                            .filter_map(|entity| {
+                                read.uids
+                                    .get(entity)
+                                    .zip(positions.get(entity))
+                                    .zip(previous_phys_cache.get(entity))
+                                    .map(|((uid, pos), previous_cache)| {
+                                        (
+                                            entity,
+                                            uid,
+                                            pos,
+                                            previous_cache,
+                                            read.masses.get(entity),
+                                            read.colliders.get(entity),
+                                            read.char_states.get(entity),
+                                        )
+                                    })
+                            })
+                            .for_each(
+                                |(
+                                    entity_other,
+                                    other,
+                                    pos_other,
+                                    previous_cache_other,
+                                    mass_other,
+                                    collider_other,
+                                    char_state_other_maybe,
+                                )| {
+                                    let collision_boundary = previous_cache.collision_boundary
+                                        + previous_cache_other.collision_boundary;
+                                    if previous_cache
+                                        .center
+                                        .distance_squared(previous_cache_other.center)
+                                        > collision_boundary.powi(2)
+                                        || entity == entity_other
+                                    {
+                                        return;
+                                    }
+
+                                    let collision_dist = previous_cache.scaled_radius
+                                        + previous_cache_other.scaled_radius;
+                                    let z_limits_other =
+                                        calc_z_limit(char_state_other_maybe, collider_other);
+
+                                    let mass_other = mass_other
+                                        .map(|m| m.0)
+                                        .unwrap_or(previous_cache_other.scale);
+                                    //This check after the pos check, as we currently don't have
+                                    // that many
+                                    // massless entites [citation needed]
+                                    if mass_other == 0.0 {
+                                        return;
+                                    }
+
+                                    entity_entity_collision_checks += 1;
+
+                                    const MIN_COLLISION_DIST: f32 = 0.3;
+                                    let increments = ((previous_cache.velocity_dt
+                                        - previous_cache_other.velocity_dt)
+                                        .magnitude()
+                                        / MIN_COLLISION_DIST)
+                                        .max(1.0)
+                                        .ceil()
+                                        as usize;
+                                    let step_delta = 1.0 / increments as f32;
+                                    let mut collided = false;
+
+                                    for i in 0..increments {
+                                        let factor = i as f32 * step_delta;
+                                        let pos = pos.0 + previous_cache.velocity_dt * factor;
+                                        let pos_other =
+                                            pos_other.0 + previous_cache_other.velocity_dt * factor;
+
+                                        let diff = pos.xy() - pos_other.xy();
+
+                                        if diff.magnitude_squared() <= collision_dist.powi(2)
+                                            && pos.z + z_limits.1 * previous_cache.scale
+                                                >= pos_other.z
+                                                    + z_limits_other.0 * previous_cache_other.scale
+                                            && pos.z + z_limits.0 * previous_cache.scale
+                                                <= pos_other.z
+                                                    + z_limits_other.1 * previous_cache_other.scale
+                                        {
+                                            if !collided {
+                                                physics.touch_entities.push(*other);
+                                                entity_entity_collisions += 1;
+                                            }
+
+                                            // Don't apply repulsive force to projectiles or if
+                                            // we're
+                                            // colliding
+                                            // with a terrain-like entity, or if we are a
+                                            // terrain-like
+                                            // entity
+                                            if diff.magnitude_squared() > 0.0
+                                                && !is_projectile
+                                                && !matches!(
+                                                    collider_other,
+                                                    Some(Collider::Voxel { .. })
+                                                )
+                                                && !matches!(collider, Some(Collider::Voxel { .. }))
+                                            {
+                                                let force = 400.0
+                                                    * (collision_dist - diff.magnitude())
+                                                    * mass_other
+                                                    / (mass + mass_other);
+
+                                                vel_delta += Vec3::from(diff.normalized())
+                                                    * force
+                                                    * step_delta;
+                                            }
+
+                                            collided = true;
+                                        }
+                                    }
+                                },
+                            );
+                    } else {
                         for (
                             entity_other,
                             other,
@@ -287,7 +483,8 @@ impl<'a> PhysicsData<'a> {
 
                             let collision_dist =
                                 previous_cache.scaled_radius + previous_cache_other.scaled_radius;
-                        let z_limits_other = calc_z_limit(char_state_other_maybe, collider_other);
+                            let z_limits_other =
+                                calc_z_limit(char_state_other_maybe, collider_other);
 
                             let mass_other = mass_other
                                 .map(|m| m.0)
@@ -313,39 +510,47 @@ impl<'a> PhysicsData<'a> {
                             for i in 0..increments {
                                 let factor = i as f32 * step_delta;
                                 let pos = pos.0 + previous_cache.velocity_dt * factor;
-                            let pos_other = pos_other.0 + previous_cache_other.velocity_dt * factor;
+                                let pos_other =
+                                    pos_other.0 + previous_cache_other.velocity_dt * factor;
 
                                 let diff = pos.xy() - pos_other.xy();
 
                                 if diff.magnitude_squared() <= collision_dist.powi(2)
                                     && pos.z + z_limits.1 * previous_cache.scale
-                                    >= pos_other.z + z_limits_other.0 * previous_cache_other.scale
+                                        >= pos_other.z
+                                            + z_limits_other.0 * previous_cache_other.scale
                                     && pos.z + z_limits.0 * previous_cache.scale
-                                    <= pos_other.z + z_limits_other.1 * previous_cache_other.scale
+                                        <= pos_other.z
+                                            + z_limits_other.1 * previous_cache_other.scale
                                 {
                                     if !collided {
                                         physics.touch_entities.push(*other);
                                         entity_entity_collisions += 1;
                                     }
 
-                                // Don't apply repulsive force to projectiles or if we're colliding
+                                    // Don't apply repulsive force to projectiles or if we're
-                                // with a terrain-like entity, or if we are a terrain-like entity
+                                    // colliding
+                                    // with a terrain-like entity, or if we are a terrain-like
+                                    // entity
                                     if diff.magnitude_squared() > 0.0
                                         && !is_projectile
                                         && !matches!(collider_other, Some(Collider::Voxel { .. }))
                                         && !matches!(collider, Some(Collider::Voxel { .. }))
                                     {
-                                    let force =
+                                        let force = 400.0
-                                        400.0 * (collision_dist - diff.magnitude()) * mass_other
+                                            * (collision_dist - diff.magnitude())
+                                            * mass_other
                                             / (mass + mass_other);
 
-                                    vel_delta += Vec3::from(diff.normalized()) * force * step_delta;
+                                        vel_delta +=
+                                            Vec3::from(diff.normalized()) * force * step_delta;
                                     }
 
                                     collided = true;
                                 }
                             }
                         }
+                    }
 
                     // Change velocity
                     vel.0 += vel_delta * read.dt.0;
@@ -835,7 +1040,9 @@ impl<'a> System<'a> for Sys {
         // it means the step needs to take into account the speeds of both
         // entities.
         psd.maintain_pushback_cache();
-        psd.apply_pushback(job);
+
+        let spatial_grid = psd.construct_spatial_grid();
+        psd.apply_pushback(job, &spatial_grid);
 
         psd.handle_movement_and_terrain(job);
     }

common/sys/src/phys/spatial_grid.rs

@@ -0,0 +1,77 @@
+use vek::*;
+
+pub struct SpatialGrid {
+    // Uses two scales of grids so that we can have a hard limit on how far to search in the
+    // smaller grid
+    grid: hashbrown::HashMap<Vec2<i32>, Vec<specs::Entity>>,
+    large_grid: hashbrown::HashMap<Vec2<i32>, Vec<specs::Entity>>,
+    // Log base 2 of the cell size of the spatial grid
+    lg2_cell_size: usize,
+    // Log base 2 of the cell size of the large spatial grid
+    lg2_large_cell_size: usize,
+    // Entities with a radius over this value are store in the coarser large_grid
+    // This is the amount of buffer space we need to add when finding the intersections with cells
+    // in the regular grid
+    radius_cutoff: u32,
+    // Stores the largest radius of the entities in the large_grid
+    // This is the amount of buffer space we need to add when finding the intersections with cells
+    // in the larger grid
+    // note: could explore some distance field type thing for querying whether there are large
+    // entities nearby that necessitate expanding the cells searched for collision (and querying
+    // how much it needs to be expanded)
+    // TODO: log this to metrics?
+    largest_large_radius: u32,
+}
+
+impl SpatialGrid {
+    pub fn new(lg2_cell_size: usize, lg2_large_cell_size: usize, radius_cutoff: u32) -> Self {
+        Self {
+            grid: Default::default(),
+            large_grid: Default::default(),
+            lg2_cell_size,
+            lg2_large_cell_size,
+            radius_cutoff,
+            largest_large_radius: radius_cutoff,
+        }
+    }
+
+    /// Add an entity at the provided 2d pos into the spatial grid
+    pub fn insert(&mut self, pos: Vec2<i32>, radius: u32, entity: specs::Entity) {
+        if radius <= self.radius_cutoff {
+            let cell = pos.map(|e| e >> self.lg2_cell_size);
+            self.grid.entry(cell).or_default().push(entity);
+        } else {
+            let cell = pos.map(|e| e >> self.lg2_large_cell_size);
+            self.large_grid.entry(cell).or_default().push(entity);
+            self.largest_large_radius = self.largest_large_radius.max(radius);
+        }
+    }
+
+    /// Get an iterator over the entities overlapping the
+    /// provided axis aligned bounding region
+    /// NOTE: for best optimization of the iterator use `for_each` rather than a
+    /// for loop
+    // TODO: a circle would be tighter (how efficient would it be to query the cells
+    // intersecting a circle?)
+    pub fn in_aabr<'a>(&'a self, aabr: Aabr<i32>) -> impl Iterator<Item = specs::Entity> + 'a {
+        let iter = |max_entity_radius, grid: &'a hashbrown::HashMap<_, _>, lg2_cell_size| {
+            // Add buffer for other entity radius
+            let min = aabr.min - max_entity_radius as i32;
+            let max = aabr.max + max_entity_radius as i32;
+            // Convert to cells
+            let min = min.map(|e| e >> lg2_cell_size);
+            let max = max.map(|e| (e + (1 << lg2_cell_size) - 1) >> lg2_cell_size);
+
+            (min.x..=max.x)
+                .flat_map(move |x| (min.y..=max.y).map(move |y| Vec2::new(x, y)))
+                .flat_map(move |cell| grid.get(&cell).into_iter().flatten())
+                .copied()
+        };
+
+        iter(self.radius_cutoff, &self.grid, self.lg2_cell_size).chain(iter(
+            self.largest_large_radius,
+            &self.large_grid,
+            self.lg2_large_cell_size,
+        ))
+    }
+}