First public release of Rapier.v0.1.0

18 files changed, 4929 insertions, 0 deletions

diff --git a/src_testbed/Inconsolata.otf b/src_testbed/Inconsolata.otf
new file mode 100644
index 0000000..3488898
--- /dev/null
+++ b/src_testbed/Inconsolata.otf
diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
new file mode 100644
index 0000000..07cef1e
--- /dev/null
+++ b/src_testbed/box2d_backend.rs
@@ -0,0 +1,237 @@
+use std::collections::HashMap;
+
+use na::{Isometry2, Vector2};
+use rapier::counters::Counters;
+use rapier::dynamics::{
+    IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
+};
+use rapier::geometry::{Collider, ColliderSet, Shape};
+use std::f32;
+
+use wrapped2d::b2;
+use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef};
+use wrapped2d::user_data::NoUserData;
+
+fn na_vec_to_b2_vec(v: Vector2<f32>) -> b2::Vec2 {
+    b2::Vec2 { x: v.x, y: v.y }
+}
+
+fn b2_vec_to_na_vec(v: b2::Vec2) -> Vector2<f32> {
+    Vector2::new(v.x, v.y)
+}
+
+fn b2_transform_to_na_isometry(v: b2::Transform) -> Isometry2<f32> {
+    Isometry2::new(b2_vec_to_na_vec(v.pos), v.rot.angle())
+}
+
+pub struct Box2dWorld {
+    world: b2::World<NoUserData>,
+    rapier2box2d: HashMap<RigidBodyHandle, b2::BodyHandle>,
+}
+
+impl Box2dWorld {
+    pub fn from_rapier(
+        gravity: Vector2<f32>,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        joints: &JointSet,
+    ) -> Self {
+        let mut world = b2::World::new(&na_vec_to_b2_vec(gravity));
+        world.set_continuous_physics(false);
+
+        let mut res = Box2dWorld {
+            world,
+            rapier2box2d: HashMap::new(),
+        };
+
+        res.insert_bodies(bodies);
+        res.insert_colliders(colliders);
+        res.insert_joints(joints);
+        res
+    }
+
+    fn insert_bodies(&mut self, bodies: &RigidBodySet) {
+        for (handle, body) in bodies.iter() {
+            let body_type = if !body.is_dynamic() {
+                b2::BodyType::Static
+            } else {
+                b2::BodyType::Dynamic
+            };
+
+            let linear_damping = 0.0;
+            let angular_damping = 0.0;
+
+            //            if let Some(rb) = body.downcast_ref::<RigidBody<f32>>() {
+            //                linear_damping = rb.linear_damping();
+            //                angular_damping = rb.angular_damping();
+            //            } else {
+            //                linear_damping = 0.0;
+            //                angular_damping = 0.0;
+            //            }
+
+            let def = b2::BodyDef {
+                body_type,
+                position: na_vec_to_b2_vec(body.position.translation.vector),
+                angle: body.position.rotation.angle(),
+                linear_velocity: na_vec_to_b2_vec(body.linvel),
+                angular_velocity: body.angvel,
+                linear_damping,
+                angular_damping,
+                ..b2::BodyDef::new()
+            };
+            let b2_handle = self.world.create_body(&def);
+            self.rapier2box2d.insert(handle, b2_handle);
+
+            // Collider.
+            let mut b2_body = self.world.body_mut(b2_handle);
+            b2_body.set_bullet(false /* collider.is_ccd_enabled() */);
+        }
+    }
+
+    fn insert_colliders(&mut self, colliders: &ColliderSet) {
+        for (_, collider) in colliders.iter() {
+            let b2_body_handle = self.rapier2box2d[&collider.parent()];
+            let mut b2_body = self.world.body_mut(b2_body_handle);
+            Self::create_fixture(&collider, &mut *b2_body);
+        }
+    }
+
+    fn insert_joints(&mut self, joints: &JointSet) {
+        for joint in joints.iter() {
+            let body_a = self.rapier2box2d[&joint.body1];
+            let body_b = self.rapier2box2d[&joint.body2];
+
+            match &joint.params {
+                JointParams::BallJoint(params) => {
+                    let def = RevoluteJointDef {
+                        body_a,
+                        body_b,
+                        collide_connected: true,
+                        local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords),
+                        local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords),
+                        reference_angle: 0.0,
+                        enable_limit: false,
+                        lower_angle: 0.0,
+                        upper_angle: 0.0,
+                        enable_motor: false,
+                        motor_speed: 0.0,
+                        max_motor_torque: 0.0,
+                    };
+
+                    self.world.create_joint(&def);
+                }
+                JointParams::FixedJoint(params) => {
+                    let def = WeldJointDef {
+                        body_a,
+                        body_b,
+                        collide_connected: true,
+                        local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.translation.vector),
+                        local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.translation.vector),
+                        reference_angle: 0.0,
+                        frequency: 0.0,
+                        damping_ratio: 0.0,
+                    };
+
+                    self.world.create_joint(&def);
+                }
+                JointParams::PrismaticJoint(params) => {
+                    let def = PrismaticJointDef {
+                        body_a,
+                        body_b,
+                        collide_connected: true,
+                        local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords),
+                        local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords),
+                        local_axis_a: na_vec_to_b2_vec(params.local_axis1().into_inner()),
+                        reference_angle: 0.0,
+                        enable_limit: params.limits_enabled,
+                        lower_translation: params.limits[0],
+                        upper_translation: params.limits[1],
+                        enable_motor: false,
+                        max_motor_force: 0.0,
+                        motor_speed: 0.0,
+                    };
+
+                    self.world.create_joint(&def);
+                }
+            }
+        }
+    }
+
+    fn create_fixture(collider: &Collider, body: &mut b2::MetaBody<NoUserData>) {
+        let center = na_vec_to_b2_vec(collider.delta().translation.vector);
+        let mut fixture_def = b2::FixtureDef::new();
+
+        fixture_def.restitution = 0.0;
+        fixture_def.friction = collider.friction;
+        fixture_def.density = collider.density();
+        fixture_def.is_sensor = collider.is_sensor();
+        fixture_def.filter = b2::Filter::new();
+
+        match collider.shape() {
+            Shape::Ball(b) => {
+                let mut b2_shape = b2::CircleShape::new();
+                b2_shape.set_radius(b.radius);
+                b2_shape.set_position(center);
+                body.create_fixture(&b2_shape, &mut fixture_def);
+            }
+            Shape::Cuboid(c) => {
+                let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
+                body.create_fixture(&b2_shape, &mut fixture_def);
+            }
+            Shape::Polygon(poly) => {
+                let points: Vec<_> = poly
+                    .vertices()
+                    .iter()
+                    .map(|p| collider.delta() * p)
+                    .map(|p| na_vec_to_b2_vec(p.coords))
+                    .collect();
+                let b2_shape = b2::PolygonShape::new_with(&points);
+                body.create_fixture(&b2_shape, &mut fixture_def);
+            }
+            Shape::HeightField(heightfield) => {
+                let mut segments = heightfield.segments();
+                let seg1 = segments.next().unwrap();
+                let mut vertices = vec![
+                    na_vec_to_b2_vec(seg1.a.coords),
+                    na_vec_to_b2_vec(seg1.b.coords),
+                ];
+
+                // TODO: this will not handle holes properly.
+                segments.for_each(|seg| {
+                    vertices.push(na_vec_to_b2_vec(seg.b.coords));
+                });
+
+                let b2_shape = b2::ChainShape::new_chain(&vertices);
+                body.create_fixture(&b2_shape, &mut fixture_def);
+            }
+            _ => eprintln!("Creating a shape unknown to the Box2d backend."),
+        }
+    }
+
+    pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) {
+        //        self.world.set_continuous_physics(world.integration_parameters.max_ccd_substeps != 0);
+
+        counters.step_started();
+        self.world.step(
+            params.dt(),
+            params.max_velocity_iterations as i32,
+            params.max_position_iterations as i32,
+        );
+        counters.step_completed();
+    }
+
+    pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
+        for (handle, mut body) in bodies.iter_mut() {
+            if let Some(pb2_handle) = self.rapier2box2d.get(&handle) {
+                let b2_body = self.world.body(*pb2_handle);
+                let pos = b2_transform_to_na_isometry(b2_body.transform().clone());
+                body.set_position(pos);
+
+                for coll_handle in body.colliders() {
+                    let collider = &mut colliders[*coll_handle];
+                    collider.set_position_debug(pos * collider.delta());
+                }
+            }
+        }
+    }
+}
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
new file mode 100644
index 0000000..62ab37e
--- /dev/null
+++ b/src_testbed/engine.rs
@@ -0,0 +1,694 @@
+#[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::Point;
+use crate::objects::ball::Ball;
+use crate::objects::box_node::Box as BoxNode;
+use crate::objects::convex::Convex;
+use crate::objects::heightfield::HeightField;
+use crate::objects::node::{GraphicsNode, Node};
+use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
+use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
+//use crate::objects::capsule::Capsule;
+//use crate::objects::convex::Convex;
+//#[cfg(feature = "fluids")]
+//use crate::objects::fluid::Fluid as FluidNode;
+//#[cfg(feature = "dim3")]
+//use crate::objects::mesh::Mesh;
+//use crate::objects::plane::Plane;
+//#[cfg(feature = "dim2")]
+//use crate::objects::polyline::Polyline;
+//#[cfg(feature = "fluids")]
+//use crate::objects::FluidRenderingMode;
+use crate::objects::capsule::Capsule;
+use crate::objects::mesh::Mesh;
+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>>,
+    #[cfg(feature = "fluids")]
+    f2sn: HashMap<FluidHandle, FluidNode>,
+    #[cfg(feature = "fluids")]
+    boundary2sn: HashMap<BoundaryHandle, FluidNode>,
+    b2color: HashMap<RigidBodyHandle, Point3<f32>>,
+    b2wireframe: HashMap<RigidBodyHandle, bool>,
+    #[cfg(feature = "fluids")]
+    f2color: HashMap<FluidHandle, Point3<f32>>,
+    ground_color: Point3<f32>,
+    camera: Camera,
+    ground_handle: Option<RigidBodyHandle>,
+    #[cfg(feature = "fluids")]
+    fluid_rendering_mode: FluidRenderingMode,
+    #[cfg(feature = "fluids")]
+    render_boundary_particles: bool,
+}
+
+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(),
+            #[cfg(feature = "fluids")]
+            f2sn: HashMap::new(),
+            #[cfg(feature = "fluids")]
+            boundary2sn: HashMap::new(),
+            b2color: HashMap::new(),
+            #[cfg(feature = "fluids")]
+            f2color: HashMap::new(),
+            ground_color: Point3::new(0.5, 0.5, 0.5),
+            b2wireframe: HashMap::new(),
+            ground_handle: None,
+            #[cfg(feature = "fluids")]
+            fluid_rendering_mode: FluidRenderingMode::StaticColor,
+            #[cfg(feature = "fluids")]
+            render_boundary_particles: false,
+        }
+    }
+
+    pub fn set_ground_handle(&mut self, handle: Option<RigidBodyHandle>) {
+        self.ground_handle = handle
+    }
+
+    #[cfg(feature = "fluids")]
+    pub fn set_fluid_rendering_mode(&mut self, mode: FluidRenderingMode) {
+        self.fluid_rendering_mode = mode;
+    }
+
+    #[cfg(feature = "fluids")]
+    pub fn enable_boundary_particles_rendering(&mut self, enabled: bool) {
+        self.render_boundary_particles = enabled;
+
+        for sn in self.boundary2sn.values_mut() {
+            sn.scene_node_mut().set_visible(enabled);
+        }
+    }
+
+    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);
+                }
+            }
+        }
+
+        #[cfg(feature = "fluids")]
+        for sn in self.f2sn.values_mut().chain(self.boundary2sn.values_mut()) {
+            let node = sn.scene_node_mut();
+            window.remove_graphics_node(node);
+        }
+
+        self.b2sn.clear();
+        #[cfg(feature = "fluids")]
+        self.f2sn.clear();
+        #[cfg(feature = "fluids")]
+        self.boundary2sn.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);
+    }
+
+    #[cfg(feature = "fluids")]
+    pub fn set_fluid_color(&mut self, f: FluidHandle, color: Point3<f32>) {
+        self.f2color.insert(f, color);
+
+        if let Some(n) = self.f2sn.get_mut(&f) {
+            n.set_color(color)
+        }
+    }
+
+    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_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);
+                }
+            }
+        }
+    }
+
+    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
+    }
+
+    #[cfg(feature = "fluids")]
+    pub fn add_fluid(
+        &mut self,
+        window: &mut Window,
+        handle: FluidHandle,
+        fluid: &Fluid<f32>,
+        particle_radius: f32,
+    ) {
+        let rand = &mut self.rand;
+        let color = *self
+            .f2color
+            .entry(handle)
+            .or_insert_with(|| Self::gen_color(rand));
+
+        self.add_fluid_with_color(window, handle, fluid, particle_radius, color);
+    }
+
+    #[cfg(feature = "fluids")]
+    pub fn add_boundary(
+        &mut self,
+        window: &mut Window,
+        handle: BoundaryHandle,
+        boundary: &Boundary<f32>,
+        particle_radius: f32,
+    ) {
+        let color = self.ground_color;
+        let node = FluidNode::new(particle_radius, &boundary.positions, color, window);
+        self.boundary2sn.insert(handle, node);
+    }
+
+    #[cfg(feature = "fluids")]
+    pub fn add_fluid_with_color(
+        &mut self,
+        window: &mut Window,
+        handle: FluidHandle,
+        fluid: &Fluid<f32>,
+        particle_radius: f32,
+        color: Point3<f32>,
+    ) {
+        let node = FluidNode::new(particle_radius, &fluid.positions, color, window);
+        self.f2sn.insert(handle, node);
+    }
+
+    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 collider = &colliders[*collider_handle];
+            self.add_collider(window, *collider_handle, collider, 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);
+    }
+
+    fn add_collider(
+        &mut self,
+        window: &mut Window,
+        handle: ColliderHandle,
+        collider: &Collider,
+        color: Point3<f32>,
+        out: &mut Vec<Node>,
+    ) {
+        match collider.shape() {
+            Shape::Ball(ball) => {
+                out.push(Node::Ball(Ball::new(handle, ball.radius, color, window)))
+            }
+            Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
+                handle,
+                poly.vertices().to_vec(),
+                color,
+                window,
+            ))),
+            Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new(
+                handle,
+                cuboid.half_extents,
+                color,
+                window,
+            ))),
+            Shape::Capsule(capsule) => {
+                out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
+            }
+            Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new(
+                handle,
+                vec![triangle.a, triangle.b, triangle.c],
+                vec![Point3::new(0, 1, 2)],
+                color,
+                window,
+            ))),
+            Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new(
+                handle,
+                trimesh.vertices().to_vec(),
+                trimesh
+                    .indices()
+                    .iter()
+                    .map(|idx| na::convert(*idx))
+                    .collect(),
+                color,
+                window,
+            ))),
+            Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new(
+                handle,
+                heightfield,
+                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<f32>,
+        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,
+        )))
+    }
+    */
+
+    #[cfg(feature = "fluids")]
+    pub fn draw_fluids(&mut self, liquid_world: &LiquidWorld<f32>) {
+        for (i, fluid) in liquid_world.fluids().iter() {
+            if let Some(node) = self.f2sn.get_mut(&i) {
+                node.update_with_fluid(fluid, self.fluid_rendering_mode)
+            }
+        }
+
+        if self.render_boundary_particles {
+            for (i, boundary) in liquid_world.boundaries().iter() {
+                if let Some(node) = self.boundary2sn.get_mut(&i) {
+                    node.update_with_boundary(boundary)
+                }
+            }
+        }
+    }
+
+    pub fn draw(&mut self, 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() {
+                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));
+    //             // }
+    //         }
+    //     }
+    // }
+