1 files changed, 55 insertions, 31 deletions

diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs
index 0d754af..54736cc 100644
--- a/src/geometry/capsule.rs
+++ b/src/geometry/capsule.rs
@@ -1,18 +1,16 @@
-use crate::geometry::AABB;
+use crate::geometry::{Ray, RayIntersection, AABB};
 use crate::math::{Isometry, Point, Rotation, Vector};
 use approx::AbsDiffEq;
 use na::Unit;
-use ncollide::query::{PointProjection, PointQuery};
-use ncollide::shape::{FeatureId, Segment};
+use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast};
+use ncollide::shape::{FeatureId, Segment, SupportMap};
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A capsule shape defined as a segment with a radius.
+/// A capsule shape defined as a round segment.
 pub struct Capsule {
-    /// The first endpoint of the capsule.
-    pub a: Point<f32>,
-    /// The second enpdoint of the capsule.
-    pub b: Point<f32>,
+    /// The axis and endpoint of the capsule.
+    pub segment: Segment<f32>,
     /// The radius of the capsule.
     pub radius: f32,
 }
@@ -39,13 +37,14 @@ impl Capsule {
 
     /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`.
     pub fn new(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
-        Self { a, b, radius }
+        let segment = Segment::new(a, b);
+        Self { segment, radius }
     }
 
     /// The axis-aligned bounding box of this capsule.
     pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
-        let a = pos * self.a;
-        let b = pos * self.b;
+        let a = pos * self.segment.a;
+        let b = pos * self.segment.b;
         let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius);
         let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius);
         AABB::new(mins.into(), maxs.into())
@@ -53,7 +52,7 @@ impl Capsule {
 
     /// The height of this capsule.
     pub fn height(&self) -> f32 {
-        (self.b - self.a).norm()
+        (self.segment.b - self.segment.a).norm()
     }
 
     /// The half-height of this capsule.
@@ -63,17 +62,17 @@ impl Capsule {
 
     /// The center of this capsule.
     pub fn center(&self) -> Point<f32> {
-        na::center(&self.a, &self.b)
+        na::center(&self.segment.a, &self.segment.b)
     }
 
     /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`.
     pub fn transform_by(&self, pos: &Isometry<f32>) -> Self {
-        Self::new(pos * self.a, pos * self.b, self.radius)
+        Self::new(pos * self.segment.a, pos * self.segment.b, self.radius)
     }
 
     /// The rotation `r` such that `r * Y` is collinear with `b - a`.
     pub fn rotation_wrt_y(&self) -> Rotation<f32> {
-        let mut dir = self.b - self.a;
+        let mut dir = self.segment.b - self.segment.a;
         if dir.y < 0.0 {
             dir = -dir;
         }
@@ -96,24 +95,49 @@ impl Capsule {
     }
 }
 
-// impl SupportMap<f32> for Capsule {
-//     fn local_support_point(&self, dir: &Vector) -> Point {
-//         let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis());
-//         self.local_support_point_toward(&dir)
-//     }
-//
-//     fn local_support_point_toward(&self, dir: &Unit<Vector>) -> Point {
-//         if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) {
-//             self.a + **dir * self.radius
-//         } else {
-//             self.b + **dir * self.radius
-//         }
-//     }
-// }
+impl SupportMap<f32> for Capsule {
+    fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
+        let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis());
+        self.local_support_point_toward(&dir)
+    }
+
+    fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
+        if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) {
+            self.segment.a + **dir * self.radius
+        } else {
+            self.segment.b + **dir * self.radius
+        }
+    }
+}
+
+impl RayCast<f32> for Capsule {
+    fn toi_and_normal_with_ray(
+        &self,
+        m: &Isometry<f32>,
+        ray: &Ray,
+        max_toi: f32,
+        solid: bool,
+    ) -> Option<RayIntersection> {
+        let ls_ray = ray.inverse_transform_by(m);
+
+        ncollide::query::ray_intersection_with_support_map_with_params(
+            &Isometry::identity(),
+            self,
+            &mut VoronoiSimplex::new(),
+            &ls_ray,
+            max_toi,
+            solid,
+        )
+        .map(|mut res| {
+            res.normal = m * res.normal;
+            res
+        })
+    }
+}
 
 // TODO: this code has been extracted from ncollide and added here
 // so we can modify it to fit with our new definition of capsule.
-// Wa should find a way to avoid this code duplication.
+// We should find a way to avoid this code duplication.
 impl PointQuery<f32> for Capsule {
     #[inline]
     fn project_point(
@@ -122,7 +146,7 @@ impl PointQuery<f32> for Capsule {
         pt: &Point<f32>,
         solid: bool,
     ) -> PointProjection<f32> {
-        let seg = Segment::new(self.a, self.b);
+        let seg = Segment::new(self.segment.a, self.segment.b);
         let proj = seg.project_point(m, pt, solid);
         let dproj = *pt - proj.point;