20 files changed, 716 insertions, 527 deletions

diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
index 941c6d5..29fd4fa 100644
--- a/src_testbed/box2d_backend.rs
+++ b/src_testbed/box2d_backend.rs
@@ -174,6 +174,14 @@ impl Box2dWorld {
             b2_shape.set_radius(b.radius);
             b2_shape.set_position(center);
             body.create_fixture(&b2_shape, &mut fixture_def);
+        } else if let Some(p) = shape.as_convex_polygon() {
+            let vertices: Vec<_> = p
+                .points()
+                .iter()
+                .map(|p| na_vec_to_b2_vec(p.coords))
+                .collect();
+            let b2_shape = b2::PolygonShape::new_with(&vertices);
+            body.create_fixture(&b2_shape, &mut fixture_def);
         } else if let Some(c) = shape.as_cuboid() {
             let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
             body.create_fixture(&b2_shape, &mut fixture_def);
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index 040c883..bcdbbca 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -6,15 +6,15 @@ 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::{ColliderHandle, ColliderSet, Shape};
 //use crate::objects::capsule::Capsule;
-//use crate::objects::convex::Convex;
+use crate::objects::convex::Convex;
 //#[cfg(feature = "dim3")]
 //use crate::objects::mesh::Mesh;
 //use crate::objects::plane::Plane;
@@ -26,6 +26,7 @@ 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;
@@ -237,7 +238,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,34 +275,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() {
+        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,
@@ -302,14 +325,19 @@ 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.as_triangle() {
+        if let Some(triangle) = shape
+            .as_triangle()
+            .or(shape.as_round_triangle().map(|r| &r.base_shape))
+        {
             out.push(Node::Mesh(Mesh::new(
                 handle,
                 vec![triangle.a, triangle.b, triangle.c],
-                vec![Point3::new(0, 1, 2)],
+                vec![[0, 1, 2]],
                 color,
                 window,
             )))
@@ -319,16 +347,21 @@ impl GraphicsManager {
             out.push(Node::Mesh(Mesh::new(
                 handle,
                 trimesh.vertices().to_vec(),
-                trimesh
-                    .indices()
-                    .iter()
-                    .map(|idx| na::convert(*idx))
-                    .collect(),
+                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,
@@ -338,13 +371,36 @@ impl GraphicsManager {
             )))
         }
 
+        #[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.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,
@@ -353,9 +409,13 @@ impl GraphicsManager {
         }
 
         #[cfg(feature = "dim3")]
-        if let Some(cone) = shape.as_cone() {
+        if let Some(cone) = shape
+            .as_cone()
+            .or(shape.as_round_cone().map(|r| &r.base_shape))
+        {
             out.push(Node::Cone(Cone::new(
                 handle,
+                *delta,
                 cone.half_height,
                 cone.radius,
                 color,
@@ -501,7 +561,7 @@ impl GraphicsManager {
         object: DefaultColliderHandle,
         colliders: &DefaultColliderSet<f32>,
         delta: Isometry<f32>,
-        shape: &Cuboid<f32>,
+        shape: &Cuboid,
         color: Point3<f32>,
         out: &mut Vec<Node>,
     ) {
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index 2251038..afdc23a 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -14,7 +14,6 @@ pub mod plugin;
 pub struct RunState {
     #[cfg(feature = "parallel")]
     pub thread_pool: rapier::rayon::ThreadPool,
-    #[cfg(feature = "parallel")]
     pub num_threads: usize,
     pub timestep_id: usize,
     pub time: f32,
@@ -24,6 +23,8 @@ impl RunState {
     pub fn new() -> Self {
         #[cfg(feature = "parallel")]
         let num_threads = num_cpus::get_physical();
+        #[cfg(not(feature = "parallel"))]
+        let num_threads = 1;
 
         #[cfg(feature = "parallel")]
         let thread_pool = rapier::rayon::ThreadPoolBuilder::new()
@@ -34,7 +35,6 @@ impl RunState {
         Self {
             #[cfg(feature = "parallel")]
             thread_pool: thread_pool,
-            #[cfg(feature = "parallel")]
             num_threads,
             timestep_id: 0,
             time: 0.0,
@@ -67,21 +67,12 @@ type Callbacks = Vec<
 #[allow(dead_code)]
 impl Harness {
     pub fn new_empty() -> Self {
-        #[cfg(feature = "parallel")]
-        let num_threads = num_cpus::get_physical();
-
-        #[cfg(feature = "parallel")]
-        let thread_pool = rapier::rayon::ThreadPoolBuilder::new()
-            .num_threads(num_threads)
-            .build()
-            .unwrap();
-
         let contact_channel = crossbeam::channel::unbounded();
         let proximity_channel = crossbeam::channel::unbounded();
         let event_handler = ChannelEventCollector::new(proximity_channel.0, contact_channel.0);
         let events = PhysicsEvents {
             contact_events: contact_channel.1,
-            proximity_events: proximity_channel.1,
+            intersection_events: proximity_channel.1,
         };
         let physics = PhysicsState::new();
         let state = RunState::new();
@@ -111,6 +102,10 @@ impl Harness {
         &mut self.physics.integration_parameters
     }
 
+    pub fn clear_callbacks(&mut self) {
+        self.callbacks.clear();
+    }
+
     pub fn physics_state_mut(&mut self) -> &mut PhysicsState {
         &mut self.physics
     }
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index 5358e7c..af9d633 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -10,6 +10,10 @@ extern crate nphysics2d as nphysics;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
 extern crate nphysics3d as nphysics;
 #[cfg(feature = "dim2")]
+extern crate parry2d as parry;
+#[cfg(feature = "dim3")]
+extern crate parry3d as parry;
+#[cfg(feature = "dim2")]
 extern crate rapier2d as rapier;
 #[cfg(feature = "dim3")]
 extern crate rapier3d as rapier;
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index d988904..8b88e28 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -1,4 +1,6 @@
-use ncollide::shape::{Ball, Capsule, Cuboid, ShapeHandle};
+#[cfg(feature = "dim2")]
+use ncollide::shape::ConvexPolygon;
+use ncollide::shape::{Ball, Capsule, Cuboid, HeightField, ShapeHandle};
 use nphysics::force_generator::DefaultForceGeneratorSet;
 use nphysics::joint::{
     DefaultJointConstraintSet, FixedConstraint, PrismaticConstraint, RevoluteConstraint,
@@ -144,6 +146,7 @@ impl NPhysicsWorld {
         self.mechanical_world
             .integration_parameters
             .set_dt(params.dt);
+        self.mechanical_world.integration_parameters.warmstart_coeff = params.warmstart_coeff;
 
         counters.step_started();
         self.mechanical_world.step(
@@ -175,19 +178,25 @@ fn nphysics_collider_from_rapier_collider(
     collider: &Collider,
     is_dynamic: bool,
 ) -> Option<ColliderDesc<f32>> {
-    let margin = ColliderDesc::<f32>::default_margin();
+    let mut margin = ColliderDesc::<f32>::default_margin();
     let mut pos = *collider.position_wrt_parent();
     let shape = collider.shape();
 
     let shape = if let Some(cuboid) = shape.as_cuboid() {
         ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
+    } else if let Some(cuboid) = shape.as_round_cuboid() {
+        margin = cuboid.border_radius;
+        ShapeHandle::new(Cuboid::new(cuboid.base_shape.half_extents))
     } else if let Some(ball) = shape.as_ball() {
         ShapeHandle::new(Ball::new(ball.radius - margin))
     } else if let Some(capsule) = shape.as_capsule() {
         pos *= capsule.transform_wrt_y();
         ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
     } else if let Some(heightfield) = shape.as_heightfield() {
-        ShapeHandle::new(heightfield.clone())
+        let heights = heightfield.heights();
+        let scale = heightfield.scale();
+        let heightfield = HeightField::new(heights.clone(), *scale);
+        ShapeHandle::new(heightfield)
     } else {
         #[cfg(feature = "dim3")]
         if let Some(trimesh) = shape.as_trimesh() {
@@ -196,7 +205,7 @@ fn nphysics_collider_from_rapier_collider(
                 trimesh
                     .indices()
                     .iter()
-                    .map(|idx| na::convert(*idx))
+                    .map(|idx| na::Point3::new(idx[0] as usize, idx[1] as usize, idx[2] as usize))
                     .collect(),
                 None,
             ))
@@ -205,7 +214,12 @@ fn nphysics_collider_from_rapier_collider(
         }
 
         #[cfg(feature = "dim2")]
-        {
+        if let Some(polygon) = shape.as_round_convex_polygon() {
+            margin = polygon.border_radius;
+            ShapeHandle::new(ConvexPolygon::try_from_points(polygon.base_shape.points()).unwrap())
+        } else if let Some(polygon) = shape.as_convex_polygon() {
+            ShapeHandle::new(ConvexPolygon::try_from_points(polygon.points()).unwrap())
+        } else {
             return None;
         }
     };
@@ -216,6 +230,7 @@ fn nphysics_collider_from_rapier_collider(
         ColliderDesc::new(shape)
             .position(pos)
             .density(density)
-            .sensor(collider.is_sensor()),
+            .sensor(collider.is_sensor())
+            .margin(margin),
     )
 }
diff --git a/src_testbed/objects/ball.rs b/src_testbed/objects/ball.rs
index f72c284..df43341 100644
--- a/src_testbed/objects/ball.rs
+++ b/src_testbed/objects/ball.rs
@@ -9,11 +9,13 @@ pub struct Ball {
     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,
@@ -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..0586d77 100644
--- a/src_testbed/objects/box_node.rs
+++ b/src_testbed/objects/box_node.rs
@@ -9,11 +9,13 @@ pub struct Box {
     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,
@@ -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..3d3e88f 100644
--- a/src_testbed/objects/capsule.rs
+++ b/src_testbed/objects/capsule.rs
@@ -9,11 +9,13 @@ pub struct Capsule {
     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,
@@ -30,6 +32,7 @@ impl Capsule {
             base_color: color,
             gfx: node,
             collider,
+            delta: delta * capsule.transform_wrt_y(),
         };
 
         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..7aa2039 100644
--- a/src_testbed/objects/cone.rs
+++ b/src_testbed/objects/cone.rs
@@ -9,11 +9,13 @@ pub struct Cone {
     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>,
@@ -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 0347144..95ef74a 100644
--- a/src_testbed/objects/convex.rs
+++ b/src_testbed/objects/convex.rs
@@ -13,25 +13,44 @@ pub struct Convex {
     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 = unimplemented!();
+        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");
@@ -59,7 +78,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..ff4dc72 100644
--- a/src_testbed/objects/cylinder.rs
+++ b/src_testbed/objects/cylinder.rs
@@ -9,11 +9,13 @@ pub struct Cylinder {
     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>,
@@ -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/objects/heightfield.rs b/src_testbed/objects/heightfield.rs
index 0815592..913a732 100644
--- a/src_testbed/objects/heightfield.rs
+++ b/src_testbed/objects/heightfield.rs
@@ -1,15 +1,16 @@
-#[cfg(feature = "dim3")]
-use crate::objects::node::{self, GraphicsNode};
 use kiss3d::window::Window;
 use na::{self, Point3};
-use ncollide::shape;
-#[cfg(feature = "dim3")]
-use ncollide::transformation::ToTriMesh;
+use parry::shape;
 use rapier::geometry::{ColliderHandle, ColliderSet};
 #[cfg(feature = "dim2")]
 use rapier::math::Point;
 #[cfg(feature = "dim3")]
-use rapier::math::Vector;
+use {
+    crate::objects::node::{self, GraphicsNode},
+    kiss3d::resource::Mesh,
+    rapier::math::Vector,
+    std::cell::RefCell,
+};
 
 pub struct HeightField {
     color: Point3<f32>,
@@ -25,7 +26,7 @@ impl HeightField {
     #[cfg(feature = "dim2")]
     pub fn new(
         collider: ColliderHandle,
-        heightfield: &shape::HeightField<f32>,
+        heightfield: &shape::HeightField,
         color: Point3<f32>,
         _: &mut Window,
     ) -> HeightField {
@@ -47,16 +48,23 @@ impl HeightField {
     #[cfg(feature = "dim3")]
     pub fn new(
         collider: ColliderHandle,
-        heightfield: &shape::HeightField<f32>,
+        heightfield: &shape::HeightField,
         color: Point3<f32>,
         window: &mut Window,
     ) -> HeightField {
-        let mesh = heightfield.to_trimesh(());
+        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_trimesh(mesh, Vector::repeat(1.0)),
+            gfx: window.add_mesh(Rc::new(RefCell::new(mesh)), Vector::repeat(1.0)),
             collider: collider,
         };
 
diff --git a/src_testbed/objects/mesh.rs b/src_testbed/objects/mesh.rs
index 5187a8b..5505de3 100644
--- a/src_testbed/objects/mesh.rs
+++ b/src_testbed/objects/mesh.rs
@@ -17,12 +17,15 @@ impl Mesh {
     pub fn new(
         collider: ColliderHandle,
         vertices: Vec<Point<f32>>,
-        indices: Vec<Point3<u32>>,
+        indices: Vec<[u32; 3]>,
         color: Point3<f32>,
         window: &mut window::Window,
     ) -> Mesh {
         let vs = vertices;
-        let is = indices.into_iter().map(na::convert).collect();
+        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;
diff --git a/src_testbed/objects/mod.rs b/src_testbed/objects/mod.rs
index e4d7bc4..8e12dbe 100644
--- a/src_testbed/objects/mod.rs
+++ b/src_testbed/objects/mod.rs
@@ -7,6 +7,7 @@ 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 d1de799..1bc491f 100644
--- a/src_testbed/objects/node.rs
+++ b/src_testbed/objects/node.rs
@@ -5,8 +5,7 @@ use crate::objects::convex::Convex;
 use crate::objects::heightfield::HeightField;
 use crate::objects::mesh::Mesh;
 //use crate::objects::plane::Plane;
-//#[cfg(feature = "dim2")]
-//use crate::objects::polyline::Polyline;
+use crate::objects::polyline::Polyline;
 use kiss3d::window::Window;
 use na::Point3;
 
@@ -26,8 +25,7 @@ pub enum Node {
     Box(Box),
     HeightField(HeightField),
     Capsule(Capsule),
-    //    #[cfg(feature = "dim2")]
-    //    Polyline(Polyline),
+    Polyline(Polyline),
     Mesh(Mesh),
     Convex(Convex),
     Cylinder(Cylinder),
@@ -42,8 +40,7 @@ impl Node {
             Node::Box(ref mut n) => n.select(),
             Node::Capsule(ref mut n) => n.select(),
             Node::HeightField(ref mut n) => n.select(),
-            //            #[cfg(feature = "dim2")]
-            //            Node::Polyline(ref mut n) => n.select(),
+            Node::Polyline(ref mut n) => n.select(),
             Node::Mesh(ref mut n) => n.select(),
             Node::Convex(ref mut n) => n.select(),
             Node::Cylinder(ref mut n) => n.select(),
@@ -58,8 +55,7 @@ impl Node {
             Node::Box(ref mut n) => n.unselect(),
             Node::Capsule(ref mut n) => n.unselect(),
             Node::HeightField(ref mut n) => n.unselect(),
-            //            #[cfg(feature = "dim2")]
-            //            Node::Polyline(ref mut n) => n.unselect(),
+            Node::Polyline(ref mut n) => n.unselect(),
             Node::Mesh(ref mut n) => n.unselect(),
             Node::Convex(ref mut n) => n.unselect(),
             Node::Cylinder(ref mut n) => n.unselect(),
@@ -74,8 +70,7 @@ impl Node {
             Node::Box(ref mut n) => n.update(colliders),
             Node::Capsule(ref mut n) => n.update(colliders),
             Node::HeightField(ref mut n) => n.update(colliders),
-            //            #[cfg(feature = "dim2")]
-            //            Node::Polyline(ref mut n) => n.update(colliders),
+            Node::Polyline(ref mut n) => n.update(colliders),
             Node::Mesh(ref mut n) => n.update(colliders),
             Node::Convex(ref mut n) => n.update(colliders),
             Node::Cylinder(ref mut n) => n.update(colliders),
@@ -86,7 +81,7 @@ impl Node {
     #[cfg(feature = "dim2")]
     pub fn draw(&mut self, window: &mut Window) {
         match *self {
-            //                    Node::Polyline(ref mut n) => n.draw(_window),
+            Node::Polyline(ref mut n) => n.draw(window),
             Node::HeightField(ref mut n) => n.draw(window),
             //            Node::Plane(ref mut n) => n.draw(_window),
             _ => {}
@@ -109,8 +104,9 @@ impl Node {
             Node::Convex(ref n) => Some(n.scene_node()),
             Node::Cylinder(ref n) => Some(n.scene_node()),
             Node::Cone(ref n) => Some(n.scene_node()),
+            Node::Polyline(_) => None,
             #[cfg(feature = "dim2")]
-            _ => None,
+            Node::HeightField(_) => None,
         }
     }
 
@@ -127,8 +123,9 @@ impl Node {
             Node::Convex(ref mut n) => Some(n.scene_node_mut()),
             Node::Cylinder(ref mut n) => Some(n.scene_node_mut()),
             Node::Cone(ref mut n) => Some(n.scene_node_mut()),
+            Node::Polyline(_) => None,
             #[cfg(feature = "dim2")]
-            _ => None,
+            Node::HeightField(_) => None,
         }
     }
 
@@ -139,8 +136,7 @@ impl Node {
             Node::Box(ref n) => n.object(),
             Node::Capsule(ref n) => n.object(),
             Node::HeightField(ref n) => n.object(),
-            //            #[cfg(feature = "dim2")]
-            //            Node::Polyline(ref n) => n.object(),
+            Node::Polyline(ref n) => n.object(),
             Node::Mesh(ref n) => n.object(),
             Node::Convex(ref n) => n.object(),
             Node::Cylinder(ref n) => n.object(),
@@ -155,8 +151,7 @@ impl Node {
             Node::Box(ref mut n) => n.set_color(color),
             Node::Capsule(ref mut n) => n.set_color(color),
             Node::HeightField(ref mut n) => n.set_color(color),
-            //            #[cfg(feature = "dim2")]
-            //            Node::Polyline(ref mut n) => n.set_color(color),
+            Node::Polyline(ref mut n) => n.set_color(color),
             Node::Mesh(ref mut n) => n.set_color(color),
             Node::Convex(ref mut n) => n.set_color(color),
             Node::Cylinder(ref mut n) => n.set_color(color),
diff --git a/src_testbed/objects/polyline.rs b/src_testbed/objects/polyline.rs
index 77841ae..98a8f24 100644
--- a/src_testbed/objects/polyline.rs
+++ b/src_testbed/objects/polyline.rs
@@ -1,38 +1,32 @@
 use kiss3d::window::Window;
-use na::{Isometry2, Point2, Point3};
-use ncollide2d::shape;
-use nphysics2d::object::{ColliderAnchor, DefaultColliderHandle, DefaultColliderSet};
+use na::Point3;
+use rapier::geometry::{ColliderHandle, ColliderSet};
+use rapier::math::{Isometry, Point};
 
 pub struct Polyline {
     color: Point3<f32>,
     base_color: Point3<f32>,
-    vertices: Vec<Point2<f32>>,
-    indices: Vec<P