Add compound shape support.

10 files changed, 122 insertions, 49 deletions

diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs
index 34a654d..b2fa485 100644
--- a/examples3d/compound3.rs
+++ b/examples3d/compound3.rs
@@ -1,6 +1,6 @@
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, ColliderShape};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -28,6 +28,7 @@ pub fn init_world(testbed: &mut Testbed) {
      * Create the cubes
      */
     let num = 8;
+    let numy = 15;
     let rad = 0.2;
 
     let shift = rad * 4.0 + rad;
@@ -37,7 +38,7 @@ pub fn init_world(testbed: &mut Testbed) {
 
     let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
 
-    for j in 0usize..15 {
+    for j in 0usize..numy {
         for i in 0..num {
             for k in 0usize..num {
                 let x = i as f32 * shift * 5.0 - centerx + offset;
@@ -47,16 +48,39 @@ pub fn init_world(testbed: &mut Testbed) {
                 // Build the rigid body.
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
-                let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
-                let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
-                    .translation(rad * 10.0, rad * 10.0, 0.0)
-                    .build();
-                let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
-                    .translation(-rad * 10.0, rad * 10.0, 0.0)
-                    .build();
-                colliders.insert(collider1, handle, &mut bodies);
-                colliders.insert(collider2, handle, &mut bodies);
-                colliders.insert(collider3, handle, &mut bodies);
+
+                // First option: attach several colliders to a single rigid-body.
+                if j < numy / 2 {
+                    let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
+                    let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                        .translation(rad * 10.0, rad * 10.0, 0.0)
+                        .build();
+                    let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                        .translation(-rad * 10.0, rad * 10.0, 0.0)
+                        .build();
+                    colliders.insert(collider1, handle, &mut bodies);
+                    colliders.insert(collider2, handle, &mut bodies);
+                    colliders.insert(collider3, handle, &mut bodies);
+                } else {
+                    // Second option: create a compound shape and attach it to a single collider.
+                    let shapes = vec![
+                        (
+                            Isometry3::identity(),
+                            ColliderShape::cuboid(rad * 10.0, rad, rad),
+                        ),
+                        (
+                            Isometry3::translation(rad * 10.0, rad * 10.0, 0.0),
+                            ColliderShape::cuboid(rad, rad * 10.0, rad),
+                        ),
+                        (
+                            Isometry3::translation(-rad * 10.0, rad * 10.0, 0.0),
+                            ColliderShape::cuboid(rad, rad * 10.0, rad),
+                        ),
+                    ];
+
+                    let collider = ColliderBuilder::compound(shapes).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                }
             }
         }
 
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 3fda8a6..5171f1b 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -13,6 +13,7 @@ use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGrap
 use crate::math::{Real, Vector};
 use crate::pipeline::EventHandler;
 use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
+use cdl::utils::IsometryOpt;
 use std::collections::HashMap;
 use std::sync::Arc;
 
@@ -522,10 +523,11 @@ impl NarrowPhase {
             let mut has_any_active_contact = false;
 
             for manifold in &mut pair.manifolds {
+                let world_pos1 = manifold.subshape_pos1.prepend_to(co1.position());
                 manifold.data.solver_contacts.clear();
                 manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent());
                 manifold.data.solver_flags = solver_flags;
-                manifold.data.normal = co1.position() * manifold.local_n1;
+                manifold.data.normal = world_pos1 * manifold.local_n1;
 
                 // Sort contacts to keep only these with distances bellow
                 // the prediction, and generate solver contacts.
@@ -536,7 +538,7 @@ impl NarrowPhase {
                     if contact.dist < prediction_distance {
                         // Generate the solver contact.
                         let solver_contact = SolverContact {
-                            point: co1.position() * contact.local_p1
+                            point: world_pos1 * contact.local_p1
                                 + manifold.data.normal * contact.dist / 2.0,
                             dist: contact.dist,
                             friction: (co1.friction + co2.friction) / 2.0,
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index a1fd74e..1b7ab4b 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -6,13 +6,13 @@ use kiss3d::window::Window;
 
 use na::Point3;
 
-use crate::math::Point;
+use crate::math::{Isometry, Point};
 use crate::objects::ball::Ball;
 use crate::objects::box_node::Box as BoxNode;
 use crate::objects::heightfield::HeightField;
 use crate::objects::node::{GraphicsNode, Node};
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
-use rapier::geometry::{Collider, ColliderHandle, ColliderSet};
+use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
 //use crate::objects::capsule::Capsule;
 use crate::objects::convex::Convex;
 //#[cfg(feature = "dim3")]
@@ -237,7 +237,14 @@ impl GraphicsManager {
         for collider_handle in bodies[handle].colliders() {
             let color = self.c2color.get(collider_handle).copied().unwrap_or(color);
             let collider = &colliders[*collider_handle];
-            self.do_add_collider(window, *collider_handle, collider, color, &mut new_nodes);
+            self.do_add_shape(
+                window,
+                *collider_handle,
+                collider.shape(),
+                &Isometry::identity(),
+                color,
+                &mut new_nodes,
+            );
         }
 
         new_nodes.iter_mut().for_each(|n| n.update(colliders));
@@ -267,37 +274,49 @@ impl GraphicsManager {
         let color = self.c2color.get(&handle).copied().unwrap_or(color);
         let mut nodes =
             std::mem::replace(self.b2sn.get_mut(&collider.parent()).unwrap(), Vec::new());
-        self.do_add_collider(window, handle, collider, color, &mut nodes);
+        self.do_add_shape(
+            window,
+            handle,
+            collider.shape(),
+            &Isometry::identity(),
+            color,
+            &mut nodes,
+        );
         self.b2sn.insert(collider.parent(), nodes);
     }
 
-    fn do_add_collider(
+    fn do_add_shape(
         &mut self,
         window: &mut Window,
         handle: ColliderHandle,
-        collider: &Collider,
+        shape: &dyn Shape,
+        delta: &Isometry<f32>,
         color: Point3<f32>,
         out: &mut Vec<Node>,
     ) {
-        let shape = collider.shape();
+        if let Some(compound) = shape.as_compound() {
+            for (shape_pos, shape) in compound.shapes() {
+                self.do_add_shape(window, handle, &**shape, shape_pos, color, out)
+            }
+        }
 
         if let Some(ball) = shape.as_ball() {
-            out.push(Node::Ball(Ball::new(handle, ball.radius, color, window)))
+            out.push(Node::Ball(Ball::new(
+                handle,
+                *delta,
+                ball.radius,
+                color,
+                window,
+            )))
         }
 
-        // Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
-        //     handle,
-        //     poly.vertices().to_vec(),
-        //     color,
-        //     window,
-        // ))),
-
         if let Some(cuboid) = shape
             .as_cuboid()
             .or(shape.as_round_cuboid().map(|r| &r.base_shape))
         {
             out.push(Node::Box(BoxNode::new(
                 handle,
+                *delta,
                 cuboid.half_extents,
                 color,
                 window,
@@ -305,7 +324,9 @@ impl GraphicsManager {
         }
 
         if let Some(capsule) = shape.as_capsule() {
-            out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
+            out.push(Node::Capsule(Capsule::new(
+                handle, *delta, capsule, color, window,
+            )))
         }
 
         if let Some(triangle) = shape
@@ -350,7 +371,9 @@ impl GraphicsManager {
             .or(shape.as_round_convex_polygon().map(|r| &r.base_shape))
         {
             let vertices = convex_polygon.points().to_vec();
-            out.push(Node::Convex(Convex::new(handle, vertices, color, window)))
+            out.push(Node::Convex(Convex::new(
+                handle, *delta, vertices, color, window,
+            )))
         }
 
         #[cfg(feature = "dim3")]
@@ -360,7 +383,7 @@ impl GraphicsManager {
         {
             let (vertices, indices) = convex_polyhedron.to_trimesh();
             out.push(Node::Convex(Convex::new(
-                handle, vertices, indices, color, window,
+                handle, *delta, vertices, indices, color, window,
             )))
         }
 
@@ -371,6 +394,7 @@ impl GraphicsManager {
         {
             out.push(Node::Cylinder(Cylinder::new(
                 handle,
+                *delta,
                 cylinder.half_height,
                 cylinder.radius,
                 color,
@@ -385,6 +409,7 @@ impl GraphicsManager {
         {
             out.push(Node::Cone(Cone::new(
                 handle,
+                *delta,
                 cone.half_height,
                 cone.radius,
                 color,
diff --git a/src_testbed/objects/ball.rs b/src_testbed/objects/ball.rs
index f72c284..4df3820 100644
--- a/src_testbed/objects/ball.rs
+++ b/src_testbed/objects/ball.rs
@@ -1,6 +1,6 @@
 use crate::objects::node::{self, GraphicsNode};
 use kiss3d::window::Window;
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier::geometry::{ColliderHandle, ColliderSet};
 use rapier::math::Isometry;
 
@@ -9,11 +9,13 @@ pub struct Ball {
     base_color: Point3<f32>,
     gfx: GraphicsNode,
     collider: ColliderHandle,
+    delta: Isometry3<f32>,
 }
 
 impl Ball {
     pub fn new(
         collider: ColliderHandle,
+        delta: Isometry3<f32>,
         radius: f32,
         color: Point3<f32>,
         window: &mut Window,
@@ -28,6 +30,7 @@ impl Ball {
             base_color: color,
             gfx: node,
             collider,
+            delta,
         };
 
         // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
@@ -55,7 +58,7 @@ impl Ball {
             colliders,
             self.collider,
             &self.color,
-            &Isometry::identity(),
+            &self.delta,
         );
     }
 
diff --git a/src_testbed/objects/box_node.rs b/src_testbed/objects/box_node.rs
index 493ffba..4235ef1 100644
--- a/src_testbed/objects/box_node.rs
+++ b/src_testbed/objects/box_node.rs
@@ -1,6 +1,6 @@
 use crate::objects::node::{self, GraphicsNode};
 use kiss3d::window;
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier::geometry::{ColliderHandle, ColliderSet};
 use rapier::math::{Isometry, Vector};
 
@@ -9,11 +9,13 @@ pub struct Box {
     base_color: Point3<f32>,
     gfx: GraphicsNode,
     collider: ColliderHandle,
+    delta: Isometry3<f32>,
 }
 
 impl Box {
     pub fn new(
         collider: ColliderHandle,
+        delta: Isometry3<f32>,
         half_extents: Vector<f32>,
         color: Point3<f32>,
         window: &mut window::Window,
@@ -29,6 +31,7 @@ impl Box {
             base_color: color,
             gfx: node,
             collider,
+            delta,
         };
 
         res.gfx.set_color(color.x, color.y, color.z);
@@ -55,7 +58,7 @@ impl Box {
             colliders,
             self.collider,
             &self.color,
-            &Isometry::identity(),
+            &self.delta,
         );
     }
 
diff --git a/src_testbed/objects/capsule.rs b/src_testbed/objects/capsule.rs
index 23160be..197d1e8 100644
--- a/src_testbed/objects/capsule.rs
+++ b/src_testbed/objects/capsule.rs
@@ -1,6 +1,6 @@
 use crate::objects::node::{self, GraphicsNode};
 use kiss3d::window;
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier::geometry::{self, ColliderHandle, ColliderSet};
 use rapier::math::Isometry;
 
@@ -9,11 +9,13 @@ pub struct Capsule {
     base_color: Point3<f32>,
     gfx: GraphicsNode,
     collider: ColliderHandle,
+    delta: Isometry3<f32>,
 }
 
 impl Capsule {
     pub fn new(
         collider: ColliderHandle,
+        delta: Isometry3<f32>,
         capsule: &geometry::Capsule,
         color: Point3<f32>,
         window: &mut window::Window,
@@ -30,6 +32,7 @@ impl Capsule {
             base_color: color,
             gfx: node,
             collider,
+            delta,
         };
 
         res.gfx.set_color(color.x, color.y, color.z);
@@ -50,7 +53,7 @@ impl Capsule {
             colliders,
             self.collider,
             &self.color,
-            &Isometry::identity(),
+            &self.delta,
         );
     }
 
diff --git a/src_testbed/objects/cone.rs b/src_testbed/objects/cone.rs
index 58b014f..89a6b47 100644
--- a/src_testbed/objects/cone.rs
+++ b/src_testbed/objects/cone.rs
@@ -1,6 +1,6 @@
 use crate::objects::node::{self, GraphicsNode};
 use kiss3d::window::Window;
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier::geometry::{ColliderHandle, ColliderSet};
 use rapier::math::Isometry;
 
@@ -9,11 +9,13 @@ pub struct Cone {
     base_color: Point3<f32>,
     gfx: GraphicsNode,
     collider: ColliderHandle,
+    delta: Isometry3<f32>,
 }
 
 impl Cone {
     pub fn new(
         collider: ColliderHandle,
+        delta: Isometry3<f32>,
         half_height: f32,
         radius: f32,
         color: Point3<f32>,
@@ -29,6 +31,7 @@ impl Cone {
             base_color: color,
             gfx: node,
             collider,
+            delta,
         };
 
         // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
@@ -56,7 +59,7 @@ impl Cone {
             colliders,
             self.collider,
             &self.color,
-            &Isometry::identity(),
+            &self.delta,
         );
     }
 
diff --git a/src_testbed/objects/convex.rs b/src_testbed/objects/convex.rs
index 9f0983d..b0e0bbb 100644
--- a/src_testbed/objects/convex.rs
+++ b/src_testbed/objects/convex.rs
@@ -5,7 +5,7 @@ use crate::math::Vector;
 use crate::math::{Isometry, Point};
 use crate::objects::node::{self, GraphicsNode};
 use kiss3d::window::Window;
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier::geometry::{ColliderHandle, ColliderSet};
 
 pub struct Convex {
@@ -13,11 +13,13 @@ pub struct Convex {
     base_color: Point3<f32>,
     gfx: GraphicsNode,
     body: ColliderHandle,
+    delta: Isometry3<f32>,
 }
 
 impl Convex {
     pub fn new(
         body: ColliderHandle,
+        delta: Isometry3<f32>,
         vertices: Vec<Point<f32>>,
         #[cfg(feature = "dim3")] indices: Vec<Point<u32>>,
         color: Point3<f32>,
@@ -39,6 +41,7 @@ impl Convex {
             base_color: color,
             gfx: node,
             body,
+            delta,
         };
 
         // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
@@ -66,7 +69,7 @@ impl Convex {
             colliders,
             self.body,
             &self.color,
-            &Isometry::identity(),
+            &self.delta,
         );
     }
 
diff --git a/src_testbed/objects/cylinder.rs b/src_testbed/objects/cylinder.rs
index 01a6737..a2ed102 100644
--- a/src_testbed/objects/cylinder.rs
+++ b/src_testbed/objects/cylinder.rs
@@ -1,6 +1,6 @@
 use crate::objects::node::{self, GraphicsNode};
 use kiss3d::window::Window;
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier::geometry::{ColliderHandle, ColliderSet};
 use rapier::math::Isometry;
 
@@ -9,11 +9,13 @@ pub struct Cylinder {
     base_color: Point3<f32>,
     gfx: GraphicsNode,
     collider: ColliderHandle,
+    delta: Isometry3<f32>,
 }
 
 impl Cylinder {
     pub fn new(
         collider: ColliderHandle,
+        delta: Isometry3<f32>,
         half_height: f32,
         radius: f32,
         color: Point3<f32>,
@@ -29,6 +31,7 @@ impl Cylinder {
             base_color: color,
             gfx: node,
             collider,
+            delta,
         };
 
         // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
@@ -56,7 +59,7 @@ impl Cylinder {
             colliders,
             self.collider,
             &self.color,
-            &Isometry::identity(),
+            &self.delta,
         );
     }
 
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index b1a529f..10b3a0a 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -17,7 +17,7 @@ use kiss3d::planar_camera::PlanarCamera;
 use kiss3d::post_processing::PostProcessingEffect;
 use kiss3d::text::Font;
 use kiss3d::window::{State, Window};
-use na::{self, Point2, Point3, Vector3};
+use na::{self, Isometry3, Point2, Point3, Vector3};
 use rapier::dynamics::{
     ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet,
 };
@@ -1525,9 +1525,13 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet)
                 };
                 let pos1 = colliders[pair.pair.collider1].position();
                 let pos2 = colliders[pair.pair.collider2].position();
-                let start = pos1 * pt.local_p1;
-                let end = pos2 * pt.local_p2;
-                let n = pos1 * manifold.local_n1;
+                let start =
+                    pos1 * manifold.subshape_pos1.unwrap_or(Isometry3::identity()) * pt.local_p1;
+                let end =
+                    pos2 * manifold.subshape_pos2.unwrap_or(Isometry3::identity()) * pt.local_p2;
+                let n = pos1
+                    * manifold.subshape_pos1.unwrap_or(Isometry3::identity())
+                    * manifold.local_n1;
 
                 use crate::engine::GraphicsWindow;
                 window.draw_graphics_line(&start, &(start + n * 0.4), &Point3::new(0.5, 1.0, 0.5));