init

.gitignore

+# Created by .ignore support plugin (hsz.mobi)
+### Node template
+# Logs
+logs
+*.log
+npm-debug.log*
+yarn-debug.log*
+yarn-error.log*
+
+# Runtime data
+pids
+*.pid
+*.seed
+*.pid.lock
+
+# Directory for instrumented libs generated by jscoverage/JSCover
+lib-cov
+
+# Coverage directory used by tools like istanbul
+coverage
+
+# nyc test coverage
+.nyc_output
+
+# Grunt intermediate storage (http://gruntjs.com/creating-plugins#storing-task-files)
+.grunt
+
+# Bower dependency directory (https://bower.io/)
+bower_components
+
+# node-waf configuration
+.lock-wscript
+
+# Compiled binary addons (https://nodejs.org/api/addons.html)
+build/Release
+
+# Dependency directories
+node_modules/
+jspm_packages/
+
+# TypeScript v1 declaration files
+typings/
+
+# Optional npm cache directory
+.npm
+
+# Optional eslint cache
+.eslintcache
+
+# Optional REPL history
+.node_repl_history
+
+# Output of 'npm pack'
+*.tgz
+
+# Yarn Integrity file
+.yarn-integrity
+
+# dotenv environment variables file
+.env
+
+# parcel-bundler cache (https://parceljs.org/)
+.cache
+
+# next.js build output
+.next
+
+# nuxt.js build output
+.nuxt
+
+# vuepress build output
+.vuepress/dist
+
+# Serverless directories
+.serverless
+

.jshintignore

+build
+examples
+node_modules
+Gruntfile.js
+README.md
+src/p2.js
+src/math/polyk.js
+src/wrapper
\ No newline at end of file

.jshintrc

+{
+    "node" : true,
+    "browser" : true,
+
+    "boss" : false,
+    "curly": true,
+    "debug": false,
+    "devel": false,
+    "eqeqeq": true,
+    "eqnull": true,
+    "evil": false,
+    "forin": false,
+    "immed": true,
+    "laxbreak": false,
+    "newcap": true,
+    "noarg": true,
+    "noempty": true,
+    "nonew": true,
+    "plusplus": false,
+    "regexp": false,
+    "smarttabs": true,
+    "sub": true,
+    "strict": false,
+    "trailing" : true,
+    "undef": true,
+    "indent": 4,
+    "shadow" : true,
+    "laxcomma": true,
+    "globals": {
+        "p2": true
+    }
+}
\ No newline at end of file

.npmignore

+# Temp files
+*~
+*#
+
+# Node Modules
+node_modules/*
+
+# File system
+.DS_Store
+
+# Sublime Text
+*.sublime-project
+*.sublime-workspace

Gruntfile.js

+var fs = require('fs');
+
+module.exports = function(grunt) {
+
+    grunt.initConfig({
+        pkg: grunt.file.readJSON('package.json'),
+
+        browserify : {
+            core : {
+                src : ["src/p2.js"],
+                dest : 'build/p2.js',
+                options:{
+                    bundleOptions : {
+                        standalone : "p2"
+                    }
+                }
+            }
+        },
+
+        uglify : {
+            build: {
+                src : ['build/p2.js'],
+                dest : 'build/p2.min.js'
+            },
+            demo: {
+                src : ['build/p2.renderer.js'],
+                dest : 'build/p2.renderer.min.js'
+            }
+        },
+
+        nodeunit: {
+            all: ['test/**/*.js'],
+        },
+
+        jshint: {
+            all: ['src/**/*.js'],
+            options:{
+                jshintrc: '.jshintrc',
+                force: true // Do not fail the task
+            }
+        },
+
+        watch: {
+            options: {
+                nospawn: false
+            },
+            source: {
+                files: 'src/**/*',
+                tasks: [
+                    'default'
+                ]
+            },
+            renderer: {
+                files: 'demos/js/*Renderer.js',
+                tasks: [
+                    'concat:renderer'
+                ]
+            },
+            test: {
+                files: ['src/**/*', 'test/**/*'],
+                tasks: [
+                    'test'
+                ]
+            },
+        },
+
+        concat: {
+            renderer: {
+                src: ['demos/js/pixi.js', 'demos/js/dat.gui.js', 'demos/js/Renderer.js', 'demos/js/WebGLRenderer.js'],
+                dest: 'build/p2.renderer.js',
+            }
+        }
+    });
+
+    grunt.loadNpmTasks('grunt-contrib-uglify');
+    grunt.loadNpmTasks('grunt-contrib-jshint');
+    grunt.loadNpmTasks('grunt-browserify');
+    grunt.loadNpmTasks('grunt-contrib-nodeunit');
+    grunt.loadNpmTasks('grunt-contrib-watch');
+    grunt.loadNpmTasks('grunt-contrib-concat');
+
+    grunt.registerTask('default', ['test','jshint','browserify','concat','uglify','addLicense','requireJsFix']);
+    grunt.registerTask('test', ['nodeunit']);
+
+    // Not sure what flag Browserify needs to do this. Fixing it manually for now.
+    grunt.registerTask('requireJsFix','Modifies the browserify bundle so it works with RequireJS',function(){
+        ['build/p2.js', 'build/p2.min.js'].forEach(function (path){
+            var text = fs.readFileSync(path).toString();
+            text = text.replace('define.amd', 'false'); // This makes the bundle skip using define() from RequireJS
+            fs.writeFileSync(path, text);
+        });
+    });
+
+    grunt.registerTask('addLicense','Adds the LICENSE to the top of the built files',function(){
+        var text = fs.readFileSync("LICENSE").toString();
+
+        var dev = fs.readFileSync("build/p2.js").toString();
+        var min = fs.readFileSync("build/p2.min.js").toString();
+
+        fs.writeFileSync("build/p2.js",text+"\n"+dev);
+        fs.writeFileSync("build/p2.min.js",text+"\n"+min);
+    });
+};

LICENSE

+/**
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2015 p2.js authors
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
\ No newline at end of file

README.md

+p2.js
+=====
+
+2D rigid body physics engine written in JavaScript. Includes collision detection, contacts, friction, restitution, motors, springs, advanced constraints and various shape types.
+
+[Demos](#demos) | [Examples](#examples) | [Documentation](http://schteppe.github.io/p2.js/docs/) | [Download](https://raw.github.com/schteppe/p2.js/master/build/p2.js) | [CDN](http://cdnjs.com/libraries/p2.js) | [Wiki](https://github.com/schteppe/p2.js/wiki)
+
+### Featured projects using p2.js
+* [Google I/O 2015 Experiment](http://www.chromeexperiments.com/detail/io-2015-experiment) by Instrument
+* [PixiLights, a Christmas Experiment](http://christmasexperiments.com/experiments/11) by Mat Groves
+* [More...](https://github.com/schteppe/p2.js/wiki/Projects-using-p2.js)
+
+### Demos
+These demos use the p2 Demo framework, which provides rendering and interactivity. Use mouse/touch to throw or create objects. Use the right menu (or console!) to tweak parameters. Or just check the source to see how to programmatically build the current scene using p2.
+
+* [Buoyancy](http://schteppe.github.io/p2.js/demos/buoyancy.html)
+* [Car](http://schteppe.github.io/p2.js/demos/car.html)
+* [CCD](http://schteppe.github.io/p2.js/demos/ccd.html)
+* [Circle container](http://schteppe.github.io/p2.js/demos/circles.html)
+* [Collision tests](http://schteppe.github.io/p2.js/demos/collisions.html)
+* [Compound objects](http://schteppe.github.io/p2.js/demos/compound.html)
+* [Concave objects](http://schteppe.github.io/p2.js/demos/concave.html)
+* [Constraints](http://schteppe.github.io/p2.js/demos/constraints.html)
+* [DistanceConstraint](http://schteppe.github.io/p2.js/demos/distanceConstraint.html)
+* [Fixed rotation](http://schteppe.github.io/p2.js/demos/fixedRotation.html)
+* [Fixed XY](http://schteppe.github.io/p2.js/demos/fixedXY.html)
+* [Friction](http://schteppe.github.io/p2.js/demos/friction.html)
+* [Gear constraint](http://schteppe.github.io/p2.js/demos/gearConstraint.html)
+* [Heightfield](http://schteppe.github.io/p2.js/demos/heightfield.html)
+* [Island solver](http://schteppe.github.io/p2.js/demos/islandSolver.html)
+* [Kinematic body](http://schteppe.github.io/p2.js/demos/kinematic.html)
+* [Lock constraint](http://schteppe.github.io/p2.js/demos/lock.html)
+* [Piston](http://schteppe.github.io/p2.js/demos/piston.html)
+* [Prismatic constraint](http://schteppe.github.io/p2.js/demos/prismatic.html)
+* [Ragdoll](http://schteppe.github.io/p2.js/demos/ragdoll.html)
+* [Sensor](http://schteppe.github.io/p2.js/demos/removeSensor.html)
+* [Restitution](http://schteppe.github.io/p2.js/demos/restitution.html)
+* [Sleep](http://schteppe.github.io/p2.js/demos/sleep.html)
+* [Segway](http://schteppe.github.io/p2.js/demos/segway.html)
+* [Sleep](http://schteppe.github.io/p2.js/demos/sleep.html)
+* [Springs](http://schteppe.github.io/p2.js/demos/springs.html)
+* [Surface velocity](http://schteppe.github.io/p2.js/demos/surfaceVelocity.html)
+* [Suspension](http://schteppe.github.io/p2.js/demos/suspension.html)
+* [Tearable constraints](http://schteppe.github.io/p2.js/demos/tearable.html)
+* [TopDownVehicle](http://schteppe.github.io/p2.js/demos/topDownVehicle.html)
+
+### Examples
+Examples showing how to use p2.js with your favorite renderer.
+
+* [Canvas: Asteroids game](http://schteppe.github.io/p2.js/examples/canvas/asteroids.html)
+* [Canvas: Box on plane](http://schteppe.github.io/p2.js/examples/canvas/box.html)
+* [Canvas: Character demo](http://schteppe.github.io/p2.js/examples/canvas/character.html)
+* [Canvas: Circle on plane](http://schteppe.github.io/p2.js/examples/canvas/circle.html)
+* [Canvas: Interpolation](http://schteppe.github.io/p2.js/examples/canvas/interpolation.html)
+* [Canvas: Mousejoint](http://schteppe.github.io/p2.js/examples/canvas/mouseJoint.html)
+* [Canvas: Raycasting](http://schteppe.github.io/p2.js/examples/canvas/raycasting.html)
+* [Canvas: Rayreflect](http://schteppe.github.io/p2.js/examples/canvas/rayreflect.html)
+* [Canvas: Sensors](http://schteppe.github.io/p2.js/examples/canvas/sensors.html)
+* [Canvas: Sensors 2](http://schteppe.github.io/p2.js/examples/canvas/sensors2.html)
+* [Pixi.js: Box on plane](http://schteppe.github.io/p2.js/examples/pixijs/box.html)
+
+### Sample code
+The following example uses the [World](http://schteppe.github.io/p2.js/docs/classes/World.html), [Circle](http://schteppe.github.io/p2.js/docs/classes/Circle.html), [Body](http://schteppe.github.io/p2.js/docs/classes/Body.html) and [Plane](http://schteppe.github.io/p2.js/docs/classes/Plane.html) classes to set up a simple physics scene with a ball on a plane.
+
+```js
+// Create a physics world, where bodies and constraints live
+var world = new p2.World({
+    gravity:[0, -9.82]
+});
+
+// Create an empty dynamic body
+var circleBody = new p2.Body({
+    mass: 5,
+    position: [0, 10]
+});
+
+// Add a circle shape to the body.
+var circleShape = new p2.Circle({ radius: 1 });
+circleBody.addShape(circleShape);
+
+// ...and add the body to the world.
+// If we don't add it to the world, it won't be simulated.
+world.addBody(circleBody);
+
+// Create an infinite ground plane.
+var groundBody = new p2.Body({
+    mass: 0 // Setting mass to 0 makes the body static
+});
+var groundShape = new p2.Plane();
+groundBody.addShape(groundShape);
+world.addBody(groundBody);
+
+// To get the trajectories of the bodies,
+// we must step the world forward in time.
+// This is done using a fixed time step size.
+var timeStep = 1 / 60; // seconds
+
+// The "Game loop". Could be replaced by, for example, requestAnimationFrame.
+setInterval(function(){
+
+    // The step method moves the bodies forward in time.
+    world.step(timeStep);
+
+    // Print the circle position to console.
+    // Could be replaced by a render call.
+    console.log("Circle y position: " + circleBody.position[1]);
+
+}, 1000 * timeStep);
+```
+
+### Install
+##### Browser
+Download either [p2.js](build/p2.js) or the minified [p2.min.js](build/p2.min.js) and include the script in your HTML:
+```html
+<script src="p2.js" type="text/javascript"></script>
+```
+
+If you would like to use ordinary ```Array``` instead of ```Float32Array```, define ```P2_ARRAY_TYPE``` globally before loading the library.
+
+```html
+<script type="text/javascript">P2_ARRAY_TYPE = Array;</script>
+<script src="p2.js" type="text/javascript"></script>
+```
+
+##### Node.js
+```
+npm install p2
+```
+Then require it like so:
+```js
+var p2 = require('p2');
+```
+
+### Supported collision pairs
+|                                                                              | Circle | Plane | Box       | Convex | Particle | Line   | Capsule | Heightfield | Ray    |
+| :--------------------------------------------------------------------------: |:------:|:-----:|:---------:|:------:|:--------:|:------:|:-------:|:-----------:|:------:|
+| [Circle](http://schteppe.github.io/p2.js/docs/classes/Circle.html)           | Yes    | -     | -         | -      | -        | -      | -       | -           | -      |
+| [Plane](http://schteppe.github.io/p2.js/docs/classes/Plane.html)             | Yes    | -     | -         | -      | -        | -      | -       | -           | -      |
+| [Box](http://schteppe.github.io/p2.js/docs/classes/Box.html)                 | Yes    | Yes   | Yes       | -      | -        | -      | -       | -           | -      |
+| [Convex](http://schteppe.github.io/p2.js/docs/classes/Convex.html)           | Yes    | Yes   | Yes       | Yes    | -        | -      | -       | -           | -      |
+| [Particle](http://schteppe.github.io/p2.js/docs/classes/Particle.html)       | Yes    | Yes   | Yes       | Yes    | -        | -      | -       | -           | -      |
+| [Line](http://schteppe.github.io/p2.js/docs/classes/Line.html)               | Yes    | Yes   | (todo)    | (todo) | -        | -      | -       | -           | -      |
+| [Capsule](http://schteppe.github.io/p2.js/docs/classes/Capsule.html)         | Yes    | Yes   | Yes       | Yes    | Yes      | (todo) | Yes     | -           | -      |
+| [Heightfield](http://schteppe.github.io/p2.js/docs/classes/Heightfield.html) | Yes    | -     | Yes       | Yes    | (todo)   | (todo) | (todo)  | -           | -      |
+| [Ray](http://schteppe.github.io/p2.js/docs/classes/Ray.html)                 | Yes    | Yes   | Yes       | Yes    | -        | Yes    | Yes     | Yes         | -      |
+
+Note that concave polygon shapes can be created using [Body.fromPolygon](http://schteppe.github.io/p2.js/docs/classes/Body.html#method_fromPolygon).
+
+### Unit testing
+Tests are written for [Nodeunit](https://github.com/caolan/nodeunit). Run the tests with the command ```grunt test```.
+
+### Contribute
+Make sure you have git, [Node.js](http://nodejs.org), NPM and [grunt](http://gruntjs.com/) installed.
+```
+git clone https://github.com/schteppe/p2.js.git; # Clone the repo
+cd p2.js;
+npm install;                                     # Install dependencies
+                                                 # (make changes to source)
+grunt;                                           # Builds build/p2.js and build/p2.min.js
+```
+The most recent commits are currently pushed to the ```master``` branch. Thanks for contributing!

p2.iml

+<?xml version="1.0" encoding="UTF-8"?>
+<module type="WEB_MODULE" version="4">
+  <component name="NewModuleRootManager" inherit-compiler-output="true">
+    <exclude-output />
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>
\ No newline at end of file

package.json

+{
+  "name": "p2",
+  "version": "0.7.1",
+  "description": "A JavaScript 2D physics engine.",
+  "author": "Stefan Hedman <schteppe@gmail.com> (http://steffe.se)",
+  "keywords": [
+    "p2.js",
+    "p2",
+    "physics",
+    "engine",
+    "2d"
+  ],
+  "main": "./build/p2.js",
+  "engines": {
+    "node": "*"
+  },
+  "repository": {
+    "type": "git",
+    "url": "https://github.com/schteppe/p2.js.git"
+  },
+  "bugs": {
+    "url": "https://github.com/schteppe/p2.js/issues"
+  },
+  "licenses": [
+    {
+      "type": "MIT"
+    }
+  ],
+  "devDependencies": {
+    "grunt": "^0.4.5",
+    "grunt-contrib-jshint": "^0.11.2",
+    "grunt-contrib-nodeunit": "^0.4.1",
+    "grunt-contrib-uglify": "~0.4.0",
+    "grunt-contrib-watch": "~0.5.0",
+    "grunt-browserify": "~2.0.1",
+    "grunt-contrib-concat": "^0.4.0"
+  },
+  "dependencies": {
+    "poly-decomp": "0.1.1"
+  }
+}

src/collision/AABB.js

+var vec2 = require('../math/vec2')
+,   Utils = require('../utils/Utils');
+
+module.exports = AABB;
+
+/**
+ * Axis aligned bounding box class.
+ * @class AABB
+ * @constructor
+ * @param {Object}  [options]
+ * @param {Array}   [options.upperBound]
+ * @param {Array}   [options.lowerBound]
+ */
+function AABB(options){
+
+    /**
+     * The lower bound of the bounding box.
+     * @property lowerBound
+     * @type {Array}
+     */
+    this.lowerBound = vec2.create();
+    if(options && options.lowerBound){
+        vec2.copy(this.lowerBound, options.lowerBound);
+    }
+
+    /**
+     * The upper bound of the bounding box.
+     * @property upperBound
+     * @type {Array}
+     */
+    this.upperBound = vec2.create();
+    if(options && options.upperBound){
+        vec2.copy(this.upperBound, options.upperBound);
+    }
+}
+
+var tmp = vec2.create();
+
+/**
+ * Set the AABB bounds from a set of points, transformed by the given position and angle.
+ * @method setFromPoints
+ * @param {Array} points An array of vec2's.
+ * @param {Array} position
+ * @param {number} angle
+ * @param {number} skinSize Some margin to be added to the AABB.
+ */
+AABB.prototype.setFromPoints = function(points, position, angle, skinSize){
+    var l = this.lowerBound,
+        u = this.upperBound;
+
+    if(typeof(angle) !== "number"){
+        angle = 0;
+    }
+
+    // Set to the first point
+    if(angle !== 0){
+        vec2.rotate(l, points[0], angle);
+    } else {
+        vec2.copy(l, points[0]);
+    }
+    vec2.copy(u, l);
+
+    // Compute cosines and sines just once
+    var cosAngle = Math.cos(angle),
+        sinAngle = Math.sin(angle);
+    for(var i = 1; i<points.length; i++){
+        var p = points[i];
+
+        if(angle !== 0){
+            var x = p[0],
+                y = p[1];
+            tmp[0] = cosAngle * x -sinAngle * y;
+            tmp[1] = sinAngle * x +cosAngle * y;
+            p = tmp;
+        }
+
+        for(var j=0; j<2; j++){
+            if(p[j] > u[j]){
+                u[j] = p[j];
+            }
+            if(p[j] < l[j]){
+                l[j] = p[j];
+            }
+        }
+    }
+
+    // Add offset
+    if(position){
+        vec2.add(this.lowerBound, this.lowerBound, position);
+        vec2.add(this.upperBound, this.upperBound, position);
+    }
+
+    if(skinSize){
+        this.lowerBound[0] -= skinSize;
+        this.lowerBound[1] -= skinSize;
+        this.upperBound[0] += skinSize;
+        this.upperBound[1] += skinSize;
+    }
+};
+
+/**
+ * Copy bounds from an AABB to this AABB
+ * @method copy
+ * @param  {AABB} aabb
+ */
+AABB.prototype.copy = function(aabb){
+    vec2.copy(this.lowerBound, aabb.lowerBound);
+    vec2.copy(this.upperBound, aabb.upperBound);
+};
+
+/**
+ * Extend this AABB so that it covers the given AABB too.
+ * @method extend
+ * @param  {AABB} aabb
+ */
+AABB.prototype.extend = function(aabb){
+    // Loop over x and y
+    var i = 2;
+    while(i--){
+        // Extend lower bound
+        var l = aabb.lowerBound[i];
+        if(this.lowerBound[i] > l){
+            this.lowerBound[i] = l;
+        }
+
+        // Upper
+        var u = aabb.upperBound[i];
+        if(this.upperBound[i] < u){
+            this.upperBound[i] = u;
+        }
+    }
+};
+
+/**
+ * Returns true if the given AABB overlaps this AABB.
+ * @method overlaps
+ * @param  {AABB} aabb
+ * @return {Boolean}
+ */
+AABB.prototype.overlaps = function(aabb){
+    var l1 = this.lowerBound,
+        u1 = this.upperBound,
+        l2 = aabb.lowerBound,
+        u2 = aabb.upperBound;
+
+    //      l2        u2
+    //      |---------|
+    // |--------|
+    // l1       u1
+
+    return ((l2[0] <= u1[0] && u1[0] <= u2[0]) || (l1[0] <= u2[0] && u2[0] <= u1[0])) &&
+           ((l2[1] <= u1[1] && u1[1] <= u2[1]) || (l1[1] <= u2[1] && u2[1] <= u1[1]));
+};
+
+/**
+ * @method containsPoint
+ * @param  {Array} point
+ * @return {boolean}
+ */
+AABB.prototype.containsPoint = function(point){
+    var l = this.lowerBound,
+        u = this.upperBound;
+    return l[0] <= point[0] && point[0] <= u[0] && l[1] <= point[1] && point[1] <= u[1];
+};
+
+/**
+ * Check if the AABB is hit by a ray.
+ * @method overlapsRay
+ * @param  {Ray} ray
+ * @return {number} -1 if no hit, a number between 0 and 1 if hit.
+ */
+AABB.prototype.overlapsRay = function(ray){
+    var t = 0;
+
+    // ray.direction is unit direction vector of ray
+    var dirFracX = 1 / ray.direction[0];
+    var dirFracY = 1 / ray.direction[1];
+
+    // this.lowerBound is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner
+    var t1 = (this.lowerBound[0] - ray.from[0]) * dirFracX;
+    var t2 = (this.upperBound[0] - ray.from[0]) * dirFracX;
+    var t3 = (this.lowerBound[1] - ray.from[1]) * dirFracY;
+    var t4 = (this.upperBound[1] - ray.from[1]) * dirFracY;
+
+    var tmin = Math.max(Math.max(Math.min(t1, t2), Math.min(t3, t4)));
+    var tmax = Math.min(Math.min(Math.max(t1, t2), Math.max(t3, t4)));
+
+    // if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us
+    if (tmax < 0){
+        //t = tmax;
+        return -1;
+    }
+
+    // if tmin > tmax, ray doesn't intersect AABB
+    if (tmin > tmax){
+        //t = tmax;
+        return -1;
+    }
+
+    return tmin;
+};
\ No newline at end of file

src/collision/Broadphase.js

+var vec2 = require('../math/vec2');
+var Body = require('../objects/Body');
+
+module.exports = Broadphase;
+
+/**
+ * Base class for broadphase implementations.
+ * @class Broadphase
+ * @constructor
+ */
+function Broadphase(type){
+
+    this.type = type;
+
+    /**
+     * The resulting overlapping pairs. Will be filled with results during .getCollisionPairs().
+     * @property result
+     * @type {Array}
+     */
+    this.result = [];
+
+    /**
+     * The world to search for collision pairs in. To change it, use .setWorld()
+     * @property world
+     * @type {World}
+     * @readOnly
+     */
+    this.world = null;
+
+    /**
+     * The bounding volume type to use in the broadphase algorithms. Should be set to Broadphase.AABB or Broadphase.BOUNDING_CIRCLE.
+     * @property {Number} boundingVolumeType
+     */
+    this.boundingVolumeType = Broadphase.AABB;
+}
+
+/**
+ * Axis aligned bounding box type.
+ * @static
+ * @property {Number} AABB
+ */
+Broadphase.AABB = 1;
+
+/**
+ * Bounding circle type.
+ * @static
+ * @property {Number} BOUNDING_CIRCLE
+ */
+Broadphase.BOUNDING_CIRCLE = 2;
+
+/**
+ * Set the world that we are searching for collision pairs in.
+ * @method setWorld
+ * @param  {World} world
+ */
+Broadphase.prototype.setWorld = function(world){
+    this.world = world;
+};
+
+/**
+ * Get all potential intersecting body pairs.
+ * @method getCollisionPairs
+ * @param  {World} world The world to search in.
+ * @return {Array} An array of the bodies, ordered in pairs. Example: A result of [a,b,c,d] means that the potential pairs are: (a,b), (c,d).
+ */
+Broadphase.prototype.getCollisionPairs = function(world){};
+
+var dist = vec2.create();
+
+/**
+ * Check whether the bounding radius of two bodies overlap.
+ * @method  boundingRadiusCheck
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {Boolean}
+ */
+Broadphase.boundingRadiusCheck = function(bodyA, bodyB){
+    vec2.sub(dist, bodyA.position, bodyB.position);
+    var d2 = vec2.squaredLength(dist),
+        r = bodyA.boundingRadius + bodyB.boundingRadius;
+    return d2 <= r*r;
+};
+
+/**
+ * Check whether the bounding radius of two bodies overlap.
+ * @method  boundingRadiusCheck
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {Boolean}
+ */
+Broadphase.aabbCheck = function(bodyA, bodyB){
+    return bodyA.getAABB().overlaps(bodyB.getAABB());
+};
+
+/**
+ * Check whether the bounding radius of two bodies overlap.
+ * @method  boundingRadiusCheck
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {Boolean}
+ */
+Broadphase.prototype.boundingVolumeCheck = function(bodyA, bodyB){
+    var result;
+
+    switch(this.boundingVolumeType){
+    case Broadphase.BOUNDING_CIRCLE:
+        result =  Broadphase.boundingRadiusCheck(bodyA,bodyB);
+        break;
+    case Broadphase.AABB:
+        result = Broadphase.aabbCheck(bodyA,bodyB);
+        break;
+    default:
+        throw new Error('Bounding volume type not recognized: '+this.boundingVolumeType);
+    }
+    return result;
+};
+
+/**
+ * Check whether two bodies are allowed to collide at all.
+ * @method  canCollide
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {Boolean}
+ */
+Broadphase.canCollide = function(bodyA, bodyB){
+    var KINEMATIC = Body.KINEMATIC;
+    var STATIC = Body.STATIC;
+
+    // Cannot collide static bodies
+    if(bodyA.type === STATIC && bodyB.type === STATIC){
+        return false;
+    }
+
+    // Cannot collide static vs kinematic bodies
+    if( (bodyA.type === KINEMATIC && bodyB.type === STATIC) ||
+        (bodyA.type === STATIC    && bodyB.type === KINEMATIC)){
+        return false;
+    }
+
+    // Cannot collide kinematic vs kinematic
+    if(bodyA.type === KINEMATIC && bodyB.type === KINEMATIC){
+        return false;
+    }
+
+    // Cannot collide both sleeping bodies
+    if(bodyA.sleepState === Body.SLEEPING && bodyB.sleepState === Body.SLEEPING){
+        return false;
+    }
+
+    // Cannot collide if one is static and the other is sleeping
+    if( (bodyA.sleepState === Body.SLEEPING && bodyB.type === STATIC) ||
+        (bodyB.sleepState === Body.SLEEPING && bodyA.type === STATIC)){
+        return false;
+    }
+
+    return true;
+};
+
+Broadphase.NAIVE = 1;
+Broadphase.SAP = 2;

src/collision/NaiveBroadphase.js

+var Circle = require('../shapes/Circle'),
+    Plane = require('../shapes/Plane'),
+    Shape = require('../shapes/Shape'),
+    Particle = require('../shapes/Particle'),
+    Broadphase = require('../collision/Broadphase'),
+    vec2 = require('../math/vec2');
+
+module.exports = NaiveBroadphase;
+
+/**
+ * Naive broadphase implementation. Does N^2 tests.
+ *
+ * @class NaiveBroadphase
+ * @constructor
+ * @extends Broadphase
+ */
+function NaiveBroadphase(){
+    Broadphase.call(this, Broadphase.NAIVE);
+}
+NaiveBroadphase.prototype = new Broadphase();
+NaiveBroadphase.prototype.constructor = NaiveBroadphase;
+
+/**
+ * Get the colliding pairs
+ * @method getCollisionPairs
+ * @param  {World} world
+ * @return {Array}
+ */
+NaiveBroadphase.prototype.getCollisionPairs = function(world){
+    var bodies = world.bodies,
+        result = this.result;
+
+    result.length = 0;
+
+    for(var i=0, Ncolliding=bodies.length; i!==Ncolliding; i++){
+        var bi = bodies[i];
+
+        for(var j=0; j<i; j++){
+            var bj = bodies[j];
+
+            if(Broadphase.canCollide(bi,bj) && this.boundingVolumeCheck(bi,bj)){
+                result.push(bi,bj);
+            }
+        }
+    }
+
+    return result;
+};
+
+/**
+ * Returns all the bodies within an AABB.
+ * @method aabbQuery
+ * @param  {World} world
+ * @param  {AABB} aabb
+ * @param {array} result An array to store resulting bodies in.
+ * @return {array}
+ */
+NaiveBroadphase.prototype.aabbQuery = function(world, aabb, result){
+    result = result || [];
+
+    var bodies = world.bodies;
+    for(var i = 0; i < bodies.length; i++){
+        var b = bodies[i];
+
+        if(b.aabbNeedsUpdate){
+            b.updateAABB();
+        }
+
+        if(b.aabb.overlaps(aabb)){
+            result.push(b);
+        }
+    }
+
+    return result;
+};
\ No newline at end of file

src/collision/Narrowphase.js

+var vec2 = require('../math/vec2')
+,   sub = vec2.sub
+,   add = vec2.add
+,   dot = vec2.dot
+,   Utils = require('../utils/Utils')
+,   ContactEquationPool = require('../utils/ContactEquationPool')
+,   FrictionEquationPool = require('../utils/FrictionEquationPool')
+,   TupleDictionary = require('../utils/TupleDictionary')
+,   Equation = require('../equations/Equation')
+,   ContactEquation = require('../equations/ContactEquation')
+,   FrictionEquation = require('../equations/FrictionEquation')
+,   Circle = require('../shapes/Circle')
+,   Convex = require('../shapes/Convex')
+,   Shape = require('../shapes/Shape')
+,   Body = require('../objects/Body')
+,   Box = require('../shapes/Box');
+
+module.exports = Narrowphase;
+
+// Temp things
+var yAxis = vec2.fromValues(0,1);
+
+var tmp1 = vec2.fromValues(0,0)
+,   tmp2 = vec2.fromValues(0,0)
+,   tmp3 = vec2.fromValues(0,0)
+,   tmp4 = vec2.fromValues(0,0)
+,   tmp5 = vec2.fromValues(0,0)
+,   tmp6 = vec2.fromValues(0,0)
+,   tmp7 = vec2.fromValues(0,0)
+,   tmp8 = vec2.fromValues(0,0)
+,   tmp9 = vec2.fromValues(0,0)
+,   tmp10 = vec2.fromValues(0,0)
+,   tmp11 = vec2.fromValues(0,0)
+,   tmp12 = vec2.fromValues(0,0)
+,   tmp13 = vec2.fromValues(0,0)
+,   tmp14 = vec2.fromValues(0,0)
+,   tmp15 = vec2.fromValues(0,0)
+,   tmp16 = vec2.fromValues(0,0)
+,   tmp17 = vec2.fromValues(0,0)
+,   tmp18 = vec2.fromValues(0,0)
+,   tmpArray = [];
+
+/**
+ * Narrowphase. Creates contacts and friction given shapes and transforms.
+ * @class Narrowphase
+ * @constructor
+ */
+function Narrowphase(){
+
+    /**
+     * @property contactEquations
+     * @type {Array}
+     */
+    this.contactEquations = [];
+
+    /**
+     * @property frictionEquations
+     * @type {Array}
+     */
+    this.frictionEquations = [];
+
+    /**
+     * Whether to make friction equations in the upcoming contacts.
+     * @property enableFriction
+     * @type {Boolean}
+     */
+    this.enableFriction = true;
+
+    /**
+     * Whether to make equations enabled in upcoming contacts.
+     * @property enabledEquations
+     * @type {Boolean}
+     */
+    this.enabledEquations = true;
+
+    /**
+     * The friction slip force to use when creating friction equations.
+     * @property slipForce
+     * @type {Number}
+     */
+    this.slipForce = 10.0;
+
+    /**
+     * The friction value to use in the upcoming friction equations.
+     * @property frictionCoefficient
+     * @type {Number}
+     */
+    this.frictionCoefficient = 0.3;
+
+    /**
+     * Will be the .relativeVelocity in each produced FrictionEquation.
+     * @property {Number} surfaceVelocity
+     */
+    this.surfaceVelocity = 0;
+
+    /**
+     * Keeps track of the allocated ContactEquations.
+     * @property {ContactEquationPool} contactEquationPool
+     *
+     * @example
+     *
+     *     // Allocate a few equations before starting the simulation.
+     *     // This way, no contact objects need to be created on the fly in the game loop.
+     *     world.narrowphase.contactEquationPool.resize(1024);
+     *     world.narrowphase.frictionEquationPool.resize(1024);
+     */
+    this.contactEquationPool = new ContactEquationPool({ size: 32 });
+
+    /**
+     * Keeps track of the allocated ContactEquations.
+     * @property {FrictionEquationPool} frictionEquationPool
+     */
+    this.frictionEquationPool = new FrictionEquationPool({ size: 64 });
+
+    /**
+     * The restitution value to use in the next contact equations.
+     * @property restitution
+     * @type {Number}
+     */
+    this.restitution = 0;
+
+    /**
+     * The stiffness value to use in the next contact equations.
+     * @property {Number} stiffness
+     */
+    this.stiffness = Equation.DEFAULT_STIFFNESS;
+
+    /**
+     * The stiffness value to use in the next contact equations.
+     * @property {Number} stiffness
+     */
+    this.relaxation = Equation.DEFAULT_RELAXATION;
+
+    /**
+     * The stiffness value to use in the next friction equations.
+     * @property frictionStiffness
+     * @type {Number}
+     */
+    this.frictionStiffness = Equation.DEFAULT_STIFFNESS;
+
+    /**
+     * The relaxation value to use in the next friction equations.
+     * @property frictionRelaxation
+     * @type {Number}
+     */
+    this.frictionRelaxation = Equation.DEFAULT_RELAXATION;
+
+    /**
+     * Enable reduction of friction equations. If disabled, a box on a plane will generate 2 contact equations and 2 friction equations. If enabled, there will be only one friction equation. Same kind of simplifications are made  for all collision types.
+     * @property enableFrictionReduction
+     * @type {Boolean}
+     * @deprecated This flag will be removed when the feature is stable enough.
+     * @default true
+     */
+    this.enableFrictionReduction = true;
+
+    /**
+     * Keeps track of the colliding bodies last step.
+     * @private
+     * @property collidingBodiesLastStep
+     * @type {TupleDictionary}
+     */
+    this.collidingBodiesLastStep = new TupleDictionary();
+
+    /**
+     * Contact skin size value to use in the next contact equations.
+     * @property {Number} contactSkinSize
+     * @default 0.01
+     */
+    this.contactSkinSize = 0.01;
+}
+
+var bodiesOverlap_shapePositionA = vec2.create();
+var bodiesOverlap_shapePositionB = vec2.create();
+
+/**
+ * @method bodiesOverlap
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {Boolean}
+ * @todo shape world transforms are wrong
+ */
+Narrowphase.prototype.bodiesOverlap = function(bodyA, bodyB){
+    var shapePositionA = bodiesOverlap_shapePositionA;
+    var shapePositionB = bodiesOverlap_shapePositionB;
+
+    // Loop over all shapes of bodyA
+    for(var k=0, Nshapesi=bodyA.shapes.length; k!==Nshapesi; k++){
+        var shapeA = bodyA.shapes[k];
+
+        bodyA.toWorldFrame(shapePositionA, shapeA.position);
+
+        // All shapes of body j
+        for(var l=0, Nshapesj=bodyB.shapes.length; l!==Nshapesj; l++){
+            var shapeB = bodyB.shapes[l];
+
+            bodyB.toWorldFrame(shapePositionB, shapeB.position);
+
+            if(this[shapeA.type | shapeB.type](
+                bodyA,
+                shapeA,
+                shapePositionA,
+                shapeA.angle + bodyA.angle,
+                bodyB,
+                shapeB,
+                shapePositionB,
+                shapeB.angle + bodyB.angle,
+                true
+            )){
+                return true;
+            }
+        }
+    }
+
+    return false;
+};
+
+/**
+ * Check if the bodies were in contact since the last reset().
+ * @method collidedLastStep
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {Boolean}
+ */
+Narrowphase.prototype.collidedLastStep = function(bodyA, bodyB){
+    var id1 = bodyA.id|0,
+        id2 = bodyB.id|0;
+    return !!this.collidingBodiesLastStep.get(id1, id2);
+};
+
+/**
+ * Throws away the old equations and gets ready to create new
+ * @method reset
+ */
+Narrowphase.prototype.reset = function(){
+    this.collidingBodiesLastStep.reset();
+
+    var eqs = this.contactEquations;
+    var l = eqs.length;
+    while(l--){
+        var eq = eqs[l],
+            id1 = eq.bodyA.id,
+            id2 = eq.bodyB.id;
+        this.collidingBodiesLastStep.set(id1, id2, true);
+    }
+
+    var ce = this.contactEquations,
+        fe = this.frictionEquations;
+    for(var i=0; i<ce.length; i++){
+        this.contactEquationPool.release(ce[i]);
+    }
+    for(var i=0; i<fe.length; i++){
+        this.frictionEquationPool.release(fe[i]);
+    }
+
+    // Reset
+    this.contactEquations.length = this.frictionEquations.length = 0;
+};
+
+/**
+ * Creates a ContactEquation, either by reusing an existing object or creating a new one.
+ * @method createContactEquation
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {ContactEquation}
+ */
+Narrowphase.prototype.createContactEquation = function(bodyA, bodyB, shapeA, shapeB){
+    var c = this.contactEquationPool.get();
+    c.bodyA = bodyA;
+    c.bodyB = bodyB;
+    c.shapeA = shapeA;
+    c.shapeB = shapeB;
+    c.restitution = this.restitution;
+    c.firstImpact = !this.collidedLastStep(bodyA,bodyB);
+    c.stiffness = this.stiffness;
+    c.relaxation = this.relaxation;
+    c.needsUpdate = true;
+    c.enabled = this.enabledEquations;
+    c.offset = this.contactSkinSize;
+
+    return c;
+};
+
+/**
+ * Creates a FrictionEquation, either by reusing an existing object or creating a new one.
+ * @method createFrictionEquation
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {FrictionEquation}
+ */
+Narrowphase.prototype.createFrictionEquation = function(bodyA, bodyB, shapeA, shapeB){
+    var c = this.frictionEquationPool.get();
+    c.bodyA = bodyA;
+    c.bodyB = bodyB;
+    c.shapeA = shapeA;
+    c.shapeB = shapeB;
+    c.setSlipForce(this.slipForce);
+    c.frictionCoefficient = this.frictionCoefficient;
+    c.relativeVelocity = this.surfaceVelocity;
+    c.enabled = this.enabledEquations;
+    c.needsUpdate = true;
+    c.stiffness = this.frictionStiffness;
+    c.relaxation = this.frictionRelaxation;
+    c.contactEquations.length = 0;
+    return c;
+};
+
+/**
+ * Creates a FrictionEquation given the data in the ContactEquation. Uses same offset vectors ri and rj, but the tangent vector will be constructed from the collision normal.
+ * @method createFrictionFromContact
+ * @param  {ContactEquation} contactEquation
+ * @return {FrictionEquation}
+ */
+Narrowphase.prototype.createFrictionFromContact = function(c){
+    var eq = this.createFrictionEquation(c.bodyA, c.bodyB, c.shapeA, c.shapeB);
+    vec2.copy(eq.contactPointA, c.contactPointA);
+    vec2.copy(eq.contactPointB, c.contactPointB);
+    vec2.rotate90cw(eq.t, c.normalA);
+    eq.contactEquations.push(c);
+    return eq;
+};
+
+// Take the average N latest contact point on the plane.
+Narrowphase.prototype.createFrictionFromAverage = function(numContacts){
+    var c = this.contactEquations[this.contactEquations.length - 1];
+    var eq = this.createFrictionEquation(c.bodyA, c.bodyB, c.shapeA, c.shapeB);
+    var bodyA = c.bodyA;
+    var bodyB = c.bodyB;
+    vec2.set(eq.contactPointA, 0, 0);
+    vec2.set(eq.contactPointB, 0, 0);
+    vec2.set(eq.t, 0, 0);
+    for(var i=0; i!==numContacts; i++){
+        c = this.contactEquations[this.contactEquations.length - 1 - i];
+        if(c.bodyA === bodyA){
+            vec2.add(eq.t, eq.t, c.normalA);
+            vec2.add(eq.contactPointA, eq.contactPointA, c.contactPointA);
+            vec2.add(eq.contactPointB, eq.contactPointB, c.contactPointB);
+        } else {
+            vec2.sub(eq.t, eq.t, c.normalA);
+            vec2.add(eq.contactPointA, eq.contactPointA, c.contactPointB);
+            vec2.add(eq.contactPointB, eq.contactPointB, c.contactPointA);
+        }
+        eq.contactEquations.push(c);
+    }
+
+    var invNumContacts = 1/numContacts;
+    vec2.scale(eq.contactPointA, eq.contactPointA, invNumContacts);
+    vec2.scale(eq.contactPointB, eq.contactPointB, invNumContacts);
+    vec2.normalize(eq.t, eq.t);
+    vec2.rotate90cw(eq.t, eq.t);
+    return eq;
+};
+
+/**
+ * Convex/line narrowphase
+ * @method convexLine
+ * @param  {Body}       convexBody
+ * @param  {Convex}     convexShape
+ * @param  {Array}      convexOffset
+ * @param  {Number}     convexAngle
+ * @param  {Body}       lineBody
+ * @param  {Line}       lineShape
+ * @param  {Array}      lineOffset
+ * @param  {Number}     lineAngle
+ * @param {boolean}     justTest
+ * @todo Implement me!
+ */
+Narrowphase.prototype[Shape.LINE | Shape.CONVEX] =
+Narrowphase.prototype.convexLine = function(
+    convexBody,
+    convexShape,
+    convexOffset,
+    convexAngle,
+    lineBody,
+    lineShape,
+    lineOffset,
+    lineAngle,
+    justTest
+){
+    // TODO
+    if(justTest){
+        return false;
+    } else {
+        return 0;
+    }
+};
+
+/**
+ * Line/box narrowphase
+ * @method lineBox
+ * @param  {Body}       lineBody
+ * @param  {Line}       lineShape
+ * @param  {Array}      lineOffset
+ * @param  {Number}     lineAngle
+ * @param  {Body}       boxBody
+ * @param  {Box}  boxShape
+ * @param  {Array}      boxOffset
+ * @param  {Number}     boxAngle
+ * @param  {Boolean}    justTest
+ * @todo Implement me!
+ */
+Narrowphase.prototype[Shape.LINE | Shape.BOX] =
+Narrowphase.prototype.lineBox = function(
+    lineBody,
+    lineShape,
+    lineOffset,
+    lineAngle,
+    boxBody,
+    boxShape,
+    boxOffset,
+    boxAngle,
+    justTest
+){
+    // TODO
+    if(justTest){
+        return false;
+    } else {
+        return 0;
+    }
+};
+
+function setConvexToCapsuleShapeMiddle(convexShape, capsuleShape){
+    vec2.set(convexShape.vertices[0], -capsuleShape.length * 0.5, -capsuleShape.radius);
+    vec2.set(convexShape.vertices[1],  capsuleShape.length * 0.5, -capsuleShape.radius);
+    vec2.set(convexShape.vertices[2],  capsuleShape.length * 0.5,  capsuleShape.radius);
+    vec2.set(convexShape.vertices[3], -capsuleShape.length * 0.5,  capsuleShape.radius);
+}
+
+var convexCapsule_tempRect = new Box({ width: 1, height: 1 }),
+    convexCapsule_tempVec = vec2.create();
+
+/**
+ * Convex/capsule narrowphase
+ * @method convexCapsule
+ * @param  {Body}       convexBody
+ * @param  {Convex}     convexShape
+ * @param  {Array}      convexPosition
+ * @param  {Number}     convexAngle
+ * @param  {Body}       capsuleBody
+ * @param  {Capsule}    capsuleShape
+ * @param  {Array}      capsulePosition
+ * @param  {Number}     capsuleAngle
+ */
+Narrowphase.prototype[Shape.CAPSULE | Shape.CONVEX] =
+Narrowphase.prototype[Shape.CAPSULE | Shape.BOX] =
+Narrowphase.prototype.convexCapsule = function(
+    convexBody,
+    convexShape,
+    convexPosition,
+    convexAngle,
+    capsuleBody,
+    capsuleShape,
+    capsulePosition,
+    capsuleAngle,
+    justTest
+){
+
+    // Check the circles
+    // Add offsets!
+    var circlePos = convexCapsule_tempVec;
+    vec2.set(circlePos, capsuleShape.length/2,0);
+    vec2.rotate(circlePos,circlePos,capsuleAngle);
+    vec2.add(circlePos,circlePos,capsulePosition);
+    var result1 = this.circleConvex(capsuleBody,capsuleShape,circlePos,capsuleAngle, convexBody,convexShape,convexPosition,convexAngle, justTest, capsuleShape.radius);
+
+    vec2.set(circlePos,-capsuleShape.length/2, 0);
+    vec2.rotate(circlePos,circlePos,capsuleAngle);
+    vec2.add(circlePos,circlePos,capsulePosition);
+    var result2 = this.circleConvex(capsuleBody,capsuleShape,circlePos,capsuleAngle, convexBody,convexShape,convexPosition,convexAngle, justTest, capsuleShape.radius);
+
+    if(justTest && (result1 || result2)){
+        return true;
+    }
+
+    // Check center rect
+    var r = convexCapsule_tempRect;
+    setConvexToCapsuleShapeMiddle(r,capsuleShape);
+    var result = this.convexConvex(convexBody,convexShape,convexPosition,convexAngle, capsuleBody,r,capsulePosition,capsuleAngle, justTest);
+
+    return result + result1 + result2;
+};
+
+/**
+ * Capsule/line narrowphase
+ * @method lineCapsule
+ * @param  {Body}       lineBody
+ * @param  {Line}       lineShape
+ * @param  {Array}      linePosition
+ * @param  {Number}     lineAngle
+ * @param  {Body}       capsuleBody
+ * @param  {Capsule}    capsuleShape
+ * @param  {Array}      capsulePosition
+ * @param  {Number}     capsuleAngle
+ * @todo Implement me!
+ */
+Narrowphase.prototype[Shape.CAPSULE | Shape.LINE] =
+Narrowphase.prototype.lineCapsule = function(
+    lineBody,
+    lineShape,
+    linePosition,
+    lineAngle,
+    capsuleBody,
+    capsuleShape,
+    capsulePosition,
+    capsuleAngle,
+    justTest
+){
+    // TODO
+    if(justTest){
+        return false;
+    } else {
+        return 0;
+    }
+};
+
+var capsuleCapsule_tempVec1 = vec2.create();
+var capsuleCapsule_tempVec2 = vec2.create();
+var capsuleCapsule_tempRect1 = new Box({ width: 1, height: 1 });
+
+/**
+ * Capsule/capsule narrowphase
+ * @method capsuleCapsule
+ * @param  {Body}       bi
+ * @param  {Capsule}    si
+ * @param  {Array}      xi
+ * @param  {Number}     ai
+ * @param  {Body}       bj
+ * @param  {Capsule}    sj
+ * @param  {Array}      xj
+ * @param  {Number}     aj
+ */
+Narrowphase.prototype[Shape.CAPSULE | Shape.CAPSULE] =
+Narrowphase.prototype.capsuleCapsule = function(bi,si,xi,ai, bj,sj,xj,aj, justTest){
+
+    var enableFrictionBefore;
+
+    // Check the circles
+    // Add offsets!
+    var circlePosi = capsuleCapsule_tempVec1,
+        circlePosj = capsuleCapsule_tempVec2;
+
+    var numContacts = 0;
+
+
+    // Need 4 circle checks, between all
+    for(var i=0; i<2; i++){
+
+        vec2.set(circlePosi,(i===0?-1:1)*si.length/2,0);
+        vec2.rotate(circlePosi,circlePosi,ai);
+        vec2.add(circlePosi,circlePosi,xi);
+
+        for(var j=0; j<2; j++){
+
+            vec2.set(circlePosj,(j===0?-1:1)*sj.length/2, 0);
+            vec2.rotate(circlePosj,circlePosj,aj);
+            vec2.add(circlePosj,circlePosj,xj);
+
+            // Temporarily turn off friction
+            if(this.enableFrictionReduction){
+                enableFrictionBefore = this.enableFriction;
+                this.enableFriction = false;
+            }
+
+            var result = this.circleCircle(bi,si,circlePosi,ai, bj,sj,circlePosj,aj, justTest, si.radius, sj.radius);
+
+            if(this.enableFrictionReduction){
+                this.enableFriction = enableFrictionBefore;
+            }
+
+            if(justTest && result){
+                return true;
+            }
+
+            numContacts += result;
+        }
+    }
+
+    if(this.enableFrictionReduction){
+        // Temporarily turn off friction
+        enableFrictionBefore = this.enableFriction;
+        this.enableFriction = false;
+    }
+
+    // Check circles against the center boxs
+    var rect = capsuleCapsule_tempRect1;
+    setConvexToCapsuleShapeMiddle(rect,si);
+    var result1 = this.convexCapsule(bi,rect,xi,ai, bj,sj,xj,aj, justTest);
+
+    if(this.enableFrictionReduction){
+        this.enableFriction = enableFrictionBefore;
+    }
+
+    if(justTest && result1){
+        return true;
+    }
+    numContacts += result1;
+
+    if(this.enableFrictionReduction){
+        // Temporarily turn off friction
+        var enableFrictionBefore = this.enableFriction;
+        this.enableFriction = false;
+    }
+
+    setConvexToCapsuleShapeMiddle(rect,sj);
+    var result2 = this.convexCapsule(bj,rect,xj,aj, bi,si,xi,ai, justTest);
+
+    if(this.enableFrictionReduction){
+        this.enableFriction = enableFrictionBefore;
+    }
+
+    if(justTest && result2){
+        return true;
+    }
+    numContacts += result2;
+
+    if(this.enableFrictionReduction){
+        if(numContacts && this.enableFriction){
+            this.frictionEquations.push(this.createFrictionFromAverage(numContacts));
+        }
+    }
+
+    return numContacts;
+};
+
+/**
+ * Line/line narrowphase
+ * @method lineLine
+ * @param  {Body}       bodyA
+ * @param  {Line}       shapeA
+ * @param  {Array}      positionA
+ * @param  {Number}     angleA
+ * @param  {Body}       bodyB
+ * @param  {Line}       shapeB
+ * @param  {Array}      positionB
+ * @param  {Number}     angleB
+ * @todo Implement me!
+ */
+Narrowphase.prototype[Shape.LINE | Shape.LINE] =
+Narrowphase.prototype.lineLine = function(
+    bodyA,
+    shapeA,
+    positionA,
+    angleA,
+    bodyB,
+    shapeB,
+    positionB,
+    angleB,
+    justTest
+){
+    // TODO
+    if(justTest){
+        return false;
+    } else {
+        return 0;
+    }
+};
+
+/**
+ * Plane/line Narrowphase
+ * @method planeLine
+ * @param  {Body}   planeBody
+ * @param  {Plane}  planeShape
+ * @param  {Array}  planeOffset
+ * @param  {Number} planeAngle
+ * @param  {Body}   lineBody
+ * @param  {Line}   lineShape
+ * @param  {Array}  lineOffset
+ * @param  {Number} lineAngle
+ */
+Narrowphase.prototype[Shape.PLANE | Shape.LINE] =
+Narrowphase.prototype.planeLine = function(planeBody, planeShape, planeOffset, planeAngle,
+                                           lineBody,  lineShape,  lineOffset,  lineAngle, justTest){
+    var worldVertex0 = tmp1,
+        worldVertex1 = tmp2,
+        worldVertex01 = tmp3,
+        worldVertex11 = tmp4,
+        worldEdge = tmp5,
+        worldEdgeUnit = tmp6,
+        dist = tmp7,
+        worldNormal = tmp8,
+        worldTangent = tmp9,
+        verts = tmpArray,
+        numContacts = 0;
+
+    // Get start and end points
+    vec2.set(worldVertex0, -lineShape.length/2, 0);
+    vec2.set(worldVertex1,  lineShape.length/2, 0);
+
+    // Not sure why we have to use worldVertex*1 here, but it won't work otherwise. Tired.
+    vec2.rotate(worldVertex01, worldVertex0, lineAngle);
+    vec2.rotate(worldVertex11, worldVertex1, lineAngle);
+
+    add(worldVertex01, worldVertex01, lineOffset);
+    add(worldVertex11, worldVertex11, lineOffset);
+
+    vec2.copy(worldVertex0,worldVertex01);
+    vec2.copy(worldVertex1,worldVertex11);
+
+    // Get vector along the line
+    sub(worldEdge, worldVertex1, worldVertex0);
+    vec2.normalize(worldEdgeUnit, worldEdge);
+
+    // Get tangent to the edge.
+    vec2.rotate90cw(worldTangent, worldEdgeUnit);
+
+    vec2.rotate(worldNormal, yAxis, planeAngle);
+
+    // Check line ends
+    verts[0] = worldVertex0;
+    verts[1] = worldVertex1;
+    for(var i=0; i<verts.length; i++){
+        var v = verts[i];
+
+        sub(dist, v, planeOffset);
+
+        var d = dot(dist,worldNormal);
+
+        if(d < 0){
+
+            if(justTest){
+                return true;
+            }
+
+            var c = this.createContactEquation(planeBody,lineBody,planeShape,lineShape);
+            numContacts++;
+
+            vec2.copy(c.normalA, worldNormal);
+            vec2.normalize(c.normalA,c.normalA);
+
+            // distance vector along plane normal
+            vec2.scale(dist, worldNormal, d);
+
+            // Vector from plane center to contact
+            sub(c.contactPointA, v, dist);
+            sub(c.contactPointA, c.contactPointA, planeBody.position);
+
+            // From line center to contact
+            sub(c.contactPointB, v,    lineOffset);
+            add(c.contactPointB, c.contactPointB, lineOffset);
+            sub(c.contactPointB, c.contactPointB, lineBody.position);
+
+            this.contactEquations.push(c);
+
+            if(!this.enableFrictionReduction){
+                if(this.enableFriction){
+                    this.frictionEquations.push(this.createFrictionFromContact(c));
+                }
+            }
+        }
+    }
+
+    if(justTest){
+        return false;
+    }
+
+    if(!this.enableFrictionReduction){
+        if(numContacts && this.enableFriction){
+            this.frictionEquations.push(this.createFrictionFromAverage(numContacts));
+        }
+    }
+
+    return numContacts;
+};
+
+Narrowphase.prototype[Shape.PARTICLE | Shape.CAPSULE] =
+Narrowphase.prototype.particleCapsule = function(
+    particleBody,
+    particleShape,
+    particlePosition,
+    particleAngle,
+    capsuleBody,
+    capsuleShape,
+    capsulePosition,
+    capsuleAngle,
+    justTest
+){
+    return this.circleLine(particleBody,particleShape,particlePosition,particleAngle, capsuleBody,capsuleShape,capsulePosition,capsuleAngle, justTest, capsuleShape.radius, 0);
+};
+
+/**
+ * Circle/line Narrowphase
+ * @method circleLine
+ * @param  {Body} circleBody
+ * @param  {Circle} circleShape
+ * @param  {Array} circleOffset
+ * @param  {Number} circleAngle
+ * @param  {Body} lineBody
+ * @param  {Line} lineShape
+ * @param  {Array} lineOffset
+ * @param  {Number} lineAngle
+ * @param {Boolean} justTest If set to true, this function will return the result (intersection or not) without adding equations.
+ * @param {Number} lineRadius Radius to add to the line. Can be used to test Capsules.
+ * @param {Number} circleRadius If set, this value overrides the circle shape radius.
+ */
+Narrowphase.prototype[Shape.CIRCLE | Shape.LINE] =
+Narrowphase.prototype.circleLine = function(
+    circleBody,
+    circleShape,
+    circleOffset,
+    circleAngle,
+    lineBody,
+    lineShape,
+    lineOffset,
+    lineAngle,
+    justTest,
+    lineRadius,
+    circleRadius
+){
+    var lineRadius = lineRadius || 0,
+        circleRadius = typeof(circleRadius)!=="undefined" ? circleRadius : circleShape.radius,
+
+        orthoDist = tmp1,
+        lineToCircleOrthoUnit = tmp2,
+        projectedPoint = tmp3,
+        centerDist = tmp4,
+        worldTangent = tmp5,
+        worldEdge = tmp6,
+        worldEdgeUnit = tmp7,
+        worldVertex0 = tmp8,
+        worldVertex1 = tmp9,
+        worldVertex01 = tmp10,
+        worldVertex11 = tmp11,
+        dist = tmp12,
+        lineToCircle = tmp13,
+        lineEndToLineRadius = tmp14,
+
+        verts = tmpArray;
+
+    // Get start and end points
+    vec2.set(worldVertex0, -lineShape.length/2, 0);
+    vec2.set(worldVertex1,  lineShape.length/2, 0);
+
+    // Not sure why we have to use worldVertex*1 here, but it won't work otherwise. Tired.
+    vec2.rotate(worldVertex01, worldVertex0, lineAngle);
+    vec2.rotate(worldVertex11, worldVertex1, lineAngle);
+
+    add(worldVertex01, worldVertex01, lineOffset);
+    add(worldVertex11, worldVertex11, lineOffset);
+
+    vec2.copy(worldVertex0,worldVertex01);
+    vec2.copy(worldVertex1,worldVertex11);
+
+    // Get vector along the line
+    sub(worldEdge, worldVertex1, worldVertex0);
+    vec2.normalize(worldEdgeUnit, worldEdge);
+
+    // Get tangent to the edge.
+    vec2.rotate90cw(worldTangent, worldEdgeUnit);
+
+    // Check distance from the plane spanned by the edge vs the circle
+    sub(dist, circleOffset, worldVertex0);
+    var d = dot(dist, worldTangent); // Distance from center of line to circle center
+    sub(centerDist, worldVertex0, lineOffset);
+
+    sub(lineToCircle, circleOffset, lineOffset);
+
+    var radiusSum = circleRadius + lineRadius;
+
+    if(Math.abs(d) < radiusSum){
+
+        // Now project the circle onto the edge
+        vec2.scale(orthoDist, worldTangent, d);
+        sub(projectedPoint, circleOffset, orthoDist);
+
+        // Add the missing line radius
+        vec2.scale(lineToCircleOrthoUnit, worldTangent, dot(worldTangent, lineToCircle));
+        vec2.normalize(lineToCircleOrthoUnit,lineToCircleOrthoUnit);
+        vec2.scale(lineToCircleOrthoUnit, lineToCircleOrthoUnit, lineRadius);
+        add(projectedPoint,projectedPoint,lineToCircleOrthoUnit);
+
+        // Check if the point is within the edge span
+        var pos =  dot(worldEdgeUnit, projectedPoint);
+        var pos0 = dot(worldEdgeUnit, worldVertex0);
+        var pos1 = dot(worldEdgeUnit, worldVertex1);
+
+        if(pos > pos0 && pos < pos1){
+            // We got contact!
+
+            if(justTest){
+                return true;
+            }
+
+            var c = this.createContactEquation(circleBody,lineBody,circleShape,lineShape);
+
+            vec2.scale(c.normalA, orthoDist, -1);
+            vec2.normalize(c.normalA, c.normalA);
+
+            vec2.scale( c.contactPointA, c.normalA,  circleRadius);
+            add(c.contactPointA, c.contactPointA, circleOffset);
+            sub(c.contactPointA, c.contactPointA, circleBody.position);
+
+            sub(c.contactPointB, projectedPoint, lineOffset);
+            add(c.contactPointB, c.contactPointB, lineOffset);
+            sub(c.contactPointB, c.contactPointB, lineBody.position);
+
+            this.contactEquations.push(c);
+
+            if(this.enableFriction){
+                this.frictionEquations.push(this.createFrictionFromContact(c));
+            }
+
+            return 1;
+        }
+    }
+
+    // Add corner
+    verts[0] = worldVertex0;
+    verts[1] = worldVertex1;
+
+    for(var i=0; i<verts.length; i++){
+        var v = verts[i];
+
+        sub(dist, v, circleOffset);
+
+        if(vec2.squaredLength(dist) < Math.pow(radiusSum, 2)){
+
+            if(justTest){
+                return true;
+            }
+
+            var c = this.createContactEquation(circleBody,lineBody,circleShape,lineShape);
+
+            vec2.copy(c.normalA, dist);
+            vec2.normalize(c.normalA,c.normalA);
+
+            // Vector from circle to contact point is the normal times the circle radius
+            vec2.scale(c.contactPointA, c.normalA, circleRadius);
+            add(c.contactPointA, c.contactPointA, circleOffset);
+            sub(c.contactPointA, c.contactPointA, circleBody.position);
+
+            sub(c.contactPointB, v, lineOffset);
+            vec2.scale(lineEndToLineRadius, c.normalA, -lineRadius);
+            add(c.contactPointB, c.contactPointB, lineEndToLineRadius);
+            add(c.contactPointB, c.contactPointB, lineOffset);
+            sub(c.contactPointB, c.contactPointB, lineBody.position);
+
+            this.contactEquations.push(c);
+
+            if(this.enableFriction){
+                this.frictionEquations.push(this.createFrictionFromContact(c));
+            }
+
+            return 1;
+        }
+    }
+
+    return 0;
+};
+
+/**
+ * Circle/capsule Narrowphase
+ * @method circleCapsule
+ * @param  {Body}   bi
+ * @param  {Circle} si
+ * @param  {Array}  xi
+ * @param  {Number} ai
+ * @param  {Body}   bj
+ * @param  {Line}   sj
+ * @param  {Array}  xj
+ * @param  {Number} aj
+ */
+Narrowphase.prototype[Shape.CIRCLE | Shape.CAPSULE] =
+Narrowphase.prototype.circleCapsule = function(bi,si,xi,ai, bj,sj,xj,aj, justTest){
+    return this.circleLine(bi,si,xi,ai, bj,sj,xj,aj, justTest, sj.radius);
+};
+
+/**
+ * Circle/convex Narrowphase.
+ * @method circleConvex
+ * @param  {Body} circleBody
+ * @param  {Circle} circleShape
+ * @param  {Array} circleOffset
+ * @param  {Number} circleAngle
+ * @param  {Body} convexBody
+ * @param  {Convex} convexShape
+ * @param  {Array} convexOffset
+ * @param  {Number} convexAngle
+ * @param  {Boolean} justTest
+ * @param  {Number} circleRadius
+ */
+Narrowphase.prototype[Shape.CIRCLE | Shape.CONVEX] =
+Narrowphase.prototype[Shape.CIRCLE | Shape.BOX] =
+Narrowphase.prototype.circleConvex = function(
+    circleBody,
+    circleShape,
+    circleOffset,
+    circleAngle,
+    convexBody,
+    convexShape,
+    convexOffset,
+    convexAngle,
+    justTest,
+    circleRadius
+){
+    var circleRadius = typeof(circleRadius)==="number" ? circleRadius : circleShape.radius;
+
+    var worldVertex0 = tmp1,
+        worldVertex1 = tmp2,
+        worldEdge = tmp3,
+        worldEdgeUnit = tmp4,
+        worldNormal = tmp5,
+        centerDist = tmp6,
+        convexToCircle = tmp7,
+        orthoDist = tmp8,
+        projectedPoint = tmp9,
+        dist = tmp10,
+        worldVertex = tmp11,
+
+        closestEdge = -1,
+        closestEdgeDistance = null,
+        closestEdgeOrthoDist = tmp12,
+        closestEdgeProjectedPoint = tmp13,
+        candidate = tmp14,
+        candidateDist = tmp15,
+        minCandidate = tmp16,
+
+        found = false,
+        minCandidateDistance = Number.MAX_VALUE;
+
+    var numReported = 0;
+
+    // New algorithm:
+    // 1. Check so center of circle is not inside the polygon. If it is, this wont work...
+    // 2. For each edge
+    // 2. 1. Get point on circle that is closest to the edge (scale normal with -radius)
+    // 2. 2. Check if point is inside.
+
+    var verts = convexShape.vertices;
+
+    // Check all edges first
+    for(var i=0; i!==verts.length+1; i++){
+        var v0 = verts[i%verts.length],
+            v1 = verts[(i+1)%verts.length];
+
+        vec2.rotate(worldVertex0, v0, convexAngle);
+        vec2.rotate(worldVertex1, v1, convexAngle);
+        add(worldVertex0, worldVertex0, convexOffset);
+        add(worldVertex1, worldVertex1, convexOffset);
+        sub(worldEdge, worldVertex1, worldVertex0);
+
+        vec2.normalize(worldEdgeUnit, worldEdge);
+
+        // Get tangent to the edge. Points out of the Convex
+        vec2.rotate90cw(worldNormal, worldEdgeUnit);
+
+        // Get point on circle, closest to the polygon
+        vec2.scale(candidate,worldNormal,-circleShape.radius);
+        add(candidate,candidate,circleOffset);
+
+        if(pointInConvex(candidate,convexShape,convexOffset,convexAngle)){
+
+            vec2.sub(candidateDist,worldVertex0,candidate);
+            var candidateDistance = Math.abs(vec2.dot(candidateDist,worldNormal));
+
+            if(candidateDistance < minCandidateDistance){
+                vec2.copy(minCandidate,candidate);
+                minCandidateDistance = candidateDistance;
+                vec2.scale(closestEdgeProjectedPoint,worldNormal,candidateDistance);
+                vec2.add(closestEdgeProjectedPoint,closestEdgeProjectedPoint,candidate);
+                found = true;
+            }
+        }
+    }
+
+    if(found){
+
+        if(justTest){
+            return true;
+        }
+
+        var c = this.createContactEquation(circleBody,convexBody,circleShape,convexShape);
+        vec2.sub(c.normalA, minCandidate, circleOffset);
+        vec2.normalize(c.normalA, c.normalA);
+
+        vec2.scale(c.contactPointA,  c.normalA, circleRadius);
+        add(c.contactPointA, c.contactPointA, circleOffset);
+        sub(c.contactPointA, c.contactPointA, circleBody.position);
+
+        sub(c.contactPointB, closestEdgeProjectedPoint, convexOffset);
+        add(c.contactPointB, c.contactPointB, convexOffset);
+        sub(c.contactPointB, c.contactPointB, convexBody.position);
+
+        this.contactEquations.push(c);
+
+        if(this.enableFriction){
+            this.frictionEquations.push( this.createFrictionFromContact(c) );
+        }
+
+        return 1;
+    }
+
+    // Check all vertices
+    if(circleRadius > 0){
+        for(var i=0; i<verts.length; i++){
+            var localVertex = verts[i];
+            vec2.rotate(worldVertex, localVertex, convexAngle);
+            add(worldVertex, worldVertex, convexOffset);
+
+            sub(dist, worldVertex, circleOffset);
+            if(vec2.squaredLength(dist) < Math.pow(circleRadius, 2)){
+
+                if(justTest){
+                    return true;
+                }
+
+                var c = this.createContactEquation(circleBody,convexBody,circleShape,convexShape);
+
+                vec2.copy(c.normalA, dist);
+                vec2.normalize(c.normalA,c.normalA);
+
+                // Vector from circle to contact point is the normal times the circle radius
+                vec2.scale(c.contactPointA, c.normalA, circleRadius);
+                add(c.contactPointA, c.contactPointA, circleOffset);
+                sub(c.contactPointA, c.contactPointA, circleBody.position);
+
+                sub(c.contactPointB, worldVertex, convexOffset);
+                add(c.contactPointB, c.contactPointB, convexOffset);
+                sub(c.contactPointB, c.contactPointB, convexBody.position);
+
+                this.contactEquations.push(c);
+
+                if(this.enableFriction){
+                    this.frictionEquations.push(this.createFrictionFromContact(c));
+                }
+
+                return 1;
+            }
+        }
+    }
+
+    return 0;
+};
+
+var pic_worldVertex0 = vec2.create(),
+    pic_worldVertex1 = vec2.create(),
+    pic_r0 = vec2.create(),
+    pic_r1 = vec2.create();
+
+/*
+ * Check if a point is in a polygon
+ */
+function pointInConvex(worldPoint,convexShape,convexOffset,convexAngle){
+    var worldVertex0 = pic_worldVertex0,
+        worldVertex1 = pic_worldVertex1,
+        r0 = pic_r0,
+        r1 = pic_r1,
+        point = worldPoint,
+        verts = convexShape.vertices,
+        lastCross = null;
+    for(var i=0; i!==verts.length+1; i++){
+        var v0 = verts[i%verts.length],
+            v1 = verts[(i+1)%verts.length];
+
+        // Transform vertices to world
+        // @todo The point should be transformed to local coordinates in the convex, no need to transform each vertex
+        vec2.rotate(worldVertex0, v0, convexAngle);
+        vec2.rotate(worldVertex1, v1, convexAngle);
+        add(worldVertex0, worldVertex0, convexOffset);
+        add(worldVertex1, worldVertex1, convexOffset);
+
+        sub(r0, worldVertex0, point);
+        sub(r1, worldVertex1, point);
+        var cross = vec2.crossLength(r0,r1);
+
+        if(lastCross===null){
+            lastCross = cross;
+        }
+
+        // If we got a different sign of the distance vector, the point is out of the polygon
+        if(cross*lastCross <= 0){
+            return false;
+        }
+        lastCross = cross;
+    }
+    return true;
+}
+
+/**
+ * Particle/convex Narrowphase
+ * @method particleConvex
+ * @param  {Body} particleBody
+ * @param  {Particle} particleShape
+ * @param  {Array} particleOffset
+ * @param  {Number} particleAngle
+ * @param  {Body} convexBody
+ * @param  {Convex} convexShape
+ * @param  {Array} convexOffset
+ * @param  {Number} convexAngle
+ * @param {Boolean} justTest
+ * @todo use pointInConvex and code more similar to circleConvex
+ * @todo don't transform each vertex, but transform the particle position to convex-local instead
+ */
+Narrowphase.prototype[Shape.PARTICLE | Shape.CONVEX] =
+Narrowphase.prototype[Shape.PARTICLE | Shape.BOX] =
+Narrowphase.prototype.particleConvex = function(
+    particleBody,
+    particleShape,
+    particleOffset,
+    particleAngle,
+    convexBody,
+    convexShape,
+    convexOffset,
+    convexAngle,
+    justTest
+){
+    var worldVertex0 = tmp1,
+        worldVertex1 = tmp2,
+        worldEdge = tmp3,
+        worldEdgeUnit = tmp4,
+        worldTangent = tmp5,
+        centerDist = tmp6,
+        convexToparticle = tmp7,
+        orthoDist = tmp8,
+        projectedPoint = tmp9,
+        dist = tmp10,
+        worldVertex = tmp11,
+        closestEdge = -1,
+        closestEdgeDistance = null,
+        closestEdgeOrthoDist = tmp12,
+        closestEdgeProjectedPoint = tmp13,
+        r0 = tmp14, // vector from particle to vertex0
+        r1 = tmp15,
+        localPoint = tmp16,
+        candidateDist = tmp17,
+        minEdgeNormal = tmp18,
+        minCandidateDistance = Number.MAX_VALUE;
+
+    var numReported = 0,
+        found = false,
+        verts = convexShape.vertices;
+
+    // Check if the particle is in the polygon at all
+    if(!pointInConvex(particleOffset,convexShape,convexOffset,convexAngle)){
+        return 0;
+    }
+
+    if(justTest){
+        return true;
+    }
+
+    // Check edges first
+    var lastCross = null;
+    for(var i=0; i!==verts.length+1; i++){
+        var v0 = verts[i%verts.length],
+            v1 = verts[(i+1)%verts.length];
+
+        // Transform vertices to world
+        vec2.rotate(worldVertex0, v0, convexAngle);
+        vec2.rotate(worldVertex1, v1, convexAngle);
+        add(worldVertex0, worldVertex0, convexOffset);
+        add(worldVertex1, worldVertex1, convexOffset);
+
+        // Get world edge
+        sub(worldEdge, worldVertex1, worldVertex0);
+        vec2.normalize(worldEdgeUnit, worldEdge);
+
+        // Get tangent to the edge. Points out of the Convex
+        vec2.rotate90cw(worldTangent, worldEdgeUnit);
+
+        // Check distance from the infinite line (spanned by the edge) to the particle
+        sub(dist, particleOffset, worldVertex0);
+        var d = dot(dist, worldTangent);
+        sub(centerDist, worldVertex0, convexOffset);
+
+        sub(convexToparticle, particleOffset, convexOffset);
+
+        vec2.sub(candidateDist,worldVertex0,particleOffset);
+        var candidateDistance = Math.abs(vec2.dot(candidateDist,worldTangent));
+
+        if(candidateDistance < minCandidateDistance){
+            minCandidateDistance = candidateDistance;
+            vec2.scale(closestEdgeProjectedPoint,worldTangent,candidateDistance);
+            vec2.add(closestEdgeProjectedPoint,closestEdgeProjectedPoint,particleOffset);
+            vec2.copy(minEdgeNormal,worldTangent);
+            found = true;
+        }
+    }
+
+    if(found){
+        var c = this.createContactEquation(particleBody,convexBody,particleShape,convexShape);
+
+        vec2.scale(c.normalA, minEdgeNormal, -1);
+        vec2.normalize(c.normalA, c.normalA);
+
+        // Particle has no extent to the contact point
+        vec2.set(c.contactPointA,  0, 0);
+        add(c.contactPointA, c.contactPointA, particleOffset);
+        sub(c.contactPointA, c.contactPointA, particleBody.position);
+
+        // From convex center to point
+        sub(c.contactPointB, closestEdgeProjectedPoint, convexOffset);
+        add(c.contactPointB, c.contactPointB, convexOffset);
+        sub(c.contactPointB, c.contactPointB, convexBody.position);
+
+        this.contactEquations.push(c);
+
+        if(this.enableFriction){
+            this.frictionEquations.push( this.createFrictionFromContact(c) );
+        }
+
+        return 1;
+    }
+
+
+    return 0;
+};
+
+/**
+ * Circle/circle Narrowphase
+ * @method circleCircle
+ * @param  {Body} bodyA
+ * @param  {Circle} shapeA
+ * @param  {Array} offsetA
+ * @param  {Number} angleA
+ * @param  {Body} bodyB
+ * @param  {Circle} shapeB
+ * @param  {Array} offsetB
+ * @param  {Number} angleB
+ * @param {Boolean} justTest
+ * @param {Number} [radiusA] Optional radius to use for shapeA
+ * @param {Number} [radiusB] Optional radius to use for shapeB
+ */
+Narrowphase.prototype[Shape.CIRCLE] =
+Narrowphase.prototype.circleCircle = function(
+    bodyA,
+    shapeA,
+    offsetA,
+    angleA,
+    bodyB,
+    shapeB,
+    offsetB,
+    angleB,
+    justTest,
+    radiusA,
+    radiusB
+){
+
+    var dist = tmp1,
+        radiusA = radiusA || shapeA.radius,
+        radiusB = radiusB || shapeB.radius;
+
+    sub(dist,offsetA,offsetB);
+    var r = radiusA + radiusB;
+    if(vec2.squaredLength(dist) > Math.pow(r,2)){
+        return 0;
+    }
+
+    if(justTest){
+        return true;
+    }
+
+    var c = this.createContactEquation(bodyA,bodyB,shapeA,shapeB);
+    sub(c.normalA, offsetB, offsetA);
+    vec2.normalize(c.normalA,c.normalA);
+
+    vec2.scale( c.contactPointA, c.normalA,  radiusA);
+    vec2.scale( c.contactPointB, c.normalA, -radiusB);
+
+    add(c.contactPointA, c.contactPointA, offsetA);
+    sub(c.contactPointA, c.contactPointA, bodyA.position);
+
+    add(c.contactPointB, c.contactPointB, offsetB);
+    sub(c.contactPointB, c.contactPointB, bodyB.position);
+
+    this.contactEquations.push(c);
+
+    if(this.enableFriction){
+        this.frictionEquations.push(this.createFrictionFromContact(c));
+    }
+    return 1;
+};
+
+/**
+ * Plane/Convex Narrowphase
+ * @method planeConvex
+ * @param  {Body} planeBody
+ * @param  {Plane} planeShape
+ * @param  {Array} planeOffset
+ * @param  {Number} planeAngle
+ * @param  {Body} convexBody
+ * @param  {Convex} convexShape
+ * @param  {Array} convexOffset
+ * @param  {Number} convexAngle
+ * @param {Boolean} justTest
+ */
+Narrowphase.prototype[Shape.PLANE | Shape.CONVEX] =
+Narrowphase.prototype[Shape.PLANE | Shape.BOX] =
+Narrowphase.prototype.planeConvex = function(
+    planeBody,
+    planeShape,
+    planeOffset,
+    planeAngle,
+    convexBody,
+    convexShape,
+    convexOffset,
+    convexAngle,
+    justTest
+){
+    var worldVertex = tmp1,
+        worldNormal = tmp2,
+        dist = tmp3;
+
+    var numReported = 0;
+    vec2.rotate(worldNormal, yAxis, planeAngle);
+
+    for(var i=0; i!==convexShape.vertices.length; i++){
+        var v = convexShape.vertices[i];
+        vec2.rotate(worldVertex, v, convexAngle);
+        add(worldVertex, worldVertex, convexOffset);
+
+        sub(dist, worldVertex, planeOffset);
+
+        if(dot(dist,worldNormal) <= 0){
+
+            if(justTest){
+                return true;
+            }
+
+            // Found vertex
+            numReported++;
+
+            var c = this.createContactEquation(planeBody,convexBody,planeShape,convexShape);
+
+            sub(dist, worldVertex, planeOffset);
+
+            vec2.copy(c.normalA, worldNormal);
+
+            var d = dot(dist, c.normalA);
+            vec2.scale(dist, c.normalA, d);
+
+            // rj is from convex center to contact
+            sub(c.contactPointB, worldVertex, convexBody.position);
+
+
+            // ri is from plane center to contact
+            sub( c.contactPointA, worldVertex, dist);
+            sub( c.contactPointA, c.contactPointA, planeBody.position);
+
+            this.contactEquations.push(c);
+
+            if(!this.enableFrictionReduction){
+                if(this.enableFriction){
+                    this.frictionEquations.push(this.createFrictionFromContact(c));
+                }
+            }
+        }
+    }
+
+    if(this.enableFrictionReduction){
+        if(this.enableFriction && numReported){
+            this.frictionEquations.push(this.createFrictionFromAverage(numReported));
+        }
+    }
+
+    return numReported;
+};
+
+/**
+ * Narrowphase for particle vs plane
+ * @method particlePlane
+ * @param  {Body}       particleBody
+ * @param  {Particle}   particleShape
+ * @param  {Array}      particleOffset
+ * @param  {Number}     particleAngle
+ * @param  {Body}       planeBody
+ * @param  {Plane}      planeShape
+ * @param  {Array}      planeOffset
+ * @param  {Number}     planeAngle
+ * @param {Boolean}     justTest
+ */
+Narrowphase.prototype[Shape.PARTICLE | Shape.PLANE] =
+Narrowphase.prototype.particlePlane = function(
+    particleBody,
+    particleShape,
+    particleOffset,
+    particleAngle,
+    planeBody,
+    planeShape,
+    planeOffset,
+    planeAngle,
+    justTest
+){
+    var dist = tmp1,
+        worldNormal = tmp2;
+
+    planeAngle = planeAngle || 0;
+
+    sub(dist, particleOffset, planeOffset);
+    vec2.rotate(worldNormal, yAxis, planeAngle);
+
+    var d = dot(dist, worldNormal);
+
+    if(d > 0){
+        return 0;
+    }
+    if(justTest){
+        return true;
+    }
+
+    var c = this.createContactEquation(planeBody,particleBody,planeShape,particleShape);
+
+    vec2.copy(c.normalA, worldNormal);
+    vec2.scale( dist, c.normalA, d );
+    // dist is now the distance vector in the normal direction
+
+    // ri is the particle position projected down onto the plane, from the plane center
+    sub( c.contactPointA, particleOffset, dist);
+    sub( c.contactPointA, c.contactPointA, planeBody.position);
+
+    // rj is from the body center to the particle center
+    sub( c.contactPointB, particleOffset, particleBody.position );
+
+    this.contactEquations.push(c);
+
+    if(this.enableFriction){
+        this.frictionEquations.push(this.createFrictionFromContact(c));
+    }
+    return 1;
+};
+
+/**
+ * Circle/Particle Narrowphase
+ * @method circleParticle
+ * @param  {Body} circleBody
+ * @param  {Circle} circleShape
+ * @param  {Array} circleOffset
+ * @param  {Number} circleAngle
+ * @param  {Body} particleBody
+ * @param  {Particle} particleShape
+ * @param  {Array} particleOffset
+ * @param  {Number} particleAngle
+ * @param  {Boolean} justTest
+ */
+Narrowphase.prototype[Shape.CIRCLE | Shape.PARTICLE] =
+Narrowphase.prototype.circleParticle = function(
+    circleBody,
+    circleShape,
+    circleOffset,
+    circleAngle,
+    particleBody,
+    particleShape,
+    particleOffset,
+    particleAngle,
+    justTest
+){
+    var dist = tmp1;
+
+    sub(dist, particleOffset, circleOffset);
+    if(vec2.squaredLength(dist) > Math.pow(circleShape.radius, 2)){
+        return 0;
+    }
+    if(justTest){
+        return true;
+    }
+
+    var c = this.createContactEquation(circleBody,particleBody,circleShape,particleShape);
+    vec2.copy(c.normalA, dist);
+    vec2.normalize(c.normalA,c.normalA);
+
+    // Vector from circle to contact point is the normal times the circle radius
+    vec2.scale(c.contactPointA, c.normalA, circleShape.radius);
+    add(c.contactPointA, c.contactPointA, circleOffset);
+    sub(c.contactPointA, c.contactPointA, circleBody.position);
+
+    // Vector from particle center to contact point is zero
+    sub(c.contactPointB, particleOffset, particleBody.position);
+
+    this.contactEquations.push(c);
+
+    if(this.enableFriction){
+        this.frictionEquations.push(this.createFrictionFromContact(c));
+    }
+
+    return 1;
+};
+
+var planeCapsule_tmpCircle = new Circle({ radius: 1 }),
+    planeCapsule_tmp1 = vec2.create(),
+    planeCapsule_tmp2 = vec2.create(),
+    planeCapsule_tmp3 = vec2.create();
+
+/**
+ * @method planeCapsule
+ * @param  {Body} planeBody
+ * @param  {Circle} planeShape
+ * @param  {Array} planeOffset
+ * @param  {Number} planeAngle
+ * @param  {Body} capsuleBody
+ * @param  {Particle} capsuleShape
+ * @param  {Array} capsuleOffset
+ * @param  {Number} capsuleAngle
+ * @param {Boolean} justTest
+ */
+Narrowphase.prototype[Shape.PLANE | Shape.CAPSULE] =
+Narrowphase.prototype.planeCapsule = function(
+    planeBody,
+    planeShape,
+    planeOffset,
+    planeAngle,
+    capsuleBody,
+    capsuleShape,
+    capsuleOffset,
+    capsuleAngle,
+    justTest
+){
+    var end1 = planeCapsule_tmp1,
+        end2 = planeCapsule_tmp2,
+        circle = planeCapsule_tmpCircle,
+        dst = planeCapsule_tmp3;
+
+    // Compute world end positions
+    vec2.set(end1, -capsuleShape.length/2, 0);
+    vec2.rotate(end1,end1,capsuleAngle);
+    add(end1,end1,capsuleOffset);
+
+    vec2.set(end2,  capsuleShape.length/2, 0);
+    vec2.rotate(end2,end2,capsuleAngle);
+    add(end2,end2,capsuleOffset);
+
+    circle.radius = capsuleShape.radius;
+
+    var enableFrictionBefore;
+
+    // Temporarily turn off friction
+    if(this.enableFrictionReduction){
+        enableFrictionBefore = this.enableFriction;
+        this.enableFriction = false;
+    }
+
+    // Do Narrowphase as two circles
+    var numContacts1 = this.circlePlane(capsuleBody,circle,end1,0, planeBody,planeShape,planeOffset,planeAngle, justTest),
+        numContacts2 = this.circlePlane(capsuleBody,circle,end2,0, planeBody,planeShape,planeOffset,planeAngle, justTest);
+
+    // Restore friction
+    if(this.enableFrictionReduction){
+        this.enableFriction = enableFrictionBefore;
+    }
+
+    if(justTest){
+        return numContacts1 || numContacts2;
+    } else {
+        var numTotal = numContacts1 + numContacts2;
+        if(this.enableFrictionReduction){
+            if(numTotal){
+                this.frictionEquations.push(this.createFrictionFromAverage(numTotal));
+            }
+        }
+        return numTotal;
+    }
+};
+
+/**
+ * Creates ContactEquations and FrictionEquations for a collision.
+ * @method circlePlane
+ * @param  {Body}    bi     The first body that should be connected to the equations.
+ * @param  {Circle}  si     The circle shape participating in the collision.
+ * @param  {Array}   xi     Extra offset to take into account for the Shape, in addition to the one in circleBody.position. Will *not* be rotated by circleBody.angle (maybe it should, for sake of homogenity?). Set to null if none.
+ * @param  {Body}    bj     The second body that should be connected to the equations.
+ * @param  {Plane}   sj     The Plane shape that is participating
+ * @param  {Array}   xj     Extra offset for the plane shape.
+ * @param  {Number}  aj     Extra angle to apply to the plane
+ */
+Narrowphase.prototype[Shape.CIRCLE | Shape.PLANE] =
+Narrowphase.prototype.circlePlane = function(   bi,si,xi,ai, bj,sj,xj,aj, justTest ){
+    var circleBody = bi,
+        circleShape = si,
+        circleOffset = xi, // Offset from body center, rotated!
+        planeBody = bj,
+        shapeB = sj,
+        planeOffset = xj,
+        planeAngle = aj;
+
+    planeAngle = planeAngle || 0;
+
+    // Vector from plane to circle
+    var planeToCircle = tmp1,
+        worldNormal = tmp2,
+        temp = tmp3;
+
+    sub(planeToCircle, circleOffset, planeOffset);
+
+    // World plane normal
+    vec2.rotate(worldNormal, yAxis, planeAngle);
+
+    // Normal direction distance
+    var d = dot(worldNormal, planeToCircle);
+
+    if(d > circleShape.radius){
+        return 0; // No overlap. Abort.
+    }
+
+    if(justTest){
+        return true;
+    }
+
+    // Create contact
+    var contact = this.createContactEquation(planeBody,circleBody,sj,si);
+
+    // ni is the plane world normal
+    vec2.copy(contact.normalA, worldNormal);
+
+    // rj is the vector from circle center to the contact point
+    vec2.scale(contact.contactPointB, contact.normalA, -circleShape.radius);
+    add(contact.contactPointB, contact.contactPointB, circleOffset);
+    sub(contact.contactPointB, contact.contactPointB, circleBody.position);
+
+    // ri is the distance from plane center to contact.
+    vec2.scale(temp, contact.normalA, d);
+    sub(contact.contactPointA, planeToCircle, temp ); // Subtract normal distance vector from the distance vector
+    add(contact.contactPointA, contact.contactPointA, planeOffset);
+    sub(contact.contactPointA, contact.contactPointA, planeBody.position);
+
+    this.contactEquations.push(contact);
+
+    if(this.enableFriction){
+        this.frictionEquations.push( this.createFrictionFromContact(contact) );
+    }
+
+    return 1;
+};
+
+/**
+ * Convex/convex Narrowphase.See <a href="http://www.altdevblogaday.com/2011/05/13/contact-generation-between-3d-convex-meshes/">this article</a> for more info.
+ * @method convexConvex
+ * @param  {Body} bi
+ * @param  {Convex} si
+ * @param  {Array} xi
+ * @param  {Number} ai
+ * @param  {Body} bj
+ * @param  {Convex} sj
+ * @param  {Array} xj
+ * @param  {Number} aj
+ */
+Narrowphase.prototype[Shape.CONVEX] =
+Narrowphase.prototype[Shape.CONVEX | Shape.BOX] =
+Narrowphase.prototype[Shape.BOX] =
+Narrowphase.prototype.convexConvex = function(  bi,si,xi,ai, bj,sj,xj,aj, justTest, precision ){
+    var sepAxis = tmp1,
+        worldPoint = tmp2,
+        worldPoint0 = tmp3,
+        worldPoint1 = tmp4,
+        worldEdge = tmp5,
+        projected = tmp6,
+        penetrationVec = tmp7,
+        dist = tmp8,
+        worldNormal = tmp9,
+        numContacts = 0,
+        precision = typeof(precision) === 'number' ? precision : 0;
+
+    var found = Narrowphase.findSeparatingAxis(si,xi,ai,sj,xj,aj,sepAxis);
+    if(!found){
+        return 0;
+    }
+
+    // Make sure the separating axis is directed from shape i to shape j
+    sub(dist,xj,xi);
+    if(dot(sepAxis,dist) > 0){
+        vec2.scale(sepAxis,sepAxis,-1);
+    }
+
+    // Find edges with normals closest to the separating axis
+    var closestEdge1 = Narrowphase.getClosestEdge(si,ai,sepAxis,true), // Flipped axis
+        closestEdge2 = Narrowphase.getClosestEdge(sj,aj,sepAxis);
+
+    if(closestEdge1 === -1 || closestEdge2 === -1){
+        return 0;
+    }
+
+    // Loop over the shapes
+    for(var k=0; k<2; k++){
+
+        var closestEdgeA = closestEdge1,
+            closestEdgeB = closestEdge2,
+            shapeA =  si, shapeB =  sj,
+            offsetA = xi, offsetB = xj,
+            angleA = ai, angleB = aj,
+            bodyA = bi, bodyB = bj;
+
+        if(k === 0){
+            // Swap!
+            var tmp;
+            tmp = closestEdgeA;
+            closestEdgeA = closestEdgeB;
+            closestEdgeB = tmp;
+
+            tmp = shapeA;
+            shapeA = shapeB;
+            shapeB = tmp;
+
+            tmp = offsetA;
+            offsetA = offsetB;
+            offsetB = tmp;
+
+            tmp = angleA;
+            angleA = angleB;
+            angleB = tmp;
+
+            tmp = bodyA;
+            bodyA = bodyB;
+            bodyB = tmp;
+        }
+
+        // Loop over 2 points in convex B
+        for(var j=closestEdgeB; j<closestEdgeB+2; j++){
+
+            // Get world point
+            var v = shapeB.vertices[(j+shapeB.vertices.length)%shapeB.vertices.length];
+            vec2.rotate(worldPoint, v, angleB);
+            add(worldPoint, worldPoint, offsetB);
+
+            var insideNumEdges = 0;
+
+            // Loop over the 3 closest edges in convex A
+            for(var i=closestEdgeA-1; i<closestEdgeA+2; i++){
+
+                var v0 = shapeA.vertices[(i  +shapeA.vertices.length)%shapeA.vertices.length],
+                    v1 = shapeA.vertices[(i+1+shapeA.vertices.length)%shapeA.vertices.length];
+
+                // Construct the edge
+                vec2.rotate(worldPoint0, v0, angleA);
+                vec2.rotate(worldPoint1, v1, angleA);
+                add(worldPoint0, worldPoint0, offsetA);
+                add(worldPoint1, worldPoint1, offsetA);
+
+                sub(worldEdge, worldPoint1, worldPoint0);
+
+                vec2.rotate90cw(worldNormal, worldEdge); // Normal points out of convex 1
+                vec2.normalize(worldNormal,worldNormal);
+
+                sub(dist, worldPoint, worldPoint0);
+
+                var d = dot(worldNormal,dist);
+
+                if((i === closestEdgeA && d <= precision) || (i !== closestEdgeA && d <= 0)){
+                    insideNumEdges++;
+                }
+            }
+
+            if(insideNumEdges >= 3){
+
+                if(justTest){
+                    return true;
+                }
+
+                // worldPoint was on the "inside" side of each of the 3 checked edges.
+                // Project it to the center edge and use the projection direction as normal
+
+                // Create contact
+                var c = this.createContactEquation(bodyA,bodyB,shapeA,shapeB);
+                numContacts++;
+
+                // Get center edge from body A
+                var v0 = shapeA.vertices[(closestEdgeA)   % shapeA.vertices.length],
+                    v1 = shapeA.vertices[(closestEdgeA+1) % shapeA.vertices.length];
+
+                // Construct the edge
+                vec2.rotate(worldPoint0, v0, angleA);
+                vec2.rotate(worldPoint1, v1, angleA);
+                add(worldPoint0, worldPoint0, offsetA);
+                add(worldPoint1, worldPoint1, offsetA);
+
+                sub(worldEdge, worldPoint1, worldPoint0);
+
+                vec2.rotate90cw(c.normalA, worldEdge); // Normal points out of convex A
+                vec2.normalize(c.normalA,c.normalA);
+
+                sub(dist, worldPoint, worldPoint0); // From edge point to the penetrating point
+                var d = dot(c.normalA,dist);             // Penetration
+                vec2.scale(penetrationVec, c.normalA, d);     // Vector penetration
+
+                sub(c.contactPointA, worldPoint, offsetA);
+                sub(c.contactPointA, c.contactPointA, penetrationVec);
+                add(c.contactPointA, c.contactPointA, offsetA);
+                sub(c.contactPointA, c.contactPointA, bodyA.position);
+
+                sub(c.contactPointB, worldPoint, offsetB);
+                add(c.contactPointB, c.contactPointB, offsetB);
+                sub(c.contactPointB, c.contactPointB, bodyB.position);
+
+                this.contactEquations.push(c);
+
+                // Todo reduce to 1 friction equation if we have 2 contact points
+                if(!this.enableFrictionReduction){
+                    if(this.enableFriction){
+                        this.frictionEquations.push(this.createFrictionFromContact(c));
+                    }
+                }
+            }
+        }
+    }
+
+    if(this.enableFrictionReduction){
+        if(this.enableFriction && numContacts){
+            this.frictionEquations.push(this.createFrictionFromAverage(numContacts));
+        }
+    }
+
+    return numContacts;
+};
+
+// .projectConvex is called by other functions, need local tmp vectors
+var pcoa_tmp1 = vec2.fromValues(0,0);
+
+/**
+ * Project a Convex onto a world-oriented axis
+ * @method projectConvexOntoAxis
+ * @static
+ * @param  {Convex} convexShape
+ * @param  {Array} convexOffset
+ * @param  {Number} convexAngle
+ * @param  {Array} worldAxis
+ * @param  {Array} result
+ */
+Narrowphase.projectConvexOntoAxis = function(convexShape, convexOffset, convexAngle, worldAxis, result){
+    var max=null,
+        min=null,
+        v,
+        value,
+        localAxis = pcoa_tmp1;
+
+    // Convert the axis to local coords of the body
+    vec2.rotate(localAxis, worldAxis, -convexAngle);
+
+    // Get projected position of all vertices
+    for(var i=0; i<convexShape.vertices.length; i++){
+        v = convexShape.vertices[i];
+        value = dot(v,localAxis);
+        if(max === null || value > max){
+            max = value;
+        }
+        if(min === null || value < min){
+            min = value;
+        }
+    }
+
+    if(min > max){
+        var t = min;
+        min = max;
+        max = t;
+    }
+
+    // Project the position of the body onto the axis - need to add this to the result
+    var offset = dot(convexOffset, worldAxis);
+
+    vec2.set( result, min + offset, max + offset);
+};
+
+// .findSeparatingAxis is called by other functions, need local tmp vectors
+var fsa_tmp1 = vec2.fromValues(0,0)
+,   fsa_tmp2 = vec2.fromValues(0,0)
+,   fsa_tmp3 = vec2.fromValues(0,0)
+,   fsa_tmp4 = vec2.fromValues(0,0)
+,   fsa_tmp5 = vec2.fromValues(0,0)
+,   fsa_tmp6 = vec2.fromValues(0,0);
+
+/**
+ * Find a separating axis between the shapes, that maximizes the separating distance between them.
+ * @method findSeparatingAxis
+ * @static
+ * @param  {Convex}     c1
+ * @param  {Array}      offset1
+ * @param  {Number}     angle1
+ * @param  {Convex}     c2
+ * @param  {Array}      offset2
+ * @param  {Number}     angle2
+ * @param  {Array}      sepAxis     The resulting axis
+ * @return {Boolean}                Whether the axis could be found.
+ */
+Narrowphase.findSeparatingAxis = function(c1,offset1,angle1,c2,offset2,angle2,sepAxis){
+    var maxDist = null,
+        overlap = false,
+        found = false,
+        edge = fsa_tmp1,
+        worldPoint0 = fsa_tmp2,
+        worldPoint1 = fsa_tmp3,
+        normal = fsa_tmp4,
+        span1 = fsa_tmp5,
+        span2 = fsa_tmp6;
+
+    if(c1 instanceof Box && c2 instanceof Box){
+
+        for(var j=0; j!==2; j++){
+            var c = c1,
+                angle = angle1;
+            if(j===1){
+                c = c2;
+                angle = angle2;
+            }
+
+            for(var i=0; i!==2; i++){
+
+                // Get the world edge
+                if(i === 0){
+                    vec2.set(normal, 0, 1);
+                } else if(i === 1) {
+                    vec2.set(normal, 1, 0);
+                }
+                if(angle !== 0){
+                    vec2.rotate(normal, normal, angle);
+                }
+
+                // Project hulls onto that normal
+                Narrowphase.projectConvexOntoAxis(c1,offset1,angle1,normal,span1);
+                Narrowphase.projectConvexOntoAxis(c2,offset2,angle2,normal,span2);
+
+                // Order by span position
+                var a=span1,
+                    b=span2,
+                    swapped = false;
+                if(span1[0] > span2[0]){
+                    b=span1;
+                    a=span2;
+                    swapped = true;
+                }
+
+                // Get separating distance
+                var dist = b[0] - a[1];
+                overlap = (dist <= 0);
+
+                if(maxDist===null || dist > maxDist){
+                    vec2.copy(sepAxis, normal);
+                    maxDist = dist;
+                    found = overlap;
+                }
+            }
+        }
+
+    } else {
+
+        for(var j=0; j!==2; j++){
+            var c = c1,
+                angle = angle1;
+            if(j===1){
+                c = c2;
+                angle = angle2;
+            }
+
+            for(var i=0; i!==c.vertices.length; i++){
+                // Get the world edge
+                vec2.rotate(worldPoint0, c.vertices[i], angle);
+                vec2.rotate(worldPoint1, c.vertices[(i+1)%c.vertices.length], angle);
+
+                sub(edge, worldPoint1, worldPoint0);
+
+                // Get normal - just rotate 90 degrees since vertices are given in CCW
+                vec2.rotate90cw(normal, edge);
+                vec2.normalize(normal,normal);
+
+                // Project hulls onto that normal
+                Narrowphase.projectConvexOntoAxis(c1,offset1,angle1,normal,span1);
+                Narrowphase.projectConvexOntoAxis(c2,offset2,angle2,normal,span2);
+
+                // Order by span position
+                var a=span1,
+                    b=span2,
+                    swapped = false;
+                if(span1[0] > span2[0]){
+                    b=span1;
+                    a=span2;
+                    swapped = true;
+                }
+
+                // Get separating distance
+                var dist = b[0] - a[1];
+                overlap = (dist <= 0);
+
+                if(maxDist===null || dist > maxDist){
+                    vec2.copy(sepAxis, normal);
+                    maxDist = dist;
+                    found = overlap;
+                }
+            }
+        }
+    }
+
+
+    /*
+    // Needs to be tested some more
+    for(var j=0; j!==2; j++){
+        var c = c1,
+            angle = angle1;
+        if(j===1){
+            c = c2;
+            angle = angle2;
+        }
+
+        for(var i=0; i!==c.axes.length; i++){
+
+            var normal = c.axes[i];
+
+            // Project hulls onto that normal
+            Narrowphase.projectConvexOntoAxis(c1, offset1, angle1, normal, span1);
+            Narrowphase.projectConvexOntoAxis(c2, offset2, angle2, normal, span2);
+
+            // Order by span position
+            var a=span1,
+                b=span2,
+                swapped = false;
+            if(span1[0] > span2[0]){
+                b=span1;
+                a=span2;
+                swapped = true;
+            }
+
+            // Get separating distance
+            var dist = b[0] - a[1];
+            overlap = (dist <= Narrowphase.convexPrecision);
+
+            if(maxDist===null || dist > maxDist){
+                vec2.copy(sepAxis, normal);
+                maxDist = dist;
+                found = overlap;
+            }
+        }
+    }
+    */
+
+    return found;
+};
+
+// .getClosestEdge is called by other functions, need local tmp vectors
+var gce_tmp1 = vec2.fromValues(0,0)
+,   gce_tmp2 = vec2.fromValues(0,0)
+,   gce_tmp3 = vec2.fromValues(0,0);
+
+/**
+ * Get the edge that has a normal closest to an axis.
+ * @method getClosestEdge
+ * @static
+ * @param  {Convex}     c
+ * @param  {Number}     angle
+ * @param  {Array}      axis
+ * @param  {Boolean}    flip
+ * @return {Number}             Index of the edge that is closest. This index and the next spans the resulting edge. Returns -1 if failed.
+ */
+Narrowphase.getClosestEdge = function(c,angle,axis,flip){
+    var localAxis = gce_tmp1,
+        edge = gce_tmp2,
+        normal = gce_tmp3;
+
+    // Convert the axis to local coords of the body
+    vec2.rotate(localAxis, axis, -angle);
+    if(flip){
+        vec2.scale(localAxis,localAxis,-1);
+    }
+
+    var closestEdge = -1,
+        N = c.vertices.length,
+        maxDot = -1;
+    for(var i=0; i!==N; i++){
+        // Get the edge
+        sub(edge, c.vertices[(i+1)%N], c.vertices[i%N]);
+
+        // Get normal - just rotate 90 degrees since vertices are given in CCW
+        vec2.rotate90cw(normal, edge);
+        vec2.normalize(normal,normal);
+
+        var d = dot(normal,localAxis);
+        if(closestEdge === -1 || d > maxDot){
+            closestEdge = i % N;
+            maxDot = d;
+        }
+    }
+
+    return closestEdge;
+};
+
+var circleHeightfield_candidate = vec2.create(),
+    circleHeightfield_dist = vec2.create(),
+    circleHeightfield_v0 = vec2.create(),
+    circleHeightfield_v1 = vec2.create(),
+    circleHeightfield_minCandidate = vec2.create(),
+    circleHeightfield_worldNormal = vec2.create(),
+    circleHeightfield_minCandidateNormal = vec2.create();
+
+/**
+ * @method circleHeightfield
+ * @param  {Body}           bi
+ * @param  {Circle}         si
+ * @param  {Array}          xi
+ * @param  {Body}           bj
+ * @param  {Heightfield}    sj
+ * @param  {Array}          xj
+ * @param  {Number}         aj
+ */
+Narrowphase.prototype[Shape.CIRCLE | Shape.HEIGHTFIELD] =
+Narrowphase.prototype.circleHeightfield = function( circleBody,circleShape,circlePos,circleAngle,
+                                                    hfBody,hfShape,hfPos,hfAngle, justTest, radius ){
+    var data = hfShape.heights,
+        radius = radius || circleShape.radius,
+        w = hfShape.elementWidth,
+        dist = circleHeightfield_dist,
+        candidate = circleHeightfield_candidate,
+        minCandidate = circleHeightfield_minCandidate,
+        minCandidateNormal = circleHeightfield_minCandidateNormal,
+        worldNormal = circleHeightfield_worldNormal,
+        v0 = circleHeightfield_v0,
+        v1 = circleHeightfield_v1;
+
+    // Get the index of the points to test against
+    var idxA = Math.floor( (circlePos[0] - radius - hfPos[0]) / w ),
+        idxB = Math.ceil(  (circlePos[0] + radius - hfPos[0]) / w );
+
+    /*if(idxB < 0 || idxA >= data.length)
+        return justTest ? false : 0;*/
+
+    if(idxA < 0){
+        idxA = 0;
+    }
+    if(idxB >= data.length){
+        idxB = data.length-1;
+    }
+
+    // Get max and min
+    var max = data[idxA],
+        min = data[idxB];
+    for(var i=idxA; i<idxB; i++){
+        if(data[i] < min){
+            min = data[i];
+        }
+        if(data[i] > max){
+            max = data[i];
+        }
+    }
+
+    if(circlePos[1]-radius > max){
+        return justTest ? false : 0;
+    }
+
+    /*
+    if(circlePos[1]+radius < min){
+        // Below the minimum point... We can just guess.
+        // TODO
+    }
+    */
+
+    // 1. Check so center of circle is not inside the field. If it is, this wont work...
+    // 2. For each edge
+    // 2. 1. Get point on circle that is closest to the edge (scale normal with -radius)
+    // 2. 2. Check if point is inside.
+
+    var found = false;
+
+    // Check all edges first
+    for(var i=idxA; i<idxB; i++){
+
+        // Get points
+        vec2.set(v0,     i*w, data[i]  );
+        vec2.set(v1, (i+1)*w, data[i+1]);
+        vec2.add(v0,v0,hfPos);
+        vec2.add(v1,v1,hfPos);
+
+        // Get normal
+        vec2.sub(worldNormal, v1, v0);
+        vec2.rotate(worldNormal, worldNormal, Math.PI/2);
+        vec2.normalize(worldNormal,worldNormal);
+
+        // Get point on circle, closest to the edge
+        vec2.scale(candidate,worldNormal,-radius);
+        vec2.add(candidate,candidate,circlePos);
+
+        // Distance from v0 to the candidate point
+        vec2.sub(dist,candidate,v0);
+
+        // Check if it is in the element "stick"
+        var d = vec2.dot(dist,worldNormal);
+        if(candidate[0] >= v0[0] && candidate[0] < v1[0] && d <= 0){
+
+            if(justTest){
+                return true;
+            }
+
+            found = true;
+
+            // Store the candidate point, projected to the edge
+            vec2.scale(dist,worldNormal,-d);
+            vec2.add(minCandidate,candidate,dist);
+            vec2.copy(minCandidateNormal,worldNormal);
+
+            var c = this.createContactEquation(hfBody,circleBody,hfShape,circleShape);
+
+            // Normal is out of the heightfield
+            vec2.copy(c.normalA, minCandidateNormal);
+
+            // Vector from circle to heightfield
+            vec2.scale(c.contactPointB,  c.normalA, -radius);
+            add(c.contactPointB, c.contactPointB, circlePos);
+            sub(c.contactPointB, c.contactPointB, circleBody.position);
+
+            vec2.copy(c.contactPointA, minCandidate);
+            vec2.sub(c.contactPointA, c.contactPointA, hfBody.position);
+
+            this.contactEquations.push(c);
+
+            if(this.enableFriction){
+                this.frictionEquations.push( this.createFrictionFromContact(c) );
+            }
+        }
+    }
+
+    // Check all vertices
+    found = false;
+    if(radius > 0){
+        for(var i=idxA; i<=idxB; i++){
+
+            // Get point
+            vec2.set(v0, i*w, data[i]);
+            vec2.add(v0,v0,hfPos);
+
+            vec2.sub(dist, circlePos, v0);
+
+            if(vec2.squaredLength(dist) < Math.pow(radius, 2)){
+
+                if(justTest){
+                    return true;
+                }
+
+                found = true;
+
+                var c = this.createContactEquation(hfBody,circleBody,hfShape,circleShape);
+
+                // Construct normal - out of heightfield
+                vec2.copy(c.normalA, dist);
+                vec2.normalize(c.normalA,c.normalA);
+
+                vec2.scale(c.contactPointB, c.normalA, -radius);
+                add(c.contactPointB, c.contactPointB, circlePos);
+                sub(c.contactPointB, c.contactPointB, circleBody.position);
+
+                sub(c.contactPointA, v0, hfPos);
+                add(c.contactPointA, c.contactPointA, hfPos);
+                sub(c.contactPointA, c.contactPointA, hfBody.position);
+
+                this.contactEquations.push(c);
+
+                if(this.enableFriction){
+                    this.frictionEquations.push(this.createFrictionFromContact(c));
+                }
+            }
+        }
+    }
+
+    if(found){
+        return 1;
+    }
+
+    return 0;
+
+};
+
+var convexHeightfield_v0 = vec2.create(),
+    convexHeightfield_v1 = vec2.create(),
+    convexHeightfield_tilePos = vec2.create(),
+    convexHeightfield_tempConvexShape = new Convex({ vertices: [vec2.create(),vec2.create(),vec2.create(),vec2.create()] });
+/**
+ * @method circleHeightfield
+ * @param  {Body}           bi
+ * @param  {Circle}         si
+ * @param  {Array}          xi
+ * @param  {Body}           bj
+ * @param  {Heightfield}    sj
+ * @param  {Array}          xj
+ * @param  {Number}         aj
+ */
+Narrowphase.prototype[Shape.BOX | Shape.HEIGHTFIELD] =
+Narrowphase.prototype[Shape.CONVEX | Shape.HEIGHTFIELD] =
+Narrowphase.prototype.convexHeightfield = function( convexBody,convexShape,convexPos,convexAngle,
+                                                    hfBody,hfShape,hfPos,hfAngle, justTest ){
+    var data = hfShape.heights,
+        w = hfShape.elementWidth,
+        v0 = convexHeightfield_v0,
+        v1 = convexHeightfield_v1,
+        tilePos = convexHeightfield_tilePos,
+        tileConvex = convexHeightfield_tempConvexShape;
+
+    // Get the index of the points to test against
+    var idxA = Math.floor( (convexBody.aabb.lowerBound[0] - hfPos[0]) / w ),
+        idxB = Math.ceil(  (convexBody.aabb.upperBound[0] - hfPos[0]) / w );
+
+    if(idxA < 0){
+        idxA = 0;
+    }
+    if(idxB >= data.length){
+        idxB = data.length-1;
+    }
+
+    // Get max and min
+    var max = data[idxA],
+        min = data[idxB];
+    for(var i=idxA; i<idxB; i++){
+        if(data[i] < min){
+            min = data[i];
+        }
+        if(data[i] > max){
+            max = data[i];
+        }
+    }
+
+    if(convexBody.aabb.lowerBound[1] > max){
+        return justTest ? false : 0;
+    }
+
+    var found = false;
+    var numContacts = 0;
+
+    // Loop over all edges
+    // TODO: If possible, construct a convex from several data points (need o check if the points make a convex shape)
+    for(var i=idxA; i<idxB; i++){
+
+        // Get points
+        vec2.set(v0,     i*w, data[i]  );
+        vec2.set(v1, (i+1)*w, data[i+1]);
+        vec2.add(v0,v0,hfPos);
+        vec2.add(v1,v1,hfPos);
+
+        // Construct a convex
+        var tileHeight = 100; // todo
+        vec2.set(tilePos, (v1[0] + v0[0])*0.5, (v1[1] + v0[1] - tileHeight)*0.5);
+
+        vec2.sub(tileConvex.vertices[0], v1, tilePos);
+        vec2.sub(tileConvex.vertices[1], v0, tilePos);
+        vec2.copy(tileConvex.vertices[2], tileConvex.vertices[1]);
+        vec2.copy(tileConvex.vertices[3], tileConvex.vertices[0]);
+        tileConvex.vertices[2][1] -= tileHeight;
+        tileConvex.vertices[3][1] -= tileHeight;
+
+        // Do convex collision
+        numContacts += this.convexConvex(   convexBody, convexShape, convexPos, convexAngle,
+                                            hfBody, tileConvex, tilePos, 0, justTest);
+    }
+
+    return numContacts;
+};
\ No newline at end of file

src/collision/Ray.js

+module.exports = Ray;
+
+var vec2 = require('../math/vec2');
+var RaycastResult = require('../collision/RaycastResult');
+var Shape = require('../shapes/Shape');
+var AABB = require('../collision/AABB');
+
+/**
+ * A line with a start and end point that is used to intersect shapes. For an example, see {{#crossLink "World/raycast:method"}}World.raycast{{/crossLink}}
+ * @class Ray
+ * @constructor
+ * @param {object} [options]
+ * @param {array} [options.from]
+ * @param {array} [options.to]
+ * @param {boolean} [options.checkCollisionResponse=true]
+ * @param {boolean} [options.skipBackfaces=false]
+ * @param {number} [options.collisionMask=-1]
+ * @param {number} [options.collisionGroup=-1]
+ * @param {number} [options.mode=Ray.ANY]
+ * @param {number} [options.callback]
+ */
+function Ray(options){
+    options = options || {};
+
+    /**
+     * Ray start point.
+     * @property {array} from
+     */
+    this.from = options.from ? vec2.fromValues(options.from[0], options.from[1]) : vec2.create();
+
+    /**
+     * Ray end point
+     * @property {array} to
+     */
+    this.to = options.to ? vec2.fromValues(options.to[0], options.to[1]) : vec2.create();
+
+    /**
+     * Set to true if you want the Ray to take .collisionResponse flags into account on bodies and shapes.
+     * @property {Boolean} checkCollisionResponse
+     */
+    this.checkCollisionResponse = options.checkCollisionResponse !== undefined ? options.checkCollisionResponse : true;
+
+    /**
+     * If set to true, the ray skips any hits with normal.dot(rayDirection) < 0.
+     * @property {Boolean} skipBackfaces
+     */
+    this.skipBackfaces = !!options.skipBackfaces;
+
+    /**
+     * @property {number} collisionMask
+     * @default -1
+     */
+    this.collisionMask = options.collisionMask !== undefined ? options.collisionMask : -1;
+
+    /**
+     * @property {number} collisionGroup
+     * @default -1
+     */
+    this.collisionGroup = options.collisionGroup !== undefined ? options.collisionGroup : -1;
+
+    /**
+     * The intersection mode. Should be {{#crossLink "Ray/ANY:property"}}Ray.ANY{{/crossLink}}, {{#crossLink "Ray/ALL:property"}}Ray.ALL{{/crossLink}} or {{#crossLink "Ray/CLOSEST:property"}}Ray.CLOSEST{{/crossLink}}.
+     * @property {number} mode
+     */
+    this.mode = options.mode !== undefined ? options.mode : Ray.ANY;
+
+    /**
+     * Current, user-provided result callback. Will be used if mode is Ray.ALL.
+     * @property {Function} callback
+     */
+    this.callback = options.callback || function(result){};
+
+    /**
+     * @readOnly
+     * @property {array} direction
+     */
+    this.direction = vec2.create();
+
+    /**
+     * Length of the ray
+     * @readOnly
+     * @property {number} length
+     */
+    this.length = 1;
+
+    this.update();
+}
+Ray.prototype.constructor = Ray;
+
+/**
+ * This raycasting mode will make the Ray traverse through all intersection points and only return the closest one.
+ * @static
+ * @property {Number} CLOSEST
+ */
+Ray.CLOSEST = 1;
+
+/**
+ * This raycasting mode will make the Ray stop when it finds the first intersection point.
+ * @static
+ * @property {Number} ANY
+ */
+Ray.ANY = 2;
+
+/**
+ * This raycasting mode will traverse all intersection points and executes a callback for each one.
+ * @static
+ * @property {Number} ALL
+ */
+Ray.ALL = 4;
+
+/**
+ * Should be called if you change the from or to point.
+ * @method update
+ */
+Ray.prototype.update = function(){
+
+    // Update .direction and .length
+    var d = this.direction;
+    vec2.sub(d, this.to, this.from);
+    this.length = vec2.length(d);
+    vec2.normalize(d, d);
+
+};
+
+/**
+ * @method intersectBodies
+ * @param {Array} bodies An array of Body objects.
+ */
+Ray.prototype.intersectBodies = function (result, bodies) {
+    for (var i = 0, l = bodies.length; !result.shouldStop(this) && i < l; i++) {
+        var body = bodies[i];
+        var aabb = body.getAABB();
+        if(aabb.overlapsRay(this) >= 0 || aabb.containsPoint(this.from)){
+            this.intersectBody(result, body);
+        }
+    }
+};
+
+var intersectBody_worldPosition = vec2.create();
+
+/**
+ * Shoot a ray at a body, get back information about the hit.
+ * @method intersectBody
+ * @private
+ * @param {Body} body
+ */
+Ray.prototype.intersectBody = function (result, body) {
+    var checkCollisionResponse = this.checkCollisionResponse;
+
+    if(checkCollisionResponse && !body.collisionResponse){
+        return;
+    }
+
+    var worldPosition = intersectBody_worldPosition;
+
+    for (var i = 0, N = body.shapes.length; i < N; i++) {
+        var shape = body.shapes[i];
+
+        if(checkCollisionResponse && !shape.collisionResponse){
+            continue; // Skip
+        }
+
+        if((this.collisionGroup & shape.collisionMask) === 0 || (shape.collisionGroup & this.collisionMask) === 0){
+            continue;
+        }
+
+        // Get world angle and position of the shape
+        vec2.rotate(worldPosition, shape.position, body.angle);
+        vec2.add(worldPosition, worldPosition, body.position);
+        var worldAngle = shape.angle + body.angle;
+
+        this.intersectShape(
+            result,
+            shape,
+            worldAngle,
+            worldPosition,
+            body
+        );
+
+        if(result.shouldStop(this)){
+            break;
+        }
+    }
+};
+
+/**
+ * @method intersectShape
+ * @private
+ * @param {Shape} shape
+ * @param {number} angle
+ * @param {array} position
+ * @param {Body} body
+ */
+Ray.prototype.intersectShape = function(result, shape, angle, position, body){
+    var from = this.from;
+
+    // Checking radius
+    var distance = distanceFromIntersectionSquared(from, this.direction, position);
+    if (distance > shape.boundingRadius * shape.boundingRadius) {
+        return;
+    }
+
+    this._currentBody = body;
+    this._currentShape = shape;
+
+    shape.raycast(result, this, position, angle);
+
+    this._currentBody = this._currentShape = null;
+};
+
+/**
+ * Get the AABB of the ray.
+ * @method getAABB
+ * @param  {AABB} aabb
+ */
+Ray.prototype.getAABB = function(result){
+    var to = this.to;
+    var from = this.from;
+    vec2.set(
+        result.lowerBound,
+        Math.min(to[0], from[0]),
+        Math.min(to[1], from[1])
+    );
+    vec2.set(
+        result.upperBound,
+        Math.max(to[0], from[0]),
+        Math.max(to[1], from[1])
+    );
+};
+
+var hitPointWorld = vec2.create();
+
+/**
+ * @method reportIntersection
+ * @private
+ * @param  {number} fraction
+ * @param  {array} normal
+ * @param  {number} [faceIndex=-1]
+ * @return {boolean} True if the intersections should continue
+ */
+Ray.prototype.reportIntersection = function(result, fraction, normal, faceIndex){
+    var from = this.from;
+    var to = this.to;
+    var shape = this._currentShape;
+    var body = this._currentBody;
+
+    // Skip back faces?
+    if(this.skipBackfaces && vec2.dot(normal, this.direction) > 0){
+        return;
+    }
+
+    switch(this.mode){
+
+    case Ray.ALL:
+        result.set(
+            normal,
+            shape,
+            body,
+            fraction,
+            faceIndex
+        );
+        this.callback(result);
+        break;
+
+    case Ray.CLOSEST:
+
+        // Store if closer than current closest
+        if(fraction < result.fraction || !result.hasHit()){
+            result.set(
+                normal,
+                shape,
+                body,
+                fraction,
+                faceIndex
+            );
+        }
+        break;
+
+    case Ray.ANY:
+
+        // Report and stop.
+        result.set(
+            normal,
+            shape,
+            body,
+            fraction,
+            faceIndex
+        );
+        break;
+    }
+};
+
+var v0 = vec2.create(),
+    intersect = vec2.create();
+function distanceFromIntersectionSquared(from, direction, position) {
+
+    // v0 is vector from from to position
+    vec2.sub(v0, position, from);
+    var dot = vec2.dot(v0, direction);
+
+    // intersect = direction * dot + from
+    vec2.scale(intersect, direction, dot);
+    vec2.add(intersect, intersect, from);
+
+    return vec2.squaredDistance(position, intersect);
+}
+

src/collision/RaycastResult.js

+var vec2 = require('../math/vec2');
+var Ray = require('../collision/Ray');
+
+module.exports = RaycastResult;
+
+/**
+ * Storage for Ray casting hit data.
+ * @class RaycastResult
+ * @constructor
+ */
+function RaycastResult(){
+
+	/**
+	 * The normal of the hit, oriented in world space.
+	 * @property {array} normal
+	 */
+	this.normal = vec2.create();
+
+	/**
+	 * The hit shape, or null.
+	 * @property {Shape} shape
+	 */
+	this.shape = null;
+
+	/**
+	 * The hit body, or null.
+	 * @property {Body} body
+	 */
+	this.body = null;
+
+	/**
+	 * The index of the hit triangle, if the hit shape was indexable.
+	 * @property {number} faceIndex
+	 * @default -1
+	 */
+	this.faceIndex = -1;
+
+	/**
+	 * Distance to the hit, as a fraction. 0 is at the "from" point, 1 is at the "to" point. Will be set to -1 if there was no hit yet.
+	 * @property {number} fraction
+	 * @default -1
+	 */
+	this.fraction = -1;
+
+	/**
+	 * If the ray should stop traversing.
+	 * @readonly
+	 * @property {Boolean} isStopped
+	 */
+	this.isStopped = false;
+}
+
+/**
+ * Reset all result data. Must be done before re-using the result object.
+ * @method reset
+ */
+RaycastResult.prototype.reset = function () {
+	vec2.set(this.normal, 0, 0);
+	this.shape = null;
+	this.body = null;
+	this.faceIndex = -1;
+	this.fraction = -1;
+	this.isStopped = false;
+};
+
+/**
+ * Get the distance to the hit point.
+ * @method getHitDistance
+ * @param {Ray} ray
+ */
+RaycastResult.prototype.getHitDistance = function (ray) {
+	return vec2.distance(ray.from, ray.to) * this.fraction;
+};
+
+/**
+ * Returns true if the ray hit something since the last reset().
+ * @method hasHit
+ */
+RaycastResult.prototype.hasHit = function () {
+	return this.fraction !== -1;
+};
+
+/**
+ * Get world hit point.
+ * @method getHitPoint
+ * @param {array} out
+ * @param {Ray} ray
+ */
+RaycastResult.prototype.getHitPoint = function (out, ray) {
+	vec2.lerp(out, ray.from, ray.to, this.fraction);
+};
+
+/**
+ * Can be called while iterating over hits to stop searching for hit points.
+ * @method stop
+ */
+RaycastResult.prototype.stop = function(){
+	this.isStopped = true;
+};
+
+/**
+ * @method shouldStop
+ * @private
+ * @param {Ray} ray
+ * @return {boolean}
+ */
+RaycastResult.prototype.shouldStop = function(ray){
+	return this.isStopped || (this.fraction !== -1 && ray.mode === Ray.ANY);
+};
+
+/**
+ * @method set
+ * @private
+ * @param {array} normal
+ * @param {Shape} shape
+ * @param {Body} body
+ * @param {number} fraction
+ */
+RaycastResult.prototype.set = function(
+	normal,
+	shape,
+	body,
+	fraction,
+	faceIndex
+){
+	vec2.copy(this.normal, normal);
+	this.shape = shape;
+	this.body = body;
+	this.fraction = fraction;
+	this.faceIndex = faceIndex;
+};
\ No newline at end of file

src/collision/SAPBroadphase.js

+var Utils = require('../utils/Utils')
+,   Broadphase = require('../collision/Broadphase');
+
+module.exports = SAPBroadphase;
+
+/**
+ * Sweep and prune broadphase along one axis.
+ *
+ * @class SAPBroadphase
+ * @constructor
+ * @extends Broadphase
+ */
+function SAPBroadphase(){
+    Broadphase.call(this,Broadphase.SAP);
+
+    /**
+     * List of bodies currently in the broadphase.
+     * @property axisList
+     * @type {Array}
+     */
+    this.axisList = [];
+
+    /**
+     * The axis to sort along. 0 means x-axis and 1 y-axis. If your bodies are more spread out over the X axis, set axisIndex to 0, and you will gain some performance.
+     * @property axisIndex
+     * @type {Number}
+     */
+    this.axisIndex = 0;
+
+    var that = this;
+    this._addBodyHandler = function(e){
+        that.axisList.push(e.body);
+    };
+
+    this._removeBodyHandler = function(e){
+        // Remove from list
+        var idx = that.axisList.indexOf(e.body);
+        if(idx !== -1){
+            that.axisList.splice(idx,1);
+        }
+    };
+}
+SAPBroadphase.prototype = new Broadphase();
+SAPBroadphase.prototype.constructor = SAPBroadphase;
+
+/**
+ * Change the world
+ * @method setWorld
+ * @param {World} world
+ */
+SAPBroadphase.prototype.setWorld = function(world){
+    // Clear the old axis array
+    this.axisList.length = 0;
+
+    // Add all bodies from the new world
+    Utils.appendArray(this.axisList, world.bodies);
+
+    // Remove old handlers, if any
+    world
+        .off("addBody",this._addBodyHandler)
+        .off("removeBody",this._removeBodyHandler);
+
+    // Add handlers to update the list of bodies.
+    world.on("addBody",this._addBodyHandler).on("removeBody",this._removeBodyHandler);
+
+    this.world = world;
+};
+
+/**
+ * Sorts bodies along an axis.
+ * @method sortAxisList
+ * @param {Array} a
+ * @param {number} axisIndex
+ * @return {Array}
+ */
+SAPBroadphase.sortAxisList = function(a, axisIndex){
+    axisIndex = axisIndex|0;
+    for(var i=1,l=a.length; i<l; i++) {
+        var v = a[i];
+        for(var j=i - 1;j>=0;j--) {
+            if(a[j].aabb.lowerBound[axisIndex] <= v.aabb.lowerBound[axisIndex]){
+                break;
+            }
+            a[j+1] = a[j];
+        }
+        a[j+1] = v;
+    }
+    return a;
+};
+
+SAPBroadphase.prototype.sortList = function(){
+    var bodies = this.axisList,
+    axisIndex = this.axisIndex;
+
+    // Sort the lists
+    SAPBroadphase.sortAxisList(bodies, axisIndex);
+};
+
+/**
+ * Get the colliding pairs
+ * @method getCollisionPairs
+ * @param  {World} world
+ * @return {Array}
+ */
+SAPBroadphase.prototype.getCollisionPairs = function(world){
+    var bodies = this.axisList,
+        result = this.result,
+        axisIndex = this.axisIndex;
+
+    result.length = 0;
+
+    // Update all AABBs if needed
+    var l = bodies.length;
+    while(l--){
+        var b = bodies[l];
+        if(b.aabbNeedsUpdate){
+            b.updateAABB();
+        }
+    }
+
+    // Sort the lists
+    this.sortList();
+
+    // Look through the X list
+    for(var i=0, N=bodies.length|0; i!==N; i++){
+        var bi = bodies[i];
+
+        for(var j=i+1; j<N; j++){
+            var bj = bodies[j];
+
+            // Bounds overlap?
+            var overlaps = (bj.aabb.lowerBound[axisIndex] <= bi.aabb.upperBound[axisIndex]);
+            if(!overlaps){
+                break;
+            }
+
+            if(Broadphase.canCollide(bi,bj) && this.boundingVolumeCheck(bi,bj)){
+                result.push(bi,bj);
+            }
+        }
+    }
+
+    return result;
+};
+
+/**
+ * Returns all the bodies within an AABB.
+ * @method aabbQuery
+ * @param  {World} world
+ * @param  {AABB} aabb
+ * @param {array} result An array to store resulting bodies in.
+ * @return {array}
+ */
+SAPBroadphase.prototype.aabbQuery = function(world, aabb, result){
+    result = result || [];
+
+    this.sortList();
+
+    var axisIndex = this.axisIndex;
+    var axis = 'x';
+    if(axisIndex === 1){ axis = 'y'; }
+    if(axisIndex === 2){ axis = 'z'; }
+
+    var axisList = this.axisList;
+    var lower = aabb.lowerBound[axis];
+    var upper = aabb.upperBound[axis];
+    for(var i = 0; i < axisList.length; i++){
+        var b = axisList[i];
+
+        if(b.aabbNeedsUpdate){
+            b.updateAABB();
+        }
+
+        if(b.aabb.overlaps(aabb)){
+            result.push(b);
+        }
+    }
+
+    return result;
+};
\ No newline at end of file

src/constraints/Constraint.js

+module.exports = Constraint;
+
+var Utils = require('../utils/Utils');
+
+/**
+ * Base constraint class.
+ *
+ * @class Constraint
+ * @constructor
+ * @author schteppe
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Number} type
+ * @param {Object} [options]
+ * @param {Object} [options.collideConnected=true]
+ */
+function Constraint(bodyA, bodyB, type, options){
+
+    /**
+     * The type of constraint. May be one of Constraint.DISTANCE, Constraint.GEAR, Constraint.LOCK, Constraint.PRISMATIC or Constraint.REVOLUTE.
+     * @property {number} type
+     */
+    this.type = type;
+
+    options = Utils.defaults(options,{
+        collideConnected : true,
+        wakeUpBodies : true,
+    });
+
+    /**
+     * Equations to be solved in this constraint
+     *
+     * @property equations
+     * @type {Array}
+     */
+    this.equations = [];
+
+    /**
+     * First body participating in the constraint.
+     * @property bodyA
+     * @type {Body}
+     */
+    this.bodyA = bodyA;
+
+    /**
+     * Second body participating in the constraint.
+     * @property bodyB
+     * @type {Body}
+     */
+    this.bodyB = bodyB;
+
+    /**
+     * Set to true if you want the connected bodies to collide.
+     * @property collideConnected
+     * @type {Boolean}
+     * @default true
+     */
+    this.collideConnected = options.collideConnected;
+
+    // Wake up bodies when connected
+    if(options.wakeUpBodies){
+        if(bodyA){
+            bodyA.wakeUp();
+        }
+        if(bodyB){
+            bodyB.wakeUp();
+        }
+    }
+}
+
+/**
+ * Updates the internal constraint parameters before solve.
+ * @method update
+ */
+Constraint.prototype.update = function(){
+    throw new Error("method update() not implmemented in this Constraint subclass!");
+};
+
+/**
+ * @static
+ * @property {number} DISTANCE
+ */
+Constraint.DISTANCE = 1;
+
+/**
+ * @static
+ * @property {number} GEAR
+ */
+Constraint.GEAR = 2;
+
+/**
+ * @static
+ * @property {number} LOCK
+ */
+Constraint.LOCK = 3;
+
+/**
+ * @static
+ * @property {number} PRISMATIC
+ */
+Constraint.PRISMATIC = 4;
+
+/**
+ * @static
+ * @property {number} REVOLUTE
+ */
+Constraint.REVOLUTE = 5;
+
+/**
+ * Set stiffness for this constraint.
+ * @method setStiffness
+ * @param {Number} stiffness
+ */
+Constraint.prototype.setStiffness = function(stiffness){
+    var eqs = this.equations;
+    for(var i=0; i !== eqs.length; i++){
+        var eq = eqs[i];
+        eq.stiffness = stiffness;
+        eq.needsUpdate = true;
+    }
+};
+
+/**
+ * Set relaxation for this constraint.
+ * @method setRelaxation
+ * @param {Number} relaxation
+ */
+Constraint.prototype.setRelaxation = function(relaxation){
+    var eqs = this.equations;
+    for(var i=0; i !== eqs.length; i++){
+        var eq = eqs[i];
+        eq.relaxation = relaxation;
+        eq.needsUpdate = true;
+    }
+};

src/constraints/DistanceConstraint.js

+var Constraint = require('./Constraint')
+,   Equation = require('../equations/Equation')
+,   vec2 = require('../math/vec2')
+,   Utils = require('../utils/Utils');
+
+module.exports = DistanceConstraint;
+
+/**
+ * Constraint that tries to keep the distance between two bodies constant.
+ *
+ * @class DistanceConstraint
+ * @constructor
+ * @author schteppe
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {object} [options]
+ * @param {number} [options.distance] The distance to keep between the anchor points. Defaults to the current distance between the bodies.
+ * @param {Array} [options.localAnchorA] The anchor point for bodyA, defined locally in bodyA frame. Defaults to [0,0].
+ * @param {Array} [options.localAnchorB] The anchor point for bodyB, defined locally in bodyB frame. Defaults to [0,0].
+ * @param {object} [options.maxForce=Number.MAX_VALUE] Maximum force to apply.
+ * @extends Constraint
+ *
+ * @example
+ *     // If distance is not given as an option, then the current distance between the bodies is used.
+ *     // In this example, the bodies will be constrained to have a distance of 2 between their centers.
+ *     var bodyA = new Body({ mass: 1, position: [-1, 0] });
+ *     var bodyB = new Body({ mass: 1, position: [1, 0] });
+ *     var constraint = new DistanceConstraint(bodyA, bodyB);
+ *     world.addConstraint(constraint);
+ *
+ * @example
+ *     // Manually set the distance and anchors
+ *     var constraint = new DistanceConstraint(bodyA, bodyB, {
+ *         distance: 1,          // Distance to keep between the points
+ *         localAnchorA: [1, 0], // Point on bodyA
+ *         localAnchorB: [-1, 0] // Point on bodyB
+ *     });
+ *     world.addConstraint(constraint);
+ */
+function DistanceConstraint(bodyA,bodyB,options){
+    options = Utils.defaults(options,{
+        localAnchorA:[0,0],
+        localAnchorB:[0,0]
+    });
+
+    Constraint.call(this,bodyA,bodyB,Constraint.DISTANCE,options);
+
+    /**
+     * Local anchor in body A.
+     * @property localAnchorA
+     * @type {Array}
+     */
+    this.localAnchorA = vec2.fromValues(options.localAnchorA[0], options.localAnchorA[1]);
+
+    /**
+     * Local anchor in body B.
+     * @property localAnchorB
+     * @type {Array}
+     */
+    this.localAnchorB = vec2.fromValues(options.localAnchorB[0], options.localAnchorB[1]);
+
+    var localAnchorA = this.localAnchorA;
+    var localAnchorB = this.localAnchorB;
+
+    /**
+     * The distance to keep.
+     * @property distance
+     * @type {Number}
+     */
+    this.distance = 0;
+
+    if(typeof(options.distance) === 'number'){
+        this.distance = options.distance;
+    } else {
+        // Use the current world distance between the world anchor points.
+        var worldAnchorA = vec2.create(),
+            worldAnchorB = vec2.create(),
+            r = vec2.create();
+
+        // Transform local anchors to world
+        vec2.rotate(worldAnchorA, localAnchorA, bodyA.angle);
+        vec2.rotate(worldAnchorB, localAnchorB, bodyB.angle);
+
+        vec2.add(r, bodyB.position, worldAnchorB);
+        vec2.sub(r, r, worldAnchorA);
+        vec2.sub(r, r, bodyA.position);
+
+        this.distance = vec2.length(r);
+    }
+
+    var maxForce;
+    if(typeof(options.maxForce)==="undefined" ){
+        maxForce = Number.MAX_VALUE;
+    } else {
+        maxForce = options.maxForce;
+    }
+
+    var normal = new Equation(bodyA,bodyB,-maxForce,maxForce); // Just in the normal direction
+    this.equations = [ normal ];
+
+    /**
+     * Max force to apply.
+     * @property {number} maxForce
+     */
+    this.maxForce = maxForce;
+
+    // g = (xi - xj).dot(n)
+    // dg/dt = (vi - vj).dot(n) = G*W = [n 0 -n 0] * [vi wi vj wj]'
+
+    // ...and if we were to include offset points:
+    // g =
+    //      (xj + rj - xi - ri).dot(n) - distance
+    //
+    // dg/dt =
+    //      (vj + wj x rj - vi - wi x ri).dot(n) =
+    //      { term 2 is near zero } =
+    //      [-n   -ri x n   n   rj x n] * [vi wi vj wj]' =
+    //      G * W
+    //
+    // => G = [-n -rixn n rjxn]
+
+    var r = vec2.create();
+    var ri = vec2.create(); // worldAnchorA
+    var rj = vec2.create(); // worldAnchorB
+    var that = this;
+    normal.computeGq = function(){
+        var bodyA = this.bodyA,
+            bodyB = this.bodyB,
+            xi = bodyA.position,
+            xj = bodyB.position;
+
+        // Transform local anchors to world
+        vec2.rotate(ri, localAnchorA, bodyA.angle);
+        vec2.rotate(rj, localAnchorB, bodyB.angle);
+
+        vec2.add(r, xj, rj);
+        vec2.sub(r, r, ri);
+        vec2.sub(r, r, xi);
+
+        //vec2.sub(r, bodyB.position, bodyA.position);
+        return vec2.length(r) - that.distance;
+    };
+
+    // Make the contact constraint bilateral
+    this.setMaxForce(maxForce);
+
+    /**
+     * If the upper limit is enabled or not.
+     * @property {Boolean} upperLimitEnabled
+     */
+    this.upperLimitEnabled = false;
+
+    /**
+     * The upper constraint limit.
+     * @property {number} upperLimit
+     */
+    this.upperLimit = 1;
+
+    /**
+     * If the lower limit is enabled or not.
+     * @property {Boolean} lowerLimitEnabled
+     */
+    this.lowerLimitEnabled = false;
+
+    /**
+     * The lower constraint limit.
+     * @property {number} lowerLimit
+     */
+    this.lowerLimit = 0;
+
+    /**
+     * Current constraint position. This is equal to the current distance between the world anchor points.
+     * @property {number} position
+     */
+    this.position = 0;
+}
+DistanceConstraint.prototype = new Constraint();
+DistanceConstraint.prototype.constructor = DistanceConstraint;
+
+/**
+ * Update the constraint equations. Should be done if any of the bodies changed position, before solving.
+ * @method update
+ */
+var n = vec2.create();
+var ri = vec2.create(); // worldAnchorA
+var rj = vec2.create(); // worldAnchorB
+DistanceConstraint.prototype.update = function(){
+    var normal = this.equations[0],
+        bodyA = this.bodyA,
+        bodyB = this.bodyB,
+        distance = this.distance,
+        xi = bodyA.position,
+        xj = bodyB.position,
+        normalEquation = this.equations[0],
+        G = normal.G;
+
+    // Transform local anchors to world
+    vec2.rotate(ri, this.localAnchorA, bodyA.angle);
+    vec2.rotate(rj, this.localAnchorB, bodyB.angle);
+
+    // Get world anchor points and normal
+    vec2.add(n, xj, rj);
+    vec2.sub(n, n, ri);
+    vec2.sub(n, n, xi);
+    this.position = vec2.length(n);
+
+    var violating = false;
+    if(this.upperLimitEnabled){
+        if(this.position > this.upperLimit){
+            normalEquation.maxForce = 0;
+            normalEquation.minForce = -this.maxForce;
+            this.distance = this.upperLimit;
+            violating = true;
+        }
+    }
+
+    if(this.lowerLimitEnabled){
+        if(this.position < this.lowerLimit){
+            normalEquation.maxForce = this.maxForce;
+            normalEquation.minForce = 0;
+            this.distance = this.lowerLimit;
+            violating = true;
+        }
+    }
+
+    if((this.lowerLimitEnabled || this.upperLimitEnabled) && !violating){
+        // No constraint needed.
+        normalEquation.enabled = false;
+        return;
+    }
+
+    normalEquation.enabled = true;
+
+    vec2.normalize(n,n);
+
+    // Caluclate cross products
+    var rixn = vec2.crossLength(ri, n),
+        rjxn = vec2.crossLength(rj, n);
+
+    // G = [-n -rixn n rjxn]
+    G[0] = -n[0];
+    G[1] = -n[1];
+    G[2] = -rixn;
+    G[3] = n[0];
+    G[4] = n[1];
+    G[5] = rjxn;
+};
+
+/**
+ * Set the max force to be used
+ * @method setMaxForce
+ * @param {Number} maxForce
+ */
+DistanceConstraint.prototype.setMaxForce = function(maxForce){
+    var normal = this.equations[0];
+    normal.minForce = -maxForce;
+    normal.maxForce =  maxForce;
+};
+
+/**
+ * Get the max force
+ * @method getMaxForce
+ * @return {Number}
+ */
+DistanceConstraint.prototype.getMaxForce = function(){
+    var normal = this.equations[0];
+    return normal.maxForce;
+};

src/constraints/GearConstraint.js

+var Constraint = require('./Constraint')
+,   Equation = require('../equations/Equation')
+,   AngleLockEquation = require('../equations/AngleLockEquation')
+,   vec2 = require('../math/vec2');
+
+module.exports = GearConstraint;
+
+/**
+ * Constrains the angle of two bodies to each other to be equal. If a gear ratio is not one, the angle of bodyA must be a multiple of the angle of bodyB.
+ * @class GearConstraint
+ * @constructor
+ * @author schteppe
+ * @param {Body}            bodyA
+ * @param {Body}            bodyB
+ * @param {Object}          [options]
+ * @param {Number}          [options.angle=0] Relative angle between the bodies. Will be set to the current angle between the bodies (the gear ratio is accounted for).
+ * @param {Number}          [options.ratio=1] Gear ratio.
+ * @param {Number}          [options.maxTorque] Maximum torque to apply.
+ * @extends Constraint
+ *
+ * @example
+ *     var constraint = new GearConstraint(bodyA, bodyB);
+ *     world.addConstraint(constraint);
+ *
+ * @example
+ *     var constraint = new GearConstraint(bodyA, bodyB, {
+ *         ratio: 2,
+ *         maxTorque: 1000
+ *     });
+ *     world.addConstraint(constraint);
+ */
+function GearConstraint(bodyA, bodyB, options){
+    options = options || {};
+
+    Constraint.call(this, bodyA, bodyB, Constraint.GEAR, options);
+
+    /**
+     * The gear ratio.
+     * @property ratio
+     * @type {Number}
+     */
+    this.ratio = options.ratio !== undefined ? options.ratio : 1;
+
+    /**
+     * The relative angle
+     * @property angle
+     * @type {Number}
+     */
+    this.angle = options.angle !== undefined ? options.angle : bodyB.angle - this.ratio * bodyA.angle;
+
+    // Send same parameters to the equation
+    options.angle = this.angle;
+    options.ratio = this.ratio;
+
+    this.equations = [
+        new AngleLockEquation(bodyA,bodyB,options),
+    ];
+
+    // Set max torque
+    if(options.maxTorque !== undefined){
+        this.setMaxTorque(options.maxTorque);
+    }
+}
+GearConstraint.prototype = new Constraint();
+GearConstraint.prototype.constructor = GearConstraint;
+
+GearConstraint.prototype.update = function(){
+    var eq = this.equations[0];
+    if(eq.ratio !== this.ratio){
+        eq.setRatio(this.ratio);
+    }
+    eq.angle = this.angle;
+};
+
+/**
+ * Set the max torque for the constraint.
+ * @method setMaxTorque
+ * @param {Number} torque
+ */
+GearConstraint.prototype.setMaxTorque = function(torque){
+    this.equations[0].setMaxTorque(torque);
+};
+
+/**
+ * Get the max torque for the constraint.
+ * @method getMaxTorque
+ * @return {Number}
+ */
+GearConstraint.prototype.getMaxTorque = function(torque){
+    return this.equations[0].maxForce;
+};
\ No newline at end of file

src/constraints/LockConstraint.js

+var Constraint = require('./Constraint')
+,   vec2 = require('../math/vec2')
+,   Equation = require('../equations/Equation');
+
+module.exports = LockConstraint;
+
+/**
+ * Locks the relative position and rotation between two bodies.
+ *
+ * @class LockConstraint
+ * @constructor
+ * @author schteppe
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Object} [options]
+ * @param {Array}  [options.localOffsetB] The offset of bodyB in bodyA's frame. If not given the offset is computed from current positions.
+ * @param {number} [options.localAngleB] The angle of bodyB in bodyA's frame. If not given, the angle is computed from current angles.
+ * @param {number} [options.maxForce]
+ * @extends Constraint
+ *
+ * @example
+ *     // Locks the relative position and rotation between bodyA and bodyB
+ *     var constraint = new LockConstraint(bodyA, bodyB);
+ *     world.addConstraint(constraint);
+ */
+function LockConstraint(bodyA, bodyB, options){
+    options = options || {};
+
+    Constraint.call(this,bodyA,bodyB,Constraint.LOCK,options);
+
+    var maxForce = ( typeof(options.maxForce)==="undefined" ? Number.MAX_VALUE : options.maxForce );
+
+    var localAngleB = options.localAngleB || 0;
+
+    // Use 3 equations:
+    // gx =   (xj - xi - l) * xhat = 0
+    // gy =   (xj - xi - l) * yhat = 0
+    // gr =   (xi - xj + r) * that = 0
+    //
+    // ...where:
+    //   l is the localOffsetB vector rotated to world in bodyA frame
+    //   r is the same vector but reversed and rotated from bodyB frame
+    //   xhat, yhat are world axis vectors
+    //   that is the tangent of r
+    //
+    // For the first two constraints, we get
+    // G*W = (vj - vi - ldot  ) * xhat
+    //     = (vj - vi - wi x l) * xhat
+    //
+    // Since (wi x l) * xhat = (l x xhat) * wi, we get
+    // G*W = [ -1   0   (-l x xhat)  1   0   0] * [vi wi vj wj]
+    //
+    // The last constraint gives
+    // GW = (vi - vj + wj x r) * that
+    //    = [  that   0  -that  (r x t) ]
+
+    var x =     new Equation(bodyA,bodyB,-maxForce,maxForce),
+        y =     new Equation(bodyA,bodyB,-maxForce,maxForce),
+        rot =   new Equation(bodyA,bodyB,-maxForce,maxForce);
+
+    var l = vec2.create(),
+        g = vec2.create(),
+        that = this;
+    x.computeGq = function(){
+        vec2.rotate(l, that.localOffsetB, bodyA.angle);
+        vec2.sub(g, bodyB.position, bodyA.position);
+        vec2.sub(g, g, l);
+        return g[0];
+    };
+    y.computeGq = function(){
+        vec2.rotate(l, that.localOffsetB, bodyA.angle);
+        vec2.sub(g, bodyB.position, bodyA.position);
+        vec2.sub(g, g, l);
+        return g[1];
+    };
+    var r = vec2.create(),
+        t = vec2.create();
+    rot.computeGq = function(){
+        vec2.rotate(r, that.localOffsetB, bodyB.angle - that.localAngleB);
+        vec2.scale(r,r,-1);
+        vec2.sub(g,bodyA.position,bodyB.position);
+        vec2.add(g,g,r);
+        vec2.rotate(t,r,-Math.PI/2);
+        vec2.normalize(t,t);
+        return vec2.dot(g,t);
+    };
+
+    /**
+     * The offset of bodyB in bodyA's frame.
+     * @property {Array} localOffsetB
+     */
+    this.localOffsetB = vec2.create();
+    if(options.localOffsetB){
+        vec2.copy(this.localOffsetB, options.localOffsetB);
+    } else {
+        // Construct from current positions
+        vec2.sub(this.localOffsetB, bodyB.position, bodyA.position);
+        vec2.rotate(this.localOffsetB, this.localOffsetB, -bodyA.angle);
+    }
+
+    /**
+     * The offset angle of bodyB in bodyA's frame.
+     * @property {Number} localAngleB
+     */
+    this.localAngleB = 0;
+    if(typeof(options.localAngleB) === 'number'){
+        this.localAngleB = options.localAngleB;
+    } else {
+        // Construct
+        this.localAngleB = bodyB.angle - bodyA.angle;
+    }
+
+    this.equations.push(x, y, rot);
+    this.setMaxForce(maxForce);
+}
+LockConstraint.prototype = new Constraint();
+LockConstraint.prototype.constructor = LockConstraint;
+
+/**
+ * Set the maximum force to be applied.
+ * @method setMaxForce
+ * @param {Number} force
+ */
+LockConstraint.prototype.setMaxForce = function(force){
+    var eqs = this.equations;
+    for(var i=0; i<this.equations.length; i++){
+        eqs[i].maxForce =  force;
+        eqs[i].minForce = -force;
+    }
+};
+
+/**
+ * Get the max force.
+ * @method getMaxForce
+ * @return {Number}
+ */
+LockConstraint.prototype.getMaxForce = function(){
+    return this.equations[0].maxForce;
+};
+
+var l = vec2.create();
+var r = vec2.create();
+var t = vec2.create();
+var xAxis = vec2.fromValues(1,0);
+var yAxis = vec2.fromValues(0,1);
+LockConstraint.prototype.update = function(){
+    var x =   this.equations[0],
+        y =   this.equations[1],
+        rot = this.equations[2],
+        bodyA = this.bodyA,
+        bodyB = this.bodyB;
+
+    vec2.rotate(l,this.localOffsetB,bodyA.angle);
+    vec2.rotate(r,this.localOffsetB,bodyB.angle - this.localAngleB);
+    vec2.scale(r,r,-1);
+
+    vec2.rotate(t,r,Math.PI/2);
+    vec2.normalize(t,t);
+
+    x.G[0] = -1;
+    x.G[1] =  0;
+    x.G[2] = -vec2.crossLength(l,xAxis);
+    x.G[3] =  1;
+
+    y.G[0] =  0;
+    y.G[1] = -1;
+    y.G[2] = -vec2.crossLength(l,yAxis);
+    y.G[4] =  1;
+
+    rot.G[0] =  -t[0];
+    rot.G[1] =  -t[1];
+    rot.G[3] =  t[0];
+    rot.G[4] =  t[1];
+    rot.G[5] =  vec2.crossLength(r,t);
+};

src/constraints/PrismaticConstraint.js

+var Constraint = require('./Constraint')
+,   ContactEquation = require('../equations/ContactEquation')
+,   Equation = require('../equations/Equation')
+,   vec2 = require('../math/vec2')
+,   RotationalLockEquation = require('../equations/RotationalLockEquation');
+
+module.exports = PrismaticConstraint;
+
+/**
+ * Constraint that only allows bodies to move along a line, relative to each other. See <a href="http://www.iforce2d.net/b2dtut/joints-prismatic">this tutorial</a>. Also called "slider constraint".
+ *
+ * @class PrismaticConstraint
+ * @constructor
+ * @extends Constraint
+ * @author schteppe
+ * @param {Body}    bodyA
+ * @param {Body}    bodyB
+ * @param {Object}  [options]
+ * @param {Number}  [options.maxForce]                Max force to be applied by the constraint
+ * @param {Array}   [options.localAnchorA]            Body A's anchor point, defined in its own local frame.
+ * @param {Array}   [options.localAnchorB]            Body B's anchor point, defined in its own local frame.
+ * @param {Array}   [options.localAxisA]              An axis, defined in body A frame, that body B's anchor point may slide along.
+ * @param {Boolean} [options.disableRotationalLock]   If set to true, bodyB will be free to rotate around its anchor point.
+ * @param {Number}  [options.upperLimit]
+ * @param {Number}  [options.lowerLimit]
+ * @todo Ability to create using only a point and a worldAxis
+ */
+function PrismaticConstraint(bodyA, bodyB, options){
+    options = options || {};
+    Constraint.call(this,bodyA,bodyB,Constraint.PRISMATIC,options);
+
+    // Get anchors
+    var localAnchorA = vec2.fromValues(0,0),
+        localAxisA = vec2.fromValues(1,0),
+        localAnchorB = vec2.fromValues(0,0);
+    if(options.localAnchorA){ vec2.copy(localAnchorA, options.localAnchorA); }
+    if(options.localAxisA){ vec2.copy(localAxisA,   options.localAxisA); }
+    if(options.localAnchorB){ vec2.copy(localAnchorB, options.localAnchorB); }
+
+    /**
+     * @property localAnchorA
+     * @type {Array}
+     */
+    this.localAnchorA = localAnchorA;
+
+    /**
+     * @property localAnchorB
+     * @type {Array}
+     */
+    this.localAnchorB = localAnchorB;
+
+    /**
+     * @property localAxisA
+     * @type {Array}
+     */
+    this.localAxisA = localAxisA;
+
+    /*
+
+    The constraint violation for the common axis point is
+
+        g = ( xj + rj - xi - ri ) * t   :=  gg*t
+
+    where r are body-local anchor points, and t is a tangent to the constraint axis defined in body i frame.
+
+        gdot =  ( vj + wj x rj - vi - wi x ri ) * t + ( xj + rj - xi - ri ) * ( wi x t )
+
+    Note the use of the chain rule. Now we identify the jacobian
+
+        G*W = [ -t      -ri x t + t x gg     t    rj x t ] * [vi wi vj wj]
+
+    The rotational part is just a rotation lock.
+
+     */
+
+    var maxForce = this.maxForce = typeof(options.maxForce)!=="undefined" ? options.maxForce : Number.MAX_VALUE;
+
+    // Translational part
+    var trans = new Equation(bodyA,bodyB,-maxForce,maxForce);
+    var ri = new vec2.create(),
+        rj = new vec2.create(),
+        gg = new vec2.create(),
+        t =  new vec2.create();
+    trans.computeGq = function(){
+        // g = ( xj + rj - xi - ri ) * t
+        return vec2.dot(gg,t);
+    };
+    trans.updateJacobian = function(){
+        var G = this.G,
+            xi = bodyA.position,
+            xj = bodyB.position;
+        vec2.rotate(ri,localAnchorA,bodyA.angle);
+        vec2.rotate(rj,localAnchorB,bodyB.angle);
+        vec2.add(gg,xj,rj);
+        vec2.sub(gg,gg,xi);
+        vec2.sub(gg,gg,ri);
+        vec2.rotate(t,localAxisA,bodyA.angle+Math.PI/2);
+
+        G[0] = -t[0];
+        G[1] = -t[1];
+        G[2] = -vec2.crossLength(ri,t) + vec2.crossLength(t,gg);
+        G[3] = t[0];
+        G[4] = t[1];
+        G[5] = vec2.crossLength(rj,t);
+    };
+    this.equations.push(trans);
+
+    // Rotational part
+    if(!options.disableRotationalLock){
+        var rot = new RotationalLockEquation(bodyA,bodyB,-maxForce,maxForce);
+        this.equations.push(rot);
+    }
+
+    /**
+     * The position of anchor A relative to anchor B, along the constraint axis.
+     * @property position
+     * @type {Number}
+     */
+    this.position = 0;
+
+    // Is this one used at all?
+    this.velocity = 0;
+
+    /**
+     * Set to true to enable lower limit.
+     * @property lowerLimitEnabled
+     * @type {Boolean}
+     */
+    this.lowerLimitEnabled = typeof(options.lowerLimit)!=="undefined" ? true : false;
+
+    /**
+     * Set to true to enable upper limit.
+     * @property upperLimitEnabled
+     * @type {Boolean}
+     */
+    this.upperLimitEnabled = typeof(options.upperLimit)!=="undefined" ? true : false;
+
+    /**
+     * Lower constraint limit. The constraint position is forced to be larger than this value.
+     * @property lowerLimit
+     * @type {Number}
+     */
+    this.lowerLimit = typeof(options.lowerLimit)!=="undefined" ? options.lowerLimit : 0;
+
+    /**
+     * Upper constraint limit. The constraint position is forced to be smaller than this value.
+     * @property upperLimit
+     * @type {Number}
+     */
+    this.upperLimit = typeof(options.upperLimit)!=="undefined" ? options.upperLimit : 1;
+
+    // Equations used for limits
+    this.upperLimitEquation = new ContactEquation(bodyA,bodyB);
+    this.lowerLimitEquation = new ContactEquation(bodyA,bodyB);
+
+    // Set max/min forces
+    this.upperLimitEquation.minForce = this.lowerLimitEquation.minForce = 0;
+    this.upperLimitEquation.maxForce = this.lowerLimitEquation.maxForce = maxForce;
+
+    /**
+     * Equation used for the motor.
+     * @property motorEquation
+     * @type {Equation}
+     */
+    this.motorEquation = new Equation(bodyA,bodyB);
+
+    /**
+     * The current motor state. Enable or disable the motor using .enableMotor
+     * @property motorEnabled
+     * @type {Boolean}
+     */
+    this.motorEnabled = false;
+
+    /**
+     * Set the target speed for the motor.
+     * @property motorSpeed
+     * @type {Number}
+     */
+    this.motorSpeed = 0;
+
+    var that = this;
+    var motorEquation = this.motorEquation;
+    var old = motorEquation.computeGW;
+    motorEquation.computeGq = function(){ return 0; };
+    motorEquation.computeGW = function(){
+        var G = this.G,
+            bi = this.bodyA,
+            bj = this.bodyB,
+            vi = bi.velocity,
+            vj = bj.velocity,
+            wi = bi.angularVelocity,
+            wj = bj.angularVelocity;
+        return this.gmult(G,vi,wi,vj,wj) + that.motorSpeed;
+    };
+}
+
+PrismaticConstraint.prototype = new Constraint();
+PrismaticConstraint.prototype.constructor = PrismaticConstraint;
+
+var worldAxisA = vec2.create(),
+    worldAnchorA = vec2.create(),
+    worldAnchorB = vec2.create(),
+    orientedAnchorA = vec2.create(),
+    orientedAnchorB = vec2.create(),
+    tmp = vec2.create();
+
+/**
+ * Update the constraint equations. Should be done if any of the bodies changed position, before solving.
+ * @method update
+ */
+PrismaticConstraint.prototype.update = function(){
+    var eqs = this.equations,
+        trans = eqs[0],
+        upperLimit = this.upperLimit,
+        lowerLimit = this.lowerLimit,
+        upperLimitEquation = this.upperLimitEquation,
+        lowerLimitEquation = this.lowerLimitEquation,
+        bodyA = this.bodyA,
+        bodyB = this.bodyB,
+        localAxisA = this.localAxisA,
+        localAnchorA = this.localAnchorA,
+        localAnchorB = this.localAnchorB;
+
+    trans.updateJacobian();
+
+    // Transform local things to world
+    vec2.rotate(worldAxisA,      localAxisA,      bodyA.angle);
+    vec2.rotate(orientedAnchorA, localAnchorA,    bodyA.angle);
+    vec2.add(worldAnchorA,       orientedAnchorA, bodyA.position);
+    vec2.rotate(orientedAnchorB, localAnchorB,    bodyB.angle);
+    vec2.add(worldAnchorB,       orientedAnchorB, bodyB.position);
+
+    var relPosition = this.position = vec2.dot(worldAnchorB,worldAxisA) - vec2.dot(worldAnchorA,worldAxisA);
+
+    // Motor
+    if(this.motorEnabled){
+        // G = [ a     a x ri   -a   -a x rj ]
+        var G = this.motorEquation.G;
+        G[0] = worldAxisA[0];
+        G[1] = worldAxisA[1];
+        G[2] = vec2.crossLength(worldAxisA,orientedAnchorB);
+        G[3] = -worldAxisA[0];
+        G[4] = -worldAxisA[1];
+        G[5] = -vec2.crossLength(worldAxisA,orientedAnchorA);
+    }
+
+    /*
+        Limits strategy:
+        Add contact equation, with normal along the constraint axis.
+        min/maxForce is set so the constraint is repulsive in the correct direction.
+        Some offset is added to either equation.contactPointA or .contactPointB to get the correct upper/lower limit.
+
+                 ^
+                 |
+      upperLimit x
+                 |    ------
+         anchorB x<---|  B |
+                 |    |    |
+        ------   |    ------
+        |    |   |
+        |  A |-->x anchorA
+        ------   |
+                 x lowerLimit
+                 |
+                axis
+     */
+
+
+    if(this.upperLimitEnabled && relPosition > upperLimit){
+        // Update contact constraint normal, etc
+        vec2.scale(upperLimitEquation.normalA, worldAxisA, -1);
+        vec2.sub(upperLimitEquation.contactPointA, worldAnchorA, bodyA.position);
+        vec2.sub(upperLimitEquation.contactPointB, worldAnchorB, bodyB.position);
+        vec2.scale(tmp,worldAxisA,upperLimit);
+        vec2.add(upperLimitEquation.contactPointA,upperLimitEquation.contactPointA,tmp);
+        if(eqs.indexOf(upperLimitEquation) === -1){
+            eqs.push(upperLimitEquation);
+        }
+    } else {
+        var idx = eqs.indexOf(upperLimitEquation);
+        if(idx !== -1){
+            eqs.splice(idx,1);
+        }
+    }
+
+    if(this.lowerLimitEnabled && relPosition < lowerLimit){
+        // Update contact constraint normal, etc
+        vec2.scale(lowerLimitEquation.normalA, worldAxisA, 1);
+        vec2.sub(lowerLimitEquation.contactPointA, worldAnchorA, bodyA.position);
+        vec2.sub(lowerLimitEquation.contactPointB, worldAnchorB, bodyB.position);
+        vec2.scale(tmp,worldAxisA,lowerLimit);
+        vec2.sub(lowerLimitEquation.contactPointB,lowerLimitEquation.contactPointB,tmp);
+        if(eqs.indexOf(lowerLimitEquation) === -1){
+            eqs.push(lowerLimitEquation);
+        }
+    } else {
+        var idx = eqs.indexOf(lowerLimitEquation);
+        if(idx !== -1){
+            eqs.splice(idx,1);
+        }
+    }
+};
+
+/**
+ * Enable the motor
+ * @method enableMotor
+ */
+PrismaticConstraint.prototype.enableMotor = function(){
+    if(this.motorEnabled){
+        return;
+    }
+    this.equations.push(this.motorEquation);
+    this.motorEnabled = true;
+};
+
+/**
+ * Disable the rotational motor
+ * @method disableMotor
+ */
+PrismaticConstraint.prototype.disableMotor = function(){
+    if(!this.motorEnabled){
+        return;
+    }
+    var i = this.equations.indexOf(this.motorEquation);
+    this.equations.splice(i,1);
+    this.motorEnabled = false;
+};
+
+/**
+ * Set the constraint limits.
+ * @method setLimits
+ * @param {number} lower Lower limit.
+ * @param {number} upper Upper limit.
+ */
+PrismaticConstraint.prototype.setLimits = function (lower, upper) {
+    if(typeof(lower) === 'number'){
+        this.lowerLimit = lower;
+        this.lowerLimitEnabled = true;
+    } else {
+        this.lowerLimit = lower;
+        this.lowerLimitEnabled = false;
+    }
+
+    if(typeof(upper) === 'number'){
+        this.upperLimit = upper;
+        this.upperLimitEnabled = true;
+    } else {
+        this.upperLimit = upper;
+        this.upperLimitEnabled = false;
+    }
+};
+

src/constraints/RevoluteConstraint.js

+var Constraint = require('./Constraint')
+,   Equation = require('../equations/Equation')
+,   RotationalVelocityEquation = require('../equations/RotationalVelocityEquation')
+,   RotationalLockEquation = require('../equations/RotationalLockEquation')
+,   vec2 = require('../math/vec2');
+
+module.exports = RevoluteConstraint;
+
+var worldPivotA = vec2.create(),
+    worldPivotB = vec2.create(),
+    xAxis = vec2.fromValues(1,0),
+    yAxis = vec2.fromValues(0,1),
+    g = vec2.create();
+
+/**
+ * Connects two bodies at given offset points, letting them rotate relative to each other around this point.
+ * @class RevoluteConstraint
+ * @constructor
+ * @author schteppe
+ * @param {Body}    bodyA
+ * @param {Body}    bodyB
+ * @param {Object}  [options]
+ * @param {Array}   [options.worldPivot] A pivot point given in world coordinates. If specified, localPivotA and localPivotB are automatically computed from this value.
+ * @param {Array}   [options.localPivotA] The point relative to the center of mass of bodyA which bodyA is constrained to.
+ * @param {Array}   [options.localPivotB] See localPivotA.
+ * @param {Number}  [options.maxForce] The maximum force that should be applied to constrain the bodies.
+ * @extends Constraint
+ *
+ * @example
+ *     // This will create a revolute constraint between two bodies with pivot point in between them.
+ *     var bodyA = new Body({ mass: 1, position: [-1, 0] });
+ *     var bodyB = new Body({ mass: 1, position: [1, 0] });
+ *     var constraint = new RevoluteConstraint(bodyA, bodyB, {
+ *         worldPivot: [0, 0]
+ *     });
+ *     world.addConstraint(constraint);
+ *
+ *     // Using body-local pivot points, the constraint could have been constructed like this:
+ *     var constraint = new RevoluteConstraint(bodyA, bodyB, {
+ *         localPivotA: [1, 0],
+ *         localPivotB: [-1, 0]
+ *     });
+ */
+function RevoluteConstraint(bodyA, bodyB, options){
+    options = options || {};
+    Constraint.call(this,bodyA,bodyB,Constraint.REVOLUTE,options);
+
+    var maxForce = this.maxForce = typeof(options.maxForce) !== "undefined" ? options.maxForce : Number.MAX_VALUE;
+
+    /**
+     * @property {Array} pivotA
+     */
+    this.pivotA = vec2.create();
+
+    /**
+     * @property {Array} pivotB
+     */
+    this.pivotB = vec2.create();
+
+    if(options.worldPivot){
+        // Compute pivotA and pivotB
+        vec2.sub(this.pivotA, options.worldPivot, bodyA.position);
+        vec2.sub(this.pivotB, options.worldPivot, bodyB.position);
+        // Rotate to local coordinate system
+        vec2.rotate(this.pivotA, this.pivotA, -bodyA.angle);
+        vec2.rotate(this.pivotB, this.pivotB, -bodyB.angle);
+    } else {
+        // Get pivotA and pivotB
+        vec2.copy(this.pivotA, options.localPivotA);
+        vec2.copy(this.pivotB, options.localPivotB);
+    }
+
+    // Equations to be fed to the solver
+    var eqs = this.equations = [
+        new Equation(bodyA,bodyB,-maxForce,maxForce),
+        new Equation(bodyA,bodyB,-maxForce,maxForce),
+    ];
+
+    var x = eqs[0];
+    var y = eqs[1];
+    var that = this;
+
+    x.computeGq = function(){
+        vec2.rotate(worldPivotA, that.pivotA, bodyA.angle);
+        vec2.rotate(worldPivotB, that.pivotB, bodyB.angle);
+        vec2.add(g, bodyB.position, worldPivotB);
+        vec2.sub(g, g, bodyA.position);
+        vec2.sub(g, g, worldPivotA);
+        return vec2.dot(g,xAxis);
+    };
+
+    y.computeGq = function(){
+        vec2.rotate(worldPivotA, that.pivotA, bodyA.angle);
+        vec2.rotate(worldPivotB, that.pivotB, bodyB.angle);
+        vec2.add(g, bodyB.position, worldPivotB);
+        vec2.sub(g, g, bodyA.position);
+        vec2.sub(g, g, worldPivotA);
+        return vec2.dot(g,yAxis);
+    };
+
+    y.minForce = x.minForce = -maxForce;
+    y.maxForce = x.maxForce =  maxForce;
+
+    this.motorEquation = new RotationalVelocityEquation(bodyA,bodyB);
+
+    /**
+     * Indicates whether the motor is enabled. Use .enableMotor() to enable the constraint motor.
+     * @property {Boolean} motorEnabled
+     * @readOnly
+     */
+    this.motorEnabled = false;
+
+    /**
+     * The constraint position.
+     * @property angle
+     * @type {Number}
+     * @readOnly
+     */
+    this.angle = 0;
+
+    /**
+     * Set to true to enable lower limit
+     * @property lowerLimitEnabled
+     * @type {Boolean}
+     */
+    this.lowerLimitEnabled = false;
+
+    /**
+     * Set to true to enable upper limit
+     * @property upperLimitEnabled
+     * @type {Boolean}
+     */
+    this.upperLimitEnabled = false;
+
+    /**
+     * The lower limit on the constraint angle.
+     * @property lowerLimit
+     * @type {Boolean}
+     */
+    this.lowerLimit = 0;
+
+    /**
+     * The upper limit on the constraint angle.
+     * @property upperLimit
+     * @type {Boolean}
+     */
+    this.upperLimit = 0;
+
+    this.upperLimitEquation = new RotationalLockEquation(bodyA,bodyB);
+    this.lowerLimitEquation = new RotationalLockEquation(bodyA,bodyB);
+    this.upperLimitEquation.minForce = 0;
+    this.lowerLimitEquation.maxForce = 0;
+}
+RevoluteConstraint.prototype = new Constraint();
+RevoluteConstraint.prototype.constructor = RevoluteConstraint;
+
+/**
+ * Set the constraint angle limits.
+ * @method setLimits
+ * @param {number} lower Lower angle limit.
+ * @param {number} upper Upper angle limit.
+ */
+RevoluteConstraint.prototype.setLimits = function (lower, upper) {
+    if(typeof(lower) === 'number'){
+        this.lowerLimit = lower;
+        this.lowerLimitEnabled = true;
+    } else {
+        this.lowerLimit = lower;
+        this.lowerLimitEnabled = false;
+    }
+
+    if(typeof(upper) === 'number'){
+        this.upperLimit = upper;
+        this.upperLimitEnabled = true;
+    } else {
+        this.upperLimit = upper;
+        this.upperLimitEnabled = false;
+    }
+};
+
+RevoluteConstraint.prototype.update = function(){
+    var bodyA =  this.bodyA,
+        bodyB =  this.bodyB,
+        pivotA = this.pivotA,
+        pivotB = this.pivotB,
+        eqs =    this.equations,
+        normal = eqs[0],
+        tangent= eqs[1],
+        x = eqs[0],
+        y = eqs[1],
+        upperLimit = this.upperLimit,
+        lowerLimit = this.lowerLimit,
+        upperLimitEquation = this.upperLimitEquation,
+        lowerLimitEquation = this.lowerLimitEquation;
+
+    var relAngle = this.angle = bodyB.angle - bodyA.angle;
+
+    if(this.upperLimitEnabled && relAngle > upperLimit){
+        upperLimitEquation.angle = upperLimit;
+        if(eqs.indexOf(upperLimitEquation) === -1){
+            eqs.push(upperLimitEquation);
+        }
+    } else {
+        var idx = eqs.indexOf(upperLimitEquation);
+        if(idx !== -1){
+            eqs.splice(idx,1);
+        }
+    }
+
+    if(this.lowerLimitEnabled && relAngle < lowerLimit){
+        lowerLimitEquation.angle = lowerLimit;
+        if(eqs.indexOf(lowerLimitEquation) === -1){
+            eqs.push(lowerLimitEquation);
+        }
+    } else {
+        var idx = eqs.indexOf(lowerLimitEquation);
+        if(idx !== -1){
+            eqs.splice(idx,1);
+        }
+    }
+
+    /*
+
+    The constraint violation is
+
+        g = xj + rj - xi - ri
+
+    ...where xi and xj are the body positions and ri and rj world-oriented offset vectors. Differentiate:
+
+        gdot = vj + wj x rj - vi - wi x ri
+
+    We split this into x and y directions. (let x and y be unit vectors along the respective axes)
+
+        gdot * x = ( vj + wj x rj - vi - wi x ri ) * x
+                 = ( vj*x + (wj x rj)*x -vi*x -(wi x ri)*x
+                 = ( vj*x + (rj x x)*wj -vi*x -(ri x x)*wi
+                 = [ -x   -(ri x x)   x   (rj x x)] * [vi wi vj wj]
+                 = G*W
+
+    ...and similar for y. We have then identified the jacobian entries for x and y directions:
+
+        Gx = [ x   (rj x x)   -x   -(ri x x)]
+        Gy = [ y   (rj x y)   -y   -(ri x y)]
+
+     */
+
+    vec2.rotate(worldPivotA, pivotA, bodyA.angle);
+    vec2.rotate(worldPivotB, pivotB, bodyB.angle);
+
+    // todo: these are a bit sparse. We could save some computations on making custom eq.computeGW functions, etc
+
+    x.G[0] = -1;
+    x.G[1] =  0;
+    x.G[2] = -vec2.crossLength(worldPivotA,xAxis);
+    x.G[3] =  1;
+    x.G[4] =  0;
+    x.G[5] =  vec2.crossLength(worldPivotB,xAxis);
+
+    y.G[0] =  0;
+    y.G[1] = -1;
+    y.G[2] = -vec2.crossLength(worldPivotA,yAxis);
+    y.G[3] =  0;
+    y.G[4] =  1;
+    y.G[5] =  vec2.crossLength(worldPivotB,yAxis);
+};
+
+/**
+ * Enable the rotational motor
+ * @method enableMotor
+ */
+RevoluteConstraint.prototype.enableMotor = function(){
+    if(this.motorEnabled){
+        return;
+    }
+    this.equations.push(this.motorEquation);
+    this.motorEnabled = true;
+};
+
+/**
+ * Disable the rotational motor
+ * @method disableMotor
+ */
+RevoluteConstraint.prototype.disableMotor = function(){
+    if(!this.motorEnabled){
+        return;
+    }
+    var i = this.equations.indexOf(this.motorEquation);
+    this.equations.splice(i,1);
+    this.motorEnabled = false;
+};
+
+/**
+ * Check if the motor is enabled.
+ * @method motorIsEnabled
+ * @deprecated use property motorEnabled instead.
+ * @return {Boolean}
+ */
+RevoluteConstraint.prototype.motorIsEnabled = function(){
+    return !!this.motorEnabled;
+};
+
+/**
+ * Set the speed of the rotational constraint motor
+ * @method setMotorSpeed
+ * @param  {Number} speed
+ */
+RevoluteConstraint.prototype.setMotorSpeed = function(speed){
+    if(!this.motorEnabled){
+        return;
+    }
+    var i = this.equations.indexOf(this.motorEquation);
+    this.equations[i].relativeVelocity = speed;
+};
+
+/**
+ * Get the speed of the rotational constraint motor
+ * @method getMotorSpeed
+ * @return {Number} The current speed, or false if the motor is not enabled.
+ */
+RevoluteConstraint.prototype.getMotorSpeed = function(){
+    if(!this.motorEnabled){
+        return false;
+    }
+    return this.motorEquation.relativeVelocity;
+};

src/equations/AngleLockEquation.js

+var Equation = require("./Equation"),
+    vec2 = require('../math/vec2');
+
+module.exports = AngleLockEquation;
+
+/**
+ * Locks the relative angle between two bodies. The constraint tries to keep the dot product between two vectors, local in each body, to zero. The local angle in body i is a parameter.
+ *
+ * @class AngleLockEquation
+ * @constructor
+ * @extends Equation
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Object} [options]
+ * @param {Number} [options.angle] Angle to add to the local vector in body A.
+ * @param {Number} [options.ratio] Gear ratio
+ */
+function AngleLockEquation(bodyA, bodyB, options){
+    options = options || {};
+    Equation.call(this,bodyA,bodyB,-Number.MAX_VALUE,Number.MAX_VALUE);
+    this.angle = options.angle || 0;
+
+    /**
+     * The gear ratio.
+     * @property {Number} ratio
+     * @private
+     * @see setRatio
+     */
+    this.ratio = typeof(options.ratio)==="number" ? options.ratio : 1;
+
+    this.setRatio(this.ratio);
+}
+AngleLockEquation.prototype = new Equation();
+AngleLockEquation.prototype.constructor = AngleLockEquation;
+
+AngleLockEquation.prototype.computeGq = function(){
+    return this.ratio * this.bodyA.angle - this.bodyB.angle + this.angle;
+};
+
+/**
+ * Set the gear ratio for this equation
+ * @method setRatio
+ * @param {Number} ratio
+ */
+AngleLockEquation.prototype.setRatio = function(ratio){
+    var G = this.G;
+    G[2] =  ratio;
+    G[5] = -1;
+    this.ratio = ratio;
+};
+
+/**
+ * Set the max force for the equation.
+ * @method setMaxTorque
+ * @param {Number} torque
+ */
+AngleLockEquation.prototype.setMaxTorque = function(torque){
+    this.maxForce =  torque;
+    this.minForce = -torque;
+};

src/equations/ContactEquation.js

+var Equation = require("./Equation"),
+    vec2 = require('../math/vec2');
+
+module.exports = ContactEquation;
+
+/**
+ * Non-penetration constraint equation. Tries to make the contactPointA and contactPointB vectors coincide, while keeping the applied force repulsive.
+ *
+ * @class ContactEquation
+ * @constructor
+ * @extends Equation
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ */
+function ContactEquation(bodyA, bodyB){
+    Equation.call(this, bodyA, bodyB, 0, Number.MAX_VALUE);
+
+    /**
+     * Vector from body i center of mass to the contact point.
+     * @property contactPointA
+     * @type {Array}
+     */
+    this.contactPointA = vec2.create();
+    this.penetrationVec = vec2.create();
+
+    /**
+     * World-oriented vector from body A center of mass to the contact point.
+     * @property contactPointB
+     * @type {Array}
+     */
+    this.contactPointB = vec2.create();
+
+    /**
+     * The normal vector, pointing out of body i
+     * @property normalA
+     * @type {Array}
+     */
+    this.normalA = vec2.create();
+
+    /**
+     * The restitution to use (0=no bounciness, 1=max bounciness).
+     * @property restitution
+     * @type {Number}
+     */
+    this.restitution = 0;
+
+    /**
+     * This property is set to true if this is the first impact between the bodies (not persistant contact).
+     * @property firstImpact
+     * @type {Boolean}
+     * @readOnly
+     */
+    this.firstImpact = false;
+
+    /**
+     * The shape in body i that triggered this contact.
+     * @property shapeA
+     * @type {Shape}
+     */
+    this.shapeA = null;
+
+    /**
+     * The shape in body j that triggered this contact.
+     * @property shapeB
+     * @type {Shape}
+     */
+    this.shapeB = null;
+}
+ContactEquation.prototype = new Equation();
+ContactEquation.prototype.constructor = ContactEquation;
+ContactEquation.prototype.computeB = function(a,b,h){
+    var bi = this.bodyA,
+        bj = this.bodyB,
+        ri = this.contactPointA,
+        rj = this.contactPointB,
+        xi = bi.position,
+        xj = bj.position;
+
+    var penetrationVec = this.penetrationVec,
+        n = this.normalA,
+        G = this.G;
+
+    // Caluclate cross products
+    var rixn = vec2.crossLength(ri,n),
+        rjxn = vec2.crossLength(rj,n);
+
+    // G = [-n -rixn n rjxn]
+    G[0] = -n[0];
+    G[1] = -n[1];
+    G[2] = -rixn;
+    G[3] = n[0];
+    G[4] = n[1];
+    G[5] = rjxn;
+
+    // Calculate q = xj+rj -(xi+ri) i.e. the penetration vector
+    vec2.add(penetrationVec,xj,rj);
+    vec2.sub(penetrationVec,penetrationVec,xi);
+    vec2.sub(penetrationVec,penetrationVec,ri);
+
+    // Compute iteration
+    var GW, Gq;
+    if(this.firstImpact && this.restitution !== 0){
+        Gq = 0;
+        GW = (1/b)*(1+this.restitution) * this.computeGW();
+    } else {
+        Gq = vec2.dot(n,penetrationVec) + this.offset;
+        GW = this.computeGW();
+    }
+
+    var GiMf = this.computeGiMf();
+    var B = - Gq * a - GW * b - h*GiMf;
+
+    return B;
+};
+
+var vi = vec2.create();
+var vj = vec2.create();
+var relVel = vec2.create();
+
+/**
+ * Get the relative velocity along the normal vector.
+ * @return {number}
+ */
+ContactEquation.prototype.getVelocityAlongNormal = function(){
+
+    this.bodyA.getVelocityAtPoint(vi, this.contactPointA);
+    this.bodyB.getVelocityAtPoint(vj, this.contactPointB);
+
+    vec2.subtract(relVel, vi, vj);
+
+    return vec2.dot(this.normalA, relVel);
+};
\ No newline at end of file

src/equations/Equation.js

+module.exports = Equation;
+
+var vec2 = require('../math/vec2'),
+    Utils = require('../utils/Utils'),
+    Body = require('../objects/Body');
+
+/**
+ * Base class for constraint equations.
+ * @class Equation
+ * @constructor
+ * @param {Body} bodyA First body participating in the equation
+ * @param {Body} bodyB Second body participating in the equation
+ * @param {number} minForce Minimum force to apply. Default: -Number.MAX_VALUE
+ * @param {number} maxForce Maximum force to apply. Default: Number.MAX_VALUE
+ */
+function Equation(bodyA, bodyB, minForce, maxForce){
+
+    /**
+     * Minimum force to apply when solving.
+     * @property minForce
+     * @type {Number}
+     */
+    this.minForce = typeof(minForce)==="undefined" ? -Number.MAX_VALUE : minForce;
+
+    /**
+     * Max force to apply when solving.
+     * @property maxForce
+     * @type {Number}
+     */
+    this.maxForce = typeof(maxForce)==="undefined" ? Number.MAX_VALUE : maxForce;
+
+    /**
+     * First body participating in the constraint
+     * @property bodyA
+     * @type {Body}
+     */
+    this.bodyA = bodyA;
+
+    /**
+     * Second body participating in the constraint
+     * @property bodyB
+     * @type {Body}
+     */
+    this.bodyB = bodyB;
+
+    /**
+     * The stiffness of this equation. Typically chosen to a large number (~1e7), but can be chosen somewhat freely to get a stable simulation.
+     * @property stiffness
+     * @type {Number}
+     */
+    this.stiffness = Equation.DEFAULT_STIFFNESS;
+
+    /**
+     * The number of time steps needed to stabilize the constraint equation. Typically between 3 and 5 time steps.
+     * @property relaxation
+     * @type {Number}
+     */
+    this.relaxation = Equation.DEFAULT_RELAXATION;
+
+    /**
+     * The Jacobian entry of this equation. 6 numbers, 3 per body (x,y,angle).
+     * @property G
+     * @type {Array}
+     */
+    this.G = new Utils.ARRAY_TYPE(6);
+    for(var i=0; i<6; i++){
+        this.G[i]=0;
+    }
+
+    this.offset = 0;
+
+    this.a = 0;
+    this.b = 0;
+    this.epsilon = 0;
+    this.timeStep = 1/60;
+
+    /**
+     * Indicates if stiffness or relaxation was changed.
+     * @property {Boolean} needsUpdate
+     */
+    this.needsUpdate = true;
+
+    /**
+     * The resulting constraint multiplier from the last solve. This is mostly equivalent to the force produced by the constraint.
+     * @property multiplier
+     * @type {Number}
+     */
+    this.multiplier = 0;
+
+    /**
+     * Relative velocity.
+     * @property {Number} relativeVelocity
+     */
+    this.relativeVelocity = 0;
+
+    /**
+     * Whether this equation is enabled or not. If true, it will be added to the solver.
+     * @property {Boolean} enabled
+     */
+    this.enabled = true;
+}
+Equation.prototype.constructor = Equation;
+
+/**
+ * The default stiffness when creating a new Equation.
+ * @static
+ * @property {Number} DEFAULT_STIFFNESS
+ * @default 1e6
+ */
+Equation.DEFAULT_STIFFNESS = 1e6;
+
+/**
+ * The default relaxation when creating a new Equation.
+ * @static
+ * @property {Number} DEFAULT_RELAXATION
+ * @default 4
+ */
+Equation.DEFAULT_RELAXATION = 4;
+
+/**
+ * Compute SPOOK parameters .a, .b and .epsilon according to the current parameters. See equations 9, 10 and 11 in the <a href="http://www8.cs.umu.se/kurser/5DV058/VT09/lectures/spooknotes.pdf">SPOOK notes</a>.
+ * @method update
+ */
+Equation.prototype.update = function(){
+    var k = this.stiffness,
+        d = this.relaxation,
+        h = this.timeStep;
+
+    this.a = 4.0 / (h * (1 + 4 * d));
+    this.b = (4.0 * d) / (1 + 4 * d);
+    this.epsilon = 4.0 / (h * h * k * (1 + 4 * d));
+
+    this.needsUpdate = false;
+};
+
+/**
+ * Multiply a jacobian entry with corresponding positions or velocities
+ * @method gmult
+ * @return {Number}
+ */
+Equation.prototype.gmult = function(G,vi,wi,vj,wj){
+    return  G[0] * vi[0] +
+            G[1] * vi[1] +
+            G[2] * wi +
+            G[3] * vj[0] +
+            G[4] * vj[1] +
+            G[5] * wj;
+};
+
+/**
+ * Computes the RHS of the SPOOK equation
+ * @method computeB
+ * @return {Number}
+ */
+Equation.prototype.computeB = function(a,b,h){
+    var GW = this.computeGW();
+    var Gq = this.computeGq();
+    var GiMf = this.computeGiMf();
+    return - Gq * a - GW * b - GiMf*h;
+};
+
+/**
+ * Computes G\*q, where q are the generalized body coordinates
+ * @method computeGq
+ * @return {Number}
+ */
+var qi = vec2.create(),
+    qj = vec2.create();
+Equation.prototype.computeGq = function(){
+    var G = this.G,
+        bi = this.bodyA,
+        bj = this.bodyB,
+        xi = bi.position,
+        xj = bj.position,
+        ai = bi.angle,
+        aj = bj.angle;
+
+    return this.gmult(G, qi, ai, qj, aj) + this.offset;
+};
+
+/**
+ * Computes G\*W, where W are the body velocities
+ * @method computeGW
+ * @return {Number}
+ */
+Equation.prototype.computeGW = function(){
+    var G = this.G,
+        bi = this.bodyA,
+        bj = this.bodyB,
+        vi = bi.velocity,
+        vj = bj.velocity,
+        wi = bi.angularVelocity,
+        wj = bj.angularVelocity;
+    return this.gmult(G,vi,wi,vj,wj) + this.relativeVelocity;
+};
+
+/**
+ * Computes G\*Wlambda, where W are the body velocities
+ * @method computeGWlambda
+ * @return {Number}
+ */
+Equation.prototype.computeGWlambda = function(){
+    var G = this.G,
+        bi = this.bodyA,
+        bj = this.bodyB,
+        vi = bi.vlambda,
+        vj = bj.vlambda,
+        wi = bi.wlambda,
+        wj = bj.wlambda;
+    return this.gmult(G,vi,wi,vj,wj);
+};
+
+/**
+ * Computes G\*inv(M)\*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies.
+ * @method computeGiMf
+ * @return {Number}
+ */
+var iMfi = vec2.create(),
+    iMfj = vec2.create();
+Equation.prototype.computeGiMf = function(){
+    var bi = this.bodyA,
+        bj = this.bodyB,
+        fi = bi.force,
+        ti = bi.angularForce,
+        fj = bj.force,
+        tj = bj.angularForce,
+        invMassi = bi.invMassSolve,
+        invMassj = bj.invMassSolve,
+        invIi = bi.invInertiaSolve,
+        invIj = bj.invInertiaSolve,
+        G = this.G;
+
+    vec2.scale(iMfi, fi, invMassi);
+    vec2.multiply(iMfi, bi.massMultiplier, iMfi);
+    vec2.scale(iMfj, fj,invMassj);
+    vec2.multiply(iMfj, bj.massMultiplier, iMfj);
+
+    return this.gmult(G,iMfi,ti*invIi,iMfj,tj*invIj);
+};
+
+/**
+ * Computes G\*inv(M)\*G'
+ * @method computeGiMGt
+ * @return {Number}
+ */
+Equation.prototype.computeGiMGt = function(){
+    var bi = this.bodyA,
+        bj = this.bodyB,
+        invMassi = bi.invMassSolve,
+        invMassj = bj.invMassSolve,
+        invIi = bi.invInertiaSolve,
+        invIj = bj.invInertiaSolve,
+        G = this.G;
+
+    return  G[0] * G[0] * invMassi * bi.massMultiplier[0] +
+            G[1] * G[1] * invMassi * bi.massMultiplier[1] +
+            G[2] * G[2] *    invIi +
+            G[3] * G[3] * invMassj * bj.massMultiplier[0] +
+            G[4] * G[4] * invMassj * bj.massMultiplier[1] +
+            G[5] * G[5] *    invIj;
+};
+
+var addToWlambda_temp = vec2.create(),
+    addToWlambda_Gi = vec2.create(),
+    addToWlambda_Gj = vec2.create(),
+    addToWlambda_ri = vec2.create(),
+    addToWlambda_rj = vec2.create(),
+    addToWlambda_Mdiag = vec2.create();
+
+/**
+ * Add constraint velocity to the bodies.
+ * @method addToWlambda
+ * @param {Number} deltalambda
+ */
+Equation.prototype.addToWlambda = function(deltalambda){
+    var bi = this.bodyA,
+        bj = this.bodyB,
+        temp = addToWlambda_temp,
+        Gi = addToWlambda_Gi,
+        Gj = addToWlambda_Gj,
+        ri = addToWlambda_ri,
+        rj = addToWlambda_rj,
+        invMassi = bi.invMassSolve,
+        invMassj = bj.invMassSolve,
+        invIi = bi.invInertiaSolve,
+        invIj = bj.invInertiaSolve,
+        Mdiag = addToWlambda_Mdiag,
+        G = this.G;
+
+    Gi[0] = G[0];
+    Gi[1] = G[1];
+    Gj[0] = G[3];
+    Gj[1] = G[4];
+
+    // Add to linear velocity
+    // v_lambda += inv(M) * delta_lamba * G
+    vec2.scale(temp, Gi, invMassi*deltalambda);
+    vec2.multiply(temp, temp, bi.massMultiplier);
+    vec2.add( bi.vlambda, bi.vlambda, temp);
+    // This impulse is in the offset frame
+    // Also add contribution to angular
+    //bi.wlambda -= vec2.crossLength(temp,ri);
+    bi.wlambda += invIi * G[2] * deltalambda;
+
+
+    vec2.scale(temp, Gj, invMassj*deltalambda);
+    vec2.multiply(temp, temp, bj.massMultiplier);
+    vec2.add( bj.vlambda, bj.vlambda, temp);
+    //bj.wlambda -= vec2.crossLength(temp,rj);
+    bj.wlambda += invIj * G[5] * deltalambda;
+};
+
+/**
+ * Compute the denominator part of the SPOOK equation: C = G\*inv(M)\*G' + eps
+ * @method computeInvC
+ * @param  {Number} eps
+ * @return {Number}
+ */
+Equation.prototype.computeInvC = function(eps){
+    return 1.0 / (this.computeGiMGt() + eps);
+};

src/equations/FrictionEquation.js

+var vec2 = require('../math/vec2')
+,   Equation = require('./Equation')
+,   Utils = require('../utils/Utils');
+
+module.exports = FrictionEquation;
+
+/**
+ * Constrains the slipping in a contact along a tangent
+ *
+ * @class FrictionEquation
+ * @constructor
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Number} slipForce
+ * @extends Equation
+ */
+function FrictionEquation(bodyA, bodyB, slipForce){
+    Equation.call(this, bodyA, bodyB, -slipForce, slipForce);
+
+    /**
+     * Relative vector from center of body A to the contact point, world oriented.
+     * @property contactPointA
+     * @type {Array}
+     */
+    this.contactPointA = vec2.create();
+
+    /**
+     * Relative vector from center of body B to the contact point, world oriented.
+     * @property contactPointB
+     * @type {Array}
+     */
+    this.contactPointB = vec2.create();
+
+    /**
+     * Tangent vector that the friction force will act along. World oriented.
+     * @property t
+     * @type {Array}
+     */
+    this.t = vec2.create();
+
+    /**
+     * ContactEquations connected to this friction equation. The contact equations can be used to rescale the max force for the friction. If more than one contact equation is given, then the max force can be set to the average.
+     * @property contactEquations
+     * @type {ContactEquation}
+     */
+    this.contactEquations = [];
+
+    /**
+     * The shape in body i that triggered this friction.
+     * @property shapeA
+     * @type {Shape}
+     * @todo Needed? The shape can be looked up via contactEquation.shapeA...
+     */
+    this.shapeA = null;
+
+    /**
+     * The shape in body j that triggered this friction.
+     * @property shapeB
+     * @type {Shape}
+     * @todo Needed? The shape can be looked up via contactEquation.shapeB...
+     */
+    this.shapeB = null;
+
+    /**
+     * The friction coefficient to use.
+     * @property frictionCoefficient
+     * @type {Number}
+     */
+    this.frictionCoefficient = 0.3;
+}
+FrictionEquation.prototype = new Equation();
+FrictionEquation.prototype.constructor = FrictionEquation;
+
+/**
+ * Set the slipping condition for the constraint. The friction force cannot be
+ * larger than this value.
+ * @method setSlipForce
+ * @param  {Number} slipForce
+ */
+FrictionEquation.prototype.setSlipForce = function(slipForce){
+    this.maxForce = slipForce;
+    this.minForce = -slipForce;
+};
+
+/**
+ * Get the max force for the constraint.
+ * @method getSlipForce
+ * @return {Number}
+ */
+FrictionEquation.prototype.getSlipForce = function(){
+    return this.maxForce;
+};
+
+FrictionEquation.prototype.computeB = function(a,b,h){
+    var bi = this.bodyA,
+        bj = this.bodyB,
+        ri = this.contactPointA,
+        rj = this.contactPointB,
+        t = this.t,
+        G = this.G;
+
+    // G = [-t -rixt t rjxt]
+    // And remember, this is a pure velocity constraint, g is always zero!
+    G[0] = -t[0];
+    G[1] = -t[1];
+    G[2] = -vec2.crossLength(ri,t);
+    G[3] = t[0];
+    G[4] = t[1];
+    G[5] = vec2.crossLength(rj,t);
+
+    var GW = this.computeGW(),
+        GiMf = this.computeGiMf();
+
+    var B = /* - g * a  */ - GW * b - h*GiMf;
+
+    return B;
+};

src/equations/RotationalLockEquation.js

+var Equation = require("./Equation"),
+    vec2 = require('../math/vec2');
+
+module.exports = RotationalLockEquation;
+
+/**
+ * Locks the relative angle between two bodies. The constraint tries to keep the dot product between two vectors, local in each body, to zero. The local angle in body i is a parameter.
+ *
+ * @class RotationalLockEquation
+ * @constructor
+ * @extends Equation
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Object} [options]
+ * @param {Number} [options.angle] Angle to add to the local vector in bodyA.
+ */
+function RotationalLockEquation(bodyA, bodyB, options){
+    options = options || {};
+    Equation.call(this, bodyA, bodyB, -Number.MAX_VALUE, Number.MAX_VALUE);
+
+    /**
+     * @property {number} angle
+     */
+    this.angle = options.angle || 0;
+
+    var G = this.G;
+    G[2] =  1;
+    G[5] = -1;
+}
+RotationalLockEquation.prototype = new Equation();
+RotationalLockEquation.prototype.constructor = RotationalLockEquation;
+
+var worldVectorA = vec2.create(),
+    worldVectorB = vec2.create(),
+    xAxis = vec2.fromValues(1,0),
+    yAxis = vec2.fromValues(0,1);
+RotationalLockEquation.prototype.computeGq = function(){
+    vec2.rotate(worldVectorA,xAxis,this.bodyA.angle+this.angle);
+    vec2.rotate(worldVectorB,yAxis,this.bodyB.angle);
+    return vec2.dot(worldVectorA,worldVectorB);
+};

src/equations/RotationalVelocityEquation.js

+var Equation = require("./Equation"),
+    vec2 = require('../math/vec2');
+
+module.exports = RotationalVelocityEquation;
+
+/**
+ * Syncs rotational velocity of two bodies, or sets a relative velocity (motor).
+ *
+ * @class RotationalVelocityEquation
+ * @constructor
+ * @extends Equation
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ */
+function RotationalVelocityEquation(bodyA, bodyB){
+    Equation.call(this, bodyA, bodyB, -Number.MAX_VALUE, Number.MAX_VALUE);
+    this.relativeVelocity = 1;
+    this.ratio = 1;
+}
+RotationalVelocityEquation.prototype = new Equation();
+RotationalVelocityEquation.prototype.constructor = RotationalVelocityEquation;
+RotationalVelocityEquation.prototype.computeB = function(a,b,h){
+    var G = this.G;
+    G[2] = -1;
+    G[5] = this.ratio;
+
+    var GiMf = this.computeGiMf();
+    var GW = this.computeGW();
+    var B = - GW * b - h*GiMf;
+
+    return B;
+};

src/events/EventEmitter.js

+/**
+ * Base class for objects that dispatches events.
+ * @class EventEmitter
+ * @constructor
+ */
+var EventEmitter = function () {};
+
+module.exports = EventEmitter;
+
+EventEmitter.prototype = {
+    constructor: EventEmitter,
+
+    /**
+     * Add an event listener
+     * @method on
+     * @param  {String} type
+     * @param  {Function} listener
+     * @return {EventEmitter} The self object, for chainability.
+     */
+    on: function ( type, listener, context ) {
+        listener.context = context || this;
+        if ( this._listeners === undefined ){
+            this._listeners = {};
+        }
+        var listeners = this._listeners;
+        if ( listeners[ type ] === undefined ) {
+            listeners[ type ] = [];
+        }
+        if ( listeners[ type ].indexOf( listener ) === - 1 ) {
+            listeners[ type ].push( listener );
+        }
+        return this;
+    },
+
+    /**
+     * Check if an event listener is added
+     * @method has
+     * @param  {String} type
+     * @param  {Function} listener
+     * @return {Boolean}
+     */
+    has: function ( type, listener ) {
+        if ( this._listeners === undefined ){
+            return false;
+        }
+        var listeners = this._listeners;
+        if(listener){
+            if ( listeners[ type ] !== undefined && listeners[ type ].indexOf( listener ) !== - 1 ) {
+                return true;
+            }
+        } else {
+            if ( listeners[ type ] !== undefined ) {
+                return true;
+            }
+        }
+
+        return false;
+    },
+
+    /**
+     * Remove an event listener
+     * @method off
+     * @param  {String} type
+     * @param  {Function} listener
+     * @return {EventEmitter} The self object, for chainability.
+     */
+    off: function ( type, listener ) {
+        if ( this._listeners === undefined ){
+            return this;
+        }
+        var listeners = this._listeners;
+        var index = listeners[ type ].indexOf( listener );
+        if ( index !== - 1 ) {
+            listeners[ type ].splice( index, 1 );
+        }
+        return this;
+    },
+
+    /**
+     * Emit an event.
+     * @method emit
+     * @param  {Object} event
+     * @param  {String} event.type
+     * @return {EventEmitter} The self object, for chainability.
+     */
+    emit: function ( event ) {
+        if ( this._listeners === undefined ){
+            return this;
+        }
+        var listeners = this._listeners;
+        var listenerArray = listeners[ event.type ];
+        if ( listenerArray !== undefined ) {
+            event.target = this;
+            for ( var i = 0, l = listenerArray.length; i < l; i ++ ) {
+                var listener = listenerArray[ i ];
+                listener.call( listener.context, event );
+            }
+        }
+        return this;
+    }
+};

src/material/ContactMaterial.js

+var Material = require('./Material');
+var Equation = require('../equations/Equation');
+
+module.exports = ContactMaterial;
+
+/**
+ * Defines what happens when two materials meet, such as what friction coefficient to use. You can also set other things such as restitution, surface velocity and constraint parameters.
+ * @class ContactMaterial
+ * @constructor
+ * @param {Material} materialA
+ * @param {Material} materialB
+ * @param {Object}   [options]
+ * @param {Number}   [options.friction=0.3]       Friction coefficient.
+ * @param {Number}   [options.restitution=0]      Restitution coefficient aka "bounciness".
+ * @param {Number}   [options.stiffness]          ContactEquation stiffness.
+ * @param {Number}   [options.relaxation]         ContactEquation relaxation.
+ * @param {Number}   [options.frictionStiffness]  FrictionEquation stiffness.
+ * @param {Number}   [options.frictionRelaxation] FrictionEquation relaxation.
+ * @param {Number}   [options.surfaceVelocity=0]  Surface velocity.
+ * @author schteppe
+ */
+function ContactMaterial(materialA, materialB, options){
+    options = options || {};
+
+    if(!(materialA instanceof Material) || !(materialB instanceof Material)){
+        throw new Error("First two arguments must be Material instances.");
+    }
+
+    /**
+     * The contact material identifier
+     * @property id
+     * @type {Number}
+     */
+    this.id = ContactMaterial.idCounter++;
+
+    /**
+     * First material participating in the contact material
+     * @property materialA
+     * @type {Material}
+     */
+    this.materialA = materialA;
+
+    /**
+     * Second material participating in the contact material
+     * @property materialB
+     * @type {Material}
+     */
+    this.materialB = materialB;
+
+    /**
+     * Friction coefficient to use in the contact of these two materials. Friction = 0 will make the involved objects super slippery, and friction = 1 will make it much less slippery. A friction coefficient larger than 1 will allow for very large friction forces, which can be convenient for preventing car tires not slip on the ground.
+     * @property friction
+     * @type {Number}
+     * @default 0.3
+     */
+    this.friction = typeof(options.friction) !== "undefined" ? Number(options.friction) : 0.3;
+
+    /**
+     * Restitution, or "bounciness" to use in the contact of these two materials. A restitution of 0 will make no bounce, while restitution=1 will approximately bounce back with the same velocity the object came with.
+     * @property restitution
+     * @type {Number}
+     * @default 0
+     */
+    this.restitution = typeof(options.restitution) !== "undefined" ? Number(options.restitution) : 0;
+
+    /**
+     * Hardness of the contact. Less stiffness will make the objects penetrate more, and will make the contact act more like a spring than a contact force. Default value is {{#crossLink "Equation/DEFAULT_STIFFNESS:property"}}Equation.DEFAULT_STIFFNESS{{/crossLink}}.
+     * @property stiffness
+     * @type {Number}
+     */
+    this.stiffness = typeof(options.stiffness) !== "undefined" ? Number(options.stiffness) : Equation.DEFAULT_STIFFNESS;
+
+    /**
+     * Relaxation of the resulting ContactEquation that this ContactMaterial generate. Default value is {{#crossLink "Equation/DEFAULT_RELAXATION:property"}}Equation.DEFAULT_RELAXATION{{/crossLink}}.
+     * @property relaxation
+     * @type {Number}
+     */
+    this.relaxation = typeof(options.relaxation) !== "undefined" ? Number(options.relaxation) : Equation.DEFAULT_RELAXATION;
+
+    /**
+     * Stiffness of the resulting friction force. For most cases, the value of this property should be a large number. I cannot think of any case where you would want less frictionStiffness. Default value is {{#crossLink "Equation/DEFAULT_STIFFNESS:property"}}Equation.DEFAULT_STIFFNESS{{/crossLink}}.
+     * @property frictionStiffness
+     * @type {Number}
+     */
+    this.frictionStiffness = typeof(options.frictionStiffness) !== "undefined" ? Number(options.frictionStiffness) : Equation.DEFAULT_STIFFNESS;
+
+    /**
+     * Relaxation of the resulting friction force. The default value should be good for most simulations. Default value is {{#crossLink "Equation/DEFAULT_RELAXATION:property"}}Equation.DEFAULT_RELAXATION{{/crossLink}}.
+     * @property frictionRelaxation
+     * @type {Number}
+     */
+    this.frictionRelaxation = typeof(options.frictionRelaxation) !== "undefined" ? Number(options.frictionRelaxation)  : Equation.DEFAULT_RELAXATION;
+
+    /**
+     * Will add surface velocity to this material. If bodyA rests on top if bodyB, and the surface velocity is positive, bodyA will slide to the right.
+     * @property {Number} surfaceVelocity
+     * @default 0
+     */
+    this.surfaceVelocity = typeof(options.surfaceVelocity) !== "undefined" ? Number(options.surfaceVelocity) : 0;
+
+    /**
+     * Offset to be set on ContactEquations. A positive value will make the bodies penetrate more into each other. Can be useful in scenes where contacts need to be more persistent, for example when stacking. Aka "cure for nervous contacts".
+     * @property contactSkinSize
+     * @type {Number}
+     */
+    this.contactSkinSize = 0.005;
+}
+
+ContactMaterial.idCounter = 0;

src/material/Material.js

+module.exports = Material;
+
+/**
+ * Defines a physics material.
+ * @class Material
+ * @constructor
+ * @param {number} id Material identifier
+ * @author schteppe
+ */
+function Material(id){
+    /**
+     * The material identifier
+     * @property id
+     * @type {Number}
+     */
+    this.id = id || Material.idCounter++;
+}
+
+Material.idCounter = 0;

src/math/polyk.js

+
+    /*
+        PolyK library
+        url: http://polyk.ivank.net
+        Released under MIT licence.
+
+        Copyright (c) 2012 Ivan Kuckir
+
+        Permission is hereby granted, free of charge, to any person
+        obtaining a copy of this software and associated documentation
+        files (the "Software"), to deal in the Software without
+        restriction, including without limitation the rights to use,
+        copy, modify, merge, publish, distribute, sublicense, and/or sell
+        copies of the Software, and to permit persons to whom the
+        Software is furnished to do so, subject to the following
+        conditions:
+
+        The above copyright notice and this permission notice shall be
+        included in all copies or substantial portions of the Software.
+
+        THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+        EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+        OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+        NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+        HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+        WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+        FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+        OTHER DEALINGS IN THE SOFTWARE.
+    */
+
+    var PolyK = {};
+
+    /*
+        Is Polygon self-intersecting?
+
+        O(n^2)
+    */
+    /*
+    PolyK.IsSimple = function(p)
+    {
+        var n = p.length>>1;
+        if(n<4) return true;
+        var a1 = new PolyK._P(), a2 = new PolyK._P();
+        var b1 = new PolyK._P(), b2 = new PolyK._P();
+        var c = new PolyK._P();
+
+        for(var i=0; i<n; i++)
+        {
+            a1.x = p[2*i  ];
+            a1.y = p[2*i+1];
+            if(i==n-1)  { a2.x = p[0    ];  a2.y = p[1    ]; }
+            else        { a2.x = p[2*i+2];  a2.y = p[2*i+3]; }
+
+            for(var j=0; j<n; j++)
+            {
+                if(Math.abs(i-j) < 2) continue;
+                if(j==n-1 && i==0) continue;
+                if(i==n-1 && j==0) continue;
+
+                b1.x = p[2*j  ];
+                b1.y = p[2*j+1];
+                if(j==n-1)  { b2.x = p[0    ];  b2.y = p[1    ]; }
+                else        { b2.x = p[2*j+2];  b2.y = p[2*j+3]; }
+
+                if(PolyK._GetLineIntersection(a1,a2,b1,b2,c) != null) return false;
+            }
+        }
+        return true;
+    }
+
+    PolyK.IsConvex = function(p)
+    {
+        if(p.length<6) return true;
+        var l = p.length - 4;
+        for(var i=0; i<l; i+=2)
+            if(!PolyK._convex(p[i], p[i+1], p[i+2], p[i+3], p[i+4], p[i+5])) return false;
+        if(!PolyK._convex(p[l  ], p[l+1], p[l+2], p[l+3], p[0], p[1])) return false;
+        if(!PolyK._convex(p[l+2], p[l+3], p[0  ], p[1  ], p[2], p[3])) return false;
+        return true;
+    }
+    */
+    PolyK.GetArea = function(p)
+    {
+        if(p.length <6) return 0;
+        var l = p.length - 2;
+        var sum = 0;
+        for(var i=0; i<l; i+=2)
+            sum += (p[i+2]-p[i]) * (p[i+1]+p[i+3]);
+        sum += (p[0]-p[l]) * (p[l+1]+p[1]);
+        return - sum * 0.5;
+    }
+    /*
+    PolyK.GetAABB = function(p)
+    {
+        var minx = Infinity;
+        var miny = Infinity;
+        var maxx = -minx;
+        var maxy = -miny;
+        for(var i=0; i<p.length; i+=2)
+        {
+            minx = Math.min(minx, p[i  ]);
+            maxx = Math.max(maxx, p[i  ]);
+            miny = Math.min(miny, p[i+1]);
+            maxy = Math.max(maxy, p[i+1]);
+        }
+        return {x:minx, y:miny, width:maxx-minx, height:maxy-miny};
+    }
+    */
+
+    PolyK.Triangulate = function(p)
+    {
+        var n = p.length>>1;
+        if(n<3) return [];
+        var tgs = [];
+        var avl = [];
+        for(var i=0; i<n; i++) avl.push(i);
+
+        var i = 0;
+        var al = n;
+        while(al > 3)
+        {
+            var i0 = avl[(i+0)%al];
+            var i1 = avl[(i+1)%al];
+            var i2 = avl[(i+2)%al];
+
+            var ax = p[2*i0],  ay = p[2*i0+1];
+            var bx = p[2*i1],  by = p[2*i1+1];
+            var cx = p[2*i2],  cy = p[2*i2+1];
+
+            var earFound = false;
+            if(PolyK._convex(ax, ay, bx, by, cx, cy))
+            {
+                earFound = true;
+                for(var j=0; j<al; j++)
+                {
+                    var vi = avl[j];
+                    if(vi==i0 || vi==i1 || vi==i2) continue;
+                    if(PolyK._PointInTriangle(p[2*vi], p[2*vi+1], ax, ay, bx, by, cx, cy)) {earFound = false; break;}
+                }
+            }
+            if(earFound)
+            {
+                tgs.push(i0, i1, i2);
+                avl.splice((i+1)%al, 1);
+                al--;
+                i= 0;
+            }
+            else if(i++ > 3*al) break;      // no convex angles :(
+        }
+        tgs.push(avl[0], avl[1], avl[2]);
+        return tgs;
+    }
+    /*
+    PolyK.ContainsPoint = function(p, px, py)
+    {
+        var n = p.length>>1;
+        var ax, ay, bx = p[2*n-2]-px, by = p[2*n-1]-py;
+        var depth = 0;
+        for(var i=0; i<n; i++)
+        {
+            ax = bx;  ay = by;
+            bx = p[2*i  ] - px;
+            by = p[2*i+1] - py;
+            if(ay< 0 && by< 0) continue;    // both "up" or both "donw"
+            if(ay>=0 && by>=0) continue;    // both "up" or both "donw"
+            if(ax< 0 && bx< 0) continue;
+
+            var lx = ax + (bx-ax)*(-ay)/(by-ay);
+            if(lx>0) depth++;
+        }
+        return (depth & 1) == 1;
+    }
+
+    PolyK.Slice = function(p, ax, ay, bx, by)
+    {
+        if(PolyK.ContainsPoint(p, ax, ay) || PolyK.ContainsPoint(p, bx, by)) return [p.slice(0)];
+
+        var a = new PolyK._P(ax, ay);
+        var b = new PolyK._P(bx, by);
+        var iscs = [];  // intersections
+        var ps = [];    // points
+        for(var i=0; i<p.length; i+=2) ps.push(new PolyK._P(p[i], p[i+1]));
+
+        for(var i=0; i<ps.length; i++)
+        {
+            var isc = new PolyK._P(0,0);
+            isc = PolyK._GetLineIntersection(a, b, ps[i], ps[(i+1)%ps.length], isc);
+
+            if(isc)
+            {
+                isc.flag = true;
+                iscs.push(isc);
+                ps.splice(i+1,0,isc);
+                i++;
+            }
+        }
+        if(iscs.length == 0) return [p.slice(0)];
+        var comp = function(u,v) {return PolyK._P.dist(a,u) - PolyK._P.dist(a,v); }
+        iscs.sort(comp);
+
+        var pgs = [];
+        var dir = 0;
+        while(iscs.length > 0)
+        {
+            var n = ps.length;
+            var i0 = iscs[0];
+            var i1 = iscs[1];
+            var ind0 = ps.indexOf(i0);
+            var ind1 = ps.indexOf(i1);
+            var solved = false;
+
+            if(PolyK._firstWithFlag(ps, ind0) == ind1) solved = true;
+            else
+            {
+                i0 = iscs[1];
+                i1 = iscs[0];
+                ind0 = ps.indexOf(i0);
+                ind1 = ps.indexOf(i1);
+                if(PolyK._firstWithFlag(ps, ind0) == ind1) solved = true;
+            }
+            if(solved)
+            {
+                dir--;
+                var pgn = PolyK._getPoints(ps, ind0, ind1);
+                pgs.push(pgn);
+                ps = PolyK._getPoints(ps, ind1, ind0);
+                i0.flag = i1.flag = false;
+                iscs.splice(0,2);
+                if(iscs.length == 0) pgs.push(ps);
+            }
+            else { dir++; iscs.reverse(); }
+            if(dir>1) break;
+        }
+        var result = [];
+        for(var i=0; i<pgs.length; i++)
+        {
+            var pg = pgs[i];
+            var npg = [];
+            for(var j=0; j<pg.length; j++) npg.push(pg[j].x, pg[j].y);
+            result.push(npg);
+        }
+        return result;
+    }
+
+    PolyK.Raycast = function(p, x, y, dx, dy, isc)
+    {
+        var l = p.length - 2;
+        var tp = PolyK._tp;
+        var a1 = tp[0], a2 = tp[1],
+        b1 = tp[2], b2 = tp[3], c = tp[4];
+        a1.x = x; a1.y = y;
+        a2.x = x+dx; a2.y = y+dy;
+
+        if(isc==null) isc = {dist:0, edge:0, norm:{x:0, y:0}, refl:{x:0, y:0}};
+        isc.dist = Infinity;
+
+        for(var i=0; i<l; i+=2)
+        {
+            b1.x = p[i  ];  b1.y = p[i+1];
+            b2.x = p[i+2];  b2.y = p[i+3];
+            var nisc = PolyK._RayLineIntersection(a1, a2, b1, b2, c);
+            if(nisc) PolyK._updateISC(dx, dy, a1, b1, b2, c, i/2, isc);
+        }
+        b1.x = b2.x;  b1.y = b2.y;
+        b2.x = p[0];  b2.y = p[1];
+        var nisc = PolyK._RayLineIntersection(a1, a2, b1, b2, c);
+        if(nisc) PolyK._updateISC(dx, dy, a1, b1, b2, c, p.length/2, isc);
+
+        return (isc.dist != Infinity) ? isc : null;
+    }
+
+    PolyK.ClosestEdge = function(p, x, y, isc)
+    {
+        var l = p.length - 2;
+        var tp = PolyK._tp;
+        var a1 = tp[0],
+        b1 = tp[2], b2 = tp[3], c = tp[4];
+        a1.x = x; a1.y = y;
+
+        if(isc==null) isc = {dist:0, edge:0, point:{x:0, y:0}, norm:{x:0, y:0}};
+        isc.dist = Infinity;
+
+        for(var i=0; i<l; i+=2)
+        {
+            b1.x = p[i  ];  b1.y = p[i+1];
+            b2.x = p[i+2];  b2.y = p[i+3];
+            PolyK._pointLineDist(a1, b1, b2, i>>1, isc);
+        }
+        b1.x = b2.x;  b1.y = b2.y;
+        b2.x = p[0];  b2.y = p[1];
+        PolyK._pointLineDist(a1, b1, b2, l>>1, isc);
+
+        var idst = 1/isc.dist;
+        isc.norm.x = (x-isc.point.x)*idst;
+        isc.norm.y = (y-isc.point.y)*idst;
+        return isc;
+    }
+
+    PolyK._pointLineDist = function(p, a, b, edge, isc)
+    {
+        var x = p.x, y = p.y, x1 = a.x, y1 = a.y, x2 = b.x, y2 = b.y;
+
+        var A = x - x1;
+        var B = y - y1;
+        var C = x2 - x1;
+        var D = y2 - y1;
+
+        var dot = A * C + B * D;
+        var len_sq = C * C + D * D;
+        var param = dot / len_sq;
+
+        var xx, yy;
+
+        if (param < 0 || (x1 == x2 && y1 == y2)) {
+            xx = x1;
+            yy = y1;
+        }
+        else if (param > 1) {
+            xx = x2;
+            yy = y2;
+        }
+        else {
+            xx = x1 + param * C;
+            yy = y1 + param * D;
+        }
+
+        var dx = x - xx;
+        var dy = y - yy;
+        var dst = Math.sqrt(dx * dx + dy * dy);
+        if(dst<isc.dist)
+        {
+            isc.dist = dst;
+            isc.edge = edge;
+            isc.point.x = xx;
+            isc.point.y = yy;
+        }
+    }
+
+    PolyK._updateISC = function(dx, dy, a1, b1, b2, c, edge, isc)
+    {
+        var nrl = PolyK._P.dist(a1, c);
+        if(nrl<isc.dist)
+        {
+            var ibl = 1/PolyK._P.dist(b1, b2);
+            var nx = -(b2.y-b1.y)*ibl;
+            var ny =  (b2.x-b1.x)*ibl;
+            var ddot = 2*(dx*nx+dy*ny);
+            isc.dist = nrl;
+            isc.norm.x = nx;
+            isc.norm.y = ny;
+            isc.refl.x = -ddot*nx+dx;
+            isc.refl.y = -ddot*ny+dy;
+            isc.edge = edge;
+        }
+    }
+
+    PolyK._getPoints = function(ps, ind0, ind1)
+    {
+        var n = ps.length;
+        var nps = [];
+        if(ind1<ind0) ind1 += n;
+        for(var i=ind0; i<= ind1; i++) nps.push(ps[i%n]);
+        return nps;
+    }
+
+    PolyK._firstWithFlag = function(ps, ind)
+    {
+        var n = ps.length;
+        while(true)
+        {
+            ind = (ind+1)%n;
+            if(ps[ind].flag) return ind;
+        }
+    }
+    */
+    PolyK._PointInTriangle = function(px, py, ax, ay, bx, by, cx, cy)
+    {
+        var v0x = cx-ax;
+        var v0y = cy-ay;
+        var v1x = bx-ax;
+        var v1y = by-ay;
+        var v2x = px-ax;
+        var v2y = py-ay;
+
+        var dot00 = v0x*v0x+v0y*v0y;
+        var dot01 = v0x*v1x+v0y*v1y;
+        var dot02 = v0x*v2x+v0y*v2y;
+        var dot11 = v1x*v1x+v1y*v1y;
+        var dot12 = v1x*v2x+v1y*v2y;
+
+        var invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
+        var u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+        var v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+        // Check if point is in triangle
+        return (u >= 0) && (v >= 0) && (u + v < 1);
+    }
+    /*
+    PolyK._RayLineIntersection = function(a1, a2, b1, b2, c)
+    {
+        var dax = (a1.x-a2.x), dbx = (b1.x-b2.x);
+        var day = (a1.y-a2.y), dby = (b1.y-b2.y);
+
+        var Den = dax*dby - day*dbx;
+        if (Den == 0) return null;  // parallel
+
+        var A = (a1.x * a2.y - a1.y * a2.x);
+        var B = (b1.x * b2.y - b1.y * b2.x);
+
+        var I = c;
+        var iDen = 1/Den;
+        I.x = ( A*dbx - dax*B ) * iDen;
+        I.y = ( A*dby - day*B ) * iDen;
+
+        if(!PolyK._InRect(I, b1, b2)) return null;
+        if((day>0 && I.y>a1.y) || (day<0 && I.y<a1.y)) return null;
+        if((dax>0 && I.x>a1.x) || (dax<0 && I.x<a1.x)) return null;
+        return I;
+    }
+
+    PolyK._GetLineIntersection = function(a1, a2, b1, b2, c)
+    {
+        var dax = (a1.x-a2.x), dbx = (b1.x-b2.x);
+        var day = (a1.y-a2.y), dby = (b1.y-b2.y);
+
+        var Den = dax*dby - day*dbx;
+        if (Den == 0) return null;  // parallel
+
+        var A = (a1.x * a2.y - a1.y * a2.x);
+        var B = (b1.x * b2.y - b1.y * b2.x);
+
+        var I = c;
+        I.x = ( A*dbx - dax*B ) / Den;
+        I.y = ( A*dby - day*B ) / Den;
+
+        if(PolyK._InRect(I, a1, a2) && PolyK._InRect(I, b1, b2)) return I;
+        return null;
+    }
+
+    PolyK._InRect = function(a, b, c)
+    {
+        if  (b.x == c.x) return (a.y>=Math.min(b.y, c.y) && a.y<=Math.max(b.y, c.y));
+        if  (b.y == c.y) return (a.x>=Math.min(b.x, c.x) && a.x<=Math.max(b.x, c.x));
+
+        if(a.x >= Math.min(b.x, c.x) && a.x <= Math.max(b.x, c.x)
+        && a.y >= Math.min(b.y, c.y) && a.y <= Math.max(b.y, c.y))
+        return true;
+        return false;
+    }
+    */
+    PolyK._convex = function(ax, ay, bx, by, cx, cy)
+    {
+        return (ay-by)*(cx-bx) + (bx-ax)*(cy-by) >= 0;
+    }
+    /*
+    PolyK._P = function(x,y)
+    {
+        this.x = x;
+        this.y = y;
+        this.flag = false;
+    }
+    PolyK._P.prototype.toString = function()
+    {
+        return "Point ["+this.x+", "+this.y+"]";
+    }
+    PolyK._P.dist = function(a,b)
+    {
+        var dx = b.x-a.x;
+        var dy = b.y-a.y;
+        return Math.sqrt(dx*dx + dy*dy);
+    }
+
+    PolyK._tp = [];
+    for(var i=0; i<10; i++) PolyK._tp.push(new PolyK._P(0,0));
+        */
+
+module.exports = PolyK;

src/math/vec2.js

+/* Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  * Redistributions of source code must retain the above copyright notice, this
+    list of conditions and the following disclaimer.
+  * Redistributions in binary form must reproduce the above copyright notice,
+    this list of conditions and the following disclaimer in the documentation
+    and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
+
+/**
+ * The vec2 object from glMatrix, with some extensions and some removed methods. See http://glmatrix.net.
+ * @class vec2
+ */
+
+var vec2 = module.exports = {};
+
+var Utils = require('../utils/Utils');
+
+/**
+ * Make a cross product and only return the z component
+ * @method crossLength
+ * @static
+ * @param  {Array} a
+ * @param  {Array} b
+ * @return {Number}
+ */
+vec2.crossLength = function(a,b){
+    return a[0] * b[1] - a[1] * b[0];
+};
+
+/**
+ * Cross product between a vector and the Z component of a vector
+ * @method crossVZ
+ * @static
+ * @param  {Array} out
+ * @param  {Array} vec
+ * @param  {Number} zcomp
+ * @return {Number}
+ */
+vec2.crossVZ = function(out, vec, zcomp){
+    vec2.rotate(out,vec,-Math.PI/2);// Rotate according to the right hand rule
+    vec2.scale(out,out,zcomp);      // Scale with z
+    return out;
+};
+
+/**
+ * Cross product between a vector and the Z component of a vector
+ * @method crossZV
+ * @static
+ * @param  {Array} out
+ * @param  {Number} zcomp
+ * @param  {Array} vec
+ * @return {Number}
+ */
+vec2.crossZV = function(out, zcomp, vec){
+    vec2.rotate(out,vec,Math.PI/2); // Rotate according to the right hand rule
+    vec2.scale(out,out,zcomp);      // Scale with z
+    return out;
+};
+
+/**
+ * Rotate a vector by an angle
+ * @method rotate
+ * @static
+ * @param  {Array} out
+ * @param  {Array} a
+ * @param  {Number} angle
+ */
+vec2.rotate = function(out,a,angle){
+    if(angle !== 0){
+        var c = Math.cos(angle),
+            s = Math.sin(angle),
+            x = a[0],
+            y = a[1];
+        out[0] = c*x -s*y;
+        out[1] = s*x +c*y;
+    } else {
+        out[0] = a[0];
+        out[1] = a[1];
+    }
+};
+
+/**
+ * Rotate a vector 90 degrees clockwise
+ * @method rotate90cw
+ * @static
+ * @param  {Array} out
+ * @param  {Array} a
+ * @param  {Number} angle
+ */
+vec2.rotate90cw = function(out, a) {
+    var x = a[0];
+    var y = a[1];
+    out[0] = y;
+    out[1] = -x;
+};
+
+/**
+ * Transform a point position to local frame.
+ * @method toLocalFrame
+ * @param  {Array} out
+ * @param  {Array} worldPoint
+ * @param  {Array} framePosition
+ * @param  {Number} frameAngle
+ */
+vec2.toLocalFrame = function(out, worldPoint, framePosition, frameAngle){
+    vec2.copy(out, worldPoint);
+    vec2.sub(out, out, framePosition);
+    vec2.rotate(out, out, -frameAngle);
+};
+
+/**
+ * Transform a point position to global frame.
+ * @method toGlobalFrame
+ * @param  {Array} out
+ * @param  {Array} localPoint
+ * @param  {Array} framePosition
+ * @param  {Number} frameAngle
+ */
+vec2.toGlobalFrame = function(out, localPoint, framePosition, frameAngle){
+    vec2.copy(out, localPoint);
+    vec2.rotate(out, out, frameAngle);
+    vec2.add(out, out, framePosition);
+};
+
+/**
+ * Transform a vector to local frame.
+ * @method vectorToLocalFrame
+ * @param  {Array} out
+ * @param  {Array} worldVector
+ * @param  {Number} frameAngle
+ */
+vec2.vectorToLocalFrame = function(out, worldVector, frameAngle){
+    vec2.rotate(out, worldVector, -frameAngle);
+};
+
+/**
+ * Transform a point position to global frame.
+ * @method toGlobalFrame
+ * @param  {Array} out
+ * @param  {Array} localVector
+ * @param  {Number} frameAngle
+ */
+vec2.vectorToGlobalFrame = function(out, localVector, frameAngle){
+    vec2.rotate(out, localVector, frameAngle);
+};
+
+/**
+ * Compute centroid of a triangle spanned by vectors a,b,c. See http://easycalculation.com/analytical/learn-centroid.php
+ * @method centroid
+ * @static
+ * @param  {Array} out
+ * @param  {Array} a
+ * @param  {Array} b
+ * @param  {Array} c
+ * @return  {Array} The out object
+ */
+vec2.centroid = function(out, a, b, c){
+    vec2.add(out, a, b);
+    vec2.add(out, out, c);
+    vec2.scale(out, out, 1/3);
+    return out;
+};
+
+/**
+ * Creates a new, empty vec2
+ * @static
+ * @method create
+ * @return {Array} a new 2D vector
+ */
+vec2.create = function() {
+    var out = new Utils.ARRAY_TYPE(2);
+    out[0] = 0;
+    out[1] = 0;
+    return out;
+};
+
+/**
+ * Creates a new vec2 initialized with values from an existing vector
+ * @static
+ * @method clone
+ * @param {Array} a vector to clone
+ * @return {Array} a new 2D vector
+ */
+vec2.clone = function(a) {
+    var out = new Utils.ARRAY_TYPE(2);
+    out[0] = a[0];
+    out[1] = a[1];
+    return out;
+};
+
+/**
+ * Creates a new vec2 initialized with the given values
+ * @static
+ * @method fromValues
+ * @param {Number} x X component
+ * @param {Number} y Y component
+ * @return {Array} a new 2D vector
+ */
+vec2.fromValues = function(x, y) {
+    var out = new Utils.ARRAY_TYPE(2);
+    out[0] = x;
+    out[1] = y;
+    return out;
+};
+
+/**
+ * Copy the values from one vec2 to another
+ * @static
+ * @method copy
+ * @param {Array} out the receiving vector
+ * @param {Array} a the source vector
+ * @return {Array} out
+ */
+vec2.copy = function(out, a) {
+    out[0] = a[0];
+    out[1] = a[1];
+    return out;
+};
+
+/**
+ * Set the components of a vec2 to the given values
+ * @static
+ * @method set
+ * @param {Array} out the receiving vector
+ * @param {Number} x X component
+ * @param {Number} y Y component
+ * @return {Array} out
+ */
+vec2.set = function(out, x, y) {
+    out[0] = x;
+    out[1] = y;
+    return out;
+};
+
+/**
+ * Adds two vec2's
+ * @static
+ * @method add
+ * @param {Array} out the receiving vector
+ * @param {Array} a the first operand
+ * @param {Array} b the second operand
+ * @return {Array} out
+ */
+vec2.add = function(out, a, b) {
+    out[0] = a[0] + b[0];
+    out[1] = a[1] + b[1];
+    return out;
+};
+
+/**
+ * Subtracts two vec2's
+ * @static
+ * @method subtract
+ * @param {Array} out the receiving vector
+ * @param {Array} a the first operand
+ * @param {Array} b the second operand
+ * @return {Array} out
+ */
+vec2.subtract = function(out, a, b) {
+    out[0] = a[0] - b[0];
+    out[1] = a[1] - b[1];
+    return out;
+};
+
+/**
+ * Alias for vec2.subtract
+ * @static
+ * @method sub
+ */
+vec2.sub = vec2.subtract;
+
+/**
+ * Multiplies two vec2's
+ * @static
+ * @method multiply
+ * @param {Array} out the receiving vector
+ * @param {Array} a the first operand
+ * @param {Array} b the second operand
+ * @return {Array} out
+ */
+vec2.multiply = function(out, a, b) {
+    out[0] = a[0] * b[0];
+    out[1] = a[1] * b[1];
+    return out;
+};
+
+/**
+ * Alias for vec2.multiply
+ * @static
+ * @method mul
+ */
+vec2.mul = vec2.multiply;
+
+/**
+ * Divides two vec2's
+ * @static
+ * @method divide
+ * @param {Array} out the receiving vector
+ * @param {Array} a the first operand
+ * @param {Array} b the second operand
+ * @return {Array} out
+ */
+vec2.divide = function(out, a, b) {
+    out[0] = a[0] / b[0];
+    out[1] = a[1] / b[1];
+    return out;
+};
+
+/**
+ * Alias for vec2.divide
+ * @static
+ * @method div
+ */
+vec2.div = vec2.divide;
+
+/**
+ * Scales a vec2 by a scalar number
+ * @static
+ * @method scale
+ * @param {Array} out the receiving vector
+ * @param {Array} a the vector to scale
+ * @param {Number} b amount to scale the vector by
+ * @return {Array} out
+ */
+vec2.scale = function(out, a, b) {
+    out[0] = a[0] * b;
+    out[1] = a[1] * b;
+    return out;
+};
+
+/**
+ * Calculates the euclidian distance between two vec2's
+ * @static
+ * @method distance
+ * @param {Array} a the first operand
+ * @param {Array} b the second operand
+ * @return {Number} distance between a and b
+ */
+vec2.distance = function(a, b) {
+    var x = b[0] - a[0],
+        y = b[1] - a[1];
+    return Math.sqrt(x*x + y*y);
+};
+
+/**
+ * Alias for vec2.distance
+ * @static
+ * @method dist
+ */
+vec2.dist = vec2.distance;
+
+/**
+ * Calculates the squared euclidian distance between two vec2's
+ * @static
+ * @method squaredDistance
+ * @param {Array} a the first operand
+ * @param {Array} b the second operand
+ * @return {Number} squared distance between a and b
+ */
+vec2.squaredDistance = function(a, b) {
+    var x = b[0] - a[0],
+        y = b[1] - a[1];
+    return x*x + y*y;
+};
+
+/**
+ * Alias for vec2.squaredDistance
+ * @static
+ * @method sqrDist
+ */
+vec2.sqrDist = vec2.squaredDistance;
+
+/**
+ * Calculates the length of a vec2
+ * @static
+ * @method length
+ * @param {Array} a vector to calculate length of
+ * @return {Number} length of a
+ */
+vec2.length = function (a) {
+    var x = a[0],
+        y = a[1];
+    return Math.sqrt(x*x + y*y);
+};
+
+/**
+ * Alias for vec2.length
+ * @method len
+ * @static
+ */
+vec2.len = vec2.length;
+
+/**
+ * Calculates the squared length of a vec2
+ * @static
+ * @method squaredLength
+ * @param {Array} a vector to calculate squared length of
+ * @return {Number} squared length of a
+ */
+vec2.squaredLength = function (a) {
+    var x = a[0],
+        y = a[1];
+    return x*x + y*y;
+};
+
+/**
+ * Alias for vec2.squaredLength
+ * @static
+ * @method sqrLen
+ */
+vec2.sqrLen = vec2.squaredLength;
+
+/**
+ * Negates the components of a vec2
+ * @static
+ * @method negate
+ * @param {Array} out the receiving vector
+ * @param {Array} a vector to negate
+ * @return {Array} out
+ */
+vec2.negate = function(out, a) {
+    out[0] = -a[0];
+    out[1] = -a[1];
+    return out;
+};
+
+/**
+ * Normalize a vec2
+ * @static
+ * @method normalize
+ * @param {Array} out the receiving vector
+ * @param {Array} a vector to normalize
+ * @return {Array} out
+ */
+vec2.normalize = function(out, a) {
+    var x = a[0],
+        y = a[1];
+    var len = x*x + y*y;
+    if (len > 0) {
+        //TODO: evaluate use of glm_invsqrt here?
+        len = 1 / Math.sqrt(len);
+        out[0] = a[0] * len;
+        out[1] = a[1] * len;
+    }
+    return out;
+};
+
+/**
+ * Calculates the dot product of two vec2's
+ * @static
+ * @method dot
+ * @param {Array} a the first operand
+ * @param {Array} b the second operand
+ * @return {Number} dot product of a and b
+ */
+vec2.dot = function (a, b) {
+    return a[0] * b[0] + a[1] * b[1];
+};
+
+/**
+ * Returns a string representation of a vector
+ * @static
+ * @method str
+ * @param {Array} vec vector to represent as a string
+ * @return {String} string representation of the vector
+ */
+vec2.str = function (a) {
+    return 'vec2(' + a[0] + ', ' + a[1] + ')';
+};
+
+/**
+ * Linearly interpolate/mix two vectors.
+ * @static
+ * @method lerp
+ * @param {Array} out
+ * @param {Array} a First vector
+ * @param {Array} b Second vector
+ * @param {number} t Lerp factor
+ */
+vec2.lerp = function (out, a, b, t) {
+    var ax = a[0],
+        ay = a[1];
+    out[0] = ax + t * (b[0] - ax);
+    out[1] = ay + t * (b[1] - ay);
+    return out;
+};
+
+/**
+ * Reflect a vector along a normal.
+ * @static
+ * @method reflect
+ * @param {Array} out
+ * @param {Array} vector
+ * @param {Array} normal
+ */
+vec2.reflect = function(out, vector, normal){
+    var dot = vector[0] * normal[0] + vector[1] * normal[1];
+    out[0] = vector[0] - 2 * normal[0] * dot;
+    out[1] = vector[1] - 2 * normal[1] * dot;
+};
+
+/**
+ * Get the intersection point between two line segments.
+ * @static
+ * @method getLineSegmentsIntersection
+ * @param  {Array} out
+ * @param  {Array} p0
+ * @param  {Array} p1
+ * @param  {Array} p2
+ * @param  {Array} p3
+ * @return {boolean} True if there was an intersection, otherwise false.
+ */
+vec2.getLineSegmentsIntersection = function(out, p0, p1, p2, p3) {
+    var t = vec2.getLineSegmentsIntersectionFraction(p0, p1, p2, p3);
+    if(t < 0){
+        return false;
+    } else {
+        out[0] = p0[0] + (t * (p1[0] - p0[0]));
+        out[1] = p0[1] + (t * (p1[1] - p0[1]));
+        return true;
+    }
+};
+
+/**
+ * Get the intersection fraction between two line segments. If successful, the intersection is at p0 + t * (p1 - p0)
+ * @static
+ * @method getLineSegmentsIntersectionFraction
+ * @param  {Array} p0
+ * @param  {Array} p1
+ * @param  {Array} p2
+ * @param  {Array} p3
+ * @return {number} A number between 0 and 1 if there was an intersection, otherwise -1.
+ */
+vec2.getLineSegmentsIntersectionFraction = function(p0, p1, p2, p3) {
+    var s1_x = p1[0] - p0[0];
+    var s1_y = p1[1] - p0[1];
+    var s2_x = p3[0] - p2[0];
+    var s2_y = p3[1] - p2[1];
+
+    var s, t;
+    s = (-s1_y * (p0[0] - p2[0]) + s1_x * (p0[1] - p2[1])) / (-s2_x * s1_y + s1_x * s2_y);
+    t = ( s2_x * (p0[1] - p2[1]) - s2_y * (p0[0] - p2[0])) / (-s2_x * s1_y + s1_x * s2_y);
+    if (s >= 0 && s <= 1 && t >= 0 && t <= 1) { // Collision detected
+        return t;
+    }
+    return -1; // No collision
+};

src/objects/Body.js

+var vec2 = require('../math/vec2')
+,   decomp = require('poly-decomp')
+,   Convex = require('../shapes/Convex')
+,   RaycastResult = require('../collision/RaycastResult')
+,   Ray = require('../collision/Ray')
+,   AABB = require('../collision/AABB')
+,   EventEmitter = require('../events/EventEmitter');
+
+module.exports = Body;
+
+/**
+ * A rigid body. Has got a center of mass, position, velocity and a number of
+ * shapes that are used for collisions.
+ *
+ * @class Body
+ * @constructor
+ * @extends EventEmitter
+ * @param {Object} [options]
+ * @param {Array} [options.force]
+ * @param {Array} [options.position]
+ * @param {Array} [options.velocity]
+ * @param {Boolean} [options.allowSleep]
+ * @param {Boolean} [options.collisionResponse]
+ * @param {Number} [options.angle=0]
+ * @param {Number} [options.angularForce=0]
+ * @param {Number} [options.angularVelocity=0]
+ * @param {Number} [options.ccdIterations=10]
+ * @param {Number} [options.ccdSpeedThreshold=-1]
+ * @param {Number} [options.fixedRotation=false]
+ * @param {Number} [options.gravityScale]
+ * @param {Number} [options.id]
+ * @param {Number} [options.mass=0] A number >= 0. If zero, the .type will be set to Body.STATIC.
+ * @param {Number} [options.sleepSpeedLimit]
+ * @param {Number} [options.sleepTimeLimit]
+ *
+ * @example
+ *
+ *     // Create a typical dynamic body
+ *     var body = new Body({
+ *         mass: 1,
+ *         position: [0, 0],
+ *         angle: 0,
+ *         velocity: [0, 0],
+ *         angularVelocity: 0
+ *     });
+ *
+ *     // Add a circular shape to the body
+ *     body.addShape(new Circle({ radius: 1 }));
+ *
+ *     // Add the body to the world
+ *     world.addBody(body);
+ */
+function Body(options){
+    options = options || {};
+
+    EventEmitter.call(this);
+
+    /**
+     * The body identifyer
+     * @property id
+     * @type {Number}
+     */
+    this.id = options.id || ++Body._idCounter;
+
+    /**
+     * The world that this body is added to. This property is set to NULL if the body is not added to any world.
+     * @property world
+     * @type {World}
+     */
+    this.world = null;
+
+    /**
+     * The shapes of the body.
+     *
+     * @property shapes
+     * @type {Array}
+     */
+    this.shapes = [];
+
+    /**
+     * The mass of the body.
+     * @property mass
+     * @type {number}
+     */
+    this.mass = options.mass || 0;
+
+    /**
+     * The inverse mass of the body.
+     * @property invMass
+     * @type {number}
+     */
+    this.invMass = 0;
+
+    /**
+     * The inertia of the body around the Z axis.
+     * @property inertia
+     * @type {number}
+     */
+    this.inertia = 0;
+
+    /**
+     * The inverse inertia of the body.
+     * @property invInertia
+     * @type {number}
+     */
+    this.invInertia = 0;
+
+    this.invMassSolve = 0;
+    this.invInertiaSolve = 0;
+
+    /**
+     * Set to true if you want to fix the rotation of the body.
+     * @property fixedRotation
+     * @type {Boolean}
+     */
+    this.fixedRotation = !!options.fixedRotation;
+
+    /**
+     * Set to true if you want to fix the body movement along the X axis. The body will still be able to move along Y.
+     * @property {Boolean} fixedX
+     */
+    this.fixedX = !!options.fixedX;
+
+    /**
+     * Set to true if you want to fix the body movement along the Y axis. The body will still be able to move along X.
+     * @property {Boolean} fixedY
+     */
+    this.fixedY = !!options.fixedY;
+
+    /**
+     * @private
+     * @property {array} massMultiplier
+     */
+    this.massMultiplier = vec2.create();
+
+    /**
+     * The position of the body
+     * @property position
+     * @type {Array}
+     */
+    this.position = vec2.fromValues(0,0);
+    if(options.position){
+        vec2.copy(this.position, options.position);
+    }
+
+    /**
+     * The interpolated position of the body. Use this for rendering.
+     * @property interpolatedPosition
+     * @type {Array}
+     */
+    this.interpolatedPosition = vec2.fromValues(0,0);
+
+    /**
+     * The interpolated angle of the body. Use this for rendering.
+     * @property interpolatedAngle
+     * @type {Number}
+     */
+    this.interpolatedAngle = 0;
+
+    /**
+     * The previous position of the body.
+     * @property previousPosition
+     * @type {Array}
+     */
+    this.previousPosition = vec2.fromValues(0,0);
+
+    /**
+     * The previous angle of the body.
+     * @property previousAngle
+     * @type {Number}
+     */
+    this.previousAngle = 0;
+
+    /**
+     * The current velocity of the body.
+     * @property velocity
+     * @type {Array}
+     */
+    this.velocity = vec2.fromValues(0,0);
+    if(options.velocity){
+        vec2.copy(this.velocity, options.velocity);
+    }
+
+    /**
+     * Constraint velocity that was added to the body during the last step.
+     * @property vlambda
+     * @type {Array}
+     */
+    this.vlambda = vec2.fromValues(0,0);
+
+    /**
+     * Angular constraint velocity that was added to the body during last step.
+     * @property wlambda
+     * @type {Array}
+     */
+    this.wlambda = 0;
+
+    /**
+     * The angle of the body, in radians.
+     * @property angle
+     * @type {number}
+     * @example
+     *     // The angle property is not normalized to the interval 0 to 2*pi, it can be any value.
+     *     // If you need a value between 0 and 2*pi, use the following function to normalize it.
+     *     function normalizeAngle(angle){
+     *         angle = angle % (2*Math.PI);
+     *         if(angle < 0){
+     *             angle += (2*Math.PI);
+     *         }
+     *         return angle;
+     *     }
+     */
+    this.angle = options.angle || 0;
+
+    /**
+     * The angular velocity of the body, in radians per second.
+     * @property angularVelocity
+     * @type {number}
+     */
+    this.angularVelocity = options.angularVelocity || 0;
+
+    /**
+     * The force acting on the body. Since the body force (and {{#crossLink "Body/angularForce:property"}}{{/crossLink}}) will be zeroed after each step, so you need to set the force before each step.
+     * @property force
+     * @type {Array}
+     *
+     * @example
+     *     // This produces a forcefield of 1 Newton in the positive x direction.
+     *     for(var i=0; i<numSteps; i++){
+     *         body.force[0] = 1;
+     *         world.step(1/60);
+     *     }
+     *
+     * @example
+     *     // This will apply a rotational force on the body
+     *     for(var i=0; i<numSteps; i++){
+     *         body.angularForce = -3;
+     *         world.step(1/60);
+     *     }
+     */
+    this.force = vec2.create();
+    if(options.force){
+        vec2.copy(this.force, options.force);
+    }
+
+    /**
+     * The angular force acting on the body. See {{#crossLink "Body/force:property"}}{{/crossLink}}.
+     * @property angularForce
+     * @type {number}
+     */
+    this.angularForce = options.angularForce || 0;
+
+    /**
+     * The linear damping acting on the body in the velocity direction. Should be a value between 0 and 1.
+     * @property damping
+     * @type {Number}
+     * @default 0.1
+     */
+    this.damping = typeof(options.damping) === "number" ? options.damping : 0.1;
+
+    /**
+     * The angular force acting on the body. Should be a value between 0 and 1.
+     * @property angularDamping
+     * @type {Number}
+     * @default 0.1
+     */
+    this.angularDamping = typeof(options.angularDamping) === "number" ? options.angularDamping : 0.1;
+
+    /**
+     * The type of motion this body has. Should be one of: {{#crossLink "Body/STATIC:property"}}Body.STATIC{{/crossLink}}, {{#crossLink "Body/DYNAMIC:property"}}Body.DYNAMIC{{/crossLink}} and {{#crossLink "Body/KINEMATIC:property"}}Body.KINEMATIC{{/crossLink}}.
+     *
+     * * Static bodies do not move, and they do not respond to forces or collision.
+     * * Dynamic bodies body can move and respond to collisions and forces.
+     * * Kinematic bodies only moves according to its .velocity, and does not respond to collisions or force.
+     *
+     * @property type
+     * @type {number}
+     *
+     * @example
+     *     // Bodies are static by default. Static bodies will never move.
+     *     var body = new Body();
+     *     console.log(body.type == Body.STATIC); // true
+     *
+     * @example
+     *     // By setting the mass of a body to a nonzero number, the body
+     *     // will become dynamic and will move and interact with other bodies.
+     *     var dynamicBody = new Body({
+     *         mass : 1
+     *     });
+     *     console.log(dynamicBody.type == Body.DYNAMIC); // true
+     *
+     * @example
+     *     // Kinematic bodies will only move if you change their velocity.
+     *     var kinematicBody = new Body({
+     *         type: Body.KINEMATIC // Type can be set via the options object.
+     *     });
+     */
+    this.type = Body.STATIC;
+
+    if(typeof(options.type) !== 'undefined'){
+        this.type = options.type;
+    } else if(!options.mass){
+        this.type = Body.STATIC;
+    } else {
+        this.type = Body.DYNAMIC;
+    }
+
+    /**
+     * Bounding circle radius.
+     * @property boundingRadius
+     * @type {Number}
+     */
+    this.boundingRadius = 0;
+
+    /**
+     * Bounding box of this body.
+     * @property aabb
+     * @type {AABB}
+     */
+    this.aabb = new AABB();
+
+    /**
+     * Indicates if the AABB needs update. Update it with {{#crossLink "Body/updateAABB:method"}}.updateAABB(){{/crossLink}}.
+     * @property aabbNeedsUpdate
+     * @type {Boolean}
+     * @see updateAABB
+     *
+     * @example
+     *     // Force update the AABB
+     *     body.aabbNeedsUpdate = true;
+     *     body.updateAABB();
+     *     console.log(body.aabbNeedsUpdate); // false
+     */
+    this.aabbNeedsUpdate = true;
+
+    /**
+     * If true, the body will automatically fall to sleep. Note that you need to enable sleeping in the {{#crossLink "World"}}{{/crossLink}} before anything will happen.
+     * @property allowSleep
+     * @type {Boolean}
+     * @default true
+     */
+    this.allowSleep = options.allowSleep !== undefined ? options.allowSleep : true;
+
+    this.wantsToSleep = false;
+
+    /**
+     * One of {{#crossLink "Body/AWAKE:property"}}Body.AWAKE{{/crossLink}}, {{#crossLink "Body/SLEEPY:property"}}Body.SLEEPY{{/crossLink}} and {{#crossLink "Body/SLEEPING:property"}}Body.SLEEPING{{/crossLink}}.
+     *
+     * The body is initially Body.AWAKE. If its velocity norm is below .sleepSpeedLimit, the sleepState will become Body.SLEEPY. If the body continues to be Body.SLEEPY for .sleepTimeLimit seconds, it will fall asleep (Body.SLEEPY).
+     *
+     * @property sleepState
+     * @type {Number}
+     * @default Body.AWAKE
+     */
+    this.sleepState = Body.AWAKE;
+
+    /**
+     * If the speed (the norm of the velocity) is smaller than this value, the body is considered sleepy.
+     * @property sleepSpeedLimit
+     * @type {Number}
+     * @default 0.2
+     */
+    this.sleepSpeedLimit = options.sleepSpeedLimit !== undefined ? options.sleepSpeedLimit : 0.2;
+
+    /**
+     * If the body has been sleepy for this sleepTimeLimit seconds, it is considered sleeping.
+     * @property sleepTimeLimit
+     * @type {Number}
+     * @default 1
+     */
+    this.sleepTimeLimit = options.sleepTimeLimit !== undefined ? options.sleepTimeLimit : 1;
+
+    /**
+     * Gravity scaling factor. If you want the body to ignore gravity, set this to zero. If you want to reverse gravity, set it to -1.
+     * @property {Number} gravityScale
+     * @default 1
+     */
+    this.gravityScale = options.gravityScale !== undefined ? options.gravityScale : 1;
+
+    /**
+     * Whether to produce contact forces when in contact with other bodies. Note that contacts will be generated, but they will be disabled. That means that this body will move through other bodies, but it will still trigger contact events, etc.
+     * @property {Boolean} collisionResponse
+     */
+    this.collisionResponse = options.collisionResponse !== undefined ? options.collisionResponse : true;
+
+    /**
+     * How long the body has been sleeping.
+     * @property {Number} idleTime
+     */
+    this.idleTime = 0;
+
+    /**
+     * The last time when the body went to SLEEPY state.
+     * @property {Number} timeLastSleepy
+     * @private
+     */
+    this.timeLastSleepy = 0;
+
+    /**
+     * If the body speed exceeds this threshold, CCD (continuous collision detection) will be enabled. Set it to a negative number to disable CCD completely for this body.
+     * @property {number} ccdSpeedThreshold
+     * @default -1
+     */
+    this.ccdSpeedThreshold = options.ccdSpeedThreshold !== undefined ? options.ccdSpeedThreshold : -1;
+
+    /**
+     * The number of iterations that should be used when searching for the time of impact during CCD. A larger number will assure that there's a small penetration on CCD collision, but a small number will give more performance.
+     * @property {number} ccdIterations
+     * @default 10
+     */
+    this.ccdIterations = options.ccdIterations !== undefined ? options.ccdIterations : 10;
+
+    this.concavePath = null;
+
+    this._wakeUpAfterNarrowphase = false;
+
+    this.updateMassProperties();
+}
+Body.prototype = new EventEmitter();
+Body.prototype.constructor = Body;
+
+Body._idCounter = 0;
+
+/**
+ * @private
+ * @method updateSolveMassProperties
+ */
+Body.prototype.updateSolveMassProperties = function(){
+    if(this.sleepState === Body.SLEEPING || this.type === Body.KINEMATIC){
+        this.invMassSolve = 0;
+        this.invInertiaSolve = 0;
+    } else {
+        this.invMassSolve = this.invMass;
+        this.invInertiaSolve = this.invInertia;
+    }
+};
+
+/**
+ * Set the total density of the body
+ * @method setDensity
+ * @param {number} density
+ */
+Body.prototype.setDensity = function(density) {
+    var totalArea = this.getArea();
+    this.mass = totalArea * density;
+    this.updateMassProperties();
+};
+
+/**
+ * Get the total area of all shapes in the body
+ * @method getArea
+ * @return {Number}
+ */
+Body.prototype.getArea = function() {
+    var totalArea = 0;
+    for(var i=0; i<this.shapes.length; i++){
+        totalArea += this.shapes[i].area;
+    }
+    return totalArea;
+};
+
+/**
+ * Get the AABB from the body. The AABB is updated if necessary.
+ * @method getAABB
+ * @return {AABB} The AABB instance (this.aabb)
+ */
+Body.prototype.getAABB = function(){
+    if(this.aabbNeedsUpdate){
+        this.updateAABB();
+    }
+    return this.aabb;
+};
+
+var shapeAABB = new AABB(),
+    tmp = vec2.create();
+
+/**
+ * Updates the AABB of the Body, and set .aabbNeedsUpdate = false.
+ * @method updateAABB
+ */
+Body.prototype.updateAABB = function() {
+    var shapes = this.shapes,
+        N = shapes.length,
+        offset = tmp,
+        bodyAngle = this.angle;
+
+    for(var i=0; i!==N; i++){
+        var shape = shapes[i],
+            angle = shape.angle + bodyAngle;
+
+        // Get shape world offset
+        vec2.rotate(offset, shape.position, bodyAngle);
+        vec2.add(offset, offset, this.position);
+
+        // Get shape AABB
+        shape.computeAABB(shapeAABB, offset, angle);
+
+        if(i===0){
+            this.aabb.copy(shapeAABB);
+        } else {
+            this.aabb.extend(shapeAABB);
+        }
+    }
+
+    this.aabbNeedsUpdate = false;
+};
+
+/**
+ * Update the bounding radius of the body (this.boundingRadius). Should be done if any of the shape dimensions or positions are changed.
+ * @method updateBoundingRadius
+ */
+Body.prototype.updateBoundingRadius = function(){
+    var shapes = this.shapes,
+        N = shapes.length,
+        radius = 0;
+
+    for(var i=0; i!==N; i++){
+        var shape = shapes[i],
+            offset = vec2.length(shape.position),
+            r = shape.boundingRadius;
+        if(offset + r > radius){
+            radius = offset + r;
+        }
+    }
+
+    this.boundingRadius = radius;
+};
+
+/**
+ * Add a shape to the body. You can pass a local transform when adding a shape,
+ * so that the shape gets an offset and angle relative to the body center of mass.
+ * Will automatically update the mass properties and bounding radius.
+ *
+ * @method addShape
+ * @param  {Shape}              shape
+ * @param  {Array} [offset] Local body offset of the shape.
+ * @param  {Number}             [angle]  Local body angle.
+ *
+ * @example
+ *     var body = new Body(),
+ *         shape = new Circle({ radius: 1 });
+ *
+ *     // Add the shape to the body, positioned in the center
+ *     body.addShape(shape);
+ *
+ *     // Add another shape to the body, positioned 1 unit length from the body center of mass along the local x-axis.
+ *     body.addShape(shape,[1,0]);
+ *
+ *     // Add another shape to the body, positioned 1 unit length from the body center of mass along the local y-axis, and rotated 90 degrees CCW.
+ *     body.addShape(shape,[0,1],Math.PI/2);
+ */
+Body.prototype.addShape = function(shape, offset, angle){
+    if(shape.body){
+        throw new Error('A shape can only be added to one body.');
+    }
+    shape.body = this;
+
+    // Copy the offset vector
+    if(offset){
+        vec2.copy(shape.position, offset);
+    } else {
+        vec2.set(shape.position, 0, 0);
+    }
+
+    shape.angle = angle || 0;
+
+    this.shapes.push(shape);
+    this.updateMassProperties();
+    this.updateBoundingRadius();
+
+    this.aabbNeedsUpdate = true;
+};
+
+/**
+ * Remove a shape
+ * @method removeShape
+ * @param  {Shape} shape
+ * @return {Boolean} True if the shape was found and removed, else false.
+ */
+Body.prototype.removeShape = function(shape){
+    var idx = this.shapes.indexOf(shape);
+
+    if(idx !== -1){
+        this.shapes.splice(idx,1);
+        this.aabbNeedsUpdate = true;
+        shape.body = null;
+        return true;
+    } else {
+        return false;
+    }
+};
+
+/**
+ * Updates .inertia, .invMass, .invInertia for this Body. Should be called when
+ * changing the structure or mass of the Body.
+ *
+ * @method updateMassProperties
+ *
+ * @example
+ *     body.mass += 1;
+ *     body.updateMassProperties();
+ */
+Body.prototype.updateMassProperties = function(){
+    if(this.type === Body.STATIC || this.type === Body.KINEMATIC){
+
+        this.mass = Number.MAX_VALUE;
+        this.invMass = 0;
+        this.inertia = Number.MAX_VALUE;
+        this.invInertia = 0;
+
+    } else {
+
+        var shapes = this.shapes,
+            N = shapes.length,
+            m = this.mass / N,
+            I = 0;
+
+        if(!this.fixedRotation){
+            for(var i=0; i<N; i++){
+                var shape = shapes[i],
+                    r2 = vec2.squaredLength(shape.position),
+                    Icm = shape.computeMomentOfInertia(m);
+                I += Icm + m*r2;
+            }
+            this.inertia = I;
+            this.invInertia = I>0 ? 1/I : 0;
+
+        } else {
+            this.inertia = Number.MAX_VALUE;
+            this.invInertia = 0;
+        }
+
+        // Inverse mass properties are easy
+        this.invMass = 1 / this.mass;
+
+        vec2.set(
+            this.massMultiplier,
+            this.fixedX ? 0 : 1,
+            this.fixedY ? 0 : 1
+        );
+    }
+};
+
+var Body_applyForce_r = vec2.create();
+
+/**
+ * Apply force to a point relative to the center of mass of the body. This could for example be a point on the RigidBody surface. Applying force this way will add to Body.force and Body.angularForce. If relativePoint is zero, the force will be applied directly on the center of mass, and the torque produced will be zero.
+ * @method applyForce
+ * @param {Array} force The force to add.
+ * @param {Array} [relativePoint] A world point to apply the force on.
+ */
+Body.prototype.applyForce = function(force, relativePoint){
+
+    // Add linear force
+    vec2.add(this.force, this.force, force);
+
+    if(relativePoint){
+
+        // Compute produced rotational force
+        var rotForce = vec2.crossLength(relativePoint,force);
+
+        // Add rotational force
+        this.angularForce += rotForce;
+    }
+};
+
+/**
+ * Apply force to a body-local point.
+ * @method applyForceLocal
+ * @param  {Array} localForce The force vector to add, oriented in local body space.
+ * @param  {Array} [localPoint] A point relative to the body in world space. If not given, it is set to zero and all of the impulse will be excerted on the center of mass.
+ */
+var Body_applyForce_forceWorld = vec2.create();
+var Body_applyForce_pointWorld = vec2.create();
+var Body_applyForce_pointLocal = vec2.create();
+Body.prototype.applyForceLocal = function(localForce, localPoint){
+    localPoint = localPoint || Body_applyForce_pointLocal;
+    var worldForce = Body_applyForce_forceWorld;
+    var worldPoint = Body_applyForce_pointWorld;
+    this.vectorToWorldFrame(worldForce, localForce);
+    this.vectorToWorldFrame(worldPoint, localPoint);
+    this.applyForce(worldForce, worldPoint);
+};
+
+/**
+ * Apply impulse to a point relative to the body. This could for example be a point on the Body surface. An impulse is a force added to a body during a short period of time (impulse = force * time). Impulses will be added to Body.velocity and Body.angularVelocity.
+ * @method applyImpulse
+ * @param  {Array} impulse The impulse vector to add, oriented in world space.
+ * @param  {Array} [relativePoint] A point relative to the body in world space. If not given, it is set to zero and all of the impulse will be excerted on the center of mass.
+ */
+var Body_applyImpulse_velo = vec2.create();
+Body.prototype.applyImpulse = function(impulseVector, relativePoint){
+    if(this.type !== Body.DYNAMIC){
+        return;
+    }
+
+    // Compute produced central impulse velocity
+    var velo = Body_applyImpulse_velo;
+    vec2.scale(velo, impulseVector, this.invMass);
+    vec2.multiply(velo, this.massMultiplier, velo);
+
+    // Add linear impulse
+    vec2.add(this.velocity, velo, this.velocity);
+
+    if(relativePoint){
+        // Compute produced rotational impulse velocity
+        var rotVelo = vec2.crossLength(relativePoint, impulseVector);
+        rotVelo *= this.invInertia;
+
+        // Add rotational Impulse
+        this.angularVelocity += rotVelo;
+    }
+};
+
+/**
+ * Apply impulse to a point relative to the body. This could for example be a point on the Body surface. An impulse is a force added to a body during a short period of time (impulse = force * time). Impulses will be added to Body.velocity and Body.angularVelocity.
+ * @method applyImpulseLocal
+ * @param  {Array} impulse The impulse vector to add, oriented in world space.
+ * @param  {Array} [relativePoint] A point relative to the body in world space. If not given, it is set to zero and all of the impulse will be excerted on the center of mass.
+ */
+var Body_applyImpulse_impulseWorld = vec2.create();
+var Body_applyImpulse_pointWorld = vec2.create();
+var Body_applyImpulse_pointLocal = vec2.create();
+Body.prototype.applyImpulseLocal = function(localImpulse, localPoint){
+    localPoint = localPoint || Body_applyImpulse_pointLocal;
+    var worldImpulse = Body_applyImpulse_impulseWorld;
+    var worldPoint = Body_applyImpulse_pointWorld;
+    this.vectorToWorldFrame(worldImpulse, localImpulse);
+    this.vectorToWorldFrame(worldPoint, localPoint);
+    this.applyImpulse(worldImpulse, worldPoint);
+};
+
+/**
+ * Transform a world point to local body frame.
+ * @method toLocalFrame
+ * @param  {Array} out          The vector to store the result in
+ * @param  {Array} worldPoint   The input world point
+ */
+Body.prototype.toLocalFrame = function(out, worldPoint){
+    vec2.toLocalFrame(out, worldPoint, this.position, this.angle);
+};
+
+/**
+ * Transform a local point to world frame.
+ * @method toWorldFrame
+ * @param  {Array} out          The vector to store the result in
+ * @param  {Array} localPoint   The input local point
+ */
+Body.prototype.toWorldFrame = function(out, localPoint){
+    vec2.toGlobalFrame(out, localPoint, this.position, this.angle);
+};
+
+/**
+ * Transform a world point to local body frame.
+ * @method vectorToLocalFrame
+ * @param  {Array} out          The vector to store the result in
+ * @param  {Array} worldVector  The input world vector
+ */
+Body.prototype.vectorToLocalFrame = function(out, worldVector){
+    vec2.vectorToLocalFrame(out, worldVector, this.angle);
+};
+
+/**
+ * Transform a local point to world frame.
+ * @method vectorToWorldFrame
+ * @param  {Array} out          The vector to store the result in
+ * @param  {Array} localVector  The input local vector
+ */
+Body.prototype.vectorToWorldFrame = function(out, localVector){
+    vec2.vectorToGlobalFrame(out, localVector, this.angle);
+};
+
+/**
+ * Reads a polygon shape path, and assembles convex shapes from that and puts them at proper offset points.
+ * @method fromPolygon
+ * @param {Array} path An array of 2d vectors, e.g. [[0,0],[0,1],...] that resembles a concave or convex polygon. The shape must be simple and without holes.
+ * @param {Object} [options]
+ * @param {Boolean} [options.optimalDecomp=false]   Set to true if you need optimal decomposition. Warning: very slow for polygons with more than 10 vertices.
+ * @param {Boolean} [options.skipSimpleCheck=false] Set to true if you already know that the path is not intersecting itself.
+ * @param {Boolean|Number} [options.removeCollinearPoints=false] Set to a number (angle threshold value) to remove collinear points, or false to keep all points.
+ * @return {Boolean} True on success, else false.
+ */
+Body.prototype.fromPolygon = function(path,options){
+    options = options || {};
+
+    // Remove all shapes
+    for(var i=this.shapes.length; i>=0; --i){
+        this.removeShape(this.shapes[i]);
+    }
+
+    var p = new decomp.Polygon();
+    p.vertices = path;
+
+    // Make it counter-clockwise
+    p.makeCCW();
+
+    if(typeof(options.removeCollinearPoints) === "number"){
+        p.removeCollinearPoints(options.removeCollinearPoints);
+    }
+
+    // Check if any line segment intersects the path itself
+    if(typeof(options.skipSimpleCheck) === "undefined"){
+        if(!p.isSimple()){
+            return false;
+        }
+    }
+
+    // Save this path for later
+    this.concavePath = p.vertices.slice(0);
+    for(var i=0; i<this.concavePath.length; i++){
+        var v = [0,0];
+        vec2.copy(v,this.concavePath[i]);
+        this.concavePath[i] = v;
+    }
+
+    // Slow or fast decomp?
+    var convexes;
+    if(options.optimalDecomp){
+        convexes = p.decomp();
+    } else {
+        convexes = p.quickDecomp();
+    }
+
+    var cm = vec2.create();
+
+    // Add convexes
+    for(var i=0; i!==convexes.length; i++){
+        // Create convex
+        var c = new Convex({ vertices: convexes[i].vertices });
+
+        // Move all vertices so its center of mass is in the local center of the convex
+        for(var j=0; j!==c.vertices.length; j++){
+            var v = c.vertices[j];
+            vec2.sub(v,v,c.centerOfMass);
+        }
+
+        vec2.scale(cm,c.centerOfMass,1);
+        c.updateTriangles();
+        c.updateCenterOfMass();
+        c.updateBoundingRadius();
+
+        // Add the shape
+        this.addShape(c,cm);
+    }
+
+    this.adjustCenterOfMass();
+
+    this.aabbNeedsUpdate = true;
+
+    return true;
+};
+
+var adjustCenterOfMass_tmp1 = vec2.fromValues(0,0),
+    adjustCenterOfMass_tmp2 = vec2.fromValues(0,0),
+    adjustCenterOfMass_tmp3 = vec2.fromValues(0,0),
+    adjustCenterOfMass_tmp4 = vec2.fromValues(0,0);
+
+/**
+ * Moves the shape offsets so their center of mass becomes the body center of mass.
+ * @method adjustCenterOfMass
+ */
+Body.prototype.adjustCenterOfMass = function(){
+    var offset_times_area = adjustCenterOfMass_tmp2,
+        sum =               adjustCenterOfMass_tmp3,
+        cm =                adjustCenterOfMass_tmp4,
+        totalArea =         0;
+    vec2.set(sum,0,0);
+
+    for(var i=0; i!==this.shapes.length; i++){
+        var s = this.shapes[i];
+        vec2.scale(offset_times_area, s.position, s.area);
+        vec2.add(sum, sum, offset_times_area);
+        totalArea += s.area;
+    }
+
+    vec2.scale(cm,sum,1/totalArea);
+
+    // Now move all shapes
+    for(var i=0; i!==this.shapes.length; i++){
+        var s = this.shapes[i];
+        vec2.sub(s.position, s.position, cm);
+    }
+
+    // Move the body position too
+    vec2.add(this.position,this.position,cm);
+
+    // And concave path
+    for(var i=0; this.concavePath && i<this.concavePath.length; i++){
+        vec2.sub(this.concavePath[i], this.concavePath[i], cm);
+    }
+
+    this.updateMassProperties();
+    this.updateBoundingRadius();
+};
+
+/**
+ * Sets the force on the body to zero.
+ * @method setZeroForce
+ */
+Body.prototype.setZeroForce = function(){
+    vec2.set(this.force,0.0,0.0);
+    this.angularForce = 0.0;
+};
+
+Body.prototype.resetConstraintVelocity = function(){
+    var b = this,
+        vlambda = b.vlambda;
+    vec2.set(vlambda,0,0);
+    b.wlambda = 0;
+};
+
+Body.prototype.addConstraintVelocity = function(){
+    var b = this,
+        v = b.velocity;
+    vec2.add( v, v, b.vlambda);
+    b.angularVelocity += b.wlambda;
+};
+
+/**
+ * Apply damping, see <a href="http://code.google.com/p/bullet/issues/detail?id=74">this</a> for details.
+ * @method applyDamping
+ * @param  {number} dt Current time step
+ */
+Body.prototype.applyDamping = function(dt){
+    if(this.type === Body.DYNAMIC){ // Only for dynamic bodies
+        var v = this.velocity;
+        vec2.scale(v, v, Math.pow(1.0 - this.damping,dt));
+        this.angularVelocity *= Math.pow(1.0 - this.angularDamping,dt);
+    }
+};
+
+/**
+ * Wake the body up. Normally you should not need this, as the body is automatically awoken at events such as collisions.
+ * Sets the sleepState to {{#crossLink "Body/AWAKE:property"}}Body.AWAKE{{/crossLink}} and emits the wakeUp event if the body wasn't awake before.
+ * @method wakeUp
+ */
+Body.prototype.wakeUp = function(){
+    var s = this.sleepState;
+    this.sleepState = Body.AWAKE;
+    this.idleTime = 0;
+    if(s !== Body.AWAKE){
+        this.emit(Body.wakeUpEvent);
+    }
+};
+
+/**
+ * Force body sleep
+ * @method sleep
+ */
+Body.prototype.sleep = function(){
+    this.sleepState = Body.SLEEPING;
+    this.angularVelocity = 0;
+    this.angularForce = 0;
+    vec2.set(this.velocity,0,0);
+    vec2.set(this.force,0,0);
+    this.emit(Body.sleepEvent);
+};
+
+/**
+ * Called every timestep to update internal sleep timer and change sleep state if needed.
+ * @method sleepTick
+ * @param {number} time The world time in seconds
+ * @param {boolean} dontSleep
+ * @param {number} dt
+ */
+Body.prototype.sleepTick = function(time, dontSleep, dt){
+    if(!this.allowSleep || this.type === Body.SLEEPING){
+        return;
+    }
+
+    this.wantsToSleep = false;
+
+    var sleepState = this.sleepState,
+        speedSquared = vec2.squaredLength(this.velocity) + Math.pow(this.angularVelocity,2),
+        speedLimitSquared = Math.pow(this.sleepSpeedLimit,2);
+
+    // Add to idle time
+    if(speedSquared >= speedLimitSquared){
+        this.idleTime = 0;
+        this.sleepState = Body.AWAKE;
+    } else {
+        this.idleTime += dt;
+        this.sleepState = Body.SLEEPY;
+    }
+    if(this.idleTime > this.sleepTimeLimit){
+        if(!dontSleep){
+            this.sleep();
+        } else {
+            this.wantsToSleep = true;
+        }
+    }
+};
+
+/**
+ * Check if the body is overlapping another body. Note that this method only works if the body was added to a World and if at least one step was taken.
+ * @method overlaps
+ * @param  {Body} body
+ * @return {boolean}
+ */
+Body.prototype.overlaps = function(body){
+    return this.world.overlapKeeper.bodiesAreOverlapping(this, body);
+};
+
+var integrate_fhMinv = vec2.create();
+var integrate_velodt = vec2.create();
+
+/**
+ * Move the body forward in time given its current velocity.
+ * @method integrate
+ * @param  {Number} dt
+ */
+Body.prototype.integrate = function(dt){
+    var minv = this.invMass,
+        f = this.force,
+        pos = this.position,
+        velo = this.velocity;
+
+    // Save old position
+    vec2.copy(this.previousPosition, this.position);
+    this.previousAngle = this.angle;
+
+    // Velocity update
+    if(!this.fixedRotation){
+        this.angularVelocity += this.angularForce * this.invInertia * dt;
+    }
+    vec2.scale(integrate_fhMinv, f, dt * minv);
+    vec2.multiply(integrate_fhMinv, this.massMultiplier, integrate_fhMinv);
+    vec2.add(velo, integrate_fhMinv, velo);
+
+    // CCD
+    if(!this.integrateToTimeOfImpact(dt)){
+
+        // Regular position update
+        vec2.scale(integrate_velodt, velo, dt);
+        vec2.add(pos, pos, integrate_velodt);
+        if(!this.fixedRotation){
+            this.angle += this.angularVelocity * dt;
+        }
+    }
+
+    this.aabbNeedsUpdate = true;
+};
+
+var result = new RaycastResult();
+var ray = new Ray({
+    mode: Ray.ALL
+});
+var direction = vec2.create();
+var end = vec2.create();
+var startToEnd = vec2.create();
+var rememberPosition = vec2.create();
+Body.prototype.integrateToTimeOfImpact = function(dt){
+
+    if(this.ccdSpeedThreshold < 0 || vec2.squaredLength(this.velocity) < Math.pow(this.ccdSpeedThreshold, 2)){
+        return false;
+    }
+
+    vec2.normalize(direction, this.velocity);
+
+    vec2.scale(end, this.velocity, dt);
+    vec2.add(end, end, this.position);
+
+    vec2.sub(startToEnd, end, this.position);
+    var startToEndAngle = this.angularVelocity * dt;
+    var len = vec2.length(startToEnd);
+
+    var timeOfImpact = 1;
+
+    var hit;
+    var that = this;
+    result.reset();
+    ray.callback = function (result) {
+        if(result.body === that){
+            return;
+        }
+        hit = result.body;
+        result.getHitPoint(end, ray);
+        vec2.sub(startToEnd, end, that.position);
+        timeOfImpact = vec2.length(startToEnd) / len;
+        result.stop();
+    };
+    vec2.copy(ray.from, this.position);
+    vec2.copy(ray.to, end);
+    ray.update();
+    this.world.raycast(result, ray);
+
+    if(!hit){
+        return false;
+    }
+
+    var rememberAngle = this.angle;
+    vec2.copy(rememberPosition, this.position);
+
+    // Got a start and end point. Approximate time of impact using binary search
+    var iter = 0;
+    var tmin = 0;
+    var tmid = 0;
+    var tmax = timeOfImpact;
+    while (tmax >= tmin && iter < this.ccdIterations) {
+        iter++;
+
+        // calculate the midpoint
+        tmid = (tmax - tmin) / 2;
+
+        // Move the body to that point
+        vec2.scale(integrate_velodt, startToEnd, timeOfImpact);
+        vec2.add(this.position, rememberPosition, integrate_velodt);
+        this.angle = rememberAngle + startToEndAngle * timeOfImpact;
+        this.updateAABB();
+
+        // check overlap
+        var overlaps = this.aabb.overlaps(hit.aabb) && this.world.narrowphase.bodiesOverlap(this, hit);
+
+        if (overlaps) {
+            // change min to search upper interval
+            tmin = tmid;
+        } else {
+            // change max to search lower interval
+            tmax = tmid;
+        }
+    }
+
+    timeOfImpact = tmid;
+
+    vec2.copy(this.position, rememberPosition);
+    this.angle = rememberAngle;
+
+    // move to TOI
+    vec2.scale(integrate_velodt, startToEnd, timeOfImpact);
+    vec2.add(this.position, this.position, integrate_velodt);
+    if(!this.fixedRotation){
+        this.angle += startToEndAngle * timeOfImpact;
+    }
+
+    return true;
+};
+
+/**
+ * Get velocity of a point in the body.
+ * @method getVelocityAtPoint
+ * @param  {Array} result A vector to store the result in
+ * @param  {Array} relativePoint A world oriented vector, indicating the position of the point to get the velocity from
+ * @return {Array} The result vector
+ */
+Body.prototype.getVelocityAtPoint = function(result, relativePoint){
+    vec2.crossVZ(result, relativePoint, this.angularVelocity);
+    vec2.subtract(result, this.velocity, result);
+    return result;
+};
+
+/**
+ * @event sleepy
+ */
+Body.sleepyEvent = {
+    type: "sleepy"
+};
+
+/**
+ * @event sleep
+ */
+Body.sleepEvent = {
+    type: "sleep"
+};
+
+/**
+ * @event wakeup
+ */
+Body.wakeUpEvent = {
+    type: "wakeup"
+};
+
+/**
+ * Dynamic body.
+ * @property DYNAMIC
+ * @type {Number}
+ * @static
+ */
+Body.DYNAMIC = 1;
+
+/**
+ * Static body.
+ * @property STATIC
+ * @type {Number}
+ * @static
+ */
+Body.STATIC = 2;
+
+/**
+ * Kinematic body.
+ * @property KINEMATIC
+ * @type {Number}
+ * @static
+ */
+Body.KINEMATIC = 4;
+
+/**
+ * @property AWAKE
+ * @type {Number}
+ * @static
+ */
+Body.AWAKE = 0;
+
+/**
+ * @property SLEEPY
+ * @type {Number}
+ * @static
+ */
+Body.SLEEPY = 1;
+
+/**
+ * @property SLEEPING
+ * @type {Number}
+ * @static
+ */
+Body.SLEEPING = 2;
+

src/objects/LinearSpring.js

+var vec2 = require('../math/vec2');
+var Spring = require('./Spring');
+var Utils = require('../utils/Utils');
+
+module.exports = LinearSpring;
+
+/**
+ * A spring, connecting two bodies.
+ *
+ * The Spring explicitly adds force and angularForce to the bodies.
+ *
+ * @class LinearSpring
+ * @extends Spring
+ * @constructor
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Object} [options]
+ * @param {number} [options.restLength]   A number > 0. Default is the current distance between the world anchor points.
+ * @param {number} [options.stiffness=100]  Spring constant (see Hookes Law). A number >= 0.
+ * @param {number} [options.damping=1]      A number >= 0. Default: 1
+ * @param {Array}  [options.worldAnchorA]   Where to hook the spring to body A, in world coordinates. Overrides the option "localAnchorA" if given.
+ * @param {Array}  [options.worldAnchorB]
+ * @param {Array}  [options.localAnchorA]   Where to hook the spring to body A, in local body coordinates. Defaults to the body center.
+ * @param {Array}  [options.localAnchorB]
+ */
+function LinearSpring(bodyA,bodyB,options){
+    options = options || {};
+
+    Spring.call(this, bodyA, bodyB, options);
+
+    /**
+     * Anchor for bodyA in local bodyA coordinates.
+     * @property localAnchorA
+     * @type {Array}
+     */
+    this.localAnchorA = vec2.fromValues(0,0);
+
+    /**
+     * Anchor for bodyB in local bodyB coordinates.
+     * @property localAnchorB
+     * @type {Array}
+     */
+    this.localAnchorB = vec2.fromValues(0,0);
+
+    if(options.localAnchorA){ vec2.copy(this.localAnchorA, options.localAnchorA); }
+    if(options.localAnchorB){ vec2.copy(this.localAnchorB, options.localAnchorB); }
+    if(options.worldAnchorA){ this.setWorldAnchorA(options.worldAnchorA); }
+    if(options.worldAnchorB){ this.setWorldAnchorB(options.worldAnchorB); }
+
+    var worldAnchorA = vec2.create();
+    var worldAnchorB = vec2.create();
+    this.getWorldAnchorA(worldAnchorA);
+    this.getWorldAnchorB(worldAnchorB);
+    var worldDistance = vec2.distance(worldAnchorA, worldAnchorB);
+
+    /**
+     * Rest length of the spring.
+     * @property restLength
+     * @type {number}
+     */
+    this.restLength = typeof(options.restLength) === "number" ? options.restLength : worldDistance;
+}
+LinearSpring.prototype = new Spring();
+LinearSpring.prototype.constructor = LinearSpring;
+
+/**
+ * Set the anchor point on body A, using world coordinates.
+ * @method setWorldAnchorA
+ * @param {Array} worldAnchorA
+ */
+LinearSpring.prototype.setWorldAnchorA = function(worldAnchorA){
+    this.bodyA.toLocalFrame(this.localAnchorA, worldAnchorA);
+};
+
+/**
+ * Set the anchor point on body B, using world coordinates.
+ * @method setWorldAnchorB
+ * @param {Array} worldAnchorB
+ */
+LinearSpring.prototype.setWorldAnchorB = function(worldAnchorB){
+    this.bodyB.toLocalFrame(this.localAnchorB, worldAnchorB);
+};
+
+/**
+ * Get the anchor point on body A, in world coordinates.
+ * @method getWorldAnchorA
+ * @param {Array} result The vector to store the result in.
+ */
+LinearSpring.prototype.getWorldAnchorA = function(result){
+    this.bodyA.toWorldFrame(result, this.localAnchorA);
+};
+
+/**
+ * Get the anchor point on body B, in world coordinates.
+ * @method getWorldAnchorB
+ * @param {Array} result The vector to store the result in.
+ */
+LinearSpring.prototype.getWorldAnchorB = function(result){
+    this.bodyB.toWorldFrame(result, this.localAnchorB);
+};
+
+var applyForce_r =              vec2.create(),
+    applyForce_r_unit =         vec2.create(),
+    applyForce_u =              vec2.create(),
+    applyForce_f =              vec2.create(),
+    applyForce_worldAnchorA =   vec2.create(),
+    applyForce_worldAnchorB =   vec2.create(),
+    applyForce_ri =             vec2.create(),
+    applyForce_rj =             vec2.create(),
+    applyForce_tmp =            vec2.create();
+
+/**
+ * Apply the spring force to the connected bodies.
+ * @method applyForce
+ */
+LinearSpring.prototype.applyForce = function(){
+    var k = this.stiffness,
+        d = this.damping,
+        l = this.restLength,
+        bodyA = this.bodyA,
+        bodyB = this.bodyB,
+        r = applyForce_r,
+        r_unit = applyForce_r_unit,
+        u = applyForce_u,
+        f = applyForce_f,
+        tmp = applyForce_tmp;
+
+    var worldAnchorA = applyForce_worldAnchorA,
+        worldAnchorB = applyForce_worldAnchorB,
+        ri = applyForce_ri,
+        rj = applyForce_rj;
+
+    // Get world anchors
+    this.getWorldAnchorA(worldAnchorA);
+    this.getWorldAnchorB(worldAnchorB);
+
+    // Get offset points
+    vec2.sub(ri, worldAnchorA, bodyA.position);
+    vec2.sub(rj, worldAnchorB, bodyB.position);
+
+    // Compute distance vector between world anchor points
+    vec2.sub(r, worldAnchorB, worldAnchorA);
+    var rlen = vec2.len(r);
+    vec2.normalize(r_unit,r);
+
+    //console.log(rlen)
+    //console.log("A",vec2.str(worldAnchorA),"B",vec2.str(worldAnchorB))
+
+    // Compute relative velocity of the anchor points, u
+    vec2.sub(u, bodyB.velocity, bodyA.velocity);
+    vec2.crossZV(tmp, bodyB.angularVelocity, rj);
+    vec2.add(u, u, tmp);
+    vec2.crossZV(tmp, bodyA.angularVelocity, ri);
+    vec2.sub(u, u, tmp);
+
+    // F = - k * ( x - L ) - D * ( u )
+    vec2.scale(f, r_unit, -k*(rlen-l) - d*vec2.dot(u,r_unit));
+
+    // Add forces to bodies
+    vec2.sub( bodyA.force, bodyA.force, f);
+    vec2.add( bodyB.force, bodyB.force, f);
+
+    // Angular force
+    var ri_x_f = vec2.crossLength(ri, f);
+    var rj_x_f = vec2.crossLength(rj, f);
+    bodyA.angularForce -= ri_x_f;
+    bodyB.angularForce += rj_x_f;
+};

src/objects/RotationalSpring.js

+var vec2 = require('../math/vec2');
+var Spring = require('./Spring');
+
+module.exports = RotationalSpring;
+
+/**
+ * A rotational spring, connecting two bodies rotation. This spring explicitly adds angularForce (torque) to the bodies.
+ *
+ * The spring can be combined with a {{#crossLink "RevoluteConstraint"}}{{/crossLink}} to make, for example, a mouse trap.
+ *
+ * @class RotationalSpring
+ * @extends Spring
+ * @constructor
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Object} [options]
+ * @param {number} [options.restAngle] The relative angle of bodies at which the spring is at rest. If not given, it's set to the current relative angle between the bodies.
+ * @param {number} [options.stiffness=100] Spring constant (see Hookes Law). A number >= 0.
+ * @param {number} [options.damping=1] A number >= 0.
+ */
+function RotationalSpring(bodyA, bodyB, options){
+    options = options || {};
+
+    Spring.call(this, bodyA, bodyB, options);
+
+    /**
+     * Rest angle of the spring.
+     * @property restAngle
+     * @type {number}
+     */
+    this.restAngle = typeof(options.restAngle) === "number" ? options.restAngle : bodyB.angle - bodyA.angle;
+}
+RotationalSpring.prototype = new Spring();
+RotationalSpring.prototype.constructor = RotationalSpring;
+
+/**
+ * Apply the spring force to the connected bodies.
+ * @method applyForce
+ */
+RotationalSpring.prototype.applyForce = function(){
+    var k = this.stiffness,
+        d = this.damping,
+        l = this.restAngle,
+        bodyA = this.bodyA,
+        bodyB = this.bodyB,
+        x = bodyB.angle - bodyA.angle,
+        u = bodyB.angularVelocity - bodyA.angularVelocity;
+
+    var torque = - k * (x - l) - d * u * 0;
+
+    bodyA.angularForce -= torque;
+    bodyB.angularForce += torque;
+};

src/objects/Spring.js

+var vec2 = require('../math/vec2');
+var Utils = require('../utils/Utils');
+
+module.exports = Spring;
+
+/**
+ * A spring, connecting two bodies. The Spring explicitly adds force and angularForce to the bodies and does therefore not put load on the constraint solver.
+ *
+ * @class Spring
+ * @constructor
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ * @param {Object} [options]
+ * @param {number} [options.stiffness=100]  Spring constant (see Hookes Law). A number >= 0.
+ * @param {number} [options.damping=1]      A number >= 0. Default: 1
+ * @param {Array}  [options.localAnchorA]   Where to hook the spring to body A, in local body coordinates. Defaults to the body center.
+ * @param {Array}  [options.localAnchorB]
+ * @param {Array}  [options.worldAnchorA]   Where to hook the spring to body A, in world coordinates. Overrides the option "localAnchorA" if given.
+ * @param {Array}  [options.worldAnchorB]
+ */
+function Spring(bodyA, bodyB, options){
+    options = Utils.defaults(options,{
+        stiffness: 100,
+        damping: 1,
+    });
+
+    /**
+     * Stiffness of the spring.
+     * @property stiffness
+     * @type {number}
+     */
+    this.stiffness = options.stiffness;
+
+    /**
+     * Damping of the spring.
+     * @property damping
+     * @type {number}
+     */
+    this.damping = options.damping;
+
+    /**
+     * First connected body.
+     * @property bodyA
+     * @type {Body}
+     */
+    this.bodyA = bodyA;
+
+    /**
+     * Second connected body.
+     * @property bodyB
+     * @type {Body}
+     */
+    this.bodyB = bodyB;
+}
+
+/**
+ * Apply the spring force to the connected bodies.
+ * @method applyForce
+ */
+Spring.prototype.applyForce = function(){
+    // To be implemented by subclasses
+};

src/objects/TopDownVehicle.js

+var vec2 = require('../math/vec2');
+var Utils = require('../utils/Utils');
+var Constraint = require('../constraints/Constraint');
+var FrictionEquation = require('../equations/FrictionEquation');
+var Body = require('../objects/Body');
+
+module.exports = TopDownVehicle;
+
+/**
+ * @class TopDownVehicle
+ * @constructor
+ * @param {Body} chassisBody A dynamic body, already added to the world.
+ * @param {Object} [options]
+ *
+ * @example
+ *
+ *     // Create a dynamic body for the chassis
+ *     var chassisBody = new Body({
+ *         mass: 1
+ *     });
+ *     var boxShape = new Box({ width: 0.5, height: 1 });
+ *     chassisBody.addShape(boxShape);
+ *     world.addBody(chassisBody);
+ *
+ *     // Create the vehicle
+ *     var vehicle = new TopDownVehicle(chassisBody);
+ *
+ *     // Add one front wheel and one back wheel - we don't actually need four :)
+ *     var frontWheel = vehicle.addWheel({
+ *         localPosition: [0, 0.5] // front
+ *     });
+ *     frontWheel.setSideFriction(4);
+ *
+ *     // Back wheel
+ *     var backWheel = vehicle.addWheel({
+ *         localPosition: [0, -0.5] // back
+ *     });
+ *     backWheel.setSideFriction(3); // Less side friction on back wheel makes it easier to drift
+ *     vehicle.addToWorld(world);
+ *
+ *     // Steer value zero means straight forward. Positive is left and negative right.
+ *     frontWheel.steerValue = Math.PI / 16;
+ *
+ *     // Engine force forward
+ *     backWheel.engineForce = 10;
+ *     backWheel.setBrakeForce(0);
+ */
+function TopDownVehicle(chassisBody, options){
+    options = options || {};
+
+    /**
+     * @property {Body} chassisBody
+     */
+    this.chassisBody = chassisBody;
+
+    /**
+     * @property {Array} wheels
+     */
+    this.wheels = [];
+
+    // A dummy body to constrain the chassis to
+    this.groundBody = new Body({ mass: 0 });
+
+    this.world = null;
+
+    var that = this;
+    this.preStepCallback = function(){
+        that.update();
+    };
+}
+
+/**
+ * @method addToWorld
+ * @param {World} world
+ */
+TopDownVehicle.prototype.addToWorld = function(world){
+    this.world = world;
+    world.addBody(this.groundBody);
+    world.on('preStep', this.preStepCallback);
+    for (var i = 0; i < this.wheels.length; i++) {
+        var wheel = this.wheels[i];
+        world.addConstraint(wheel);
+    }
+};
+
+/**
+ * @method removeFromWorld
+ * @param {World} world
+ */
+TopDownVehicle.prototype.removeFromWorld = function(){
+    var world = this.world;
+    world.removeBody(this.groundBody);
+    world.off('preStep', this.preStepCallback);
+    for (var i = 0; i < this.wheels.length; i++) {
+        var wheel = this.wheels[i];
+        world.removeConstraint(wheel);
+    }
+    this.world = null;
+};
+
+/**
+ * @method addWheel
+ * @param {object} [wheelOptions]
+ * @return {WheelConstraint}
+ */
+TopDownVehicle.prototype.addWheel = function(wheelOptions){
+    var wheel = new WheelConstraint(this,wheelOptions);
+    this.wheels.push(wheel);
+    return wheel;
+};
+
+/**
+ * @method update
+ */
+TopDownVehicle.prototype.update = function(){
+    for (var i = 0; i < this.wheels.length; i++) {
+        this.wheels[i].update();
+    }
+};
+
+/**
+ * @class WheelConstraint
+ * @constructor
+ * @extends {Constraint}
+ * @param {Vehicle} vehicle
+ * @param {object} [options]
+ * @param {Array} [options.localForwardVector]The local wheel forward vector in local body space. Default is zero.
+ * @param {Array} [options.localPosition] The local position of the wheen in the chassis body. Default is zero - the center of the body.
+ * @param {Array} [options.sideFriction=5] The max friction force in the sideways direction.
+ */
+function WheelConstraint(vehicle, options){
+    options = options || {};
+
+    this.vehicle = vehicle;
+
+    this.forwardEquation = new FrictionEquation(vehicle.chassisBody, vehicle.groundBody);
+
+    this.sideEquation = new FrictionEquation(vehicle.chassisBody, vehicle.groundBody);
+
+    /**
+     * @property {number} steerValue
+     */
+    this.steerValue = 0;
+
+    /**
+     * @property {number} engineForce
+     */
+    this.engineForce = 0;
+
+    this.setSideFriction(options.sideFriction !== undefined ? options.sideFriction : 5);
+
+    /**
+     * @property {Array} localForwardVector
+     */
+    this.localForwardVector = vec2.fromValues(0, 1);
+    if(options.localForwardVector){
+        vec2.copy(this.localForwardVector, options.localForwardVector);
+    }
+
+    /**
+     * @property {Array} localPosition
+     */
+    this.localPosition = vec2.fromValues(0, 0);
+    if(options.localPosition){
+        vec2.copy(this.localPosition, options.localPosition);
+    }
+
+    Constraint.apply(this, vehicle.chassisBody, vehicle.groundBody);
+
+    this.equations.push(
+        this.forwardEquation,
+        this.sideEquation
+    );
+
+    this.setBrakeForce(0);
+}
+WheelConstraint.prototype = new Constraint();
+
+/**
+ * @method setForwardFriction
+ */
+WheelConstraint.prototype.setBrakeForce = function(force){
+    this.forwardEquation.setSlipForce(force);
+};
+
+/**
+ * @method setSideFriction
+ */
+WheelConstraint.prototype.setSideFriction = function(force){
+    this.sideEquation.setSlipForce(force);
+};
+
+var worldVelocity = vec2.create();
+var relativePoint = vec2.create();
+
+/**
+ * @method getSpeed
+ */
+WheelConstraint.prototype.getSpeed = function(){
+    this.vehicle.chassisBody.vectorToWorldFrame(relativePoint, this.localForwardVector);
+    this.vehicle.chassisBody.getVelocityAtPoint(worldVelocity, relativePoint);
+    return vec2.dot(worldVelocity, relativePoint);
+};
+
+var tmpVec = vec2.create();
+
+/**
+ * @method update
+ */
+WheelConstraint.prototype.update = function(){
+
+    // Directional
+    this.vehicle.chassisBody.vectorToWorldFrame(this.forwardEquation.t, this.localForwardVector);
+    vec2.rotate(this.sideEquation.t, this.localForwardVector, Math.PI / 2);
+    this.vehicle.chassisBody.vectorToWorldFrame(this.sideEquation.t, this.sideEquation.t);
+
+    vec2.rotate(this.forwardEquation.t, this.forwardEquation.t, this.steerValue);
+    vec2.rotate(this.sideEquation.t, this.sideEquation.t, this.steerValue);
+
+    // Attachment point
+    this.vehicle.chassisBody.toWorldFrame(this.forwardEquation.contactPointB, this.localPosition);
+    vec2.copy(this.sideEquation.contactPointB, this.forwardEquation.contactPointB);
+
+    this.vehicle.chassisBody.vectorToWorldFrame(this.forwardEquation.contactPointA, this.localPosition);
+    vec2.copy(this.sideEquation.contactPointA, this.forwardEquation.contactPointA);
+
+    // Add engine force
+    vec2.normalize(tmpVec, this.forwardEquation.t);
+    vec2.scale(tmpVec, tmpVec, this.engineForce);
+
+    this.vehicle.chassisBody.applyForce(tmpVec, this.forwardEquation.contactPointA);
+};
\ No newline at end of file

src/p2.js

+// Export p2 classes
+var p2 = module.exports = {
+    AABB :                          require('./collision/AABB'),
+    AngleLockEquation :             require('./equations/AngleLockEquation'),
+    Body :                          require('./objects/Body'),
+    Broadphase :                    require('./collision/Broadphase'),
+    Capsule :                       require('./shapes/Capsule'),
+    Circle :                        require('./shapes/Circle'),
+    Constraint :                    require('./constraints/Constraint'),
+    ContactEquation :               require('./equations/ContactEquation'),
+    ContactEquationPool :           require('./utils/ContactEquationPool'),
+    ContactMaterial :               require('./material/ContactMaterial'),
+    Convex :                        require('./shapes/Convex'),
+    DistanceConstraint :            require('./constraints/DistanceConstraint'),
+    Equation :                      require('./equations/Equation'),
+    EventEmitter :                  require('./events/EventEmitter'),
+    FrictionEquation :              require('./equations/FrictionEquation'),
+    FrictionEquationPool :          require('./utils/FrictionEquationPool'),
+    GearConstraint :                require('./constraints/GearConstraint'),
+    GSSolver :                      require('./solver/GSSolver'),
+    Heightfield :                   require('./shapes/Heightfield'),
+    Line :                          require('./shapes/Line'),
+    LockConstraint :                require('./constraints/LockConstraint'),
+    Material :                      require('./material/Material'),
+    Narrowphase :                   require('./collision/Narrowphase'),
+    NaiveBroadphase :               require('./collision/NaiveBroadphase'),
+    Particle :                      require('./shapes/Particle'),
+    Plane :                         require('./shapes/Plane'),
+    Pool :                          require('./utils/Pool'),
+    RevoluteConstraint :            require('./constraints/RevoluteConstraint'),
+    PrismaticConstraint :           require('./constraints/PrismaticConstraint'),
+    Ray :                           require('./collision/Ray'),
+    RaycastResult :                 require('./collision/RaycastResult'),
+    Box :                           require('./shapes/Box'),
+    RotationalVelocityEquation :    require('./equations/RotationalVelocityEquation'),
+    SAPBroadphase :                 require('./collision/SAPBroadphase'),
+    Shape :                         require('./shapes/Shape'),
+    Solver :                        require('./solver/Solver'),
+    Spring :                        require('./objects/Spring'),
+    TopDownVehicle :                require('./objects/TopDownVehicle'),
+    LinearSpring :                  require('./objects/LinearSpring'),
+    RotationalSpring :              require('./objects/RotationalSpring'),
+    Utils :                         require('./utils/Utils'),
+    World :                         require('./world/World'),
+    vec2 :                          require('./math/vec2'),
+    version :                       '0.7.1',
+};
+
+Object.defineProperty(p2, 'Rectangle', {
+    get: function() {
+        console.warn('The Rectangle class has been renamed to Box.');
+        return this.Box;
+    }
+});
\ No newline at end of file

src/shapes/Box.js

+var vec2 = require('../math/vec2')
+,   Shape = require('./Shape')
+,   Convex = require('./Convex');
+
+module.exports = Box;
+
+/**
+ * Box shape class.
+ * @class Box
+ * @constructor
+ * @param {object} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ * @param {Number} [options.width=1] Total width of the box
+ * @param {Number} [options.height=1] Total height of the box
+ * @extends Convex
+ */
+function Box(options){
+    if(typeof(arguments[0]) === 'number' && typeof(arguments[1]) === 'number'){
+        options = {
+            width: arguments[0],
+            height: arguments[1]
+        };
+        console.warn('The Rectangle has been renamed to Box and its constructor signature has changed. Please use the following format: new Box({ width: 1, height: 1, ... })');
+    }
+    options = options || {};
+
+    /**
+     * Total width of the box
+     * @property width
+     * @type {Number}
+     */
+    var width = this.width = options.width || 1;
+
+    /**
+     * Total height of the box
+     * @property height
+     * @type {Number}
+     */
+    var height = this.height = options.height || 1;
+
+    var verts = [
+        vec2.fromValues(-width/2, -height/2),
+        vec2.fromValues( width/2, -height/2),
+        vec2.fromValues( width/2,  height/2),
+        vec2.fromValues(-width/2,  height/2)
+    ];
+    var axes = [
+        vec2.fromValues(1, 0),
+        vec2.fromValues(0, 1)
+    ];
+
+    options.vertices = verts;
+    options.axes = axes;
+    options.type = Shape.BOX;
+    Convex.call(this, options);
+}
+Box.prototype = new Convex();
+Box.prototype.constructor = Box;
+
+/**
+ * Compute moment of inertia
+ * @method computeMomentOfInertia
+ * @param  {Number} mass
+ * @return {Number}
+ */
+Box.prototype.computeMomentOfInertia = function(mass){
+    var w = this.width,
+        h = this.height;
+    return mass * (h*h + w*w) / 12;
+};
+
+/**
+ * Update the bounding radius
+ * @method updateBoundingRadius
+ */
+Box.prototype.updateBoundingRadius = function(){
+    var w = this.width,
+        h = this.height;
+    this.boundingRadius = Math.sqrt(w*w + h*h) / 2;
+};
+
+var corner1 = vec2.create(),
+    corner2 = vec2.create(),
+    corner3 = vec2.create(),
+    corner4 = vec2.create();
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out      The resulting AABB.
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Box.prototype.computeAABB = function(out, position, angle){
+    out.setFromPoints(this.vertices,position,angle,0);
+};
+
+Box.prototype.updateArea = function(){
+    this.area = this.width * this.height;
+};
+

src/shapes/Capsule.js

+var Shape = require('./Shape')
+,   vec2 = require('../math/vec2');
+
+module.exports = Capsule;
+
+/**
+ * Capsule shape class.
+ * @class Capsule
+ * @constructor
+ * @extends Shape
+ * @param {object} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ * @param {Number} [options.length=1] The distance between the end points
+ * @param {Number} [options.radius=1] Radius of the capsule
+ * @example
+ *     var capsuleShape = new Capsule({
+ *         length: 1,
+ *         radius: 2
+ *     });
+ *     body.addShape(capsuleShape);
+ */
+function Capsule(options){
+    if(typeof(arguments[0]) === 'number' && typeof(arguments[1]) === 'number'){
+        options = {
+            length: arguments[0],
+            radius: arguments[1]
+        };
+        console.warn('The Capsule constructor signature has changed. Please use the following format: new Capsule({ radius: 1, length: 1 })');
+    }
+    options = options || {};
+
+    /**
+     * The distance between the end points.
+     * @property {Number} length
+     */
+    this.length = options.length || 1;
+
+    /**
+     * The radius of the capsule.
+     * @property {Number} radius
+     */
+    this.radius = options.radius || 1;
+
+    options.type = Shape.CAPSULE;
+    Shape.call(this, options);
+}
+Capsule.prototype = new Shape();
+Capsule.prototype.constructor = Capsule;
+
+/**
+ * Compute the mass moment of inertia of the Capsule.
+ * @method conputeMomentOfInertia
+ * @param  {Number} mass
+ * @return {Number}
+ * @todo
+ */
+Capsule.prototype.computeMomentOfInertia = function(mass){
+    // Approximate with rectangle
+    var r = this.radius,
+        w = this.length + r, // 2*r is too much, 0 is too little
+        h = r*2;
+    return mass * (h*h + w*w) / 12;
+};
+
+/**
+ * @method updateBoundingRadius
+ */
+Capsule.prototype.updateBoundingRadius = function(){
+    this.boundingRadius = this.radius + this.length/2;
+};
+
+/**
+ * @method updateArea
+ */
+Capsule.prototype.updateArea = function(){
+    this.area = Math.PI * this.radius * this.radius + this.radius * 2 * this.length;
+};
+
+var r = vec2.create();
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out      The resulting AABB.
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Capsule.prototype.computeAABB = function(out, position, angle){
+    var radius = this.radius;
+
+    // Compute center position of one of the the circles, world oriented, but with local offset
+    vec2.set(r,this.length / 2,0);
+    if(angle !== 0){
+        vec2.rotate(r,r,angle);
+    }
+
+    // Get bounds
+    vec2.set(out.upperBound,  Math.max(r[0]+radius, -r[0]+radius),
+                              Math.max(r[1]+radius, -r[1]+radius));
+    vec2.set(out.lowerBound,  Math.min(r[0]-radius, -r[0]-radius),
+                              Math.min(r[1]-radius, -r[1]-radius));
+
+    // Add offset
+    vec2.add(out.lowerBound, out.lowerBound, position);
+    vec2.add(out.upperBound, out.upperBound, position);
+};
+
+var intersectCapsule_hitPointWorld = vec2.create();
+var intersectCapsule_normal = vec2.create();
+var intersectCapsule_l0 = vec2.create();
+var intersectCapsule_l1 = vec2.create();
+var intersectCapsule_unit_y = vec2.fromValues(0,1);
+
+/**
+ * @method raycast
+ * @param  {RaycastResult} result
+ * @param  {Ray} ray
+ * @param  {array} position
+ * @param  {number} angle
+ */
+Capsule.prototype.raycast = function(result, ray, position, angle){
+    var from = ray.from;
+    var to = ray.to;
+    var direction = ray.direction;
+
+    var hitPointWorld = intersectCapsule_hitPointWorld;
+    var normal = intersectCapsule_normal;
+    var l0 = intersectCapsule_l0;
+    var l1 = intersectCapsule_l1;
+
+    // The sides
+    var halfLen = this.length / 2;
+    for(var i=0; i<2; i++){
+
+        // get start and end of the line
+        var y = this.radius * (i*2-1);
+        vec2.set(l0, -halfLen, y);
+        vec2.set(l1, halfLen, y);
+        vec2.toGlobalFrame(l0, l0, position, angle);
+        vec2.toGlobalFrame(l1, l1, position, angle);
+
+        var delta = vec2.getLineSegmentsIntersectionFraction(from, to, l0, l1);
+        if(delta >= 0){
+            vec2.rotate(normal, intersectCapsule_unit_y, angle);
+            vec2.scale(normal, normal, (i*2-1));
+            ray.reportIntersection(result, delta, normal, -1);
+            if(result.shouldStop(ray)){
+                return;
+            }
+        }
+    }
+
+    // Circles
+    var diagonalLengthSquared = Math.pow(this.radius, 2) + Math.pow(halfLen, 2);
+    for(var i=0; i<2; i++){
+        vec2.set(l0, halfLen * (i*2-1), 0);
+        vec2.toGlobalFrame(l0, l0, position, angle);
+
+        var a = Math.pow(to[0] - from[0], 2) + Math.pow(to[1] - from[1], 2);
+        var b = 2 * ((to[0] - from[0]) * (from[0] - l0[0]) + (to[1] - from[1]) * (from[1] - l0[1]));
+        var c = Math.pow(from[0] - l0[0], 2) + Math.pow(from[1] - l0[1], 2) - Math.pow(this.radius, 2);
+        var delta = Math.pow(b, 2) - 4 * a * c;
+
+        if(delta < 0){
+            // No intersection
+            continue;
+
+        } else if(delta === 0){
+            // single intersection point
+            vec2.lerp(hitPointWorld, from, to, delta);
+
+            if(vec2.squaredDistance(hitPointWorld, position) > diagonalLengthSquared){
+                vec2.sub(normal, hitPointWorld, l0);
+                vec2.normalize(normal,normal);
+                ray.reportIntersection(result, delta, normal, -1);
+                if(result.shouldStop(ray)){
+                    return;
+                }
+            }
+
+        } else {
+            var sqrtDelta = Math.sqrt(delta);
+            var inv2a = 1 / (2 * a);
+            var d1 = (- b - sqrtDelta) * inv2a;
+            var d2 = (- b + sqrtDelta) * inv2a;
+
+            if(d1 >= 0 && d1 <= 1){
+                vec2.lerp(hitPointWorld, from, to, d1);
+                if(vec2.squaredDistance(hitPointWorld, position) > diagonalLengthSquared){
+                    vec2.sub(normal, hitPointWorld, l0);
+                    vec2.normalize(normal,normal);
+                    ray.reportIntersection(result, d1, normal, -1);
+                    if(result.shouldStop(ray)){
+                        return;
+                    }
+                }
+            }
+
+            if(d2 >= 0 && d2 <= 1){
+                vec2.lerp(hitPointWorld, from, to, d2);
+                if(vec2.squaredDistance(hitPointWorld, position) > diagonalLengthSquared){
+                    vec2.sub(normal, hitPointWorld, l0);
+                    vec2.normalize(normal,normal);
+                    ray.reportIntersection(result, d2, normal, -1);
+                    if(result.shouldStop(ray)){
+                        return;
+                    }
+                }
+            }
+        }
+    }
+};
\ No newline at end of file

src/shapes/Circle.js

+var Shape = require('./Shape')
+,    vec2 = require('../math/vec2');
+
+module.exports = Circle;
+
+/**
+ * Circle shape class.
+ * @class Circle
+ * @extends Shape
+ * @constructor
+ * @param {options} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ * @param {number} [options.radius=1] The radius of this circle
+ *
+ * @example
+ *     var circleShape = new Circle({ radius: 1 });
+ *     body.addShape(circleShape);
+ */
+function Circle(options){
+    if(typeof(arguments[0]) === 'number'){
+        options = {
+            radius: arguments[0]
+        };
+        console.warn('The Circle constructor signature has changed. Please use the following format: new Circle({ radius: 1 })');
+    }
+    options = options || {};
+
+    /**
+     * The radius of the circle.
+     * @property radius
+     * @type {number}
+     */
+    this.radius = options.radius || 1;
+
+    options.type = Shape.CIRCLE;
+    Shape.call(this, options);
+}
+Circle.prototype = new Shape();
+Circle.prototype.constructor = Circle;
+
+/**
+ * @method computeMomentOfInertia
+ * @param  {Number} mass
+ * @return {Number}
+ */
+Circle.prototype.computeMomentOfInertia = function(mass){
+    var r = this.radius;
+    return mass * r * r / 2;
+};
+
+/**
+ * @method updateBoundingRadius
+ * @return {Number}
+ */
+Circle.prototype.updateBoundingRadius = function(){
+    this.boundingRadius = this.radius;
+};
+
+/**
+ * @method updateArea
+ * @return {Number}
+ */
+Circle.prototype.updateArea = function(){
+    this.area = Math.PI * this.radius * this.radius;
+};
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out      The resulting AABB.
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Circle.prototype.computeAABB = function(out, position, angle){
+    var r = this.radius;
+    vec2.set(out.upperBound,  r,  r);
+    vec2.set(out.lowerBound, -r, -r);
+    if(position){
+        vec2.add(out.lowerBound, out.lowerBound, position);
+        vec2.add(out.upperBound, out.upperBound, position);
+    }
+};
+
+var Ray_intersectSphere_intersectionPoint = vec2.create();
+var Ray_intersectSphere_normal = vec2.create();
+
+/**
+ * @method raycast
+ * @param  {RaycastResult} result
+ * @param  {Ray} ray
+ * @param  {array} position
+ * @param  {number} angle
+ */
+Circle.prototype.raycast = function(result, ray, position, angle){
+    var from = ray.from,
+        to = ray.to,
+        r = this.radius;
+
+    var a = Math.pow(to[0] - from[0], 2) + Math.pow(to[1] - from[1], 2);
+    var b = 2 * ((to[0] - from[0]) * (from[0] - position[0]) + (to[1] - from[1]) * (from[1] - position[1]));
+    var c = Math.pow(from[0] - position[0], 2) + Math.pow(from[1] - position[1], 2) - Math.pow(r, 2);
+    var delta = Math.pow(b, 2) - 4 * a * c;
+
+    var intersectionPoint = Ray_intersectSphere_intersectionPoint;
+    var normal = Ray_intersectSphere_normal;
+
+    if(delta < 0){
+        // No intersection
+        return;
+
+    } else if(delta === 0){
+        // single intersection point
+        vec2.lerp(intersectionPoint, from, to, delta);
+
+        vec2.sub(normal, intersectionPoint, position);
+        vec2.normalize(normal,normal);
+
+        ray.reportIntersection(result, delta, normal, -1);
+
+    } else {
+        var sqrtDelta = Math.sqrt(delta);
+        var inv2a = 1 / (2 * a);
+        var d1 = (- b - sqrtDelta) * inv2a;
+        var d2 = (- b + sqrtDelta) * inv2a;
+
+        if(d1 >= 0 && d1 <= 1){
+            vec2.lerp(intersectionPoint, from, to, d1);
+
+            vec2.sub(normal, intersectionPoint, position);
+            vec2.normalize(normal,normal);
+
+            ray.reportIntersection(result, d1, normal, -1);
+
+            if(result.shouldStop(ray)){
+                return;
+            }
+        }
+
+        if(d2 >= 0 && d2 <= 1){
+            vec2.lerp(intersectionPoint, from, to, d2);
+
+            vec2.sub(normal, intersectionPoint, position);
+            vec2.normalize(normal,normal);
+
+            ray.reportIntersection(result, d2, normal, -1);
+        }
+    }
+};
\ No newline at end of file

src/shapes/Convex.js

+var Shape = require('./Shape')
+,   vec2 = require('../math/vec2')
+,   polyk = require('../math/polyk')
+,   decomp = require('poly-decomp');
+
+module.exports = Convex;
+
+/**
+ * Convex shape class.
+ * @class Convex
+ * @constructor
+ * @extends Shape
+ * @param {object} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ * @param {Array} [options.vertices] An array of vertices that span this shape. Vertices are given in counter-clockwise (CCW) direction.
+ * @param {Array} [options.axes] An array of unit length vectors, representing the symmetry axes in the convex.
+ * @example
+ *     // Create a box
+ *     var vertices = [[-1,-1], [1,-1], [1,1], [-1,1]];
+ *     var convexShape = new Convex({ vertices: vertices });
+ *     body.addShape(convexShape);
+ */
+function Convex(options){
+    if(Array.isArray(arguments[0])){
+        options = {
+            vertices: arguments[0],
+            axes: arguments[1]
+        };
+        console.warn('The Convex constructor signature has changed. Please use the following format: new Convex({ vertices: [...], ... })');
+    }
+    options = options || {};
+
+    /**
+     * Vertices defined in the local frame.
+     * @property vertices
+     * @type {Array}
+     */
+    this.vertices = [];
+
+    // Copy the verts
+    var vertices = options.vertices !== undefined ? options.vertices : [];
+    for(var i=0; i < vertices.length; i++){
+        var v = vec2.create();
+        vec2.copy(v, vertices[i]);
+        this.vertices.push(v);
+    }
+
+    /**
+     * Axes defined in the local frame.
+     * @property axes
+     * @type {Array}
+     */
+    this.axes = [];
+
+    if(options.axes){
+
+        // Copy the axes
+        for(var i=0; i < options.axes.length; i++){
+            var axis = vec2.create();
+            vec2.copy(axis, options.axes[i]);
+            this.axes.push(axis);
+        }
+
+    } else {
+
+        // Construct axes from the vertex data
+        for(var i = 0; i < this.vertices.length; i++){
+            // Get the world edge
+            var worldPoint0 = this.vertices[i];
+            var worldPoint1 = this.vertices[(i+1) % this.vertices.length];
+
+            var normal = vec2.create();
+            vec2.sub(normal, worldPoint1, worldPoint0);
+
+            // Get normal - just rotate 90 degrees since vertices are given in CCW
+            vec2.rotate90cw(normal, normal);
+            vec2.normalize(normal, normal);
+
+            this.axes.push(normal);
+        }
+
+    }
+
+    /**
+     * The center of mass of the Convex
+     * @property centerOfMass
+     * @type {Array}
+     */
+    this.centerOfMass = vec2.fromValues(0,0);
+
+    /**
+     * Triangulated version of this convex. The structure is Array of 3-Arrays, and each subarray contains 3 integers, referencing the vertices.
+     * @property triangles
+     * @type {Array}
+     */
+    this.triangles = [];
+
+    if(this.vertices.length){
+        this.updateTriangles();
+        this.updateCenterOfMass();
+    }
+
+    /**
+     * The bounding radius of the convex
+     * @property boundingRadius
+     * @type {Number}
+     */
+    this.boundingRadius = 0;
+
+    options.type = Shape.CONVEX;
+    Shape.call(this, options);
+
+    this.updateBoundingRadius();
+    this.updateArea();
+    if(this.area < 0){
+        throw new Error("Convex vertices must be given in conter-clockwise winding.");
+    }
+}
+Convex.prototype = new Shape();
+Convex.prototype.constructor = Convex;
+
+var tmpVec1 = vec2.create();
+var tmpVec2 = vec2.create();
+
+/**
+ * Project a Convex onto a world-oriented axis
+ * @method projectOntoAxis
+ * @static
+ * @param  {Array} offset
+ * @param  {Array} localAxis
+ * @param  {Array} result
+ */
+Convex.prototype.projectOntoLocalAxis = function(localAxis, result){
+    var max=null,
+        min=null,
+        v,
+        value,
+        localAxis = tmpVec1;
+
+    // Get projected position of all vertices
+    for(var i=0; i<this.vertices.length; i++){
+        v = this.vertices[i];
+        value = vec2.dot(v, localAxis);
+        if(max === null || value > max){
+            max = value;
+        }
+        if(min === null || value < min){
+            min = value;
+        }
+    }
+
+    if(min > max){
+        var t = min;
+        min = max;
+        max = t;
+    }
+
+    vec2.set(result, min, max);
+};
+
+Convex.prototype.projectOntoWorldAxis = function(localAxis, shapeOffset, shapeAngle, result){
+    var worldAxis = tmpVec2;
+
+    this.projectOntoLocalAxis(localAxis, result);
+
+    // Project the position of the body onto the axis - need to add this to the result
+    if(shapeAngle !== 0){
+        vec2.rotate(worldAxis, localAxis, shapeAngle);
+    } else {
+        worldAxis = localAxis;
+    }
+    var offset = vec2.dot(shapeOffset, worldAxis);
+
+    vec2.set(result, result[0] + offset, result[1] + offset);
+};
+
+
+/**
+ * Update the .triangles property
+ * @method updateTriangles
+ */
+Convex.prototype.updateTriangles = function(){
+
+    this.triangles.length = 0;
+
+    // Rewrite on polyk notation, array of numbers
+    var polykVerts = [];
+    for(var i=0; i<this.vertices.length; i++){
+        var v = this.vertices[i];
+        polykVerts.push(v[0],v[1]);
+    }
+
+    // Triangulate
+    var triangles = polyk.Triangulate(polykVerts);
+
+    // Loop over all triangles, add their inertia contributions to I
+    for(var i=0; i<triangles.length; i+=3){
+        var id1 = triangles[i],
+            id2 = triangles[i+1],
+            id3 = triangles[i+2];
+
+        // Add to triangles
+        this.triangles.push([id1,id2,id3]);
+    }
+};
+
+var updateCenterOfMass_centroid = vec2.create(),
+    updateCenterOfMass_centroid_times_mass = vec2.create(),
+    updateCenterOfMass_a = vec2.create(),
+    updateCenterOfMass_b = vec2.create(),
+    updateCenterOfMass_c = vec2.create(),
+    updateCenterOfMass_ac = vec2.create(),
+    updateCenterOfMass_ca = vec2.create(),
+    updateCenterOfMass_cb = vec2.create(),
+    updateCenterOfMass_n = vec2.create();
+
+/**
+ * Update the .centerOfMass property.
+ * @method updateCenterOfMass
+ */
+Convex.prototype.updateCenterOfMass = function(){
+    var triangles = this.triangles,
+        verts = this.vertices,
+        cm = this.centerOfMass,
+        centroid = updateCenterOfMass_centroid,
+        n = updateCenterOfMass_n,
+        a = updateCenterOfMass_a,
+        b = updateCenterOfMass_b,
+        c = updateCenterOfMass_c,
+        ac = updateCenterOfMass_ac,
+        ca = updateCenterOfMass_ca,
+        cb = updateCenterOfMass_cb,
+        centroid_times_mass = updateCenterOfMass_centroid_times_mass;
+
+    vec2.set(cm,0,0);
+    var totalArea = 0;
+
+    for(var i=0; i!==triangles.length; i++){
+        var t = triangles[i],
+            a = verts[t[0]],
+            b = verts[t[1]],
+            c = verts[t[2]];
+
+        vec2.centroid(centroid,a,b,c);
+
+        // Get mass for the triangle (density=1 in this case)
+        // http://math.stackexchange.com/questions/80198/area-of-triangle-via-vectors
+        var m = Convex.triangleArea(a,b,c);
+        totalArea += m;
+
+        // Add to center of mass
+        vec2.scale(centroid_times_mass, centroid, m);
+        vec2.add(cm, cm, centroid_times_mass);
+    }
+
+    vec2.scale(cm,cm,1/totalArea);
+};
+
+/**
+ * Compute the mass moment of inertia of the Convex.
+ * @method computeMomentOfInertia
+ * @param  {Number} mass
+ * @return {Number}
+ * @see http://www.gamedev.net/topic/342822-moment-of-inertia-of-a-polygon-2d/
+ */
+Convex.prototype.computeMomentOfInertia = function(mass){
+    var denom = 0.0,
+        numer = 0.0,
+        N = this.vertices.length;
+    for(var j = N-1, i = 0; i < N; j = i, i ++){
+        var p0 = this.vertices[j];
+        var p1 = this.vertices[i];
+        var a = Math.abs(vec2.crossLength(p0,p1));
+        var b = vec2.dot(p1,p1) + vec2.dot(p1,p0) + vec2.dot(p0,p0);
+        denom += a * b;
+        numer += a;
+    }
+    return (mass / 6.0) * (denom / numer);
+};
+
+/**
+ * Updates the .boundingRadius property
+ * @method updateBoundingRadius
+ */
+Convex.prototype.updateBoundingRadius = function(){
+    var verts = this.vertices,
+        r2 = 0;
+
+    for(var i=0; i!==verts.length; i++){
+        var l2 = vec2.squaredLength(verts[i]);
+        if(l2 > r2){
+            r2 = l2;
+        }
+    }
+
+    this.boundingRadius = Math.sqrt(r2);
+};
+
+/**
+ * Get the area of the triangle spanned by the three points a, b, c. The area is positive if the points are given in counter-clockwise order, otherwise negative.
+ * @static
+ * @method triangleArea
+ * @param {Array} a
+ * @param {Array} b
+ * @param {Array} c
+ * @return {Number}
+ */
+Convex.triangleArea = function(a,b,c){
+    return (((b[0] - a[0])*(c[1] - a[1]))-((c[0] - a[0])*(b[1] - a[1]))) * 0.5;
+};
+
+/**
+ * Update the .area
+ * @method updateArea
+ */
+Convex.prototype.updateArea = function(){
+    this.updateTriangles();
+    this.area = 0;
+
+    var triangles = this.triangles,
+        verts = this.vertices;
+    for(var i=0; i!==triangles.length; i++){
+        var t = triangles[i],
+            a = verts[t[0]],
+            b = verts[t[1]],
+            c = verts[t[2]];
+
+        // Get mass for the triangle (density=1 in this case)
+        var m = Convex.triangleArea(a,b,c);
+        this.area += m;
+    }
+};
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Convex.prototype.computeAABB = function(out, position, angle){
+    out.setFromPoints(this.vertices, position, angle, 0);
+};
+
+var intersectConvex_rayStart = vec2.create();
+var intersectConvex_rayEnd = vec2.create();
+var intersectConvex_normal = vec2.create();
+
+/**
+ * @method raycast
+ * @param  {RaycastResult} result
+ * @param  {Ray} ray
+ * @param  {array} position
+ * @param  {number} angle
+ */
+Convex.prototype.raycast = function(result, ray, position, angle){
+    var rayStart = intersectConvex_rayStart;
+    var rayEnd = intersectConvex_rayEnd;
+    var normal = intersectConvex_normal;
+    var vertices = this.vertices;
+
+    // Transform to local shape space
+    vec2.toLocalFrame(rayStart, ray.from, position, angle);
+    vec2.toLocalFrame(rayEnd, ray.to, position, angle);
+
+    var n = vertices.length;
+
+    for (var i = 0; i < n && !result.shouldStop(ray); i++) {
+        var q1 = vertices[i];
+        var q2 = vertices[(i+1) % n];
+        var delta = vec2.getLineSegmentsIntersectionFraction(rayStart, rayEnd, q1, q2);
+
+        if(delta >= 0){
+            vec2.sub(normal, q2, q1);
+            vec2.rotate(normal, normal, -Math.PI / 2 + angle);
+            vec2.normalize(normal, normal);
+            ray.reportIntersection(result, delta, normal, i);
+        }
+    }
+};

src/shapes/Heightfield.js

+var Shape = require('./Shape')
+,    vec2 = require('../math/vec2')
+,    Utils = require('../utils/Utils');
+
+module.exports = Heightfield;
+
+/**
+ * Heightfield shape class. Height data is given as an array. These data points are spread out evenly with a distance "elementWidth".
+ * @class Heightfield
+ * @extends Shape
+ * @constructor
+ * @param {object} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ * @param {array} [options.heights] An array of Y values that will be used to construct the terrain.
+ * @param {Number} [options.minValue] Minimum value of the data points in the data array. Will be computed automatically if not given.
+ * @param {Number} [options.maxValue] Maximum value.
+ * @param {Number} [options.elementWidth=0.1] World spacing between the data points in X direction.
+ *
+ * @example
+ *     // Generate some height data (y-values).
+ *     var heights = [];
+ *     for(var i = 0; i < 1000; i++){
+ *         var y = 0.5 * Math.cos(0.2 * i);
+ *         heights.push(y);
+ *     }
+ *
+ *     // Create the heightfield shape
+ *     var heightfieldShape = new Heightfield({
+ *         heights: heights,
+ *         elementWidth: 1 // Distance between the data points in X direction
+ *     });
+ *     var heightfieldBody = new Body();
+ *     heightfieldBody.addShape(heightfieldShape);
+ *     world.addBody(heightfieldBody);
+ *
+ * @todo Should use a scale property with X and Y direction instead of just elementWidth
+ */
+function Heightfield(options){
+    if(Array.isArray(arguments[0])){
+        options = {
+            heights: arguments[0]
+        };
+
+        if(typeof(arguments[1]) === 'object'){
+            for(var key in arguments[1]){
+                options[key] = arguments[1][key];
+            }
+        }
+
+        console.warn('The Heightfield constructor signature has changed. Please use the following format: new Heightfield({ heights: [...], ... })');
+    }
+    options = options || {};
+
+    /**
+     * An array of numbers, or height values, that are spread out along the x axis.
+     * @property {array} heights
+     */
+    this.heights = options.heights ? options.heights.slice(0) : [];
+
+    /**
+     * Max value of the heights
+     * @property {number} maxValue
+     */
+    this.maxValue = options.maxValue || null;
+
+    /**
+     * Max value of the heights
+     * @property {number} minValue
+     */
+    this.minValue = options.minValue || null;
+
+    /**
+     * The width of each element
+     * @property {number} elementWidth
+     */
+    this.elementWidth = options.elementWidth || 0.1;
+
+    if(options.maxValue === undefined || options.minValue === undefined){
+        this.updateMaxMinValues();
+    }
+
+    options.type = Shape.HEIGHTFIELD;
+    Shape.call(this, options);
+}
+Heightfield.prototype = new Shape();
+Heightfield.prototype.constructor = Heightfield;
+
+/**
+ * Update the .minValue and the .maxValue
+ * @method updateMaxMinValues
+ */
+Heightfield.prototype.updateMaxMinValues = function(){
+    var data = this.heights;
+    var maxValue = data[0];
+    var minValue = data[0];
+    for(var i=0; i !== data.length; i++){
+        var v = data[i];
+        if(v > maxValue){
+            maxValue = v;
+        }
+        if(v < minValue){
+            minValue = v;
+        }
+    }
+    this.maxValue = maxValue;
+    this.minValue = minValue;
+};
+
+/**
+ * @method computeMomentOfInertia
+ * @param  {Number} mass
+ * @return {Number}
+ */
+Heightfield.prototype.computeMomentOfInertia = function(mass){
+    return Number.MAX_VALUE;
+};
+
+Heightfield.prototype.updateBoundingRadius = function(){
+    this.boundingRadius = Number.MAX_VALUE;
+};
+
+Heightfield.prototype.updateArea = function(){
+    var data = this.heights,
+        area = 0;
+    for(var i=0; i<data.length-1; i++){
+        area += (data[i]+data[i+1]) / 2 * this.elementWidth;
+    }
+    this.area = area;
+};
+
+var points = [
+    vec2.create(),
+    vec2.create(),
+    vec2.create(),
+    vec2.create()
+];
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out      The resulting AABB.
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Heightfield.prototype.computeAABB = function(out, position, angle){
+    vec2.set(points[0], 0, this.maxValue);
+    vec2.set(points[1], this.elementWidth * this.heights.length, this.maxValue);
+    vec2.set(points[2], this.elementWidth * this.heights.length, this.minValue);
+    vec2.set(points[3], 0, this.minValue);
+    out.setFromPoints(points, position, angle);
+};
+
+/**
+ * Get a line segment in the heightfield
+ * @method getLineSegment
+ * @param  {array} start Where to store the resulting start point
+ * @param  {array} end Where to store the resulting end point
+ * @param  {number} i
+ */
+Heightfield.prototype.getLineSegment = function(start, end, i){
+    var data = this.heights;
+    var width = this.elementWidth;
+    vec2.set(start, i * width, data[i]);
+    vec2.set(end, (i + 1) * width, data[i + 1]);
+};
+
+Heightfield.prototype.getSegmentIndex = function(position){
+    return Math.floor(position[0] / this.elementWidth);
+};
+
+Heightfield.prototype.getClampedSegmentIndex = function(position){
+    var i = this.getSegmentIndex(position);
+    i = Math.min(this.heights.length, Math.max(i, 0)); // clamp
+    return i;
+};
+
+var intersectHeightfield_hitPointWorld = vec2.create();
+var intersectHeightfield_worldNormal = vec2.create();
+var intersectHeightfield_l0 = vec2.create();
+var intersectHeightfield_l1 = vec2.create();
+var intersectHeightfield_localFrom = vec2.create();
+var intersectHeightfield_localTo = vec2.create();
+var intersectHeightfield_unit_y = vec2.fromValues(0,1);
+
+// Returns 1 if the lines intersect, otherwise 0.
+function getLineSegmentsIntersection (out, p0, p1, p2, p3) {
+
+    var s1_x, s1_y, s2_x, s2_y;
+    s1_x = p1[0] - p0[0];
+    s1_y = p1[1] - p0[1];
+    s2_x = p3[0] - p2[0];
+    s2_y = p3[1] - p2[1];
+
+    var s, t;
+    s = (-s1_y * (p0[0] - p2[0]) + s1_x * (p0[1] - p2[1])) / (-s2_x * s1_y + s1_x * s2_y);
+    t = ( s2_x * (p0[1] - p2[1]) - s2_y * (p0[0] - p2[0])) / (-s2_x * s1_y + s1_x * s2_y);
+    if (s >= 0 && s <= 1 && t >= 0 && t <= 1) { // Collision detected
+        var intX = p0[0] + (t * s1_x);
+        var intY = p0[1] + (t * s1_y);
+        out[0] = intX;
+        out[1] = intY;
+        return t;
+    }
+    return -1; // No collision
+}
+
+/**
+ * @method raycast
+ * @param  {RayResult} result
+ * @param  {Ray} ray
+ * @param  {array} position
+ * @param  {number} angle
+ */
+Heightfield.prototype.raycast = function(result, ray, position, angle){
+    var from = ray.from;
+    var to = ray.to;
+    var direction = ray.direction;
+
+    var hitPointWorld = intersectHeightfield_hitPointWorld;
+    var worldNormal = intersectHeightfield_worldNormal;
+    var l0 = intersectHeightfield_l0;
+    var l1 = intersectHeightfield_l1;
+    var localFrom = intersectHeightfield_localFrom;
+    var localTo = intersectHeightfield_localTo;
+
+    // get local ray start and end
+    vec2.toLocalFrame(localFrom, from, position, angle);
+    vec2.toLocalFrame(localTo, to, position, angle);
+
+    // Get the segment range
+    var i0 = this.getClampedSegmentIndex(localFrom);
+    var i1 = this.getClampedSegmentIndex(localTo);
+    if(i0 > i1){
+        var tmp = i0;
+        i0 = i1;
+        i1 = tmp;
+    }
+
+    // The segments
+    for(var i=0; i<this.heights.length - 1; i++){
+        this.getLineSegment(l0, l1, i);
+        var t = vec2.getLineSegmentsIntersectionFraction(localFrom, localTo, l0, l1);
+        if(t >= 0){
+            vec2.sub(worldNormal, l1, l0);
+            vec2.rotate(worldNormal, worldNormal, angle + Math.PI / 2);
+            vec2.normalize(worldNormal, worldNormal);
+            ray.reportIntersection(result, t, worldNormal, -1);
+            if(result.shouldStop(ray)){
+                return;
+            }
+        }
+    }
+};
\ No newline at end of file

src/shapes/Line.js

+var Shape = require('./Shape')
+,   vec2 = require('../math/vec2');
+
+module.exports = Line;
+
+/**
+ * Line shape class. The line shape is along the x direction, and stretches from [-length/2, 0] to [length/2,0].
+ * @class Line
+ * @param {object} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ * @param {Number} [options.length=1] The total length of the line
+ * @extends Shape
+ * @constructor
+ */
+function Line(options){
+    if(typeof(arguments[0]) === 'number'){
+        options = {
+            length: arguments[0]
+        };
+        console.warn('The Line constructor signature has changed. Please use the following format: new Line({ length: 1, ... })');
+    }
+    options = options || {};
+
+    /**
+     * Length of this line
+     * @property {Number} length
+     * @default 1
+     */
+    this.length = options.length || 1;
+
+    options.type = Shape.LINE;
+    Shape.call(this, options);
+}
+Line.prototype = new Shape();
+Line.prototype.constructor = Line;
+
+Line.prototype.computeMomentOfInertia = function(mass){
+    return mass * Math.pow(this.length,2) / 12;
+};
+
+Line.prototype.updateBoundingRadius = function(){
+    this.boundingRadius = this.length/2;
+};
+
+var points = [vec2.create(),vec2.create()];
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out      The resulting AABB.
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Line.prototype.computeAABB = function(out, position, angle){
+    var l2 = this.length / 2;
+    vec2.set(points[0], -l2,  0);
+    vec2.set(points[1],  l2,  0);
+    out.setFromPoints(points,position,angle,0);
+};
+
+var raycast_hitPoint = vec2.create();
+var raycast_normal = vec2.create();
+var raycast_l0 = vec2.create();
+var raycast_l1 = vec2.create();
+var raycast_unit_y = vec2.fromValues(0,1);
+
+/**
+ * @method raycast
+ * @param  {RaycastResult} result
+ * @param  {Ray} ray
+ * @param  {number} angle
+ * @param  {array} position
+ */
+Line.prototype.raycast = function(result, ray, position, angle){
+    var from = ray.from;
+    var to = ray.to;
+
+    var l0 = raycast_l0;
+    var l1 = raycast_l1;
+
+    // get start and end of the line
+    var halfLen = this.length / 2;
+    vec2.set(l0, -halfLen, 0);
+    vec2.set(l1, halfLen, 0);
+    vec2.toGlobalFrame(l0, l0, position, angle);
+    vec2.toGlobalFrame(l1, l1, position, angle);
+
+    var fraction = vec2.getLineSegmentsIntersectionFraction(l0, l1, from, to);
+    if(fraction >= 0){
+        var normal = raycast_normal;
+        vec2.rotate(normal, raycast_unit_y, angle); // todo: this should depend on which side the ray comes from
+        ray.reportIntersection(result, fraction, normal, -1);
+    }
+};
\ No newline at end of file

src/shapes/Particle.js

+var Shape = require('./Shape')
+,   vec2 = require('../math/vec2');
+
+module.exports = Particle;
+
+/**
+ * Particle shape class.
+ * @class Particle
+ * @constructor
+ * @param {object} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ * @extends Shape
+ */
+function Particle(options){
+    options = options || {};
+	options.type = Shape.PARTICLE;
+    Shape.call(this, options);
+}
+Particle.prototype = new Shape();
+Particle.prototype.constructor = Particle;
+
+Particle.prototype.computeMomentOfInertia = function(mass){
+    return 0; // Can't rotate a particle
+};
+
+Particle.prototype.updateBoundingRadius = function(){
+    this.boundingRadius = 0;
+};
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Particle.prototype.computeAABB = function(out, position, angle){
+    vec2.copy(out.lowerBound, position);
+    vec2.copy(out.upperBound, position);
+};

src/shapes/Plane.js

+var Shape =  require('./Shape')
+,    vec2 =  require('../math/vec2')
+,    Utils = require('../utils/Utils');
+
+module.exports = Plane;
+
+/**
+ * Plane shape class. The plane is facing in the Y direction.
+ * @class Plane
+ * @extends Shape
+ * @constructor
+ * @param {object} [options] (Note that this options object will be passed on to the {{#crossLink "Shape"}}{{/crossLink}} constructor.)
+ */
+function Plane(options){
+    options = options || {};
+    options.type = Shape.PLANE;
+    Shape.call(this, options);
+}
+Plane.prototype = new Shape();
+Plane.prototype.constructor = Plane;
+
+/**
+ * Compute moment of inertia
+ * @method computeMomentOfInertia
+ */
+Plane.prototype.computeMomentOfInertia = function(mass){
+    return 0; // Plane is infinite. The inertia should therefore be infinty but by convention we set 0 here
+};
+
+/**
+ * Update the bounding radius
+ * @method updateBoundingRadius
+ */
+Plane.prototype.updateBoundingRadius = function(){
+    this.boundingRadius = Number.MAX_VALUE;
+};
+
+/**
+ * @method computeAABB
+ * @param  {AABB}   out
+ * @param  {Array}  position
+ * @param  {Number} angle
+ */
+Plane.prototype.computeAABB = function(out, position, angle){
+    var a = angle % (2 * Math.PI);
+    var set = vec2.set;
+    var max = 1e7;
+    var lowerBound = out.lowerBound;
+    var upperBound = out.upperBound;
+
+    // Set max bounds
+    set(lowerBound, -max, -max);
+    set(upperBound,  max,  max);
+
+    if(a === 0){
+        // y goes from -inf to 0
+        upperBound[1] = 0;
+        // set(lowerBound, -max, -max);
+        // set(upperBound,  max,  0);
+
+    } else if(a === Math.PI / 2){
+
+        // x goes from 0 to inf
+        lowerBound[0] = 0;
+        // set(lowerBound, 0, -max);
+        // set(upperBound,      max,  max);
+
+    } else if(a === Math.PI){
+
+        // y goes from 0 to inf
+        lowerBound[1] = 0;
+        // set(lowerBound, -max, 0);
+        // set(upperBound,  max, max);
+
+    } else if(a === 3*Math.PI/2){
+
+        // x goes from -inf to 0
+        upperBound[0] = 0;
+        // set(lowerBound, -max,     -max);
+        // set(upperBound,  0,  max);
+
+    }
+};
+
+Plane.prototype.updateArea = function(){
+    this.area = Number.MAX_VALUE;
+};
+
+var intersectPlane_planePointToFrom = vec2.create();
+var intersectPlane_dir_scaled_with_t = vec2.create();
+var intersectPlane_hitPoint = vec2.create();
+var intersectPlane_normal = vec2.create();
+var intersectPlane_len = vec2.create();
+
+/**
+ * @method raycast
+ * @param  {RayResult} result
+ * @param  {Ray} ray
+ * @param  {array} position
+ * @param  {number} angle
+ */
+Plane.prototype.raycast = function(result, ray, position, angle){
+    var from = ray.from;
+    var to = ray.to;
+    var direction = ray.direction;
+    var planePointToFrom = intersectPlane_planePointToFrom;
+    var dir_scaled_with_t = intersectPlane_dir_scaled_with_t;
+    var hitPoint = intersectPlane_hitPoint;
+    var normal = intersectPlane_normal;
+    var len = intersectPlane_len;
+
+    // Get plane normal
+    vec2.set(normal, 0, 1);
+    vec2.rotate(normal, normal, angle);
+
+    vec2.sub(len, from, position);
+    var planeToFrom = vec2.dot(len, normal);
+    vec2.sub(len, to, position);
+    var planeToTo = vec2.dot(len, normal);
+
+    if(planeToFrom * planeToTo > 0){
+        // "from" and "to" are on the same side of the plane... bail out
+        return;
+    }
+
+    if(vec2.squaredDistance(from, to) < planeToFrom * planeToFrom){
+        return;
+    }
+
+    var n_dot_dir = vec2.dot(normal, direction);
+
+    vec2.sub(planePointToFrom, from, position);
+    var t = -vec2.dot(normal, planePointToFrom) / n_dot_dir / ray.length;
+
+    ray.reportIntersection(result, t, normal, -1);
+};
\ No newline at end of file

src/shapes/Shape.js

+module.exports = Shape;
+
+var vec2 = require('../math/vec2');
+
+/**
+ * Base class for shapes.
+ * @class Shape
+ * @constructor
+ * @param {object} [options]
+ * @param {array} [options.position]
+ * @param {number} [options.angle=0]
+ * @param {number} [options.collisionGroup=1]
+ * @param {number} [options.collisionMask=1]
+ * @param {boolean} [options.sensor=false]
+ * @param {boolean} [options.collisionResponse=true]
+ * @param {object} [options.type=0]
+ */
+function Shape(options){
+    options = options || {};
+
+    /**
+     * The body this shape is attached to. A shape can only be attached to a single body.
+     * @property {Body} body
+     */
+    this.body = null;
+
+    /**
+     * Body-local position of the shape.
+     * @property {Array} position
+     */
+    this.position = vec2.fromValues(0,0);
+    if(options.position){
+        vec2.copy(this.position, options.position);
+    }
+
+    /**
+     * Body-local angle of the shape.
+     * @property {number} angle
+     */
+    this.angle = options.angle || 0;
+
+    /**
+     * The type of the shape. One of:
+     *
+     * * {{#crossLink "Shape/CIRCLE:property"}}Shape.CIRCLE{{/crossLink}}
+     * * {{#crossLink "Shape/PARTICLE:property"}}Shape.PARTICLE{{/crossLink}}
+     * * {{#crossLink "Shape/PLANE:property"}}Shape.PLANE{{/crossLink}}
+     * * {{#crossLink "Shape/CONVEX:property"}}Shape.CONVEX{{/crossLink}}
+     * * {{#crossLink "Shape/LINE:property"}}Shape.LINE{{/crossLink}}
+     * * {{#crossLink "Shape/BOX:property"}}Shape.BOX{{/crossLink}}
+     * * {{#crossLink "Shape/CAPSULE:property"}}Shape.CAPSULE{{/crossLink}}
+     * * {{#crossLink "Shape/HEIGHTFIELD:property"}}Shape.HEIGHTFIELD{{/crossLink}}
+     *
+     * @property {number} type
+     */
+    this.type = options.type || 0;
+
+    /**
+     * Shape object identifier.
+     * @type {Number}
+     * @property id
+     */
+    this.id = Shape.idCounter++;
+
+    /**
+     * Bounding circle radius of this shape
+     * @property boundingRadius
+     * @type {Number}
+     */
+    this.boundingRadius = 0;
+
+    /**
+     * Collision group that this shape belongs to (bit mask). See <a href="http://www.aurelienribon.com/blog/2011/07/box2d-tutorial-collision-filtering/">this tutorial</a>.
+     * @property collisionGroup
+     * @type {Number}
+     * @example
+     *     // Setup bits for each available group
+     *     var PLAYER = Math.pow(2,0),
+     *         ENEMY =  Math.pow(2,1),
+     *         GROUND = Math.pow(2,2)
+     *
+     *     // Put shapes into their groups
+     *     player1Shape.collisionGroup = PLAYER;
+     *     player2Shape.collisionGroup = PLAYER;
+     *     enemyShape  .collisionGroup = ENEMY;
+     *     groundShape .collisionGroup = GROUND;
+     *
+     *     // Assign groups that each shape collide with.
+     *     // Note that the players can collide with ground and enemies, but not with other players.
+     *     player1Shape.collisionMask = ENEMY | GROUND;
+     *     player2Shape.collisionMask = ENEMY | GROUND;
+     *     enemyShape  .collisionMask = PLAYER | GROUND;
+     *     groundShape .collisionMask = PLAYER | ENEMY;
+     *
+     * @example
+     *     // How collision check is done
+     *     if(shapeA.collisionGroup & shapeB.collisionMask)!=0 && (shapeB.collisionGroup & shapeA.collisionMask)!=0){
+     *         // The shapes will collide
+     *     }
+     */
+    this.collisionGroup = options.collisionGroup !== undefined ? options.collisionGroup : 1;
+
+    /**
+     * Whether to produce contact forces when in contact with other bodies. Note that contacts will be generated, but they will be disabled. That means that this shape will move through other body shapes, but it will still trigger contact events, etc.
+     * @property {Boolean} collisionResponse
+     */
+    this.collisionResponse = options.collisionResponse !== undefined ? options.collisionResponse : true;
+
+    /**
+     * Collision mask of this shape. See .collisionGroup.
+     * @property collisionMask
+     * @type {Number}
+     */
+    this.collisionMask = options.collisionMask !== undefined ? options.collisionMask : 1;
+
+    /**
+     * Material to use in collisions for this Shape. If this is set to null, the world will use default material properties instead.
+     * @property material
+     * @type {Material}
+     */
+    this.material = options.material || null;
+
+    /**
+     * Area of this shape.
+     * @property area
+     * @type {Number}
+     */
+    this.area = 0;
+
+    /**
+     * Set to true if you want this shape to be a sensor. A sensor does not generate contacts, but it still reports contact events. This is good if you want to know if a shape is overlapping another shape, without them generating contacts.
+     * @property {Boolean} sensor
+     */
+    this.sensor = options.sensor !== undefined ? options.sensor : false;
+
+    if(this.type){
+        this.updateBoundingRadius();
+    }
+
+    this.updateArea();
+}
+
+Shape.idCounter = 0;
+
+/**
+ * @static
+ * @property {Number} CIRCLE
+ */
+Shape.CIRCLE =      1;
+
+/**
+ * @static
+ * @property {Number} PARTICLE
+ */
+Shape.PARTICLE =    2;
+
+/**
+ * @static
+ * @property {Number} PLANE
+ */
+Shape.PLANE =       4;
+
+/**
+ * @static
+ * @property {Number} CONVEX
+ */
+Shape.CONVEX =      8;
+
+/**
+ * @static
+ * @property {Number} LINE
+ */
+Shape.LINE =        16;
+
+/**
+ * @static
+ * @property {Number} BOX
+ */
+Shape.BOX =   32;
+
+Object.defineProperty(Shape, 'RECTANGLE', {
+    get: function() {
+        console.warn('Shape.RECTANGLE is deprecated, use Shape.BOX instead.');
+        return Shape.BOX;
+    }
+});
+
+/**
+ * @static
+ * @property {Number} CAPSULE
+ */
+Shape.CAPSULE =     64;
+
+/**
+ * @static
+ * @property {Number} HEIGHTFIELD
+ */
+Shape.HEIGHTFIELD = 128;
+
+/**
+ * Should return the moment of inertia around the Z axis of the body given the total mass. See <a href="http://en.wikipedia.org/wiki/List_of_moments_of_inertia">Wikipedia's list of moments of inertia</a>.
+ * @method computeMomentOfInertia
+ * @param  {Number} mass
+ * @return {Number} If the inertia is infinity or if the object simply isn't possible to rotate, return 0.
+ */
+Shape.prototype.computeMomentOfInertia = function(mass){};
+
+/**
+ * Returns the bounding circle radius of this shape.
+ * @method updateBoundingRadius
+ * @return {Number}
+ */
+Shape.prototype.updateBoundingRadius = function(){};
+
+/**
+ * Update the .area property of the shape.
+ * @method updateArea
+ */
+Shape.prototype.updateArea = function(){
+    // To be implemented in all subclasses
+};
+
+/**
+ * Compute the world axis-aligned bounding box (AABB) of this shape.
+ * @method computeAABB
+ * @param  {AABB} out The resulting AABB.
+ * @param  {Array} position World position of the shape.
+ * @param  {Number} angle World angle of the shape.
+ */
+Shape.prototype.computeAABB = function(out, position, angle){
+    // To be implemented in each subclass
+};
+
+/**
+ * Perform raycasting on this shape.
+ * @method raycast
+ * @param  {RayResult} result Where to store the resulting data.
+ * @param  {Ray} ray The Ray that you want to use for raycasting.
+ * @param  {array} position World position of the shape (the .position property will be ignored).
+ * @param  {number} angle World angle of the shape (the .angle property will be ignored).
+ */
+Shape.prototype.raycast = function(result, ray, position, angle){
+    // To be implemented in each subclass
+};
\ No newline at end of file

src/solver/GSSolver.js

+var vec2 = require('../math/vec2')
+,   Solver = require('./Solver')
+,   Utils = require('../utils/Utils')
+,   FrictionEquation = require('../equations/FrictionEquation');
+
+module.exports = GSSolver;
+
+/**
+ * Iterative Gauss-Seidel constraint equation solver.
+ *
+ * @class GSSolver
+ * @constructor
+ * @extends Solver
+ * @param {Object} [options]
+ * @param {Number} [options.iterations=10]
+ * @param {Number} [options.tolerance=0]
+ */
+function GSSolver(options){
+    Solver.call(this,options,Solver.GS);
+    options = options || {};
+
+    /**
+     * The max number of iterations to do when solving. More gives better results, but is more expensive.
+     * @property iterations
+     * @type {Number}
+     */
+    this.iterations = options.iterations || 10;
+
+    /**
+     * The error tolerance, per constraint. If the total error is below this limit, the solver will stop iterating. Set to zero for as good solution as possible, but to something larger than zero to make computations faster.
+     * @property tolerance
+     * @type {Number}
+     * @default 1e-7
+     */
+    this.tolerance = options.tolerance || 1e-7;
+
+    this.arrayStep = 30;
+    this.lambda = new Utils.ARRAY_TYPE(this.arrayStep);
+    this.Bs =     new Utils.ARRAY_TYPE(this.arrayStep);
+    this.invCs =  new Utils.ARRAY_TYPE(this.arrayStep);
+
+    /**
+     * Set to true to set all right hand side terms to zero when solving. Can be handy for a few applications.
+     * @property useZeroRHS
+     * @type {Boolean}
+     * @todo Remove, not used
+     */
+    this.useZeroRHS = false;
+
+    /**
+     * Number of solver iterations that are used to approximate normal forces used for friction (F_friction = mu * F_normal). These friction forces will override any other friction forces that are set. If you set frictionIterations = 0, then this feature will be disabled.
+     *
+     * Use only frictionIterations > 0 if the approximated normal force (F_normal = mass * gravity) is not good enough. Examples of where it can happen is in space games where gravity is zero, or in tall stacks where the normal force is large at bottom but small at top.
+     *
+     * @property frictionIterations
+     * @type {Number}
+     * @default 0
+     */
+    this.frictionIterations = options.frictionIterations !== undefined ? 0 : options.frictionIterations;
+
+    /**
+     * The number of iterations that were made during the last solve. If .tolerance is zero, this value will always be equal to .iterations, but if .tolerance is larger than zero, and the solver can quit early, then this number will be somewhere between 1 and .iterations.
+     * @property {Number} usedIterations
+     */
+    this.usedIterations = 0;
+}
+GSSolver.prototype = new Solver();
+GSSolver.prototype.constructor = GSSolver;
+
+function setArrayZero(array){
+    var l = array.length;
+    while(l--){
+        array[l] = +0.0;
+    }
+}
+
+/**
+ * Solve the system of equations
+ * @method solve
+ * @param  {Number}  h       Time step
+ * @param  {World}   world    World to solve
+ */
+GSSolver.prototype.solve = function(h, world){
+
+    this.sortEquations();
+
+    var iter = 0,
+        maxIter = this.iterations,
+        maxFrictionIter = this.frictionIterations,
+        equations = this.equations,
+        Neq = equations.length,
+        tolSquared = Math.pow(this.tolerance*Neq, 2),
+        bodies = world.bodies,
+        Nbodies = world.bodies.length,
+        add = vec2.add,
+        set = vec2.set,
+        useZeroRHS = this.useZeroRHS,
+        lambda = this.lambda;
+
+    this.usedIterations = 0;
+
+    if(Neq){
+        for(var i=0; i!==Nbodies; i++){
+            var b = bodies[i];
+
+            // Update solve mass
+            b.updateSolveMassProperties();
+        }
+    }
+
+    // Things that does not change during iteration can be computed once
+    if(lambda.length < Neq){
+        lambda = this.lambda =  new Utils.ARRAY_TYPE(Neq + this.arrayStep);
+        this.Bs =               new Utils.ARRAY_TYPE(Neq + this.arrayStep);
+        this.invCs =            new Utils.ARRAY_TYPE(Neq + this.arrayStep);
+    }
+    setArrayZero(lambda);
+    var invCs = this.invCs,
+        Bs = this.Bs,
+        lambda = this.lambda;
+
+    for(var i=0; i!==equations.length; i++){
+        var c = equations[i];
+        if(c.timeStep !== h || c.needsUpdate){
+            c.timeStep = h;
+            c.update();
+        }
+        Bs[i] =     c.computeB(c.a,c.b,h);
+        invCs[i] =  c.computeInvC(c.epsilon);
+    }
+
+    var q, B, c, deltalambdaTot,i,j;
+
+    if(Neq !== 0){
+
+        for(i=0; i!==Nbodies; i++){
+            var b = bodies[i];
+
+            // Reset vlambda
+            b.resetConstraintVelocity();
+        }
+
+        if(maxFrictionIter){
+            // Iterate over contact equations to get normal forces
+            for(iter=0; iter!==maxFrictionIter; iter++){
+
+                // Accumulate the total error for each iteration.
+                deltalambdaTot = 0.0;
+
+                for(j=0; j!==Neq; j++){
+                    c = equations[j];
+
+                    var deltalambda = GSSolver.iterateEquation(j,c,c.epsilon,Bs,invCs,lambda,useZeroRHS,h,iter);
+                    deltalambdaTot += Math.abs(deltalambda);
+                }
+
+                this.usedIterations++;
+
+                // If the total error is small enough - stop iterate
+                if(deltalambdaTot*deltalambdaTot <= tolSquared){
+                    break;
+                }
+            }
+
+            GSSolver.updateMultipliers(equations, lambda, 1/h);
+
+            // Set computed friction force
+            for(j=0; j!==Neq; j++){
+                var eq = equations[j];
+                if(eq instanceof FrictionEquation){
+                    var f = 0.0;
+                    for(var k=0; k!==eq.contactEquations.length; k++){
+                        f += eq.contactEquations[k].multiplier;
+                    }
+                    f *= eq.frictionCoefficient / eq.contactEquations.length;
+                    eq.maxForce =  f;
+                    eq.minForce = -f;
+                }
+            }
+        }
+
+        // Iterate over all equations
+        for(iter=0; iter!==maxIter; iter++){
+
+            // Accumulate the total error for each iteration.
+            deltalambdaTot = 0.0;
+
+            for(j=0; j!==Neq; j++){
+                c = equations[j];
+
+                var deltalambda = GSSolver.iterateEquation(j,c,c.epsilon,Bs,invCs,lambda,useZeroRHS,h,iter);
+                deltalambdaTot += Math.abs(deltalambda);
+            }
+
+            this.usedIterations++;
+
+            // If the total error is small enough - stop iterate
+            if(deltalambdaTot*deltalambdaTot <= tolSquared){
+                break;
+            }
+        }
+
+        // Add result to velocity
+        for(i=0; i!==Nbodies; i++){
+            bodies[i].addConstraintVelocity();
+        }
+
+        GSSolver.updateMultipliers(equations, lambda, 1/h);
+    }
+};
+
+// Sets the .multiplier property of each equation
+GSSolver.updateMultipliers = function(equations, lambda, invDt){
+    // Set the .multiplier property of each equation
+    var l = equations.length;
+    while(l--){
+        equations[l].multiplier = lambda[l] * invDt;
+    }
+};
+
+GSSolver.iterateEquation = function(j,eq,eps,Bs,invCs,lambda,useZeroRHS,dt,iter){
+    // Compute iteration
+    var B = Bs[j],
+        invC = invCs[j],
+        lambdaj = lambda[j],
+        GWlambda = eq.computeGWlambda();
+
+    var maxForce = eq.maxForce,
+        minForce = eq.minForce;
+
+    if(useZeroRHS){
+        B = 0;
+    }
+
+    var deltalambda = invC * ( B - GWlambda - eps * lambdaj );
+
+    // Clamp if we are not within the min/max interval
+    var lambdaj_plus_deltalambda = lambdaj + deltalambda;
+    if(lambdaj_plus_deltalambda < minForce*dt){
+        deltalambda = minForce*dt - lambdaj;
+    } else if(lambdaj_plus_deltalambda > maxForce*dt){
+        deltalambda = maxForce*dt - lambdaj;
+    }
+    lambda[j] += deltalambda;
+    eq.addToWlambda(deltalambda);
+
+    return deltalambda;
+};

src/solver/Solver.js

+var Utils = require('../utils/Utils')
+,   EventEmitter = require('../events/EventEmitter');
+
+module.exports = Solver;
+
+/**
+ * Base class for constraint solvers.
+ * @class Solver
+ * @constructor
+ * @extends EventEmitter
+ */
+function Solver(options,type){
+    options = options || {};
+
+    EventEmitter.call(this);
+
+    this.type = type;
+
+    /**
+     * Current equations in the solver.
+     *
+     * @property equations
+     * @type {Array}
+     */
+    this.equations = [];
+
+    /**
+     * Function that is used to sort all equations before each solve.
+     * @property equationSortFunction
+     * @type {function|boolean}
+     */
+    this.equationSortFunction = options.equationSortFunction || false;
+}
+Solver.prototype = new EventEmitter();
+Solver.prototype.constructor = Solver;
+
+/**
+ * Method to be implemented in each subclass
+ * @method solve
+ * @param  {Number} dt
+ * @param  {World} world
+ */
+Solver.prototype.solve = function(dt,world){
+    throw new Error("Solver.solve should be implemented by subclasses!");
+};
+
+var mockWorld = {bodies:[]};
+
+/**
+ * Solves all constraints in an island.
+ * @method solveIsland
+ * @param  {Number} dt
+ * @param  {Island} island
+ */
+Solver.prototype.solveIsland = function(dt,island){
+
+    this.removeAllEquations();
+
+    if(island.equations.length){
+        // Add equations to solver
+        this.addEquations(island.equations);
+        mockWorld.bodies.length = 0;
+        island.getBodies(mockWorld.bodies);
+
+        // Solve
+        if(mockWorld.bodies.length){
+            this.solve(dt,mockWorld);
+        }
+    }
+};
+
+/**
+ * Sort all equations using the .equationSortFunction. Should be called by subclasses before solving.
+ * @method sortEquations
+ */
+Solver.prototype.sortEquations = function(){
+    if(this.equationSortFunction){
+        this.equations.sort(this.equationSortFunction);
+    }
+};
+
+/**
+ * Add an equation to be solved.
+ *
+ * @method addEquation
+ * @param {Equation} eq
+ */
+Solver.prototype.addEquation = function(eq){
+    if(eq.enabled){
+        this.equations.push(eq);
+    }
+};
+
+/**
+ * Add equations. Same as .addEquation, but this time the argument is an array of Equations
+ *
+ * @method addEquations
+ * @param {Array} eqs
+ */
+Solver.prototype.addEquations = function(eqs){
+    //Utils.appendArray(this.equations,eqs);
+    for(var i=0, N=eqs.length; i!==N; i++){
+        var eq = eqs[i];
+        if(eq.enabled){
+            this.equations.push(eq);
+        }
+    }
+};
+
+/**
+ * Remove an equation.
+ *
+ * @method removeEquation
+ * @param {Equation} eq
+ */
+Solver.prototype.removeEquation = function(eq){
+    var i = this.equations.indexOf(eq);
+    if(i !== -1){
+        this.equations.splice(i,1);
+    }
+};
+
+/**
+ * Remove all currently added equations.
+ *
+ * @method removeAllEquations
+ */
+Solver.prototype.removeAllEquations = function(){
+    this.equations.length=0;
+};
+
+Solver.GS = 1;
+Solver.ISLAND = 2;

src/utils/ContactEquationPool.js

+var ContactEquation = require('../equations/ContactEquation');
+var Pool = require('./Pool');
+
+module.exports = ContactEquationPool;
+
+/**
+ * @class
+ */
+function ContactEquationPool() {
+	Pool.apply(this, arguments);
+}
+ContactEquationPool.prototype = new Pool();
+ContactEquationPool.prototype.constructor = ContactEquationPool;
+
+/**
+ * @method create
+ * @return {ContactEquation}
+ */
+ContactEquationPool.prototype.create = function () {
+	return new ContactEquation();
+};
+
+/**
+ * @method destroy
+ * @param {ContactEquation} equation
+ * @return {ContactEquationPool}
+ */
+ContactEquationPool.prototype.destroy = function (equation) {
+	equation.bodyA = equation.bodyB = null;
+	return this;
+};

src/utils/FrictionEquationPool.js

+var FrictionEquation = require('../equations/FrictionEquation');
+var Pool = require('./Pool');
+
+module.exports = FrictionEquationPool;
+
+/**
+ * @class
+ */
+function FrictionEquationPool() {
+	Pool.apply(this, arguments);
+}
+FrictionEquationPool.prototype = new Pool();
+FrictionEquationPool.prototype.constructor = FrictionEquationPool;
+
+/**
+ * @method create
+ * @return {FrictionEquation}
+ */
+FrictionEquationPool.prototype.create = function () {
+	return new FrictionEquation();
+};
+
+/**
+ * @method destroy
+ * @param {FrictionEquation} equation
+ * @return {FrictionEquationPool}
+ */
+FrictionEquationPool.prototype.destroy = function (equation) {
+	equation.bodyA = equation.bodyB = null;
+	return this;
+};

src/utils/IslandNodePool.js

+var IslandNode = require('../world/IslandNode');
+var Pool = require('./Pool');
+
+module.exports = IslandNodePool;
+
+/**
+ * @class
+ */
+function IslandNodePool() {
+	Pool.apply(this, arguments);
+}
+IslandNodePool.prototype = new Pool();
+IslandNodePool.prototype.constructor = IslandNodePool;
+
+/**
+ * @method create
+ * @return {IslandNode}
+ */
+IslandNodePool.prototype.create = function () {
+	return new IslandNode();
+};
+
+/**
+ * @method destroy
+ * @param {IslandNode} node
+ * @return {IslandNodePool}
+ */
+IslandNodePool.prototype.destroy = function (node) {
+	node.reset();
+	return this;
+};

src/utils/IslandPool.js

+var Island = require('../world/Island');
+var Pool = require('./Pool');
+
+module.exports = IslandPool;
+
+/**
+ * @class
+ */
+function IslandPool() {
+	Pool.apply(this, arguments);
+}
+IslandPool.prototype = new Pool();
+IslandPool.prototype.constructor = IslandPool;
+
+/**
+ * @method create
+ * @return {Island}
+ */
+IslandPool.prototype.create = function () {
+	return new Island();
+};
+
+/**
+ * @method destroy
+ * @param {Island} island
+ * @return {IslandPool}
+ */
+IslandPool.prototype.destroy = function (island) {
+	island.reset();
+	return this;
+};

src/utils/OverlapKeeper.js

+var TupleDictionary = require('./TupleDictionary');
+var OverlapKeeperRecord = require('./OverlapKeeperRecord');
+var OverlapKeeperRecordPool = require('./OverlapKeeperRecordPool');
+var Utils = require('./Utils');
+
+module.exports = OverlapKeeper;
+
+/**
+ * Keeps track of overlaps in the current state and the last step state.
+ * @class OverlapKeeper
+ * @constructor
+ */
+function OverlapKeeper() {
+    this.overlappingShapesLastState = new TupleDictionary();
+    this.overlappingShapesCurrentState = new TupleDictionary();
+    this.recordPool = new OverlapKeeperRecordPool({ size: 16 });
+    this.tmpDict = new TupleDictionary();
+    this.tmpArray1 = [];
+}
+
+/**
+ * Ticks one step forward in time. This will move the current overlap state to the "old" overlap state, and create a new one as current.
+ * @method tick
+ */
+OverlapKeeper.prototype.tick = function() {
+    var last = this.overlappingShapesLastState;
+    var current = this.overlappingShapesCurrentState;
+
+    // Save old objects into pool
+    var l = last.keys.length;
+    while(l--){
+        var key = last.keys[l];
+        var lastObject = last.getByKey(key);
+        var currentObject = current.getByKey(key);
+        if(lastObject){
+            // The record is only used in the "last" dict, and will be removed. We might as well pool it.
+            this.recordPool.release(lastObject);
+        }
+    }
+
+    // Clear last object
+    last.reset();
+
+    // Transfer from new object to old
+    last.copy(current);
+
+    // Clear current object
+    current.reset();
+};
+
+/**
+ * @method setOverlapping
+ * @param {Body} bodyA
+ * @param {Body} shapeA
+ * @param {Body} bodyB
+ * @param {Body} shapeB
+ */
+OverlapKeeper.prototype.setOverlapping = function(bodyA, shapeA, bodyB, shapeB) {
+    var last = this.overlappingShapesLastState;
+    var current = this.overlappingShapesCurrentState;
+
+    // Store current contact state
+    if(!current.get(shapeA.id, shapeB.id)){
+        var data = this.recordPool.get();
+        data.set(bodyA, shapeA, bodyB, shapeB);
+        current.set(shapeA.id, shapeB.id, data);
+    }
+};
+
+OverlapKeeper.prototype.getNewOverlaps = function(result){
+    return this.getDiff(this.overlappingShapesLastState, this.overlappingShapesCurrentState, result);
+};
+
+OverlapKeeper.prototype.getEndOverlaps = function(result){
+    return this.getDiff(this.overlappingShapesCurrentState, this.overlappingShapesLastState, result);
+};
+
+/**
+ * Checks if two bodies are currently overlapping.
+ * @method bodiesAreOverlapping
+ * @param  {Body} bodyA
+ * @param  {Body} bodyB
+ * @return {boolean}
+ */
+OverlapKeeper.prototype.bodiesAreOverlapping = function(bodyA, bodyB){
+    var current = this.overlappingShapesCurrentState;
+    var l = current.keys.length;
+    while(l--){
+        var key = current.keys[l];
+        var data = current.data[key];
+        if((data.bodyA === bodyA && data.bodyB === bodyB) || data.bodyA === bodyB && data.bodyB === bodyA){
+            return true;
+        }
+    }
+    return false;
+};
+
+OverlapKeeper.prototype.getDiff = function(dictA, dictB, result){
+    var result = result || [];
+    var last = dictA;
+    var current = dictB;
+
+    result.length = 0;
+
+    var l = current.keys.length;
+    while(l--){
+        var key = current.keys[l];
+        var data = current.data[key];
+
+        if(!data){
+            throw new Error('Key '+key+' had no data!');
+        }
+
+        var lastData = last.data[key];
+        if(!lastData){
+            // Not overlapping in last state, but in current.
+            result.push(data);
+        }
+    }
+
+    return result;
+};
+
+OverlapKeeper.prototype.isNewOverlap = function(shapeA, shapeB){
+    var idA = shapeA.id|0,
+        idB = shapeB.id|0;
+    var last = this.overlappingShapesLastState;
+    var current = this.overlappingShapesCurrentState;
+    // Not in last but in new
+    return !!!last.get(idA, idB) && !!current.get(idA, idB);
+};
+
+OverlapKeeper.prototype.getNewBodyOverlaps = function(result){
+    this.tmpArray1.length = 0;
+    var overlaps = this.getNewOverlaps(this.tmpArray1);
+    return this.getBodyDiff(overlaps, result);
+};
+
+OverlapKeeper.prototype.getEndBodyOverlaps = function(result){
+    this.tmpArray1.length = 0;
+    var overlaps = this.getEndOverlaps(this.tmpArray1);
+    return this.getBodyDiff(overlaps, result);
+};
+
+OverlapKeeper.prototype.getBodyDiff = function(overlaps, result){
+    result = result || [];
+    var accumulator = this.tmpDict;
+
+    var l = overlaps.length;
+
+    while(l--){
+        var data = overlaps[l];
+
+        // Since we use body id's for the accumulator, these will be a subset of the original one
+        accumulator.set(data.bodyA.id|0, data.bodyB.id|0, data);
+    }
+
+    l = accumulator.keys.length;
+    while(l--){
+        var data = accumulator.getByKey(accumulator.keys[l]);
+        if(data){
+            result.push(data.bodyA, data.bodyB);
+        }
+    }
+
+    accumulator.reset();
+
+    return result;
+};

src/utils/OverlapKeeperRecord.js

+module.exports = OverlapKeeperRecord;
+
+/**
+ * Overlap data container for the OverlapKeeper
+ * @class OverlapKeeperRecord
+ * @constructor
+ * @param {Body} bodyA
+ * @param {Shape} shapeA
+ * @param {Body} bodyB
+ * @param {Shape} shapeB
+ */
+function OverlapKeeperRecord(bodyA, shapeA, bodyB, shapeB){
+    /**
+     * @property {Shape} shapeA
+     */
+    this.shapeA = shapeA;
+    /**
+     * @property {Shape} shapeB
+     */
+    this.shapeB = shapeB;
+    /**
+     * @property {Body} bodyA
+     */
+    this.bodyA = bodyA;
+    /**
+     * @property {Body} bodyB
+     */
+    this.bodyB = bodyB;
+}
+
+/**
+ * Set the data for the record
+ * @method set
+ * @param {Body} bodyA
+ * @param {Shape} shapeA
+ * @param {Body} bodyB
+ * @param {Shape} shapeB
+ */
+OverlapKeeperRecord.prototype.set = function(bodyA, shapeA, bodyB, shapeB){
+    OverlapKeeperRecord.call(this, bodyA, shapeA, bodyB, shapeB);
+};

src/utils/OverlapKeeperRecordPool.js

+var OverlapKeeperRecord = require('./OverlapKeeperRecord');
+var Pool = require('./Pool');
+
+module.exports = OverlapKeeperRecordPool;
+
+/**
+ * @class
+ */
+function OverlapKeeperRecordPool() {
+	Pool.apply(this, arguments);
+}
+OverlapKeeperRecordPool.prototype = new Pool();
+OverlapKeeperRecordPool.prototype.constructor = OverlapKeeperRecordPool;
+
+/**
+ * @method create
+ * @return {OverlapKeeperRecord}
+ */
+OverlapKeeperRecordPool.prototype.create = function () {
+	return new OverlapKeeperRecord();
+};
+
+/**
+ * @method destroy
+ * @param {OverlapKeeperRecord} record
+ * @return {OverlapKeeperRecordPool}
+ */
+OverlapKeeperRecordPool.prototype.destroy = function (record) {
+	record.bodyA = record.bodyB = record.shapeA = record.shapeB = null;
+	return this;
+};

src/utils/Pool.js

+module.exports = Pool;
+
+/**
+ * @class Object pooling utility.
+ */
+function Pool(options) {
+	options = options || {};
+
+	/**
+	 * @property {Array} objects
+	 * @type {Array}
+	 */
+	this.objects = [];
+
+	if(options.size !== undefined){
+		this.resize(options.size);
+	}
+}
+
+/**
+ * @method resize
+ * @param {number} size
+ * @return {Pool} Self, for chaining
+ */
+Pool.prototype.resize = function (size) {
+	var objects = this.objects;
+
+	while (objects.length > size) {
+		objects.pop();
+	}
+
+	while (objects.length < size) {
+		objects.push(this.create());
+	}
+
+	return this;
+};
+
+/**
+ * Get an object from the pool or create a new instance.
+ * @method get
+ * @return {Object}
+ */
+Pool.prototype.get = function () {
+	var objects = this.objects;
+	return objects.length ? objects.pop() : this.create();
+};
+
+/**
+ * Clean up and put the object back into the pool for later use.
+ * @method release
+ * @param {Object} object
+ * @return {Pool} Self for chaining
+ */
+Pool.prototype.release = function (object) {
+	this.destroy(object);
+	this.objects.push(object);
+	return this;
+};

src/utils/TupleDictionary.js

+var Utils = require('./Utils');
+
+module.exports = TupleDictionary;
+
+/**
+ * @class TupleDictionary
+ * @constructor
+ */
+function TupleDictionary() {
+
+    /**
+     * The data storage
+     * @property data
+     * @type {Object}
+     */
+    this.data = {};
+
+    /**
+     * Keys that are currently used.
+     * @property {Array} keys
+     */
+    this.keys = [];
+}
+
+/**
+ * Generate a key given two integers
+ * @method getKey
+ * @param  {number} i
+ * @param  {number} j
+ * @return {string}
+ */
+TupleDictionary.prototype.getKey = function(id1, id2) {
+    id1 = id1|0;
+    id2 = id2|0;
+
+    if ( (id1|0) === (id2|0) ){
+        return -1;
+    }
+
+    // valid for values < 2^16
+    return ((id1|0) > (id2|0) ?
+        (id1 << 16) | (id2 & 0xFFFF) :
+        (id2 << 16) | (id1 & 0xFFFF))|0
+        ;
+};
+
+/**
+ * @method getByKey
+ * @param  {Number} key
+ * @return {Object}
+ */
+TupleDictionary.prototype.getByKey = function(key) {
+    key = key|0;
+    return this.data[key];
+};
+
+/**
+ * @method get
+ * @param  {Number} i
+ * @param  {Number} j
+ * @return {Number}
+ */
+TupleDictionary.prototype.get = function(i, j) {
+    return this.data[this.getKey(i, j)];
+};
+
+/**
+ * Set a value.
+ * @method set
+ * @param  {Number} i
+ * @param  {Number} j
+ * @param {Number} value
+ */
+TupleDictionary.prototype.set = function(i, j, value) {
+    if(!value){
+        throw new Error("No data!");
+    }
+
+    var key = this.getKey(i, j);
+
+    // Check if key already exists
+    if(!this.data[key]){
+        this.keys.push(key);
+    }
+
+    this.data[key] = value;
+
+    return key;
+};
+
+/**
+ * Remove all data.
+ * @method reset
+ */
+TupleDictionary.prototype.reset = function() {
+    var data = this.data,
+        keys = this.keys;
+
+    var l = keys.length;
+    while(l--) {
+        delete data[keys[l]];
+    }
+
+    keys.length = 0;
+};
+
+/**
+ * Copy another TupleDictionary. Note that all data in this dictionary will be removed.
+ * @method copy
+ * @param {TupleDictionary} dict The TupleDictionary to copy into this one.
+ */
+TupleDictionary.prototype.copy = function(dict) {
+    this.reset();
+    Utils.appendArray(this.keys, dict.keys);
+    var l = dict.keys.length;
+    while(l--){
+        var key = dict.keys[l];
+        this.data[key] = dict.data[key];
+    }
+};

src/utils/Utils.js

+/* global P2_ARRAY_TYPE */
+
+module.exports = Utils;
+
+/**
+ * Misc utility functions
+ * @class Utils
+ * @constructor
+ */
+function Utils(){}
+
+/**
+ * Append the values in array b to the array a. See <a href="http://stackoverflow.com/questions/1374126/how-to-append-an-array-to-an-existing-javascript-array/1374131#1374131">this</a> for an explanation.
+ * @method appendArray
+ * @static
+ * @param  {Array} a
+ * @param  {Array} b
+ */
+Utils.appendArray = function(a,b){
+    if (b.length < 150000) {
+        a.push.apply(a, b);
+    } else {
+        for (var i = 0, len = b.length; i !== len; ++i) {
+            a.push(b[i]);
+        }
+    }
+};
+
+/**
+ * Garbage free Array.splice(). Does not allocate a new array.
+ * @method splice
+ * @static
+ * @param  {Array} array
+ * @param  {Number} index
+ * @param  {Number} howmany
+ */
+Utils.splice = function(array,index,howmany){
+    howmany = howmany || 1;
+    for (var i=index, len=array.length-howmany; i < len; i++){
+        array[i] = array[i + howmany];
+    }
+    array.length = len;
+};
+
+/**
+ * The array type to use for internal numeric computations throughout the library. Float32Array is used if it is available, but falls back on Array. If you want to set array type manually, inject it via the global variable P2_ARRAY_TYPE. See example below.
+ * @static
+ * @property {function} ARRAY_TYPE
+ * @example
+ *     <script>
+ *         <!-- Inject your preferred array type before loading p2.js -->
+ *         P2_ARRAY_TYPE = Array;
+ *     </script>
+ *     <script src="p2.js"></script>
+ */
+if(typeof P2_ARRAY_TYPE !== 'undefined') {
+    Utils.ARRAY_TYPE = P2_ARRAY_TYPE;
+} else if (typeof Float32Array !== 'undefined'){
+    Utils.ARRAY_TYPE = Float32Array;
+} else {
+    Utils.ARRAY_TYPE = Array;
+}
+
+/**
+ * Extend an object with the properties of another
+ * @static
+ * @method extend
+ * @param  {object} a
+ * @param  {object} b
+ */
+Utils.extend = function(a,b){
+    for(var key in b){
+        a[key] = b[key];
+    }
+};
+
+/**
+ * Extend an options object with default values.
+ * @static
+ * @method defaults
+ * @param  {object} options The options object. May be falsy: in this case, a new object is created and returned.
+ * @param  {object} defaults An object containing default values.
+ * @return {object} The modified options object.
+ */
+Utils.defaults = function(options, defaults){
+    options = options || {};
+    for(var key in defaults){
+        if(!(key in options)){
+            options[key] = defaults[key];
+        }
+    }
+    return options;
+};

src/world/Island.js

+var Body = require('../objects/Body');
+
+module.exports = Island;
+
+/**
+ * An island of bodies connected with equations.
+ * @class Island
+ * @constructor
+ */
+function Island(){
+
+    /**
+     * Current equations in this island.
+     * @property equations
+     * @type {Array}
+     */
+    this.equations = [];
+
+    /**
+     * Current bodies in this island.
+     * @property bodies
+     * @type {Array}
+     */
+    this.bodies = [];
+}
+
+/**
+ * Clean this island from bodies and equations.
+ * @method reset
+ */
+Island.prototype.reset = function(){
+    this.equations.length = this.bodies.length = 0;
+};
+
+var bodyIds = [];
+
+/**
+ * Get all unique bodies in this island.
+ * @method getBodies
+ * @return {Array} An array of Body
+ */
+Island.prototype.getBodies = function(result){
+    var bodies = result || [],
+        eqs = this.equations;
+    bodyIds.length = 0;
+    for(var i=0; i!==eqs.length; i++){
+        var eq = eqs[i];
+        if(bodyIds.indexOf(eq.bodyA.id)===-1){
+            bodies.push(eq.bodyA);
+            bodyIds.push(eq.bodyA.id);
+        }
+        if(bodyIds.indexOf(eq.bodyB.id)===-1){
+            bodies.push(eq.bodyB);
+            bodyIds.push(eq.bodyB.id);
+        }
+    }
+    return bodies;
+};
+
+/**
+ * Check if the entire island wants to sleep.
+ * @method wantsToSleep
+ * @return {Boolean}
+ */
+Island.prototype.wantsToSleep = function(){
+    for(var i=0; i<this.bodies.length; i++){
+        var b = this.bodies[i];
+        if(b.type === Body.DYNAMIC && !b.wantsToSleep){
+            return false;
+        }
+    }
+    return true;
+};
+
+/**
+ * Make all bodies in the island sleep.
+ * @method sleep
+ */
+Island.prototype.sleep = function(){
+    for(var i=0; i<this.bodies.length; i++){
+        var b = this.bodies[i];
+        b.sleep();
+    }
+    return true;
+};

src/world/IslandManager.js

+var vec2 = require('../math/vec2')
+,   Island = require('./Island')
+,   IslandNode = require('./IslandNode')
+,   IslandNodePool = require('./../utils/IslandNodePool')
+,   IslandPool = require('./../utils/IslandPool')
+,   Body = require('../objects/Body');
+
+module.exports = IslandManager;
+
+/**
+ * Splits the system of bodies and equations into independent islands
+ *
+ * @class IslandManager
+ * @constructor
+ * @param {Object} [options]
+ * @extends Solver
+ */
+function IslandManager(options){
+
+    /**
+     * @property nodePool
+     * @type {IslandNodePool}
+     */
+    this.nodePool = new IslandNodePool({ size: 16 });
+
+    /**
+     * @property islandPool
+     * @type {IslandPool}
+     */
+    this.islandPool = new IslandPool({ size: 8 });
+
+    /**
+     * The equations to split. Manually fill this array before running .split().
+     * @property {Array} equations
+     */
+    this.equations = [];
+
+    /**
+     * The resulting {{#crossLink "Island"}}{{/crossLink}}s.
+     * @property {Array} islands
+     */
+    this.islands = [];
+
+    /**
+     * The resulting graph nodes.
+     * @property {Array} nodes
+     */
+    this.nodes = [];
+
+    /**
+     * The node queue, used when traversing the graph of nodes.
+     * @private
+     * @property {Array} queue
+     */
+    this.queue = [];
+}
+
+/**
+ * Get an unvisited node from a list of nodes.
+ * @static
+ * @method getUnvisitedNode
+ * @param  {Array} nodes
+ * @return {IslandNode|boolean} The node if found, else false.
+ */
+IslandManager.getUnvisitedNode = function(nodes){
+    var Nnodes = nodes.length;
+    for(var i=0; i!==Nnodes; i++){
+        var node = nodes[i];
+        if(!node.visited && node.body.type === Body.DYNAMIC){
+            return node;
+        }
+    }
+    return false;
+};
+
+/**
+ * Visit a node.
+ * @method visit
+ * @param  {IslandNode} node
+ * @param  {Array} bds
+ * @param  {Array} eqs
+ */
+IslandManager.prototype.visit = function (node,bds,eqs){
+    bds.push(node.body);
+    var Neqs = node.equations.length;
+    for(var i=0; i!==Neqs; i++){
+        var eq = node.equations[i];
+        if(eqs.indexOf(eq) === -1){ // Already added?
+            eqs.push(eq);
+        }
+    }
+};
+
+/**
+ * Runs the search algorithm, starting at a root node. The resulting bodies and equations will be stored in the provided arrays.
+ * @method bfs
+ * @param  {IslandNode} root The node to start from
+ * @param  {Array} bds  An array to append resulting Bodies to.
+ * @param  {Array} eqs  An array to append resulting Equations to.
+ */
+IslandManager.prototype.bfs = function(root,bds,eqs){
+
+    // Reset the visit queue
+    var queue = this.queue;
+    queue.length = 0;
+
+    // Add root node to queue
+    queue.push(root);
+    root.visited = true;
+    this.visit(root,bds,eqs);
+
+    // Process all queued nodes
+    while(queue.length) {
+
+        // Get next node in the queue
+        var node = queue.pop();
+
+        // Visit unvisited neighboring nodes
+        var child;
+        while((child = IslandManager.getUnvisitedNode(node.neighbors))) {
+            child.visited = true;
+            this.visit(child,bds,eqs);
+
+            // Only visit the children of this node if it's dynamic
+            if(child.body.type === Body.DYNAMIC){
+                queue.push(child);
+            }
+        }
+    }
+};
+
+/**
+ * Split the world into independent islands. The result is stored in .islands.
+ * @method split
+ * @param  {World} world
+ * @return {Array} The generated islands
+ */
+IslandManager.prototype.split = function(world){
+    var bodies = world.bodies,
+        nodes = this.nodes,
+        equations = this.equations;
+
+    // Move old nodes to the node pool
+    while(nodes.length){
+        this.nodePool.release(nodes.pop());
+    }
+
+    // Create needed nodes, reuse if possible
+    for(var i=0; i!==bodies.length; i++){
+        var node = this.nodePool.get();
+        node.body = bodies[i];
+        nodes.push(node);
+        // if(this.nodePool.length){
+        //     var node = this.nodePool.pop();
+        //     node.reset();
+        //     node.body = bodies[i];
+        //     nodes.push(node);
+        // } else {
+        //     nodes.push(new IslandNode(bodies[i]));
+        // }
+    }
+
+    // Add connectivity data. Each equation connects 2 bodies.
+    for(var k=0; k!==equations.length; k++){
+        var eq=equations[k],
+            i=bodies.indexOf(eq.bodyA),
+            j=bodies.indexOf(eq.bodyB),
+            ni=nodes[i],
+            nj=nodes[j];
+        ni.neighbors.push(nj);
+        nj.neighbors.push(ni);
+        ni.equations.push(eq);
+        nj.equations.push(eq);
+    }
+
+    // Move old islands to the island pool
+    var islands = this.islands;
+    for(var i=0; i<islands.length; i++){
+        this.islandPool.release(islands[i]);
+    }
+    islands.length = 0;
+
+    // Get islands
+    var child;
+    while((child = IslandManager.getUnvisitedNode(nodes))){
+
+        // Create new island
+        var island = this.islandPool.get();
+
+        // Get all equations and bodies in this island
+        this.bfs(child, island.bodies, island.equations);
+
+        islands.push(island);
+    }
+
+    return islands;
+};

src/world/IslandNode.js

+module.exports = IslandNode;
+
+/**
+ * Holds a body and keeps track of some additional properties needed for graph traversal.
+ * @class IslandNode
+ * @constructor
+ * @param {Body} body
+ */
+function IslandNode(body){
+
+	/**
+	 * The body that is contained in this node.
+	 * @property {Body} body
+	 */
+    this.body = body;
+
+    /**
+     * Neighboring IslandNodes
+     * @property {Array} neighbors
+     */
+    this.neighbors = [];
+
+    /**
+     * Equations connected to this node.
+     * @property {Array} equations
+     */
+    this.equations = [];
+
+    /**
+     * If this node was visiting during the graph traversal.
+     * @property visited
+     * @type {Boolean}
+     */
+    this.visited = false;
+}
+
+/**
+ * Clean this node from bodies and equations.
+ * @method reset
+ */
+IslandNode.prototype.reset = function(){
+    this.equations.length = 0;
+    this.neighbors.length = 0;
+    this.visited = false;
+    this.body = null;
+};

src/world/World.js

+var  GSSolver = require('../solver/GSSolver')
+,    Solver = require('../solver/Solver')
+,    Ray = require('../collision/Ray')
+,    vec2 = require('../math/vec2')
+,    Circle = require('../shapes/Circle')
+,    Convex = require('../shapes/Convex')
+,    Line = require('../shapes/Line')
+,    Plane = require('../shapes/Plane')
+,    Capsule = require('../shapes/Capsule')
+,    Particle = require('../shapes/Particle')
+,    EventEmitter = require('../events/EventEmitter')
+,    Body = require('../objects/Body')
+,    Shape = require('../shapes/Shape')
+,    LinearSpring = require('../objects/LinearSpring')
+,    Material = require('../material/Material')
+,    ContactMaterial = require('../material/ContactMaterial')
+,    DistanceConstraint = require('../constraints/DistanceConstraint')
+,    Constraint = require('../constraints/Constraint')
+,    LockConstraint = require('../constraints/LockConstraint')
+,    RevoluteConstraint = require('../constraints/RevoluteConstraint')
+,    PrismaticConstraint = require('../constraints/PrismaticConstraint')
+,    GearConstraint = require('../constraints/GearConstraint')
+,    Broadphase = require('../collision/Broadphase')
+,    AABB = require('../collision/AABB')
+,    SAPBroadphase = require('../collision/SAPBroadphase')
+,    Narrowphase = require('../collision/Narrowphase')
+,    Utils = require('../utils/Utils')
+,    OverlapKeeper = require('../utils/OverlapKeeper')
+,    IslandManager = require('./IslandManager')
+,    RotationalSpring = require('../objects/RotationalSpring');
+
+module.exports = World;
+
+/**
+ * The dynamics world, where all bodies and constraints live.
+ *
+ * @class World
+ * @constructor
+ * @param {Object} [options]
+ * @param {Solver} [options.solver] Defaults to GSSolver.
+ * @param {Array} [options.gravity] Defaults to y=-9.78.
+ * @param {Broadphase} [options.broadphase] Defaults to SAPBroadphase
+ * @param {Boolean} [options.islandSplit=true]
+ * @extends EventEmitter
+ *
+ * @example
+ *     var world = new World({
+ *         gravity: [0, -10],
+ *         broadphase: new SAPBroadphase()
+ *     });
+ *     world.addBody(new Body());
+ */
+function World(options){
+    EventEmitter.apply(this);
+
+    options = options || {};
+
+    /**
+     * All springs in the world. To add a spring to the world, use {{#crossLink "World/addSpring:method"}}{{/crossLink}}.
+     *
+     * @property springs
+     * @type {Array}
+     */
+    this.springs = [];
+
+    /**
+     * All bodies in the world. To add a body to the world, use {{#crossLink "World/addBody:method"}}{{/crossLink}}.
+     * @property {Array} bodies
+     */
+    this.bodies = [];
+
+    /**
+     * Disabled body collision pairs. See {{#crossLink "World/disableBodyCollision:method"}}.
+     * @private
+     * @property {Array} disabledBodyCollisionPairs
+     */
+    this.disabledBodyCollisionPairs = [];
+
+    /**
+     * The solver used to satisfy constraints and contacts. Default is {{#crossLink "GSSolver"}}{{/crossLink}}.
+     * @property {Solver} solver
+     */
+    this.solver = options.solver || new GSSolver();
+
+    /**
+     * The narrowphase to use to generate contacts.
+     *
+     * @property narrowphase
+     * @type {Narrowphase}
+     */
+    this.narrowphase = new Narrowphase(this);
+
+    /**
+     * The island manager of this world.
+     * @property {IslandManager} islandManager
+     */
+    this.islandManager = new IslandManager();
+
+    /**
+     * Gravity in the world. This is applied on all bodies in the beginning of each step().
+     *
+     * @property gravity
+     * @type {Array}
+     */
+    this.gravity = vec2.fromValues(0, -9.78);
+    if(options.gravity){
+        vec2.copy(this.gravity, options.gravity);
+    }
+
+    /**
+     * Gravity to use when approximating the friction max force (mu*mass*gravity).
+     * @property {Number} frictionGravity
+     */
+    this.frictionGravity = vec2.length(this.gravity) || 10;
+
+    /**
+     * Set to true if you want .frictionGravity to be automatically set to the length of .gravity.
+     * @property {Boolean} useWorldGravityAsFrictionGravity
+     * @default true
+     */
+    this.useWorldGravityAsFrictionGravity = true;
+
+    /**
+     * If the length of .gravity is zero, and .useWorldGravityAsFrictionGravity=true, then switch to using .frictionGravity for friction instead. This fallback is useful for gravityless games.
+     * @property {Boolean} useFrictionGravityOnZeroGravity
+     * @default true
+     */
+    this.useFrictionGravityOnZeroGravity = true;
+
+    /**
+     * The broadphase algorithm to use.
+     *
+     * @property broadphase
+     * @type {Broadphase}
+     */
+    this.broadphase = options.broadphase || new SAPBroadphase();
+    this.broadphase.setWorld(this);
+
+    /**
+     * User-added constraints.
+     *
+     * @property constraints
+     * @type {Array}
+     */
+    this.constraints = [];
+
+    /**
+     * Dummy default material in the world, used in .defaultContactMaterial
+     * @property {Material} defaultMaterial
+     */
+    this.defaultMaterial = new Material();
+
+    /**
+     * The default contact material to use, if no contact material was set for the colliding materials.
+     * @property {ContactMaterial} defaultContactMaterial
+     */
+    this.defaultContactMaterial = new ContactMaterial(this.defaultMaterial,this.defaultMaterial);
+
+    /**
+     * For keeping track of what time step size we used last step
+     * @property lastTimeStep
+     * @type {Number}
+     */
+    this.lastTimeStep = 1/60;
+
+    /**
+     * Enable to automatically apply spring forces each step.
+     * @property applySpringForces
+     * @type {Boolean}
+     * @default true
+     */
+    this.applySpringForces = true;
+
+    /**
+     * Enable to automatically apply body damping each step.
+     * @property applyDamping
+     * @type {Boolean}
+     * @default true
+     */
+    this.applyDamping = true;
+
+    /**
+     * Enable to automatically apply gravity each step.
+     * @property applyGravity
+     * @type {Boolean}
+     * @default true
+     */
+    this.applyGravity = true;
+
+    /**
+     * Enable/disable constraint solving in each step.
+     * @property solveConstraints
+     * @type {Boolean}
+     * @default true
+     */
+    this.solveConstraints = true;
+
+    /**
+     * The ContactMaterials added to the World.
+     * @property contactMaterials
+     * @type {Array}
+     */
+    this.contactMaterials = [];
+
+    /**
+     * World time.
+     * @property time
+     * @type {Number}
+     */
+    this.time = 0.0;
+    this.accumulator = 0;
+
+    /**
+     * Is true during step().
+     * @property {Boolean} stepping
+     */
+    this.stepping = false;
+
+    /**
+     * Bodies that are scheduled to be removed at the end of the step.
+     * @property {Array} bodiesToBeRemoved
+     * @private
+     */
+    this.bodiesToBeRemoved = [];
+
+    /**
+     * Whether to enable island splitting. Island splitting can be an advantage for both precision and performance. See {{#crossLink "IslandManager"}}{{/crossLink}}.
+     * @property {Boolean} islandSplit
+     * @default true
+     */
+    this.islandSplit = typeof(options.islandSplit)!=="undefined" ? !!options.islandSplit : true;
+
+    /**
+     * Set to true if you want to the world to emit the "impact" event. Turning this off could improve performance.
+     * @property emitImpactEvent
+     * @type {Boolean}
+     * @default true
+     */
+    this.emitImpactEvent = true;
+
+    // Id counters
+    this._constraintIdCounter = 0;
+    this._bodyIdCounter = 0;
+
+    /**
+     * Fired after the step().
+     * @event postStep
+     */
+    this.postStepEvent = {
+        type : "postStep"
+    };
+
+    /**
+     * Fired when a body is added to the world.
+     * @event addBody
+     * @param {Body} body
+     */
+    this.addBodyEvent = {
+        type : "addBody",
+        body : null
+    };
+
+    /**
+     * Fired when a body is removed from the world.
+     * @event removeBody
+     * @param {Body} body
+     */
+    this.removeBodyEvent = {
+        type : "removeBody",
+        body : null
+    };
+
+    /**
+     * Fired when a spring is added to the world.
+     * @event addSpring
+     * @param {Spring} spring
+     */
+    this.addSpringEvent = {
+        type : "addSpring",
+        spring : null
+    };
+
+    /**
+     * Fired when a first contact is created between two bodies. This event is fired after the step has been done.
+     * @event impact
+     * @param {Body} bodyA
+     * @param {Body} bodyB
+     */
+    this.impactEvent = {
+        type: "impact",
+        bodyA : null,
+        bodyB : null,
+        shapeA : null,
+        shapeB : null,
+        contactEquation : null
+    };
+
+    /**
+     * Fired after the Broadphase has collected collision pairs in the world.
+     * Inside the event handler, you can modify the pairs array as you like, to
+     * prevent collisions between objects that you don't want.
+     * @event postBroadphase
+     * @param {Array} pairs An array of collision pairs. If this array is [body1,body2,body3,body4], then the body pairs 1,2 and 3,4 would advance to narrowphase.
+     */
+    this.postBroadphaseEvent = {
+        type: "postBroadphase",
+        pairs: null
+    };
+
+    /**
+     * How to deactivate bodies during simulation. Possible modes are: {{#crossLink "World/NO_SLEEPING:property"}}World.NO_SLEEPING{{/crossLink}}, {{#crossLink "World/BODY_SLEEPING:property"}}World.BODY_SLEEPING{{/crossLink}} and {{#crossLink "World/ISLAND_SLEEPING:property"}}World.ISLAND_SLEEPING{{/crossLink}}.
+     * If sleeping is enabled, you might need to {{#crossLink "Body/wakeUp:method"}}wake up{{/crossLink}} the bodies if they fall asleep when they shouldn't. If you want to enable sleeping in the world, but want to disable it for a particular body, see {{#crossLink "Body/allowSleep:property"}}Body.allowSleep{{/crossLink}}.
+     * @property sleepMode
+     * @type {number}
+     * @default World.NO_SLEEPING
+     */
+    this.sleepMode = World.NO_SLEEPING;
+
+    /**
+     * Fired when two shapes starts start to overlap. Fired in the narrowphase, during step.
+     * @event beginContact
+     * @param {Shape} shapeA
+     * @param {Shape} shapeB
+     * @param {Body}  bodyA
+     * @param {Body}  bodyB
+     * @param {Array} contactEquations
+     */
+    this.beginContactEvent = {
+        type: "beginContact",
+        shapeA: null,
+        shapeB: null,
+        bodyA: null,
+        bodyB: null,
+        contactEquations: []
+    };
+
+    /**
+     * Fired when two shapes stop overlapping, after the narrowphase (during step).
+     * @event endContact
+     * @param {Shape} shapeA
+     * @param {Shape} shapeB
+     * @param {Body}  bodyA
+     * @param {Body}  bodyB
+     */
+    this.endContactEvent = {
+        type: "endContact",
+        shapeA: null,
+        shapeB: null,
+        bodyA: null,
+        bodyB: null
+    };
+
+    /**
+     * Fired just before equations are added to the solver to be solved. Can be used to control what equations goes into the solver.
+     * @event preSolve
+     * @param {Array} contactEquations  An array of contacts to be solved.
+     * @param {Array} frictionEquations An array of friction equations to be solved.
+     */
+    this.preSolveEvent = {
+        type: "preSolve",
+        contactEquations: null,
+        frictionEquations: null
+    };
+
+    // For keeping track of overlapping shapes
+    this.overlappingShapesLastState = { keys:[] };
+    this.overlappingShapesCurrentState = { keys:[] };
+
+    /**
+     * @property {OverlapKeeper} overlapKeeper
+     */
+    this.overlapKeeper = new OverlapKeeper();
+}
+World.prototype = new Object(EventEmitter.prototype);
+World.prototype.constructor = World;
+
+/**
+ * Never deactivate bodies.
+ * @static
+ * @property {number} NO_SLEEPING
+ */
+World.NO_SLEEPING = 1;
+
+/**
+ * Deactivate individual bodies if they are sleepy.
+ * @static
+ * @property {number} BODY_SLEEPING
+ */
+World.BODY_SLEEPING = 2;
+
+/**
+ * Deactivates bodies that are in contact, if all of them are sleepy. Note that you must enable {{#crossLink "World/islandSplit:property"}}.islandSplit{{/crossLink}} for this to work.
+ * @static
+ * @property {number} ISLAND_SLEEPING
+ */
+World.ISLAND_SLEEPING = 4;
+
+/**
+ * Add a constraint to the simulation.
+ *
+ * @method addConstraint
+ * @param {Constraint} constraint
+ * @example
+ *     var constraint = new LockConstraint(bodyA, bodyB);
+ *     world.addConstraint(constraint);
+ */
+World.prototype.addConstraint = function(constraint){
+    this.constraints.push(constraint);
+};
+
+/**
+ * Add a ContactMaterial to the simulation.
+ * @method addContactMaterial
+ * @param {ContactMaterial} contactMaterial
+ */
+World.prototype.addContactMaterial = function(contactMaterial){
+    this.contactMaterials.push(contactMaterial);
+};
+
+/**
+ * Removes a contact material
+ *
+ * @method removeContactMaterial
+ * @param {ContactMaterial} cm
+ */
+World.prototype.removeContactMaterial = function(cm){
+    var idx = this.contactMaterials.indexOf(cm);
+    if(idx!==-1){
+        Utils.splice(this.contactMaterials,idx,1);
+    }
+};
+
+/**
+ * Get a contact material given two materials
+ * @method getContactMaterial
+ * @param {Material} materialA
+ * @param {Material} materialB
+ * @return {ContactMaterial} The matching ContactMaterial, or false on fail.
+ * @todo Use faster hash map to lookup from material id's
+ */
+World.prototype.getContactMaterial = function(materialA,materialB){
+    var cmats = this.contactMaterials;
+    for(var i=0, N=cmats.length; i!==N; i++){
+        var cm = cmats[i];
+        if( (cm.materialA.id === materialA.id) && (cm.materialB.id === materialB.id) ||
+            (cm.materialA.id === materialB.id) && (cm.materialB.id === materialA.id) ){
+            return cm;
+        }
+    }
+    return false;
+};
+
+/**
+ * Removes a constraint
+ *
+ * @method removeConstraint
+ * @param {Constraint} constraint
+ */
+World.prototype.removeConstraint = function(constraint){
+    var idx = this.constraints.indexOf(constraint);
+    if(idx!==-1){
+        Utils.splice(this.constraints,idx,1);
+    }
+};
+
+var step_r = vec2.create(),
+    step_runit = vec2.create(),
+    step_u = vec2.create(),
+    step_f = vec2.create(),
+    step_fhMinv = vec2.create(),
+    step_velodt = vec2.create(),
+    step_mg = vec2.create(),
+    xiw = vec2.fromValues(0,0),
+    xjw = vec2.fromValues(0,0),
+    zero = vec2.fromValues(0,0),
+    interpvelo = vec2.fromValues(0,0);
+
+/**
+ * Step the physics world forward in time.
+ *
+ * There are two modes. The simple mode is fixed timestepping without interpolation. In this case you only use the first argument. The second case uses interpolation. In that you also provide the time since the function was last used, as well as the maximum fixed timesteps to take.
+ *
+ * @method step
+ * @param {Number} dt                       The fixed time step size to use.
+ * @param {Number} [timeSinceLastCalled=0]  The time elapsed since the function was last called.
+ * @param {Number} [maxSubSteps=10]         Maximum number of fixed steps to take per function call.
+ *
+ * @example
+ *     // Simple fixed timestepping without interpolation
+ *     var fixedTimeStep = 1 / 60;
+ *     var world = new World();
+ *     var body = new Body({ mass: 1 });
+ *     world.addBody(body);
+ *
+ *     function animate(){
+ *         requestAnimationFrame(animate);
+ *         world.step(fixedTimeStep);
+ *         renderBody(body.position, body.angle);
+ *     }
+ *
+ *     // Start animation loop
+ *     requestAnimationFrame(animate);
+ *
+ * @example
+ *     // Fixed timestepping with interpolation
+ *     var maxSubSteps = 10;
+ *     var lastTimeSeconds;
+ *
+ *     function animate(t){
+ *         requestAnimationFrame(animate);
+ *         timeSeconds = t / 1000;
+ *         lastTimeSeconds = lastTimeSeconds || timeSeconds;
+ *
+ *         deltaTime = timeSeconds - lastTimeSeconds;
+ *         world.step(fixedTimeStep, deltaTime, maxSubSteps);
+ *
+ *         renderBody(body.interpolatedPosition, body.interpolatedAngle);
+ *     }
+ *
+ *     // Start animation loop
+ *     requestAnimationFrame(animate);
+ *
+ * @see http://bulletphysics.org/mediawiki-1.5.8/index.php/Stepping_The_World
+ */
+World.prototype.step = function(dt,timeSinceLastCalled,maxSubSteps){
+    maxSubSteps = maxSubSteps || 10;
+    timeSinceLastCalled = timeSinceLastCalled || 0;
+
+    if(timeSinceLastCalled === 0){ // Fixed, simple stepping
+
+        this.internalStep(dt);
+
+        // Increment time
+        this.time += dt;
+
+    } else {
+
+        this.accumulator += timeSinceLastCalled;
+        var substeps = 0;
+        while (this.accumulator >= dt && substeps < maxSubSteps) {
+            // Do fixed steps to catch up
+            this.internalStep(dt);
+            this.time += dt;
+            this.accumulator -= dt;
+            substeps++;
+        }
+
+        var t = (this.accumulator % dt) / dt;
+        for(var j=0; j!==this.bodies.length; j++){
+            var b = this.bodies[j];
+            vec2.lerp(b.interpolatedPosition, b.previousPosition, b.position, t);
+            b.interpolatedAngle = b.previousAngle + t * (b.angle - b.previousAngle);
+        }
+    }
+};
+
+var endOverlaps = [];
+
+/**
+ * Make a fixed step.
+ * @method internalStep
+ * @param  {number} dt
+ * @private
+ */
+World.prototype.internalStep = function(dt){
+    this.stepping = true;
+
+    var that = this,
+        Nsprings = this.springs.length,
+        springs = this.springs,
+        bodies = this.bodies,
+        g = this.gravity,
+        solver = this.solver,
+        Nbodies = this.bodies.length,
+        broadphase = this.broadphase,
+        np = this.narrowphase,
+        constraints = this.constraints,
+        t0, t1,
+        fhMinv = step_fhMinv,
+        velodt = step_velodt,
+        mg = step_mg,
+        scale = vec2.scale,
+        add = vec2.add,
+        rotate = vec2.rotate,
+        islandManager = this.islandManager;
+
+    this.overlapKeeper.tick();
+
+    this.lastTimeStep = dt;
+
+    // Update approximate friction gravity.
+    if(this.useWorldGravityAsFrictionGravity){
+        var gravityLen = vec2.length(this.gravity);
+        if(!(gravityLen === 0 && this.useFrictionGravityOnZeroGravity)){
+            // Nonzero gravity. Use it.
+            this.frictionGravity = gravityLen;
+        }
+    }
+
+    // Add gravity to bodies
+    if(this.applyGravity){
+        for(var i=0; i!==Nbodies; i++){
+            var b = bodies[i],
+                fi = b.force;
+            if(b.type !== Body.DYNAMIC || b.sleepState === Body.SLEEPING){
+                continue;
+            }
+            vec2.scale(mg,g,b.mass*b.gravityScale); // F=m*g
+            add(fi,fi,mg);
+        }
+    }
+
+    // Add spring forces
+    if(this.applySpringForces){
+        for(var i=0; i!==Nsprings; i++){
+            var s = springs[i];
+            s.applyForce();
+        }
+    }
+
+    if(this.applyDamping){
+        for(var i=0; i!==Nbodies; i++){
+            var b = bodies[i];
+            if(b.type === Body.DYNAMIC){
+                b.applyDamping(dt);
+            }
+        }
+    }
+
+    // Broadphase
+    var result = broadphase.getCollisionPairs(this);
+
+    // Remove ignored collision pairs
+    var ignoredPairs = this.disabledBodyCollisionPairs;
+    for(var i=ignoredPairs.length-2; i>=0; i-=2){
+        for(var j=result.length-2; j>=0; j-=2){
+            if( (ignoredPairs[i]   === result[j] && ignoredPairs[i+1] === result[j+1]) ||
+                (ignoredPairs[i+1] === result[j] && ignoredPairs[i]   === result[j+1])){
+                result.splice(j,2);
+            }
+        }
+    }
+
+    // Remove constrained pairs with collideConnected == false
+    var Nconstraints = constraints.length;
+    for(i=0; i!==Nconstraints; i++){
+        var c = constraints[i];
+        if(!c.collideConnected){
+            for(var j=result.length-2; j>=0; j-=2){
+                if( (c.bodyA === result[j] && c.bodyB === result[j+1]) ||
+                    (c.bodyB === result[j] && c.bodyA === result[j+1])){
+                    result.splice(j,2);
+                }
+            }
+        }
+    }
+
+    // postBroadphase event
+    this.postBroadphaseEvent.pairs = result;
+    this.emit(this.postBroadphaseEvent);
+    this.postBroadphaseEvent.pairs = null;
+
+    // Narrowphase
+    np.reset(this);
+    for(var i=0, Nresults=result.length; i!==Nresults; i+=2){
+        var bi = result[i],
+            bj = result[i+1];
+
+        // Loop over all shapes of body i
+        for(var k=0, Nshapesi=bi.shapes.length; k!==Nshapesi; k++){
+            var si = bi.shapes[k],
+                xi = si.position,
+                ai = si.angle;
+
+            // All shapes of body j
+            for(var l=0, Nshapesj=bj.shapes.length; l!==Nshapesj; l++){
+                var sj = bj.shapes[l],
+                    xj = sj.position,
+                    aj = sj.angle;
+
+                var cm = this.defaultContactMaterial;
+                if(si.material && sj.material){
+                    var tmp = this.getContactMaterial(si.material,sj.material);
+                    if(tmp){
+                        cm = tmp;
+                    }
+                }
+
+                this.runNarrowphase(np,bi,si,xi,ai,bj,sj,xj,aj,cm,this.frictionGravity);
+            }
+        }
+    }
+
+    // Wake up bodies
+    for(var i=0; i!==Nbodies; i++){
+        var body = bodies[i];
+        if(body._wakeUpAfterNarrowphase){
+            body.wakeUp();
+            body._wakeUpAfterNarrowphase = false;
+        }
+    }
+
+    // Emit end overlap events
+    if(this.has('endContact')){
+        this.overlapKeeper.getEndOverlaps(endOverlaps);
+        var e = this.endContactEvent;
+        var l = endOverlaps.length;
+        while(l--){
+            var data = endOverlaps[l];
+            e.shapeA = data.shapeA;
+            e.shapeB = data.shapeB;
+            e.bodyA = data.bodyA;
+            e.bodyB = data.bodyB;
+            this.emit(e);
+        }
+        endOverlaps.length = 0;
+    }
+
+    var preSolveEvent = this.preSolveEvent;
+    preSolveEvent.contactEquations = np.contactEquations;
+    preSolveEvent.frictionEquations = np.frictionEquations;
+    this.emit(preSolveEvent);
+    preSolveEvent.contactEquations = preSolveEvent.frictionEquations = null;
+
+    // update constraint equations
+    var Nconstraints = constraints.length;
+    for(i=0; i!==Nconstraints; i++){
+        constraints[i].update();
+    }
+
+    if(np.contactEquations.length || np.frictionEquations.length || Nconstraints){
+        if(this.islandSplit){
+            // Split into islands
+            islandManager.equations.length = 0;
+            Utils.appendArray(islandManager.equations, np.contactEquations);
+            Utils.appendArray(islandManager.equations, np.frictionEquations);
+            for(i=0; i!==Nconstraints; i++){
+                Utils.appendArray(islandManager.equations, constraints[i].equations);
+            }
+            islandManager.split(this);
+
+            for(var i=0; i!==islandManager.islands.length; i++){
+                var island = islandManager.islands[i];
+                if(island.equations.length){
+                    solver.solveIsland(dt,island);
+                }
+            }
+
+        } else {
+
+            // Add contact equations to solver
+            solver.addEquations(np.contactEquations);
+            solver.addEquations(np.frictionEquations);
+
+            // Add user-defined constraint equations
+            for(i=0; i!==Nconstraints; i++){
+                solver.addEquations(constraints[i].equations);
+            }
+
+            if(this.solveConstraints){
+                solver.solve(dt,this);
+            }
+
+            solver.removeAllEquations();
+        }
+    }
+
+    // Step forward
+    for(var i=0; i!==Nbodies; i++){
+        var body = bodies[i];
+
+        // if(body.sleepState !== Body.SLEEPING && body.type !== Body.STATIC){
+        body.integrate(dt);
+        // }
+    }
+
+    // Reset force
+    for(var i=0; i!==Nbodies; i++){
+        bodies[i].setZeroForce();
+    }
+
+    // Emit impact event
+    if(this.emitImpactEvent && this.has('impact')){
+        var ev = this.impactEvent;
+        for(var i=0; i!==np.contactEquations.length; i++){
+            var eq = np.contactEquations[i];
+            if(eq.firstImpact){
+                ev.bodyA = eq.bodyA;
+                ev.bodyB = eq.bodyB;
+                ev.shapeA = eq.shapeA;
+                ev.shapeB = eq.shapeB;
+                ev.contactEquation = eq;
+                this.emit(ev);
+            }
+        }
+    }
+
+    // Sleeping update
+    if(this.sleepMode === World.BODY_SLEEPING){
+        for(i=0; i!==Nbodies; i++){
+            bodies[i].sleepTick(this.time, false, dt);
+        }
+    } else if(this.sleepMode === World.ISLAND_SLEEPING && this.islandSplit){
+
+        // Tell all bodies to sleep tick but dont sleep yet
+        for(i=0; i!==Nbodies; i++){
+            bodies[i].sleepTick(this.time, true, dt);
+        }
+
+        // Sleep islands
+        for(var i=0; i<this.islandManager.islands.length; i++){
+            var island = this.islandManager.islands[i];
+            if(island.wantsToSleep()){
+                island.sleep();
+            }
+        }
+    }
+
+    this.stepping = false;
+
+    // Remove bodies that are scheduled for removal
+    var bodiesToBeRemoved = this.bodiesToBeRemoved;
+    for(var i=0; i!==bodiesToBeRemoved.length; i++){
+        this.removeBody(bodiesToBeRemoved[i]);
+    }
+    bodiesToBeRemoved.length = 0;
+
+    this.emit(this.postStepEvent);
+};
+
+/**
+ * Runs narrowphase for the shape pair i and j.
+ * @method runNarrowphase
+ * @param  {Narrowphase} np
+ * @param  {Body} bi
+ * @param  {Shape} si
+ * @param  {Array} xi
+ * @param  {Number} ai
+ * @param  {Body} bj
+ * @param  {Shape} sj
+ * @param  {Array} xj
+ * @param  {Number} aj
+ * @param  {Number} mu
+ */
+World.prototype.runNarrowphase = function(np,bi,si,xi,ai,bj,sj,xj,aj,cm,glen){
+
+    // Check collision groups and masks
+    if(!((si.collisionGroup & sj.collisionMask) !== 0 && (sj.collisionGroup & si.collisionMask) !== 0)){
+        return;
+    }
+
+    // Get world position and angle of each shape
+    vec2.rotate(xiw, xi, bi.angle);
+    vec2.rotate(xjw, xj, bj.angle);
+    vec2.add(xiw, xiw, bi.position);
+    vec2.add(xjw, xjw, bj.position);
+    var aiw = ai + bi.angle;
+    var ajw = aj + bj.angle;
+
+    np.enableFriction = cm.friction > 0;
+    np.frictionCoefficient = cm.friction;
+    var reducedMass;
+    if(bi.type === Body.STATIC || bi.type === Body.KINEMATIC){
+        reducedMass = bj.mass;
+    } else if(bj.type === Body.STATIC || bj.type === Body.KINEMATIC){
+        reducedMass = bi.mass;
+    } else {
+        reducedMass = (bi.mass*bj.mass)/(bi.mass+bj.mass);
+    }
+    np.slipForce = cm.friction*glen*reducedMass;
+    np.restitution = cm.restitution;
+    np.surfaceVelocity = cm.surfaceVelocity;
+    np.frictionStiffness = cm.frictionStiffness;
+    np.frictionRelaxation = cm.frictionRelaxation;
+    np.stiffness = cm.stiffness;
+    np.relaxation = cm.relaxation;
+    np.contactSkinSize = cm.contactSkinSize;
+    np.enabledEquations = bi.collisionResponse && bj.collisionResponse && si.collisionResponse && sj.collisionResponse;
+
+    var resolver = np[si.type | sj.type],
+        numContacts = 0;
+    if (resolver) {
+        var sensor = si.sensor || sj.sensor;
+        var numFrictionBefore = np.frictionEquations.length;
+        if (si.type < sj.type) {
+            numContacts = resolver.call(np, bi,si,xiw,aiw, bj,sj,xjw,ajw, sensor);
+        } else {
+            numContacts = resolver.call(np, bj,sj,xjw,ajw, bi,si,xiw,aiw, sensor);
+        }
+        var numFrictionEquations = np.frictionEquations.length - numFrictionBefore;
+
+        if(numContacts){
+
+            if( bi.allowSleep &&
+                bi.type === Body.DYNAMIC &&
+                bi.sleepState  === Body.SLEEPING &&
+                bj.sleepState  === Body.AWAKE &&
+                bj.type !== Body.STATIC
+            ){
+                var speedSquaredB = vec2.squaredLength(bj.velocity) + Math.pow(bj.angularVelocity,2);
+                var speedLimitSquaredB = Math.pow(bj.sleepSpeedLimit,2);
+                if(speedSquaredB >= speedLimitSquaredB*2){
+                    bi._wakeUpAfterNarrowphase = true;
+                }
+            }
+
+            if( bj.allowSleep &&
+                bj.type === Body.DYNAMIC &&
+                bj.sleepState  === Body.SLEEPING &&
+                bi.sleepState  === Body.AWAKE &&
+                bi.type !== Body.STATIC
+            ){
+                var speedSquaredA = vec2.squaredLength(bi.velocity) + Math.pow(bi.angularVelocity,2);
+                var speedLimitSquaredA = Math.pow(bi.sleepSpeedLimit,2);
+                if(speedSquaredA >= speedLimitSquaredA*2){
+                    bj._wakeUpAfterNarrowphase = true;
+                }
+            }
+
+            this.overlapKeeper.setOverlapping(bi, si, bj, sj);
+            if(this.has('beginContact') && this.overlapKeeper.isNewOverlap(si, sj)){
+
+                // Report new shape overlap
+                var e = this.beginContactEvent;
+                e.shapeA = si;
+                e.shapeB = sj;
+                e.bodyA = bi;
+                e.bodyB = bj;
+
+                // Reset contact equations
+                e.contactEquations.length = 0;
+
+                if(typeof(numContacts)==="number"){
+                    for(var i=np.contactEquations.length-numContacts; i<np.contactEquations.length; i++){
+                        e.contactEquations.push(np.contactEquations[i]);
+                    }
+                }
+
+                this.emit(e);
+            }
+
+            // divide the max friction force by the number of contacts
+            if(typeof(numContacts)==="number" && numFrictionEquations > 1){ // Why divide by 1?
+                for(var i=np.frictionEquations.length-numFrictionEquations; i<np.frictionEquations.length; i++){
+                    var f = np.frictionEquations[i];
+                    f.setSlipForce(f.getSlipForce() / numFrictionEquations);
+                }
+            }
+        }
+    }
+
+};
+
+/**
+ * Add a spring to the simulation
+ *
+ * @method addSpring
+ * @param {Spring} spring
+ */
+World.prototype.addSpring = function(spring){
+    this.springs.push(spring);
+    var evt = this.addSpringEvent;
+    evt.spring = spring;
+    this.emit(evt);
+    evt.spring = null;
+};
+
+/**
+ * Remove a spring
+ *
+ * @method removeSpring
+ * @param {Spring} spring
+ */
+World.prototype.removeSpring = function(spring){
+    var idx = this.springs.indexOf(spring);
+    if(idx !== -1){
+        Utils.splice(this.springs,idx,1);
+    }
+};
+
+/**
+ * Add a body to the simulation
+ *
+ * @method addBody
+ * @param {Body} body
+ *
+ * @example
+ *     var world = new World(),
+ *         body = new Body();
+ *     world.addBody(body);
+ * @todo What if this is done during step?
+ */
+World.prototype.addBody = function(body){
+    if(this.bodies.indexOf(body) === -1){
+        this.bodies.push(body);
+        body.world = this;
+        var evt = this.addBodyEvent;
+        evt.body = body;
+        this.emit(evt);
+        evt.body = null;
+    }
+};
+
+/**
+ * Remove a body from the simulation. If this method is called during step(), the body removal is scheduled to after the step.
+ *
+ * @method removeBody
+ * @param {Body} body
+ */
+World.prototype.removeBody = function(body){
+    if(this.stepping){
+        this.bodiesToBeRemoved.push(body);
+    } else {
+        body.world = null;
+        var idx = this.bodies.indexOf(body);
+        if(idx!==-1){
+            Utils.splice(this.bodies,idx,1);
+            this.removeBodyEvent.body = body;
+            body.resetConstraintVelocity();
+            this.emit(this.removeBodyEvent);
+            this.removeBodyEvent.body = null;
+        }
+    }
+};
+
+/**
+ * Get a body by its id.
+ * @method getBodyById
+ * @param {number} id
+ * @return {Body} The body, or false if it was not found.
+ */
+World.prototype.getBodyById = function(id){
+    var bodies = this.bodies;
+    for(var i=0; i<bodies.length; i++){
+        var b = bodies[i];
+        if(b.id === id){
+            return b;
+        }
+    }
+    return false;
+};
+
+/**
+ * Disable collision between two bodies
+ * @method disableBodyCollision
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ */
+World.prototype.disableBodyCollision = function(bodyA,bodyB){
+    this.disabledBodyCollisionPairs.push(bodyA,bodyB);
+};
+
+/**
+ * Enable collisions between the given two bodies
+ * @method enableBodyCollision
+ * @param {Body} bodyA
+ * @param {Body} bodyB
+ */
+World.prototype.enableBodyCollision = function(bodyA,bodyB){
+    var pairs = this.disabledBodyCollisionPairs;
+    for(var i=0; i<pairs.length; i+=2){
+        if((pairs[i] === bodyA && pairs[i+1] === bodyB) || (pairs[i+1] === bodyA && pairs[i] === bodyB)){
+            pairs.splice(i,2);
+            return;
+        }
+    }
+};
+
+/**
+ * Resets the World, removes all bodies, constraints and springs.
+ *
+ * @method clear
+ */
+World.prototype.clear = function(){
+
+    this.time = 0;
+
+    // Remove all solver equations
+    if(this.solver && this.solver.equations.length){
+        this.solver.removeAllEquations();
+    }
+
+    // Remove all constraints
+    var cs = this.constraints;
+    for(var i=cs.length-1; i>=0; i--){
+        this.removeConstraint(cs[i]);
+    }
+
+    // Remove all bodies
+    var bodies = this.bodies;
+    for(var i=bodies.length-1; i>=0; i--){
+        this.removeBody(bodies[i]);
+    }
+
+    // Remove all springs
+    var springs = this.springs;
+    for(var i=springs.length-1; i>=0; i--){
+        this.removeSpring(springs[i]);
+    }
+
+    // Remove all contact materials
+    var cms = this.contactMaterials;
+    for(var i=cms.length-1; i>=0; i--){
+        this.removeContactMaterial(cms[i]);
+    }
+
+    World.apply(this);
+};
+
+var hitTest_tmp1 = vec2.create(),
+    hitTest_zero = vec2.fromValues(0,0),
+    hitTest_tmp2 = vec2.fromValues(0,0);
+
+/**
+ * Test if a world point overlaps bodies
+ * @method hitTest
+ * @param  {Array}  worldPoint  Point to use for intersection tests
+ * @param  {Array}  bodies      A list of objects to check for intersection
+ * @param  {Number} precision   Used for matching against particles and lines. Adds some margin to these infinitesimal objects.
+ * @return {Array}              Array of bodies that overlap the point
+ * @todo Should use an api similar to the raycast function
+ * @todo Should probably implement a .containsPoint method for all shapes. Would be more efficient
+ * @todo Should use the broadphase
+ */
+World.prototype.hitTest = function(worldPoint,bodies,precision){
+    precision = precision || 0;
+
+    // Create a dummy particle body with a particle shape to test against the bodies
+    var pb = new Body({ position:worldPoint }),
+        ps = new Particle(),
+        px = worldPoint,
+        pa = 0,
+        x = hitTest_tmp1,
+        zero = hitTest_zero,
+        tmp = hitTest_tmp2;
+    pb.addShape(ps);
+
+    var n = this.narrowphase,
+        result = [];
+
+    // Check bodies
+    for(var i=0, N=bodies.length; i!==N; i++){
+        var b = bodies[i];
+
+        for(var j=0, NS=b.shapes.length; j!==NS; j++){
+            var s = b.shapes[j];
+
+            // Get shape world position + angle
+            vec2.rotate(x, s.position, b.angle);
+            vec2.add(x, x, b.position);
+            var a = s.angle + b.angle;
+
+            if( (s instanceof Circle    && n.circleParticle  (b,s,x,a,     pb,ps,px,pa, true)) ||
+                (s instanceof Convex    && n.particleConvex  (pb,ps,px,pa, b,s,x,a,     true)) ||
+                (s instanceof Plane     && n.particlePlane   (pb,ps,px,pa, b,s,x,a,     true)) ||
+                (s instanceof Capsule   && n.particleCapsule (pb,ps,px,pa, b,s,x,a,     true)) ||
+                (s instanceof Particle  && vec2.squaredLength(vec2.sub(tmp,x,worldPoint)) < precision*precision)
+                ){
+                result.push(b);
+            }
+        }
+    }
+
+    return result;
+};
+
+/**
+ * Set the stiffness for all equations and contact materials.
+ * @method setGlobalStiffness
+ * @param {Number} stiffness
+ */
+World.prototype.setGlobalStiffness = function(stiffness){
+
+    // Set for all constraints
+    var constraints = this.constraints;
+    for(var i=0; i !== constraints.length; i++){
+        var c = constraints[i];
+        for(var j=0; j !== c.equations.length; j++){
+            var eq = c.equations[j];
+            eq.stiffness = stiffness;
+            eq.needsUpdate = true;
+        }
+    }
+
+    // Set for all contact materials
+    var contactMaterials = this.contactMaterials;
+    for(var i=0; i !== contactMaterials.length; i++){
+        var c = contactMaterials[i];
+        c.stiffness = c.frictionStiffness = stiffness;
+    }
+
+    // Set for default contact material
+    var c = this.defaultContactMaterial;
+    c.stiffness = c.frictionStiffness = stiffness;
+};
+
+/**
+ * Set the relaxation for all equations and contact materials.
+ * @method setGlobalRelaxation
+ * @param {Number} relaxation
+ */
+World.prototype.setGlobalRelaxation = function(relaxation){
+
+    // Set for all constraints
+    for(var i=0; i !== this.constraints.length; i++){
+        var c = this.constraints[i];
+        for(var j=0; j !== c.equations.length; j++){
+            var eq = c.equations[j];
+            eq.relaxation = relaxation;
+            eq.needsUpdate = true;
+        }
+    }
+
+    // Set for all contact materials
+    for(var i=0; i !== this.contactMaterials.length; i++){
+        var c = this.contactMaterials[i];
+        c.relaxation = c.frictionRelaxation = relaxation;
+    }
+
+    // Set for default contact material
+    var c = this.defaultContactMaterial;
+    c.relaxation = c.frictionRelaxation = relaxation;
+};
+
+var tmpAABB = new AABB();
+var tmpArray = [];
+
+/**
+ * Ray cast against all bodies in the world.
+ * @method raycast
+ * @param  {RaycastResult} result
+ * @param  {Ray} ray
+ * @return {boolean} True if any body was hit.
+ *
+ * @example
+ *     var ray = new Ray({
+ *         mode: Ray.CLOSEST, // or ANY
+ *         from: [0, 0],
+ *         to: [10, 0],
+ *     });
+ *     var result = new RaycastResult();
+ *     world.raycast(result, ray);
+ *
+ *     // Get the hit point
+ *     var hitPoint = vec2.create();
+ *     result.getHitPoint(hitPoint, ray);
+ *     console.log('Hit point: ', hitPoint[0], hitPoint[1], ' at distance ' + result.getHitDistance(ray));
+ *
+ * @example
+ *     var ray = new Ray({
+ *         mode: Ray.ALL,
+ *         from: [0, 0],
+ *         to: [10, 0],
+ *         callback: function(result){
+ *
+ *             // Print some info about the hit
+ *             console.log('Hit body and shape: ', result.body, result.shape);
+ *
+ *             // Get the hit point
+ *             var hitPoint = vec2.create();
+ *             result.getHitPoint(hitPoint, ray);
+ *             console.log('Hit point: ', hitPoint[0], hitPoint[1], ' at distance ' + result.getHitDistance(ray));
+ *
+ *             // If you are happy with the hits you got this far, you can stop the traversal here:
+ *             result.stop();
+ *         }
+ *     });
+ *     var result = new RaycastResult();
+ *     world.raycast(result, ray);
+ */
+World.prototype.raycast = function(result, ray){
+
+    // Get all bodies within the ray AABB
+    ray.getAABB(tmpAABB);
+    this.broadphase.aabbQuery(this, tmpAABB, tmpArray);
+    ray.intersectBodies(result, tmpArray);
+    tmpArray.length = 0;
+
+    return result.hasHit();
+};

test/collision/AABB.js

+var AABB = require(__dirname + '/../../src/collision/AABB');
+var vec2 = require(__dirname + '/../../src/math/vec2');
+
+exports.construct = function(test){
+    var aabb = new AABB({
+        lowerBound: [-1, -2],
+        upperBound: [1, 2]
+    });
+    test.equal(aabb.lowerBound[0], -1);
+    test.equal(aabb.lowerBound[1], -2);
+    test.equal(aabb.upperBound[0], 1);
+    test.equal(aabb.upperBound[1], 2);
+    test.done();
+};
+
+exports.copy = function(test){
+    var aabb = new AABB({
+        lowerBound: [-1, -2],
+        upperBound: [1, 2]
+    });
+    var aabb2 = new AABB();
+    aabb2.copy(aabb);
+    test.equal(aabb2.lowerBound[0], -1);
+    test.equal(aabb2.lowerBound[1], -2);
+    test.equal(aabb2.upperBound[0], 1);
+    test.equal(aabb2.upperBound[1], 2);
+    test.done();
+};
+
+exports.extend = function(test){
+    // STUB
+    test.done();
+};
+
+exports.overlaps = function(test){
+    var a = new AABB(),
+        b = new AABB();
+
+    // Same aabb
+    vec2.set(a.lowerBound, -1, -1);
+    vec2.set(a.upperBound,  1,  1);
+    vec2.set(b.lowerBound, -1, -1);
+    vec2.set(b.upperBound,  1,  1);
+    test.ok(a.overlaps(b),'should detect overlap of self');
+
+    // Corner overlaps
+    vec2.set(b.lowerBound,  1,  1);
+    vec2.set(b.upperBound,  2,  2);
+    test.ok(a.overlaps(b),'should detect corner overlap');
+
+    // Separate
+    vec2.set(b.lowerBound,  1.1,  1.1);
+    test.ok(!a.overlaps(b),'should detect separated');
+
+    // fully inside
+    vec2.set(b.lowerBound, -0.5, -0.5);
+    vec2.set(b.upperBound,  0.5,  0.5);
+    test.ok(a.overlaps(b),'should detect if aabb is fully inside other aabb');
+    vec2.set(b.lowerBound, -1.5, -1.5);
+    vec2.set(b.upperBound,  1.5,  1.5);
+    test.ok(a.overlaps(b),'should detect if aabb is fully inside other aabb');
+
+    // Translated
+    vec2.set(b.lowerBound, -3, -0.5);
+    vec2.set(b.upperBound, -2,  0.5);
+    test.ok(!a.overlaps(b),'should detect translated');
+
+    test.done();
+};
+
+exports.containsPoint = function(test){
+    var aabb = new AABB({
+        lowerBound: [-1, -1],
+        upperBound: [1, 1]
+    });
+    test.ok(aabb.containsPoint([0,0]));
+    test.ok(aabb.containsPoint([1,1]));
+    test.ok(aabb.containsPoint([-1,-1]));
+    test.ok(!aabb.containsPoint([2,2]));
+    test.done();
+};
+
+exports.overlapsRay = function(test){
+    test.done();
+};
+
+exports.setFromPoints = function(test){
+    var a = new AABB();
+    var points = [
+        [-1,-1],
+        [1, 1],
+        [0, 0],
+    ];
+    var position = [1,1];
+    var angle = 0;
+    a.setFromPoints(points, position, angle);
+
+    test.equal(a.lowerBound[0], 0);
+    test.equal(a.lowerBound[1], 0);
+    test.equal(a.upperBound[0], 2);
+    test.equal(a.upperBound[1], 2);
+
+    // One point
+    a.setFromPoints([[1,2]], [0,0], 0);
+    test.equal(a.lowerBound[0], 1);
+    test.equal(a.lowerBound[1], 2);
+    test.equal(a.upperBound[0], 1);
+    test.equal(a.upperBound[1], 2);
+
+    // Rotated 45 degrees
+    a.setFromPoints(points, [0,0], Math.PI/4);
+    test.ok(Math.abs(a.lowerBound[0]) < 0.01);
+    test.ok(Math.abs(a.upperBound[0]) < 0.01);
+
+    test.done();
+};
+

test/collision/Broadphase.js

+var Broadphase = require(__dirname + '/../../src/collision/Broadphase');
+
+exports.construct = function(test){
+    var b = new Broadphase(Broadphase.NAIVE);
+    test.equal(b.type, Broadphase.NAIVE);
+    test.done();
+};
+
+exports.boundingRadiusCheck = function(test){
+    // STUB
+    test.done();
+};
+
+exports.boundingRadiusCheck = function(test){
+    // STUB
+    test.done();
+};
+
+exports.canCollide = function(test){
+    // STUB
+    test.done();
+};
+
+exports.getCollisionPairs = function(test){
+    // STUB
+    test.done();
+};
+
+exports.setWorld = function(test){
+    // STUB
+    test.done();
+};
+

test/collision/NaiveBroadphase.js

+var NaiveBroadphase = require(__dirname + '/../../src/collision/NaiveBroadphase');
+var Broadphase = require(__dirname + '/../../src/collision/Broadphase');
+
+exports.construct = function(test){
+    var broadphase = new NaiveBroadphase();
+    test.equal(broadphase.type, Broadphase.NAIVE);
+    test.done();
+};
+
+exports.getCollisionPairs = function(test){
+    // STUB
+    test.done();
+};
+

test/collision/Narrowphase.js

+var Narrowphase = require(__dirname + '/../../src/collision/Narrowphase')
+,   Convex = require(__dirname + "/../../src/shapes/Convex")
+,   Circle = require(__dirname + "/../../src/shapes/Circle")
+,   Body = require(__dirname + "/../../src/objects/Body")
+,   Box = require(__dirname + "/../../src/shapes/Box")
+,   Capsule = require(__dirname + "/../../src/shapes/Capsule")
+,   Plane = require(__dirname + "/../../src/shapes/Plane")
+,   Particle = require(__dirname + "/../../src/shapes/Particle")
+,   Line = require(__dirname + "/../../src/shapes/Line")
+,   ContactEquation = require(__dirname + "/../../src/equations/ContactEquation")
+,   FrictionEquation = require(__dirname + "/../../src/equations/FrictionEquation")
+,   vec2 = require(__dirname + "/../../src/math/vec2");
+
+var rect,
+    circle,
+    convex,
+    capsule,
+    plane,
+    line,
+    particle,
+    narrowphase,
+    bodyA,
+    bodyB,
+    position=[0,0],
+    angle=0,
+    eps = 0.01;
+
+exports.setUp = function(callback){
+
+    // Rect
+    rect = new Box();
+
+    // Circle
+    var verts = [];
+    var N = 50;
+    for(var i=0; i<N; i++){
+        verts.push(vec2.fromValues( Math.cos(2*Math.PI / N * i),
+                                    Math.sin(2*Math.PI / N * i) ));
+    }
+    circle = new Circle({ radius: 1 });
+    convex = new Convex({ vertices: verts });
+    capsule = new Capsule({ length: 1, radius: 1 });
+    plane = new Plane();
+    particle = new Particle();
+    line = new Line();
+
+    narrowphase = new Narrowphase();
+
+    bodyA = new Body();
+    bodyB = new Body();
+
+    callback();
+};
+
+exports.construct = function(test){
+    var narrowphase = new Narrowphase();
+    test.done();
+};
+
+exports.capsuleCapsule = function(test){
+    var result = narrowphase.capsuleCapsule(bodyA, capsule, position, angle, bodyB, capsule, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.capsuleCapsule(bodyA, capsule, position, angle, bodyB, capsule, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.planeCapsule = function(test){
+    var result = narrowphase.planeCapsule(bodyA, plane, position, angle, bodyB, capsule, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.planeCapsule(bodyA, plane, position, angle, bodyB, capsule, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.circleCapsule = function(test){
+    var result = narrowphase.circleCapsule(bodyA, circle, position, angle, bodyB, capsule, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.circleCapsule(bodyA, circle, position, angle, bodyB, capsule, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.circleCircle = function(test){
+    var result = narrowphase.circleCircle(bodyA, circle, position, angle, bodyB, circle, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.circleCircle(bodyA, circle, position, angle, bodyB, circle, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.circleConvex = function(test){
+    var result = narrowphase.circleConvex(bodyA, circle, position, angle, bodyB, convex, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.circleConvex(bodyA, circle, position, angle, bodyB, convex, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.circleLine = function(test){
+    var result = narrowphase.circleLine(bodyA, circle, position, angle, bodyB, line, position, angle);
+    test.equal(typeof result, 'number');
+
+    var result = narrowphase.circleLine(bodyA, circle, position, angle, bodyB, line, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.circleParticle = function(test){
+    var result = narrowphase.circleParticle(bodyA, circle, position, angle, bodyB, particle, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.circleParticle(bodyA, circle, position, angle, bodyB, particle, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.circlePlane = function(test){
+    var result = narrowphase.circlePlane(bodyA, circle, position, angle, bodyB, plane, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.circlePlane(bodyA, circle, position, angle, bodyB, plane, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.collidedLastStep = function(test){
+    test.equal(typeof narrowphase.collidedLastStep(bodyA, bodyB), 'boolean');
+    test.done();
+};
+
+exports.convexCapsule = function(test){
+    var result = narrowphase.convexCapsule(bodyA, convex, position, angle, bodyB, capsule, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.convexCapsule(bodyA, convex, position, angle, bodyB, capsule, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.convexConvex = function(test){
+    var result = narrowphase.convexConvex(bodyA, convex, position, angle, bodyB, convex, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.convexConvex(bodyA, convex, position, angle, bodyB, convex, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.convexLine = function(test){
+    var result = narrowphase.convexLine(bodyA, convex, position, angle, bodyB, line, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.convexLine(bodyA, convex, position, angle, bodyB, line, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.planeConvex = function(test){
+    var result = narrowphase.planeConvex(bodyA, plane, position, angle, bodyB, convex, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.planeConvex(bodyA, plane, position, angle, bodyB, convex, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.createContactEquation = function(test){
+    var contact = narrowphase.createContactEquation(bodyA, bodyB, plane, rect);
+    test.ok(contact instanceof ContactEquation);
+    test.done();
+};
+
+exports.createFrictionEquation = function(test){
+    var eq = narrowphase.createFrictionEquation(bodyA, bodyB, plane, rect);
+    test.ok(eq instanceof FrictionEquation);
+    test.done();
+};
+
+exports.createFrictionFromContact = function(test){
+    // STUB
+    test.done();
+};
+
+exports.findSeparatingAxis = function(test){
+    var axis = vec2.create();
+    Narrowphase.findSeparatingAxis(convex,[0,0],0,convex,[0,1+eps],0,axis);
+
+    // Check length
+    var l = vec2.length(axis);
+    test.ok(l > 1-eps);
+    test.ok(l < 1+eps);
+
+    // Check direction - should be quite near up/down direction
+    var d = vec2.dot(axis, [0,1]);
+    test.ok(Math.abs(d) > 1-eps);
+
+
+
+    // Check what happens if there is overlap
+    Narrowphase.findSeparatingAxis(convex,[0,0],0,convex,[0,0.5],0,axis);
+
+    // Check direction - should still be quite near up/down direction
+    var d = vec2.dot(axis, [0,1]);
+    test.ok(Math.abs(d) > 1-eps);
+
+
+
+    // Check what happens if there is diagonal overlap
+    Narrowphase.findSeparatingAxis(convex,[0,0],0,convex,[0.5,0.5],0,axis);
+
+    // Check direction
+    var d = vec2.dot(axis, vec2.normalize([1,1],[1,1]));
+    test.ok(Math.abs(d) > 1-eps);
+
+    test.done();
+};
+
+exports.getClosestEdge = function(test){
+    var i = Narrowphase.getClosestEdge(convex, 0, [1,0]);
+
+    // Should be first or last edge
+    test.ok(i !== -1);
+
+    // Last edge is given by i == vs.length-2 since it is spanned by vs.length-2 to vs.length-1
+    test.ok(i===0 || i % (convex.vertices.length-2) === 0);
+
+    test.done();
+};
+
+exports.lineCapsule = function(test){
+    var result = narrowphase.lineCapsule(bodyA, line, position, angle, bodyB, capsule, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.lineCapsule(bodyA, line, position, angle, bodyB, capsule, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.lineLine = function(test){
+    var result = narrowphase.lineLine(bodyA, line, position, angle, bodyB, line, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.lineLine(bodyA, line, position, angle, bodyB, line, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.lineBox = function(test){
+    var result = narrowphase.lineBox(bodyA, line, position, angle, bodyB, rect, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.lineBox(bodyA, line, position, angle, bodyB, rect, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.particleConvex = function(test){
+    var result = narrowphase.particleConvex(bodyA, particle, position, angle, bodyB, convex, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.particleConvex(bodyA, particle, position, angle, bodyB, convex, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.particlePlane = function(test){
+    var result = narrowphase.particlePlane(bodyA, particle, position, angle, bodyB, plane, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.particlePlane(bodyA, particle, position, angle, bodyB, plane, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.planeLine = function(test){
+    var result = narrowphase.planeLine(bodyA, plane, position, angle, bodyB, line, position, angle);
+    test.equal(typeof result, 'number');
+
+    result = narrowphase.planeLine(bodyA, plane, position, angle, bodyB, line, position, angle, true);
+    test.equal(typeof result, 'boolean');
+
+    test.done();
+};
+
+exports.projectConvexOntoAxis = function(test){
+    var span = vec2.create();
+
+    // Moved along axis propendicular to the projection axis
+    Narrowphase.projectConvexOntoAxis(convex,[1,0],0,[0,1],span);
+
+    test.ok(span[0] > -1 - eps);
+    test.ok(span[0] < -1 + eps);
+    test.ok(span[1] > 1 - eps);
+    test.ok(span[1] < 1 + eps);
+
+    // Along the x axis
+    Narrowphase.projectConvexOntoAxis(convex,[0,1],0,[0,1],span);
+
+    test.ok(span[0] > 0 - eps);
+    test.ok(span[0] < 0 + eps);
+    test.ok(span[1] > 2 - eps);
+    test.ok(span[1] < 2 + eps);
+
+    // Along the x axis - rotated 180 degrees - should not do anything special
+    Narrowphase.projectConvexOntoAxis(convex,[0,1],Math.PI / 2,[0,1],span);
+
+    test.ok(span[0] > 0 - eps);
+    test.ok(span[0] < 0 + eps);
+    test.ok(span[1] > 2 - eps);
+    test.ok(span[1] < 2 + eps);
+
+    var span = vec2.create();
+    Narrowphase.projectConvexOntoAxis(rect,[1,0],0,[0,1],span);
+    test.done();
+};
+
+exports.reset = function(test){
+    var contact = narrowphase.createContactEquation(bodyA, bodyB, plane, rect);
+    narrowphase.contactEquations.push(contact);
+    narrowphase.reset();
+    test.equal(narrowphase.contactEquations.length, 0);
+    test.equal(narrowphase.frictionEquations.length, 0);
+    test.done();
+};
+
+
+exports.bodiesOverlap = function(test){
+    bodyA.addShape(new Circle({ radius: 1 }));
+    bodyB.addShape(new Circle({ radius: 1 }));
+    test.ok(narrowphase.bodiesOverlap(bodyA, bodyB));
+    bodyB.position[0] = 10;
+    test.ok(!narrowphase.bodiesOverlap(bodyA, bodyB));
+    test.done();
+};
+

test/collision/Ray.js

+var vec2 = require("../../src/math/vec2");
+var Circle = require('../../src/shapes/Circle');
+var Plane = require('../../src/shapes/Plane');
+var Ray = require('../../src/collision/Ray');
+var Body = require('../../src/objects/Body');
+var RaycastResult = require('../../src/collision/RaycastResult');
+var AABB = require('../../src/collision/AABB');
+var Heightfield = require('../../src/shapes/Heightfield');
+
+module.exports = {
+	construct: function(test){
+		var ray = new Ray({
+			from: [4,3],
+			to: [1,2],
+			skipBackfaces: true,
+			collisionMask: 4,
+			collisionGroup: 4|2,
+			mode: Ray.ALL
+		});
+		test.equal(ray.from[0], 4);
+		test.equal(ray.from[1], 3);
+		test.equal(ray.to[0], 1);
+		test.equal(ray.to[1], 2);
+		test.equal(ray.skipBackfaces, true);
+		test.equal(ray.collisionMask, 4);
+		test.equal(ray.collisionGroup, 4|2);
+		test.equal(ray.mode, Ray.ALL);
+		test.done();
+	},
+	update: function(test){
+		var ray = new Ray({
+			from: [0,0],
+			to: [1,0]
+		});
+		test.equal(ray.direction[0], 1);
+		test.equal(ray.direction[1], 0);
+		test.equal(ray.length, 1);
+
+		ray.to[0] = 0;
+		ray.to[1] = 2;
+		ray.update();
+		test.equal(ray.direction[0], 0);
+		test.equal(ray.direction[1], 1);
+		test.equal(ray.length, 2);
+
+		test.done();
+	},
+	getAABB: function(test){
+		var ray = new Ray({
+			from: [0,0],
+			to: [1,1]
+		});
+		var aabb = new AABB();
+		ray.getAABB(aabb);
+		test.deepEqual(aabb, new AABB({ lowerBound: [0,0], upperBound: [1,1] }));
+		test.done();
+	}
+};

test/collision/RaycastResult.js

+var vec2 = require("../../src/math/vec2");
+var Circle = require('../../src/shapes/Circle');
+var Plane = require('../../src/shapes/Plane');
+var Ray = require('../../src/collision/Ray');
+var Body = require('../../src/objects/Body');
+var RaycastResult = require('../../src/collision/RaycastResult');
+var Heightfield = require('../../src/shapes/Heightfield');
+
+module.exports = {
+	construct: function(test){
+		var result = new RaycastResult();
+		test.done();
+	},
+	reset: function(test){
+		var result = new RaycastResult();
+		var result2 = new RaycastResult();
+		result.normal[0] = 1;
+		result.fraction = 1;
+		result.shape = new Plane();
+		result.body = new Body();
+		result.faceIndex = 123;
+		result.reset();
+		test.deepEqual(result, result2);
+		test.done();
+	},
+	getHitDistance: function(test){
+		var result = new RaycastResult();
+		var plane = new Plane();
+		var ray = new Ray({
+			from: [0,2],
+			to: [0,-2]
+		});
+		plane.raycast(result, ray, [0,0], 0);
+		test.equal(result.getHitDistance(ray), 2);
+		test.done();
+	},
+	hasHit: function(test){
+		var result = new RaycastResult();
+		test.ok(!result.hasHit());
+		test.done();
+	},
+	getHitPoint: function(test){
+		var result = new RaycastResult();
+		var plane = new Plane();
+		var ray = new Ray({
+			from: [0,2],
+			to: [0,-2]
+		});
+		plane.raycast(result, ray, [0,0], 0);
+		var point = [1, 2];
+		result.getHitPoint(point, ray);
+		test.deepEqual(point, [0,0]);
+		test.done();
+	},
+	stop: function(test){
+		var result = new RaycastResult();
+		result.stop();
+		test.ok(result.isStopped);
+		test.done();
+	}
+};

test/collision/SAPBroadphase.js

+var SAPBroadphase = require(__dirname + '/../../src/collision/SAPBroadphase');
+
+exports.construct = function(test){
+    var broadphase = new SAPBroadphase();
+    test.done();
+};
+
+exports.checkBounds = function(test){
+    // STUB
+    test.done();
+};
+
+exports.getCollisionPairs = function(test){
+    // STUB
+    test.done();
+};
+
+exports.setWorld = function(test){
+    // STUB
+    test.done();
+};
+
+exports.sortAxisListX = function(test){
+    // STUB
+    test.done();
+};
+
+exports.sortAxisListY = function(test){
+    // STUB
+    test.done();
+};
+

test/constraints/Constraint.js

+var Constraint = require(__dirname + '/../../src/constraints/Constraint');
+
+exports.construct = function(test){
+    var constraint = new Constraint();
+    test.done();
+};
+
+exports.update = function(test){
+    // STUB
+    test.done();
+};
+
+exports.setStiffness = function(test){
+	// STUB
+	test.done();
+};
+
+exports.setRelaxation = function(test){
+	// STUB
+	test.done();
+};
\ No newline at end of file

test/constraints/DistanceConstraint.js

+var DistanceConstraint = require(__dirname + '/../../src/constraints/DistanceConstraint');
+
+exports.construct = function(test){
+    // STUB
+    test.done();
+};
+
+exports.getMaxForce = function(test){
+    // STUB
+    test.done();
+};
+
+exports.setMaxForce = function(test){
+    // STUB
+    test.done();
+};
+
+exports.update = function(test){
+    // STUB
+    test.done();
+};
+

test/constraints/GearConstraint.js

+var GearConstraint = require(__dirname + '/../../src/constraints/GearConstraint');
+
+exports.construct = function(test){
+    // STUB
+    test.done();
+};
+

test/constraints/LockConstraint.js

+var LockConstraint = require(__dirname + '/../../src/constraints/LockConstraint');
+
+exports.construct = function(test){
+    // STUB
+    test.done();
+};
+

test/constraints/PrismaticConstraint.js

+var PrismaticConstraint = require(__dirname + '/../../src/constraints/PrismaticConstraint');
+
+exports.construct = function(test){
+    // STUB
+    test.done();
+};
+
+exports.disableMotor = function(test){
+    // STUB
+    test.done();
+};
+
+exports.enableMotor = function(test){
+    // STUB
+    test.done();
+};
+
+exports.update = function(test){
+    // STUB
+    test.done();
+};
+

test/constraints/RevoluteConstraint.js

+var RevoluteConstraint = require(__dirname + '/../../src/constraints/RevoluteConstraint');
+
+exports.construct = function(test){
+    // STUB
+    test.done();
+};
+
+exports.disableMotor = function(test){
+    // STUB
+    test.done();
+};
+
+exports.enableMotor = function(test){
+    // STUB
+    test.done();
+};
+
+exports.getMotorSpeed = function(test){
+    // STUB
+    test.done();
+};
+
+exports.motorIsEnabled = function(test){
+    // STUB
+    test.done();
+};
+
+exports.setMotorSpeed = function(test){
+    // STUB
+    test.done();
+};
+

test/equations/AngleLockEquation.js

+var AngleLockEquation = require(__dirname + '/../../src/equations/AngleLockEquation');
+
+exports.construct = function(test){
+    // STUB
+    test.done();
+};
+

test/equations/ContactEquation.js

+var ContactEquation = require(__dirname + '/../../src/equations/ContactEquation');
+var Body = require(__dirname + '/../../src/objects/Body');
+
+exports.construct = function(test){
+	var bodyA = new Body();
+	var bodyB = new Body();
+    var eq = new ContactEquation(bodyA, bodyB);
+
+    test.done();
+};
+
+exports.computeB = function(test){
+    // TODO
+    test.done();
+};
+
+exports.getVelocityAlongNormal = function(test){
+    var bodyA = new Body({ position: [-1, 0] });
+	var bodyB = new Body({ position: [1, 0] });
+    var eq = new ContactEquation(bodyA, bodyB);
+	test.equal(eq.getVelocityAlongNormal(), 0);
+	eq.normalA[0] = 1; // points out from bodyA toward bodyB
+	eq.normalA[1] = 0;
+	bodyB.velocity[0] = 1; // Away from bodyA along the positive normal direction
+	test.equal(eq.getVelocityAlongNormal(), -1);
+
+    test.done();
+};
\ No newline at end of file

test/equations/Equation.js

+var Equation = require(__dirname + '/../../src/equations/Equation');
+var Body = require(__dirname + '/../../src/objects/Body');
+
+exports.construct = function(test){
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var minForce = -100;
+    var maxForce = 100;
+    var eq = new Equation(bodyA, bodyB, minForce, maxForce);
+    test.equal(eq.minForce, minForce);
+    test.equal(eq.maxForce, maxForce);
+    test.equal(eq.bodyA, bodyA);
+    test.equal(eq.bodyB, bodyB);
+    test.done();
+};
+
+exports.addToWlambda = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeB = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeGW = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeGWlambda = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeGiMGt = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeGiMf = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeGq = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeInvC = function(test){
+    // STUB
+    test.done();
+};
+
+exports.updateSpookParams = function(test){
+    // STUB
+    test.done();
+};
+

test/equations/FrictionEquation.js

+var FrictionEquation = require(__dirname + '/../../src/equations/FrictionEquation');
+var Body = require(__dirname + '/../../src/objects/Body');
+
+exports.construct = function(test){
+	var slipForce = 100;
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var eq = new FrictionEquation(bodyA, bodyB, slipForce);
+    test.equal(eq.minForce, -slipForce);
+    test.equal(eq.maxForce, slipForce);
+    test.done();
+};
+
+exports.setSlipForce = function(test){
+    // STUB
+    test.done();
+};
+

test/equations/RotationalLockEquation.js

+var RotationalLockEquation = require(__dirname + '/../../src/equations/RotationalLockEquation');
+var Body = require(__dirname + '/../../src/objects/Body');
+
+exports.construct = function(test){
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var eq = new RotationalLockEquation(bodyA, bodyB);
+    test.done();
+};
+

test/equations/RotationalVelocityEquation.js

+var RotationalVelocityEquation = require(__dirname + '/../../src/equations/RotationalVelocityEquation');
+var Body = require(__dirname + '/../../src/objects/Body');
+
+exports.construct = function(test){
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var eq = new RotationalVelocityEquation(bodyA, bodyB);
+    test.done();
+};
+

test/events/EventEmitter.js

+var EventEmitter = require(__dirname + '/../../src/events/EventEmitter');
+
+exports.construct = function(test){
+    var emitter = new EventEmitter();
+    test.done();
+};
+
+exports.emit = function(test){
+    // STUB
+    test.done();
+};
+
+exports.has = function(test){
+    // STUB
+    test.done();
+};
+
+exports.off = function(test){
+    // STUB
+    test.done();
+};
+
+exports.on = function(test){
+    // STUB
+    test.done();
+};
+

test/material/ContactMaterial.js

+var ContactMaterial =   require(__dirname + '/../../src/material/ContactMaterial')
+,   Material =          require(__dirname + '/../../src/material/Material')
+
+exports.construct = function(test){
+    var m1 = new Material(),
+        m2 = new Material();
+    new ContactMaterial(m1,m2);
+
+    test.throws(function(){
+        new ContactMaterial();
+    },'Should throw error if not passed materials');
+
+    test.notEqual(  new ContactMaterial(m1,m2).id,
+                    new ContactMaterial(m1,m2).id,
+                    'Should get new ids');
+
+    test.done();
+};
+

test/material/Material.js

+var Material = require(__dirname + '/../../src/material/Material');
+
+exports.construct = function(test){
+    var material1 = new Material();
+    var material2 = new Material();
+    test.notEqual(material1.id, material2.id);
+    test.done();
+};
+

test/math/vec2.js

+var vec2 = require(__dirname + '/../../src/math/vec2');
+
+exports.centroid = function(test){
+    var a=[0,0],
+        b=[1,0],
+        c=[2,3],
+        out=[0,0];
+    vec2.centroid(out,a,b,c);
+    test.deepEqual(out,[1,1]);
+    test.done();
+};
+
+exports.crossLength = function(test){
+    var a=[1,0],
+        b=[0,1],
+        out=[0,0];
+    test.deepEqual(vec2.crossLength(a,b),1);
+    test.done();
+};
+
+exports.crossVZ = function(test){
+    // STUB
+    test.done();
+};
+
+exports.crossZV = function(test){
+    // STUB
+    test.done();
+};
+
+exports.getX = function(test){
+    // STUB
+    test.done();
+};
+
+exports.getY = function(test){
+    // STUB
+    test.done();
+};
+
+exports.rotate = function(test){
+    var a=[1,0],
+        out=[0,0];
+    vec2.rotate(out,a,Math.PI);
+    test.ok(vec2.distance(out,[-1,0])<0.001);
+    test.done();
+};
+

test/objects/Body.js

+var Body = require(__dirname + '/../../src/objects/Body');
+var Circle = require(__dirname + '/../../src/shapes/Circle');
+var Box = require(__dirname + '/../../src/shapes/Box');
+var World = require(__dirname + '/../../src/world/World');
+var AABB = require(__dirname + '/../../src/collision/AABB');
+var vec2 = require(__dirname + '/../../src/math/vec2');
+var Shape = require(__dirname + '/../../src/shapes/Shape');
+
+
+module.exports = {
+    construct: function(test){
+
+        // Static via mass=0
+        var body = new Body({
+            mass : 0,
+        });
+        test.equal(body.invMass,0);
+        test.equal(body.type,Body.STATIC);
+
+        // Setting things via options
+        var o = {
+            position:[0,1],
+            velocity:[1,2],
+            force:[3,4],
+            angularVelocity:5,
+            angularForce:5,
+            angle:Math.PI/2
+        };
+        body = new Body(o);
+        test.equal(vec2.distance(body.position, o.position),0);
+        test.equal(vec2.distance(body.velocity, o.velocity),0);
+        test.equal(vec2.distance(body.force,    o.force   ),0);
+        test.equal(body.angle,                  o.angle);
+        test.equal(body.angularVelocity,        o.angularVelocity);
+        test.equal(body.angularForce,           o.angularForce);
+
+        // id tick
+        test.equal(new Body().id+1, new Body().id);
+
+        test.done();
+    },
+
+    addShape: function(test){
+        // STUB
+        test.done();
+    },
+
+    adjustCenterOfMass: function(test){
+        // STUB
+        test.done();
+    },
+
+    applyDamping: function(test){
+        // STUB
+        test.done();
+    },
+
+    applyForce: {
+        withPoint: function(test){
+            var body = new Body({ mass: 1, position: [2,3] });
+            var force = [0,1];
+            var point = [1,0];
+
+            body.applyForce(force, point);
+            test.equal(body.force[0], 0);
+            test.equal(body.force[1], 1);
+            test.equal(body.angularForce, 1); // [1,0,0] cross [0,1,0] is [0,0,1]
+
+            test.done();
+        },
+        withoutPoint: function(test){
+            var body = new Body({ mass: 1, position: [2,3] });
+            var force = [0,1];
+
+            body.applyForce(force);
+            test.equal(body.force[0], 0);
+            test.equal(body.force[1], 1);
+            test.equal(body.angularForce, 0);
+
+            test.done();
+        },
+    },
+
+    applyForceLocal: {
+        withPoint: function(test){
+            var bodyA = new Body({
+                mass: 1,
+                position: [2,3],
+                angle: Math.PI // rotated 180 degrees
+            });
+            bodyA.addShape(new Circle({ radius: 1 }));
+            bodyA.applyForceLocal([-1,0],[0,1]);
+            test.ok(bodyA.angularForce > 0);
+            test.ok(bodyA.force[0] > 0);
+            test.ok(Math.abs(bodyA.force[1]) < 0.001);
+            test.done();
+        },
+        withoutPoint: function(test){
+            var bodyA = new Body({
+                mass: 1,
+                position: [2,3],
+                angle: Math.PI // rotated 180 degrees
+            });
+            bodyA.addShape(new Circle({ radius: 1 }));
+            bodyA.applyForceLocal([-1,0]);
+            test.equal(bodyA.angularForce, 0);
+            test.ok(bodyA.force[0] > 0);
+            test.ok(Math.abs(bodyA.force[1]) < 0.001);
+            test.done();
+        }
+    },
+
+    applyImpulse: {
+        withPoint: function(test){
+            var bodyA = new Body({ mass: 1, position: [2,3] });
+            bodyA.addShape(new Circle({ radius: 1 }));
+            bodyA.applyImpulse([-1,0],[0,1]);
+            test.ok(bodyA.angularVelocity !== 0);
+            test.ok(bodyA.velocity[0] !== 0);
+            test.equal(bodyA.velocity[1], 0);
+            test.done();
+        },
+        withoutPoint: function(test){
+            var bodyA = new Body({ mass: 1, position: [2,3] });
+            bodyA.addShape(new Circle({ radius: 1 }));
+            bodyA.applyImpulse([-1,0]);
+            test.equal(bodyA.angularVelocity, 0);
+            test.ok(bodyA.velocity[0] !== 0);
+            test.equal(bodyA.velocity[1], 0);
+            test.done();
+        }
+    },
+
+    applyImpulseLocal: {
+        withPoint: function(test){
+            var bodyA = new Body({
+                mass: 1,
+                position: [2,3],
+                angle: Math.PI // rotated 180 degrees
+            });
+            bodyA.addShape(new Circle({ radius: 1 }));
+            bodyA.applyImpulseLocal([-1,0],[0,1]);
+            test.ok(bodyA.angularVelocity > 0);
+            test.ok(bodyA.velocity[0] > 0);
+            test.ok(Math.abs(bodyA.velocity[1]) < 0.001);
+            test.done();
+        },
+        withoutPoint: function(test){
+            var bodyA = new Body({
+                mass: 1,
+                position: [2,3],
+                angle: Math.PI // rotated 180 degrees
+            });
+            bodyA.addShape(new Circle({ radius: 1 }));
+            bodyA.applyImpulseLocal([-1,0]);
+            test.equal(bodyA.angularVelocity, 0);
+            test.ok(bodyA.velocity[0] > 0);
+            test.ok(Math.abs(bodyA.velocity[1]) < 0.001);
+            test.done();
+        }
+    },
+
+    fromPolygon: function(test){
+        var b = new Body();
+        test.ok(b.fromPolygon( [[-1, 1],
+                                [-1, 0],
+                                [1, 0],
+                                [1, 1],
+                                [0.5, 0.5]]));
+
+        test.ok(b.shapes.length > 0);
+
+        test.done();
+    },
+
+    overlaps: function(test){
+        var bodyA = new Body({ mass: 1 });
+        var bodyB = new Body({ mass: 1 });
+        bodyA.addShape(new Circle());
+        bodyB.addShape(new Circle());
+        var world = new World();
+        world.addBody(bodyA);
+        world.addBody(bodyB);
+        world.step(1/60);
+        test.ok(bodyA.overlaps(bodyB));
+        test.done();
+    },
+
+    removeShape: function(test){
+        var body = new Body();
+        body.addShape(new Circle({ radius: 1 }));
+        test.ok(body.removeShape(body.shapes[0]));
+        test.ok(!body.removeShape(new Circle({ radius: 1 })));
+        test.equal(body.shapes.length, 0);
+        test.done();
+    },
+
+    getArea: function(test){
+        var body = new Body();
+        body.addShape(new Box({ width: 1, height: 1 }));
+        test.equal(body.getArea(), 1);
+        test.done();
+    },
+
+    getAABB: function(test){
+        var body = new Body();
+        body.addShape(new Box({ width: 1, height: 1 }));
+        test.deepEqual(body.getAABB(), new AABB({ lowerBound: [-0.5, -0.5], upperBound: [0.5, 0.5] }));
+        test.done();
+    },
+
+    setDensity: function(test){
+        var body = new Body({ mass: 1 });
+        body.addShape(new Circle({ radius: 1 }));
+        var inertiaBefore = body.inertia;
+        body.setDensity(10);
+        test.equal(body.mass, body.getArea() * 10);
+        test.ok(inertiaBefore !== body.inertia);
+        test.done();
+    },
+
+    setZeroForce: function(test){
+        var b = new Body({ force:[1,2], angularForce:3 });
+        b.setZeroForce();
+        test.equal(vec2.length(b.force),0);
+        test.equal(b.angularForce,0);
+        test.done();
+    },
+
+    sleep: function(test){
+        var b = new Body({ mass: 1 });
+        test.equal(b.sleepState, Body.AWAKE);
+        b.sleep();
+        test.equal(b.sleepState, Body.SLEEPING);
+        test.done();
+    },
+
+    toLocalFrame: function(test){
+        var b = new Body({ position: [1,1] });
+        var localPoint = [123, 456];
+        b.toLocalFrame(localPoint, [1,1]);
+        test.deepEqual(localPoint, [0,0]);
+        test.done();
+    },
+
+    toWorldFrame: function(test){
+        var b = new Body({ position: [1,1] });
+        var worldPoint = [123, 456];
+        b.toWorldFrame(worldPoint, [1,1]);
+        test.deepEqual(worldPoint, [2,2]);
+        test.done();
+    },
+
+    vectorToLocalFrame: function(test){
+        // STUB
+        test.done();
+    },
+
+    vectorToWorldFrame: function(test){
+        // STUB
+        test.done();
+    },
+
+    updateAABB: function(test){
+        var b = new Body();
+        b.updateAABB();
+
+        var b = new Body(),
+            s = new Circle({ radius: 1 });
+        b.addShape(s);
+        b.updateAABB();
+
+        test.equal(b.aabb.lowerBound[0], -1, 'Lower AABB bound should be -1');
+        test.equal(b.aabb.upperBound[0],  1, 'Upper AABB bound should be 1');
+        test.equal(b.aabb.lowerBound[1], -1, 'Lower AABB bound should be -1');
+        test.equal(b.aabb.upperBound[1],  1, 'Upper AABB bound should be 1');
+
+        var b = new Body(),
+            s = new Circle({ radius: 1 }),
+            offset = [-2,3];
+        b.addShape(s,offset,Math.PI/2);
+        b.updateAABB();
+
+        test.equal(b.aabb.lowerBound[0], -s.radius + offset[0]);
+        test.equal(b.aabb.upperBound[0],  s.radius + offset[0]);
+        test.equal(b.aabb.lowerBound[1], -s.radius + offset[1]);
+        test.equal(b.aabb.upperBound[1],  s.radius + offset[1]);
+
+        test.done();
+    },
+
+    updateBoundingRadius: function(test){
+        var body = new Body({ mass: 1 });
+        var shape = new Circle({ radius: 1 });
+        body.addShape(shape);
+        test.equal(body.boundingRadius, 1);
+        shape.radius = 2;
+        shape.updateBoundingRadius();
+        body.updateBoundingRadius();
+        test.equal(body.boundingRadius, 2);
+        test.done();
+    },
+
+    updateMassProperties: function(test){
+        // STUB
+        test.done();
+    },
+
+    wakeUp: function(test){
+        var b = new Body({ mass: 1 });
+        b.sleep();
+        test.equal(b.sleepState, Body.SLEEPING);
+        b.wakeUp();
+        test.equal(b.sleepState, Body.AWAKE);
+        test.done();
+    },
+
+    integrate: function(test){
+        var b = new Body({ velocity: [1,0], mass: 1 });
+        b.integrate(1);
+        test.deepEqual(b.position, vec2.fromValues(1,0));
+        test.done();
+    },
+
+    getVelocityAtPoint: function(test){
+        var body = new Body({
+            mass: 1,
+            velocity: [1,0]
+        });
+        var velocity = [0,0];
+        body.getVelocityAtPoint(velocity, [0,0]);
+        test.deepEqual(velocity, [1,0]);
+
+        body.velocity[0] = 0;
+        body.angularVelocity = 1;
+        body.getVelocityAtPoint(velocity, [1,0]);
+        test.ok(Math.abs(velocity[0]) < 0.001);
+        test.equal(velocity[1], 1); // r x w = 1 x 1 = 1
+
+        test.done();
+    },
+
+    collisionResponse: function(test){
+        var bodyA = new Body({ mass: 1, position: [1, 0] });
+        bodyA.addShape(new Circle({ radius: 1 }));
+
+        var bodyB = new Body({ mass: 1, position: [-1, 0] });
+        bodyB.addShape(new Circle({ radius: 1 }));
+
+        var world = new World();
+        world.addBody(bodyA);
+        world.addBody(bodyB);
+
+        world.step(1 / 60);
+        test.ok(world.narrowphase.contactEquations[0].enabled);
+
+        bodyA.collisionResponse = false;
+        world.step(1 / 60);
+        test.ok(!world.narrowphase.contactEquations[0].enabled);
+
+        bodyA.collisionResponse = true;
+        bodyA.shapes[0].collisionResponse = false;
+        world.step(1 / 60);
+        test.ok(!world.narrowphase.contactEquations[0].enabled);
+
+        test.done();
+    }
+};

test/objects/LinearSpring.js

+var LinearSpring = require(__dirname + '/../../src/objects/LinearSpring');
+var Body = require(__dirname + '/../../src/objects/Body');
+var vec2 = require(__dirname + '/../../src/math/vec2');
+
+var bodyA, bodyB, spring;
+var options = {
+    restLength: 1,
+    stiffness: 2,
+    damping: 3,
+    localAnchorA: [4,5],
+    localAnchorB: [6,7],
+};
+
+exports.setUp = function(callback){
+    bodyA = new Body();
+    bodyB = new Body();
+    spring = new LinearSpring(bodyA, bodyB, options);
+    callback();
+};
+
+exports.construct = function(test){
+    test.equal(spring.stiffness,    options.stiffness);
+    test.equal(spring.restLength,   options.restLength);
+    test.equal(spring.damping,      options.damping);
+
+    // TODO: pass worldAnchorA/B
+
+    test.done();
+};
+
+exports.applyForce = function(test){
+    spring.setWorldAnchorA([0,0]);
+    spring.setWorldAnchorB([0,0]);
+    bodyA.position[0] = 1;
+    bodyB.position[0] = -1;
+    spring.applyForce();
+    test.ok(bodyA.force[0] < 0);
+    test.ok(bodyB.force[0] > 0);
+    test.done();
+};
+
+exports.getWorldAnchorA = function(test){
+    var v = vec2.create();
+    spring.getWorldAnchorA(v);
+    test.equal(v[0], options.localAnchorA[0]);
+    test.equal(v[1], options.localAnchorA[1]);
+    test.done();
+};
+
+exports.getWorldAnchorB = function(test){
+    var v = vec2.create();
+    spring.getWorldAnchorB(v);
+    test.equal(v[0], options.localAnchorB[0]);
+    test.equal(v[1], options.localAnchorB[1]);
+    test.done();
+};
+
+exports.setWorldAnchorA = function(test){
+    var v = vec2.create();
+    spring.setWorldAnchorA([0,0]);
+    spring.getWorldAnchorA(v);
+    test.equal(v[0], 0);
+    test.equal(v[1], 0);
+    test.done();
+};
+
+exports.setWorldAnchorB = function(test){
+    var v = vec2.create();
+    spring.setWorldAnchorB([0,0]);
+    spring.getWorldAnchorB(v);
+    test.equal(v[0], 0);
+    test.equal(v[1], 0);
+    test.done();
+};
+

test/objects/RotationalSpring.js

+var RotationalSpring = require(__dirname + '/../../src/objects/RotationalSpring');
+var Body = require(__dirname + '/../../src/objects/Body');
+var vec2 = require(__dirname + '/../../src/math/vec2');
+
+var bodyA, bodyB, spring;
+var options = {
+    restAngle: 1,
+    stiffness: 2,
+    damping: 3,
+};
+
+exports.setUp = function(callback){
+    bodyA = new Body();
+    bodyB = new Body();
+    spring = new RotationalSpring(bodyA, bodyB, options);
+    callback();
+};
+
+exports.construct = function(test){
+    test.equal(spring.stiffness,    options.stiffness);
+    test.equal(spring.restAngle,    options.restAngle);
+    test.equal(spring.damping,      options.damping);
+    test.done();
+};
+
+exports.applyForce = function(test){
+    bodyA.angle = Math.PI / 4;
+    bodyB.angle = 0;
+    spring.applyForce();
+    test.ok(bodyA.angularForce < 0);
+    test.ok(bodyB.angularForce > 0);
+    test.done();
+};

test/shapes/Box.js

+var Box = require(__dirname + '/../../src/shapes/Box');
+var Ray =   require(__dirname + '/../../src/collision/Ray');
+var RaycastResult =   require(__dirname + '/../../src/collision/RaycastResult');
+var AABB = require(__dirname + '/../../src/collision/AABB');
+
+exports.construct = function(test){
+    var rect = new Box();
+    test.equal(rect.width,1);
+    test.equal(rect.height,1);
+    var rect = new Box({ width: 2, height: 3 });
+    test.done();
+};
+
+exports.computeAABB = function(test){
+    var aabb = new AABB(),
+        offset = [2,3];
+
+    var c = new Box({ width: 1, height: 2 });
+    c.computeAABB(aabb,offset,Math.PI / 2);
+    test.equal(aabb.lowerBound[0], -1   + offset[0]);
+    test.equal(aabb.lowerBound[1], -0.5 + offset[1]);
+    test.equal(aabb.upperBound[0],  1   + offset[0]);
+    test.equal(aabb.upperBound[1],  0.5 + offset[1]);
+
+    test.done();
+};
+
+exports.computeMomentOfInertia = function(test){
+    // STUB
+    test.done();
+};
+
+exports.updateBoundingRadius = function(test){
+    // STUB
+    test.done();
+};
+
+exports.raycast = function(test){
+    var ray = new Ray({
+        mode: Ray.CLOSEST,
+        from: [0,0],
+        to: [10,0]
+    });
+
+    var shape = new Box();
+    var result = new RaycastResult();
+    shape.raycast(result, ray, [1,0], Math.PI / 2);
+
+    test.done();
+};
\ No newline at end of file

test/shapes/Capsule.js

+var Capsule = require(__dirname + '/../../src/shapes/Capsule');
+var Ray = require(__dirname + '/../../src/collision/Ray');
+var RaycastResult = require(__dirname + '/../../src/collision/RaycastResult');
+
+exports.construct = function(test){
+    var capsule = new Capsule({ length: 2, radius: 3 });
+    test.equal(capsule.length, 2);
+    test.equal(capsule.radius, 3);
+    test.done();
+};
+
+exports.computeAABB = function(test){
+    // STUB
+    test.done();
+};
+
+exports.computeMomentOfInertia = function(test){
+    // STUB
+    test.done();
+};
+
+exports.raycast = function(test){
+    var ray = new Ray({
+        mode: Ray.CLOSEST,
+        from: [0,0],
+        to: [10,0]
+    });
+
+    var capsule = new Capsule({ length: 1, radius: 0.5 });
+    var result = new RaycastResult();
+    capsule.raycast(result, ray, [1,0], Math.PI / 2);
+
+    test.done();
+};
+

test/shapes/Circle.js

+var Circle = require(__dirname + '/../../src/shapes/Circle');
+var AABB = require(__dirname + '/../../src/collision/AABB');
+var Ray = require(__dirname + '/../../src/collision/Ray');
+var RaycastResult = require(__dirname + '/../../src/collision/RaycastResult');
+
+exports.construct = function(test){
+    var circle = new Circle({ radius: 2 });
+    test.equal(circle.radius, 2);
+    test.done();
+};
+
+exports.computeAABB = function(test){
+    var aabb = new AABB(),
+        offset = [2,3];
+
+    var c = new Circle({ radius: 1 });
+    c.computeAABB(aabb,offset,Math.PI / 2);
+    test.equal(aabb.lowerBound[0],-1 + offset[0]);
+    test.equal(aabb.lowerBound[1],-1 + offset[1]);
+    test.equal(aabb.upperBound[0], 1 + offset[0]);
+    test.equal(aabb.upperBound[1], 1 + offset[1]);
+
+    test.done();
+};
+
+exports.raycast = function(test){
+    var ray = new Ray({
+		mode: Ray.CLOSEST,
+        from: [-10,0],
+        to: [10,0]
+    });
+
+    var shape = new Circle({ radius: 0.5 });
+    var result = new RaycastResult();
+    shape.raycast(result, ray, [0,0], 0);
+    test.equal(result.normal[0], -1);
+    test.equal(result.normal[1], 0);
+
+    test.done();
+};
+

test/shapes/Convex.js

+var Convex = require(__dirname + '/../../src/shapes/Convex')
+,   vec2 =   require(__dirname + '/../../src/math/vec2')
+,   AABB =   require(__dirname + '/../../src/collision/AABB')
+,   Shape =   require(__dirname + '/../../src/shapes/Shape')
+,   Ray =   require(__dirname + '/../../src/collision/Ray')
+,   RaycastResult =   require(__dirname + '/../../src/collision/RaycastResult');
+
+exports.construct = function(test){
+    new Convex({ vertices: []});
+
+    test.throws(function(){
+        var c = new Convex({ vertices: [[-1,-1],
+                            [-1, 1],
+                            [ 1, 1],
+                            [ 1,-1]] });
+    },"Should throw exception on clockwise winding.");
+
+    test.done();
+};
+
+exports.computeAABB = function(test){
+    var w = 2,
+        h = 1;
+    var c = new Convex({ vertices: [
+        [-w/2,-h/2],
+        [ w/2,-h/2],
+        [ w/2, h/2],
+        [-w/2, h/2],
+    ]});
+
+    var aabb = new AABB();
+    c.computeAABB(aabb,[1,2],0);
+
+    test.equal(aabb.lowerBound[0],-w/2 + 1);
+    test.equal(aabb.lowerBound[1],-h/2 + 2);
+    test.equal(aabb.upperBound[0], w/2 + 1);
+    test.equal(aabb.upperBound[1], h/2 + 2);
+
+    test.done();
+};
+
+exports.computeMomentOfInertia = function(test){
+    var w = 2,
+        h = 1;
+    var c = new Convex({ vertices: [
+        [-w/2,-h/2],
+        [ w/2,-h/2],
+        [ w/2, h/2],
+        [-w/2, h/2],
+    ]});
+    var mass = 1;
+    var I = c.computeMomentOfInertia(mass);
+    var boxInertia = mass*(h*h+w*w)/12;
+    test.ok(Math.abs(I-boxInertia) < 0.01,'Convex dont compute square inertia correctly');
+    test.done();
+};
+
+exports.triangleArea = function(test){
+    test.equal( Convex.triangleArea([0, 0], [1, 0], [1, 1]),  1/2);
+    test.done();
+};
+
+exports.updateArea = function(test){
+    var c = new Convex({ vertices: [[-1,-1],
+                        [ 1,-1],
+                        [ 1, 1],
+                        [-1, 1]]});
+    c.updateArea();
+    test.equal(c.area, 4)
+
+    var c = new Convex({ vertices: [
+        [990, 0],
+        [990, 10],
+        [0, 10],
+        [0, 0]
+    ]});
+    test.equal(c.area,9900);
+
+    test.done();
+};
+
+exports.updateBoundingRadius = function(test){
+    var w = 2,
+        h = 1;
+    var c = new Convex({ vertices: [
+        [-w/2,-h/2],
+        [ w/2,-h/2],
+        [ w/2, h/2],
+    ]});
+
+    test.equal(c.boundingRadius, Math.sqrt(w*w/4+h*h/4));
+
+    test.done();
+};
+
+exports.updateCenterOfMass = function(test){
+
+    // Test with box
+    var c = new Convex({ vertices: [[-1,-1],
+                        [ 1,-1],
+                        [ 1, 1],
+                        [-1, 1]]});
+    c.updateCenterOfMass();
+    test.equal(c.centerOfMass[0],0);
+    test.equal(c.centerOfMass[1],0);
+
+    // rotate and translate all points
+    var offset = vec2.fromValues(1,1);
+    for(var i=0; i!==c.vertices.length; i++){
+        var v = c.vertices[i];
+        vec2.rotate(v,v,Math.PI/4);
+        vec2.add(v,v,offset);
+    }
+
+    c.updateCenterOfMass();
+
+    test.ok(Math.abs(c.centerOfMass[0]-offset[0]) < 0.01);
+    test.ok(Math.abs(c.centerOfMass[1]-offset[1]) < 0.01);
+
+    test.done();
+};
+
+exports.updateTriangles = function(test){
+    var w = 2,
+        h = 1;
+    var c = new Convex({ vertices: [
+        [-w/2,-h/2],
+        [ w/2,-h/2],
+        [ w/2, h/2],
+    ]});
+    c.updateTriangles();
+    test.deepEqual([ [ 0, 1, 2 ] ],c.triangles);
+
+    test.done();
+};
+
+exports.raycast = function(test){
+    var ray = new Ray({
+        mode: Ray.CLOSEST,
+        from: [0,0],
+        to: [10,0]
+    });
+
+    var w = 1,
+        h = 1;
+    var shape = new Convex({ vertices: [
+        [-w/2,-h/2],
+        [ w/2,-h/2],
+        [ w/2, h/2],
+        [-w/2, h/2],
+    ]});
+    var result = new RaycastResult();
+    shape.raycast(result, ray, [1,0], Math.PI / 2);
+
+    test.done();
+};
\ No newline at end of file

test/shapes/Line.js

+var Line = require(__dirname + '/../../src/shapes/Line')
+,   Ray =   require(__dirname + '/../../src/collision/Ray')
+,   RaycastResult =   require(__dirname + '/../../src/collision/RaycastResult');
+
+exports.construct = function(test){
+    var line = new Line({ length: 2 });
+    test.equal(line.length, 2);
+
+    line = new Line();
+    test.equal(line.length, 1);
+
+    test.done();
+};
+
+exports.computeAABB = function(test){
+    // STUB
+    test.done();
+};
+
+exports.raycast = function(test){
+    var ray = new Ray({
+		mode: Ray.CLOSEST,
+        from: [0,0],
+        to: [10,0]
+    });
+
+    var shape = new Line({ length: 1 });
+    var result = new RaycastResult();
+    shape.raycast(result, ray, [1,0], Math.PI / 2);
+
+    test.done();
+};
\ No newline at end of file

test/shapes/Particle.js

+var Particle = require(__dirname + '/../../src/shapes/Particle');
+
+exports.construct = function(test){
+    var particle = new Particle();
+    test.done();
+};
+
+exports.computeAABB = function(test){
+    // STUB
+    test.done();
+};
+

test/shapes/Plane.js

+var Plane = require(__dirname + '/../../src/shapes/Plane');
+var AABB = require(__dirname + '/../../src/collision/AABB');
+var Ray =   require(__dirname + '/../../src/collision/Ray');
+var RaycastResult =   require(__dirname + '/../../src/collision/RaycastResult');
+
+exports.construct = function(test){
+    var plane = new Plane();
+    test.done();
+};
+
+exports.computeAABB = function(test){
+	var plane = new Plane();
+	var aabb = new AABB();
+    plane.computeAABB(aabb, [0,0], 0);
+    test.done();
+};
+
+exports.computeMomentOfInertia = function(test){
+    test.equal((new Plane()).computeMomentOfInertia(), 0);
+    test.done();
+};
+
+exports.updateBoundingRadius = function(test){
+	var plane = new Plane();
+    plane.updateBoundingRadius();
+    test.ok(plane.boundingRadius > 0);
+    test.done();
+};
+
+exports.raycast = function(test){
+    var ray = new Ray({
+        mode: Ray.CLOSEST,
+        from: [0,0],
+        to: [10,0]
+    });
+
+    var shape = new Plane();
+    var result = new RaycastResult();
+    shape.raycast(result, ray, [1,0], Math.PI / 2);
+
+    test.done();
+};
\ No newline at end of file

test/shapes/Shape.js

+var Shape = require(__dirname + '/../../src/shapes/Shape');
+
+exports.construct = function(test){
+    var shape = new Shape();
+    test.done();
+};

test/solver/GSSolver.js

+var GSSolver = require(__dirname + '/../../src/solver/GSSolver');
+var World = require(__dirname + '/../../src/world/World');
+var Body = require(__dirname + '/../../src/objects/Body');
+var DistanceConstraint = require(__dirname + '/../../src/constraints/DistanceConstraint');
+
+exports.construct = function(test){
+    var solver = new GSSolver();
+    test.done();
+};
+
+exports.solve = function(test){
+    var world = new World();
+    var bodyA = new Body({
+        mass: 1
+    });
+    var bodyB = new Body({
+        mass: 1,
+        position:[0,1.001]
+    });
+    var constraint = new DistanceConstraint(bodyA, bodyB, {
+        distance:1
+    });
+    world.addConstraint(constraint);
+    var solver = new GSSolver({
+        iterations: 10
+    });
+    solver.addEquations(constraint.equations);
+    solver.solve(1/60, world);
+    test.equal(solver.usedIterations, solver.iterations, 'should use all iterations if tolerance is zero');
+    solver.tolerance = 1;
+    solver.solve(1/60, world);
+    test.ok(solver.usedIterations < 10, 'should use less iterations if tolerance is nonzero');
+    test.done();
+};
+

test/solver/Solver.js

+var Solver = require(__dirname + '/../../src/solver/Solver');
+
+exports.construct = function(test){
+    var solver = new Solver();
+    test.done();
+};
+
+exports.addEquation = function(test){
+    // STUB
+    test.done();
+};
+
+exports.addEquations = function(test){
+    // STUB
+    test.done();
+};
+
+exports.removeAllEquations = function(test){
+    // STUB
+    test.done();
+};
+
+exports.removeEquation = function(test){
+    // STUB
+    test.done();
+};
+
+exports.solve = function(test){
+    // STUB
+    test.done();
+};
+
+exports.sortEquations = function(test){
+    // STUB
+    test.done();
+};
+

test/utils/ContactEquationPool.js

+var ContactEquationPool = require(__dirname + '/../../src/utils/ContactEquationPool');
+var ContactEquation = require(__dirname + '/../../src/equations/ContactEquation');
+
+exports.construct = function(test){
+    var pool = new ContactEquationPool();
+    test.done();
+};
+
+exports.resize = function(test){
+    var pool = new ContactEquationPool();
+    pool.resize(10);
+    test.equal(pool.objects.length, 10);
+    test.done();
+};
+
+exports.getRelease = function(test){
+    var pool = new ContactEquationPool();
+    test.equal(pool.objects.length, 0);
+    var object = pool.get();
+    object.bodyA = 'asd';
+    object.bodyB = 'asd2';
+    test.ok(object instanceof ContactEquation, 'should create contact equations');
+    test.equal(pool.objects.length, 0, 'should not increase pool size when creating');
+    pool.release(object);
+    test.equal(object.bodyA, null, 'should clean released object');
+    test.equal(object.bodyB, null, 'should clean released object');
+    test.equal(pool.objects.length, 1, 'should add released object to pool');
+    var object2 = pool.get();
+    test.equal(object, object2, 'should return pooled object');
+    test.done();
+};
+

test/utils/OverlapKeeper.js

+var OverlapKeeper = require(__dirname + '/../../src/utils/OverlapKeeper');
+var Body = require(__dirname + '/../../src/objects/Body');
+var Circle = require(__dirname + '/../../src/shapes/Circle');
+
+exports.construct = function(test){
+    var keeper = new OverlapKeeper();
+    test.done();
+};
+
+exports.tick = function(test){
+    var keeper = new OverlapKeeper();
+
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var shapeA = new Circle({ radius: 1 });
+    var shapeB = new Circle({ radius: 1 });
+
+    var keeper = new OverlapKeeper();
+    keeper.recordPool.resize(0);
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+
+    test.equal(keeper.recordPool.objects.length, 0);
+
+    keeper.tick();
+
+    test.equal(keeper.recordPool.objects.length, 0);
+
+    keeper.tick();
+
+    test.equal(keeper.recordPool.objects.length, 1);
+
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+
+    test.equal(keeper.recordPool.objects.length, 0);
+
+    test.done();
+};
+
+exports.getEndOverlaps = function(test){
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var bodyC = new Body();
+    var shapeA = new Circle({ radius: 1 });
+    var shapeB = new Circle({ radius: 1 });
+    var shapeC = new Circle({ radius: 1 });
+
+    var keeper = new OverlapKeeper();
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+
+    var result = keeper.getEndOverlaps();
+    test.equal(result.length, 0);
+
+    keeper.tick();
+
+    var result = keeper.getEndOverlaps();
+    test.equal(result.length, 1);
+
+    keeper.tick();
+
+    var result = keeper.getEndOverlaps();
+    test.equal(result.length, 0);
+
+    keeper.tick();
+
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+    keeper.setOverlapping(bodyC, shapeC, bodyB, shapeB);
+    keeper.setOverlapping(bodyC, shapeC, bodyA, shapeA);
+
+    var result = keeper.getEndOverlaps();
+    test.equal(result.length, 0);
+
+    keeper.tick();
+
+    var result = keeper.getEndOverlaps();
+    test.equal(result.length, 3);
+
+    test.done();
+};
+
+exports.getNewOverlaps = function(test){
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var bodyC = new Body();
+    var shapeA = new Circle({ radius: 1 });
+    var shapeB = new Circle({ radius: 1 });
+    var shapeC = new Circle({ radius: 1 });
+    var keeper = new OverlapKeeper();
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+
+    var result = keeper.getNewOverlaps();
+    test.equal(result.length, 1);
+    test.equal(result[0].bodyA, bodyA);
+    test.equal(result[0].bodyB, bodyB);
+    test.equal(result[0].shapeA, shapeA);
+    test.equal(result[0].shapeB, shapeB);
+
+    keeper.tick();
+
+    var result = keeper.getNewOverlaps();
+    test.equal(result.length, 0);
+
+    keeper.tick();
+
+    var result = keeper.getNewOverlaps();
+    test.equal(result.length, 0);
+
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+    keeper.setOverlapping(bodyC, shapeC, bodyB, shapeB);
+    keeper.setOverlapping(bodyC, shapeC, bodyA, shapeA);
+
+    var result = keeper.getNewOverlaps();
+    test.equal(result.length, 3);
+
+    test.done();
+};
+
+exports.isNewOverlap = function(test){
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var shapeA = new Circle({ radius: 1 });
+    var shapeB = new Circle({ radius: 1 });
+    var keeper = new OverlapKeeper();
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+
+    var result = keeper.isNewOverlap(shapeA, shapeB);
+    test.equal(result, true);
+
+    keeper.tick();
+
+    var result = keeper.isNewOverlap(shapeA, shapeB);
+    test.equal(result, false);
+
+    keeper.tick();
+
+    var result = keeper.isNewOverlap(shapeA, shapeB);
+    test.equal(result, false);
+
+    test.done();
+};
+
+exports.getNewBodyOverlaps = function(test){
+    var keeper = new OverlapKeeper();
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var bodyC = new Body();
+    var shapeA = new Circle();
+    var shapeB = new Circle();
+    var shapeC = new Circle();
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+
+    var result = keeper.getNewBodyOverlaps();
+    test.equal(result.length, 2);
+
+    keeper.tick();
+
+    var result = keeper.getNewBodyOverlaps();
+    test.equal(result.length, 0);
+
+    keeper.tick();
+
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+    keeper.setOverlapping(bodyC, shapeC, bodyB, shapeB);
+    keeper.setOverlapping(bodyC, shapeC, bodyA, shapeA);
+
+    var result = keeper.getNewBodyOverlaps();
+
+    test.equal(result.length, 6);
+
+    test.done();
+};
+
+exports.getEndBodyOverlaps = function(test){
+    var bodyA = new Body();
+    var bodyB = new Body();
+    var shapeA = new Circle({ radius: 1 });
+    var shapeB = new Circle({ radius: 1 });
+    var keeper = new OverlapKeeper();
+    keeper.setOverlapping(bodyA, shapeA, bodyB, shapeB);
+
+    var result = keeper.getEndBodyOverlaps();
+    test.equal(result.length, 0);
+
+    keeper.tick();
+
+    var result = keeper.getEndBodyOverlaps();
+    test.equal(result.length, 2);
+
+    test.done();
+};
\ No newline at end of file

test/utils/TupleDictionary.js

+var TupleDictionary = require(__dirname + '/../../src/utils/TupleDictionary');
+
+exports.construct = function(test){
+	var dict = new TupleDictionary();
+    test.done();
+};
+
+exports.getKey = function(test){
+	var dict = new TupleDictionary();
+	var key = dict.getKey(1, 2);
+	test.ok(typeof key === 'number');
+    test.done();
+};
+
+exports.get = function(test){
+	var dict = new TupleDictionary();
+	var a = { id: 1 };
+	var b = { id: 2 };
+	var c = { id: 3 };
+	dict.set(a.id, b.id, c);
+
+	var obj = dict.get(a.id, b.id);
+	test.deepEqual(obj, c, 'could not get by keys');
+
+	obj = dict.get(b.id, a.id);
+	test.deepEqual(obj, c, 'could not get by flipped keys');
+
+	obj = dict.get(3, 4, c);
+	test.equal(obj, undefined, 'could not get unset');
+
+    test.done();
+};
+
+exports.set = function(test){
+	var dict = new TupleDictionary();
+
+	dict.set(1, 2, 3);
+	var key = dict.getKey(1,2);
+	test.deepEqual(dict.keys, [key]);
+	test.equal(dict.data[key], 3);
+
+	dict.set(2, 1, 3);
+	test.deepEqual(dict.keys, [key]);
+
+	dict.set(2, 1, 3);
+	test.deepEqual(dict.keys, [key]);
+
+    test.done();
+};
+
+
+exports.reset = function(test){
+	var dict = new TupleDictionary();
+
+	dict.set(1, 2, 1);
+	dict.set(3, 4, 1);
+
+	dict.reset();
+	test.equal(dict.keys.length, 0);
+
+    test.done();
+};
+
+
+exports.copy = function(test){
+	var dict1 = new TupleDictionary();
+	var dict2 = new TupleDictionary();
+
+	dict1.set(1, 2, 1);
+	dict1.set(2, 3, 2);
+	dict1.set(4, 3, 3);
+
+	dict2.copy(dict1);
+
+	test.deepEqual(dict1, dict2);
+
+    test.done();
+};
\ No newline at end of file

test/utils/Utils.js

+var Utils = require(__dirname + '/../../src/utils/Utils');
+
+exports.appendArray = function(test){
+    // STUB
+    test.done();
+};
+
+exports.splice = function(test){
+    // STUB
+    test.done();
+};
+
+exports.extend = function(test){
+    // STUB
+    test.done();
+};
+
+exports.defaults = function(test){
+    // STUB
+    test.done();
+};
+

test/world/IslandManager.js

+var World = require(__dirname + '/../../src/world/World')
+,   Body = require(__dirname + '/../../src/objects/Body')
+,   IslandManager = require(__dirname + '/../../src/world/IslandManager')
+,   IslandNode = require(__dirname + '/../../src/world/IslandNode')
+,   Equation = require(__dirname + '/../../src/equations/Equation')
+,   DistanceConstraint = require(__dirname + '/../../src/constraints/DistanceConstraint')
+
+exports.construct = function(test){
+    new IslandManager();
+    test.done();
+};
+
+exports.getUnvisitedNode = function(test){
+    var node = IslandManager.getUnvisitedNode([]);
+    test.equal(node,false);
+
+    node = IslandManager.getUnvisitedNode([new IslandNode(new Body())]);
+    test.ok(!node);
+
+    node = IslandManager.getUnvisitedNode([new IslandNode(new Body({ mass:1 }))]);
+    test.ok(node instanceof IslandNode);
+
+    test.done();
+};
+
+exports.visit = function(test){
+    var manager = new IslandManager();
+    manager.visit(new IslandNode(new Body()),[],[]);
+    test.done();
+};
+
+exports.bfs = function(test){
+    var manager = new IslandManager();
+    var bodyA = new Body({ mass:1 });
+    var bodyB = new Body({ mass:1 });
+    var nodeA = new IslandNode(bodyA);
+    var nodeB = new IslandNode(bodyB);
+
+    var bodies=[],
+        equations=[];
+    manager.bfs(nodeA,bodies,equations);
+    test.deepEqual(bodies,[bodyA]);
+
+    var eq = new Equation(bodyA,bodyB);
+    nodeA.neighbors.push(nodeB);
+    nodeA.equations.push(eq);
+    nodeB.neighbors.push(nodeA);
+    nodeB.equations.push(eq);
+    bodies = [];
+    equations = [];
+    manager.bfs(nodeA,bodies,equations);
+    test.deepEqual(bodies,[bodyA,bodyB]);
+    test.deepEqual(equations,[eq]);
+
+    test.done();
+};
+
+exports.split = function(test){
+    var world = new World();
+    var islands = world.islandManager.split(world);
+    test.equal(islands.length,0);
+
+    var bodyA = new Body({ mass:1 });
+    var bodyB = new Body({ mass:1 });
+    world.addBody(bodyA);
+    world.addBody(bodyB);
+    world.islandManager.equations.push(new Equation(bodyA,bodyB));
+    islands = world.islandManager.split(world);
+    test.equal(islands.length,1);
+
+    test.done();
+};
\ No newline at end of file

test/world/World.js

+var World = require(__dirname + '/../../src/world/World')
+,   Body = require(__dirname + '/../../src/objects/Body')
+,   Circle = require(__dirname + '/../../src/shapes/Circle')
+,   Plane = require(__dirname + '/../../src/shapes/Plane')
+,   Ray = require(__dirname + '/../../src/collision/Ray')
+,   RaycastResult = require(__dirname + '/../../src/collision/RaycastResult')
+,   Convex = require(__dirname + '/../../src/shapes/Convex');
+
+exports.construct = function(test){
+    var world = new World();
+
+    var options = {
+        gravity: [123,456]
+    };
+    world = new World(options);
+    test.equal(world.gravity[0], options.gravity[0]);
+    test.equal(world.gravity[1], options.gravity[1]);
+    test.done();
+};
+
+exports.raycast = function(test){
+    var world = new World();
+    var shape = new Plane();
+    var body = new Body();
+    body.addShape(shape, [0,0], Math.PI / 2);
+    world.addBody(body);
+    var result = new RaycastResult();
+    var ray = new Ray({
+        from: [0, -10],
+        to: [0, 10]
+    });
+    world.raycast(result, ray);
+    test.ok(result.hasHit());
+
+    var hitPointWorld = [1,1];
+    result.getHitPoint(hitPointWorld, ray);
+    test.equal(hitPointWorld[0], 0);
+    test.equal(hitPointWorld[1], 0);
+
+    test.equal(result.getHitDistance(ray), 10);
+
+    test.done();
+};
+
+exports.addBody = function(test){
+    // STUB
+    test.done();
+};
+
+exports.addConstraint = function(test){
+    // STUB
+    test.done();
+};
+
+exports.addContactMaterial = function(test){
+    // STUB
+    test.done();
+};
+
+exports.addSpring = function(test){
+    // STUB
+    test.done();
+};
+
+exports.clear = function(test){
+    // STUB
+    test.done();
+};
+
+exports.clone = function(test){
+    // STUB
+    test.done();
+};
+
+exports.disableBodyCollision = function(test){
+    var bodyA = new Body({ mass:1 }),
+        bodyB = new Body({ mass:1 }),
+        world = new World();
+    bodyA.addShape(new Circle({ radius: 1 }));
+    bodyB.addShape(new Circle({ radius: 1 }));
+    world.addBody(bodyA);
+    world.addBody(bodyB);
+    world.disableBodyCollision(bodyA,bodyB);
+    world.step(1/60);
+    test.equal(world.narrowphase.contactEquations.length,0);
+    world.enableBodyCollision(bodyA,bodyB);
+    world.step(1/60);
+    test.equal(world.narrowphase.contactEquations.length,1);
+    test.done();
+};
+
+exports.getBodyById = function(test){
+    // STUB
+    test.done();
+};
+
+exports.getContactMaterial = function(test){
+    // STUB
+    test.done();
+};
+
+exports.hitTest = function(test){
+    var b = new Body(),
+        world = new World();
+    world.addBody(b);
+    test.deepEqual(world.hitTest([0,0],[b]) , [], "Should miss bodies without shapes");
+
+    b.addShape(new Circle({ radius: 1 }));
+    test.deepEqual(world.hitTest([0,0],[b]) , [b], "Should hit Circle");
+    test.deepEqual(world.hitTest([1.1,0],[b]) , [], "Should miss Circle");
+
+    b = new Body();
+    b.addShape(new Convex({ vertices: [ [-1,-1],
+                            [ 1,-1],
+                            [ 1, 1],
+                            [-1, 1]] }));
+    test.deepEqual(world.hitTest([0,0],  [b]) , [b],  "Should hit Convex");
+    test.deepEqual(world.hitTest([1.1,0],[b]) , [], "Should miss Convex");
+    test.done();
+};
+
+exports.integrateBody = function(test){
+    // STUB
+    test.done();
+};
+
+exports.removeBody = function(test){
+    // STUB
+    test.done();
+};
+
+exports.removeConstraint = function(test){
+    // STUB
+    test.done();
+};
+
+exports.removeContactMaterial = function(test){
+    // STUB
+    test.done();
+};
+
+exports.removeSpring = function(test){
+    // STUB
+    test.done();
+};
+
+exports.runNarrowphase = function(test){
+    // STUB
+    test.done();
+};
+
+exports.setGlobalStiffness = function(test){
+    var world = new World();
+    world.setGlobalStiffness(123);
+    test.equal(world.defaultContactMaterial.stiffness, 123);
+    test.done();
+};
+
+exports.setGlobalRelaxation = function(test){
+    var world = new World();
+    world.setGlobalRelaxation(123);
+    test.equal(world.defaultContactMaterial.relaxation, 123);
+    test.done();
+};
+
+exports.step = function(test){
+    // STUB
+    test.done();
+};
+
+exports.events = {
+    beginContact : function(test){
+        var world = new World(),
+            bodyA = new Body({ mass:1 }),
+            bodyB = new Body({ mass:1 });
+        world.addBody(bodyA);
+        world.addBody(bodyB);
+        var shapeA = new Circle({ radius: 1 }),
+            shapeB = new Circle({ radius: 1 });
+        bodyA.addShape(shapeA);
+        bodyB.addShape(shapeB);
+        var beginContactHits = 0,
+            endContactHits = 0;
+        world.on("beginContact",function(evt){
+            test.ok( evt.shapeA.id === shapeA.id || evt.shapeA.id === shapeB.id );
+            test.ok( evt.shapeB.id === shapeA.id || evt.shapeB.id === shapeB.id );
+            test.ok( evt.bodyA.id === bodyA.id || evt.bodyA.id === bodyB.id );
+            test.ok( evt.bodyB.id === bodyA.id || evt.bodyB.id === bodyB.id );
+            beginContactHits++;
+        });
+        world.on("endContact",function(evt){
+            test.ok( evt.shapeA.id === shapeA.id || evt.shapeA.id === shapeB.id );
+            test.ok( evt.shapeB.id === shapeA.id || evt.shapeB.id === shapeB.id );
+            test.ok( evt.bodyA.id === bodyA.id || evt.bodyA.id === bodyB.id );
+            test.ok( evt.bodyB.id === bodyA.id || evt.bodyB.id === bodyB.id );
+            endContactHits++;
+        });
+
+        // First overlap - one new beginContact
+        world.step(1/60);
+        test.equal(beginContactHits, 1);
+        test.equal(endContactHits, 0);
+
+        // Still overlapping - should maintain
+        world.step(1/60);
+        test.equal(beginContactHits, 1);
+        test.equal(endContactHits, 0);
+
+        // End the overlap
+        bodyA.position[0] = 10;
+        world.step(1/60);
+        test.equal(beginContactHits, 1);
+        test.equal(endContactHits,1);
+
+        test.done();
+    },
+
+
+    beginContact2 : function(test){
+        var world = new World(),
+            // 3 circles, A overlaps B which overlaps C
+            bodyA = new Body({ mass:1, position:[-1.1,0] }),
+            bodyB = new Body({ mass:1, position:[0,0] }),
+            bodyC = new Body({ mass:1, position:[1.1,0] });
+        world.addBody(bodyA);
+        world.addBody(bodyB);
+        world.addBody(bodyC);
+        var shapeA = new Circle({ radius: 1 }),
+            shapeB = new Circle({ radius: 1 }),
+            shapeC = new Circle({ radius: 1 });
+        bodyA.addShape(shapeA);
+        bodyB.addShape(shapeB);
+        bodyC.addShape(shapeC);
+
+        var beginContactHits = 0,
+            endContactHits = 0;
+
+        world.on("beginContact",function(evt){
+            beginContactHits++;
+        });
+
+        world.on("endContact",function(evt){
+            endContactHits++;
+        });
+
+        // First overlap - two new beginContact
+        world.step(1/60);
+        test.equal(beginContactHits, 2);
+        test.equal(endContactHits, 0);
+
+        // Still overlapping - should not report anything
+        world.step(1/60);
+        test.equal(beginContactHits, 2);
+        test.equal(endContactHits, 0);
+
+        // End one overlap
+        bodyA.position[1] = 10;
+        world.step(1/60);
+        test.equal(beginContactHits, 2);
+        test.equal(endContactHits,1);
+
+        // End another overlap
+        bodyB.position[1] = -10;
+        world.step(1/60);
+        test.equal(beginContactHits, 2);
+        test.equal(endContactHits,2);
+
+        test.done();
+    },
+};

yuidoc.json

+{
+    "name": "p2.js",
+    "options": {
+        "outdir" : "docs",
+        "paths" : ["./src/"],
+        "exclude": ".DS_Store,.svn,CVS,.git,build_rollup_tmp,build_tmp,gl-matrix"
+    }
+}