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 c6968291..4a9cc776 100644 --- a/src/query/intersection_test/intersection_test_composite_shape_shape.rs +++ b/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( @@ -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, + msum_margin: Vector, + + dispatcher: &'a D, + pos12: &'a Isometry, + g1: &'a G1, + g2: &'a dyn Shape, +} + +impl<'a, D: ?Sized, G1: ?Sized> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> +where + D: QueryDispatcher, + G1: TypedSimdCompositeShape, +{ + /// Initialize a visitor for checking if a composite-shape and a shape intersect. + pub fn new( + dispatcher: &'a D, + pos12: &'a Isometry, + 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 + for IntersectionCompositeShapeShapeBestFirstVisitor<'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)); + + 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], + } + } +} diff --git a/src/query/intersection_test/mod.rs b/src/query/intersection_test/mod.rs index a3efc8de..fceb5383 100644 --- a/src/query/intersection_test/mod.rs +++ b/src/query/intersection_test/mod.rs @@ -6,9 +6,11 @@ pub use self::intersection_test_ball_point_query::{ intersection_test_ball_point_query, intersection_test_point_query_ball, }; #[cfg(feature = "std")] +// TODO: remove this once we get rid of IntersectionCompositeShapeShapeBestFirstVisitor +#[allow(deprecated)] 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::{