diff --git a/common/src/comp/phys.rs b/common/src/comp/phys.rs index e03552722b..d765caf04c 100644 --- a/common/src/comp/phys.rs +++ b/common/src/comp/phys.rs @@ -53,6 +53,8 @@ pub struct PreviousPhysCache { /// Calculates a Sphere over the Entity for quick boundary checking pub collision_boundary: f32, pub scale: f32, + /// The scaled radius of the collider, with defaults if the collider or the + /// scale are absent pub scaled_radius: f32, pub ori: Quaternion, } diff --git a/common/src/util/spatial_grid.rs b/common/src/util/spatial_grid.rs index aab6b67381..7aaf98e6ad 100644 --- a/common/src/util/spatial_grid.rs +++ b/common/src/util/spatial_grid.rs @@ -100,6 +100,45 @@ impl SpatialGrid { self.in_aabr(aabr) } + /// Get an iterator over the entities in cells that overlap with a circle of + /// the given radius whose center is swept across the line from p0 to p1 + /// NOTE: for best optimization of the iterator use `for_each` + /// rather than a for loop + pub fn in_swept_circle<'a>( + &'a self, + p0: Vec2, + p1: Vec2, + radius: f32, + ) -> impl Iterator + 'a { + let iter = |max_entity_radius, grid: &'a hashbrown::HashMap<_, _>, lg2_cell_size| { + // Add buffer for other entity radius + let ylo = (p0.y.min(p1.y) - radius - max_entity_radius) as i32; + let yhi = (p0.y.max(p1.y) + radius + max_entity_radius) as i32; + // Convert to cells + let ylo = ylo >> lg2_cell_size; + let yhi = (yhi + (1 << lg2_cell_size) - 1) >> lg2_cell_size; + + (ylo..=yhi) + .flat_map(move |y| { + let mid = LineSegment2 { start: p0, end: p1 } + .projected_point(Vec2::new(p0.x, y as f32)); + let xlo = (mid.x - radius - max_entity_radius) as i32; + let xhi = (mid.x + radius + max_entity_radius) as i32; + let xlo = xlo >> lg2_cell_size; + let xhi = (xhi + (1 << lg2_cell_size) - 1) >> lg2_cell_size; + (xlo..=xhi).map(move |x| Vec2::new(x, y)) + }) + .flat_map(move |cell| grid.get(&cell).into_iter().flatten()) + .copied() + }; + + iter(self.radius_cutoff as f32, &self.grid, self.lg2_cell_size).chain(iter( + self.largest_large_radius as f32, + &self.large_grid, + self.lg2_large_cell_size, + )) + } + pub fn clear(&mut self) { self.grid.clear(); self.large_grid.clear(); diff --git a/common/systems/src/phys.rs b/common/systems/src/phys.rs index 48c36e506d..f9cbbf8fea 100644 --- a/common/systems/src/phys.rs +++ b/common/systems/src/phys.rs @@ -217,7 +217,7 @@ impl<'a> PhysicsData<'a> { let half_height = (z_limits.1 - z_limits.0) / 2.0; phys_cache.velocity_dt = vel.0 * self.read.dt.0; - let entity_center = position.0 + Vec3::new(0.0, z_limits.0 + half_height, 0.0); + let entity_center = position.0 + Vec3::new(0.0, 0.0, z_limits.0 + half_height); let flat_radius = collider.map(|c| c.get_radius()).unwrap_or(0.5) * scale; let radius = (flat_radius.powi(2) + half_height.powi(2)).sqrt(); @@ -342,11 +342,12 @@ impl<'a> PhysicsData<'a> { let mut vel_delta = Vec3::zero(); - let query_center = previous_cache.center.xy(); - let query_radius = previous_cache.collision_boundary; + let query_p0 = pos.0.xy(); + let query_p1 = (pos.0 + previous_cache.velocity_dt).xy(); + let query_radius = previous_cache.scaled_radius; spatial_grid - .in_circle_aabr(query_center, query_radius) + .in_swept_circle(query_p0, query_p1, query_radius) .filter_map(|entity| { read.uids .get(entity)