From f6a107d71b9b920692307a0836311698806a8ea7 Mon Sep 17 00:00:00 2001 From: Benjamin Saunders Date: Sat, 13 Jun 2020 17:27:39 -0700 Subject: [PATCH] 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) } }