Add cone support.

15 files changed, 257 insertions, 18 deletions

diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs
index 826df4c..2f26dde 100644
--- a/examples3d/heightfield3.rs
+++ b/examples3d/heightfield3.rs
@@ -54,10 +54,11 @@ 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 % 3 {
+                let collider = match j % 4 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    _ => ColliderBuilder::cylinder(rad, rad).build(),
+                    2 => ColliderBuilder::cylinder(rad, rad).build(),
+                    _ => ColliderBuilder::cone(rad, rad).build(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);
diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs
index c3fa799..4e2fc19 100644
--- a/examples3d/primitives3.rs
+++ b/examples3d/primitives3.rs
@@ -50,10 +50,11 @@ 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 % 3 {
+                let collider = match j % 4 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    _ => ColliderBuilder::cylinder(rad, rad).build(),
+                    2 => ColliderBuilder::cylinder(rad, rad).build(),
+                    _ => ColliderBuilder::cone(rad, rad).build(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);
diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs
index 40aede2..2d6c0bb 100644
--- a/examples3d/trimesh3.rs
+++ b/examples3d/trimesh3.rs
@@ -64,10 +64,11 @@ 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 % 3 {
+                let collider = match j % 4 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    _ => ColliderBuilder::cylinder(rad, rad).build(),
+                    2 => ColliderBuilder::cylinder(rad, rad).build(),
+                    _ => ColliderBuilder::cone(rad, rad).build(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);
diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs
new file mode 100644
index 0000000..0fb61b6
--- /dev/null
+++ b/src/dynamics/mass_properties_cone.rs
@@ -0,0 +1,30 @@
+use crate::dynamics::MassProperties;
+use crate::geometry::Cone;
+use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
+
+impl MassProperties {
+    pub(crate) fn cone_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0;
+        let sq_radius = radius * radius;
+        let sq_height = half_height * half_height * 4.0;
+        let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0;
+        let principal = sq_radius * 3.0 / 10.0;
+
+        (volume, Vector::new(off_principal, principal, off_principal))
+    }
+
+    pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self {
+        let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius);
+        let cyl_mass = cyl_vol * density;
+
+        Self::with_principal_inertia_frame(
+            Point::new(0.0, -half_height / 2.0, 0.0),
+            cyl_mass,
+            cyl_unit_i * cyl_mass,
+            Rotation::identity(),
+        )
+    }
+}
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 512bd8b..10cdd29 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -22,6 +22,8 @@ mod joint;
 mod mass_properties;
 mod mass_properties_ball;
 mod mass_properties_capsule;
+#[cfg(feature = "dim3")]
+mod mass_properties_cone;
 mod mass_properties_cuboid;
 mod mass_properties_cylinder;
 #[cfg(feature = "dim2")]
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index c6cf6d0..f952b15 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,10 +1,10 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
-#[cfg(feature = "dim3")]
-use crate::geometry::PolygonalFeatureMap;
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph,
-    Polygon, Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh,
+    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
+    Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh,
 };
+#[cfg(feature = "dim3")]
+use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap};
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use downcast_rs::{impl_downcast, DowncastSync};
 use erased_serde::Serialize;
@@ -40,6 +40,13 @@ impl ColliderShape {
         ColliderShape(Arc::new(Cylinder::new(half_height, radius)))
     }
 
+    /// Initialize a cone shape defined by its half-height
+    /// (along along the y axis) and its basis radius.
+    #[cfg(feature = "dim3")]
+    pub fn cone(half_height: f32, radius: f32) -> Self {
+        ColliderShape(Arc::new(Cone::new(half_height, radius)))
+    }
+
     /// Initialize a cuboid shape defined by its half-extents.
     pub fn cuboid(half_extents: Vector<f32>) -> Self {
         ColliderShape(Arc::new(Cuboid::new(half_extents)))
@@ -171,6 +178,13 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
                             .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
                         Arc::new(shape) as Arc<dyn Shape>
                     }
+                    #[cfg(feature = "dim3")]
+                    Some(ShapeType::Cone) => {
+                        let shape: Cone = seq
+                            .next_element()?
+                            .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",
@@ -328,6 +342,13 @@ impl ColliderBuilder {
         Self::new(ColliderShape::cylinder(half_height, 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")]
+    pub fn cone(half_height: f32, radius: f32) -> Self {
+        Self::new(ColliderShape::cone(half_height, radius))
+    }
+
     /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
     #[cfg(feature = "dim2")]
     pub fn cuboid(hx: f32, hy: f32) -> Self {
diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs
index 01bbc46..70ac84c 100644
--- a/src/geometry/contact_generator/contact_dispatcher.rs
+++ b/src/geometry/contact_generator/contact_dispatcher.rs
@@ -76,7 +76,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
             | (ShapeType::Capsule, ShapeType::Ball)
             | (ShapeType::Ball, ShapeType::Capsule)
             | (ShapeType::Cylinder, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Cylinder) => (
+            | (ShapeType::Ball, ShapeType::Cylinder)
+            | (ShapeType::Cone, ShapeType::Ball)
+            | (ShapeType::Ball, ShapeType::Cone) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_convex,
                     ..PrimitiveContactGenerator::default()
@@ -99,7 +101,10 @@ impl ContactDispatcher for DefaultContactDispatcher {
                     None,
                 )
             }
-            (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) => (
+            (ShapeType::Cylinder, _)
+            | (_, ShapeType::Cylinder)
+            | (ShapeType::Cone, _)
+            | (_, ShapeType::Cone) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_pfm_pfm,
                     ..PrimitiveContactGenerator::default()
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 71573ed..d569248 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -35,6 +35,9 @@ pub type HeightField = ncollide::shape::HeightField<f32>;
 /// A cylindrical shape.
 #[cfg(feature = "dim3")]
 pub type Cylinder = ncollide::shape::Cylinder<f32>;
+/// A cone shape.
+#[cfg(feature = "dim3")]
+pub type Cone = ncollide::shape::Cone<f32>;
 /// An axis-aligned bounding box.
 pub type AABB = ncollide::bounding_volume::AABB<f32>;
 /// Event triggered when two non-sensor colliders start or stop being in contact.
diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs
index 70be5d0..fc5e066 100644
--- a/src/geometry/polygonal_feature_map.rs
+++ b/src/geometry/polygonal_feature_map.rs
@@ -1,5 +1,5 @@
 use crate::geometry::PolyhedronFace;
-use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle};
+use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle};
 use crate::math::{Point, Vector};
 use approx::AbsDiffEq;
 use na::{Unit, Vector2, Vector3};
@@ -85,3 +85,49 @@ impl PolygonalFeatureMap for Cylinder {
         }
     }
 }
+
+impl PolygonalFeatureMap for Cone {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
+        // About feature ids. It is very similar to the feature ids of cylinders.
+        // At all times, we consider our cone to be approximated as follows:
+        // - The curved part is approximated by a single segment.
+        // - The flat cap of the cone is approximated by a square.
+        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
+        //   `y` coordinate has an ID of 1.
+        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
+        //   when looking at the cap with an eye looking towards +y).
+        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
+        // - The bottom cap has its face feature ID of 9.
+        // - Note that at all times, one of the cap's vertices are the same as the curved-part
+        //   segment endpoints.
+        let dir2 = Vector2::new(dir.x, dir.z)
+            .try_normalize(f32::default_epsilon())
+            .unwrap_or(Vector2::x());
+
+        if dir.y > 0.0 {
+            // We return a segment lying on the cone's curved part.
+            out_features.vertices[0] = Point::new(
+                dir2.x * self.radius,
+                -self.half_height,
+                dir2.y * self.radius,
+            );
+            out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0);
+            out_features.eids = [0, 0, 0, 0];
+            out_features.fid = 0;
+            out_features.num_vertices = 2;
+            out_features.vids = [1, 11, 11, 11];
+        } else {
+            // We return a square approximation of the cone cap.
+            let y = -self.half_height;
+            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
+            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
+            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
+            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
+
+            out_features.eids = [2, 4, 6, 8];
+            out_features.fid = 9;
+            out_features.num_vertices = 4;
+            out_features.vids = [1, 3, 5, 7];
+        }
+    }
+}
diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs
index c9e0a3a..b972a3e 100644
--- a/src/geometry/shape.rs
+++ b/src/geometry/shape.rs
@@ -1,10 +1,10 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
-#[cfg(feature = "dim3")]
-use crate::geometry::PolygonalFeatureMap;
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph,
-    Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh,
+    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
+    Proximity, Ray, RayIntersection, Triangle, Trimesh,
 };
+#[cfg(feature = "dim3")]
+use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap};
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use downcast_rs::{impl_downcast, DowncastSync};
 use erased_serde::Serialize;
@@ -35,6 +35,9 @@ pub enum ShapeType {
     #[cfg(feature = "dim3")]
     /// A cylindrical shape.
     Cylinder,
+    #[cfg(feature = "dim3")]
+    /// A cylindrical shape.
+    Cone,
     // /// A custom shape type.
     // Custom(u8),
 }
@@ -104,6 +107,11 @@ impl dyn Shape {
     pub fn as_cylinder(&self) -> Option<&Cylinder> {
         self.downcast_ref()
     }
+
+    /// Converts this abstract shape to a cone, if it is one.
+    pub fn as_cone(&self) -> Option<&Cone> {
+        self.downcast_ref()
+    }
 }
 
 impl Shape for Ball {
@@ -273,3 +281,28 @@ impl Shape for Cylinder {
         Some(self as &dyn PolygonalFeatureMap)
     }
 }
+
+#[cfg(feature = "dim3")]
+impl Shape for Cone {
+    #[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::from_cone(density, self.half_height, self.radius)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Cone
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+        Some(self as &dyn PolygonalFeatureMap)
+    }
+}
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index ca1a2b8..1908745 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -26,6 +26,7 @@ use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
 //#[cfg(feature = "fluids")]
 //use crate::objects::FluidRenderingMode;
 use crate::objects::capsule::Capsule;
+use crate::objects::cone::Cone;
 use crate::objects::cylinder::Cylinder;
 use crate::objects::mesh::Mesh;
 use rand::{Rng, SeedableRng};
@@ -419,6 +420,17 @@ impl GraphicsManager {
                 window,
             )))
         }
+
+        #[cfg(feature = "dim3")]
+        if let Some(cone) = shape.as_cone() {
+            out.push(Node::Cone(Cone::new(
+                handle,
+                cone.half_height,
+                cone.radius,
+                color,
+                window,
+            )))
+        }
     }
 
     /*
diff --git a/src_testbed/objects/capsule.rs b/src_testbed/objects/capsule.rs
index 23160be..f285b81 100644
--- a/src_testbed/objects/capsule.rs
+++ b/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")]
diff --git a/src_testbed/objects/cone.rs b/src_testbed/objects/cone.rs
new file mode 100644
index 0000000..58b014f
--- /dev/null
+++ b/src_testbed/objects/cone.rs
@@ -0,0 +1,74 @@
+use crate::objects::node::{self, GraphicsNode};
+use kiss3d::window::Window;
+use na::Point3;
+use rapier::geometry::{ColliderHandle, ColliderSet};
+use rapier::math::Isometry;
+
+pub struct Cone {
+    color: Point3<f32>,
+    base_color: Point3<f32>,
+    gfx: GraphicsNode,
+    collider: ColliderHandle,
+}
+
+impl Cone {
+    pub fn new(
+        collider: ColliderHandle,
+        half_height: f32,
+        radius: f32,
+        color: Point3<f32>,
+        window: &mut Window,
+    ) -> Cone {
+        #[cfg(feature = "dim2")]
+        let node = window.add_rectangle(radius, half_height);
+        #[cfg(feature = "dim3")]
+        let node = window.add_cone(radius, half_height * 2.0);
+
+        let mut res = Cone {
+            color,
+            base_color: color,
+            gfx: node,
+            collider,
+        };
+
+        // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
+        res.gfx.set_color(color.x, color.y, color.z);
+        res
+    }
+
+    pub fn select(&mut self) {
+        self.color = Point3::new(1.0, 0.0, 0.0);
+    }
+
+    pub fn unselect(&mut self) {
+        self.color = self.base_color;
+    }
+
+    pub fn set_color(&mut self, color: Point3<f32>) {
+        self.gfx.set_color(color.x, color.y, color.z);
+        self.color = color;
+        self.base_color = color;
+    }
+
+    pub fn update(&mut self, colliders: &ColliderSet) {
+        node::update_scene_node(
+            &mut self.gfx,
+            colliders,
+            self.collider,
+            &self.color,
+            &Isometry::identity(),
+        );
+    }
+
+    pub fn scene_node(&self) -> &GraphicsNode {
+        &self.gfx
+    }
+
+    pub fn scene_node_mut(&mut self) -> &mut GraphicsNode {
+        &mut self.gfx
+    }
+
+    pub fn object(&self) -> ColliderHandle {
+        self.collider
+    }
+}
diff --git a/src_testbed/objects/mod.rs b/src_testbed/objects/mod.rs
index 51db9d4..e4d7bc4 100644
--- a/src_testbed/objects/mod.rs
+++ b/src_testbed/objects/mod.rs
@@ -1,6 +1,7 @@
 pub mod ball;
 pub mod box_node;
 pub mod capsule;
+pub mod cone;
 pub mod convex;
 pub mod cylinder;
 pub mod heightfield;
diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs
index 14668e8..d1de799 100644
--- a/src_testbed/objects/node.rs
+++ b/src_testbed/objects/node.rs
@@ -10,6 +10,7 @@ use crate::objects::mesh::Mesh;
 use kiss3d::window::Window;
 use na::Point3;
 
+use crate::objects::cone::Cone;
 use crate::objects::cylinder::Cylinder;
 use rapier::geometry::{ColliderHandle, ColliderSet};
 use rapier::math::Isometry;
@@ -30,6 +31,7 @@ pub enum Node {
     Mesh(Mesh),
     Convex(Convex),
     Cylinder(Cylinder),
+    Cone(Cone),
 }
 
 impl Node {
@@ -45,6 +47,7 @@ impl Node {
             Node::Mesh(ref mut n) => n.select(),
             Node::Convex(ref mut n) => n.select(),
             Node::Cylinder(ref mut n) => n.select(),
+            Node::Cone(ref mut n) => n.select(),
         }
     }
 
@@ -60,6 +63,7 @@ impl Node {
             Node::Mesh(ref mut n) => n.unselect(),
             Node::Convex(ref mut n) => n.unselect(),
             Node::Cylinder(ref mut n) => n.unselect(),
+            Node::Cone(ref mut n) => n.unselect(),
         }
     }
 
@@ -75,6 +79,7 @@ impl Node {
             Node::Mesh(ref mut n) => n.update(colliders),
             Node::Convex(ref mut n) => n.update(colliders),
             Node::Cylinder(ref mut n) => n.update(colliders),
+            Node::Cone(ref mut n) => n.update(colliders),
         }
     }
 
@@ -103,6 +108,7 @@ impl Node {
             Node::Mesh(ref n) => Some(n.scene_node()),
             Node::Convex(ref n) => Some(n.scene_node()),
             Node::Cylinder(ref n) => Some(n.scene_node()),
+            Node::Cone(ref n) => Some(n.scene_node()),
             #[cfg(feature = "dim2")]
             _ => None,
         }
@@ -120,6 +126,7 @@ impl Node {
             Node::Mesh(ref mut n) => Some(n.scene_node_mut()),
             Node::Convex(ref mut n) => Some(n.scene_node_mut()),
             Node::Cylinder(ref mut n) => Some(n.scene_node_mut()),
+            Node::Cone(ref mut n) => Some(n.scene_node_mut()),
             #[cfg(feature = "dim2")]
             _ => None,
         }
@@ -137,6 +144,7 @@ impl Node {
             Node::Mesh(ref n) => n.object(),
             Node::Convex(ref n) => n.object(),
             Node::Cylinder(ref n) => n.object(),
+            Node::Cone(ref n) => n.object(),
         }
     }
 
@@ -152,6 +160,7 @@ impl Node {
             Node::Mesh(ref mut n) => n.set_color(color),
             Node::Convex(ref mut n) => n.set_color(color),
             Node::Cylinder(ref mut n) => n.set_color(color),
+            Node::Cone(ref mut n) => n.set_color(color),
         }
     }
 }