diff --git a/build/ncollide2d/examples/ray_bvt2d.rs b/build/ncollide2d/examples/ray_bvt2d.rs index 0e99764d2..8c9076416 100644 --- a/build/ncollide2d/examples/ray_bvt2d.rs +++ b/build/ncollide2d/examples/ray_bvt2d.rs @@ -3,8 +3,8 @@ extern crate ncollide2d; use na::{Isometry2, Point2, Vector2}; use ncollide2d::bounding_volume::{self, BoundingSphere, HasBoundingVolume}; -use ncollide2d::partitioning::{BVH, BVT}; -use ncollide2d::query::{visitors::RayInterferencesCollector, Ray, RayCast}; +use ncollide2d::partitioning::{VisitStatus, BVH, BVT}; +use ncollide2d::query::{visitors::RayInterferencesVisitor, Ray, RayCast}; use ncollide2d::shape::{Ball, Cuboid}; /* @@ -61,20 +61,24 @@ fn main() { /* * Collecting all objects with bounding volumes intersecting the ray. */ - let mut collector_hit: Vec = Vec::new(); - let mut collector_miss: Vec = Vec::new(); + let mut hit_count = 0; + let mut miss_count = 0; // We need a new scope here to avoid borrowing issues. { - let mut visitor_hit = - RayInterferencesCollector::new(&ray_hit, std::f64::MAX, &mut collector_hit); - let mut visitor_miss = - RayInterferencesCollector::new(&ray_miss, std::f64::MAX, &mut collector_miss); + let mut visitor_hit = RayInterferencesVisitor::new(&ray_hit, std::f64::MAX, |_| { + hit_count += 1; + VisitStatus::Continue + }); + let mut visitor_miss = RayInterferencesVisitor::new(&ray_miss, std::f64::MAX, |_| { + miss_count += 1; + VisitStatus::Continue + }); bvt.visit(&mut visitor_hit); bvt.visit(&mut visitor_miss); } - assert!(collector_hit.len() == 3); - assert!(collector_miss.len() == 0); + assert_eq!(hit_count, 3); + assert_eq!(miss_count, 0); } diff --git a/build/ncollide3d/examples/ray_bvt3d.rs b/build/ncollide3d/examples/ray_bvt3d.rs index 792dc6a79..43cd3de46 100644 --- a/build/ncollide3d/examples/ray_bvt3d.rs +++ b/build/ncollide3d/examples/ray_bvt3d.rs @@ -3,8 +3,8 @@ extern crate ncollide3d; use na::{Isometry3, Point3, Vector3}; use ncollide3d::bounding_volume::{self, BoundingSphere, HasBoundingVolume}; -use ncollide3d::partitioning::{BVH, BVT}; -use ncollide3d::query::{visitors::RayInterferencesCollector, Ray, RayCast}; +use ncollide3d::partitioning::{VisitStatus, BVH, BVT}; +use ncollide3d::query::{visitors::RayInterferencesVisitor, Ray, RayCast}; use ncollide3d::shape::{Ball, Capsule, Cone, Cuboid}; /* @@ -60,20 +60,24 @@ fn main() { /* * Ray cast using a visitor. */ - let mut collector_hit: Vec = Vec::new(); - let mut collector_miss: Vec = Vec::new(); + let mut hit_count = 0; + let mut miss_count = 0; // We need a new scope here to avoid borrowing issues. { - let mut visitor_hit = - RayInterferencesCollector::new(&ray_hit, std::f64::MAX, &mut collector_hit); - let mut visitor_miss = - RayInterferencesCollector::new(&ray_miss, std::f64::MAX, &mut collector_miss); + let mut visitor_hit = RayInterferencesVisitor::new(&ray_hit, std::f64::MAX, |_| { + hit_count += 1; + VisitStatus::Continue + }); + let mut visitor_miss = RayInterferencesVisitor::new(&ray_miss, std::f64::MAX, |_| { + miss_count += 1; + VisitStatus::Continue + }); bvt.visit(&mut visitor_hit); bvt.visit(&mut visitor_miss); } - assert!(collector_hit.len() == 3); - assert!(collector_miss.len() == 0); + assert_eq!(hit_count, 3); + assert_eq!(miss_count, 0); } diff --git a/src/pipeline/broad_phase/dbvt_broad_phase.rs b/src/pipeline/broad_phase/dbvt_broad_phase.rs index b024df45c..414cf5f14 100644 --- a/src/pipeline/broad_phase/dbvt_broad_phase.rs +++ b/src/pipeline/broad_phase/dbvt_broad_phase.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingVolume; use crate::math::Point; -use crate::partitioning::{DBVTLeaf, DBVTLeafId, BVH, DBVT}; +use crate::partitioning::{DBVTLeaf, DBVTLeafId, VisitStatus, BVH, DBVT}; use crate::pipeline::broad_phase::{ BroadPhase, BroadPhaseInterferenceHandler, BroadPhaseProxyHandle, }; use crate::query::visitors::{ - BoundingVolumeInterferencesCollector, PointInterferencesCollector, RayInterferencesCollector, + BoundingVolumeInterferencesVisitor, PointInterferencesVisitor, RayInterferencesVisitor, RayIntersectionCostFnVisitor, }; use crate::query::{PointQuery, Ray, RayCast, RayIntersection}; @@ -67,7 +67,6 @@ pub struct DBVTBroadPhase { purge_all: bool, // Just to avoid dynamic allocations. - collector: Vec, leaves_to_update: Vec>, proxies_to_update: VecDeque<(BroadPhaseProxyHandle, BV)>, } @@ -85,7 +84,6 @@ where stree: DBVT::new(), pairs: HashMap::with_hasher(DeterministicState::new()), purge_all: false, - collector: Vec::new(), leaves_to_update: Vec::new(), proxies_to_update: VecDeque::new(), margin, @@ -171,7 +169,7 @@ where BV: BoundingVolume + RayCast + PointQuery + Any + Send + Sync + Clone, T: Any + Send + Sync + Clone, { - fn update(&mut self, handler: &mut dyn BroadPhaseInterferenceHandler) { + fn update(&mut self, mut handler: &mut dyn BroadPhaseInterferenceHandler) { /* * Remove from the trees all nodes that have been deleted or modified. */ @@ -218,32 +216,34 @@ where for leaf in self.leaves_to_update.drain(..) { { let proxy1 = &self.proxies[leaf.data.uid()]; - { - let mut visitor = BoundingVolumeInterferencesCollector::new( + let mut visitor = { + let proxies = &self.proxies; + let pairs = &mut self.pairs; + let handler = &mut handler; + let leaf = &leaf; + BoundingVolumeInterferencesVisitor::new( &leaf.bounding_volume, - &mut self.collector, - ); - - self.tree.visit(&mut visitor); - self.stree.visit(&mut visitor); - } - - // Event generation. - for proxy_key2 in self.collector.iter() { - let proxy2 = &self.proxies[proxy_key2.uid()]; - - if handler.is_interference_allowed(&proxy1.data, &proxy2.data) { - match self.pairs.entry(SortedPair::new(leaf.data, *proxy_key2)) { - Entry::Occupied(entry) => *entry.into_mut() = true, - Entry::Vacant(entry) => { - handler.interference_started(&proxy1.data, &proxy2.data); - let _ = entry.insert(true); + move |proxy_key2: &BroadPhaseProxyHandle| { + // Event generation. + let proxy2 = &proxies[proxy_key2.uid()]; + + if handler.is_interference_allowed(&proxy1.data, &proxy2.data) { + match pairs.entry(SortedPair::new(leaf.data, *proxy_key2)) { + Entry::Occupied(entry) => *entry.into_mut() = true, + Entry::Vacant(entry) => { + handler.interference_started(&proxy1.data, &proxy2.data); + let _ = entry.insert(true); + } + } } - } - } - } - self.collector.clear(); + VisitStatus::Continue + }, + ) + }; + + self.tree.visit(&mut visitor); + self.stree.visit(&mut visitor); } let proxy1 = &mut self.proxies[leaf.data.uid()]; @@ -385,48 +385,36 @@ where } fn interferences_with_bounding_volume<'a>(&'a self, bv: &BV, out: &mut Vec<&'a T>) { - let mut collector = Vec::new(); - - { - let mut visitor = BoundingVolumeInterferencesCollector::new(bv, &mut collector); - - self.tree.visit(&mut visitor); - self.stree.visit(&mut visitor); - } + let mut visitor = + BoundingVolumeInterferencesVisitor::new(bv, |handle: &BroadPhaseProxyHandle| { + out.push(&self.proxies[handle.uid()].data); + VisitStatus::Continue + }); - for l in collector.into_iter() { - out.push(&self.proxies[l.uid()].data) - } + self.tree.visit(&mut visitor); + self.stree.visit(&mut visitor); } fn interferences_with_ray<'a>(&'a self, ray: &Ray, max_toi: N, out: &mut Vec<&'a T>) { - let mut collector = Vec::new(); - - { - let mut visitor = RayInterferencesCollector::new(ray, max_toi, &mut collector); - - self.tree.visit(&mut visitor); - self.stree.visit(&mut visitor); - } + let mut visitor = + RayInterferencesVisitor::new(ray, max_toi, |handle: &BroadPhaseProxyHandle| { + out.push(&self.proxies[handle.uid()].data); + VisitStatus::Continue + }); - for l in collector.into_iter() { - out.push(&self.proxies[l.uid()].data) - } + self.tree.visit(&mut visitor); + self.stree.visit(&mut visitor); } fn interferences_with_point<'a>(&'a self, point: &Point, out: &mut Vec<&'a T>) { - let mut collector = Vec::new(); - - { - let mut visitor = PointInterferencesCollector::new(point, &mut collector); - - self.tree.visit(&mut visitor); - self.stree.visit(&mut visitor); - } + let mut visitor = + PointInterferencesVisitor::new(point, |handle: &BroadPhaseProxyHandle| { + out.push(&self.proxies[handle.uid()].data); + VisitStatus::Continue + }); - for l in collector.into_iter() { - out.push(&self.proxies[l.uid()].data) - } + self.tree.visit(&mut visitor); + self.stree.visit(&mut visitor); } /// Returns the first object that interferes with a ray. diff --git a/src/pipeline/narrow_phase/contact_generator/composite_shape_composite_shape_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/composite_shape_composite_shape_manifold_generator.rs index ae4e37673..a29e6b850 100644 --- a/src/pipeline/narrow_phase/contact_generator/composite_shape_composite_shape_manifold_generator.rs +++ b/src/pipeline/narrow_phase/contact_generator/composite_shape_composite_shape_manifold_generator.rs @@ -1,10 +1,10 @@ use crate::math::Isometry; +use crate::partitioning::VisitStatus; use crate::pipeline::narrow_phase::{ ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator, }; use crate::query::{ - visitors::AABBSetsInterferencesCollector, ContactManifold, ContactPrediction, - ContactPreprocessor, + visitors::AABBSetsInterferencesVisitor, ContactManifold, ContactPrediction, ContactPreprocessor, }; use crate::shape::{CompositeShape, Shape}; use crate::utils::DeterministicState; @@ -14,7 +14,6 @@ use std::collections::{hash_map::Entry, HashMap}; /// Collision detector between a concave shape and another shape. pub struct CompositeShapeCompositeShapeManifoldGenerator { sub_detectors: HashMap<(usize, usize), (ContactAlgorithm, usize), DeterministicState>, - interferences: Vec<(usize, usize)>, timestamp: usize, } @@ -23,7 +22,6 @@ impl CompositeShapeCompositeShapeManifoldGenerator { pub fn new() -> CompositeShapeCompositeShapeManifoldGenerator { CompositeShapeCompositeShapeManifoldGenerator { sub_detectors: HashMap::with_hasher(DeterministicState), - interferences: Vec::new(), timestamp: 0, } } @@ -49,36 +47,33 @@ impl CompositeShapeCompositeShapeManifoldGenerator { // For transforming AABBs from g2 in the local space of g1. let ls_m2_abs_rot = ls_m2.rotation.to_rotation_matrix().matrix().abs(); - { - let mut visitor = AABBSetsInterferencesCollector::new( - prediction.linear(), - &ls_m2, - &ls_m2_abs_rot, - &mut self.interferences, - ); - g1.bvh().visit_bvtt(g2.bvh(), &mut visitor); - } - - for id in self.interferences.drain(..) { - match self.sub_detectors.entry(id) { - Entry::Occupied(mut entry) => { - entry.get_mut().1 = self.timestamp; - } - Entry::Vacant(entry) => { - let mut new_detector = None; + let mut visitor = AABBSetsInterferencesVisitor::new( + prediction.linear(), + &ls_m2, + &ls_m2_abs_rot, + |a, b| { + match self.sub_detectors.entry((*a, *b)) { + Entry::Occupied(mut entry) => { + entry.get_mut().1 = self.timestamp; + } + Entry::Vacant(entry) => { + let mut new_detector = None; - g1.map_part_at(id.0, &Isometry::identity(), &mut |_, g1| { - g2.map_part_at(id.1, &Isometry::identity(), &mut |_, g2| { - new_detector = dispatcher.get_contact_algorithm(g1, g2) + g1.map_part_at(*a, &Isometry::identity(), &mut |_, g1| { + g2.map_part_at(*b, &Isometry::identity(), &mut |_, g2| { + new_detector = dispatcher.get_contact_algorithm(g1, g2) + }); }); - }); - if let Some(new_detector) = new_detector { - let _ = entry.insert((new_detector, self.timestamp)); + if let Some(new_detector) = new_detector { + let _ = entry.insert((new_detector, self.timestamp)); + } } } - } - } + VisitStatus::Continue + }, + ); + g1.bvh().visit_bvtt(g2.bvh(), &mut visitor); // Update all collisions let timestamp = self.timestamp; diff --git a/src/pipeline/narrow_phase/contact_generator/composite_shape_shape_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/composite_shape_shape_manifold_generator.rs index 85573081b..a73e69522 100644 --- a/src/pipeline/narrow_phase/contact_generator/composite_shape_shape_manifold_generator.rs +++ b/src/pipeline/narrow_phase/contact_generator/composite_shape_shape_manifold_generator.rs @@ -1,10 +1,11 @@ use crate::bounding_volume::{self, BoundingVolume}; use crate::math::Isometry; +use crate::partitioning::VisitStatus; use crate::pipeline::narrow_phase::{ ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator, }; use crate::query::{ - visitors::BoundingVolumeInterferencesCollector, ContactManifold, ContactPrediction, + visitors::BoundingVolumeInterferencesVisitor, ContactManifold, ContactPrediction, ContactPreprocessor, ContactTrackingMode, }; use crate::shape::{CompositeShape, Shape}; @@ -15,7 +16,6 @@ use std::collections::{hash_map::Entry, HashMap}; /// Collision detector between a concave shape and another shape. pub struct CompositeShapeShapeManifoldGenerator { sub_detectors: HashMap, usize), DeterministicState>, - interferences: Vec, flip: bool, timestamp: usize, } @@ -25,7 +25,6 @@ impl CompositeShapeShapeManifoldGenerator { pub fn new(flip: bool) -> CompositeShapeShapeManifoldGenerator { CompositeShapeShapeManifoldGenerator { sub_detectors: HashMap::with_hasher(DeterministicState), - interferences: Vec::new(), flip, timestamp: 0, } @@ -49,14 +48,7 @@ impl CompositeShapeShapeManifoldGenerator { // Find new collisions let ls_m2 = m1.inverse() * m2.clone(); let ls_aabb2 = bounding_volume::aabb(g2, &ls_m2).loosened(prediction.linear()); - - { - let mut visitor = - BoundingVolumeInterferencesCollector::new(&ls_aabb2, &mut self.interferences); - g1.bvh().visit(&mut visitor); - } - - for i in self.interferences.drain(..) { + let mut visitor = BoundingVolumeInterferencesVisitor::new(&ls_aabb2, |&i| { match self.sub_detectors.entry(i) { Entry::Occupied(mut entry) => entry.get_mut().1 = self.timestamp, Entry::Vacant(entry) => { @@ -75,7 +67,10 @@ impl CompositeShapeShapeManifoldGenerator { } } } - } + + VisitStatus::Continue + }); + g1.bvh().visit(&mut visitor); // Update all collisions let timestamp = self.timestamp; diff --git a/src/pipeline/narrow_phase/contact_generator/trimesh_trimesh_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/trimesh_trimesh_manifold_generator.rs index 46536566f..776481fa0 100644 --- a/src/pipeline/narrow_phase/contact_generator/trimesh_trimesh_manifold_generator.rs +++ b/src/pipeline/narrow_phase/contact_generator/trimesh_trimesh_manifold_generator.rs @@ -1,7 +1,8 @@ use crate::math::{Isometry, Vector}; +use crate::partitioning::VisitStatus; use crate::pipeline::narrow_phase::{ContactDispatcher, ContactManifoldGenerator}; use crate::query::{ - self, visitors::AABBSetsInterferencesCollector, Contact, ContactKinematic, ContactManifold, + self, visitors::AABBSetsInterferencesVisitor, Contact, ContactKinematic, ContactManifold, ContactPrediction, ContactPreprocessor, ContactTrackingMode, NeighborhoodGeometry, }; use crate::shape::{ @@ -9,7 +10,6 @@ use crate::shape::{ SegmentPointLocation, Shape, TriMesh, Triangle, }; use na::{self, RealField, Unit}; -use std::mem; /// Collision detector between a concave shape and another shape. pub struct TriMeshTriMeshManifoldGenerator { @@ -17,7 +17,6 @@ pub struct TriMeshTriMeshManifoldGenerator { new_contacts: Vec<(Contact, FeatureId, FeatureId)>, convex_feature1: ConvexPolygonalFeature, convex_feature2: ConvexPolygonalFeature, - interferences: Vec<(usize, usize)>, } impl TriMeshTriMeshManifoldGenerator { @@ -28,7 +27,6 @@ impl TriMeshTriMeshManifoldGenerator { new_contacts: Vec::new(), convex_feature1: ConvexPolygonalFeature::with_size(3), convex_feature2: ConvexPolygonalFeature::with_size(3), - interferences: Vec::new(), } } } @@ -538,24 +536,18 @@ impl ContactManifoldGenerator for TriMeshTriMeshManifoldGenerat // For transforming AABBs from mesh2 in the local space of mesh1. let m12_abs_rot = m12.rotation.to_rotation_matrix().matrix().abs(); - { - let mut visitor = AABBSetsInterferencesCollector::new( - prediction.linear(), - &m12, - &m12_abs_rot, - &mut self.interferences, - ); - mesh1.bvh().visit_bvtt(mesh2.bvh(), &mut visitor); - } - - let mut interferences = mem::replace(&mut self.interferences, Vec::new()); - for id in interferences.drain(..) { - self.compute_faces_closest_points( - &m12, &m21, m1, mesh1, id.0, proc1, m2, mesh2, id.1, proc2, prediction, - manifold, - ); - } - self.interferences = interferences; + let mut visitor = AABBSetsInterferencesVisitor::new( + prediction.linear(), + &m12, + &m12_abs_rot, + |&a, &b| { + self.compute_faces_closest_points( + &m12, &m21, m1, mesh1, a, proc1, m2, mesh2, b, proc2, prediction, manifold, + ); + VisitStatus::Continue + }, + ); + mesh1.bvh().visit_bvtt(mesh2.bvh(), &mut visitor); true } else { diff --git a/src/pipeline/narrow_phase/proximity_detector/composite_shape_shape_proximity_detector.rs b/src/pipeline/narrow_phase/proximity_detector/composite_shape_shape_proximity_detector.rs index 91953895c..717989275 100644 --- a/src/pipeline/narrow_phase/proximity_detector/composite_shape_shape_proximity_detector.rs +++ b/src/pipeline/narrow_phase/proximity_detector/composite_shape_shape_proximity_detector.rs @@ -1,7 +1,8 @@ use crate::bounding_volume::{self, BoundingVolume}; use crate::math::Isometry; +use crate::partitioning::VisitStatus; use crate::pipeline::narrow_phase::{ProximityAlgorithm, ProximityDetector, ProximityDispatcher}; -use crate::query::{visitors::BoundingVolumeInterferencesCollector, Proximity}; +use crate::query::{visitors::BoundingVolumeInterferencesVisitor, Proximity}; use crate::shape::{CompositeShape, Shape}; use crate::utils::DeterministicState; use na::{self, RealField}; @@ -11,7 +12,6 @@ use std::collections::{hash_map::Entry, HashMap}; pub struct CompositeShapeShapeProximityDetector { sub_detectors: HashMap, DeterministicState>, to_delete: Vec, - interferences: Vec, intersecting_key: usize, flip: bool, } @@ -22,7 +22,6 @@ impl CompositeShapeShapeProximityDetector { CompositeShapeShapeProximityDetector { sub_detectors: HashMap::with_hasher(DeterministicState), to_delete: Vec::new(), - interferences: Vec::new(), intersecting_key: usize::max_value(), flip, } @@ -48,7 +47,6 @@ impl CompositeShapeShapeProximityDetector { } self.to_delete.clear(); - self.interferences.clear(); // First, test if the previously intersecting shapes are still intersecting. if self.intersecting_key != usize::max_value() { @@ -98,14 +96,9 @@ impl CompositeShapeShapeProximityDetector { } } + let mut result = Some(result); // Find new proximities. - { - let mut visitor = - BoundingVolumeInterferencesCollector::new(&ls_aabb2, &mut self.interferences); - g1.bvh().visit(&mut visitor); - } - - for key in &self.interferences { + let mut visitor = BoundingVolumeInterferencesVisitor::new(&ls_aabb2, |key| { let entry = self.sub_detectors.entry(*key); let detector = match entry { Entry::Occupied(entry) => Some(entry.into_mut()), @@ -139,20 +132,31 @@ impl CompositeShapeShapeProximityDetector { } }); - match prox? { - Proximity::Intersecting => { - self.intersecting_key = *key; - return Some(Proximity::Intersecting); // No need to search further. + if let Some(prox) = prox { + match prox { + Proximity::Intersecting => { + self.intersecting_key = *key; + result = Some(Proximity::Intersecting); + return VisitStatus::ExitEarly; // No need to search further. + } + Proximity::WithinMargin => result = Some(Proximity::WithinMargin), + Proximity::Disjoint => {} } - Proximity::WithinMargin => result = Proximity::WithinMargin, - Proximity::Disjoint => {} + } else { + result = None; + return VisitStatus::ExitEarly; } } - } - // Disjoints or within margin. - self.intersecting_key = usize::max_value(); - Some(result) + VisitStatus::Continue + }); + g1.bvh().visit(&mut visitor); + + if result == Some(Proximity::Intersecting) { + // Disjoints or within margin. + self.intersecting_key = usize::max_value(); + } + result } } diff --git a/src/query/contact/contact_composite_shape_shape.rs b/src/query/contact/contact_composite_shape_shape.rs index 42f7d828f..936d170b5 100644 --- a/src/query/contact/contact_composite_shape_shape.rs +++ b/src/query/contact/contact_composite_shape_shape.rs @@ -1,6 +1,7 @@ use crate::bounding_volume::BoundingVolume; use crate::math::Isometry; -use crate::query::visitors::BoundingVolumeInterferencesCollector; +use crate::partitioning::VisitStatus; +use crate::query::visitors::BoundingVolumeInterferencesVisitor; use crate::query::{self, Contact}; use crate::shape::{CompositeShape, Shape}; use na::{self, RealField}; @@ -20,16 +21,9 @@ where let ls_m2 = m1.inverse() * m2.clone(); let ls_aabb2 = g2.aabb(&ls_m2).loosened(prediction); - let mut interferences = Vec::new(); - - { - let mut visitor = BoundingVolumeInterferencesCollector::new(&ls_aabb2, &mut interferences); - g1.bvh().visit(&mut visitor); - } - let mut res = None::>; - for i in interferences.into_iter() { + let mut visitor = BoundingVolumeInterferencesVisitor::new(&ls_aabb2, |&i| { g1.map_part_at(i, m1, &mut |m, part| { if let Some(c) = query::contact(m, part, m2, g2, prediction) { let replace = res.map_or(true, |cbest| c.depth > cbest.depth); @@ -39,7 +33,10 @@ where } } }); - } + + VisitStatus::Continue + }); + g1.bvh().visit(&mut visitor); res } diff --git a/src/query/visitors/aabb_sets_interferences_collector.rs b/src/query/visitors/aabb_sets_interferences_visitor.rs similarity index 67% rename from src/query/visitors/aabb_sets_interferences_collector.rs rename to src/query/visitors/aabb_sets_interferences_visitor.rs index 7635d3a02..6b5951018 100644 --- a/src/query/visitors/aabb_sets_interferences_collector.rs +++ b/src/query/visitors/aabb_sets_interferences_visitor.rs @@ -2,9 +2,15 @@ use crate::bounding_volume::{BoundingVolume, AABB}; use crate::math::{Isometry, Matrix, Vector}; use crate::partitioning::{SimultaneousVisitor, VisitStatus}; use na::RealField; +use std::marker::PhantomData; /// Spatial partitioning data structure visitor collecting interferences with a given bounding volume. -pub struct AABBSetsInterferencesCollector<'a, N: 'a + RealField, T: 'a> { +pub struct AABBSetsInterferencesVisitor< + 'a, + N: 'a + RealField, + T: 'a, + Visitor: FnMut(&T, &T) -> VisitStatus, +> { /// The transform from the local-space of the second bounding volumes to the local space of the first. pub ls_m2: &'a Isometry, /// The absolute value of the rotation matrix representing `ls_m2.rotation`. @@ -16,29 +22,33 @@ pub struct AABBSetsInterferencesCollector<'a, N: 'a + RealField, T: 'a> { /// AABB pairs closer than `tolerance` will be reported as intersecting. pub tolerence: N, /// The data contained by the nodes with bounding volumes intersecting `self.bv`. - pub collector: &'a mut Vec<(T, T)>, + pub visitor: Visitor, + _data: PhantomData<&'a T>, } -impl<'a, N: RealField, T> AABBSetsInterferencesCollector<'a, N, T> { - /// Creates a new `AABBSetsInterferencesCollector`. +impl<'a, N: RealField, T, Visitor: FnMut(&T, &T) -> VisitStatus> + AABBSetsInterferencesVisitor<'a, N, T, Visitor> +{ + /// Creates a new `AABBSetsInterferencesVisitor`. #[inline] pub fn new( tolerence: N, ls_m2: &'a Isometry, ls_m2_abs_rot: &'a Matrix, - collector: &'a mut Vec<(T, T)>, - ) -> AABBSetsInterferencesCollector<'a, N, T> { - AABBSetsInterferencesCollector { + visitor: Visitor, + ) -> AABBSetsInterferencesVisitor<'a, N, T, Visitor> { + AABBSetsInterferencesVisitor { tolerence, ls_m2, ls_m2_abs_rot, - collector, + visitor, + _data: PhantomData, } } } -impl<'a, N: RealField, T: Clone> SimultaneousVisitor> - for AABBSetsInterferencesCollector<'a, N, T> +impl<'a, N: RealField, T, Visitor: FnMut(&T, &T) -> VisitStatus> SimultaneousVisitor> + for AABBSetsInterferencesVisitor<'a, N, T, Visitor> { #[inline] fn visit( @@ -55,10 +65,10 @@ impl<'a, N: RealField, T: Clone> SimultaneousVisitor> if left_bv.intersects(&ls_right_bv) { if let (Some(a), Some(b)) = (left_data, right_data) { - self.collector.push((a.clone(), b.clone())) + (self.visitor)(a, b) + } else { + VisitStatus::Continue } - - VisitStatus::Continue } else { VisitStatus::Stop } diff --git a/src/query/visitors/bounding_volume_interferences_collector.rs b/src/query/visitors/bounding_volume_interferences_collector.rs deleted file mode 100644 index f5512bb7f..000000000 --- a/src/query/visitors/bounding_volume_interferences_collector.rs +++ /dev/null @@ -1,52 +0,0 @@ -use crate::bounding_volume::BoundingVolume; -use crate::partitioning::{VisitStatus, Visitor}; -use na::RealField; -use std::marker::PhantomData; - -/// Spatial partitioning data structure visitor collecting interferences with a given bounding volume. -pub struct BoundingVolumeInterferencesCollector<'a, N: 'a, T: 'a, BV: 'a> { - /// The bounding volume used for interference tests. - pub bv: &'a BV, - /// The data contained by the nodes with bounding volumes intersecting `self.bv`. - pub collector: &'a mut Vec, - _point: PhantomData, -} - -impl<'a, N, T, BV> BoundingVolumeInterferencesCollector<'a, N, T, BV> -where - N: RealField, - BV: BoundingVolume, -{ - /// Creates a new `BoundingVolumeInterferencesCollector`. - #[inline] - pub fn new( - bv: &'a BV, - buffer: &'a mut Vec, - ) -> BoundingVolumeInterferencesCollector<'a, N, T, BV> { - BoundingVolumeInterferencesCollector { - bv: bv, - collector: buffer, - _point: PhantomData, - } - } -} - -impl<'a, N, T, BV> Visitor for BoundingVolumeInterferencesCollector<'a, N, T, BV> -where - N: RealField, - T: Clone, - BV: BoundingVolume, -{ - #[inline] - fn visit(&mut self, bv: &BV, t: Option<&T>) -> VisitStatus { - if bv.intersects(self.bv) { - if let Some(t) = t { - self.collector.push(t.clone()) - } - - VisitStatus::Continue - } else { - VisitStatus::Stop - } - } -} diff --git a/src/query/visitors/bounding_volume_interferences_visitor.rs b/src/query/visitors/bounding_volume_interferences_visitor.rs new file mode 100644 index 000000000..dcddc8f36 --- /dev/null +++ b/src/query/visitors/bounding_volume_interferences_visitor.rs @@ -0,0 +1,59 @@ +use crate::bounding_volume::BoundingVolume; +use crate::partitioning::{VisitStatus, Visitor}; +use na::RealField; +use std::marker::PhantomData; + +/// Spatial partitioning data structure visitor visiting interferences with a given bounding volume. +pub struct BoundingVolumeInterferencesVisitor< + 'a, + N: 'a, + T: 'a, + BV: 'a, + Visitor: FnMut(&T) -> VisitStatus, +> { + /// The bounding volume used for interference tests. + pub bv: &'a BV, + /// Visitor function. + pub visitor: Visitor, + _data: PhantomData<(N, &'a T)>, +} + +impl<'a, N, T, BV, Visitor: FnMut(&T) -> VisitStatus> + BoundingVolumeInterferencesVisitor<'a, N, T, BV, Visitor> +where + N: RealField, + BV: BoundingVolume, +{ + /// Creates a new `BoundingVolumeInterferencesVisitor`. + #[inline] + pub fn new( + bv: &'a BV, + visitor: Visitor, + ) -> BoundingVolumeInterferencesVisitor<'a, N, T, BV, Visitor> { + BoundingVolumeInterferencesVisitor { + bv, + visitor, + _data: PhantomData, + } + } +} + +impl<'a, N, T, BV, VisitorFn: FnMut(&T) -> VisitStatus> Visitor + for BoundingVolumeInterferencesVisitor<'a, N, T, BV, VisitorFn> +where + N: RealField, + BV: BoundingVolume, +{ + #[inline] + fn visit(&mut self, bv: &BV, t: Option<&T>) -> VisitStatus { + if bv.intersects(self.bv) { + if let Some(t) = t { + (self.visitor)(t) + } else { + VisitStatus::Continue + } + } else { + VisitStatus::Stop + } + } +} diff --git a/src/query/visitors/mod.rs b/src/query/visitors/mod.rs index 7c33abd83..1bcd17f4d 100644 --- a/src/query/visitors/mod.rs +++ b/src/query/visitors/mod.rs @@ -1,17 +1,17 @@ //! Visitors for performing geometric queries exploiting spatial partitioning data structures. -pub use self::aabb_sets_interferences_collector::AABBSetsInterferencesCollector; -pub use self::bounding_volume_interferences_collector::BoundingVolumeInterferencesCollector; +pub use self::aabb_sets_interferences_visitor::AABBSetsInterferencesVisitor; +pub use self::bounding_volume_interferences_visitor::BoundingVolumeInterferencesVisitor; pub use self::composite_closest_point_visitor::CompositeClosestPointVisitor; pub use self::composite_point_containment_test::CompositePointContainmentTest; -pub use self::point_interferences_collector::PointInterferencesCollector; -pub use self::ray_interferences_collector::RayInterferencesCollector; +pub use self::point_interferences_visitor::PointInterferencesVisitor; +pub use self::ray_interferences_visitor::RayInterferencesVisitor; pub use self::ray_intersection_cost_fn_visitor::RayIntersectionCostFnVisitor; -mod aabb_sets_interferences_collector; -mod bounding_volume_interferences_collector; +mod aabb_sets_interferences_visitor; +mod bounding_volume_interferences_visitor; mod composite_closest_point_visitor; mod composite_point_containment_test; -mod point_interferences_collector; -mod ray_interferences_collector; +mod point_interferences_visitor; +mod ray_interferences_visitor; mod ray_intersection_cost_fn_visitor; diff --git a/src/query/visitors/point_interferences_collector.rs b/src/query/visitors/point_interferences_collector.rs deleted file mode 100644 index e5315cc42..000000000 --- a/src/query/visitors/point_interferences_collector.rs +++ /dev/null @@ -1,47 +0,0 @@ -use crate::math::{Isometry, Point}; -use crate::partitioning::{VisitStatus, Visitor}; -use crate::query::PointQuery; -use na::RealField; - -// FIXME: add a point cost fn. - -/// Spatial partitioning structure visitor collecting nodes that may contain a given point. -pub struct PointInterferencesCollector<'a, N: 'a + RealField, T: 'a> { - /// Point to be tested. - pub point: &'a Point, - /// The data contained by the nodes which bounding volume contain `self.point`. - pub collector: &'a mut Vec, -} - -impl<'a, N: RealField, T> PointInterferencesCollector<'a, N, T> { - /// Creates a new `PointInterferencesCollector`. - #[inline] - pub fn new( - point: &'a Point, - buffer: &'a mut Vec, - ) -> PointInterferencesCollector<'a, N, T> { - PointInterferencesCollector { - point: point, - collector: buffer, - } - } -} - -impl<'a, N, T, BV> Visitor for PointInterferencesCollector<'a, N, T> -where - N: RealField, - T: Clone, - BV: PointQuery, -{ - #[inline] - fn visit(&mut self, bv: &BV, t: Option<&T>) -> VisitStatus { - if bv.contains_point(&Isometry::identity(), self.point) { - if let Some(t) = t { - self.collector.push(t.clone()); - } - VisitStatus::Continue - } else { - VisitStatus::Stop - } - } -} diff --git a/src/query/visitors/point_interferences_visitor.rs b/src/query/visitors/point_interferences_visitor.rs new file mode 100644 index 000000000..92d19080f --- /dev/null +++ b/src/query/visitors/point_interferences_visitor.rs @@ -0,0 +1,58 @@ +use crate::math::{Isometry, Point}; +use crate::partitioning::{VisitStatus, Visitor}; +use crate::query::PointQuery; +use na::RealField; +use std::marker::PhantomData; + +// FIXME: add a point cost fn. + +/// Spatial partitioning structure visiting nodes that may contain a given point. +pub struct PointInterferencesVisitor< + 'a, + N: 'a + RealField, + T: 'a, + Visitor: FnMut(&T) -> VisitStatus, +> { + /// Point to be tested. + pub point: &'a Point, + /// Visitor function. + pub visitor: Visitor, + _data: PhantomData<&'a T>, +} + +impl<'a, N: RealField, T, Visitor: FnMut(&T) -> VisitStatus> + PointInterferencesVisitor<'a, N, T, Visitor> +{ + /// Creates a new `PointInterferencesVisitor`. + #[inline] + pub fn new( + point: &'a Point, + visitor: Visitor, + ) -> PointInterferencesVisitor<'a, N, T, Visitor> { + PointInterferencesVisitor { + point, + visitor, + _data: PhantomData, + } + } +} + +impl<'a, N, T, VisitorFn: FnMut(&T) -> VisitStatus, BV> Visitor + for PointInterferencesVisitor<'a, N, T, VisitorFn> +where + N: RealField, + BV: PointQuery, +{ + #[inline] + fn visit(&mut self, bv: &BV, t: Option<&T>) -> VisitStatus { + if bv.contains_point(&Isometry::identity(), self.point) { + if let Some(t) = t { + (self.visitor)(t) + } else { + VisitStatus::Continue + } + } else { + VisitStatus::Stop + } + } +} diff --git a/src/query/visitors/ray_interferences_collector.rs b/src/query/visitors/ray_interferences_collector.rs deleted file mode 100644 index 6f4267cc4..000000000 --- a/src/query/visitors/ray_interferences_collector.rs +++ /dev/null @@ -1,50 +0,0 @@ -use crate::math::Isometry; -use crate::partitioning::{VisitStatus, Visitor}; -use crate::query::{Ray, RayCast}; -use na::RealField; - -/// Bounding Volume Tree visitor collecting interferences with a given ray. -pub struct RayInterferencesCollector<'a, N: 'a + RealField, T: 'a> { - /// Ray to be tested. - pub ray: &'a Ray, - /// The maximum allowed time of impact. - pub max_toi: N, - /// The data contained by the nodes which bounding volume intersects `self.ray`. - pub collector: &'a mut Vec, -} - -impl<'a, N: RealField, T> RayInterferencesCollector<'a, N, T> { - /// Creates a new `RayInterferencesCollector`. - #[inline] - pub fn new( - ray: &'a Ray, - max_toi: N, - buffer: &'a mut Vec, - ) -> RayInterferencesCollector<'a, N, T> { - RayInterferencesCollector { - ray, - max_toi, - collector: buffer, - } - } -} - -impl<'a, N, T, BV> Visitor for RayInterferencesCollector<'a, N, T> -where - N: RealField, - T: Clone, - BV: RayCast, -{ - #[inline] - fn visit(&mut self, bv: &BV, t: Option<&T>) -> VisitStatus { - if bv.intersects_ray(&Isometry::identity(), self.ray, self.max_toi) { - if let Some(t) = t { - self.collector.push(t.clone()) - } - - VisitStatus::Continue - } else { - VisitStatus::Stop - } - } -} diff --git a/src/query/visitors/ray_interferences_visitor.rs b/src/query/visitors/ray_interferences_visitor.rs new file mode 100644 index 000000000..f8d301281 --- /dev/null +++ b/src/query/visitors/ray_interferences_visitor.rs @@ -0,0 +1,56 @@ +use crate::math::Isometry; +use crate::partitioning::{VisitStatus, Visitor}; +use crate::query::{Ray, RayCast}; +use na::RealField; +use std::marker::PhantomData; + +/// Bounding Volume Tree visitor visiting interferences with a given ray. +pub struct RayInterferencesVisitor<'a, N: 'a + RealField, T: 'a, Visitor: FnMut(&T) -> VisitStatus> +{ + /// Ray to be tested. + pub ray: &'a Ray, + /// The maximum allowed time of impact. + pub max_toi: N, + /// Visitor function. + pub visitor: Visitor, + _data: PhantomData<&'a T>, +} + +impl<'a, N: RealField, T, Visitor: FnMut(&T) -> VisitStatus> + RayInterferencesVisitor<'a, N, T, Visitor> +{ + /// Creates a new `RayInterferencesVisitor`. + #[inline] + pub fn new( + ray: &'a Ray, + max_toi: N, + visitor: Visitor, + ) -> RayInterferencesVisitor<'a, N, T, Visitor> { + RayInterferencesVisitor { + ray, + max_toi, + visitor, + _data: PhantomData, + } + } +} + +impl<'a, N, T, VisitorFn: FnMut(&T) -> VisitStatus, BV> Visitor + for RayInterferencesVisitor<'a, N, T, VisitorFn> +where + N: RealField, + BV: RayCast, +{ + #[inline] + fn visit(&mut self, bv: &BV, t: Option<&T>) -> VisitStatus { + if bv.intersects_ray(&Isometry::identity(), self.ray, self.max_toi) { + if let Some(t) = t { + (self.visitor)(t) + } else { + VisitStatus::Continue + } + } else { + VisitStatus::Stop + } + } +} diff --git a/src/transformation/hacd.rs b/src/transformation/hacd.rs index 7a6aedb55..6109bf385 100644 --- a/src/transformation/hacd.rs +++ b/src/transformation/hacd.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::{self, BoundingVolume, AABB}; use crate::math::Isometry; use crate::num::{Bounded, Zero}; -use crate::partitioning::{BVH, BVT}; +use crate::partitioning::{VisitStatus, BVH, BVT}; use crate::procedural::{IndexBuffer, TriMesh}; use crate::query::algorithms::VoronoiSimplex; use crate::query::{ - self, visitors::BoundingVolumeInterferencesCollector, Ray, RayCast, RayIntersection, + self, visitors::BoundingVolumeInterferencesVisitor, Ray, RayCast, RayIntersection, }; use crate::shape::SupportMap; use crate::transformation; @@ -528,48 +528,45 @@ impl DualGraphEdge { // FIXME: (optimization) find a way to stop the cast if we exceed the max concavity. if !self.iray_cast && self.iv1 == a1.len() && self.iv2 == a2.len() { self.iray_cast = true; - let mut internal_rays = Vec::new(); - - { - let aabb = v1.aabb.merged(&v2.aabb); - let mut visitor = - BoundingVolumeInterferencesCollector::new(&aabb, &mut internal_rays); - - bvt.visit(&mut visitor); - } - let uancestors_v1 = v1.uancestors.as_ref().unwrap(); let uancestors_v2 = v2.uancestors.as_ref().unwrap(); - for ray_id in internal_rays.iter() { - let ray = &rays[*ray_id]; - - if !uancestors_v1.contains(ray_id) && !uancestors_v2.contains(ray_id) { - // XXX: we could just remove the zero-dir rays from the ancestors list! - if ray.dir.is_zero() { - // The ray was set to zero if it was invalid. - continue; + let aabb = v1.aabb.merged(&v2.aabb); + let mut visitor = { + BoundingVolumeInterferencesVisitor::new(&aabb, |ray_id| { + let ray = &rays[*ray_id]; + + if uancestors_v1.contains(ray_id) + || uancestors_v2.contains(ray_id) && + // XXX: we could just remove the zero-dir rays from the ancestors list! + // The ray was set to zero if it was invalid. + ray.dir.is_zero() + { + return VisitStatus::Continue; } // We determine if the point is inside of the convex hull or not. // XXX: use a point-in-implicit test instead of a ray-cast! - match chull.toi_with_ray(&Isometry::identity(), ray, N::max_value(), true) { - None => continue, - Some(inter) => { - if inter.is_zero() { - // ok, the point is inside, performe the real ray-cast. - cast_ray( - &chull, - ray, - *ray_id, - &mut self.concavity, - &mut self.ancestors, - ); - } + if let Some(inter) = + chull.toi_with_ray(&Isometry::identity(), ray, N::max_value(), true) + { + if inter.is_zero() { + // ok, the point is inside, performe the real ray-cast. + cast_ray( + &chull, + ray, + *ray_id, + &mut self.concavity, + &mut self.ancestors, + ); } } - } - } + + VisitStatus::Continue + }) + }; + + bvt.visit(&mut visitor); } self.mcost = -(self.concavity + max_concavity * self.shape);