Re-add IntersectionCompositeShapeShapeBestFirstVisitor

src/query/intersection_test/intersection_test_composite_shape_shape.rs

@@ -1,10 +1,14 @@
+#![allow(deprecated)] // Silence warning until we actually remove IntersectionCompositeShapeShapeBestFirstVisitor
+
 use crate::bounding_volume::SimdAabb;
-use crate::math::{Isometry, Real, SIMD_WIDTH};
-use crate::partitioning::{SimdVisitStatus, SimdVisitor};
+use crate::math::{Isometry, Real, SimdReal, Vector, SIMD_WIDTH};
+use crate::partitioning::{
+    SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdVisitStatus, SimdVisitor,
+};
 use crate::query::QueryDispatcher;
 use crate::shape::{Shape, TypedSimdCompositeShape};
 use crate::utils::{DefaultStorage, IsometryOpt};
-use simba::simd::SimdBool as _;
+use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
 
 /// Intersection test between a composite shape (`Mesh`, `Compound`) and any other shape.
 pub fn intersection_test_composite_shape_shape<D: ?Sized, G1: ?Sized>(
@@ -113,3 +117,92 @@ where
         SimdVisitStatus::MaybeContinue(mask)
     }
 }
+
+/// A visitor for checking if a composite-shape and a shape intersect.
+#[deprecated(note = "Use IntersectionCompositeShapeShapeVisitor instead.")]
+pub struct IntersectionCompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?Sized + 'a> {
+    msum_shift: Vector<SimdReal>,
+    msum_margin: Vector<SimdReal>,
+
+    dispatcher: &'a D,
+    pos12: &'a Isometry<Real>,
+    g1: &'a G1,
+    g2: &'a dyn Shape,
+}
+
+impl<'a, D: ?Sized, G1: ?Sized> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1>
+where
+    D: QueryDispatcher,
+    G1: TypedSimdCompositeShape<QbvhStorage = DefaultStorage>,
+{
+    /// Initialize a visitor for checking if a composite-shape and a shape intersect.
+    pub fn new(
+        dispatcher: &'a D,
+        pos12: &'a Isometry<Real>,
+        g1: &'a G1,
+        g2: &'a dyn Shape,
+    ) -> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> {
+        let ls_aabb2 = g2.compute_aabb(&pos12);
+
+        IntersectionCompositeShapeShapeBestFirstVisitor {
+            dispatcher,
+            msum_shift: Vector::splat(-ls_aabb2.center().coords),
+            msum_margin: Vector::splat(ls_aabb2.half_extents()),
+            pos12,
+            g1,
+            g2,
+        }
+    }
+}
+
+impl<'a, D: ?Sized, G1: ?Sized> SimdBestFirstVisitor<G1::PartId, SimdAabb>
+    for IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1>
+where
+    D: QueryDispatcher,
+    G1: TypedSimdCompositeShape<QbvhStorage = DefaultStorage>,
+{
+    type Result = (G1::PartId, bool);
+
+    fn visit(
+        &mut self,
+        best: Real,
+        bv: &SimdAabb,
+        data: Option<[Option<&G1::PartId>; SIMD_WIDTH]>,
+    ) -> SimdBestFirstVisitStatus<Self::Result> {
+        // 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));
+
+        if let Some(data) = data {
+            let bitmask = mask.bitmask();
+            let mut found_intersection = false;
+
+            for ii in 0..SIMD_WIDTH {
+                if (bitmask & (1 << ii)) != 0 && data[ii].is_some() {
+                    let part_id = *data[ii].unwrap();
+                    self.g1.map_untyped_part_at(part_id, |part_pos1, g1| {
+                        found_intersection = self.dispatcher.intersection_test(
+                            &part_pos1.inv_mul(self.pos12),
+                            g1,
+                            self.g2,
+                        ) == Ok(true);
+                    });
+
+                    if found_intersection {
+                        return SimdBestFirstVisitStatus::ExitEarly(Some((part_id, true)));
+                    }
+                }
+            }
+        }
+
+        SimdBestFirstVisitStatus::MaybeContinue {
+            weights: dist,
+            mask,
+            results: [None; SIMD_WIDTH],
+        }
+    }
+}

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,
-    IntersectionCompositeShapeShapeVisitor,
+    IntersectionCompositeShapeShapeBestFirstVisitor, IntersectionCompositeShapeShapeVisitor,
 };
 pub use self::intersection_test_cuboid_cuboid::intersection_test_cuboid_cuboid;
 pub use self::intersection_test_cuboid_segment::{