From 2ac354aa46b4af932be6752e82c870a44a141ea7 Mon Sep 17 00:00:00 2001 From: Camilo Talero Date: Thu, 25 Apr 2024 12:38:24 -0500 Subject: [PATCH] Add feature to traverse a bvh passing a context from parent to children (#191) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add feature to traverse a bvh passing a context from parent to children * Add bvh visitor thatt allows a context to be passed to the children of a node Co-authored-by: Sébastien Crozet * Export SimdVisitorWithContext * Remove SimdVisitorWithContext default trait implementation Removed because `SimdVisitor`'s default implementation causes issues. It will be automatically implemented for types that may want to implement `SimdVisitorWithContext` and thus cause them to automatically implement the default trait implementation. Since there can only be one trait implementation per type, this effectively prevents the use of the new trait. * warnings fixes --------- Co-authored-by: Makogan (Makogan) Co-authored-by: Sébastien Crozet Co-authored-by: Sébastien Crozet --- CHANGELOG.md | 5 +- src/partitioning/mod.rs | 2 +- src/partitioning/qbvh/traversal.rs | 70 ++++++++++++++++++- src/partitioning/visitor.rs | 16 +++++ .../contact_manifolds_workspace.rs | 1 + src/shape/shape.rs | 1 + src/utils/weighted_value.rs | 2 +- 7 files changed, 93 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 74219da5..cca06ded 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,7 +1,10 @@ # Change Log -## v0.13.7 +## Unreleased + +- Added `Qbvh::traverse_depth_first_with_context`, `Qbvh::traverse_depth_first_node_with_stack_and_context`, and the related `SimdVisitorWithContext` trait to allow parent nodes to pass a custom context to its children during recursion. +## v0.13.7 ### Modified - The `point_in_poly2d` now handles arbitrary (convex and non-convex) polygons. The previous implementation diff --git a/src/partitioning/mod.rs b/src/partitioning/mod.rs index 48c03823..847129ec 100644 --- a/src/partitioning/mod.rs +++ b/src/partitioning/mod.rs @@ -11,7 +11,7 @@ pub use self::qbvh::{ pub use self::visitor::{ParallelSimdSimultaneousVisitor, ParallelSimdVisitor}; pub use self::visitor::{ SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdSimultaneousVisitStatus, - SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, + SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, SimdVisitorWithContext, }; /// A quaternary bounding-volume-hierarchy. diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index dcdaf792..49d12010 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -2,7 +2,7 @@ use crate::bounding_volume::{Aabb, SimdAabb}; use crate::math::Real; -use crate::partitioning::visitor::SimdSimultaneousVisitStatus; +use crate::partitioning::visitor::{SimdSimultaneousVisitStatus, SimdVisitorWithContext}; use crate::partitioning::{ GenericQbvh, QbvhStorage, SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, @@ -109,6 +109,74 @@ impl> GenericQbvh( + &self, + visitor: &mut impl SimdVisitorWithContext, + context: Context, + ) -> bool { + self.traverse_depth_first_node_with_stack_and_context(visitor, &mut Vec::new(), 0, context) + } + + /// Performs a depth-first traversal on the BVH and propagates a context down, + /// from the root to each of its descendants. The context can be modified + /// during the query. + /// + /// # Return + /// + /// Returns `false` if the traversal exited early, and `true` otherwise. + pub fn traverse_depth_first_node_with_stack_and_context( + &self, + visitor: &mut impl SimdVisitorWithContext, + stack: &mut Vec<(u32, Context)>, + start_node: u32, + context: Context, + ) -> bool { + stack.clear(); + + if !self.nodes.is_empty() { + stack.push((start_node, context)); + } + while let Some((entry, context)) = stack.pop() { + let node = &self.nodes[entry as usize]; + let leaf_data = if node.is_leaf() { + Some( + array![|ii| Some(&self.proxies.get_at(node.children[ii] as usize)?.data); SIMD_WIDTH], + ) + } else { + None + }; + + let (visit_result, contexts) = visitor.visit(&node.simd_aabb, leaf_data, context); + match visit_result { + SimdVisitStatus::ExitEarly => { + return false; + } + SimdVisitStatus::MaybeContinue(mask) => { + let bitmask = mask.bitmask(); + + for ii in 0..SIMD_WIDTH { + if (bitmask & (1 << ii)) != 0 && !node.is_leaf() { + // Internal node, visit the child. + // Un fortunately, we have this check because invalid Aabbs + // return a hit as well. + if node.children[ii] as usize <= self.nodes.len() { + stack.push((node.children[ii], contexts[ii].clone())); + } + } + } + } + } + } + + true + } + /// Performs a best-first-search on the BVH. /// /// Returns the content of the leaf with the smallest associated cost, and a result of diff --git a/src/partitioning/visitor.rs b/src/partitioning/visitor.rs index 3fd48be2..5d9b899b 100644 --- a/src/partitioning/visitor.rs +++ b/src/partitioning/visitor.rs @@ -78,6 +78,22 @@ where (self)(bv, data) } } + +/// Trait implemented by visitor called during the traversal of a spatial partitioning data structure. +pub trait SimdVisitorWithContext { + /// Execute an operation on the content of a node of the spatial partitioning structure. + /// + /// Returns whether the traversal should continue on the node's children, if it should not continue + /// on those children, or if the whole traversal should be exited early. Also returns + /// a context, which may or may not be identical to the input context. + fn visit( + &mut self, + bv: &SimdBV, + data: Option<[Option<&LeafData>; SIMD_WIDTH]>, + context: Context, + ) -> (SimdVisitStatus, [Context; SIMD_WIDTH]); +} + /// Trait implemented by visitor called during a simultaneous spatial partitioning data structure tarversal. pub trait SimdSimultaneousVisitor { /// Execute an operation on the content of two nodes, one from each structure. diff --git a/src/query/contact_manifolds/contact_manifolds_workspace.rs b/src/query/contact_manifolds/contact_manifolds_workspace.rs index f808a0dd..cf46eaf6 100644 --- a/src/query/contact_manifolds/contact_manifolds_workspace.rs +++ b/src/query/contact_manifolds/contact_manifolds_workspace.rs @@ -42,6 +42,7 @@ enum DeserializableWorkspaceData { CompositeShapeCompositeShapeContactManifoldsWorkspace, ), CompositeShapeShapeContactManifoldsWorkspace(CompositeShapeShapeContactManifoldsWorkspace), + #[allow(dead_code)] // The u32 is needed to match `TypedWorkspaceData`. Custom(u32), } diff --git a/src/shape/shape.rs b/src/shape/shape.rs index 3755b68e..2c9aa64d 100644 --- a/src/shape/shape.rs +++ b/src/shape/shape.rs @@ -221,6 +221,7 @@ pub(crate) enum DeserializableTypedShape { #[cfg(feature = "std")] RoundConvexPolygon(RoundConvexPolygon), /// A custom user-defined shape identified by a number. + #[allow(dead_code)] // The u32 is needed to match `TypedShape`. Custom(u32), } diff --git a/src/utils/weighted_value.rs b/src/utils/weighted_value.rs index 2e54258e..b4619332 100644 --- a/src/utils/weighted_value.rs +++ b/src/utils/weighted_value.rs @@ -1,5 +1,5 @@ use crate::math::Real; -use std::cmp::{Eq, Ord, Ordering, PartialEq, PartialOrd}; +use std::cmp::Ordering; #[derive(Copy, Clone)] pub struct WeightedValue {