diff --git a/examples/debug_draw.rs b/examples/debug_draw.rs
new file mode 100644
index 0000000..248222e
--- /dev/null
+++ b/examples/debug_draw.rs
@@ -0,0 +1,128 @@
+#![feature(box_syntax)]
+use std::any::Any;
+
+extern crate libc;
+extern crate liquidfun;
+
+use liquidfun::box2d::collision::shapes::polygon_shape::*;
+use liquidfun::box2d::common::math::*;
+use liquidfun::box2d::dynamics::body::*;
+use liquidfun::box2d::dynamics::fixture::*;
+use liquidfun::box2d::dynamics::world::*;
+use liquidfun::box2d::common::draw::*;
+
+fn main() {
+
+	// Define the gravity vector.
+	let gravity = Vec2::new(0.0, -10.0);
+
+	// Construct a world object, which will hold and simulate the rigid bodies.
+	let mut world = World::new(&gravity);
+
+	// Define the ground body.
+	let mut ground_body_def = BodyDef::default();
+	ground_body_def.position.set(0.0, -10.0);
+
+	// Call the body factory which allocates memory for the ground body
+	// from a pool and creates the ground box shape (also from a pool).
+	// The body is also added to the world.
+	let ground_body = world.create_body(&ground_body_def);
+
+	// Define the ground box shape.
+	let mut ground_box = PolygonShape::new();
+
+	// The extents are the half-widths of the box.
+	ground_box.set_as_box(50.0, 10.0);
+
+	// Add the ground fixture to the ground body.
+	ground_body.create_fixture_from_shape(&ground_box, 0.0);
+
+	// Define the dynamic body. We set its position and call the body factory.
+	let mut body_def = BodyDef::default();
+	body_def.body_type = BodyType::DynamicBody;
+	body_def.position.set(0.0, 4.0);
+	let body = world.create_body(&body_def);
+
+	// Define another box shape for our dynamic body.
+	let mut dynamic_box = PolygonShape::new();
+	dynamic_box.set_as_box(1.0, 1.0);
+
+	// Define the dynamic body fixture.
+	let mut fixture_def = FixtureDef::new(&dynamic_box);
+
+	// Set the box density to be non-zero, so it will be dynamic.
+	fixture_def.density = 1.0;
+
+	// Override the default friction.
+	fixture_def.friction = 0.3;
+
+	// Add the shape to the body.
+	body.create_fixture(&fixture_def);
+
+	// Register a routine for debug drawing. The debug draw functions are called
+	// inside with World::draw_debug_data() method. The debug draw object is owned
+	// by you and must remain in scope.
+	let mut debug_draw = DebugDraw::new(MyDebugDraw::new());
+	debug_draw.get().scale = 10.;
+	world.set_debug_draw(&mut debug_draw);
+	debug_draw.set_flags(!0);
+
+	world.draw_debug_data();
+
+	/* this would crash:
+		{
+			let mut debug_draw = DebugDraw::new(MyDebugDraw::new());
+			debug_draw.get().scale = 10.;
+			world.set_debug_draw(&mut debug_draw);
+			debug_draw.set_flags(!0);
+		} // debug_draw goes out of scope here but world still has pointer to it
+
+		world.draw_debug_data();
+
+		// if debug_draw goes out of scope before world, call world.clear_debug_draw()
+	*/
+}
+
+struct MyDebugDraw {
+	scale: f32,
+}
+
+impl MyDebugDraw {
+	pub fn new() -> Self {
+		Self {
+			scale: 1.,
+		}
+	}
+}
+
+impl Drop for MyDebugDraw {
+	fn drop(&mut self) {
+		println!("drop MyDebugDraw");
+	}
+}
+
+impl Draw for MyDebugDraw {
+	fn as_any(&mut self) -> &mut Any { self }
+	fn draw_polygon(&mut self, vertices: &[Vec2], color: &Color) {
+		println!("self.scale {}", self.scale);
+		println!("draw_polygon {:?} {:?}", vertices, color);
+	}
+	fn draw_solid_polygon(&mut self, vertices: &[Vec2], color: &Color) {
+		println!("draw_solid_polygon {:?} {:?}", vertices, color);
+	}
+	fn draw_circle(&mut self, center: &Vec2, radius: f32, color: &Color) {
+		println!("draw_circle {:?} {:?} {:?}", center, radius, color);
+	}
+	fn draw_solid_circle(&mut self, center: &Vec2, radius: f32, axis: &Vec2, color: &Color) {
+		println!("draw_solid_circle {:?} {:?} {:?} {:?}", center, radius, axis, color);
+	}
+	fn draw_particles(&mut self, centers: &[Vec2], colors: &[ParticleColor], radius: f32) {
+		println!("draw_particles {:?} {:?} {:?}", centers, colors, radius);
+	}
+	fn draw_segment(&mut self, p1: &Vec2, p2: &Vec2, color: &Color) {
+		println!("draw_segment {:?} {:?} {:?}", p1, p2, color);
+	}
+	fn draw_transform(&mut self, xf: &Transform) {
+		println!("draw_transform {:?}", xf);
+	}
+}
diff --git a/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.cpp b/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.cpp
index f495615..9c31624 100644
--- a/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.cpp
+++ b/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.cpp
@@ -30,4 +30,8 @@ extern "C" {
         return static_cast<b2Shape*>(reinterpret_cast<b2PolygonShape*>(self));
     }
 
+    void b2PolygonShape_Set(b2PolygonShape* self, const b2Vec2* vertices, int32 count) {
+        self->Set(vertices, count);
+    }
+
 } // extern C
\ No newline at end of file
diff --git a/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.h b/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.h
index 4259a27..0280755 100644
--- a/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.h
+++ b/liquidfun-c/Box2D/Collision/Shapes/c_b2PolygonShape.h
@@ -12,7 +12,7 @@ extern "C" {
 	void b2PolygonShape_SetAsBox(b2PolygonShape* self, float32 hx, float32 hy);
 	void b2PolygonShape_SetAsBox_Oriented(b2PolygonShape* self, float32 hx, float32 hy, const b2Vec2& center, float32 angle);
 	b2Shape* b2PolygonShape_Upcast(b2PolygonShape* self);
-
+	void b2PolygonShape_Set(b2PolygonShape* self, const b2Vec2* vertices, int32 count);
 
 #ifdef __cplusplus
 } // extern C
diff --git a/liquidfun-c/Box2D/Common/c_b2Draw.cpp b/liquidfun-c/Box2D/Common/c_b2Draw.cpp
new file mode 100644
index 0000000..3af92da
--- /dev/null
+++ b/liquidfun-c/Box2D/Common/c_b2Draw.cpp
@@ -0,0 +1,30 @@
+#include <Box2D/Box2D.h>
+#include "c_b2Draw.h"
+
+extern "C" {
+
+	CppDebugDraw* CppDebugDraw_new(DrawTrait* debugDraw) {
+		return new CppDebugDraw(debugDraw);
+	}
+
+	void CppDebugDraw_delete(CppDebugDraw* debugDraw) {
+		delete debugDraw;
+	}
+
+	void CppDebugDraw_SetFlags(CppDebugDraw* self, uint32 flags) {
+		return self->SetFlags(flags);
+	}
+
+	uint32 CppDebugDraw_GetFlags(CppDebugDraw* self) {
+		return self->GetFlags();
+	}
+
+	void CppDebugDraw_AppendFlags(CppDebugDraw* self, uint32 flags) {
+		return self->AppendFlags(flags);
+	}
+
+	void CppDebugDraw_ClearFlags(CppDebugDraw* self, uint32 flags) {
+		return self->ClearFlags(flags);
+	}
+
+} // extern C
diff --git a/liquidfun-c/Box2D/Common/c_b2Draw.h b/liquidfun-c/Box2D/Common/c_b2Draw.h
new file mode 100644
index 0000000..8f80741
--- /dev/null
+++ b/liquidfun-c/Box2D/Common/c_b2Draw.h
@@ -0,0 +1,73 @@
+#ifndef C_B2_DRAW
+#define C_B2_DRAW
+
+struct DrawTrait {};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	void DrawTrait_DrawPolygon(DrawTrait* self, const b2Vec2* vertices, int32 vertexCount, const b2Color& color);
+	void DrawTrait_DrawSolidPolygon(DrawTrait* self, const b2Vec2* vertices, int32 vertexCount, const b2Color& color);
+	void DrawTrait_DrawCircle(DrawTrait* self, const b2Vec2& center, float32 radius, const b2Color& color);
+	void DrawTrait_DrawSolidCircle(DrawTrait* self, const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color);
+	void DrawTrait_DrawParticles(DrawTrait* self, const b2Vec2 *centers, float32 radius, const b2ParticleColor *colors, int32 count);
+	void DrawTrait_DrawSegment(DrawTrait* self, const b2Vec2& p1, const b2Vec2& p2, const b2Color& color);
+	void DrawTrait_DrawTransform(DrawTrait* self, const b2Transform& xf);
+
+#ifdef __cplusplus
+} // extern C
+#endif
+
+class CppDebugDraw: public b2Draw {
+public:
+	CppDebugDraw(DrawTrait* debugDraw): self(debugDraw) {}
+
+	virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) {
+		DrawTrait_DrawPolygon(self, vertices, vertexCount, color);
+	}
+
+	virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) {
+		DrawTrait_DrawSolidPolygon(self, vertices, vertexCount, color);
+	}
+
+	virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) {
+		DrawTrait_DrawCircle(self, center, radius, color);
+	}
+
+	virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) {
+		DrawTrait_DrawSolidCircle(self, center, radius, axis, color);
+	}
+
+	virtual void DrawParticles(const b2Vec2 *centers, float32 radius, const b2ParticleColor *colors, int32 count) {
+		DrawTrait_DrawParticles(self, centers, radius, colors, count);
+	}
+
+	virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) {
+		DrawTrait_DrawSegment(self, p1, p2, color);
+	}
+
+	virtual void DrawTransform(const b2Transform& xf) {
+		DrawTrait_DrawTransform(self, xf);
+	}
+
+protected:
+	DrawTrait* self;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	CppDebugDraw* CppDebugDraw_new(DrawTrait* debugDraw);
+	void CppDebugDraw_delete(CppDebugDraw* self);
+	void CppDebugDraw_SetFlags(CppDebugDraw* self, uint32 flags);
+	uint32 CppDebugDraw_GetFlags(CppDebugDraw* self);
+	void CppDebugDraw_AppendFlags(CppDebugDraw* self, uint32 flags);
+	void CppDebugDraw_ClearFlags(CppDebugDraw* self, uint32 flags);
+
+#ifdef __cplusplus
+} // extern C
+#endif
+
+#endif
\ No newline at end of file
diff --git a/liquidfun-c/Box2D/Dynamics/c_b2Body.cpp b/liquidfun-c/Box2D/Dynamics/c_b2Body.cpp
index d3e1adb..4c314d6 100644
--- a/liquidfun-c/Box2D/Dynamics/c_b2Body.cpp
+++ b/liquidfun-c/Box2D/Dynamics/c_b2Body.cpp
@@ -4,16 +4,16 @@
 extern "C" {
 
 	b2Fixture* b2Body_CreateFixture(b2Body* self, const b2FixtureDef* def) {
-	    return self->CreateFixture(def);
+		return self->CreateFixture(def);
 	}
 
 	b2Fixture* b2Body_CreateFixture_FromShape(b2Body* self, const b2Shape* shape, float32 density) {
-	    return self->CreateFixture(shape, density);
+		return self->CreateFixture(shape, density);
 	}
 	
 	float32 b2Body_GetAngle(const b2Body* self) {
-	    return self->GetAngle();
-	}	
+		return self->GetAngle();
+	}
 
 	b2Fixture* b2Body_GetFixtureList(b2Body* self) {
 		return self->GetFixtureList();
@@ -24,7 +24,7 @@ extern "C" {
 	}
 
 	const b2Vec2& b2Body_GetPosition(const b2Body* self) {
-	    return self->GetPosition();
+		return self->GetPosition();
 	}
 
 	void* b2Body_GetUserData(const b2Body* self) {
@@ -35,9 +35,21 @@ extern "C" {
 		return self->GetWorld();
 	}
 
-	b2Vec2 b2Body_GetLocalPoint(const b2Body* self, const b2Vec2& worldPoint) {
+	const b2Vec2& b2Body_GetLocalPoint(const b2Body* self, const b2Vec2& worldPoint) {
 		return self->GetLocalPoint(worldPoint);
 	}
 
+	void b2Body_SetTransform(b2Body* self, const b2Vec2& position, float32 angle) {
+		self->SetTransform(position, angle);
+	}
+
+	void b2Body_SetLinearVelocity(b2Body* self, const b2Vec2& v) {
+		self->SetLinearVelocity(v);
+	}
+
+	const b2Vec2& b2Body_GetLinearVelocity(const b2Body* self) {
+		return self->GetLinearVelocity();
+	}
+
 } // extern C
 
diff --git a/liquidfun-c/Box2D/Dynamics/c_b2Body.h b/liquidfun-c/Box2D/Dynamics/c_b2Body.h
index 162d8a7..904bf67 100644
--- a/liquidfun-c/Box2D/Dynamics/c_b2Body.h
+++ b/liquidfun-c/Box2D/Dynamics/c_b2Body.h
@@ -13,7 +13,10 @@ extern "C" {
 	b2Body* b2Body_GetNext(b2Body* self);
 	void* b2Body_GetUserData(const b2Body* self);
 	b2World* b2Body_GetWorld(b2Body* self);
-	b2Vec2 b2Body_GetLocalPoint(const b2Body* self, const b2Vec2& worldPoint);
+	const b2Vec2& b2Body_GetLocalPoint(const b2Body* self, const b2Vec2& worldPoint);
+	void b2Body_SetTransform(b2Body* self, const b2Vec2& position, float32 angle);
+	void b2Body_SetLinearVelocity(b2Body* self, const b2Vec2& v);
+	const b2Vec2& b2Body_GetLinearVelocity(const b2Body* self);
 
 #ifdef __cplusplus
 } // extern C
diff --git a/liquidfun-c/Box2D/Dynamics/c_b2World.cpp b/liquidfun-c/Box2D/Dynamics/c_b2World.cpp
index f5514c3..9b93e6f 100644
--- a/liquidfun-c/Box2D/Dynamics/c_b2World.cpp
+++ b/liquidfun-c/Box2D/Dynamics/c_b2World.cpp
@@ -87,6 +87,18 @@ extern "C" {
         self->Step(timeStep, velocityIterations, positionIterations);
     }
 
+    void b2World_SetDebugDraw(b2World* self, b2Draw* debugDraw) {
+        // self->SetDebugDraw(new CppDebugDraw(debugDraw));
+        self->SetDebugDraw(debugDraw);
+    }
+
+    /*b2Draw* b2World_GetDebugDraw(b2World* self) {
+        return self->GetDebugDraw();
+    }*/
+
+    void b2World_DrawDebugData(b2World* self) {
+        self->DrawDebugData();
+    }
 
 } // extern C
 
diff --git a/liquidfun-c/Box2D/Dynamics/c_b2World.h b/liquidfun-c/Box2D/Dynamics/c_b2World.h
index 4e3a36f..3702e8c 100644
--- a/liquidfun-c/Box2D/Dynamics/c_b2World.h
+++ b/liquidfun-c/Box2D/Dynamics/c_b2World.h
@@ -38,6 +38,9 @@ extern "C" {
 
 	b2ParticleSystem* b2World_CreateParticleSystem(b2World* self, const b2ParticleSystemDef* def);
 	void b2World_Step(b2World* self, float32 timeStep, int32 velocityIterations, int32 positionIterations);
+	void b2World_SetDebugDraw(b2World* self, b2Draw* debugDraw);
+	// b2Draw* b2World_GetDebugDraw(b2World* self);
+    void b2World_DrawDebugData(b2World* self);
 
 #ifdef __cplusplus
 } // extern C
diff --git a/liquidfun-c/c_box2d.cpp b/liquidfun-c/c_box2d.cpp
index 43285f4..8ea73b2 100644
--- a/liquidfun-c/c_box2d.cpp
+++ b/liquidfun-c/c_box2d.cpp
@@ -57,6 +57,7 @@
 #include "Box2D/Collision/Shapes/c_b2ChainShape.cpp"
 #include "Box2D/Collision/Shapes/c_b2PolygonShape.cpp"
 #include "Box2D/Common/c_b2Math.cpp"
+#include "Box2D/Common/c_b2Draw.cpp"
 #include "Box2D/Dynamics/c_b2Body.cpp"
 #include "Box2D/Dynamics/c_b2Fixture.cpp"
 #include "Box2D/Dynamics/c_b2World.cpp"
diff --git a/liquidfun-cpp/Box2D/Dynamics/b2World.h b/liquidfun-cpp/Box2D/Dynamics/b2World.h
index ce0b8c1..ae098d9 100644
--- a/liquidfun-cpp/Box2D/Dynamics/b2World.h
+++ b/liquidfun-cpp/Box2D/Dynamics/b2World.h
@@ -69,6 +69,10 @@ class b2World
 	/// by you and must remain in scope.
 	void SetDebugDraw(b2Draw* debugDraw);
 
+	/*b2Draw* GetDebugDraw() {
+		return m_debugDraw;
+	}*/
+
 	/// Create a rigid body given a definition. No reference to the definition
 	/// is retained.
 	/// @warning This function is locked during callbacks.
diff --git a/src/box2d/collision/shapes/polygon_shape.rs b/src/box2d/collision/shapes/polygon_shape.rs
index 4198e35..b4d119a 100644
--- a/src/box2d/collision/shapes/polygon_shape.rs
+++ b/src/box2d/collision/shapes/polygon_shape.rs
@@ -12,6 +12,7 @@ extern {
     fn b2PolygonShape_SetAsBox(ptr: *mut B2PolygonShape, hx: Float32, hy: Float32);
     fn b2PolygonShape_SetAsBox_Oriented(ptr: *mut B2PolygonShape, hx: Float32, hy: Float32, center: &Vec2, angle: Float32);
     fn b2PolygonShape_Upcast(ptr: *mut B2PolygonShape) -> *mut B2Shape;
+    fn b2PolygonShape_Set(ptr: *mut B2PolygonShape, vertices: *const Vec2, count: Int32);
 }
 
 /// A convex polygon. It is assumed that the interior of the polygon is to
@@ -79,6 +80,15 @@ impl PolygonShape {
             b2PolygonShape_SetAsBox_Oriented(self.ptr, hx, hy, center, angle);
         }
     }
+
+    /// Build a convex polygon.
+    /// @param hx the half-width.
+    /// @param hy the half-height.
+    pub fn set(&mut self, vertices: &[Vec2]) {
+        unsafe {
+            b2PolygonShape_Set(self.ptr, vertices.as_ptr(), vertices.len() as Int32);
+        }
+    }
 }
 
 impl Drop for PolygonShape {
diff --git a/src/box2d/common/draw.rs b/src/box2d/common/draw.rs
new file mode 100644
index 0000000..d65d422
--- /dev/null
+++ b/src/box2d/common/draw.rs
@@ -0,0 +1,120 @@
+#![allow(non_snake_case)]
+
+use std::slice::from_raw_parts;
+use std::any::Any;
+
+use super::math::*;
+use super::settings::*;
+
+/// Flags for specifying what to draw, using set_flags()
+#[repr(u32)]
+#[derive(Debug, Copy, Clone, PartialEq, Eq, Hash)]
+pub enum DrawFlags {
+	/// draw shapes
+	ShapeBit        = 0x0001,
+	
+	/// draw joint connections
+	JointBit        = 0x0002,
+
+	/// draw axis aligned bounding boxes
+	AabbBit         = 0x0004,
+
+	/// draw broad-phase pairs
+	PairBit	        = 0x0008,
+
+	/// draw center of mass frame
+	CenterOfMassBit	= 0x0010,
+
+	/// draw particles
+	ParticleBit     = 0x0020,
+}
+
+/// Color for debug drawing. Each value has the range [0,1].
+#[repr(C)]
+#[derive(Debug, Default, Copy, Clone, PartialEq)]
+pub struct Color { pub r: f32, pub g: f32, pub b: f32 }
+
+/// Small color object for each particle
+#[repr(C)]
+#[derive(Debug, Default, Copy, Clone, PartialEq, Eq, Hash)]
+pub struct ParticleColor { pub r: u8, pub g: u8, pub b: u8, pub a: u8 }
+
+/// Implement and register this trait with a World to provide debug drawing of physics
+/// using World.set_debug_draw()
+pub trait Draw {
+
+	/// Put this in your impl: fn as_any(&mut self) -> &mut Any { self }
+	fn as_any(&mut self) -> &mut Any;
+
+	/// Draw a closed polygon provided in CCW order.
+	fn draw_polygon(&mut self, vertices: &[Vec2], color: &Color);
+	
+	/// Draw a solid closed polygon provided in CCW order.
+	fn draw_solid_polygon(&mut self, vertices: &[Vec2], color: &Color);
+
+	/// Draw a circle.
+	fn draw_circle(&mut self, center: &Vec2, radius: f32, color: &Color);
+
+	/// Draw a solid circle.
+	fn draw_solid_circle(&mut self, center: &Vec2, radius: f32, axis: &Vec2, color: &Color);
+
+	/// Draw a particle array
+	fn draw_particles(&mut self, centers: &[Vec2], colors: &[ParticleColor], radius: f32);
+
+	/// Draw a line segment.
+	fn draw_segment(&mut self, p1: &Vec2, p2: &Vec2, color: &Color);
+
+	/// Draw a transform. Choose your own length scale.
+	fn draw_transform(&mut self, xf: &Transform);
+}
+
+pub type DrawTrait = Box<Draw>;
+
+#[no_mangle]
+pub extern fn DrawTrait_DrawPolygon(this: *mut DrawTrait, vertices: *const Vec2, vertexCount: Int32, color: &Color) {
+	unsafe {
+		(**this).draw_polygon(from_raw_parts(vertices, vertexCount as usize), color);
+	}
+}
+
+#[no_mangle]
+pub extern fn DrawTrait_DrawSolidPolygon(this: *mut DrawTrait, vertices: *const Vec2, vertexCount: Int32, color: &Color) {
+	unsafe {
+		(**this).draw_solid_polygon(from_raw_parts(vertices, vertexCount as usize), color);
+	}
+}
+
+#[no_mangle]
+pub extern fn DrawTrait_DrawCircle(this: *mut DrawTrait, center: &Vec2, radius: f32, color: &Color) {
+	unsafe {
+		(**this).draw_circle(center, radius, color);
+	}
+}
+
+#[no_mangle]
+pub extern fn DrawTrait_DrawSolidCircle(this: *mut DrawTrait, center: &Vec2, radius: f32, axis: &Vec2, color: &Color) {
+	unsafe {
+		(**this).draw_solid_circle(center, radius, axis, color);
+	}
+}
+
+#[no_mangle]
+pub extern fn DrawTrait_DrawParticles(this: *mut DrawTrait, centers: *const Vec2, radius: f32, colors: *const ParticleColor, count: Int32) {
+	unsafe {
+		(**this).draw_particles(from_raw_parts(centers, count as usize), from_raw_parts(colors, count as usize), radius);
+	}
+}
+
+#[no_mangle]
+pub extern fn DrawTrait_DrawSegment(this: *mut DrawTrait, p1: &Vec2, p2: &Vec2, color: &Color) {
+	unsafe {
+		(**this).draw_segment(p1, p2, color);
+	}
+}
+
+#[no_mangle]
+pub extern fn DrawTrait_DrawTransform(this: *mut DrawTrait, xf: &Transform) {
+	unsafe {
+		(**this).draw_transform(xf);
+	}
+}
diff --git a/src/box2d/common/math.rs b/src/box2d/common/math.rs
index 6f2c8de..08fc821 100644
--- a/src/box2d/common/math.rs
+++ b/src/box2d/common/math.rs
@@ -27,3 +27,14 @@ impl Vec2 {
 	}
 }
 
+/// A transform contains translation and rotation. It is used to represent
+/// the position and orientation of rigid frames.
+#[repr(C)]
+#[derive(Debug, Default, Copy, Clone, PartialEq)]
+pub struct Transform { pub p: Vec2, pub q: Rot }
+
+/// Rotation
+/// Sine and cosine
+#[repr(C)]
+#[derive(Debug, Default, Copy, Clone, PartialEq)]
+pub struct Rot { pub s: f32, pub c: f32 }
diff --git a/src/box2d/common/mod.rs b/src/box2d/common/mod.rs
index 83f2df1..efd85d8 100644
--- a/src/box2d/common/mod.rs
+++ b/src/box2d/common/mod.rs
@@ -1,2 +1,3 @@
 pub mod math;
-pub mod settings;
\ No newline at end of file
+pub mod settings;
+pub mod draw;
\ No newline at end of file
diff --git a/src/box2d/dynamics/body.rs b/src/box2d/dynamics/body.rs
index eee3090..ae2bed0 100644
--- a/src/box2d/dynamics/body.rs
+++ b/src/box2d/dynamics/body.rs
@@ -104,13 +104,16 @@ pub enum B2Body {}
 extern {
     fn b2Body_CreateFixture_FromShape(this: *mut B2Body, shape: *const B2Shape, density: Float32) -> *mut B2Fixture;
     fn b2Body_CreateFixture(this: *mut B2Body, def: *mut FixtureDef) -> *mut B2Fixture;
-    fn b2Body_GetAngle(this: *mut B2Body) -> Float32;
+    fn b2Body_GetAngle(this: *const B2Body) -> Float32;
     fn b2Body_GetFixtureList(this: *mut B2Body) -> *mut B2Fixture;
     fn b2Body_GetNext(this: *mut B2Body) -> *mut B2Body;
-    fn b2Body_GetPosition(this: *mut B2Body) -> &Vec2;
+    fn b2Body_GetPosition(this: *const B2Body) -> &Vec2;
     fn b2Body_GetUserData(this: *const B2Body) -> usize;
     fn b2Body_GetWorld(this: *const B2Body) -> *mut B2World;
     fn b2Body_GetLocalPoint(this: *const B2Body, worldPoint: &Vec2) -> Vec2;
+    fn b2Body_SetTransform(this: *mut B2Body, position: &Vec2, angle: Float32);
+    fn b2Body_SetLinearVelocity(this: *mut B2Body, v: &Vec2);
+    fn b2Body_GetLinearVelocity(this: *const B2Body) -> &Vec2;
 }
 
 /// A rigid body. These are created via b2World::CreateBody.
@@ -213,4 +216,21 @@ impl Body {
         }
     }
 
+    pub fn set_transform(&mut self, position: &Vec2, angle: f32) {
+        unsafe {
+            b2Body_SetTransform(self.ptr, position, angle)
+        }
+    }
+
+    pub fn set_linear_velocity(&mut self, v: &Vec2) {
+        unsafe {
+            b2Body_SetLinearVelocity(self.ptr, v)
+        }
+    }
+
+    pub fn get_linear_velocity(&self) -> &Vec2 {
+        unsafe {
+            b2Body_GetLinearVelocity(self.ptr)
+        }
+    }
 }
diff --git a/src/box2d/dynamics/world.rs b/src/box2d/dynamics/world.rs
index bfa7c89..89ba76e 100644
--- a/src/box2d/dynamics/world.rs
+++ b/src/box2d/dynamics/world.rs
@@ -1,184 +1,312 @@
 use libc::size_t;
 use std::ptr;
+use std::marker::PhantomData;
+
 use super::body::*;
 use super::super::common::math::*;
 use super::super::common::settings::*;
 use super::super::particle::particle_system::*;
 use super::joints;
+use super::super::common::draw::*;
 
 pub enum B2World {}
 
 extern {
-    fn b2World_CreateBody(world: *mut B2World, bd: *const BodyDef) -> *mut B2Body;
-    fn b2World_CreateParticleSystem(world: *mut B2World, def: *const ParticleSystemDef) -> *mut B2ParticleSystem;
-    fn b2World_Delete(world: *mut B2World);
-    fn b2World_GetBodyCount(world: *const B2World) -> Int32;
-    fn b2World_GetJointCount(world: *const B2World) -> Int32;
-    fn b2World_GetBodyList(world: *const B2World) -> *mut B2Body;
-    fn b2World_GetGravity(world: *mut B2World) -> Vec2;
-    fn b2World_GetParticleSystemList(world: *const B2World) -> *mut B2ParticleSystem;
-    fn b2World_New(gravity: *const Vec2) -> *mut B2World;
-    fn b2World_Step(this: *mut B2World, timeStep: Float32, velocityIterations: Int32, positionIterations: Int32);
-
-    fn b2World_CreateRevoluteJoint(
-        world: *mut B2World,
-        
-        joint_type: joints::JointType,
-        user_data: size_t,
-        body_a: *mut B2Body,
-        body_b: *mut B2Body,
-        collide_connected: bool,
-
-        local_anchor_a: Vec2,
-        local_anchor_b: Vec2,
-        reference_angle: Float32,
-        enable_limit: bool,
-        lower_angle: Float32,
-        upper_angle: Float32,
-        enable_motor: bool,
-        motor_speed: Float32,
-        max_motor_torque: Float32
-    ) -> *mut joints::revolute_joint::B2RevoluteJoint;
+	fn b2World_CreateBody(world: *mut B2World, bd: *const BodyDef) -> *mut B2Body;
+	fn b2World_CreateParticleSystem(world: *mut B2World, def: *const ParticleSystemDef) -> *mut B2ParticleSystem;
+	fn b2World_Delete(world: *mut B2World);
+	fn b2World_GetBodyCount(world: *const B2World) -> Int32;
+	fn b2World_GetJointCount(world: *const B2World) -> Int32;
+	fn b2World_GetBodyList(world: *const B2World) -> *mut B2Body;
+	fn b2World_GetGravity(world: *mut B2World) -> Vec2;
+	fn b2World_GetParticleSystemList(world: *const B2World) -> *mut B2ParticleSystem;
+	fn b2World_New(gravity: *const Vec2) -> *mut B2World;
+	fn b2World_Step(this: *mut B2World, timeStep: Float32, velocityIterations: Int32, positionIterations: Int32);
+
+	fn b2World_CreateRevoluteJoint(
+		world: *mut B2World,
+		
+		joint_type: joints::JointType,
+		user_data: size_t,
+		body_a: *mut B2Body,
+		body_b: *mut B2Body,
+		collide_connected: bool,
+
+		local_anchor_a: Vec2,
+		local_anchor_b: Vec2,
+		reference_angle: Float32,
+		enable_limit: bool,
+		lower_angle: Float32,
+		upper_angle: Float32,
+		enable_motor: bool,
+		motor_speed: Float32,
+		max_motor_torque: Float32
+	) -> *mut joints::revolute_joint::B2RevoluteJoint;
 
+	fn b2World_SetDebugDraw(this: *mut B2World, debug_draw: *mut CppDebugDraw);
+	// fn b2World_GetDebugDraw(this: *mut B2World) -> *mut CppDebugDraw;
+	fn b2World_DrawDebugData(this: *mut B2World);
 }
 
 /// The world class manages all physics entities, dynamic simulation,
 /// and asynchronous queries. The world also contains efficient memory
 /// management facilities.
 pub struct World {
-	pub ptr: *mut B2World
+	pub ptr: *mut B2World,
 }
 
 impl World {
 
-    /// Construct a world object.
-    /// @param gravity the world gravity vector.
-    pub fn new(gravity: &Vec2) -> World {
-        unsafe {
-            World { ptr: b2World_New(gravity) }
-        }
-    }
-
-    /// Create a rigid body given a definition. No reference to the definition
-    /// is retained.
-    /// @warning This function is locked during callbacks.
-    pub fn create_body(&mut self, def: &BodyDef) -> Body {
-        unsafe {
-            Body { ptr: b2World_CreateBody(self.ptr, def) }
-        }
-    }
-
-    /// Create a revolute joint to constrain bodies together. No reference to the definition
-    /// is retained. This may cause the connected bodies to cease colliding.
-    /// @warning This function is locked during callbacks.
-    pub fn create_revolute_joint(&mut self, def: &(joints::JointDef, joints::revolute_joint::RevoluteJointDef)) -> joints::revolute_joint::RevoluteJoint {
-        unsafe {
-            joints::revolute_joint::RevoluteJoint {ptr: b2World_CreateRevoluteJoint(
-                self.ptr,
-                def.0.joint_type,
-                def.0.user_data,
-                match def.0.body_a {
-                    Some(ref b) =>b.ptr,
-                    None => ptr::null_mut()
-                },
-                match def.0.body_b {
-                    Some(ref b) =>b.ptr,
-                    None => ptr::null_mut()
-                },
-                def.0.collide_connected,
-                def.1.local_anchor_a,
-                def.1.local_anchor_b,
-                def.1.reference_angle,
-                def.1.enable_limit,
-                def.1.lower_angle,
-                def.1.upper_angle,
-                def.1.enable_motor,
-                def.1.motor_speed,
-                def.1.max_motor_torque
-            )}
-        }
-    }
-
-    /// Create a particle system given a definition. No reference to the
-    /// definition is retained.
-    /// @warning This function is locked during callbacks.
-    pub fn create_particle_system(&self, def: &ParticleSystemDef) -> ParticleSystem {
-        unsafe {
-            ParticleSystem { ptr: b2World_CreateParticleSystem(self.ptr, def) }
-        }
-    }
-
-    /// Get the number of bodies.
-    pub fn get_body_count(&self) -> i32 {
-        unsafe {
-            b2World_GetBodyCount(self.ptr)
-        }
-    }
-
-    /// Get the number of joints.
-    pub fn get_joint_count(&self) -> i32 {
-        unsafe {
-            b2World_GetJointCount(self.ptr)
-        }
-    }
-
-    /// Get the world body list. With the returned body, use b2Body::GetNext to get
-    /// the next body in the world list. A NULL body indicates the end of the list.
-    /// @return the head of the world body list.
-    pub fn get_body_list(&self) -> Option<Body> {
-        let ptr;
-        unsafe {
-            ptr = b2World_GetBodyList(self.ptr);
-        }
-
-        if ptr.is_null() {
-            None
-        } else {
-            Some(Body { ptr: ptr })
-        }
-    }
-
-    /// Get the world particle-system list. With the returned body, use
-    /// b2ParticleSystem::GetNext to get the next particle-system in the world
-    /// list. A NULL particle-system indicates the end of the list.
-    /// @return the head of the world particle-system list.
-    pub fn get_particle_system_list(&self) -> Option<ParticleSystem> {
-        let ptr;
-        unsafe {
-            ptr = b2World_GetParticleSystemList(self.ptr);
-        }
-
-        if ptr.is_null() {
-            None
-        } else {
-            Some(ParticleSystem { ptr: ptr })
-        }
-    }
-
-    /// Get the global gravity vector.
-    pub fn get_gravity(&mut self) -> Vec2 {
-    	unsafe {
-    		b2World_GetGravity(self.ptr)
-    	}
-    }
-
-    /// Take a time step. This performs collision detection, integration,
-    /// and constraint solution.
-    /// @param timeStep the amount of time to simulate, this should not vary.
-    /// @param velocityIterations for the velocity constraint solver.
-    /// @param positionIterations for the position constraint solver.
-    pub fn step(&mut self, time_step: f32, velocity_iterations: i32, position_iterations: i32) {
-        unsafe {
-            b2World_Step(self.ptr, time_step, velocity_iterations, position_iterations);
-        }
-    }    
+	/// Construct a world object.
+	/// @param gravity the world gravity vector.
+	pub fn new(gravity: &Vec2) -> World {
+		unsafe {
+			World { ptr: b2World_New(gravity) }
+		}
+	}
+
+	/// Create a rigid body given a definition. No reference to the definition
+	/// is retained.
+	/// @warning This function is locked during callbacks.
+	pub fn create_body(&mut self, def: &BodyDef) -> Body {
+		unsafe {
+			Body { ptr: b2World_CreateBody(self.ptr, def) }
+		}
+	}
+
+	/// Create a revolute joint to constrain bodies together. No reference to the definition
+	/// is retained. This may cause the connected bodies to cease colliding.
+	/// @warning This function is locked during callbacks.
+	pub fn create_revolute_joint(&mut self, def: &(joints::JointDef, joints::revolute_joint::RevoluteJointDef)) -> joints::revolute_joint::RevoluteJoint {
+		unsafe {
+			joints::revolute_joint::RevoluteJoint {ptr: b2World_CreateRevoluteJoint(
+				self.ptr,
+				def.0.joint_type,
+				def.0.user_data,
+				match def.0.body_a {
+					Some(ref b) =>b.ptr,
+					None => ptr::null_mut()
+				},
+				match def.0.body_b {
+					Some(ref b) =>b.ptr,
+					None => ptr::null_mut()
+				},
+				def.0.collide_connected,
+				def.1.local_anchor_a,
+				def.1.local_anchor_b,
+				def.1.reference_angle,
+				def.1.enable_limit,
+				def.1.lower_angle,
+				def.1.upper_angle,
+				def.1.enable_motor,
+				def.1.motor_speed,
+				def.1.max_motor_torque
+			)}
+		}
+	}
+
+	/// Create a particle system given a definition. No reference to the
+	/// definition is retained.
+	/// @warning This function is locked during callbacks.
+	pub fn create_particle_system(&self, def: &ParticleSystemDef) -> ParticleSystem {
+		unsafe {
+			ParticleSystem { ptr: b2World_CreateParticleSystem(self.ptr, def) }
+		}
+	}
+
+	/// Get the number of bodies.
+	pub fn get_body_count(&self) -> i32 {
+		unsafe {
+			b2World_GetBodyCount(self.ptr)
+		}
+	}
+
+	/// Get the number of joints.
+	pub fn get_joint_count(&self) -> i32 {
+		unsafe {
+			b2World_GetJointCount(self.ptr)
+		}
+	}
+
+	/// Get the world body list. With the returned body, use b2Body::GetNext to get
+	/// the next body in the world list. A NULL body indicates the end of the list.
+	/// @return the head of the world body list.
+	pub fn get_body_list(&self) -> Option<Body> {
+		let ptr;
+		unsafe {
+			ptr = b2World_GetBodyList(self.ptr);
+		}
+
+		if ptr.is_null() {
+			None
+		} else {
+			Some(Body { ptr: ptr })
+		}
+	}
 
+	/// Get the world particle-system list. With the returned body, use
+	/// b2ParticleSystem::GetNext to get the next particle-system in the world
+	/// list. A NULL particle-system indicates the end of the list.
+	/// @return the head of the world particle-system list.
+	pub fn get_particle_system_list(&self) -> Option<ParticleSystem> {
+		let ptr;
+		unsafe {
+			ptr = b2World_GetParticleSystemList(self.ptr);
+		}
+
+		if ptr.is_null() {
+			None
+		} else {
+			Some(ParticleSystem { ptr: ptr })
+		}
+	}
+
+	/// Get the global gravity vector.
+	pub fn get_gravity(&mut self) -> Vec2 {
+		unsafe {
+			b2World_GetGravity(self.ptr)
+		}
+	}
+
+	/// Take a time step. This performs collision detection, integration,
+	/// and constraint solution.
+	/// @param timeStep the amount of time to simulate, this should not vary.
+	/// @param velocityIterations for the velocity constraint solver.
+	/// @param positionIterations for the position constraint solver.
+	pub fn step(&mut self, time_step: f32, velocity_iterations: i32, position_iterations: i32) {
+		unsafe {
+			b2World_Step(self.ptr, time_step, velocity_iterations, position_iterations);
+		}
+	}
+
+	/// Register a routine for debug drawing. The debug draw functions are called
+	/// inside with World::draw_debug_data() method. The debug draw object is owned
+	/// by you and must remain in scope.
+	pub fn set_debug_draw<T: Draw + 'static>(&mut self, debug_draw: &mut DebugDraw<T>) {
+		unsafe {
+			b2World_SetDebugDraw(self.ptr, debug_draw.handle.ptr);
+		}
+	}
+
+	/// World stores a pointer to DebugDraw when you call World.set_debug_draw().
+	/// You have to make sure your DebugDraw instance stays alive as long as the world.
+	/// If the world outlives your DebugDraw instance, draw_debug_data() would dereference
+	/// an invalid pointer. So call this function to set the internal DebugDraw pointer to null.
+	pub fn clear_debug_draw(&mut self) {
+		unsafe {
+			b2World_SetDebugDraw(self.ptr, ptr::null_mut());
+		}
+	}
+
+	/// Call this to draw shapes and other debug draw data. This is intentionally non-const.
+	pub fn draw_debug_data(&mut self) {
+		unsafe {
+			b2World_DrawDebugData(self.ptr);
+		}
+	}
 }
 
 impl Drop for World {
-    fn drop(&mut self) {
-        unsafe {
-            b2World_Delete(self.ptr);
-        }
-    }
-}
\ No newline at end of file
+	fn drop(&mut self) {
+		unsafe {
+			b2World_Delete(self.ptr);
+		}
+	}
+}
+
+pub enum CppDebugDraw {}
+
+extern {
+	fn CppDebugDraw_new(debug_draw: *mut DrawTrait) -> *mut CppDebugDraw;
+	fn CppDebugDraw_delete(this: *mut CppDebugDraw);
+	fn CppDebugDraw_SetFlags(this: *mut CppDebugDraw, flags: UInt32);
+	fn CppDebugDraw_GetFlags(this: *mut CppDebugDraw) -> UInt32;
+	fn CppDebugDraw_AppendFlags(this: *mut CppDebugDraw, flags: UInt32);
+	fn CppDebugDraw_ClearFlags(this: *mut CppDebugDraw, flags: UInt32);
+}
+
+struct CppDebugDrawHandle {
+	draw_trait: *mut DrawTrait,
+	ptr: *mut CppDebugDraw,
+}
+
+impl CppDebugDrawHandle {
+	pub fn new(debug_draw: *mut DrawTrait) -> Self {
+		unsafe {
+			Self {
+				draw_trait: debug_draw,
+				ptr: CppDebugDraw_new(debug_draw)
+			}
+		}
+	}
+}
+
+impl Drop for CppDebugDrawHandle {
+	fn drop(&mut self) {
+		unsafe {
+			CppDebugDraw_delete(self.ptr);
+		}
+	}
+}
+
+/// Call DebugDraw::new() to create a new DebugDraw instance from an instance of your type
+/// that implements the Draw trait and pass this to World::set_debug_draw()
+pub struct DebugDraw<T: Draw + 'static> {
+	handle: CppDebugDrawHandle,
+	phantom: PhantomData<T>,
+}
+
+impl<T: Draw + 'static> DebugDraw<T> {
+
+	/// Creates a new DebugDraw instance.
+	pub fn new(debug_draw: T) -> Self {
+		Self {
+			handle: CppDebugDrawHandle::new(Box::into_raw(box (box debug_draw as Box<Draw>))),
+			phantom: PhantomData,
+		}
+	}
+
+	/// Get direct access to your Draw instance.
+	pub fn get(&mut self) -> &mut T {
+		match unsafe { (*self.handle.draw_trait).as_any().downcast_mut::<T>() } {
+			Some(x) => x,
+			None => panic!("invalid downcast of Box<Draw> to Box<T> where T: Draw")
+		}
+	}
+
+	/// Set the drawing flags.
+	pub fn set_flags(&mut self, flags: u32) {
+		unsafe {
+			CppDebugDraw_SetFlags(self.handle.ptr, flags);
+		}
+	}
+
+	/// Get the drawing flags.
+	pub fn get_flags(&mut self) -> u32 {
+		unsafe {
+			CppDebugDraw_GetFlags(self.handle.ptr)
+		}
+	}
+
+	/// Append flags to the current flags.
+	pub fn append_flags(&mut self, flags: u32) {
+		unsafe {
+			CppDebugDraw_AppendFlags(self.handle.ptr, flags);
+		}
+	}
+
+	/// Clear flags from the current flags.
+	pub fn clear_flags(&mut self, flags: u32) {
+		unsafe {
+			CppDebugDraw_ClearFlags(self.handle.ptr, flags);
+		}
+	}
+}
+
+impl<T: Draw + 'static> Drop for DebugDraw<T> {
+	fn drop(&mut self) {
+		unsafe {
+			drop(Box::from_raw(self.handle.draw_trait));
+		}
+	}
+}
diff --git a/src/lib.rs b/src/lib.rs
index 7b03920..6bd79ab 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -50,6 +50,9 @@
 //! }
 //! ```
 
+#![allow(renamed_and_removed_lints)]
+#![feature(box_syntax)]
+
 extern crate libc;
 #[macro_use]
 extern crate bitflags;
diff --git a/tests/hello-world.rs b/tests/hello-world.rs
index 6e767b6..e2bda85 100644
--- a/tests/hello-world.rs
+++ b/tests/hello-world.rs
@@ -73,8 +73,9 @@ fn hello_world() {
 		// Now print the position and angle of the body.
 		let position = body.get_position();
 		let angle = body.get_angle();
+		let vel = body.get_linear_velocity();
 
-		println!("{:?} angle: {:?}", position, angle);
+		println!("{:?} angle: {:?}, vel: {:?}", position, angle, vel);
 	}
 
 	// When the world destructor is called, all bodies and joints are freed. This can