diff --git a/src/algorithm/minkowski/gjk/mod.rs b/src/algorithm/minkowski/gjk/mod.rs
index 01ca715..ab0f237 100644
--- a/src/algorithm/minkowski/gjk/mod.rs
+++ b/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,
diff --git a/src/algorithm/minkowski/gjk/simplex/simplex2d.rs b/src/algorithm/minkowski/gjk/simplex/simplex2d.rs
index ed88b85..af73e0b 100644
--- a/src/algorithm/minkowski/gjk/simplex/simplex2d.rs
+++ b/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 {
diff --git a/src/algorithm/minkowski/gjk/simplex/simplex3d.rs b/src/algorithm/minkowski/gjk/simplex/simplex3d.rs
index 403c0c8..9f03682 100644
--- a/src/algorithm/minkowski/gjk/simplex/simplex3d.rs
+++ b/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)
diff --git a/src/algorithm/minkowski/mod.rs b/src/algorithm/minkowski/mod.rs
index 4a4ed5d..64d756b 100644
--- a/src/algorithm/minkowski/mod.rs
+++ b/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 Sub
for SupportPoint
+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};