Redefine capsules as a segment with a radius, allowing us to reuse the pfm_pfm_contact generator for it.

examples3d/heightfield3.rs

@@ -54,13 +54,14 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                let collider = match j % 4 {
+                let collider = match j % 5 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
                     // Rounded cylinders are much more efficient that cylinder, even if the
                     // rounding margin is small.
                     2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
-                    _ => ColliderBuilder::cone(rad, rad).build(),
+                    3 => ColliderBuilder::cone(rad, rad).build(),
+                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);

examples3d/primitives3.rs

@@ -50,13 +50,14 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                let collider = match j % 4 {
+                let collider = match j % 5 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
                     // Rounded cylinders are much more efficient that cylinder, even if the
                     // rounding margin is small.
                     2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
-                    _ => ColliderBuilder::cone(rad, rad).build(),
+                    3 => ColliderBuilder::cone(rad, rad).build(),
+                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);

examples3d/trimesh3.rs

@@ -64,13 +64,14 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                let collider = match j % 4 {
+                let collider = match j % 5 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
                     // Rounded cylinders are much more efficient that cylinder, even if the
                     // rounding margin is small.
                     2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
-                    _ => ColliderBuilder::cone(rad, rad).build(),
+                    3 => ColliderBuilder::cone(rad, rad).build(),
+                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);

src/dynamics/mass_properties_capsule.rs

@@ -1,22 +1,24 @@
 use crate::dynamics::MassProperties;
 use crate::math::Point;
 #[cfg(feature = "dim3")]
-use crate::math::Rotation;
+use crate::{geometry::Capsule, math::Rotation};
 
 impl MassProperties {
-    pub(crate) fn from_capsule(density: f32, half_height: f32, radius: f32) -> Self {
+    pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
+        let half_height = (b - a).norm() / 2.0;
         let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
         let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius);
         let cap_vol = cyl_vol + ball_vol;
         let cap_mass = cap_vol * density;
         let mut cap_unit_i = cyl_unit_i + ball_unit_i;
+        let local_com = na::center(&a, &b);
 
         #[cfg(feature = "dim2")]
         {
             let h = half_height * 2.0;
             let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
             cap_unit_i += extra;
-            Self::new(Point::origin(), cap_mass, cap_unit_i * cap_mass)
+            Self::new(local_com, cap_mass, cap_unit_i * cap_mass)
         }
 
         #[cfg(feature = "dim3")]
@@ -25,11 +27,12 @@ impl MassProperties {
             let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
             cap_unit_i.x += extra;
             cap_unit_i.z += extra;
+            let local_frame = Capsule::new(a, b, radius).rotation_wrt_y();
             Self::with_principal_inertia_frame(
-                Point::origin(),
+                local_com,
                 cap_mass,
                 cap_unit_i * cap_mass,
-                Rotation::identity(),
+                local_frame,
             )
         }
     }

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::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast};
-use ncollide::shape::{FeatureId, Segment};
+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.
+    /// The axis and endpoint of the capsule.
-    pub a: Point<f32>,
+    pub segment: Segment<f32>,
-    /// The second enpdoint of the capsule.
-    pub b: Point<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 a = pos * self.segment.a;
-        let b = pos * self.b;
+        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 {
+impl SupportMap<f32> for Capsule {
-//     fn local_support_point(&self, dir: &Vector) -> Point {
+    fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
-//         let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis());
+        let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis());
-//         self.local_support_point_toward(&dir)
+        self.local_support_point_toward(&dir)
-//     }
+    }
-//
+
-//     fn local_support_point_toward(&self, dir: &Unit<Vector>) -> Point {
+    fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
-//         if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) {
+        if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) {
-//             self.a + **dir * self.radius
+            self.segment.a + **dir * self.radius
-//         } else {
+        } else {
-//             self.b + **dir * self.radius
+            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;
 

src/geometry/collider.rs

@@ -1,7 +1,7 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
     Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Proximity,
-    Shape, ShapeType, Triangle, Trimesh,
+    Segment, Shape, ShapeType, Triangle, Trimesh,
 };
 #[cfg(feature = "dim3")]
 use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, Rounded};
@@ -58,9 +58,14 @@ impl ColliderShape {
         ColliderShape(Arc::new(Cuboid::new(half_extents)))
     }
 
-    /// Initialize a capsule shape aligned with the `y` axis.
+    /// Initialize a capsule shape from its endpoints and radius.
-    pub fn capsule(half_height: f32, radius: f32) -> Self {
+    pub fn capsule(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
-        ColliderShape(Arc::new(Capsule::new(half_height, radius)))
+        ColliderShape(Arc::new(Capsule::new(a, b, radius)))
+    }
+
+    /// Initialize a segment shape from its endpoints.
+    pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
+        ColliderShape(Arc::new(Segment::new(a, b)))
     }
 
     /// Initializes a triangle shape.
@@ -157,6 +162,7 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
                     Some(ShapeType::Cuboid) => deser::<A, Cuboid>(&mut seq)?,
                     Some(ShapeType::Capsule) => deser::<A, Capsule>(&mut seq)?,
                     Some(ShapeType::Triangle) => deser::<A, Triangle>(&mut seq)?,
+                    Some(ShapeType::Segment) => deser::<A, Segment>(&mut seq)?,
                     Some(ShapeType::Trimesh) => deser::<A, Trimesh>(&mut seq)?,
                     Some(ShapeType::HeightField) => deser::<A, HeightField>(&mut seq)?,
                     #[cfg(feature = "dim3")]
@@ -349,25 +355,21 @@ impl ColliderBuilder {
 
     /// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
     pub fn capsule_x(half_height: f32, radius: f32) -> Self {
-        #[cfg(feature = "dim2")]
+        let p = Point::from(Vector::x() * half_height);
-        let rot = -std::f32::consts::FRAC_PI_2;
+        Self::new(ColliderShape::capsule(-p, p, radius))
-        #[cfg(feature = "dim3")]
-        let rot = Vector::z() * -std::f32::consts::FRAC_PI_2;
-        Self::new(ColliderShape::capsule(half_height, radius))
-            .position(Isometry::new(na::zero(), rot))
     }
 
     /// Initialize a new collider builder with a capsule shape aligned with the `y` axis.
     pub fn capsule_y(half_height: f32, radius: f32) -> Self {
-        Self::new(ColliderShape::capsule(half_height, radius))
+        let p = Point::from(Vector::y() * half_height);
+        Self::new(ColliderShape::capsule(-p, p, radius))
     }
 
     /// Initialize a new collider builder with a capsule shape aligned with the `z` axis.
     #[cfg(feature = "dim3")]
     pub fn capsule_z(half_height: f32, radius: f32) -> Self {
-        let rot = Vector::x() * std::f32::consts::FRAC_PI_2;
+        let p = Point::from(Vector::z() * half_height);
-        Self::new(ColliderShape::capsule(half_height, radius))
+        Self::new(ColliderShape::capsule(-p, p, radius))
-            .position(Isometry::new(na::zero(), rot))
     }
 
     /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
@@ -377,11 +379,8 @@ impl ColliderBuilder {
     }
 
     /// Initializes a collider builder with a segment shape.
-    ///
-    /// A segment shape is modeled by a capsule with a 0 radius.
     pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
-        let (pos, half_height) = crate::utils::segment_to_capsule(&a, &b);
+        Self::new(ColliderShape::segment(a, b))
-        Self::new(ColliderShape::capsule(half_height, 0.0)).position(pos)
     }
 
     /// Initializes a collider builder with a triangle shape.

src/geometry/contact_generator/capsule_capsule_contact_generator.rs

@@ -154,8 +154,8 @@ pub fn generate_contacts<'a>(
     let pos12 = pos1.inverse() * pos2;
     let pos21 = pos12.inverse();
 
-    let seg1 = capsule1.segment();
+    let seg1 = capsule1.segment;
-    let seg2_1 = capsule2.segment().transformed(&pos12);
+    let seg2_1 = capsule2.segment.transformed(&pos12);
     let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
         (&seg1.a, &seg1.b),
         (&seg2_1.a, &seg2_1.b),

src/geometry/contact_generator/contact_dispatcher.rs

@@ -99,7 +99,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
             | (ShapeType::Cone, _)
             | (_, ShapeType::Cone)
             | (ShapeType::RoundCylinder, _)
-            | (_, ShapeType::RoundCylinder) => (
+            | (_, ShapeType::RoundCylinder)
+            | (ShapeType::Capsule, _)
+            | (_, ShapeType::Capsule) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_pfm_pfm,
                     ..PrimitiveContactGenerator::default()

src/geometry/contact_generator/cuboid_capsule_contact_generator.rs

@@ -54,7 +54,7 @@ pub fn generate_contacts<'a>(
         return;
     }
 
-    let segment2 = capsule2.segment();
+    let segment2 = capsule2.segment;
 
     /*
      *

src/geometry/mod.rs

@@ -1,6 +1,7 @@
 //! Structures related to geometry: colliders, shapes, etc.
 
 pub use self::broad_phase_multi_sap::BroadPhase;
+pub use self::capsule::Capsule;
 pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
 pub use self::collider_set::{ColliderHandle, ColliderSet};
 pub use self::contact::{
@@ -22,8 +23,8 @@ pub use self::rounded::{Roundable, Rounded};
 pub use self::trimesh::Trimesh;
 pub use ncollide::query::Proximity;
 
-/// A capsule shape.
+/// A segment shape.
-pub type Capsule = ncollide::shape::Capsule<f32>;
+pub type Segment = ncollide::shape::Segment<f32>;
 /// A cuboid shape.
 pub type Cuboid = ncollide::shape::Cuboid<f32>;
 /// A triangle shape.
@@ -94,6 +95,7 @@ mod trimesh;
 mod waabb;
 mod wquadtree;
 //mod z_order;
+mod capsule;
 #[cfg(feature = "dim3")]
 mod polygonal_feature_map;
 mod rounded;

src/geometry/polygonal_feature_map.rs

@@ -1,9 +1,8 @@
 use crate::geometry::PolyhedronFace;
-use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle};
+use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Segment, Triangle};
 use crate::math::{Point, Vector};
 use approx::AbsDiffEq;
 use na::{Unit, Vector2};
-use ncollide::shape::Segment;
 use ncollide::shape::SupportMap;
 
 /// Trait implemented by convex shapes with features with polyhedral approximations.
@@ -11,7 +10,7 @@ pub trait PolygonalFeatureMap: SupportMap<f32> {
     fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace);
 }
 
-impl PolygonalFeatureMap for Segment<f32> {
+impl PolygonalFeatureMap for Segment {
     fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
         *out_feature = PolyhedronFace::from(*self);
     }

src/geometry/polyhedron_feature3d.rs

@@ -40,6 +40,8 @@ impl From<Triangle> for PolyhedronFace {
 
 impl From<Segment<f32>> for PolyhedronFace {
     fn from(seg: Segment<f32>) -> Self {
+        // Vertices have feature ids 0 and 2.
+        // The segment interior has feature id 1.
         Self {
             vertices: [seg.a, seg.b, seg.b, seg.b],
             vids: [0, 2, 2, 2],

src/geometry/shape.rs

@@ -1,5 +1,7 @@
 use crate::dynamics::MassProperties;
-use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Triangle, Trimesh};
+use crate::geometry::{
+    Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Segment, Triangle, Trimesh,
+};
 use crate::math::Isometry;
 use downcast_rs::{impl_downcast, DowncastSync};
 use erased_serde::Serialize;
@@ -24,6 +26,8 @@ pub enum ShapeType {
     Cuboid,
     /// A capsule shape.
     Capsule,
+    /// A segment shape.
+    Segment,
     /// A triangle shape.
     Triangle,
     /// A triangle mesh shape.
@@ -205,16 +209,20 @@ impl Shape for Capsule {
     }
 
     fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
-        self.bounding_volume(position)
+        self.aabb(position)
     }
 
     fn mass_properties(&self, density: f32) -> MassProperties {
-        MassProperties::from_capsule(density, self.half_height, self.radius)
+        MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius)
     }
 
     fn shape_type(&self) -> ShapeType {
         ShapeType::Capsule
     }
+
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((&self.segment as &dyn PolygonalFeatureMap, self.radius))
+    }
 }
 
 impl Shape for Triangle {
@@ -241,6 +249,30 @@ impl Shape for Triangle {
     }
 }
 
+impl Shape for Segment {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, _density: f32) -> MassProperties {
+        MassProperties::zero()
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Segment
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+}
+
 impl Shape for Trimesh {
     #[cfg(feature = "serde-serialize")]
     fn as_serialize(&self) -> Option<&dyn Serialize> {

src_testbed/nphysics_backend.rs

@@ -184,7 +184,8 @@ fn nphysics_collider_from_rapier_collider(
     } else if let Some(ball) = shape.as_ball() {
         ShapeHandle::new(Ball::new(ball.radius - margin))
     } else if let Some(capsule) = shape.as_capsule() {
-        ShapeHandle::new(Capsule::new(capsule.half_height, capsule.radius))
+        pos *= capsule.transform_wrt_y();
+        ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
     } else if let Some(heightfield) = shape.as_heightfield() {
         ShapeHandle::new(heightfield.clone())
     } else {

src_testbed/objects/capsule.rs

@@ -19,7 +19,7 @@ impl Capsule {
         window: &mut window::Window,
     ) -> Capsule {
         let r = capsule.radius;
-        let h = capsule.half_height * 2.0;
+        let h = capsule.half_height() * 2.0;
         #[cfg(feature = "dim2")]
         let node = window.add_planar_capsule(r, h);
         #[cfg(feature = "dim3")]

src_testbed/physx_backend.rs

@@ -433,8 +433,15 @@ fn physx_collider_from_rapier_collider(
     } else if let Some(ball) = shape.as_ball() {
         ColliderDesc::Sphere(ball.radius)
     } else if let Some(capsule) = shape.as_capsule() {
-        let rot = UnitQuaternion::rotation_between(&Vector3::x(), &Vector3::y());
+        let center = capsule.center();
-        local_pose *= rot.unwrap_or(UnitQuaternion::identity());
+        let mut dir = capsule.segment.b - capsule.segment.a;
+
+        if dir.x < 0.0 {
+            dir = -dir;
+        }
+
+        let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
+        local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
         ColliderDesc::Capsule(capsule.radius, capsule.height())
     } else if let Some(trimesh) = shape.as_trimesh() {
         ColliderDesc::TriMesh {