diff options
| author | Crozet Sébastien <developer@crozet.re> | 2020-12-17 13:23:00 +0100 |
|---|---|---|
| committer | Crozet Sébastien <developer@crozet.re> | 2020-12-29 11:31:59 +0100 |
| commit | 29717c2887b2db39faf9c25053730b661dc5da2b (patch) | |
| tree | d7ec5abf85af4b3519ead56891dda23e02c08323 /src/geometry/proximity_detector | |
| parent | e231bacec608fa5efd24f7a876572927dbd6c9c4 (diff) | |
| download | rapier-29717c2887b2db39faf9c25053730b661dc5da2b.tar.gz rapier-29717c2887b2db39faf9c25053730b661dc5da2b.tar.bz2 rapier-29717c2887b2db39faf9c25053730b661dc5da2b.zip | |
Externalize the proximity code (renamed intersection).
Diffstat (limited to 'src/geometry/proximity_detector')
12 files changed, 0 insertions, 850 deletions
diff --git a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs deleted file mode 100644 index 65c141c..0000000 --- a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs +++ /dev/null @@ -1,68 +0,0 @@ -use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; - -use crate::geometry::Proximity; -use crate::math::Point; -#[cfg(feature = "simd-is-enabled")] -use { - crate::geometry::{proximity_detector::PrimitiveProximityDetectionContextSimd, WBall}, - crate::math::{SimdReal, SIMD_WIDTH}, - simba::simd::SimdValue, -}; - -#[cfg(feature = "simd-is-enabled")] -fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdReal { - let dcenter = ball2.center - ball1.center; - let center_dist = dcenter.magnitude(); - center_dist - ball1.radius - ball2.radius -} - -#[cfg(feature = "simd-is-enabled")] -pub fn detect_proximity_ball_ball_simd( - ctxt: &mut PrimitiveProximityDetectionContextSimd, -) -> [Proximity; SIMD_WIDTH] { - let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; - let radii_a = - SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); - let radii_b = - SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); - - let wball_a = WBall::new(Point::origin(), radii_a); - let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b); - let distances = ball_distance_simd(&wball_a, &wball_b); - let mut proximities = [Proximity::Disjoint; SIMD_WIDTH]; - - for i in 0..SIMD_WIDTH { - // FIXME: compare the dist before computing the proximity. - let dist = distances.extract(i); - if dist > ctxt.prediction_distance { - proximities[i] = Proximity::Disjoint; - } else if dist > 0.0 { - proximities[i] = Proximity::WithinMargin; - } else { - proximities[i] = Proximity::Intersecting - } - } - - proximities -} - -pub fn detect_proximity_ball_ball(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - let pos_ba = ctxt.position2.inverse() * ctxt.position1; - let radius_a = ctxt.shape1.as_ball().unwrap().radius; - let radius_b = ctxt.shape2.as_ball().unwrap().radius; - - let center_a = Point::origin(); - let center_b = pos_ba.inverse_transform_point(&Point::origin()); - - let dcenter = center_b - center_a; - let center_dist = dcenter.magnitude(); - let dist = center_dist - radius_a - radius_b; - - if dist > ctxt.prediction_distance { - Proximity::Disjoint - } else if dist > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } -} diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs deleted file mode 100644 index 19ead5c..0000000 --- a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs +++ /dev/null @@ -1,42 +0,0 @@ -use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{Ball, Proximity}; -use crate::math::Isometry; -use eagl::query::PointQuery; - -pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let Some(ball1) = ctxt.shape1.as_ball() { - do_detect_proximity(ctxt.shape2, ball1, &ctxt) - } else if let Some(ball2) = ctxt.shape2.as_ball() { - do_detect_proximity(ctxt.shape1, ball2, &ctxt) - } else { - panic!("Invalid shape types provide.") - } -} - -fn do_detect_proximity<P: ?Sized + PointQuery>( - point_query1: &P, - ball2: &Ball, - ctxt: &PrimitiveProximityDetectionContext, -) -> Proximity { - let local_p2_1 = ctxt - .position1 - .inverse_transform_point(&ctxt.position2.translation.vector.into()); - - let proj = point_query1.project_local_point(&local_p2_1, cfg!(feature = "dim3")); - let dpos = local_p2_1 - proj.local_point; - let dist = dpos.norm(); - - if proj.is_inside { - return Proximity::Intersecting; - } - - if dist <= ball2.radius + ctxt.prediction_distance { - if dist <= ball2.radius { - Proximity::Intersecting - } else { - Proximity::WithinMargin - } - } else { - Proximity::Disjoint - } -} diff --git a/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs b/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs deleted file mode 100644 index 8b13789..0000000 --- a/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs +++ /dev/null @@ -1 +0,0 @@ - diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs deleted file mode 100644 index b43f53d..0000000 --- a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs +++ /dev/null @@ -1,78 +0,0 @@ -use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::Proximity; -use crate::math::Isometry; -use eagl::query::sat; -use eagl::shape::Cuboid; - -pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { - detect_proximity( - ctxt.prediction_distance, - cube1, - ctxt.position1, - cube2, - ctxt.position2, - ) - } else { - unreachable!() - } -} - -pub fn detect_proximity<'a>( - prediction_distance: f32, - cube1: &'a Cuboid, - pos1: &'a Isometry<f32>, - cube2: &'a Cuboid, - pos2: &'a Isometry<f32>, -) -> Proximity { - let pos12 = pos1.inverse() * pos2; - let pos21 = pos12.inverse(); - - /* - * - * Point-Face - * - */ - let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12).0; - if sep1 > prediction_distance { - return Proximity::Disjoint; - } - - let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21).0; - if sep2 > prediction_distance { - return Proximity::Disjoint; - } - - /* - * - * Edge-Edge cases - * - */ - #[cfg(feature = "dim2")] - let sep3 = -f32::MAX; // This case does not exist in 2D. - #[cfg(feature = "dim3")] - let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12).0; - if sep3 > prediction_distance { - return Proximity::Disjoint; - } - - if sep2 > sep1 && sep2 > sep3 { - if sep2 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } else if sep3 > sep1 { - if sep3 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } else { - if sep1 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } -} diff --git a/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs deleted file mode 100644 index 8b13789..0000000 --- a/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs +++ /dev/null @@ -1 +0,0 @@ - diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs deleted file mode 100644 index 5ac12a8..0000000 --- a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs +++ /dev/null @@ -1,90 +0,0 @@ -use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{Cuboid, Proximity, Triangle}; -use crate::math::Isometry; -use eagl::query::sat; - -pub fn detect_proximity_cuboid_triangle( - ctxt: &mut PrimitiveProximityDetectionContext, -) -> Proximity { - if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { - detect_proximity( - ctxt.prediction_distance, - cube1, - ctxt.position1, - triangle2, - ctxt.position2, - ) - } else if let (Some(triangle1), Some(cube2)) = - (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) - { - detect_proximity( - ctxt.prediction_distance, - cube2, - ctxt.position2, - triangle1, - ctxt.position1, - ) - } else { - panic!("Invalid shape types") - } -} - -pub fn detect_proximity<'a>( - prediction_distance: f32, - cube1: &'a Cuboid, - pos1: &'a Isometry<f32>, - triangle2: &'a Triangle, - pos2: &'a Isometry<f32>, -) -> Proximity { - let pos12 = pos1.inverse() * pos2; - let pos21 = pos12.inverse(); - - /* - * - * Point-Face cases. - * - */ - let sep1 = - sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12).0; - if sep1 > prediction_distance { - return Proximity::Disjoint; - } - - let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cube1, &pos21).0; - if sep2 > prediction_distance { - return Proximity::Disjoint; - } - - /* - * - * Edge-Edge cases. - * - */ - #[cfg(feature = "dim2")] - let sep3 = -f32::MAX; // This case does not exist in 2D. - #[cfg(feature = "dim3")] - let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12).0; - if sep3 > prediction_distance { - return Proximity::Disjoint; - } - - if sep2 > sep1 && sep2 > sep3 { - if sep2 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } else if sep3 > sep1 { - if sep3 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } else { - if sep1 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } -} diff --git a/src/geometry/proximity_detector/mod.rs b/src/geometry/proximity_detector/mod.rs deleted file mode 100644 index fc904da..0000000 --- a/src/geometry/proximity_detector/mod.rs +++ /dev/null @@ -1,30 +0,0 @@ -pub use self::ball_ball_proximity_detector::detect_proximity_ball_ball; -#[cfg(feature = "simd-is-enabled")] -pub use self::ball_ball_proximity_detector::detect_proximity_ball_ball_simd; -pub use self::ball_convex_proximity_detector::detect_proximity_ball_convex; -pub use self::cuboid_cuboid_proximity_detector::detect_proximity_cuboid_cuboid; -pub use self::cuboid_triangle_proximity_detector::detect_proximity_cuboid_triangle; -pub use self::polygon_polygon_proximity_detector::detect_proximity_polygon_polygon; -pub use self::proximity_detector::{ - PrimitiveProximityDetectionContext, PrimitiveProximityDetector, ProximityDetectionContext, - ProximityDetector, ProximityPhase, -}; -#[cfg(feature = "simd-is-enabled")] -pub use self::proximity_detector::{ - PrimitiveProximityDetectionContextSimd, ProximityDetectionContextSimd, -}; -pub use self::proximity_dispatcher::{DefaultProximityDispatcher, ProximityDispatcher}; -pub use self::trimesh_shape_proximity_detector::{ - detect_proximity_trimesh_shape, TriMeshShapeProximityDetectorWorkspace, -}; - -mod ball_ball_proximity_detector; -mod ball_convex_proximity_detector; -mod ball_polygon_proximity_detector; -mod cuboid_cuboid_proximity_detector; -mod cuboid_polygon_proximity_detector; -mod cuboid_triangle_proximity_detector; -mod polygon_polygon_proximity_detector; -mod proximity_detector; -mod proximity_dispatcher; -mod trimesh_shape_proximity_detector; diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs deleted file mode 100644 index 12a8e45..0000000 --- a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs +++ /dev/null @@ -1,57 +0,0 @@ -#![allow(dead_code)] - -use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Polygon, Proximity}; -use crate::math::Isometry; - -pub fn detect_proximity_polygon_polygon( - _ctxt: &mut PrimitiveProximityDetectionContext, -) -> Proximity { - unimplemented!() - // if let (Some(polygon1), Some(polygon2)) = (ctxt.shape1.as_polygon(), ctxt.shape2.as_polygon()) { - // detect_proximity( - // ctxt.prediction_distance, - // polygon1, - // &ctxt.position1, - // polygon2, - // &ctxt.position2, - // ) - // } else { - // unreachable!() - // } -} - -fn detect_proximity<'a>( - prediction_distance: f32, - p1: &'a Polygon, - m1: &'a Isometry<f32>, - p2: &'a Polygon, - m2: &'a Isometry<f32>, -) -> Proximity { - let m12 = m1.inverse() * m2; - let m21 = m12.inverse(); - - let sep1 = sat::polygon_polygon_compute_separation_features(p1, p2, &m12); - if sep1.0 > prediction_distance { - return Proximity::Disjoint; - } - - let sep2 = sat::polygon_polygon_compute_separation_features(p2, p1, &m21); - if sep2.0 > prediction_distance { - return Proximity::Disjoint; - } - - if sep2.0 > sep1.0 { - if sep2.0 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } else { - if sep1.0 > 0.0 { - Proximity::WithinMargin - } else { - Proximity::Intersecting - } - } -} diff --git a/src/geometry/proximity_detector/proximity_detector.rs b/src/geometry/proximity_detector/proximity_detector.rs deleted file mode 100644 index ea362de..0000000 --- a/src/geometry/proximity_detector/proximity_detector.rs +++ /dev/null @@ -1,212 +0,0 @@ -use crate::geometry::{ - Collider, ColliderSet, Proximity, ProximityDispatcher, ProximityEvent, ProximityPair, Shape, -}; -use crate::math::Isometry; -#[cfg(feature = "simd-is-enabled")] -use crate::math::{SimdReal, SIMD_WIDTH}; -use crate::pipeline::EventHandler; -use std::any::Any; - -#[derive(Copy, Clone)] -pub enum ProximityPhase { - NearPhase(ProximityDetector), - ExactPhase(PrimitiveProximityDetector), -} - -impl ProximityPhase { - #[inline] - pub fn detect_proximity( - self, - mut context: ProximityDetectionContext, - events: &dyn EventHandler, - ) { - let proximity = match self { - Self::NearPhase(gen) => (gen.detect_proximity)(&mut context), - Self::ExactPhase(gen) => { - // Build the primitive context from the non-primitive context and dispatch. - let collider1 = &context.colliders[context.pair.pair.collider1]; - let collider2 = &context.colliders[context.pair.pair.collider2]; - - let mut context2 = PrimitiveProximityDetectionContext { - prediction_distance: context.prediction_distance, - collider1, - collider2, - shape1: collider1.shape(), - shape2: collider2.shape(), - position1: collider1.position(), - position2: collider2.position(), - workspace: context.pair.detector_workspace.as_mut().map(|w| &mut **w), - }; - - (gen.detect_proximity)(&mut context2) - } - }; - - if context.pair.proximity != proximity { - events.handle_proximity_event(ProximityEvent::new( - context.pair.pair.collider1, - context.pair.pair.collider2, - context.pair.proximity, - proximity, - )) - } - - context.pair.proximity = proximity; - } - - #[cfg(feature = "simd-is-enabled")] - #[inline] - pub fn detect_proximity_simd( - self, - mut context: ProximityDetectionContextSimd, - events: &dyn EventHandler, - ) { - let proximities = match self { - Self::NearPhase(gen) => (gen.detect_proximity_simd)(&mut context), - Self::ExactPhase(gen) => { - // Build the primitive context from the non-primitive context and dispatch. - use arrayvec::ArrayVec; - let mut colliders_arr: ArrayVec<[(&Collider, &Collider); SIMD_WIDTH]> = - ArrayVec::new(); - let mut workspace_arr: ArrayVec< - [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH], - > = ArrayVec::new(); - - for pair in context.pairs.iter_mut() { - let collider1 = &context.colliders[pair.pair.collider1]; - let collider2 = &context.colliders[pair.pair.collider2]; - colliders_arr.push((collider1, collider2)); - workspace_arr.push(pair.detector_workspace.as_mut().map(|w| &mut **w)); - } - - let max_index = colliders_arr.len() - 1; - let colliders1 = array![|ii| colliders_arr[ii.min(max_index)].0; SIMD_WIDTH]; - let colliders2 = array![|ii| colliders_arr[ii.min(max_index)].1; SIMD_WIDTH]; - - let mut context2 = PrimitiveProximityDetectionContextSimd { - prediction_distance: context.prediction_distance, - colliders1, - colliders2, - shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH], - shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH], - positions1: &Isometry::from( - array![|ii| *colliders1[ii].position(); SIMD_WIDTH], - ), - positions2: &Isometry::from( - array![|ii| *colliders2[ii].position(); SIMD_WIDTH], - ), - workspaces: workspace_arr.as_mut_slice(), - }; - - (gen.detect_proximity_simd)(&mut context2) - } - }; - - for (i, pair) in context.pairs.iter_mut().enumerate() { - if pair.proximity != proximities[i] { - events.handle_proximity_event(ProximityEvent::new( - pair.pair.collider1, - pair.pair.collider2, - pair.proximity, - proximities[i], - )) - } - pair.proximity = proximities[i]; - } - } -} - -pub struct PrimitiveProximityDetectionContext<'a> { - pub prediction_distance: f32, - pub collider1: &'a Collider, - pub collider2: &'a Collider, - pub shape1: &'a dyn Shape, - pub shape2: &'a dyn Shape, - pub position1: &'a Isometry<f32>, - pub position2: &'a Isometry<f32>, - pub workspace: Option<&'a mut (dyn Any + Send + Sync)>, -} - -#[cfg(feature = "simd-is-enabled")] -pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> { - pub prediction_distance: f32, - pub colliders1: [&'a Collider; SIMD_WIDTH], - pub colliders2: [&'a Collider; SIMD_WIDTH], - pub shapes1: [&'a dyn Shape; SIMD_WIDTH], - pub shapes2: [&'a dyn Shape; SIMD_WIDTH], - pub positions1: &'a Isometry<SimdReal>, - pub positions2: &'a Isometry<SimdReal>, - pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>], -} - -#[derive(Copy, Clone)] -pub struct PrimitiveProximityDetector { - pub detect_proximity: fn(&mut PrimitiveProximityDetectionContext) -> Proximity, - #[cfg(feature = "simd-is-enabled")] - pub detect_proximity_simd: - fn(&mut PrimitiveProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH], -} - -impl PrimitiveProximityDetector { - fn unimplemented_fn(_ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - Proximity::Disjoint - } - #[cfg(feature = "simd-is-enabled")] - fn unimplemented_simd_fn( - _ctxt: &mut PrimitiveProximityDetectionContextSimd, - ) -> [Proximity; SIMD_WIDTH] { - [Proximity::Disjoint; SIMD_WIDTH] - } -} - -impl Default for PrimitiveProximityDetector { - fn default() -> Self { - Self { - detect_proximity: Self::unimplemented_fn, - #[cfg(feature = "simd-is-enabled")] - detect_proximity_simd: Self::unimplemented_simd_fn, - } - } -} - -pub struct ProximityDetectionContext<'a> { - pub dispatcher: &'a dyn ProximityDispatcher, - pub prediction_distance: f32, - pub colliders: &'a ColliderSet, - pub pair: &'a mut ProximityPair, -} - -#[cfg(feature = "simd-is-enabled")] -pub struct ProximityDetectionContextSimd<'a, 'b> { - pub dispatcher: &'a dyn ProximityDispatcher, - pub prediction_distance: f32, - pub colliders: &'a ColliderSet, - pub pairs: &'a mut [&'b mut ProximityPair], -} - -#[derive(Copy, Clone)] -pub struct ProximityDetector { - pub detect_proximity: fn(&mut ProximityDetectionContext) -> Proximity, - #[cfg(feature = "simd-is-enabled")] - pub detect_proximity_simd: fn(&mut ProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH], -} - -impl ProximityDetector { - fn unimplemented_fn(_ctxt: &mut ProximityDetectionContext) -> Proximity { - Proximity::Disjoint - } - #[cfg(feature = "simd-is-enabled")] - fn unimplemented_simd_fn(_ctxt: &mut ProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH] { - [Proximity::Disjoint; SIMD_WIDTH] - } -} - -impl Default for ProximityDetector { - fn default() -> Self { - Self { - detect_proximity: Self::unimplemented_fn, - #[cfg(feature = "simd-is-enabled")] - detect_proximity_simd: Self::unimplemented_simd_fn, - } - } -} diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs deleted file mode 100644 index 709521e..0000000 --- a/src/geometry/proximity_detector/proximity_dispatcher.rs +++ /dev/null @@ -1,136 +0,0 @@ -use crate::geometry::proximity_detector::{ - PrimitiveProximityDetector, ProximityDetector, ProximityPhase, - TriMeshShapeProximityDetectorWorkspace, -}; -use crate::geometry::ShapeType; -use std::any::Any; - -/// Trait implemented by structures responsible for selecting a collision-detection algorithm -/// for a given pair of shapes. -pub trait ProximityDispatcher { - /// Select the proximity detection algorithm for the given pair of primitive shapes. - fn dispatch_primitives( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> ( - PrimitiveProximityDetector, - Option<Box<dyn Any + Send + Sync>>, - ); - /// Select the proximity detection algorithm for the given pair of non-primitive shapes. - fn dispatch( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>); -} - -/// The default proximity dispatcher used by Rapier. -pub struct DefaultProximityDispatcher; - -impl ProximityDispatcher for DefaultProximityDispatcher { - fn dispatch_primitives( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> ( - PrimitiveProximityDetector, - Option<Box<dyn Any + Send + Sync>>, - ) { - match (shape1, shape2) { - (ShapeType::Ball, ShapeType::Ball) => ( - PrimitiveProximityDetector { - #[cfg(feature = "simd-is-enabled")] - detect_proximity_simd: super::detect_proximity_ball_ball_simd, - detect_proximity: super::detect_proximity_ball_ball, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Cuboid, ShapeType::Cuboid) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_cuboid_cuboid, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Polygon, ShapeType::Polygon) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_polygon_polygon, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Triangle, ShapeType::Ball) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_ball_convex, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Ball, ShapeType::Triangle) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_ball_convex, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Cuboid, ShapeType::Ball) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_ball_convex, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Ball, ShapeType::Cuboid) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_ball_convex, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Triangle, ShapeType::Cuboid) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_cuboid_triangle, - ..PrimitiveProximityDetector::default() - }, - None, - ), - (ShapeType::Cuboid, ShapeType::Triangle) => ( - PrimitiveProximityDetector { - detect_proximity: super::detect_proximity_cuboid_triangle, - ..PrimitiveProximityDetector::default() - }, - None, - ), - _ => (PrimitiveProximityDetector::default(), None), - } - } - - fn dispatch( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>) { - match (shape1, shape2) { - (ShapeType::TriMesh, _) => ( - ProximityPhase::NearPhase(ProximityDetector { - detect_proximity: super::detect_proximity_trimesh_shape, - ..ProximityDetector::default() - }), - Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())), - ), - (_, ShapeType::TriMesh) => ( - ProximityPhase::NearPhase(ProximityDetector { - detect_proximity: super::detect_proximity_trimesh_shape, - ..ProximityDetector::default() - }), - Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())), - ), - _ => { - let (gen, workspace) = self.dispatch_primitives(shape1, shape2); - (ProximityPhase::ExactPhase(gen), workspace) - } - } - } -} diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs deleted file mode 100644 index 961de3b..0000000 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ /dev/null @@ -1,135 +0,0 @@ -use crate::eagl::bounding_volume::{BoundingVolume, AABB}; -use crate::geometry::proximity_detector::{ - PrimitiveProximityDetectionContext, ProximityDetectionContext, -}; -use crate::geometry::{Collider, Proximity, ShapeType, TriMesh}; - -pub struct TriMeshShapeProximityDetectorWorkspace { - interferences: Vec<u32>, - local_aabb2: AABB, - old_interferences: Vec<u32>, -} - -impl TriMeshShapeProximityDetectorWorkspace { - pub fn new() -> Self { - Self { - interferences: Vec::new(), - local_aabb2: AABB::new_invalid(), - old_interferences: Vec::new(), - } - } -} - -pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> Proximity { - let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; - let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - - if let Some(trimesh1) = collider1.shape().as_trimesh() { - do_detect_proximity(trimesh1, collider1, collider2, ctxt) - } else if let Some(trimesh2) = collider2.shape().as_trimesh() { - do_detect_proximity(trimesh2, collider2, collider1, ctxt) - } else { - panic!("Invalid shape types provided.") - } -} - -fn do_detect_proximity( - trimesh1: &TriMesh, - collider1: &Collider, - collider2: &Collider, - ctxt: &mut ProximityDetectionContext, -) -> Proximity { - let workspace: &mut TriMeshShapeProximityDetectorWorkspace = ctxt - .pair - .detector_workspace - .as_mut() - .expect("The TriMeshShapeProximityDetectorWorkspace is missing.") - .downcast_mut() - .expect("Invalid workspace type, expected a TriMeshShapeProximityDetectorWorkspace."); - - /* - * Compute interferences. - */ - let pos12 = collider1.position.inverse() * collider2.position; - // TODO: somehow precompute the AABB and reuse it? - let mut new_local_aabb2 = collider2 - .shape() - .compute_aabb(&pos12) - .loosened(ctxt.prediction_distance); - let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2); - - if !same_local_aabb2 { - let extra_margin = - (new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1)); - new_local_aabb2.mins -= extra_margin; - new_local_aabb2.maxs += extra_margin; - - let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? - std::mem::swap( - &mut workspace.old_interferences, - &mut workspace.interferences, - ); - - workspace.interferences.clear(); - trimesh1 - .quadtree() - .intersect_aabb(&local_aabb2, &mut workspace.interferences); - workspace.local_aabb2 = local_aabb2; - } - - /* - * Dispatch to the specific solver by keeping the previous manifold if we already had one. - */ - let new_interferences = &workspace.interferences; - let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); - let mut best_proximity = Proximity::Disjoint; - let shape_type2 = collider2.shape().shape_type(); - - for triangle_id in new_interferences.iter() { - if *triangle_id >= trimesh1.num_triangles() as u32 { - // Because of SIMD padding, the broad-phase may return triangle indices greater - // than the max. - continue; - } - |
