From 1a84bf2af34176508bdda8d0f2ad2e46dc5c4df9 Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Sun, 16 May 2021 17:49:20 +0200
Subject: Replace Kiss3d by Bevy for the testbed renderer.

---
 src_testbed/camera.rs              |   94 ++
 src_testbed/engine.rs              |  712 -------------
 src_testbed/graphics.rs            |  400 ++++++++
 src_testbed/harness/mod.rs         |   59 +-
 src_testbed/lib.rs                 |    9 +-
 src_testbed/objects/ball.rs        |   76 --
 src_testbed/objects/box_node.rs    |   76 --
 src_testbed/objects/capsule.rs     |   77 --
 src_testbed/objects/cone.rs        |   77 --
 src_testbed/objects/convex.rs      |   96 --
 src_testbed/objects/cylinder.rs    |   77 --
 src_testbed/objects/heightfield.rs |  128 ---
 src_testbed/objects/mesh.rs        |  111 ---
 src_testbed/objects/mod.rs         |   12 -
 src_testbed/objects/node.rs        |  547 +++++++---
 src_testbed/objects/plane.rs       |  132 ---
 src_testbed/objects/polyline.rs    |   67 --
 src_testbed/plugin.rs              |   12 +-
 src_testbed/testbed.rs             | 1928 ++++++++++++++++--------------------
 src_testbed/ui.rs                  |  719 ++++----------
 20 files changed, 1957 insertions(+), 3452 deletions(-)
 create mode 100644 src_testbed/camera.rs
 delete mode 100644 src_testbed/engine.rs
 create mode 100644 src_testbed/graphics.rs
 delete mode 100644 src_testbed/objects/ball.rs
 delete mode 100644 src_testbed/objects/box_node.rs
 delete mode 100644 src_testbed/objects/capsule.rs
 delete mode 100644 src_testbed/objects/cone.rs
 delete mode 100644 src_testbed/objects/convex.rs
 delete mode 100644 src_testbed/objects/cylinder.rs
 delete mode 100644 src_testbed/objects/heightfield.rs
 delete mode 100644 src_testbed/objects/mesh.rs
 delete mode 100644 src_testbed/objects/plane.rs
 delete mode 100644 src_testbed/objects/polyline.rs

(limited to 'src_testbed')

diff --git a/src_testbed/camera.rs b/src_testbed/camera.rs
new file mode 100644
index 0000000..529e6c7
--- /dev/null
+++ b/src_testbed/camera.rs
@@ -0,0 +1,94 @@
+// NOTE: this is inspired from the `bevy-orbit-controls` projects but
+//       with some modifications like Panning, and 2D support.
+use bevy::input::mouse::MouseMotion;
+use bevy::input::mouse::MouseScrollUnit::{Line, Pixel};
+use bevy::input::mouse::MouseWheel;
+use bevy::prelude::*;
+use bevy::render::camera::Camera;
+
+const LINE_TO_PIXEL_RATIO: f32 = 0.1;
+
+pub struct OrbitCamera {
+    pub zoom: f32,
+    pub center: Vec3,
+    pub pan_sensitivity: f32,
+    pub zoom_sensitivity: f32,
+    pub pan_button: MouseButton,
+    pub enabled: bool,
+}
+
+impl Default for OrbitCamera {
+    fn default() -> Self {
+        OrbitCamera {
+            zoom: 100.0,
+            center: Vec3::ZERO,
+            pan_sensitivity: 1.0,
+            zoom_sensitivity: 0.8,
+            pan_button: MouseButton::Right,
+            enabled: true,
+        }
+    }
+}
+
+// Adapted from the 3D orbit camera from bevy-orbit-controls
+pub struct OrbitCameraPlugin;
+impl OrbitCameraPlugin {
+    fn update_transform_system(
+        mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
+    ) {
+        for (camera, mut transform) in query.iter_mut() {
+            if camera.enabled {
+                transform.translation = camera.center;
+                transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
+            }
+        }
+    }
+
+    fn mouse_motion_system(
+        _time: Res<Time>,
+        mut mouse_motion_events: EventReader<MouseMotion>,
+        mouse_button_input: Res<Input<MouseButton>>,
+        mut query: Query<(&mut OrbitCamera, &mut Transform, &mut Camera)>,
+    ) {
+        let mut delta = Vec2::ZERO;
+        for event in mouse_motion_events.iter() {
+            delta += event.delta;
+        }
+        for (mut camera, _, _) in query.iter_mut() {
+            if !camera.enabled {
+                continue;
+            }
+
+            if mouse_button_input.pressed(camera.pan_button) {
+                let delta = delta * camera.pan_sensitivity;
+                camera.center += Vec3::new(-delta.x, delta.y, 0.0);
+            }
+        }
+    }
+
+    fn zoom_system(
+        mut mouse_wheel_events: EventReader<MouseWheel>,
+        mut query: Query<&mut OrbitCamera, With<Camera>>,
+    ) {
+        let mut total = 0.0;
+        for event in mouse_wheel_events.iter() {
+            total -= event.y
+                * match event.unit {
+                    Line => 1.0,
+                    Pixel => LINE_TO_PIXEL_RATIO,
+                };
+        }
+        for mut camera in query.iter_mut() {
+            if camera.enabled {
+                camera.zoom *= camera.zoom_sensitivity.powf(total);
+            }
+        }
+    }
+}
+impl Plugin for OrbitCameraPlugin {
+    fn build(&self, app: &mut AppBuilder) {
+        app.add_system(Self::mouse_motion_system.system())
+            .add_system(Self::zoom_system.system())
+            .add_system(Self::update_transform_system.system());
+    }
+}
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
deleted file mode 100644
index 876cb7e..0000000
--- a/src_testbed/engine.rs
+++ /dev/null
@@ -1,712 +0,0 @@
-#[cfg(feature = "dim3")]
-use kiss3d::camera::ArcBall as Camera;
-#[cfg(feature = "dim2")]
-use kiss3d::planar_camera::Sidescroll as Camera;
-use kiss3d::window::Window;
-
-use na::Point3;
-
-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::{ColliderHandle, ColliderSet, Shape};
-//use crate::objects::capsule::Capsule;
-use crate::objects::convex::Convex;
-//#[cfg(feature = "dim3")]
-//use crate::objects::mesh::Mesh;
-//use crate::objects::plane::Plane;
-//#[cfg(feature = "dim2")]
-//use crate::objects::polyline::Polyline;
-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 crate::objects::polyline::Polyline;
-use rand::{Rng, SeedableRng};
-use rand_pcg::Pcg32;
-use std::collections::HashMap;
-
-pub trait GraphicsWindow {
-    fn remove_graphics_node(&mut self, node: &mut GraphicsNode);
-    fn draw_graphics_line(&mut self, p1: &Point<f32>, p2: &Point<f32>, color: &Point3<f32>);
-}
-
-impl GraphicsWindow for Window {
-    fn remove_graphics_node(&mut self, node: &mut GraphicsNode) {
-        #[cfg(feature = "dim2")]
-        self.remove_planar_node(node);
-        #[cfg(feature = "dim3")]
-        self.remove_node(node);
-    }
-
-    fn draw_graphics_line(&mut self, p1: &Point<f32>, p2: &Point<f32>, color: &Point3<f32>) {
-        #[cfg(feature = "dim2")]
-        self.draw_planar_line(p1, p2, color);
-        #[cfg(feature = "dim3")]
-        self.draw_line(p1, p2, color);
-    }
-}
-
-pub struct GraphicsManager {
-    rand: Pcg32,
-    b2sn: HashMap<RigidBodyHandle, Vec<Node>>,
-    b2color: HashMap<RigidBodyHandle, Point3<f32>>,
-    c2color: HashMap<ColliderHandle, Point3<f32>>,
-    b2wireframe: HashMap<RigidBodyHandle, bool>,
-    ground_color: Point3<f32>,
-    camera: Camera,
-}
-
-impl GraphicsManager {
-    pub fn new() -> GraphicsManager {
-        let mut camera;
-
-        #[cfg(feature = "dim3")]
-        {
-            camera = Camera::new(Point3::new(10.0, 10.0, 10.0), Point3::new(0.0, 0.0, 0.0));
-            camera.set_rotate_modifiers(Some(kiss3d::event::Modifiers::Control));
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            camera = Camera::new();
-            camera.set_zoom(50.0);
-        }
-
-        GraphicsManager {
-            camera,
-            rand: Pcg32::seed_from_u64(0),
-            b2sn: HashMap::new(),
-            b2color: HashMap::new(),
-            c2color: HashMap::new(),
-            ground_color: Point3::new(0.5, 0.5, 0.5),
-            b2wireframe: HashMap::new(),
-        }
-    }
-
-    pub fn clear(&mut self, window: &mut Window) {
-        for sns in self.b2sn.values_mut() {
-            for sn in sns.iter_mut() {
-                if let Some(node) = sn.scene_node_mut() {
-                    window.remove_graphics_node(node);
-                }
-            }
-        }
-
-        self.b2sn.clear();
-        self.c2color.clear();
-        self.b2color.clear();
-        self.b2wireframe.clear();
-        self.rand = Pcg32::seed_from_u64(0);
-    }
-
-    pub fn remove_body_nodes(&mut self, window: &mut Window, body: RigidBodyHandle) {
-        if let Some(sns) = self.b2sn.get_mut(&body) {
-            for sn in sns.iter_mut() {
-                if let Some(node) = sn.scene_node_mut() {
-                    window.remove_graphics_node(node);
-                }
-            }
-        }
-
-        self.b2sn.remove(&body);
-    }
-
-    pub fn set_body_color(&mut self, b: RigidBodyHandle, color: Point3<f32>) {
-        self.b2color.insert(b, color);
-
-        if let Some(ns) = self.b2sn.get_mut(&b) {
-            for n in ns.iter_mut() {
-                n.set_color(color)
-            }
-        }
-    }
-
-    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);
-
-        if let Some(ns) = self.b2sn.get_mut(&b) {
-            for n in ns.iter_mut().filter_map(|n| n.scene_node_mut()) {
-                if enabled {
-                    n.set_surface_rendering_activation(true);
-                    n.set_lines_width(1.0);
-                } else {
-                    n.set_surface_rendering_activation(false);
-                    n.set_lines_width(1.0);
-                }
-            }
-        }
-    }
-
-    pub fn toggle_wireframe_mode(&mut self, colliders: &ColliderSet, enabled: bool) {
-        for n in self.b2sn.values_mut().flat_map(|val| val.iter_mut()) {
-            let force_wireframe = if let Some(collider) = colliders.get(n.collider()) {
-                collider.is_sensor()
-                    || self
-                        .b2wireframe
-                        .get(&collider.parent())
-                        .cloned()
-                        .unwrap_or(false)
-            } else {
-                false
-            };
-
-            if let Some(node) = n.scene_node_mut() {
-                if force_wireframe || enabled {
-                    node.set_lines_width(1.0);
-                    node.set_surface_rendering_activation(false);
-                } else {
-                    node.set_lines_width(0.0);
-                    node.set_surface_rendering_activation(true);
-                }
-            }
-        }
-    }
-
-    pub fn next_color(&mut self) -> Point3<f32> {
-        Self::gen_color(&mut self.rand)
-    }
-
-    fn gen_color(rng: &mut Pcg32) -> Point3<f32> {
-        let mut color: Point3<f32> = rng.gen();
-        color *= 1.5;
-        color.x = color.x.min(1.0);
-        color.y = color.y.min(1.0);
-        color.z = color.z.min(1.0);
-        color
-    }
-
-    fn alloc_color(&mut self, handle: RigidBodyHandle, is_static: bool) -> Point3<f32> {
-        let mut color = self.ground_color;
-
-        if !is_static {
-            match self.b2color.get(&handle).cloned() {
-                Some(c) => color = c,
-                None => color = Self::gen_color(&mut self.rand),
-            }
-        }
-
-        self.set_body_color(handle, color);
-
-        color
-    }
-
-    pub fn add(
-        &mut self,
-        window: &mut Window,
-        handle: RigidBodyHandle,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
-    ) {
-        let body = bodies.get(handle).unwrap();
-
-        let color = self
-            .b2color
-            .get(&handle)
-            .cloned()
-            .unwrap_or_else(|| self.alloc_color(handle, !body.is_dynamic()));
-
-        self.add_with_color(window, handle, bodies, colliders, color)
-    }
-
-    pub fn add_with_color(
-        &mut self,
-        window: &mut Window,
-        handle: RigidBodyHandle,
-        bodies: &RigidBodySet,
-        colliders: &ColliderSet,
-        color: Point3<f32>,
-    ) {
-        //        let body = bodies.get(handle).unwrap();
-        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.do_add_shape(
-                window,
-                *collider_handle,
-                collider.shape(),
-                &Isometry::identity(),
-                color,
-                &mut new_nodes,
-            );
-        }
-
-        new_nodes.iter_mut().for_each(|n| n.update(colliders));
-
-        for node in new_nodes.iter_mut().filter_map(|n| n.scene_node_mut()) {
-            if self.b2wireframe.get(&handle).cloned() == Some(true) {
-                node.set_lines_width(1.0);
-                node.set_surface_rendering_activation(false);
-            } else {
-                node.set_lines_width(0.0);
-                node.set_surface_rendering_activation(true);
-            }
-        }
-
-        let nodes = self.b2sn.entry(handle).or_insert_with(Vec::new);
-        nodes.append(&mut new_nodes);
-    }
-
-    pub fn add_collider(
-        &mut self,
-        window: &mut Window,
-        handle: ColliderHandle,
-        colliders: &ColliderSet,
-    ) {
-        let collider = &colliders[handle];
-        let color = *self.b2color.get(&collider.parent()).unwrap();
-        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_shape(
-            window,
-            handle,
-            collider.shape(),
-            &Isometry::identity(),
-            color,
-            &mut nodes,
-        );
-        self.b2sn.insert(collider.parent(), nodes);
-    }
-
-    fn do_add_shape(
-        &mut self,
-        window: &mut Window,
-        handle: ColliderHandle,
-        shape: &dyn Shape,
-        delta: &Isometry<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        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,
-                *delta,
-                ball.radius,
-                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,
-            )))
-        }
-
-        if let Some(capsule) = shape.as_capsule() {
-            out.push(Node::Capsule(Capsule::new(
-                handle, *delta, capsule, color, window,
-            )))
-        }
-
-        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],
-                vec![[0, 1, 2]],
-                color,
-                window,
-            )))
-        }
-
-        if let Some(trimesh) = shape.as_trimesh() {
-            out.push(Node::Mesh(Mesh::new(
-                handle,
-                trimesh.vertices().to_vec(),
-                trimesh.indices().to_vec(),
-                color,
-                window,
-            )))
-        }
-
-        if let Some(polyline) = shape.as_polyline() {
-            out.push(Node::Polyline(Polyline::new(
-                handle,
-                polyline.vertices().to_vec(),
-                polyline.indices().to_vec(),
-                color,
-            )))
-        }
-
-        if let Some(heightfield) = shape.as_heightfield() {
-            out.push(Node::HeightField(HeightField::new(
-                handle,
-                heightfield,
-                color,
-                window,
-            )))
-        }
-
-        #[cfg(feature = "dim2")]
-        if let Some(convex_polygon) = shape
-            .as_convex_polygon()
-            .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, *delta, vertices, color, window,
-            )))
-        }
-
-        #[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, *delta, vertices, indices, color, window,
-            )))
-        }
-
-        #[cfg(feature = "dim3")]
-        if let Some(cylinder) = shape
-            .as_cylinder()
-            .or(shape.as_round_cylinder().map(|r| &r.base_shape))
-        {
-            out.push(Node::Cylinder(Cylinder::new(
-                handle,
-                *delta,
-                cylinder.half_height,
-                cylinder.radius,
-                color,
-                window,
-            )))
-        }
-
-        #[cfg(feature = "dim3")]
-        if let Some(cone) = shape
-            .as_cone()
-            .or(shape.as_round_cone().map(|r| &r.base_shape))
-        {
-            out.push(Node::Cone(Cone::new(
-                handle,
-                *delta,
-                cone.half_height,
-                cone.radius,
-                color,
-                window,
-            )))
-        }
-    }
-
-    /*
-    fn add_plane(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        shape: &shape::Plane<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let pos = colliders.get(object).unwrap().position();
-        let position = Point::from(pos.translation.vector);
-        let normal = pos * shape.normal();
-
-        out.push(Node::Plane(Plane::new(
-            object, colliders, &position, &normal, color, window,
-        )))
-    }
-
-    #[cfg(feature = "dim2")]
-    fn add_polyline(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        shape: &shape::Polyline<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let vertices = shape.points().to_vec();
-        let indices = shape.edges().iter().map(|e| e.indices).collect();
-
-        out.push(Node::Polyline(Polyline::new(
-            object, colliders, delta, vertices, indices, color, window,
-        )))
-    }
-
-    #[cfg(feature = "dim3")]
-    fn add_mesh(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        shape: &TriMesh<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let points = shape.points();
-        let faces = shape.faces();
-
-        let is = faces
-            .iter()
-            .map(|f| Point3::new(f.indices.x as u32, f.indices.y as u32, f.indices.z as u32))
-            .collect();
-
-        out.push(Node::Mesh(Mesh::new(
-            object,
-            colliders,
-            delta,
-            points.to_vec(),
-            is,
-            color,
-            window,
-        )))
-    }
-
-    fn add_heightfield(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        heightfield: &shape::HeightField<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        out.push(Node::HeightField(HeightField::new(
-            object,
-            colliders,
-            delta,
-            heightfield,
-            color,
-            window,
-        )))
-    }
-
-    fn add_capsule(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        shape: &shape::Capsule<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let margin = colliders.get(object).unwrap().margin();
-        out.push(Node::Capsule(Capsule::new(
-            object,
-            colliders,
-            delta,
-            shape.radius() + margin,
-            shape.height(),
-            color,
-            window,
-        )))
-    }
-
-    fn add_ball(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        shape: &shape::Ball<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let margin = colliders.get(object).unwrap().margin();
-        out.push(Node::Ball(Ball::new(
-            object,
-            colliders,
-            delta,
-            shape.radius() + margin,
-            color,
-            window,
-        )))
-    }
-
-    fn add_box(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        shape: &Cuboid,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let margin = colliders.get(object).unwrap().margin();
-
-        out.push(Node::Box(Box::new(
-            object,
-            colliders,
-            delta,
-            shape.half_extents() + Vector::repeat(margin),
-            color,
-            window,
-        )))
-    }
-
-    #[cfg(feature = "dim2")]
-    fn add_convex(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        shape: &ConvexPolygon<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let points = shape.points();
-
-        out.push(Node::Convex(Convex::new(
-            object,
-            colliders,
-            delta,
-            points.to_vec(),
-            color,
-            window,
-        )))
-    }
-
-    #[cfg(feature = "dim3")]
-    fn add_convex(
-        &mut self,
-        window: &mut Window,
-        object: DefaultColliderHandle,
-        colliders: &DefaultColliderSet<f32>,
-        delta: Isometry<f32>,
-        shape: &ConvexHull<f32>,
-        color: Point3<f32>,
-        out: &mut Vec<Node>,
-    ) {
-        let mut chull = transformation::convex_hull(shape.points());
-        chull.replicate_vertices();
-        chull.recompute_normals();
-
-        out.push(Node::Convex(Convex::new(
-            object, colliders, delta, &chull, color, window,
-        )))
-    }
-    */
-
-    pub fn draw(&mut self, _bodies: &RigidBodySet, colliders: &ColliderSet, window: &mut Window) {
-        // use kiss3d::camera::Camera;
-        // println!(
-        //     "camera eye {:?}, at: {:?}",
-        //     self.camera.eye(),
-        //     self.camera.at()
-        // );
-        for (_, ns) in self.b2sn.iter_mut() {
-            for n in ns.iter_mut() {
-                // if let Some(co) = colliders.get(n.collider()) {
-                //     let bo = &_bodies[co.parent()];
-                //
-                //     if bo.is_dynamic() {
-                //         if bo.is_ccd_active() {
-                //             n.set_color(Point3::new(1.0, 0.0, 0.0));
-                //         } else {
-                //             n.set_color(Point3::new(0.0, 1.0, 0.0));
-                //         }
-                //     }
-                // }
-
-                n.update(colliders);
-                n.draw(window);
-            }
-        }
-    }
-
-    // pub fn draw_positions(&mut self, window: &mut Window, rbs: &RigidBodies<f32>) {
-    //     for (_, ns) in self.b2sn.iter_mut() {
-    //         for n in ns.iter_mut() {
-    //             let object = n.object();
-    //             let rb = rbs.get(object).expect("Rigid body not found.");
-
-    //             // if let WorldObjectBorrowed::RigidBody(rb) = object {
-    //                 let t      = rb.position();
-    //                 let center = rb.center_of_mass();
-
-    //                 let rotmat = t.rotation.to_rotation_matrix().unwrap();
-    //                 let x = rotmat.column(0) * 0.25f32;
-    //                 let y = rotmat.column(1) * 0.25f32;
-    //                 let z = rotmat.column(2) * 0.25f32;
-
-    //                 window.draw_line(center, &(*center + x), &Point3::new(1.0, 0.0, 0.0));
-    //                 window.draw_line(center, &(*center + y), &Point3::new(0.0, 1.0, 0.0));
-    //                 window.draw_line(center, &(*center + z), &Point3::new(0.0, 0.0, 1.0));
-    //             // }
-    //         }
-    //     }
-    // }
-
-    pub fn camera(&self) -> &Camera {
-        &self.camera
-    }
-
-    pub fn camera_mut(&mut self) -> &mut Camera {
-        &mut self.camera
-    }
-
-    #[cfg(feature = "dim3")]
-    pub fn look_at(&mut self, eye: Point<f32>, at: Point<f32>) {
-        self.camera.look_at(eye, at);
-    }
-
-    #[cfg(feature = "dim2")]
-    pub fn look_at(&mut self, at: Point<f32>, zoom: f32) {
-        self.camera.look_at(at, zoom);
-    }
-
-    pub fn body_nodes(&self, handle: RigidBodyHandle) -> Option<&Vec<Node>> {
-        self.b2sn.get(&handle)
-    }
-
-    pub fn body_nodes_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut Vec<Node>> {
-        self.b2sn.get_mut(&handle)
-    }
-
-    pub fn nodes(&self) -> impl Iterator<Item = &Node> {
-        self.b2sn.values().flat_map(|val| val.iter())
-    }
-
-    pub fn nodes_mut(&mut self) -> impl Iterator<Item = &mut Node> {
-        self.b2sn.values_mut().flat_map(|val| val.iter_mut())
-    }
-
-    #[cfg(feature = "dim3")]
-    pub fn set_up_axis(&mut self, up_axis: na::Vector3<f32>) {
-        self.camera.set_up_axis(up_axis);
-    }
-}
-
-impl Default for GraphicsManager {
-    fn default() -> Self {
-        Self::new()
-    }
-}
diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs
new file mode 100644
index 0000000..2e3d695
--- /dev/null
+++ b/src_testbed/graphics.rs
@@ -0,0 +1,400 @@
+use bevy::prelude::*;
+
+use na::Point3;
+
+use crate::math::Isometry;
+use crate::objects::node::EntityWithGraphics;
+use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
+use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
+//use crate::objects::capsule::Capsule;
+//#[cfg(feature = "dim3")]
+//use crate::objects::mesh::Mesh;
+//use crate::objects::plane::Plane;
+//#[cfg(feature = "dim2")]
+//use crate::objects::polyline::Polyline;
+// use crate::objects::mesh::Mesh;
+use rand::{Rng, SeedableRng};
+use rand_pcg::Pcg32;
+use std::collections::HashMap;
+
+pub struct GraphicsManager {
+    rand: Pcg32,
+    b2sn: HashMap<RigidBodyHandle, Vec<EntityWithGraphics>>,
+    b2color: HashMap<RigidBodyHandle, Point3<f32>>,
+    c2color: HashMap<ColliderHandle, Point3<f32>>,
+    b2wireframe: HashMap<RigidBodyHandle, bool>,
+    ground_color: Point3<f32>,
+    prefab_meshes: HashMap<ShapeType, Handle<Mesh>>,
+}
+
+impl GraphicsManager {
+    pub fn new() -> GraphicsManager {
+        GraphicsManager {
+            rand: Pcg32::seed_from_u64(0),
+            b2sn: HashMap::new(),
+            b2color: HashMap::new(),
+            c2color: HashMap::new(),
+            ground_color: Point3::new(0.5, 0.5, 0.5),
+            b2wireframe: HashMap::new(),
+            prefab_meshes: HashMap::new(),
+        }
+    }
+
+    pub fn clear(&mut self, commands: &mut Commands) {
+        for sns in self.b2sn.values_mut() {
+            for sn in sns.iter_mut() {
+                commands.entity(sn.entity).despawn()
+            }
+        }
+
+        self.b2sn.clear();
+        self.c2color.clear();
+        self.b2color.clear();
+        self.b2wireframe.clear();
+        self.rand = Pcg32::seed_from_u64(0);
+    }
+
+    pub fn remove_collider_nodes(
+        &mut self,
+        commands: &mut Commands,
+        body: RigidBodyHandle,
+        collider: ColliderHandle,
+    ) {
+        if let Some(sns) = self.b2sn.get_mut(&body) {
+            for sn in sns.iter_mut() {
+                if sn.collider == collider {
+                    commands.entity(sn.entity).despawn();
+                }
+            }
+        }
+    }
+
+    pub fn remove_body_nodes(&mut self, commands: &mut Commands, body: RigidBodyHandle) {
+        if let Some(sns) = self.b2sn.get_mut(&body) {
+            for sn in sns.iter_mut() {
+                commands.entity(sn.entity).despawn();
+            }
+        }
+
+        self.b2sn.remove(&body);
+    }
+
+    pub fn set_body_color(
+        &mut self,
+        materials: &mut Assets<StandardMaterial>,
+        b: RigidBodyHandle,
+        color: Point3<f32>,
+    ) {
+        self.b2color.insert(b, color);
+
+        if let Some(ns) = self.b2sn.get_mut(&b) {
+            for n in ns.iter_mut() {
+                n.set_color(materials, color)
+            }
+        }
+    }
+
+    pub fn set_initial_body_color(&mut self, b: RigidBodyHandle, color: Point3<f32>) {
+        self.b2color.insert(b, color);
+    }
+
+    pub fn set_initial_collider_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);
+
+        if let Some(_ns) = self.b2sn.get_mut(&b) {
+            // for n in ns.iter_mut().filter_map(|n| n.scene_node_mut()) {
+            //     if enabled {
+            //         n.set_surface_rendering_activation(true);
+            //         n.set_lines_width(1.0);
+            //     } else {
+            //         n.set_surface_rendering_activation(false);
+            //         n.set_lines_width(1.0);
+            //     }
+            // }
+        }
+    }
+
+    pub fn toggle_wireframe_mode(&mut self, colliders: &ColliderSet, _enabled: bool) {
+        for n in self.b2sn.values_mut().flat_map(|val| val.iter_mut()) {
+            let _force_wireframe = if let Some(collider) = colliders.get(n.collider) {
+                collider.is_sensor()
+                    || self
+                        .b2wireframe
+                        .get(&collider.parent())
+                        .cloned()
+                        .unwrap_or(false)
+            } else {
+                false
+            };
+
+            // if let Some(node) = n.scene_node_mut() {
+            //     if force_wireframe || enabled {
+            //         node.set_lines_width(1.0);
+            //         node.set_surface_rendering_activation(false);
+            //     } else {
+            //         node.set_lines_width(0.0);
+            //         node.set_surface_rendering_activation(true);
+            //     }
+            // }
+        }
+    }
+
+    pub fn next_color(&mut self) -> Point3<f32> {
+        Self::gen_color(&mut self.rand)
+    }
+
+    fn gen_color(rng: &mut Pcg32) -> Point3<f32> {
+        let mut color: Point3<f32> = rng.gen();
+        color *= 1.5;
+        color.x = color.x.min(1.0);
+        color.y = color.y.min(1.0);
+        color.z = color.z.min(1.0);
+        color
+    }
+
+    fn alloc_color(
+        &mut self,
+        materials: &mut Assets<StandardMaterial>,
+        handle: RigidBodyHandle,
+        is_static: bool,
+    ) -> Point3<f32> {
+        let mut color = self.ground_color;
+
+        if !is_static {
+            match self.b2color.get(&handle).cloned() {
+                Some(c) => color = c,
+                None => color = Self::gen_color(&mut self.rand),
+            }
+        }
+
+        self.set_body_color(materials, handle, color);
+
+        color
+    }
+
+    pub fn add(
+        &mut self,
+        commands: &mut Commands,
+        meshes: &mut Assets<Mesh>,
+        materials: &mut Assets<StandardMaterial>,
+        components: &mut Query<(&mut Transform,)>,
+        handle: RigidBodyHandle,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+    ) {
+        let body = bodies.get(handle).unwrap();
+
+        let color = self
+            .b2color
+            .get(&handle)
+            .cloned()
+            .unwrap_or_else(|| self.alloc_color(materials, handle, !body.is_dynamic()));
+
+        self.add_with_color(
+            commands, meshes, materials, components, handle, bodies, colliders, color,
+        )
+    }
+
+    pub fn add_with_color(
+        &mut self,
+        commands: &mut Commands,
+        meshes: &mut Assets<Mesh>,
+        materials: &mut Assets<StandardMaterial>,
+        components: &mut Query<(&mut Transform,)>,
+        handle: RigidBodyHandle,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        color: Point3<f32>,
+    ) {
+        //        let body = bodies.get(handle).unwrap();
+        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.do_add_shape(
+                commands,
+                meshes,
+                materials,
+                *collider_handle,
+                collider.shape(),
+                collider.is_sensor(),
+                collider.position(),
+                &Isometry::identity(),
+                color,
+                &mut new_nodes,
+            );
+        }
+
+        new_nodes
+            .iter_mut()
+            .for_each(|n| n.update(colliders, components));
+
+        // for node in new_nodes.iter_mut().filter_map(|n| n.scene_node_mut()) {
+        //     if self.b2wireframe.get(&handle).cloned() == Some(true) {
+        //         node.set_lines_width(1.0);
+        //         node.set_surface_rendering_activation(false);
+        //     } else {
+        //         node.set_lines_width(0.0);
+        //         node.set_surface_rendering_activation(true);
+        //     }
+        // }
+
+        let nodes = self.b2sn.entry(handle).or_insert_with(Vec::new);
+        nodes.append(&mut new_nodes);
+    }
+
+    pub fn add_collider(
+        &mut self,
+        commands: &mut Commands,
+        meshes: &mut Assets<Mesh>,
+        materials: &mut Assets<StandardMaterial>,
+        handle: ColliderHandle,
+        colliders: &ColliderSet,
+    ) {
+        let collider = &colliders[handle];
+        let color = *self.b2color.get(&collider.parent()).unwrap();
+        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_shape(
+            commands,
+            meshes,
+            materials,
+            handle,
+            collider.shape(),
+            collider.is_sensor(),
+            collider.position(),
+            &Isometry::identity(),
+            color,
+            &mut nodes,
+        );
+        self.b2sn.insert(collider.parent(), nodes);
+    }
+
+    fn do_add_shape(
+        &mut self,
+        commands: &mut Commands,
+        meshes: &mut Assets<Mesh>,
+        materials: &mut Assets<StandardMaterial>,
+        handle: ColliderHandle,
+        shape: &dyn Shape,
+        sensor: bool,
+        pos: &Isometry<f32>,
+        delta: &Isometry<f32>,
+        color: Point3<f32>,
+        out: &mut Vec<EntityWithGraphics>,
+    ) {
+        if let Some(compound) = shape.as_compound() {
+            for (shape_pos, shape) in compound.shapes() {
+                self.do_add_shape(
+                    commands,
+                    meshes,
+                    materials,
+                    handle,
+                    &**shape,
+                    sensor,
+                    pos,
+                    &(shape_pos * delta),
+                    color,
+                    out,
+                )
+            }
+        } else {
+            if self.prefab_meshes.is_empty() {
+                EntityWithGraphics::gen_prefab_meshes(&mut self.prefab_meshes, meshes);
+            }
+
+            let node = EntityWithGraphics::spawn(
+                commands,
+                meshes,
+                materials,
+                &self.prefab_meshes,
+                shape,
+                handle,
+                *pos,
+                *delta,
+                color,
+                sensor,
+            );
+            out.push(node);
+        }
+    }
+
+    pub fn draw(
+        &mut self,
+        _bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        components: &mut Query<(&mut Transform,)>,
+    ) {
+        for (_, ns) in self.b2sn.iter_mut() {
+            for n in ns.iter_mut() {
+                // if let Some(co) = colliders.get(n.collider()) {
+                //     let bo = &_bodies[co.parent()];
+                //
+                //     if bo.is_dynamic() {
+                //         if bo.is_ccd_active() {
+                //             n.set_color(Point3::new(1.0, 0.0, 0.0));
+                //         } else {
+                //             n.set_color(Point3::new(0.0, 1.0, 0.0));
+                //         }
+                //     }
+                // }
+
+                n.update(colliders, components);
+            }
+        }
+    }
+
+    // pub fn draw_positions(&mut self, window: &mut Window, rbs: &RigidBodies<f32>) {
+    //     for (_, ns) in self.b2sn.iter_mut() {
+    //         for n in ns.iter_mut() {
+    //             let object = n.object();
+    //             let rb = rbs.get(object).expect("Rigid body not found.");
+
+    //             // if let WorldObjectBorrowed::RigidBody(rb) = object {
+    //                 let t      = rb.position();
+    //                 let center = rb.center_of_mass();
+
+    //                 let rotmat = t.rotation.to_rotation_matrix().unwrap();
+    //                 let x = rotmat.column(0) * 0.25f32;
+    //                 let y = rotmat.column(1) * 0.25f32;
+    //                 let z = rotmat.column(2) * 0.25f32;
+
+    //                 window.draw_line(center, &(*center + x), &Point3::new(1.0, 0.0, 0.0));
+    //                 window.draw_line(center, &(*center + y), &Point3::new(0.0, 1.0, 0.0));
+    //                 window.draw_line(center, &(*center + z), &Point3::new(0.0, 0.0, 1.0));
+    //             // }
+    //         }
+    //     }
+    // }
+
+    pub fn body_nodes(&self, handle: RigidBodyHandle) -> Option<&Vec<EntityWithGraphics>> {
+        self.b2sn.get(&handle)
+    }
+
+    pub fn body_nodes_mut(
+        &mut self,
+        handle: RigidBodyHandle,
+    ) -> Option<&mut Vec<EntityWithGraphics>> {
+        self.b2sn.get_mut(&handle)
+    }
+
+    pub fn nodes(&self) -> impl Iterator<Item = &EntityWithGraphics> {
+        self.b2sn.values().flat_map(|val| val.iter())
+    }
+
+    pub fn nodes_mut(&mut self) -> impl Iterator<Item = &mut EntityWithGraphics> {
+        self.b2sn.values_mut().flat_map(|val| val.iter_mut())
+    }
+}
+
+impl Default for GraphicsManager {
+    fn default() -> Self {
+        Self::new()
+    }
+}
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index e37e63d..c85fbc4 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -1,8 +1,7 @@
 use crate::{
     physics::{PhysicsEvents, PhysicsState},
-    GraphicsManager,
+    TestbedGraphics,
 };
-use kiss3d::window::Window;
 use plugin::HarnessPlugin;
 use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet};
 use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
@@ -52,17 +51,8 @@ pub struct Harness {
     pub state: RunState,
 }
 
-type Callbacks = Vec<
-    Box<
-        dyn FnMut(
-            Option<&mut Window>,
-            Option<&mut GraphicsManager>,
-            &mut PhysicsState,
-            &PhysicsEvents,
-            &RunState,
-        ),
-    >,
->;
+type Callbacks =
+    Vec<Box<dyn FnMut(Option<&mut TestbedGraphics>, &mut PhysicsState, &PhysicsEvents, &RunState)>>;
 
 #[allow(dead_code)]
 impl Harness {
@@ -145,13 +135,7 @@ impl Harness {
     }
 
     pub fn add_callback<
-        F: FnMut(
-                Option<&mut Window>,
-                Option<&mut GraphicsManager>,
-                &mut PhysicsState,
-                &PhysicsEvents,
-                &RunState,
-            ) + 'static,
+        F: FnMut(Option<&mut TestbedGraphics>, &mut PhysicsState, &PhysicsEvents, &RunState) + 'static,
     >(
         &mut self,
         callback: F,
@@ -160,14 +144,10 @@ impl Harness {
     }
 
     pub fn step(&mut self) {
-        self.step_with_graphics(None, None);
+        self.step_with_graphics(None);
     }
 
-    pub fn step_with_graphics(
-        &mut self,
-        window: Option<&mut Window>,
-        graphics: Option<&mut GraphicsManager>,
-    ) {
+    pub fn step_with_graphics(&mut self, mut graphics: Option<&mut TestbedGraphics>) {
         #[cfg(feature = "parallel")]
         {
             let physics = &mut self.physics;
@@ -214,26 +194,13 @@ impl Harness {
             plugin.step(&mut self.physics, &self.state)
         }
 
-        // FIXME: This assumes either window & graphics are Some, or they are all None
-        // this is required as we cannot pass Option<&mut Window> & Option<&mut GraphicsManager directly in a loop
-        // there must be a better way of doing this?
-        match (window, graphics) {
-            (Some(window), Some(graphics)) => {
-                for f in &mut self.callbacks {
-                    f(
-                        Some(window),
-                        Some(graphics),
-                        &mut self.physics,
-                        &self.events,
-                        &self.state,
-                    );
-                }
-            }
-            _ => {
-                for f in &mut self.callbacks {
-                    f(None, None, &mut self.physics, &self.events, &self.state);
-                }
-            }
+        for f in &mut self.callbacks {
+            f(
+                graphics.as_deref_mut(),
+                &mut self.physics,
+                &self.events,
+                &self.state,
+            );
         }
 
         for plugin in &mut self.plugins {
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index af9d633..db362cc 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -1,5 +1,3 @@
-#[macro_use]
-extern crate kiss3d;
 extern crate nalgebra as na;
 #[cfg(feature = "dim2")]
 extern crate ncollide2d as ncollide;
@@ -25,15 +23,16 @@ extern crate bitflags;
 #[macro_use]
 extern crate log;
 
-pub use crate::engine::GraphicsManager;
+pub use crate::graphics::GraphicsManager;
 pub use crate::harness::plugin::HarnessPlugin;
 pub use crate::physics::PhysicsState;
 pub use crate::plugin::TestbedPlugin;
-pub use crate::testbed::Testbed;
+pub use crate::testbed::{Testbed, TestbedApp, TestbedGraphics};
 
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
 mod box2d_backend;
-mod engine;
+mod camera;
+mod graphics;
 pub mod harness;
 #[cfg(feature = "other-backends")]
 mod nphysics_backend;
diff --git a/src_testbed/objects/ball.rs b/src_testbed/objects/ball.rs
deleted file mode 100644
index df43341..0000000
--- a/src_testbed/objects/ball.rs
+++ /dev/null
@@ -1,76 +0,0 @@
-use crate::objects::node::{self, GraphicsNode};
-use kiss3d::window::Window;
-use na::Point3;
-use rapier::geometry::{ColliderHandle, ColliderSet};
-use rapier::math::Isometry;
-
-pub struct Ball {
-    color: Point3<f32>,
-    base_color: Point3<f32>,
-    gfx: GraphicsNode,
-    collider: ColliderHandle,
-    delta: Isometry<f32>,
-}
-
-impl Ball {
-    pub fn new(
-        collider: ColliderHandle,
-        delta: Isometry<f32>,
-        radius: f32,
-        color: Point3<f32>,
-        window: &mut Window,
-    ) -> Ball {
-        #[cfg(feature = "dim2")]
-        let node = window.add_circle(radius);
-        #[cfg(feature = "dim3")]
-        let node = window.add_sphere(radius);
-
-        let mut res = Ball {
-            color,
-            base_color: color,
-            gfx: node,
-            collider,
-            delta,
-        };
-
-        // 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,
-            &self.delta,
-        );
-    }
-
-    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/box_node.rs b/src_testbed/objects/box_node.rs
deleted file mode 100644
index 0586d77..0000000
--- a/src_testbed/objects/box_node.rs
+++ /dev/null
@@ -1,76 +0,0 @@
-use crate::objects::node::{self, GraphicsNode};
-use kiss3d::window;
-use na::Point3;
-use rapier::geometry::{ColliderHandle, ColliderSet};
-use rapier::math::{Isometry, Vector};
-
-pub struct Box {
-    color: Point3<f32>,
-    base_color: Point3<f32>,
-    gfx: GraphicsNode,
-    collider: ColliderHandle,
-    delta: Isometry<f32>,
-}
-
-impl Box {
-    pub fn new(
-        collider: ColliderHandle,
-        delta: Isometry<f32>,
-        half_extents: Vector<f32>,
-        color: Point3<f32>,
-        window: &mut window::Window,
-    ) -> Box {
-        let extents = half_extents * 2.0;
-        #[cfg(feature = "dim2")]
-        let node = window.add_rectangle(extents.x, extents.y);
-        #[cfg(feature = "dim3")]
-        let node = window.add_cube(extents.x, extents.y, extents.z);
-
-        let mut res = Box {
-            color,
-            base_color: color,
-            gfx: node,
-            collider,
-            delta,
-        };
-
-        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,
-            &self.delta,
-        );
-    }
-
-    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/capsule.rs b/src_testbed/objects/capsule.rs
deleted file mode 100644
index 3d3e88f..0000000
--- a/src_testbed/objects/capsule.rs
+++ /dev/null
@@ -1,77 +0,0 @@
-use crate::objects::node::{self, GraphicsNode};
-use kiss3d::window;
-use na::Point3;
-use rapier::geometry::{self, ColliderHandle, ColliderSet};
-use rapier::math::Isometry;
-
-pub struct Capsule {
-    color: Point3<f32>,
-    base_color: Point3<f32>,
-    gfx: GraphicsNode,
-    collider: ColliderHandle,
-    delta: Isometry<f32>,
-}
-
-impl Capsule {
-    pub fn new(
-        collider: ColliderHandle,
-        delta: Isometry<f32>,
-        capsule: &geometry::Capsule,
-        color: Point3<f32>,
-        window: &mut window::Window,
-    ) -> Capsule {
-        let r = capsule.radius;
-        let h = capsule.half_height() * 2.0;
-        #[cfg(feature = "dim2")]
-        let node = window.add_planar_capsule(r, h);
-        #[cfg(feature = "dim3")]
-        let node = window.add_capsule(r, h);
-
-        let mut res = Capsule {
-            color,
-            base_color: color,
-            gfx: node,
-            collider,
-            delta: delta * capsule.transform_wrt_y(),
-        };
-
-        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 update(&mut self, colliders: &ColliderSet) {
-        node::update_scene_node(
-            &mut self.gfx,
-            colliders,
-            self.collider,
-            &self.color,
-            &self.delta,
-        );
-    }
-
-    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 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/cone.rs b/src_testbed/objects/cone.rs
deleted file mode 100644
index 7aa2039..0000000
--- a/src_testbed/objects/cone.rs
+++ /dev/null
@@ -1,77 +0,0 @@
-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,
-    delta: Isometry<f32>,
-}
-
-impl Cone {
-    pub fn new(
-        collider: ColliderHandle,
-        delta: Isometry<f32>,
-        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,
-            delta,
-        };
-
-        // 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,
-            &self.delta,
-        );
-    }
-
-    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/convex.rs b/src_testbed/objects/convex.rs
deleted file mode 100644
index 95ef74a..0000000
--- a/src_testbed/objects/convex.rs
+++ /dev/null
@@ -1,96 +0,0 @@
-#![allow(warnings)] // TODO: remove this.
-
-#[cfg(feature = "dim2")]
-use crate::math::Vector;
-use crate::math::{Isometry, Point};
-use crate::objects::node::{self, GraphicsNode};
-use kiss3d::window::Window;
-use na::Point3;
-use rapier::geometry::{ColliderHandle, ColliderSet};
-
-pub struct Convex {
-    color: Point3<f32>,
-    base_color: Point3<f32>,
-    gfx: GraphicsNode,
-    body: ColliderHandle,
-    delta: Isometry<f32>,
-}
-
-impl Convex {
-    pub fn new(
-        body: ColliderHandle,
-        delta: Isometry<f32>,
-        vertices: Vec<Point<f32>>,
-        #[cfg(feature = "dim3")] indices: Vec<[u32; 3]>,
-        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 = {
-            use std::cell::RefCell;
-            use std::rc::Rc;
-            let mut mesh_vertices = Vec::new();
-            let mut mesh_indices = Vec::new();
-            for idx in indices {
-                let i = mesh_vertices.len() as u16;
-                mesh_vertices.push(vertices[idx[0] as usize]);
-                mesh_vertices.push(vertices[idx[1] as usize]);
-                mesh_vertices.push(vertices[idx[2] as usize]);
-                mesh_indices.push(Point3::new(i, i + 1, i + 2));
-            }
-
-            let mesh = kiss3d::resource::Mesh::new(mesh_vertices, mesh_indices, None, None, false);
-            window.add_mesh(Rc::new(RefCell::new(mesh)), na::Vector3::from_element(1.0))
-        };
-
-        let mut res = Convex {
-            color,
-            base_color: color,
-            gfx: node,
-            body,
-            delta,
-        };
-
-        // 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.body,
-            &self.color,
-            &self.delta,
-        );
-    }
-
-    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.body
-    }
-}
diff --git a/src_testbed/objects/cylinder.rs b/src_testbed/objects/cylinder.rs
deleted file mode 100644
index ff4dc72..0000000
--- a/src_testbed/objects/cylinder.rs
+++ /dev/null
@@ -1,77 +0,0 @@
-use crate::objects::node::{self, GraphicsNode};
-use kiss3d::window::Window;
-use na::Point3;
-use rapier::geometry::{ColliderHandle, ColliderSet};
-use rapier::math::Isometry;
-
-pub struct Cylinder {
-    color: Point3<f32>,
-    base_color: Point3<f32>,
-    gfx: GraphicsNode,
-    collider: ColliderHandle,
-    delta: Isometry<f32>,
-}
-
-impl Cylinder {
-    pub fn new(
-        collider: ColliderHandle,
-        delta: Isometry<f32>,
-        half_height: f32,
-        radius: f32,
-        color: Point3<f32>,
-        window: &mut Window,
-    ) -> Cylinder {
-        #[cfg(feature = "dim2")]
-        let node = window.add_rectangle(radius, half_height);
-        #[cfg(feature = "dim3")]
-        let node = window.add_cylinder(radius, half_height * 2.0);
-
-        let mut res = Cylinder {
-            color,
-            base_color: color,
-            gfx: node,
-            collider,
-            delta,
-        };
-
-        // 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,
-            &self.delta,
-        );
-    }
-
-    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/heightfield.rs b/src_testbed/objects/heightfield.rs
deleted file mode 100644
index 913a732..0000000
--- a/src_testbed/objects/heightfield.rs
+++ /dev/null
@@ -1,128 +0,0 @@
-use kiss3d::window::Window;
-use na::{self, Point3};
-use parry::shape;
-use rapier::geometry::{ColliderHandle, ColliderSet};
-#[cfg(feature = "dim2")]
-use rapier::math::Point;
-#[cfg(feature = "dim3")]
-use {
-    crate::objects::node::{self, GraphicsNode},
-    kiss3d::resource::Mesh,
-    rapier::math::Vector,
-    std::cell::RefCell,
-};
-
-pub struct HeightField {
-    color: Point3<f32>,
-    base_color: Point3<f32>,
-    #[cfg(feature = "dim2")]
-    vertices: Vec<Point<f32>>,
-    #[cfg(feature = "dim3")]
-    gfx: GraphicsNode,
-    collider: ColliderHandle,
-}
-
-impl HeightField {
-    #[cfg(feature = "dim2")]
-    pub fn new(
-        collider: ColliderHandle,
-        heightfield: &shape::HeightField,
-        color: Point3<f32>,
-        _: &mut Window,
-    ) -> HeightField {
-        let mut vertices = Vec::new();
-
-        for seg in heightfield.segments() {
-            vertices.push(seg.a);
-            vertices.push(seg.b);
-        }
-
-        HeightField {
-            color,
-            base_color: color,
-            vertices,
-            collider,
-        }
-    }
-
-    #[cfg(feature = "dim3")]
-    pub fn new(
-        collider: ColliderHandle,
-        heightfield: &shape::HeightField,
-        color: Point3<f32>,
-        window: &mut Window,
-    ) -> HeightField {
-        use std::rc::Rc;
-
-        let (vertices, indices) = heightfield.to_trimesh();
-        let indices = indices
-            .into_iter()
-            .map(|idx| Point3::new(idx[0] as u16, idx[1] as u16, idx[2] as u16))
-            .collect();
-        let mesh = Mesh::new(vertices, indices, None, None, false);
-
-        let mut res = HeightField {
-            color,
-            base_color: color,
-            gfx: window.add_mesh(Rc::new(RefCell::new(mesh)), Vector::repeat(1.0)),
-            collider: collider,
-        };
-
-        res.gfx.enable_backface_culling(false);
-        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>) {
-        #[cfg(feature = "dim3")]
-        {
-            self.gfx.set_color(color.x, color.y, color.z);
-        }
-        self.color = color;
-        self.base_color = color;
-    }
-
-    #[cfg(feature = "dim3")]
-    pub fn update(&mut self, colliders: &ColliderSet) {
-        node::update_scene_node(
-            &mut self.gfx,
-            colliders,
-            self.collider,
-            &self.color,
-            &na::Isometry::identity(),
-        );
-    }
-
-    #[cfg(feature = "dim2")]
-    pub fn update(&mut self, _colliders: &ColliderSet) {}
-
-    #[cfg(feature = "dim3")]
-    pub fn scene_node(&self) -> &GraphicsNode {
-        &self.gfx
-    }
-
-    #[cfg(feature = "dim3")]
-    pub fn scene_node_mut(&mut self) -> &mut GraphicsNode {
-        &mut self.gfx
-    }
-
-    pub fn object(&self) -> ColliderHandle {
-        self.collider
-    }
-
-    #[cfg(feature = "dim2")]
-    pub fn draw(&mut self, window: &mut Window) {
-        for vtx in self.vertices.chunks(2) {
-            window.draw_planar_line(&vtx[0], &vtx[1], &self.color)
-        }
-    }
-}
diff --git a/src_testbed/objects/mesh.rs b/src_testbed/objects/mesh.rs
deleted file mode 100644
index 5505de3..0000000
--- a/src_testbed/objects/mesh.rs
+++ /dev/null
@@ -1,111 +0,0 @@
-use crate::objects::node::{self, GraphicsNode};
-use kiss3d::window;
-use na::Point3;
-use rapier::geometry::{ColliderHandle, ColliderSet};
-use rapier::math::{Isometry, Point};
-use std::cell::RefCell;
-use std::rc::Rc;
-
-pub struct Mesh {
-    color: Point3<f32>,
-    base_color: Point3<f32>,
-    gfx: GraphicsNode,
-    collider: ColliderHandle,
-}
-
-impl Mesh {
-    pub fn new(
-        collider: ColliderHandle,
-        vertices: Vec<Point<f32>>,
-        indices: Vec<[u32; 3]>,
-        color: Point3<f32>,
-        window: &mut window::Window,
-    ) -> Mesh {
-        let vs = vertices;
-        let is = indices
-            .into_iter()
-            .map(|idx| Point3::new(idx[0] as u16, idx[1] as u16, idx[2] as u16))
-            .collect();
-
-        let mesh;
-        let gfx;
-
-        #[cfg(feature = "dim2")]
-        {
-            mesh = kiss3d::resource::PlanarMesh::new(vs, is, None, false);
-            gfx = window.add_planar_mesh(
-                Rc::new(RefCell::new(mesh)),
-                crate::math::Vector::from_element(1.0),
-            );
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            mesh = kiss3d::resource::Mesh::new(vs, is, None, None, false);
-            gfx = window.add_mesh(Rc::new(RefCell::new(mesh)), na::Vector3::from_element(1.0));
-        }
-
-        let mut res = Mesh {
-            color,
-            base_color: color,
-            gfx,
-            collider,
-        };
-
-        res.gfx.enable_backface_culling(false);
-        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(),
-        );
-
-        // // Update if some deformation occurred.
-        // // FIXME: don't update if it did not move.
-        // if let Some(c) = colliders.get(self.collider) {
-        //     if let ColliderAnchor::OnDeformableBody { .. } = c.anchor() {
-        //         let shape = c.shape().as_shape::<TriMesh<f32>>().unwrap();
-        //         let vtx = shape.points();
-        //
-        //         self.gfx.modify_vertices(&mut |vertices| {
-        //             for (v, new_v) in vertices.iter_mut().zip(vtx.iter()) {
-        //                 *v = *new_v
-        //             }
-        //         });
-        //         self.gfx.recompute_normals();
-        //     }
-        // }
-    }
-
-    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 8e12dbe..492bc84 100644
--- a/src_testbed/objects/mod.rs
+++ b/src_testbed/objects/mod.rs
@@ -1,13 +1 @@
-pub mod ball;
-pub mod box_node;
-pub mod capsule;
-pub mod cone;
-pub mod convex;
-pub mod cylinder;
-pub mod heightfield;
-pub mod mesh;
 pub mod node;
-pub mod polyline;
-//pub mod plane;
-//#[cfg(feature = "dim2")]
-//pub mod polyline;
diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs
index 1bc491f..be2fa71 100644
-