Add example for 3D convex polyhedron.

5 files changed, 205 insertions, 13 deletions

diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs
index dfbbc51..f35cb8e 100644
--- a/benchmarks3d/all_benchmarks3.rs
+++ b/benchmarks3d/all_benchmarks3.rs
@@ -14,6 +14,7 @@ mod balls3;
 mod boxes3;
 mod capsules3;
 mod compound3;
+mod convex_polyhedron3;
 mod heightfield3;
 mod joint_ball3;
 mod joint_fixed3;
@@ -52,6 +53,7 @@ pub fn main() {
         ("Boxes", boxes3::init_world),
         ("Capsules", capsules3::init_world),
         ("Compound", compound3::init_world),
+        ("Convex polyhedron", convex_polyhedron3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),
diff --git a/benchmarks3d/convex_polyhedron3.rs b/benchmarks3d/convex_polyhedron3.rs
new file mode 100644
index 0000000..07bfb98
--- /dev/null
+++ b/benchmarks3d/convex_polyhedron3.rs
@@ -0,0 +1,71 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx + offset;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz + offset;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let cylinder = rapier3d::cdl::shape::Cylinder::new(rad, rad).to_trimesh(4);
+                let collider = ColliderBuilder::round_convex_hull(&cylinder.0, 0.1)
+                    .unwrap()
+                    .build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+
+        offset -= 0.05 * rad * (num as f32 - 1.0);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 46153ac..71f0676 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,13 +1,15 @@
-use crate::cdl::shape::HalfSpace;
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::InteractionGroups;
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use cdl::bounding_volume::AABB;
 use cdl::shape::{
-    Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle,
+    Ball, Capsule, Cuboid, HalfSpace, HeightField, RoundCuboid, RoundTriangle, Segment, Shape,
+    ShapeType, ShapeWithBorder, TriMesh, Triangle,
 };
 #[cfg(feature = "dim3")]
-use cdl::shape::{Cone, Cylinder, RoundCylinder};
+use cdl::shape::{
+    Cone, ConvexPolyhedron, Cylinder, RoundCone, RoundConvexPolyhedron, RoundCylinder,
+};
 use na::Point3;
 use std::ops::Deref;
 use std::sync::Arc;
@@ -42,11 +44,21 @@ impl ColliderShape {
     /// radius of the sphere used for dilating the cylinder).
     #[cfg(feature = "dim3")]
     pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self {
-        ColliderShape(Arc::new(RoundCylinder::new(
-            half_height,
-            radius,
+        ColliderShape(Arc::new(ShapeWithBorder {
+            base_shape: Cylinder::new(half_height, radius),
+            border_radius,
+        }))
+    }
+
+    /// Initialize a rounded cone 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 round_cone(half_height: f32, radius: f32, border_radius: f32) -> Self {
+        ColliderShape(Arc::new(ShapeWithBorder {
+            base_shape: Cone::new(half_height, radius),
             border_radius,
-        )))
+        }))
     }
 
     /// Initialize a cone shape defined by its half-height
@@ -81,6 +93,40 @@ impl ColliderShape {
         ColliderShape(Arc::new(TriMesh::new(vertices, indices)))
     }
 
+    #[cfg(feature = "dim3")]
+    pub fn convex_hull(points: &[Point<f32>]) -> Option<Self> {
+        ConvexPolyhedron::from_convex_hull(points).map(|ch| ColliderShape(Arc::new(ch)))
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn convex_mesh(points: Vec<Point<f32>>, indices: &[usize]) -> Option<Self> {
+        ConvexPolyhedron::from_convex_mesh(points, indices).map(|ch| ColliderShape(Arc::new(ch)))
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn round_convex_hull(points: &[Point<f32>], border_radius: f32) -> Option<Self> {
+        ConvexPolyhedron::from_convex_hull(points).map(|ch| {
+            ColliderShape(Arc::new(ShapeWithBorder {
+                base_shape: ch,
+                border_radius,
+            }))
+        })
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn round_convex_mesh(
+        points: Vec<Point<f32>>,
+        indices: &[usize],
+        border_radius: f32,
+    ) -> Option<Self> {
+        ConvexPolyhedron::from_convex_mesh(points, indices).map(|ch| {
+            ColliderShape(Arc::new(ShapeWithBorder {
+                base_shape: ch,
+                border_radius,
+            }))
+        })
+    }
+
     /// Initializes an heightfield shape defined by its set of height and a scale
     /// factor along each coordinate axis.
     #[cfg(feature = "dim2")]
@@ -169,12 +215,22 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
                     Some(ShapeType::TriMesh) => deser::<A, TriMesh>(&mut seq)?,
                     Some(ShapeType::HeightField) => deser::<A, HeightField>(&mut seq)?,
                     Some(ShapeType::HalfSpace) => deser::<A, HalfSpace>(&mut seq)?,
+                    Some(ShapeType::RoundCuboid) => deser::<A, RoundCuboid>(&mut seq)?,
+                    Some(ShapeType::RoundTriangle) => deser::<A, RoundTriangle>(&mut seq)?,
                     #[cfg(feature = "dim3")]
                     Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
                     #[cfg(feature = "dim3")]
+                    Some(ShapeType::ConvexPolyhedron) => deser::<A, ConvexPolyhedron>(&mut seq)?,
+                    #[cfg(feature = "dim3")]
                     Some(ShapeType::Cone) => deser::<A, Cone>(&mut seq)?,
                     #[cfg(feature = "dim3")]
                     Some(ShapeType::RoundCylinder) => deser::<A, RoundCylinder>(&mut seq)?,
+                    #[cfg(feature = "dim3")]
+                    Some(ShapeType::RoundCone) => deser::<A, RoundCone>(&mut seq)?,
+                    #[cfg(feature = "dim3")]
+                    Some(ShapeType::RoundConvexPolyhedron) => {
+                        deser::<A, RoundConvexPolyhedron>(&mut seq)?
+                    }
                     None => {
                         return Err(serde::de::Error::custom(
                             "found invalid shape type to deserialize",
@@ -365,6 +421,18 @@ impl ColliderBuilder {
         Self::new(ColliderShape::cone(half_height, radius))
     }
 
+    /// Initialize a new collider builder with a rounded cone 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 round_cone(half_height: f32, radius: f32, border_radius: f32) -> Self {
+        Self::new(ColliderShape::round_cone(
+            half_height,
+            radius,
+            border_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 {
@@ -411,6 +479,30 @@ impl ColliderBuilder {
         Self::new(ColliderShape::trimesh(vertices, indices))
     }
 
+    #[cfg(feature = "dim3")]
+    pub fn convex_hull(points: &[Point<f32>]) -> Option<Self> {
+        ColliderShape::convex_hull(points).map(|cp| Self::new(cp))
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn round_convex_hull(points: &[Point<f32>], border_radius: f32) -> Option<Self> {
+        ColliderShape::round_convex_hull(points, border_radius).map(|cp| Self::new(cp))
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn convex_mesh(points: Vec<Point<f32>>, indices: &[usize]) -> Option<Self> {
+        ColliderShape::convex_mesh(points, indices).map(|cp| Self::new(cp))
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn round_convex_mesh(
+        points: Vec<Point<f32>>,
+        indices: &[usize],
+        border_radius: f32,
+    ) -> Option<Self> {
+        ColliderShape::round_convex_mesh(points, indices, border_radius).map(|cp| Self::new(cp))
+    }
+
     /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
     /// factor along each coordinate axis.
     #[cfg(feature = "dim2")]
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index a864cfd..b1ecc8b 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -14,7 +14,7 @@ use crate::objects::node::{GraphicsNode, Node};
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
 use rapier::geometry::{Collider, ColliderHandle, ColliderSet};
 //use crate::objects::capsule::Capsule;
-//use crate::objects::convex::Convex;
+use crate::objects::convex::Convex;
 //#[cfg(feature = "dim3")]
 //use crate::objects::mesh::Mesh;
 //use crate::objects::plane::Plane;
@@ -292,7 +292,10 @@ impl GraphicsManager {
         //     window,
         // ))),
 
-        if let Some(cuboid) = shape.as_cuboid() {
+        if let Some(cuboid) = shape
+            .as_cuboid()
+            .or(shape.as_round_cuboid().map(|r| &r.base_shape))
+        {
             out.push(Node::Box(BoxNode::new(
                 handle,
                 cuboid.half_extents,
@@ -305,7 +308,10 @@ impl GraphicsManager {
             out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
         }
 
-        if let Some(triangle) = shape.as_triangle() {
+        if let Some(triangle) = shape
+            .as_triangle()
+            .or(shape.as_round_triangle().map(|r| &r.base_shape))
+        {
             out.push(Node::Mesh(Mesh::new(
                 handle,
                 vec![triangle.a, triangle.b, triangle.c],
@@ -339,9 +345,20 @@ impl GraphicsManager {
         }
 
         #[cfg(feature = "dim3")]
+        if let Some(convex_polyhedron) = shape
+            .as_convex_polyhedron()
+            .or(shape.as_round_convex_polyhedron().map(|r| &r.base_shape))
+        {
+            let (vertices, indices) = convex_polyhedron.to_trimesh();
+            out.push(Node::Convex(Convex::new(
+                handle, vertices, indices, color, window,
+            )))
+        }
+
+        #[cfg(feature = "dim3")]
         if let Some(cylinder) = shape
             .as_cylinder()
-            .or(shape.as_round_cylinder().map(|r| &r.cylinder))
+            .or(shape.as_round_cylinder().map(|r| &r.base_shape))
         {
             out.push(Node::Cylinder(Cylinder::new(
                 handle,
@@ -353,7 +370,10 @@ impl GraphicsManager {
         }
 
         #[cfg(feature = "dim3")]
-        if let Some(cone) = shape.as_cone() {
+        if let Some(cone) = shape
+            .as_cone()
+            .or(shape.as_round_cone().map(|r| &r.base_shape))
+        {
             out.push(Node::Cone(Cone::new(
                 handle,
                 cone.half_height,
diff --git a/src_testbed/objects/convex.rs b/src_testbed/objects/convex.rs
index 0347144..9f0983d 100644
--- a/src_testbed/objects/convex.rs
+++ b/src_testbed/objects/convex.rs
@@ -19,13 +19,20 @@ impl Convex {
     pub fn new(
         body: ColliderHandle,
         vertices: Vec<Point<f32>>,
+        #[cfg(feature = "dim3")] indices: Vec<Point<u32>>,
         color: Point3<f32>,
         window: &mut Window,
     ) -> Convex {
         #[cfg(feature = "dim2")]
         let node = window.add_convex_polygon(vertices, Vector::from_element(1.0));
         #[cfg(feature = "dim3")]
-        let node = unimplemented!();
+        let node = {
+            use std::cell::RefCell;
+            use std::rc::Rc;
+            let is = indices.into_iter().map(na::convert).collect();
+            let mesh = kiss3d::resource::Mesh::new(vertices, is, None, None, false);
+            window.add_mesh(Rc::new(RefCell::new(mesh)), na::Vector3::from_element(1.0))
+        };
 
         let mut res = Convex {
             color,