Accelerate world queries with ordered bounds hierarchy and triangle culling

crates/skate-core/src/physics/board_world.rs

@@ -3,6 +3,7 @@
 //! triangle fixup82AD3130, material combine82763078 and contact retention
 //! determine the physical result, in world-triangle then moving-volume order.
 pub mod query_metadata;
+mod query_index;
 use crate::math::Vector3;
 use query_metadata::{Bounds, QueryMetadata};
 
@@ -102,7 +103,9 @@ pub struct BoardWorldVolume {
 /// the returned contacts are valid until the next mutable call.
 pub struct BoardWorld {
     triangles: Vec<WorldTriangle>,
+    triangle_bounds: Vec<Bounds>,
     query_metadata: Option<QueryMetadata>,
+    query_index: query_index::QueryIndex,
     contacts: Vec<BoardCollision>,
     buffer: ContactBuffer,
     maximum_fatness: f32,
@@ -110,6 +113,9 @@ pub struct BoardWorld {
 
 impl BoardWorld {
     pub fn new(triangles: Vec<WorldTriangle>) -> Self {
+        let triangle_bounds = triangles.iter().map(|t| Bounds::from_points(t.triangle.vertices)
+            .unwrap_or(Bounds { min: Vector3::new(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY),
+                max: Vector3::new(f32::INFINITY, f32::INFINITY, f32::INFINITY) })).collect();
         let maximum_fatness = triangles
             .iter()
             .map(|t| t.triangle.fatness)
@@ -117,7 +123,9 @@ impl BoardWorld {
         Self {
             maximum_fatness,
             triangles,
+            triangle_bounds,
             query_metadata: None,
+            query_index: query_index::QueryIndex::default(),
             contacts: Vec::new(),
             buffer: ContactBuffer {
                 count: 0,
@@ -139,6 +147,7 @@ impl BoardWorld {
     ) -> Result<Self, &'static str> {
         metadata.validate(&triangles)?;
         let mut world = Self::new(triangles);
+        world.query_index = query_index::QueryIndex::new(&metadata.meshes);
         world.query_metadata = Some(metadata);
         Ok(world)
     }
@@ -171,11 +180,9 @@ impl BoardWorld {
         .map(f32::abs)
         .fold(1., f32::max);
         let bounds = bounds.expanded(self.maximum_fatness + scale * (8. * f32::EPSILON));
-        metadata
-            .meshes
-            .iter()
-            .filter(|m| m.local_bounds.overlaps(bounds))
-            .map(|m| m.triangle_range.clone())
+        self.query_index.query(bounds, &metadata.meshes)
+            .into_iter()
+            .map(|i| metadata.meshes[i].triangle_range.clone())
             .collect()
     }
 
@@ -185,9 +192,11 @@ impl BoardWorld {
         end: Vector3,
         radius: f32,
     ) -> impl Iterator<Item = (usize, &WorldTriangle)> {
-        self.candidate_ranges(Bounds::from_points([start, end]).map(|b| b.expanded(radius)))
+        let bounds = Bounds::from_points([start, end]).map(|b| conservative_bounds(b, radius + self.maximum_fatness));
+        self.candidate_ranges(bounds)
             .into_iter()
             .flatten()
+            .filter(move |&i| self.query_metadata.is_none() || bounds.is_none_or(|b| self.triangle_bounds[i].overlaps(b)))
             .map(|i| (i, &self.triangles[i]))
     }
 
@@ -287,10 +296,11 @@ impl BoardWorld {
         self.buffer.distance_squared_threshold = retention.duplicate_distance_squared;
         self.buffer.deferred_reduction = u8::from(retention.deferred_reduction);
         let padding = query.volume_padding.max(0.) + query.maximum_separating_distance.max(0.);
-        let bounds: Option<Vec<_>> = volumes
+        let volume_bounds: Vec<_> = volumes
             .iter()
-            .map(|v| primitive_bounds(v.primitive))
+            .map(|v| primitive_bounds(v.primitive).map(|b| conservative_bounds(b, padding + self.maximum_fatness)))
             .collect();
+        let bounds: Option<Vec<_>> = volume_bounds.iter().copied().collect();
         let bounds = bounds
             .and_then(|b| Bounds::from_points(b.iter().flat_map(|b| [b.min, b.max])))
             .map(|b| b.expanded(padding));
@@ -301,7 +311,11 @@ impl BoardWorld {
         };
         for index in ranges.into_iter().flatten() {
             let entry = &self.triangles[index];
-            for volume in volumes {
+            for (volume, volume_bounds) in volumes.iter().zip(&volume_bounds) {
+                if self.query_metadata.is_some()
+                    && volume_bounds.is_some_and(|b| !self.triangle_bounds[index].overlaps(b)) {
+                    continue;
+                }
                 let Some(manifold) = primitive_triangle_world_contacts(
                     volume.primitive,
                     entry.triangle,
@@ -354,10 +368,13 @@ fn primitive_bounds(primitive: ContactPrimitive) -> Option<Bounds> {
             axis,
             half_length,
             radius,
-        } => (
-            center,
-            radius + half_length.abs() * (axis.x.abs() + axis.y.abs() + axis.z.abs()),
-        ),
+        } => {
+            let offset = Vector3::new(axis.x * half_length, axis.y * half_length, axis.z * half_length);
+            return Bounds::from_points([
+                Vector3::new(center.x - offset.x, center.y - offset.y, center.z - offset.z),
+                Vector3::new(center.x + offset.x, center.y + offset.y, center.z + offset.z),
+            ]).map(|b| b.expanded(radius.abs()));
+        },
         ContactPrimitive::RoundedBox {
             center,
             basis,
@@ -365,13 +382,12 @@ fn primitive_bounds(primitive: ContactPrimitive) -> Option<Bounds> {
             radius,
         } => {
             let half = [half_extents.x, half_extents.y, half_extents.z];
-            let extent: f32 = basis
-                .columns
-                .iter()
-                .zip(half)
-                .map(|(axis, h)| h.abs() * axis.iter().map(|v| v.abs()).sum::<f32>())
-                .sum();
-            (center, radius + extent)
+            let extent: [f32; 3] = std::array::from_fn(|axis| radius.abs() + basis.columns.iter().zip(half)
+                .map(|(column, h)| h.abs() * column[axis].abs()).sum::<f32>());
+            return Bounds::from_points([
+                Vector3::new(center.x - extent[0], center.y - extent[1], center.z - extent[2]),
+                Vector3::new(center.x + extent[0], center.y + extent[1], center.z + extent[2]),
+            ]);
         }
         ContactPrimitive::Triangle(t) => {
             return Bounds::from_points(t.vertices).map(|b| b.expanded(t.fatness));
@@ -383,6 +399,12 @@ fn primitive_bounds(primitive: ContactPrimitive) -> Option<Bounds> {
     Bounds::from_points([center]).map(|b| b.expanded(radius.abs()))
 }
 
+fn conservative_bounds(bounds: Bounds, padding: f32) -> Bounds {
+    let scale = [bounds.min.x, bounds.min.y, bounds.min.z, bounds.max.x, bounds.max.y, bounds.max.z]
+        .into_iter().map(f32::abs).fold(1., f32::max);
+    bounds.expanded(padding + scale * (8. * f32::EPSILON))
+}
+
 /// Retention reads only contact header geometry, body IDs and material/tag.
 /// BoardStep builds the actual body workspaces after this frame's force queue
 /// has been applied, preventing stale acceleration copies in solver contacts.

crates/skate-core/src/physics/board_world/query_index.rs

@@ -0,0 +1,87 @@
+//! Static bounds hierarchy over authored query meshes. Results are sorted back
+//! into source order before narrow-phase, preserving equal-hit/contact order.
+use super::query_metadata::{Bounds, QueryMesh};
+use std::ops::Range;
+
+#[derive(Default)]
+pub(super) struct QueryIndex {
+    nodes: Vec<Node>,
+    order: Vec<usize>,
+}
+struct Node {
+    bounds: Bounds,
+    children: Option<(usize, usize)>,
+    range: Range<usize>,
+}
+impl QueryIndex {
+    pub(super) fn new(meshes: &[QueryMesh]) -> Self {
+        let mut index = Self { nodes: Vec::new(), order: (0..meshes.len()).collect() };
+        if !meshes.is_empty() { index.build(meshes, 0..meshes.len()); }
+        index
+    }
+    fn build(&mut self, meshes: &[QueryMesh], range: Range<usize>) -> usize {
+        let bounds = Bounds::from_points(self.order[range.clone()].iter()
+            .flat_map(|&i| [meshes[i].local_bounds.min, meshes[i].local_bounds.max])).unwrap();
+        let id = self.nodes.len();
+        self.nodes.push(Node { bounds, children: None, range: range.clone() });
+        if range.len() > 8 {
+            let extents = [bounds.max.x - bounds.min.x, bounds.max.y - bounds.min.y, bounds.max.z - bounds.min.z];
+            let axis = (0..3).max_by(|&a, &b| extents[a].total_cmp(&extents[b])).unwrap();
+            let center = |i: usize| {
+                let b = meshes[i].local_bounds;
+                let low = [b.min.x, b.min.y, b.min.z][axis];
+                let high = [b.max.x, b.max.y, b.max.z][axis];
+                low * 0.5 + high * 0.5
+            };
+            let mid = range.start + range.len() / 2;
+            self.order[range.clone()].select_nth_unstable_by(range.len() / 2,
+                |&a, &b| center(a).total_cmp(&center(b)).then(a.cmp(&b)));
+            let left = self.build(meshes, range.start..mid);
+            let right = self.build(meshes, mid..range.end);
+            self.nodes[id].children = Some((left, right));
+        }
+        id
+    }
+    pub(super) fn query(&self, bounds: Bounds, meshes: &[QueryMesh]) -> Vec<usize> {
+        let mut result = Vec::new();
+        if self.nodes.is_empty() { return result; }
+        let mut stack = vec![0];
+        while let Some(id) = stack.pop() {
+            let node = &self.nodes[id];
+            if !node.bounds.overlaps(bounds) { continue; }
+            if let Some((left, right)) = node.children {
+                stack.push(right); stack.push(left);
+            } else {
+                result.extend(self.order[node.range.clone()].iter().copied()
+                    .filter(|&i| meshes[i].local_bounds.overlaps(bounds)));
+            }
+        }
+        result.sort_unstable();
+        result
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::{math::Vector3, physics::{drive_frames::RetailAffineTransform, board_world::query_metadata::QueryPool}};
+    #[test]
+    fn hierarchy_matches_linear_scan_including_boundaries_and_source_order() {
+        let meshes: Vec<_> = (0..1024).map(|i| {
+            let x = ((i * 37) % 32) as f32 * 10.;
+            let z = (i / 32) as f32 * 10.;
+            QueryMesh { triangle_range: i..i+1,
+                local_to_world: RetailAffineTransform::IDENTITY, world_to_local: RetailAffineTransform::IDENTITY,
+                local_bounds: Bounds { min: Vector3::new(x, 0., z), max: Vector3::new(x+5., 5., z+5.) },
+                matching_group: -1, pool: QueryPool::Ground }
+        }).collect();
+        let index = QueryIndex::new(&meshes);
+        for x in [-100., 0., 5., 10., 47., 155., 310., 320.] {
+            for radius in [0., 5., 31., 1000.] {
+                let bounds = Bounds::from_points([Vector3::new(x, 0., x)]).unwrap().expanded(radius);
+                let expected: Vec<_> = meshes.iter().enumerate().filter(|(_, m)| m.local_bounds.overlaps(bounds)).map(|(i, _)| i).collect();
+                assert_eq!(index.query(bounds, &meshes), expected);
+            }
+        }
+    }
+}

crates/skate-core/src/physics/tests/board_world.rs

@@ -205,6 +205,36 @@ fn cluster_culling_preserves_predictive_contacts_for_every_shape() {
     }
 }
 
+#[test]
+fn triangle_bounds_match_unfiltered_contacts_near_edges_and_rotated_boxes() {
+    let mut floor = triangle();
+    floor.triangle.fatness = 0.15;
+    let mut indexed = clustered(vec![floor]);
+    let mut full = BoardWorld::new(vec![floor]);
+    let mut settings = config();
+    settings.query.volume_padding = 0.1;
+    settings.query.maximum_separating_distance = 0.8;
+    for x in [-10.5, -10., -5., 0., 5., 10., 10.5] {
+        for z in [-10.5, -10., 0., 10., 10.5] {
+            for y in [-0.1, 0.2, 1., 2.] {
+                let center = Vector3::new(x, y, z);
+                for primitive in [
+                    ContactPrimitive::Sphere(Sphere { center, radius: 0.2 }),
+                    ContactPrimitive::Capsule { center, axis: Vector3::new(0.6, 0.8, 0.), half_length: 0.7, radius: 0.2 },
+                    ContactPrimitive::RoundedBox { center,
+                        basis: Basis3 { columns: [[0.6, 0., 0.8], [0., 1., 0.], [-0.8, 0., 0.6]] },
+                        half_extents: Vector3::new(0.6, 0.2, 0.1), radius: 0.05 },
+                ] {
+                    let volumes = [BoardWorldVolume { body: CollisionBody::Board(BodyId::ORDER[0]), primitive,
+                        linear_velocity: Vector3::new(20., -60., -30.), material: settings.material }];
+                    assert_eq!(format!("{:?}", indexed.query_primitives(&volumes, settings.query, settings.retention)),
+                        format!("{:?}", full.query_primitives(&volumes, settings.query, settings.retention)));
+                }
+            }
+        }
+    }
+}
+
 #[test]
 fn finite_world_geometry_produces_four_ordered_wheel_contacts_with_combined_materials() {
     let mut world = BoardWorld::new(vec![triangle()]);