Add compound shape support.

1 files changed, 43 insertions, 18 deletions

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,