fix: Reimplement time of impact, was only working for a small set of …

…scenarios before

src/algorithm/minkowski/gjk/mod.rs

@@ -178,94 +178,66 @@ where
             - left_transform.start.transform_point(P::origin());
         let right_lin_vel = right_transform.end.transform_point(P::origin())
             - right_transform.start.transform_point(P::origin());
-        let r = left_lin_vel - right_lin_vel;
+        let ray = right_lin_vel - left_lin_vel;
 
         // initialize time of impact
         let mut lambda = S::zero();
         let mut normal = P::Diff::zero();
-
-        // build the start transforms
-        let mut left_curr_transform = left_transform
-            .start
-            .translation_interpolate(left_transform.end, lambda);
-        let mut right_curr_transform = right_transform
-            .start
-            .translation_interpolate(right_transform.end, lambda);
+        let mut ray_origin = P::origin();
 
         // build simplex and get the first support point
         let mut simplex = Vec::with_capacity(5);
-        simplex.push(SupportPoint::from_minkowski(
+        let p = SupportPoint::from_minkowski(
             left,
-            &left_curr_transform,
+            left_transform.start,
             right,
-            &right_curr_transform,
-            &-r,
-        ));
-        let mut d = simplex[0].v;
-        for _ in 0..self.max_iterations {
-            // d almost at origin means we have a hit
-            if d.magnitude2() <= self.continuous_tolerance {
-                let mut contact = Contact::new_with_point(
-                    CollisionStrategy::FullResolution,
-                    normal.normalize(),
-                    d.magnitude(),
-                    simplex.last().unwrap().sup_a,
-                );
-                contact.time_of_impact = lambda;
-                return Some(contact);
-            }
-
-            // time of impact > 1 means miss
-            if lambda > S::one() {
-                return None;
-            }
+            right_transform.start,
+            &-ray,
+        );
+        let mut v = p.v;
 
+        while v.magnitude2() > self.continuous_tolerance {
             let p = SupportPoint::from_minkowski(
                 left,
-                &left_curr_transform,
+                left_transform.start,
                 right,
-                &right_curr_transform,
-                &-d,
+                right_transform.start,
+                &-v,
             );
 
-            let vp = d.dot(p.v);
-            if vp > S::zero() {
-                let vr = d.dot(r);
-                if vr >= -self.continuous_tolerance {
-                    return None;
+            let vp = v.dot(p.v);
+            let vr = v.dot(ray);
+            if vp > lambda * vr {
+                if vr > S::zero() {
+                    lambda = vp / vr;
+                    if lambda > S::one() {
+                        return None;
+                    }
+                    ray_origin = P::from_vec(ray * lambda);
+                    simplex.clear();
+                    normal = -v;
                 } else {
-                    // we have a potential hit, move origin forwards along the ray
-                    lambda -= vp / vr;
-
-                    // interpolate translation of shapes along the ray
-                    left_curr_transform = left_transform
-                        .start
-                        .translation_interpolate(left_transform.end, lambda);
-                    right_curr_transform = right_transform
-                        .start
-                        .translation_interpolate(right_transform.end, lambda);
-                    normal = d;
+                    return None;
                 }
             }
-            simplex.push(p);
-
-            // if reduction returns true, we have a hit, so return time of impact
-            // if not, the simplex is reduced to the closest feature to the origin, and v is the
-            // normal of the feature in the direction of the origin
-            if self.simplex_processor
-                .reduce_to_closest_feature(&mut simplex, &mut d)
-            {
-                let mut contact = Contact::new_with_point(
-                    CollisionStrategy::FullResolution,
-                    normal.normalize(),
-                    d.magnitude(),
-                    simplex.last().unwrap().sup_a,
-                );
-                contact.time_of_impact = lambda;
-                return Some(contact);
-            }
+            simplex.push(p - ray_origin);
+            v = self.simplex_processor.get_closest_point_to_origin(&mut simplex);
+        }
+        if v.magnitude2() <= self.continuous_tolerance {
+            let transform = right_transform
+                .start
+                .translation_interpolate(right_transform.end, lambda);
+            let mut contact = Contact::new_with_point(
+                CollisionStrategy::FullResolution,
+                -normal.normalize(),
+                v.magnitude(),
+                transform.transform_point(ray_origin),
+            );
+            contact.time_of_impact = lambda;
+            Some(contact)
+        } else {
+            None
         }
-        None
     }
 
     /// Compute the distance between the given primitives.
@@ -769,10 +741,10 @@ mod tests {
 
     #[test]
     fn test_gjk_time_of_impact_2d() {
-        let left = Rectangle::new(10., 10.);
+        let left = Rectangle::new(10., 20.);
         let left_start_transform = transform(0., 0., 0.);
         let left_end_transform = transform(30., 0., 0.);
-        let right = Rectangle::new(10., 10.);
+        let right = Rectangle::new(10., 11.);
         let right_transform = transform(15., 0., 0.);
         let gjk = GJK2::new();
 
@@ -784,6 +756,9 @@ mod tests {
         ).unwrap();
 
         assert_ulps_eq!(0.1666667, contact.time_of_impact);
+        assert_eq!(Vector2::new(-1., 0.), contact.normal);
+        assert_eq!(0., contact.penetration_depth);
+        assert_eq!(Point2::new(10., 0.), contact.contact_point);
 
         assert!(gjk.intersection_time_of_impact(
             &left,
@@ -795,10 +770,10 @@ mod tests {
 
     #[test]
     fn test_gjk_time_of_impact_3d() {
-        let left = Cuboid::new(10., 10., 10.);
+        let left = Cuboid::new(10., 11., 10.);
         let left_start_transform = transform_3d(0., 0., 0., 0.);
         let left_end_transform = transform_3d(30., 0., 0., 0.);
-        let right = Cuboid::new(10., 10., 10.);
+        let right = Cuboid::new(10., 15., 10.);
         let right_transform = transform_3d(15., 0., 0., 0.);
         let gjk = GJK3::new();
 
@@ -810,6 +785,9 @@ mod tests {
         ).unwrap();
 
         assert_ulps_eq!(0.1666667, contact.time_of_impact);
+        assert_eq!(Vector3::new(-1., 0., 0.), contact.normal);
+        assert_eq!(0., contact.penetration_depth);
+        assert_eq!(Point3::new(10., 0., 0.), contact.contact_point);
 
         assert!(gjk.intersection_time_of_impact(
             &left,

src/algorithm/minkowski/gjk/simplex/simplex2d.rs

@@ -84,7 +84,11 @@ where
         }
 
         // compute closest point to origin on the simplex (which is now an edge)
-        get_closest_point_on_edge(&simplex[1].v, &simplex[0].v, &Vector2::zero())
+        if simplex.len() == 1 {
+            simplex[0].v
+        } else {
+            get_closest_point_on_edge(&simplex[1].v, &simplex[0].v, &Vector2::zero())
+        }
     }
 
     fn new() -> Self {

src/algorithm/minkowski/gjk/simplex/simplex3d.rs

@@ -114,7 +114,9 @@ where
             return d;
         }
 
-        if simplex.len() == 2 {
+        if simplex.len() == 1 {
+            simplex[0].v
+        } else if simplex.len() == 2 {
             get_closest_point_on_edge(&simplex[1].v, &simplex[0].v, &Vector3::zero())
         } else {
             get_closest_point_on_face(&simplex[2].v, &simplex[1].v, &simplex[0].v, &d)

src/algorithm/minkowski/mod.rs

@@ -3,7 +3,7 @@
 pub use self::epa::{EPA2, EPA3, EPA};
 pub use self::gjk::{GJK2, GJK3, SimplexProcessor, GJK};
 
-use std::ops::Neg;
+use std::ops::{Neg, Sub};
 
 use cgmath::prelude::*;
 use prelude::*;
@@ -60,6 +60,21 @@ where
     }
 }
 
+impl<P> Sub<P> for SupportPoint<P>
+where
+    P: EuclideanSpace,
+{
+    type Output = Self;
+
+    fn sub(self, rhs: P) -> Self {
+        SupportPoint {
+            v: self.v - rhs.to_vec(),
+            sup_a: self.sup_a,
+            sup_b: self.sup_b,
+        }
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use cgmath::{Basis2, Decomposed, Rad, Rotation2, Vector2};