From 5159538b85256eb41aefbecca5fc6e938835dc59 Mon Sep 17 00:00:00 2001 From: chazmm Date: Sun, 6 Sep 2026 15:17:27 +0100 Subject: [PATCH] Accelerate world queries with ordered bounds hierarchy and triangle culling --- crates/skate-core/src/physics/board_world.rs | 62 ++++++++----- .../src/physics/board_world/query_index.rs | 87 +++++++++++++++++++ .../src/physics/tests/board_world.rs | 30 +++++++ 3 files changed, 159 insertions(+), 20 deletions(-) create mode 100644 crates/skate-core/src/physics/board_world/query_index.rs diff --git a/crates/skate-core/src/physics/board_world.rs b/crates/skate-core/src/physics/board_world.rs index 0fd0618a..13d60fe4 100644 --- a/crates/skate-core/src/physics/board_world.rs +++ b/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, + triangle_bounds: Vec, query_metadata: Option, + query_index: query_index::QueryIndex, contacts: Vec, buffer: ContactBuffer, maximum_fatness: f32, @@ -110,6 +113,9 @@ pub struct BoardWorld { impl BoardWorld { pub fn new(triangles: Vec) -> 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 { 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 { - 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> = 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> = 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 { 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 { 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::()) - .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::()); + 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::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. diff --git a/crates/skate-core/src/physics/board_world/query_index.rs b/crates/skate-core/src/physics/board_world/query_index.rs new file mode 100644 index 00000000..b5c4b691 --- /dev/null +++ b/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, + order: Vec, +} +struct Node { + bounds: Bounds, + children: Option<(usize, usize)>, + range: Range, +} +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 { + 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(¢er(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 { + 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); + } + } + } +} diff --git a/crates/skate-core/src/physics/tests/board_world.rs b/crates/skate-core/src/physics/tests/board_world.rs index 9957407c..7c00b0de 100644 --- a/crates/skate-core/src/physics/tests/board_world.rs +++ b/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()]);