Merge pull request #41 from dimforge/cylinder

Add cylinder and cone support + use a trait-object for shapes.

1 files changed, 15 insertions, 6 deletions

diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs
index 7e235c2..d211cf1 100644
--- a/src/geometry/contact.rs
+++ b/src/geometry/contact.rs
@@ -430,16 +430,26 @@ impl ContactManifold {
 
     #[inline]
     pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool {
+        //        const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
+        const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
+        const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded.
+        self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD)
+    }
+
+    #[inline]
+    pub(crate) fn try_update_contacts_eps(
+        &mut self,
+        pos12: &Isometry<f32>,
+        angle_dot_threshold: f32,
+        dist_sq_threshold: f32,
+    ) -> bool {
         if self.points.len() == 0 {
             return false;
         }
 
-        //        const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
-        const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
-
         let local_n2 = pos12 * self.local_n2;
 
-        if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
+        if -self.local_n1.dot(&local_n2) < angle_dot_threshold {
             return false;
         }
 
@@ -455,8 +465,7 @@ impl ContactManifold {
             }
             let new_local_p1 = local_p2 - self.local_n1 * dist;
 
-            let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
-            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
+            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
                 return false;
             }