From 01b6e5b83f9f26b8d528e835a1e2d4a3afe32a96 Mon Sep 17 00:00:00 2001 From: Benjamin Saunders Date: Sat, 13 Jun 2020 16:04:49 -0700 Subject: [PATCH 1/3] Remove unnecessary argument --- .../heightfield_shape_manifold_generator.rs | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs index 92b965659..c1cbbd940 100644 --- a/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs +++ b/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs @@ -35,7 +35,6 @@ impl HeightFieldShapeManifoldGenerator { proc2: Option<&dyn ContactPreprocessor>, prediction: &ContactPrediction, manifold: &mut ContactManifold, - flip: bool, ) { self.timestamp += 1; @@ -48,7 +47,7 @@ impl HeightFieldShapeManifoldGenerator { .entry(i) { Entry::Occupied(mut entry) => { - let ok = if flip { + let ok = if self.flip { entry.get_mut().0.generate_contacts( dispatcher, m2, @@ -79,14 +78,14 @@ impl HeightFieldShapeManifoldGenerator { } } Entry::Vacant(entry) => { - let new_detector = if flip { + let new_detector = if self.flip { dispatcher.get_contact_algorithm(g2, elt1) } else { dispatcher.get_contact_algorithm(elt1, g2) }; if let Some(mut new_detector) = new_detector { - if flip { + if self.flip { let _ = new_detector.generate_contacts( dispatcher, m2, @@ -138,12 +137,12 @@ impl ContactManifoldGenerator for HeightFieldShapeManifoldGener ) -> bool { if !self.flip { if let Some(hf) = a.as_shape::>() { - self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold, false); + self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold); return true; } } else { if let Some(hf) = b.as_shape::>() { - self.do_update(d, mb, hf, proc2, ma, a, proc1, prediction, manifold, true); + self.do_update(d, mb, hf, proc2, ma, a, proc1, prediction, manifold); return true; } } From bb52d088a4cce04554cfcbf99009ddf2d3471966 Mon Sep 17 00:00:00 2001 From: Benjamin Saunders Date: Sat, 13 Jun 2020 17:17:44 -0700 Subject: [PATCH 2/3] Extend ContactDispatcher to support automatically flipping objects --- .../contact_manifold_generator.rs | 78 ++++++++++++++++++- .../default_contact_dispatcher.rs | 68 +++++++++------- .../narrow_phase/contact_generator/mod.rs | 2 +- src/pipeline/narrow_phase/mod.rs | 2 +- src/query/contact/contact_kinematic.rs | 8 ++ src/query/contact/contact_manifold.rs | 16 +++- 6 files changed, 142 insertions(+), 32 deletions(-) diff --git a/src/pipeline/narrow_phase/contact_generator/contact_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/contact_manifold_generator.rs index 61b2f293c..251a16b87 100644 --- a/src/pipeline/narrow_phase/contact_generator/contact_manifold_generator.rs +++ b/src/pipeline/narrow_phase/contact_generator/contact_manifold_generator.rs @@ -33,15 +33,91 @@ pub trait ContactManifoldGenerator: Any + Send + Sync { fn init_manifold(&self) -> ContactManifold { ContactManifold::new() } + + /// Construct a version of `Self` that flips the arguments corresponding to each object. + /// + /// Allows implementations to assume a particular argument order (e.g. placing a shape of a + /// particular type first) for convenience, while still allowing a `ContactDispatcher` to + /// support either argument ordering. + fn flip(self) -> FlippedContactManifoldGenerator + where + Self: Sized, + { + FlippedContactManifoldGenerator(self) + } +} + +pub struct FlippedContactManifoldGenerator(T); + +impl> ContactManifoldGenerator + for FlippedContactManifoldGenerator +{ + fn generate_contacts( + &mut self, + d: &dyn ContactDispatcher, + ma: &Isometry, + a: &dyn Shape, + proc1: Option<&dyn ContactPreprocessor>, + mb: &Isometry, + b: &dyn Shape, + proc2: Option<&dyn ContactPreprocessor>, + prediction: &ContactPrediction, + manifold: &mut ContactManifold, + ) -> bool { + let dispatcher = FlippedContactDispatcher(d); + manifold.flip_new_contacts(); + let result = self.0.generate_contacts( + &dispatcher, + ma, + a, + proc1, + mb, + b, + proc2, + prediction, + manifold, + ); + manifold.flip_new_contacts(); + result + } } pub type ContactAlgorithm = Box>; -pub trait ContactDispatcher: Any + Send + Sync { +pub trait ContactDispatcher: Send + Sync { /// Allocate a collision algorithm corresponding to a pair of objects with the given shapes. + /// + /// Shorthand for `self.get_flipped_contact_algorithm(false, a, b)`. fn get_contact_algorithm( &self, a: &dyn Shape, b: &dyn Shape, + ) -> Option> { + self.get_flipped_contact_algorithm(false, a, b) + } + + /// Allocate a collision algorithm corresponding to a pair of objects with the given shapes. + /// + /// If `flip` is true, `a` and `b` are reversed, as are the corresponding arguments to any + /// returned `ContactAlgorithm`. + fn get_flipped_contact_algorithm( + &self, + flip: bool, + a: &dyn Shape, + b: &dyn Shape, ) -> Option>; } + +/// A view of a `ContactDispatcher` which inverts the value of `flip` in `get_flipped_contact_algorithm` +struct FlippedContactDispatcher<'a, N>(&'a dyn ContactDispatcher); + +impl ContactDispatcher for FlippedContactDispatcher<'_, N> { + fn get_flipped_contact_algorithm( + &self, + flip: bool, + a: &dyn Shape, + b: &dyn Shape, + ) -> Option> { + self.0.get_flipped_contact_algorithm(!flip, a, b) + } +} diff --git a/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs b/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs index b9c7a965d..d041e5d35 100644 --- a/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs +++ b/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs @@ -4,9 +4,9 @@ use crate::pipeline::{ BallBallManifoldGenerator, BallConvexPolyhedronManifoldGenerator, CapsuleCapsuleManifoldGenerator, CapsuleShapeManifoldGenerator, CompositeShapeCompositeShapeManifoldGenerator, CompositeShapeShapeManifoldGenerator, - ContactAlgorithm, ContactDispatcher, ConvexPolyhedronConvexPolyhedronManifoldGenerator, - HeightFieldShapeManifoldGenerator, PlaneBallManifoldGenerator, - PlaneConvexPolyhedronManifoldGenerator, + ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator, + ConvexPolyhedronConvexPolyhedronManifoldGenerator, HeightFieldShapeManifoldGenerator, + PlaneBallManifoldGenerator, PlaneConvexPolyhedronManifoldGenerator, }; #[cfg(feature = "dim3")] use crate::shape::TriMesh; @@ -24,8 +24,9 @@ impl DefaultContactDispatcher { } impl ContactDispatcher for DefaultContactDispatcher { - fn get_contact_algorithm( + fn get_flipped_contact_algorithm( &self, + flip: bool, a: &dyn Shape, b: &dyn Shape, ) -> Option> { @@ -44,55 +45,68 @@ impl ContactDispatcher for DefaultContactDispatcher { let b_is_trimesh = b.is_shape::>(); if a_is_trimesh && b_is_trimesh { - return Some(Box::new(TriMeshTriMeshManifoldGenerator::::new())); + return Some(wrap(flip, TriMeshTriMeshManifoldGenerator::::new())); } } if a_is_heightfield || b_is_heightfield { - return Some(Box::new(HeightFieldShapeManifoldGenerator::::new( - b_is_heightfield, - ))); + return Some(wrap( + flip, + HeightFieldShapeManifoldGenerator::::new(b_is_heightfield), + )); } else if a_is_capsule && b_is_capsule { - Some(Box::new(CapsuleCapsuleManifoldGenerator::::new())) + Some(wrap(flip, CapsuleCapsuleManifoldGenerator::::new())) } else if a_is_capsule || b_is_capsule { - Some(Box::new(CapsuleShapeManifoldGenerator::::new( - b_is_capsule, - ))) + Some(wrap( + flip, + CapsuleShapeManifoldGenerator::::new(b_is_capsule), + )) } else if a_is_ball && b_is_ball { - Some(Box::new(BallBallManifoldGenerator::::new())) + Some(wrap(flip, BallBallManifoldGenerator::::new())) } else if a_is_plane && b_is_ball { - Some(Box::new(PlaneBallManifoldGenerator::::new(false))) + Some(wrap(flip, PlaneBallManifoldGenerator::::new(false))) } else if a_is_ball && b_is_plane { - Some(Box::new(PlaneBallManifoldGenerator::::new(true))) + Some(wrap(flip, PlaneBallManifoldGenerator::::new(true))) } else if a_is_plane && b.is_support_map() { let gen = PlaneConvexPolyhedronManifoldGenerator::::new(false); - Some(Box::new(gen)) + Some(wrap(flip, gen)) } else if b_is_plane && a.is_support_map() { let gen = PlaneConvexPolyhedronManifoldGenerator::::new(true); - Some(Box::new(gen)) + Some(wrap(flip, gen)) } else if a_is_ball && b.is_convex_polyhedron() { let gen = BallConvexPolyhedronManifoldGenerator::::new(false); - Some(Box::new(gen)) + Some(wrap(flip, gen)) } else if b_is_ball && a.is_convex_polyhedron() { let gen = BallConvexPolyhedronManifoldGenerator::::new(true); - Some(Box::new(gen)) + Some(wrap(flip, gen)) } else if a.is_convex_polyhedron() && b.is_convex_polyhedron() { let gen = ConvexPolyhedronConvexPolyhedronManifoldGenerator::new(); - Some(Box::new(gen)) + Some(wrap(flip, gen)) } else if a.is_composite_shape() && b.is_composite_shape() { - Some(Box::new( + Some(wrap( + flip, CompositeShapeCompositeShapeManifoldGenerator::::new(), )) } else if a.is_composite_shape() { - Some(Box::new(CompositeShapeShapeManifoldGenerator::::new( - false, - ))) + Some(wrap( + flip, + CompositeShapeShapeManifoldGenerator::::new(false), + )) } else if b.is_composite_shape() { - Some(Box::new(CompositeShapeShapeManifoldGenerator::::new( - true, - ))) + Some(wrap( + flip, + CompositeShapeShapeManifoldGenerator::::new(true), + )) } else { None } } } + +fn wrap>(flip: bool, x: T) -> ContactAlgorithm { + if flip { + Box::new(x.flip()) + } else { + Box::new(x) + } +} diff --git a/src/pipeline/narrow_phase/contact_generator/mod.rs b/src/pipeline/narrow_phase/contact_generator/mod.rs index 467b93106..ab01a053d 100644 --- a/src/pipeline/narrow_phase/contact_generator/mod.rs +++ b/src/pipeline/narrow_phase/contact_generator/mod.rs @@ -7,7 +7,7 @@ pub use self::composite_shape_composite_shape_manifold_generator::CompositeShape pub use self::composite_shape_shape_manifold_generator::CompositeShapeShapeManifoldGenerator; #[doc(inline)] pub use self::contact_manifold_generator::{ - ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator, + ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator, FlippedContactManifoldGenerator, }; pub use self::convex_polyhedron_convex_polyhedron_manifold_generator::ConvexPolyhedronConvexPolyhedronManifoldGenerator; pub use self::default_contact_dispatcher::DefaultContactDispatcher; diff --git a/src/pipeline/narrow_phase/mod.rs b/src/pipeline/narrow_phase/mod.rs index 4d0d64776..84b54256f 100644 --- a/src/pipeline/narrow_phase/mod.rs +++ b/src/pipeline/narrow_phase/mod.rs @@ -9,7 +9,7 @@ pub use self::contact_generator::{ CompositeShapeCompositeShapeManifoldGenerator, CompositeShapeShapeManifoldGenerator, ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator, ConvexPolyhedronConvexPolyhedronManifoldGenerator, DefaultContactDispatcher, - HeightFieldShapeManifoldGenerator, PlaneBallManifoldGenerator, + FlippedContactManifoldGenerator, HeightFieldShapeManifoldGenerator, PlaneBallManifoldGenerator, PlaneConvexPolyhedronManifoldGenerator, }; pub use self::events::{ContactEvent, ContactEvents, EventPool, ProximityEvent, ProximityEvents}; diff --git a/src/query/contact/contact_kinematic.rs b/src/query/contact/contact_kinematic.rs index 102c5b18b..80f2e7357 100644 --- a/src/query/contact/contact_kinematic.rs +++ b/src/query/contact/contact_kinematic.rs @@ -1,3 +1,5 @@ +use std::mem; + use crate::math::{Isometry, Point, Vector}; use crate::query::{self, Contact}; use crate::shape::{FeatureId, Shape}; @@ -371,4 +373,10 @@ impl ContactKinematic { Some(Contact::new(world1, world2, normal, depth)) } + + /// Swap fields associated with objects 1 and 2 + pub fn flip(&mut self) { + mem::swap(&mut self.approx1, &mut self.approx2); + mem::swap(&mut self.margin1, &mut self.margin2); + } } diff --git a/src/query/contact/contact_manifold.rs b/src/query/contact/contact_manifold.rs index b240f576e..ea6f2dcad 100644 --- a/src/query/contact/contact_manifold.rs +++ b/src/query/contact/contact_manifold.rs @@ -36,6 +36,7 @@ pub struct ContactManifold { deepest: usize, contacts: Slab<(TrackedContact, usize)>, cache: ContactCache, + flip_new_contacts: bool, } impl ContactManifold { @@ -49,6 +50,7 @@ impl ContactManifold { persistence: 1, contacts: Slab::new(), cache: ContactCache::DistanceBased(Vec::new(), na::convert(0.02)), + flip_new_contacts: false, } } @@ -170,14 +172,19 @@ impl ContactManifold { preprocessor1: Option<&dyn ContactPreprocessor>, preprocessor2: Option<&dyn ContactPreprocessor>, ) -> bool { + if self.flip_new_contacts { + contact.flip(); + kinematic.flip(); + } + if let Some(pp) = preprocessor1 { - if !pp.process_contact(&mut contact, &mut kinematic, true) { + if !pp.process_contact(&mut contact, &mut kinematic, !self.flip_new_contacts) { return false; } } if let Some(pp) = preprocessor2 { - if !pp.process_contact(&mut contact, &mut kinematic, false) { + if !pp.process_contact(&mut contact, &mut kinematic, self.flip_new_contacts) { return false; } } @@ -274,4 +281,9 @@ impl ContactManifold { } } } + + /// Toggle whether `push` flips contacts + pub(crate) fn flip_new_contacts(&mut self) { + self.flip_new_contacts ^= true; + } } From f1da60b1c2da1b29ede8ae1dae4ae6bbbb71ac27 Mon Sep 17 00:00:00 2001 From: Benjamin Saunders Date: Sat, 13 Jun 2020 17:27:39 -0700 Subject: [PATCH 3/3] Remove dynamic flipping from heightfield and plane vs. ball tests --- .../default_contact_dispatcher.rs | 8 +- .../heightfield_shape_manifold_generator.rs | 93 ++++++------------- .../plane_ball_manifold_generator.rs | 29 ++---- 3 files changed, 37 insertions(+), 93 deletions(-) diff --git a/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs b/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs index d041e5d35..75d026e73 100644 --- a/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs +++ b/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs @@ -51,8 +51,8 @@ impl ContactDispatcher for DefaultContactDispatcher { if a_is_heightfield || b_is_heightfield { return Some(wrap( - flip, - HeightFieldShapeManifoldGenerator::::new(b_is_heightfield), + flip ^ b_is_heightfield, + HeightFieldShapeManifoldGenerator::::new(), )); } else if a_is_capsule && b_is_capsule { Some(wrap(flip, CapsuleCapsuleManifoldGenerator::::new())) @@ -64,9 +64,9 @@ impl ContactDispatcher for DefaultContactDispatcher { } else if a_is_ball && b_is_ball { Some(wrap(flip, BallBallManifoldGenerator::::new())) } else if a_is_plane && b_is_ball { - Some(wrap(flip, PlaneBallManifoldGenerator::::new(false))) + Some(wrap(flip, PlaneBallManifoldGenerator::::new())) } else if a_is_ball && b_is_plane { - Some(wrap(flip, PlaneBallManifoldGenerator::::new(true))) + Some(wrap(!flip, PlaneBallManifoldGenerator::::new())) } else if a_is_plane && b.is_support_map() { let gen = PlaneConvexPolyhedronManifoldGenerator::::new(false); Some(wrap(flip, gen)) diff --git a/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs index c1cbbd940..13c6fdd7b 100644 --- a/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs +++ b/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs @@ -10,16 +10,14 @@ use std::collections::{hash_map::Entry, HashMap}; /// Collision detector between an heightfield and another shape. pub struct HeightFieldShapeManifoldGenerator { sub_detectors: HashMap, usize), DeterministicState>, - flip: bool, timestamp: usize, } impl HeightFieldShapeManifoldGenerator { /// Creates a new collision detector between an heightfield and another shape. - pub fn new(flip: bool) -> HeightFieldShapeManifoldGenerator { + pub fn new() -> HeightFieldShapeManifoldGenerator { HeightFieldShapeManifoldGenerator { sub_detectors: HashMap::with_hasher(DeterministicState), - flip, timestamp: 0, } } @@ -47,20 +45,27 @@ impl HeightFieldShapeManifoldGenerator { .entry(i) { Entry::Occupied(mut entry) => { - let ok = if self.flip { - entry.get_mut().0.generate_contacts( - dispatcher, - m2, - g2, - proc2, - m1, - elt1, - Some(&(proc1, part_proc1)), - prediction, - manifold, - ) - } else { - entry.get_mut().0.generate_contacts( + let ok = entry.get_mut().0.generate_contacts( + dispatcher, + m1, + elt1, + Some(&(proc1, part_proc1)), + m2, + g2, + proc2, + prediction, + manifold, + ); + + if ok { + entry.get_mut().1 = self.timestamp; + } + } + Entry::Vacant(entry) => { + let new_detector = dispatcher.get_contact_algorithm(elt1, g2); + + if let Some(mut new_detector) = new_detector { + let _ = new_detector.generate_contacts( dispatcher, m1, elt1, @@ -70,46 +75,7 @@ impl HeightFieldShapeManifoldGenerator { proc2, prediction, manifold, - ) - }; - - if ok { - entry.get_mut().1 = self.timestamp; - } - } - Entry::Vacant(entry) => { - let new_detector = if self.flip { - dispatcher.get_contact_algorithm(g2, elt1) - } else { - dispatcher.get_contact_algorithm(elt1, g2) - }; - - if let Some(mut new_detector) = new_detector { - if self.flip { - let _ = new_detector.generate_contacts( - dispatcher, - m2, - g2, - proc2, - m1, - elt1, - Some(&(proc1, part_proc1)), - prediction, - manifold, - ); - } else { - let _ = new_detector.generate_contacts( - dispatcher, - m1, - elt1, - Some(&(proc1, part_proc1)), - m2, - g2, - proc2, - prediction, - manifold, - ); - } + ); let _ = entry.insert((new_detector, self.timestamp)); } } @@ -135,16 +101,9 @@ impl ContactManifoldGenerator for HeightFieldShapeManifoldGener prediction: &ContactPrediction, manifold: &mut ContactManifold, ) -> bool { - if !self.flip { - if let Some(hf) = a.as_shape::>() { - self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold); - return true; - } - } else { - if let Some(hf) = b.as_shape::>() { - self.do_update(d, mb, hf, proc2, ma, a, proc1, prediction, manifold); - return true; - } + if let Some(hf) = a.as_shape::>() { + self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold); + return true; } return false; diff --git a/src/pipeline/narrow_phase/contact_generator/plane_ball_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/plane_ball_manifold_generator.rs index 8b7098919..0f1438197 100644 --- a/src/pipeline/narrow_phase/contact_generator/plane_ball_manifold_generator.rs +++ b/src/pipeline/narrow_phase/contact_generator/plane_ball_manifold_generator.rs @@ -11,7 +11,6 @@ use std::marker::PhantomData; /// Collision detector between g1 plane and g1 shape implementing the `SupportMap` trait. #[derive(Clone)] pub struct PlaneBallManifoldGenerator { - flip: bool, phantom: PhantomData, } @@ -19,9 +18,8 @@ impl PlaneBallManifoldGenerator { /// Creates g1 new persistent collision detector between g1 plane and g1 shape with g1 support /// mapping function. #[inline] - pub fn new(flip: bool) -> PlaneBallManifoldGenerator { + pub fn new() -> PlaneBallManifoldGenerator { PlaneBallManifoldGenerator { - flip, phantom: PhantomData, } } @@ -36,7 +34,6 @@ impl PlaneBallManifoldGenerator { proc2: Option<&dyn ContactPreprocessor>, prediction: &ContactPrediction, manifold: &mut ContactManifold, - flip: bool, ) -> bool { if let (Some(plane), Some(ball)) = (g1.as_shape::>(), g2.as_shape::>()) { let plane_normal = m1 * plane.normal(); @@ -61,19 +58,11 @@ impl PlaneBallManifoldGenerator { let approx_ball = NeighborhoodGeometry::Point; let approx_plane = NeighborhoodGeometry::Plane(*plane.normal()); - if !flip { - contact = Contact::new(world1, world2, plane_normal, depth); - kinematic.set_approx1(f1, local1, approx_plane); - kinematic.set_approx2(f2, local2, approx_ball); - kinematic.set_dilation2(ball.radius()); - let _ = manifold.push(contact, kinematic, Point::origin(), proc1, proc2); - } else { - contact = Contact::new(world2, world1, -plane_normal, depth); - kinematic.set_approx1(f2, local2, approx_ball); - kinematic.set_dilation1(ball.radius()); - kinematic.set_approx2(f1, local1, approx_plane); - let _ = manifold.push(contact, kinematic, Point::origin(), proc2, proc1); - } + contact = Contact::new(world1, world2, plane_normal, depth); + kinematic.set_approx1(f1, local1, approx_plane); + kinematic.set_approx2(f2, local2, approx_ball); + kinematic.set_dilation2(ball.radius()); + let _ = manifold.push(contact, kinematic, Point::origin(), proc1, proc2); } true @@ -97,10 +86,6 @@ impl ContactManifoldGenerator for PlaneBallManifoldGenerator prediction: &ContactPrediction, manifold: &mut ContactManifold, ) -> bool { - if !self.flip { - Self::do_update_to(m1, g1, proc1, m2, g2, proc2, prediction, manifold, false) - } else { - Self::do_update_to(m2, g2, proc2, m1, g1, proc1, prediction, manifold, true) - } + Self::do_update_to(m1, g1, proc1, m2, g2, proc2, prediction, manifold) } }