Add a more precise in_swept_circle to SpatialGrid, and use it in entity-entity broadphase.

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<f32>,
 }

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<f32>,
+        p1: Vec2<f32>,
+        radius: f32,
+    ) -> 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 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();

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)