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 | |
| parent | e231bacec608fa5efd24f7a876572927dbd6c9c4 (diff) | |
| download | rapier-29717c2887b2db39faf9c25053730b661dc5da2b.tar.gz rapier-29717c2887b2db39faf9c25053730b661dc5da2b.tar.bz2 rapier-29717c2887b2db39faf9c25053730b661dc5da2b.zip | |
Externalize the proximity code (renamed intersection).
22 files changed, 144 insertions, 1051 deletions
diff --git a/examples3d/sensor3.rs b/examples3d/sensor3.rs index 7b218fe..234eaf3 100644 --- a/examples3d/sensor3.rs +++ b/examples3d/sensor3.rs @@ -1,6 +1,6 @@ use na::Point3; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; -use rapier3d::geometry::{ColliderBuilder, ColliderSet, Proximity}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { @@ -74,10 +74,11 @@ pub fn init_world(testbed: &mut Testbed) { // Callback that will be executed on the main loop to handle proximities. testbed.add_callback(move |_, physics, events, graphics, _| { - while let Ok(prox) = events.proximity_events.try_recv() { - let color = match prox.new_status { - Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0), - Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0), + while let Ok(prox) = events.intersection_events.try_recv() { + let color = if prox.intersecting { + Point3::new(1.0, 1.0, 0.0) + } else { + Point3::new(0.5, 0.5, 1.0) }; let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent(); diff --git a/src/geometry/interaction_graph.rs b/src/geometry/interaction_graph.rs index cae8095..54a19ce 100644 --- a/src/geometry/interaction_graph.rs +++ b/src/geometry/interaction_graph.rs @@ -79,6 +79,19 @@ impl<T> InteractionGraph<T> { self.graph.raw_edges().iter().map(move |edge| &edge.weight) } + /// All the interactions on this graph with the corresponding endpoint weights. + pub fn interactions_with_endpoints( + &self, + ) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> { + self.graph.raw_edges().iter().map(move |edge| { + ( + self.graph.raw_nodes()[edge.source().index()].weight, + self.graph.raw_nodes()[edge.target().index()].weight, + &edge.weight, + ) + }) + } + /// The interaction between the two collision objects identified by their graph index. pub fn interaction_pair( &self, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 9f32a7f..efb0cd4 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -11,10 +11,7 @@ pub use self::interaction_graph::{ }; pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; -pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; -pub use self::proximity_pair::ProximityPair; pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; -pub use eagl::query::Proximity; pub use eagl::query::{KinematicsCategory, TrackedContact}; @@ -62,36 +59,24 @@ pub enum ContactEvent { #[derive(Copy, Clone, Debug)] /// Events occurring when two collision objects start or stop being in close proximity, contact, or disjoint. -pub struct ProximityEvent { +pub struct IntersectionEvent { /// The first collider to which the proximity event applies. pub collider1: ColliderHandle, /// The second collider to which the proximity event applies. pub collider2: ColliderHandle, - /// The previous state of proximity between the two collision objects. - pub prev_status: Proximity, - /// The new state of proximity between the two collision objects. - pub new_status: Proximity, + /// Are the two colliders intersecting? + pub intersecting: bool, } -impl ProximityEvent { +impl IntersectionEvent { /// Instantiates a new proximity event. /// /// Panics if `prev_status` is equal to `new_status`. - pub fn new( - collider1: ColliderHandle, - collider2: ColliderHandle, - prev_status: Proximity, - new_status: Proximity, - ) -> Self { - assert_ne!( - prev_status, new_status, - "The previous and new status of a proximity event must not be the same." - ); + pub fn new(collider1: ColliderHandle, collider2: ColliderHandle, intersecting: bool) -> Self { Self { collider1, collider2, - prev_status, - new_status, + intersecting, } } } @@ -117,8 +102,6 @@ mod contact_pair; mod interaction_graph; mod narrow_phase; mod polygon; -mod proximity_detector; -mod proximity_pair; pub(crate) mod sat; //mod z_order; mod interaction_groups; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 240ed31..d9a4f31 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -2,26 +2,22 @@ use rayon::prelude::*; use crate::dynamics::RigidBodySet; -use crate::geometry::proximity_detector::{ - DefaultProximityDispatcher, ProximityDetectionContext, ProximityDispatcher, -}; -use eagl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; +use eagl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher}; //#[cfg(feature = "simd-is-enabled")] //use crate::geometry::{ // contact_generator::ContactGenerationContextSimd, -// proximity_detector::ProximityDetectionContextSimd, WBall, +// intersection_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactManifoldData, - ContactPairFilter, PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, - RemovedCollider, SolverFlags, + ContactPairFilter, IntersectionEvent, PairFilterContext, ProximityPairFilter, RemovedCollider, + SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] //use crate::math::{SimdReal, SIMD_WIDTH}; use crate::data::pubsub::Subscription; use crate::data::Coarena; -use crate::eagl::query::Proximity; use crate::pipeline::EventHandler; use std::collections::HashMap; //use simba::simd::SimdValue; @@ -30,14 +26,14 @@ use std::collections::HashMap; #[derive(Copy, Clone, Debug, PartialEq, Eq)] struct ColliderGraphIndices { contact_graph_index: ColliderGraphIndex, - proximity_graph_index: ColliderGraphIndex, + intersection_graph_index: ColliderGraphIndex, } impl ColliderGraphIndices { fn invalid() -> Self { Self { contact_graph_index: InteractionGraph::<ContactPair>::invalid_graph_index(), - proximity_graph_index: InteractionGraph::<ProximityPair>::invalid_graph_index(), + intersection_graph_index: InteractionGraph::<bool>::invalid_graph_index(), } } } @@ -47,13 +43,13 @@ impl ColliderGraphIndices { #[derive(Clone)] pub struct NarrowPhase { contact_graph: InteractionGraph<ContactPair>, - proximity_graph: InteractionGraph<ProximityPair>, + intersection_graph: InteractionGraph<bool>, graph_indices: Coarena<ColliderGraphIndices>, removed_colliders: Option<Subscription<RemovedCollider>>, // ball_ball: Vec<usize>, // Workspace: Vec<*mut ContactPair>, // shape_shape: Vec<usize>, // Workspace: Vec<*mut ContactPair>, - // ball_ball_prox: Vec<usize>, // Workspace: Vec<*mut ProximityPair>, - // shape_shape_prox: Vec<usize>, // Workspace: Vec<*mut ProximityPair>, + // ball_ball_prox: Vec<usize>, // Workspace: Vec<*mut bool>, + // shape_shape_prox: Vec<usize>, // Workspace: Vec<*mut bool>, } pub(crate) type ContactManifoldIndex = usize; @@ -63,7 +59,7 @@ impl NarrowPhase { pub fn new() -> Self { Self { contact_graph: InteractionGraph::new(), - proximity_graph: InteractionGraph::new(), + intersection_graph: InteractionGraph::new(), graph_indices: Coarena::new(), removed_colliders: None, // ball_ball: Vec::new(), @@ -78,9 +74,9 @@ impl NarrowPhase { &self.contact_graph } - /// The proximity graph containing all proximity pairs and their proximity information. - pub fn proximity_graph(&self) -> &InteractionGraph<ProximityPair> { - &self.proximity_graph + /// The intersection graph containing all intersection pairs and their intersection information. + pub fn intersection_graph(&self) -> &InteractionGraph<bool> { + &self.intersection_graph } /// All the contacts involving the given collider. @@ -92,15 +88,16 @@ impl NarrowPhase { Some(self.contact_graph.interactions_with(id.contact_graph_index)) } - /// All the proximities involving the given collider. - pub fn proximities_with( - &self, + /// All the intersections involving the given collider. + pub fn intersections_with<'a>( + &'a self, collider: ColliderHandle, - ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ProximityPair)>> { + ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, bool)> + 'a> { let id = self.graph_indices.get(collider)?; Some( - self.proximity_graph - .interactions_with(id.proximity_graph_index), + self.intersection_graph + .interactions_with(id.intersection_graph_index) + .map(|e| (e.0, e.1, *e.2)), ) } @@ -121,21 +118,20 @@ impl NarrowPhase { .map(|c| c.2) } - /// The proximity pair involving two specific colliders. + /// The intersection pair involving two specific colliders. /// - /// If this returns `None`, there is no intersection between the two colliders. - /// If this returns `Some`, then there may be an intersection between the two colliders. Check the - /// value of [`ProximityPair::proximity`] method to see if there is an actual intersection. - pub fn proximity_pair( + /// If this returns `None` or `Some(false)`, then there is no intersection between the two colliders. + /// If this returns `Some(true)`, then there may be an intersection between the two colliders. + pub fn intersection_pair( &self, collider1: ColliderHandle, collider2: ColliderHandle, - ) -> Option<&ProximityPair> { + ) -> Option<bool> { let id1 = self.graph_indices.get(collider1)?; let id2 = self.graph_indices.get(collider2)?; - self.proximity_graph - .interaction_pair(id1.proximity_graph_index, id2.proximity_graph_index) - .map(|c| c.2) + self.intersection_graph + .interaction_pair(id1.intersection_graph_index, id2.intersection_graph_index) + .map(|c| *c.2) } /// All the contact pairs maintained by this narrow-phase. @@ -143,9 +139,13 @@ impl NarrowPhase { self.contact_graph.interactions() } - /// All the proximity pairs maintained by this narrow-phase. - pub fn proximity_pairs(&self) -> impl Iterator<Item = &ProximityPair> { - self.proximity_graph.interactions() + /// All the intersection pairs maintained by this narrow-phase. + pub fn intersection_pairs<'a>( + &'a self, + ) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, bool)> + 'a { + self.intersection_graph + .interactions_with_endpoints() + .map(|e| (e.0, e.1, *e.2)) } // #[cfg(feature = "parallel")] @@ -164,7 +164,7 @@ impl NarrowPhase { // TODO: avoid these hash-maps. // They are necessary to handle the swap-remove done internally - // by the contact/proximity graphs when a node is removed. + // by the contact/intersection graphs when a node is removed. let mut prox_id_remap = HashMap::new(); let mut contact_id_remap = HashMap::new(); let mut i = 0; @@ -173,17 +173,17 @@ impl NarrowPhase { // NOTE: if the collider does not have any graph indices currently, there is nothing // to remove in the narrow-phase for this collider. if let Some(graph_idx) = self.graph_indices.get(collider.handle) { - let proximity_graph_id = prox_id_remap + let intersection_graph_id = prox_id_remap .get(&collider.handle) .copied() - .unwrap_or(graph_idx.proximity_graph_index); + .unwrap_or(graph_idx.intersection_graph_index); let contact_graph_id = contact_id_remap .get(&collider.handle) .copied() .unwrap_or(graph_idx.contact_graph_index); self.remove_collider( - proximity_graph_id, + intersection_graph_id, contact_graph_id, colliders, bodies, @@ -201,7 +201,7 @@ impl NarrowPhase { pub(crate) fn remove_collider<'a>( &mut self, - proximity_graph_id: ColliderGraphIndex, + intersection_graph_id: ColliderGraphIndex, contact_graph_id: ColliderGraphIndex, colliders: &mut ColliderSet, bodies: &mut RigidBodySet, @@ -221,11 +221,11 @@ impl NarrowPhase { // We have to manage the fact that one other collider will // have its graph index changed because of the node's swap-remove. - if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) { + if let Some(replacement) = self.intersection_graph.remove_node(intersection_graph_id) { if let Some(replacement) = self.graph_indices.get_mut(replacement) { - replacement.proximity_graph_index = proximity_graph_id; + replacement.intersection_graph_index = intersection_graph_id; } else { - prox_id_remap.insert(replacement, proximity_graph_id); + prox_id_remap.insert(replacement, intersection_graph_id); } } @@ -265,35 +265,33 @@ impl NarrowPhase { if co1.is_sensor() || co2.is_sensor() { // NOTE: the collider won't have a graph index as long // as it does not interact with anything. - if !InteractionGraph::<ProximityPair>::is_graph_index_valid( - gid1.proximity_graph_index, + if !InteractionGraph::<bool>::is_graph_index_valid( + gid1.intersection_graph_index, ) { - gid1.proximity_graph_index = - self.proximity_graph.graph.add_node(pair.collider1); + gid1.intersection_graph_index = + self.intersection_graph.graph.add_node(pair.collider1); } - if !InteractionGraph::<ProximityPair>::is_graph_index_valid( - gid2.proximity_graph_index, + if !InteractionGraph::<bool>::is_graph_index_valid( + gid2.intersection_graph_index, ) { - gid2.proximity_graph_index = - self.proximity_graph.graph.add_node(pair.collider2); + gid2.intersection_graph_index = + self.intersection_graph.graph.add_node(pair.collider2); } if self - .proximity_graph + .intersection_graph .graph - .find_edge(gid1.proximity_graph_index, gid2.proximity_graph_index) + .find_edge( + gid1.intersection_graph_index, + gid2.intersection_graph_index, + ) .is_none() { - let dispatcher = DefaultProximityDispatcher; - let generator = dispatcher - .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); - let interaction = - ProximityPair::new(*pair, generator.0, generator.1); - let _ = self.proximity_graph.add_edge( - gid1.proximity_graph_index, - gid2.proximity_graph_index, - interaction, + let _ = self.intersection_graph.add_edge( + gid1.intersection_graph_index, + gid2.intersection_graph_index, + false, ); } } else { @@ -343,22 +341,19 @@ impl NarrowPhase { self.graph_indices.get(pair.collider2), ) { if co1.is_sensor() || co2.is_sensor() { - let prox_pair = self.proximity_graph.remove_edge( - gid1.proximity_graph_index, - gid2.proximity_graph_index, + let was_intersecting = self.intersection_graph.remove_edge( + gid1.intersection_graph_index, + gid2.intersection_graph_index, ); - // Emit a proximity lost event if we had a proximity before removing the edge. - if let Some(prox) = prox_pair { - if prox.proximity != Proximity::Disjoint { - let prox_event = ProximityEvent::new( - pair.collider1, - pair.collider2, - prox.proximity, - Proximity::Disjoint, - ); - events.handle_proximity_event(prox_event) - } + // Emit an intersection lost event if we had an intersection before removing the edge. + if Some(true) == was_intersecting { + let prox_event = IntersectionEvent::new( + pair.collider1, + pair.collider2, + false, + ); + events.handle_intersection_event(prox_event) } } else { let contact_pair = self.contact_graph.remove_edge( @@ -387,7 +382,7 @@ impl NarrowPhase { } } - pub(crate) fn compute_proximities( + pub(crate) fn compute_intersections( &mut self, prediction_distance: f32, bodies: &RigidBodySet, @@ -395,10 +390,12 @@ impl NarrowPhase { pair_filter: Option<&dyn ProximityPairFilter>, events: &dyn EventHandler, ) { - par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| { - let pair = &mut edge.weight; - let co1 = &colliders[pair.pair.collider1]; - let co2 = &colliders[pair.pair.collider2]; + let nodes = &self.intersection_graph.graph.nodes; + par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| { + let handle1 = nodes[edge.source().index()].weight; + let handle2 = nodes[edge.target().index()].weight; + let co1 = &colliders[handle1]; + let co2 = &colliders[handle2]; // FIXME: avoid lookup into bodies. let rb1 = &bodies[co1.parent]; @@ -408,17 +405,17 @@ impl NarrowPhase { || (rb2.is_sleeping() && rb1.is_static()) || (rb1.is_sleeping() && rb2.is_sleeping()) { - // No need to update this proximity because nothing moved. + // No need to update this intersection because nothing moved. return; } if !co1.collision_groups.test(co2.collision_groups) { - // The proximity is not allowed. + // The intersection is not allowed. return; } if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() { - // Default filtering rule: no proximity between two non-dynamic bodies. + // Default filtering rule: no intersection between two non-dynamic bodies. return; } @@ -430,34 +427,26 @@ impl NarrowPhase { collider2: co2, }; - if !filter.filter_proximity_pair(&context) { - // No proximity allowed. + if !filter.filter_intersection_pair(&context) { + // No intersection allowed. return; } } - let dispatcher = DefaultProximityDispatcher; - if pair.detector.is_none() { - // We need a redispatch for this detector. - // This can happen, e.g., after restoring a snapshot of the narrow-phase. - let (detector, workspace) = - dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); - pair.detector = Some(detector); - pair.detector_workspace = workspace; - } - - let context = ProximityDetectionContext { - dispatcher: &dispatcher, - prediction_distance, - colliders, - pair, - }; + let pos12 = co1.position().inverse() * co2.position(); + let dispatcher = DefaultQueryDispatcher; - context - .pair - .detector - .unwrap() - .detect_proximity(context, events); + if let Ok(intersection) = dispatcher.intersection_test(&pos12, co1.shape(), co2.shape()) + { + if intersection != edge.weight { + edge.weight = intersection; + events.handle_intersection_event(IntersectionEvent::new( + handle1, + handle2, + intersection, + )); + } + } }); } 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 @@ - |
