1 files changed, 72 insertions, 23 deletions

diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index 217da48..1bafdb6 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -9,11 +9,10 @@ use na::Point3;
 use crate::math::Point;
 use crate::objects::ball::Ball;
 use crate::objects::box_node::Box as BoxNode;
-use crate::objects::convex::Convex;
 use crate::objects::heightfield::HeightField;
 use crate::objects::node::{GraphicsNode, Node};
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
-use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
+use rapier::geometry::{Collider, ColliderHandle, ColliderSet};
 //use crate::objects::capsule::Capsule;
 //use crate::objects::convex::Convex;
 //#[cfg(feature = "fluids")]
@@ -26,6 +25,10 @@ use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
 //#[cfg(feature = "fluids")]
 //use crate::objects::FluidRenderingMode;
 use crate::objects::capsule::Capsule;
+#[cfg(feature = "dim3")]
+use crate::objects::cone::Cone;
+#[cfg(feature = "dim3")]
+use crate::objects::cylinder::Cylinder;
 use crate::objects::mesh::Mesh;
 use rand::{Rng, SeedableRng};
 use rand_pcg::Pcg32;
@@ -60,6 +63,7 @@ pub struct GraphicsManager {
     #[cfg(feature = "fluids")]
     boundary2sn: HashMap<BoundaryHandle, FluidNode>,
     b2color: HashMap<RigidBodyHandle, Point3<f32>>,
+    c2color: HashMap<ColliderHandle, Point3<f32>>,
     b2wireframe: HashMap<RigidBodyHandle, bool>,
     #[cfg(feature = "fluids")]
     f2color: HashMap<FluidHandle, Point3<f32>>,
@@ -97,6 +101,7 @@ impl GraphicsManager {
             #[cfg(feature = "fluids")]
             boundary2sn: HashMap::new(),
             b2color: HashMap::new(),
+            c2color: HashMap::new(),
             #[cfg(feature = "fluids")]
             f2color: HashMap::new(),
             ground_color: Point3::new(0.5, 0.5, 0.5),
@@ -183,6 +188,10 @@ impl GraphicsManager {
         }
     }
 
+    pub fn set_collider_initial_color(&mut self, c: ColliderHandle, color: Point3<f32>) {
+        self.c2color.insert(c, color);
+    }
+
     pub fn set_body_wireframe(&mut self, b: RigidBodyHandle, enabled: bool) {
         self.b2wireframe.insert(b, enabled);
 
@@ -321,6 +330,7 @@ impl GraphicsManager {
         let mut new_nodes = Vec::new();
 
         for collider_handle in bodies[handle].colliders() {
+            let color = self.c2color.get(collider_handle).copied().unwrap_or(color);
             let collider = &colliders[*collider_handle];
             self.add_collider(window, *collider_handle, collider, color, &mut new_nodes);
         }
@@ -349,33 +359,44 @@ impl GraphicsManager {
         color: Point3<f32>,
         out: &mut Vec<Node>,
     ) {
-        match collider.shape() {
-            Shape::Ball(ball) => {
-                out.push(Node::Ball(Ball::new(handle, ball.radius, color, window)))
-            }
-            Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
-                handle,
-                poly.vertices().to_vec(),
-                color,
-                window,
-            ))),
-            Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new(
+        let shape = collider.shape();
+
+        if let Some(ball) = shape.as_ball() {
+            out.push(Node::Ball(Ball::new(handle, 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() {
+            out.push(Node::Box(BoxNode::new(
                 handle,
                 cuboid.half_extents,
                 color,
                 window,
-            ))),
-            Shape::Capsule(capsule) => {
-                out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
-            }
-            Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new(
+            )))
+        }
+
+        if let Some(capsule) = shape.as_capsule() {
+            out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
+        }
+
+        if let Some(triangle) = shape.as_triangle() {
+            out.push(Node::Mesh(Mesh::new(
                 handle,
                 vec![triangle.a, triangle.b, triangle.c],
                 vec![Point3::new(0, 1, 2)],
                 color,
                 window,
-            ))),
-            Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new(
+            )))
+        }
+
+        if let Some(trimesh) = shape.as_trimesh() {
+            out.push(Node::Mesh(Mesh::new(
                 handle,
                 trimesh.vertices().to_vec(),
                 trimesh
@@ -385,13 +406,41 @@ impl GraphicsManager {
                     .collect(),
                 color,
                 window,
-            ))),
-            Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new(
+            )))
+        }
+
+        if let Some(heightfield) = shape.as_heightfield() {
+            out.push(Node::HeightField(HeightField::new(
                 handle,
                 heightfield,
                 color,
                 window,
-            ))),
+            )))
+        }
+
+        #[cfg(feature = "dim3")]
+        if let Some(cylinder) = shape
+            .as_cylinder()
+            .or(shape.as_round_cylinder().map(|r| &r.cylinder))
+        {
+            out.push(Node::Cylinder(Cylinder::new(
+                handle,
+                cylinder.half_height,
+                cylinder.radius,
+                color,
+                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,
+            )))
         }
     }