From 977ce5947b9ec544e249e557696a17c436621261 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 8 Mar 2023 11:48:22 +0100 Subject: [PATCH] Improve performance of intersection test on composite shapes --- ...intersection_test_composite_shape_shape.rs | 56 ++++++++----------- src/query/intersection_test/mod.rs | 2 +- 2 files changed, 23 insertions(+), 35 deletions(-) diff --git a/src/query/intersection_test/intersection_test_composite_shape_shape.rs b/src/query/intersection_test/intersection_test_composite_shape_shape.rs index c555ea9f..1b5969e6 100644 --- a/src/query/intersection_test/intersection_test_composite_shape_shape.rs +++ b/src/query/intersection_test/intersection_test_composite_shape_shape.rs @@ -1,6 +1,8 @@ use crate::bounding_volume::SimdAabb; use crate::math::{Isometry, Real, SimdReal, Vector, SIMD_WIDTH}; -use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; +use crate::partitioning::{ + SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdVisitStatus, SimdVisitor, +}; use crate::query::QueryDispatcher; use crate::shape::{Shape, TypedSimdCompositeShape}; use crate::utils::{DefaultStorage, IsometryOpt}; @@ -17,13 +19,10 @@ where D: QueryDispatcher, G1: TypedSimdCompositeShape, { - let mut visitor = - IntersectionCompositeShapeShapeBestFirstVisitor::new(dispatcher, pos12, g1, g2); + let mut visitor = IntersectionCompositeShapeShapeVisitor::new(dispatcher, pos12, g1, g2); - g1.typed_qbvh() - .traverse_best_first(&mut visitor) - .map(|e| e.1 .1) - .unwrap_or(false) + let _ = g1.typed_qbvh().traverse_depth_first(&mut visitor); + visitor.found_intersection } /// Proximity between a shape and a composite (`Mesh`, `Compound`) shape. @@ -41,17 +40,18 @@ where } /// A visitor for checking if a composite-shape and a shape intersect. -pub struct IntersectionCompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?Sized + 'a> { - msum_shift: Vector, - msum_margin: Vector, +pub struct IntersectionCompositeShapeShapeVisitor<'a, D: ?Sized, G1: ?Sized + 'a> { + ls_aabb2: SimdAabb, dispatcher: &'a D, pos12: &'a Isometry, g1: &'a G1, g2: &'a dyn Shape, + + found_intersection: bool, } -impl<'a, D: ?Sized, G1: ?Sized> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> +impl<'a, D: ?Sized, G1: ?Sized> IntersectionCompositeShapeShapeVisitor<'a, D, G1> where D: QueryDispatcher, G1: TypedSimdCompositeShape, @@ -62,41 +62,32 @@ where pos12: &'a Isometry, g1: &'a G1, g2: &'a dyn Shape, - ) -> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> { + ) -> IntersectionCompositeShapeShapeVisitor<'a, D, G1> { let ls_aabb2 = g2.compute_aabb(&pos12); - IntersectionCompositeShapeShapeBestFirstVisitor { + IntersectionCompositeShapeShapeVisitor { dispatcher, - msum_shift: Vector::splat(-ls_aabb2.center().coords), - msum_margin: Vector::splat(ls_aabb2.half_extents()), + ls_aabb2: SimdAabb::splat(ls_aabb2), pos12, g1, g2, + found_intersection: false, } } } -impl<'a, D: ?Sized, G1: ?Sized> SimdBestFirstVisitor - for IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> +impl<'a, D: ?Sized, G1: ?Sized> SimdVisitor + for IntersectionCompositeShapeShapeVisitor<'a, D, G1> where D: QueryDispatcher, G1: TypedSimdCompositeShape, { - type Result = (G1::PartId, bool); - fn visit( &mut self, - best: Real, bv: &SimdAabb, data: Option<[Option<&G1::PartId>; SIMD_WIDTH]>, - ) -> SimdBestFirstVisitStatus { - // Compute the minkowski sum of the two Aabbs. - let msum = SimdAabb { - mins: bv.mins + self.msum_shift + (-self.msum_margin), - maxs: bv.maxs + self.msum_shift + self.msum_margin, - }; - let dist = msum.distance_to_origin(); - let mask = dist.simd_lt(SimdReal::splat(best)); + ) -> SimdVisitStatus { + let mask = self.ls_aabb2.intersects(bv); if let Some(data) = data { let bitmask = mask.bitmask(); @@ -114,16 +105,13 @@ where }); if found_intersection { - return SimdBestFirstVisitStatus::ExitEarly(Some((part_id, true))); + self.found_intersection = true; + return SimdVisitStatus::ExitEarly; } } } } - SimdBestFirstVisitStatus::MaybeContinue { - weights: dist, - mask, - results: [None; SIMD_WIDTH], - } + SimdVisitStatus::MaybeContinue(mask) } } diff --git a/src/query/intersection_test/mod.rs b/src/query/intersection_test/mod.rs index 610da44d..a3efc8de 100644 --- a/src/query/intersection_test/mod.rs +++ b/src/query/intersection_test/mod.rs @@ -8,7 +8,7 @@ pub use self::intersection_test_ball_point_query::{ #[cfg(feature = "std")] pub use self::intersection_test_composite_shape_shape::{ intersection_test_composite_shape_shape, intersection_test_shape_composite_shape, - IntersectionCompositeShapeShapeBestFirstVisitor, + IntersectionCompositeShapeShapeVisitor, }; pub use self::intersection_test_cuboid_cuboid::intersection_test_cuboid_cuboid; pub use self::intersection_test_cuboid_segment::{