1 files changed, 78 insertions, 4 deletions
diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs
index 452b380..44446af 100644
--- a/src/geometry/sat.rs
+++ b/src/geometry/sat.rs
@@ -1,8 +1,20 @@
-use crate::geometry::{cuboid, Cuboid, Polygon, Triangle};
+use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle};
 use crate::math::{Isometry, Point, Vector, DIM};
 use crate::utils::WSign;
 use na::Unit;
-use ncollide::shape::{Segment, SupportMap};
+use ncollide::shape::SupportMap;
+
+#[allow(dead_code)]
+pub fn support_map_support_map_compute_separation(
+    sm1: &impl SupportMap<f32>,
+    sm2: &impl SupportMap<f32>,
+    m12: &Isometry<f32>,
+    dir1: &Unit<Vector<f32>>,
+) -> f32 {
+    let p1 = sm1.local_support_point_toward(dir1);
+    let p2 = sm2.support_point_toward(m12, &-*dir1);
+    (p2 - p1).dot(dir1)
+}
 
 #[allow(dead_code)]
 pub fn polygon_polygon_compute_separation_features(
@@ -128,7 +140,6 @@ pub fn cuboid_cuboid_find_local_separating_normal_oneway(
  *
  *
  */
-
 #[cfg(feature = "dim3")]
 pub fn cube_support_map_compute_separation_wrt_local_line<S: SupportMap<f32>>(
     cube1: &Cuboid,
@@ -207,7 +218,7 @@ pub fn cube_triangle_find_local_separating_edge_twoway(
 #[cfg(feature = "dim3")]
 pub fn cube_segment_find_local_separating_edge_twoway(
     cube1: &Cuboid,
-    segment2: &Segment<f32>,
+    segment2: &Segment,
     pos12: &Isometry<f32>,
     pos21: &Isometry<f32>,
 ) -> (f32, Vector<f32>) {
@@ -293,3 +304,66 @@ pub fn segment_cuboid_find_local_separating_normal_oneway(
 ) -> (f32, Vector<f32>) {
     point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12)
 }
+
+/*
+ * Capsules
+ */
+#[cfg(feature = "dim3")]
+pub fn triangle_segment_find_local_separating_normal_oneway(
+    triangle1: &Triangle,
+    segment2: &Segment,
+    m12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    if let Some(dir) = triangle1.normal() {
+        let p2a = segment2.support_point_toward(m12, &-dir);
+        let p2b = segment2.support_point_toward(m12, &dir);
+        let sep_a = (p2a - triangle1.a).dot(&dir);
+        let sep_b = -(p2b - triangle1.a).dot(&dir);
+
+        if sep_a >= sep_b {
+            (sep_a, *dir)
+        } else {
+            (sep_b, -*dir)
+        }
+    } else {
+        (-f32::MAX, Vector::zeros())
+    }
+}
+
+#[cfg(feature = "dim3")]
+pub fn segment_triangle_find_local_separating_edge(
+    segment1: &Segment,
+    triangle2: &Triangle,
+    pos12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    let x2 = pos12 * (triangle2.b - triangle2.a);
+    let y2 = pos12 * (triangle2.c - triangle2.b);
+    let z2 = pos12 * (triangle2.a - triangle2.c);
+    let dir1 = segment1.scaled_direction();
+
+    let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)];
+    let axes1 = [
+        crosses1[0],
+        crosses1[1],
+        crosses1[2],
+        -crosses1[0],
+        -crosses1[1],
+        -crosses1[2],
+    ];
+    let mut max_separation = -f32::MAX;
+    let mut sep_dir = axes1[0];
+
+    for axis1 in &axes1 {
+        if let Some(axis1) = Unit::try_new(*axis1, 0.0) {
+            let sep =
+                support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1);
+
+            if sep > max_separation {
+                max_separation = sep;
+                sep_dir = *axis1;
+            }
+        }
+    }
+
+    (max_separation, sep_dir)
+}