From d3bbca49ce51a7f116ee4569fe4e7010737d1656 Mon Sep 17 00:00:00 2001 From: Imbris Date: Tue, 16 Mar 2021 01:13:52 -0400 Subject: [PATCH] Setup spatial grid for entity versus entity collisions --- Cargo.lock | 1 + client/src/lib.rs | 1 + common/base/src/lib.rs | 17 ++ common/sys/Cargo.toml | 3 + common/sys/src/phys.rs | 393 +++++++++++++++++++++------- common/sys/src/phys/spatial_grid.rs | 77 ++++++ 6 files changed, 399 insertions(+), 93 deletions(-) create mode 100644 common/sys/src/phys/spatial_grid.rs diff --git a/Cargo.lock b/Cargo.lock index 752f4e4387..a61ace6dab 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -5640,6 +5640,7 @@ dependencies = [ "bincode", "hashbrown", "indexmap", + "inline_tweak", "rand 0.8.3", "rayon", "scopeguard", diff --git a/client/src/lib.rs b/client/src/lib.rs index bb0dc530b4..788ef5d70f 100644 --- a/client/src/lib.rs +++ b/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 ( diff --git a/common/base/src/lib.rs b/common/base/src/lib.rs index c1e1704880..3042e738a6 100644 --- a/common/base/src/lib.rs +++ b/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 { diff --git a/common/sys/Cargo.toml b/common/sys/Cargo.toml index db02ba1e45..b16366ef81 100644 --- a/common/sys/Cargo.toml +++ b/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"] } diff --git a/common/sys/src/phys.rs b/common/sys/src/phys.rs index a1b18cd3af..9fc58bcba3 100644 --- a/common/sys/src/phys.rs +++ b/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, @@ -128,7 +132,7 @@ impl<'a> PhysicsData<'a> { fn maintain_pushback_cache(&mut self) { span!(_guard, "Maintain pushback cache"); - //Add PreviousPhysCache for all relevant entities + // Add PreviousPhysCache for all relevant entities for entity in ( &self.read.entities, &self.write.velocities, @@ -155,7 +159,7 @@ impl<'a> PhysicsData<'a> { }); } - //Update PreviousPhysCache + // Update PreviousPhysCache for (_, vel, position, mut phys_cache, collider, scale, cs, _, _, _) in ( &self.read.entities, &self.write.velocities, @@ -189,8 +193,67 @@ impl<'a> PhysicsData<'a> { } } - fn apply_pushback(&mut self, job: &mut Job) { + 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::() 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, 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,102 +310,244 @@ impl<'a> PhysicsData<'a> { let mut entity_entity_collision_checks = 0; let mut entity_entity_collisions = 0; - for ( - entity_other, - other, - pos_other, - previous_cache_other, - mass_other, - collider_other, - _, - _, - _, - _, - char_state_other_maybe, - ) in ( - &read.entities, - &read.uids, - positions, - previous_phys_cache, - read.masses.maybe(), - read.colliders.maybe(), - !&read.projectiles, - !&read.mountings, - !&read.beams, - !&read.shockwaves, - read.char_states.maybe(), - ) - .join() - { - 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 + 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, + pos_other, + previous_cache_other, + mass_other, + collider_other, + _, + _, + _, + _, + char_state_other_maybe, + ) in ( + &read.entities, + &read.uids, + positions, + previous_phys_cache, + read.masses.maybe(), + read.colliders.maybe(), + !&read.projectiles, + !&read.mountings, + !&read.beams, + !&read.shockwaves, + read.char_states.maybe(), + ) + .join() { - continue; - } - - 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 { - continue; - } - - 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 + 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 { - if !collided { - physics.touch_entities.push(*other); - entity_entity_collisions += 1; - } + continue; + } - // 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 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 { + continue; + } + + 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 { - let force = - 400.0 * (collision_dist - diff.magnitude()) * mass_other + 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; - } + vel_delta += + Vec3::from(diff.normalized()) * force * step_delta; + } - collided = true; + collided = true; + } } } } @@ -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); } diff --git a/common/sys/src/phys/spatial_grid.rs b/common/sys/src/phys/spatial_grid.rs new file mode 100644 index 0000000000..0e5cfd716a --- /dev/null +++ b/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, Vec>, + large_grid: hashbrown::HashMap, Vec>, + // 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, 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) -> impl Iterator + '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, + )) + } +}