Add rounded cylinder.

examples3d/heightfield3.rs

@@ -57,7 +57,9 @@ pub fn init_world(testbed: &mut Testbed) {
                 let collider = match j % 4 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    2 => ColliderBuilder::cylinder(rad, rad).build(),
+                    // Rounded cylinders are much more efficient that cylinder, even if the
+                    // rounding margin is small.
+                    2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(),
                     _ => ColliderBuilder::cone(rad, rad).build(),
                 };
 

examples3d/primitives3.rs

@@ -53,7 +53,9 @@ pub fn init_world(testbed: &mut Testbed) {
                 let collider = match j % 4 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    2 => ColliderBuilder::cylinder(rad, rad).build(),
+                    // Rounded cylinders are much more efficient that cylinder, even if the
+                    // rounding margin is small.
+                    2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(),
                     _ => ColliderBuilder::cone(rad, rad).build(),
                 };
 

examples3d/trimesh3.rs

@@ -67,7 +67,9 @@ pub fn init_world(testbed: &mut Testbed) {
                 let collider = match j % 4 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    2 => ColliderBuilder::cylinder(rad, rad).build(),
+                    // Rounded cylinders are much more efficient that cylinder, even if the
+                    // rounding margin is small.
+                    2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(),
                     _ => ColliderBuilder::cone(rad, rad).build(),
                 };
 

src/geometry/collider.rs

@@ -1,7 +1,7 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
     Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
-    Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh,
+    Proximity, Ray, RayIntersection, Rounded, Shape, ShapeType, Triangle, Trimesh,
 };
 #[cfg(feature = "dim3")]
 use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap};
@@ -40,6 +40,17 @@ impl ColliderShape {
         ColliderShape(Arc::new(Cylinder::new(half_height, radius)))
     }
 
+    /// Initialize a rounded cylindrical shape defined by its half-height
+    /// (along along the y axis), its radius, and its roundedness (the
+    /// radius of the sphere used for dilating the cylinder).
+    #[cfg(feature = "dim3")]
+    pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self {
+        ColliderShape(Arc::new(Rounded::new(
+            Cylinder::new(half_height, radius),
+            rounding_radius,
+        )))
+    }
+
     /// Initialize a cone shape defined by its half-height
     /// (along along the y axis) and its basis radius.
     #[cfg(feature = "dim3")]
@@ -127,13 +138,20 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
                     .next_element()?
                     .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
 
+                fn deser<'de, A, S: Shape + serde::Deserialize<'de>>(
+                    seq: &mut A,
+                ) -> Result<Arc<dyn Shape>, A::Error>
+                where
+                    A: serde::de::SeqAccess<'de>,
+                {
+                    let shape: S = seq.next_element()?.ok_or_else(|| {
+                        serde::de::Error::custom("Failed to deserialize builtin shape.")
+                    })?;
+                    Ok(Arc::new(shape) as Arc<dyn Shape>)
+                }
+
                 let shape = match ShapeType::from_i32(tag) {
-                    Some(ShapeType::Ball) => {
+                    Some(ShapeType::Ball) => deser::<A, Ball>(&mut seq)?,
-                        let shape: Ball = seq
-                            .next_element()?
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-                        Arc::new(shape) as Arc<dyn Shape>
-                    }
                     Some(ShapeType::Polygon) => {
                         unimplemented!()
                         // let shape: Polygon = seq
@@ -141,50 +159,17 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
                         //     .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
                         // Arc::new(shape) as Arc<dyn Shape>
                     }
-                    Some(ShapeType::Cuboid) => {
+                    Some(ShapeType::Cuboid) => deser::<A, Cuboid>(&mut seq)?,
-                        let shape: Cuboid = seq
+                    Some(ShapeType::Capsule) => deser::<A, Capsule>(&mut seq)?,
-                            .next_element()?
+                    Some(ShapeType::Triangle) => deser::<A, Triangle>(&mut seq)?,
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
+                    Some(ShapeType::Trimesh) => deser::<A, Trimesh>(&mut seq)?,
-                        Arc::new(shape) as Arc<dyn Shape>
+                    Some(ShapeType::HeightField) => deser::<A, HeightField>(&mut seq)?,
-                    }
-                    Some(ShapeType::Capsule) => {
-                        let shape: Capsule = seq
-                            .next_element()?
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-                        Arc::new(shape) as Arc<dyn Shape>
-                    }
-                    Some(ShapeType::Triangle) => {
-                        let shape: Triangle = seq
-                            .next_element()?
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-                        Arc::new(shape) as Arc<dyn Shape>
-                    }
-                    Some(ShapeType::Trimesh) => {
-                        let shape: Trimesh = seq
-                            .next_element()?
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-                        Arc::new(shape) as Arc<dyn Shape>
-                    }
-                    Some(ShapeType::HeightField) => {
-                        let shape: HeightField = seq
-                            .next_element()?
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-                        Arc::new(shape) as Arc<dyn Shape>
-                    }
                     #[cfg(feature = "dim3")]
-                    Some(ShapeType::Cylinder) => {
+                    Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
-                        let shape: Cylinder = seq
-                            .next_element()?
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-                        Arc::new(shape) as Arc<dyn Shape>
-                    }
                     #[cfg(feature = "dim3")]
-                    Some(ShapeType::Cone) => {
+                    Some(ShapeType::Cone) => deser::<A, Cone>(&mut seq)?,
-                        let shape: Cone = seq
+                    #[cfg(feature = "dim3")]
-                            .next_element()?
+                    Some(ShapeType::RoundedCylinder) => deser::<A, Rounded<Cylinder>>(&mut seq)?,
-                            .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-                        Arc::new(shape) as Arc<dyn Shape>
-                    }
                     None => {
                         return Err(serde::de::Error::custom(
                             "found invalid shape type to deserialize",
@@ -342,6 +327,18 @@ impl ColliderBuilder {
         Self::new(ColliderShape::cylinder(half_height, radius))
     }
 
+    /// Initialize a new collider builder with a rounded cylindrical shape defined by its half-height
+    /// (along along the y axis), its radius, and its roundedness (the
+    /// radius of the sphere used for dilating the cylinder).
+    #[cfg(feature = "dim3")]
+    pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self {
+        Self::new(ColliderShape::rounded_cylinder(
+            half_height,
+            radius,
+            rounding_radius,
+        ))
+    }
+
     /// Initialize a new collider builder with a cone shape defined by its half-height
     /// (along along the y axis) and its basis radius.
     #[cfg(feature = "dim3")]

src/geometry/contact_generator/contact_dispatcher.rs

@@ -69,16 +69,7 @@ impl ContactDispatcher for DefaultContactDispatcher {
                 },
                 None,
             ),
-            (ShapeType::Cuboid, ShapeType::Ball)
+            (_, ShapeType::Ball) | (ShapeType::Ball, _) => (
-            | (ShapeType::Ball, ShapeType::Cuboid)
-            | (ShapeType::Triangle, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Triangle)
-            | (ShapeType::Capsule, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Capsule)
-            | (ShapeType::Cylinder, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Cylinder)
-            | (ShapeType::Cone, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Cone) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_convex,
                     ..PrimitiveContactGenerator::default()
@@ -104,7 +95,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
             (ShapeType::Cylinder, _)
             | (_, ShapeType::Cylinder)
             | (ShapeType::Cone, _)
-            | (_, ShapeType::Cone) => (
+            | (_, ShapeType::Cone)
+            | (ShapeType::RoundedCylinder, _)
+            | (_, ShapeType::RoundedCylinder) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_pfm_pfm,
                     ..PrimitiveContactGenerator::default()

src/geometry/contact_generator/pfm_pfm_contact_generator.rs

@@ -29,11 +29,11 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace {
 }
 
 pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Some(pfm1), Some(pfm2)) = (
+    if let (Some((pfm1, round_radius1)), Some((pfm2, round_radius2))) = (
         ctxt.shape1.as_polygonal_feature_map(),
         ctxt.shape2.as_polygonal_feature_map(),
     ) {
-        do_generate_contacts(pfm1, pfm2, ctxt);
+        do_generate_contacts(pfm1, round_radius1, pfm2, round_radius2, ctxt);
         ctxt.manifold.update_warmstart_multiplier();
         ctxt.manifold.sort_contacts(ctxt.prediction_distance);
     }
@@ -41,7 +41,9 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
 
 fn do_generate_contacts(
     pfm1: &dyn PolygonalFeatureMap,
+    round_radius1: f32,
     pfm2: &dyn PolygonalFeatureMap,
+    round_radius2: f32,
     ctxt: &mut PrimitiveContactGenerationContext,
 ) {
     let pos12 = ctxt.position1.inverse() * ctxt.position2;
@@ -64,12 +66,13 @@ fn do_generate_contacts(
         .downcast_mut()
         .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
 
+    let total_prediction = ctxt.prediction_distance + round_radius1 + round_radius2;
     let contact = query::contact_support_map_support_map_with_params(
         &Isometry::identity(),
         pfm1,
         &pos12,
         pfm2,
-        ctxt.prediction_distance,
+        total_prediction,
         &mut workspace.simplex,
         workspace.last_gjk_dir,
     );
@@ -87,7 +90,7 @@ fn do_generate_contacts(
             workspace.feature2.transform_by(&pos12);
 
             PolyhedronFace::contacts(
-                ctxt.prediction_distance,
+                total_prediction,
                 &workspace.feature1,
                 &normal1,
                 &workspace.feature2,
@@ -95,6 +98,14 @@ fn do_generate_contacts(
                 ctxt.manifold,
             );
 
+            if round_radius1 != 0.0 || round_radius2 != 0.0 {
+                for contact in &mut ctxt.manifold.points {
+                    contact.local_p1 += *normal1 * round_radius1;
+                    contact.local_p2 += *normal2 * round_radius2;
+                    contact.dist -= round_radius1 + round_radius2;
+                }
+            }
+
             // Adjust points to take the radius into account.
             ctxt.manifold.local_n1 = *normal1;
             ctxt.manifold.local_n2 = *normal2;

src/geometry/mod.rs

@@ -18,7 +18,7 @@ pub use self::narrow_phase::NarrowPhase;
 pub use self::polygon::Polygon;
 pub use self::proximity::ProximityPair;
 pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher};
-pub use self::rounded::Rounded;
+pub use self::rounded::{Roundable, Rounded};
 pub use self::trimesh::Trimesh;
 pub use ncollide::query::Proximity;
 

src/geometry/rounded.rs

@@ -1,7 +1,115 @@
+use crate::geometry::{Cylinder, ShapeType};
+use crate::math::{Isometry, Point, Vector};
+use na::Unit;
+use ncollide::query::{
+    algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection,
+};
+use ncollide::shape::{FeatureId, SupportMap};
+
+/// A shape which corner can be rounded.
+pub trait Roundable {
+    /// The ShapeType fo this shape after rounding its corners.
+    fn rounded_shape_type() -> ShapeType;
+}
+
+impl Roundable for Cylinder {
+    fn rounded_shape_type() -> ShapeType {
+        ShapeType::RoundedCylinder
+    }
+}
+
 /// A rounded shape.
-pub struct Rounded<S> {
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Rounded<S: Roundable> {
     /// The shape being rounded.
     pub shape: S,
     /// The rounding radius.
     pub radius: f32,
 }
+
+impl<S: Roundable> Rounded<S> {
+    /// Create sa new shape where all its edges and vertices are rounded by a radius of `radius`.
+    ///
+    /// This is done by applying a dilation of the given radius to the shape.
+    pub fn new(shape: S, radius: f32) -> Self {
+        Self { shape, radius }
+    }
+}
+
+impl<S: Roundable + SupportMap<f32>> SupportMap<f32> for Rounded<S> {
+    fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
+        self.local_support_point_toward(&Unit::new_normalize(*dir))
+    }
+
+    fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
+        self.shape.local_support_point_toward(dir) + **dir * self.radius
+    }
+
+    fn support_point(&self, transform: &Isometry<f32>, dir: &Vector<f32>) -> Point<f32> {
+        let local_dir = transform.inverse_transform_vector(dir);
+        transform * self.local_support_point(&local_dir)
+    }
+
+    fn support_point_toward(
+        &self,
+        transform: &Isometry<f32>,
+        dir: &Unit<Vector<f32>>,
+    ) -> Point<f32> {
+        let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir));
+        transform * self.local_support_point_toward(&local_dir)
+    }
+}
+
+impl<S: Roundable + SupportMap<f32>> RayCast<f32> for Rounded<S> {
+    fn toi_and_normal_with_ray(
+        &self,
+        m: &Isometry<f32>,
+        ray: &Ray<f32>,
+        max_toi: f32,
+        solid: bool,
+    ) -> Option<RayIntersection<f32>> {
+        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: if PointQuery had a `project_point_with_normal` method, we could just
+// call this and adjust the projected point accordingly.
+impl<S: Roundable + SupportMap<f32>> PointQuery<f32> for Rounded<S> {
+    #[inline]
+    fn project_point(
+        &self,
+        m: &Isometry<f32>,
+        point: &Point<f32>,
+        solid: bool,
+    ) -> PointProjection<f32> {
+        ncollide::query::point_projection_on_support_map(
+            m,
+            self,
+            &mut VoronoiSimplex::new(),
+            point,
+            solid,
+        )
+    }
+
+    #[inline]
+    fn project_point_with_feature(
+        &self,
+        m: &Isometry<f32>,
+        point: &Point<f32>,
+    ) -> (PointProjection<f32>, FeatureId) {
+        (self.project_point(m, point, false), FeatureId::Unknown)
+    }
+}

src/geometry/shape.rs

@@ -1,7 +1,7 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
     Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
-    Proximity, Ray, RayIntersection, Triangle, Trimesh,
+    Proximity, Ray, RayIntersection, Roundable, Rounded, Triangle, Trimesh,
 };
 #[cfg(feature = "dim3")]
 use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap};
@@ -9,7 +9,7 @@ use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use downcast_rs::{impl_downcast, DowncastSync};
 use erased_serde::Serialize;
 use na::Point3;
-use ncollide::bounding_volume::{HasBoundingVolume, AABB};
+use ncollide::bounding_volume::{BoundingVolume, HasBoundingVolume, AABB};
 use ncollide::query::{PointQuery, RayCast};
 use num::Zero;
 use num_derive::FromPrimitive;
@@ -40,6 +40,18 @@ pub enum ShapeType {
     Cone,
     // /// A custom shape type.
     // Custom(u8),
+    // /// A cuboid with rounded corners.
+    // RoundedCuboid,
+    // /// A triangle with rounded corners.
+    // RoundedTriangle,
+    // /// A triangle-mesh with rounded corners.
+    // RoundedTrimesh,
+    // /// An heightfield with rounded corners.
+    // RoundedHeightField,
+    /// A cylinder with rounded corners.
+    RoundedCylinder,
+    // /// A cone with rounded corners.
+    // RoundedCone,
 }
 
 /// Trait implemented by shapes usable by Rapier.
@@ -61,7 +73,7 @@ pub trait Shape: RayCast<f32> + PointQuery<f32> + DowncastSync {
 
     /// Converts this shape to a polygonal feature-map, if it is one.
     #[cfg(feature = "dim3")]
-    fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
         None
     }
 
@@ -112,6 +124,15 @@ impl dyn Shape {
     pub fn as_cone(&self) -> Option<&Cone> {
         self.downcast_ref()
     }
+
+    /// Converts this abstract shape to a cone, if it is one.
+    pub fn as_rounded<S>(&self) -> Option<&Rounded<S>>
+    where
+        S: Roundable,
+        Rounded<S>: Shape,
+    {
+        self.downcast_ref()
+    }
 }
 
 impl Shape for Ball {
@@ -171,8 +192,8 @@ impl Shape for Cuboid {
     }
 
     #[cfg(feature = "dim3")]
-    fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
-        Some(self as &dyn PolygonalFeatureMap)
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
     }
 }
 
@@ -214,8 +235,8 @@ impl Shape for Triangle {
     }
 
     #[cfg(feature = "dim3")]
-    fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
-        Some(self as &dyn PolygonalFeatureMap)
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
     }
 }
 
@@ -277,8 +298,8 @@ impl Shape for Cylinder {
     }
 
     #[cfg(feature = "dim3")]
-    fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
-        Some(self as &dyn PolygonalFeatureMap)
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
     }
 }
 
@@ -302,7 +323,32 @@ impl Shape for Cone {
     }
 
     #[cfg(feature = "dim3")]
-    fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
-        Some(self as &dyn PolygonalFeatureMap)
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl Shape for Rounded<Cylinder> {
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.shape.compute_aabb(position).loosened(self.radius)
+    }
+
+    fn mass_properties(&self, density: f32) -> MassProperties {
+        // We ignore the margin here.
+        self.shape.mass_properties(density)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::RoundedCylinder
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((&self.shape as &dyn PolygonalFeatureMap, self.radius))
     }
 }

src_testbed/engine.rs

@@ -411,7 +411,7 @@ impl GraphicsManager {
         }
 
         #[cfg(feature = "dim3")]
-        if let Some(cylinder) = shape.as_cylinder() {
+        if let Some(cylinder) = shape.as_cylinder().or(shape.as_rounded().map(|r| &r.shape)) {
             out.push(Node::Cylinder(Cylinder::new(
                 handle,
                 cylinder.half_height,