#

API Docs for: 0.6.0
Show: Inherited Protected Private Deprecated

# File: src/world/Narrowphase.js

    module.exports = Narrowphase;

    var Shape = require('../shapes/Shape');
    var Vec3 = require('../math/Vec3');
    var Transform = require('../math/Transform');
    var ConvexPolyhedron = require('../shapes/ConvexPolyhedron');
    var Quaternion = require('../math/Quaternion');
    var Solver = require('../solver/Solver');
    var Vec3Pool = require('../utils/Vec3Pool');
    var ContactEquation = require('../equations/ContactEquation');

    /**
     * Helper class for the World. Generates ContactEquations.
     * @class Narrowphase
     * @constructor
     * @todo Sphere-ConvexPolyhedron contacts
     * @todo Contact reduction
     * @todo  should move methods to prototype
     */
    function Narrowphase(){

        /**
         * Turns on or off contact reduction. Can be handy to turn off when debugging new collision types.
         * @property bool contactReduction
         */
        this.contactReduction = false;

        /**
         * Internal storage of pooled contact points.
         * @property {Array} contactPointPool
         */
        this.contactPointPool = [];

        /**
         * Pooled vectors.
         * @property {Vec3Pool} v3pool
         */
        this.v3pool = new Vec3Pool();
    }

    /**
     * Swaps the body references in the contact
     * @method swapResult
     * @param object r
     */
    Narrowphase.prototype.swapResult = function(r){
        var temp;
        temp = r.ri;
        r.ri = r.rj;
        r.rj = temp;
        r.ni.negate(r.ni);
        temp = r.bi;
        r.bi = r.bj;
        r.bj = temp;
    };

    /**
     * Removes unnecessary members of an array of ContactEquation.
     * @method reduceContacts
     * @param {Array} contacts
     */
    Narrowphase.prototype.reduceContacts = function(contacts){

    };

    /**
     * Make a contact object, by using the internal pool or creating a new one.
     * @method makeResult
     * @return {ContactEquation}
     */
    Narrowphase.prototype.makeResult = function(bi, bj, si, sj, rsi, rsj){
        var c;
        if(this.contactPointPool.length){
            c = this.contactPointPool.pop();
            c.bi = bi;
            c.bj = bj;
        } else {
            c = new ContactEquation(bi, bj);
        }

        c.enabled = true;
        c.si = rsi || si;
        c.sj = rsj || sj;

        return c;
    };

    var tmpVec1 = new Vec3();
    var tmpVec2 = new Vec3();
    var tmpQuat1 = new Quaternion();
    var tmpQuat2 = new Quaternion();

    /**
     * Generate all contacts between a list of body pairs
     * @method getContacts
     * @param {array} p1 Array of body indices
     * @param {array} p2 Array of body indices
     * @param {World} world
     * @param {array} result Array to store generated contacts
     * @param {array} oldcontacts Optional. Array of reusable contact objects
     */
    Narrowphase.prototype.getContacts = function(p1,p2,world,result,oldcontacts){
        // Save old contact objects
        this.contactPointPool = oldcontacts;

        var qi = tmpQuat1;
        var qj = tmpQuat2;
        var xi = tmpVec1;
        var xj = tmpVec2;

        for(var k=0, N=p1.length; k!==N; k++){

            // Get current collision bodies
            var bi = p1[k],
                bj = p2[k];

            for (var i = 0; i < bi.shapes.length; i++) {
                bi.quaternion.mult(bi.shapeOrientations[i], qi);
                bi.quaternion.vmult(bi.shapeOffsets[i], xi);
                xi.vadd(bi.position, xi);
                var si = bi.shapes[i];

                for (var j = 0; j < bj.shapes.length; j++) {

                    // Compute world transform of shapes
                    bj.quaternion.mult(bj.shapeOrientations[j], qj);
                    bj.quaternion.vmult(bj.shapeOffsets[j], xj);
                    xj.vadd(bj.position, xj);
                    var sj = bj.shapes[j];

                    // Get contacts
                    var resolver = this[si.type | sj.type];
                    if(resolver){
                        if (si.type < sj.type) {
                            resolver.call(this, result, si,sj,xi,xj,qi,qj,bi,bj,si,sj);
                        } else {
                            resolver.call(this, result, sj,si,xj,xi,qj,qi,bj,bi,si,sj);
                        }
                    }
                    /*
                    this.narrowphase(
                        result,
                        bi.shapes[i],
                        bj.shapes[j],
                        xi, xj,
                        qi, qj,
                        bi, bj
                    );
                    */
                }
            }
        }
    };

    var numWarnings = 0;
    var maxWarnings = 10;

    function warn(msg){
        if(numWarnings > maxWarnings){
            return;
        }

        numWarnings++;

        console.warn(msg);
    }

    /**
     * Narrowphase calculation. Get the ContactEquations given two shapes: i and j
     * @method narrowphase
     * @param {array} result The result one will get back with all the contact point information
     * @param {Shape} si Colliding shape. If not given, particle is assumed.
     * @param {Shape} sj
     * @param {Vec3} xi Position of the center of mass
     * @param {Vec3} xj
     * @param {Quaternion} qi Rotation around the center of mass
     * @param {Quaternion} qj
     */
    /*
    Narrowphase.prototype.narrowphase = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        var swapped = false,
            types = Shape.types,
            SPHERE = types.SPHERE,
            PLANE = types.PLANE,
            PARTICLE = types.PARTICLE,
            BOX = types.BOX,
            COMPOUND = types.COMPOUND,
            CONVEXPOLYHEDRON = types.CONVEXPOLYHEDRON,
            HEIGHTFIELD = types.HEIGHTFIELD;

        if(si.type > sj.type){
            var temp;

            temp = sj;
            sj = si;
            si = temp;

            temp = xj;
            xj = xi;
            xi = temp;

            temp = qj;
            qj = qi;
            qi = temp;

            temp = bj;
            bj = bi;
            bi = temp;

            swapped = true;
        }

        if(si && sj){
            if(si.type === SPHERE){

                switch(sj.type){
                case SPHERE: // sphere-sphere
                    this.sphereSphere(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case PLANE: // sphere-plane
                    this.spherePlane(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case BOX: // sphere-box
                    this.sphereBox(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case CONVEXPOLYHEDRON: // sphere-convexpolyhedron
                    this.sphereConvex(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case HEIGHTFIELD: // sphere-heightfield
                    this.sphereHeightfield(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case PARTICLE: // Particle vs sphere
                    this.particleSphere(result,sj,si,xj,xi,qj,qi,bj,bi);
                    break;
                default:
                    warn("Collision between Shape.types.SPHERE and "+sj.type+" not implemented yet.");
                    break;
                }

            } else if(si.type === types.PLANE){

                switch(sj.type){
                case PLANE: // plane-plane
                    // Should not give collision
                    break;
                case BOX: // plane-box
                    this.planeBox(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case CONVEXPOLYHEDRON: // plane-convex polyhedron
                    this.planeConvex(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case PARTICLE: // plane-convex polyhedron
                    this.particlePlane(result,sj,si,xj,xi,qj,qi,bj,bi);
                    break;
                default:
                    warn("Collision between Shape.types.PLANE and "+sj.type+" not implemented yet.");
                    break;
                }

            } else if(si.type===types.BOX){

                switch(sj.type){
                case BOX: // box-box
                    // Do convex/convex instead
                    this.convexConvex(result,si.convexPolyhedronRepresentation,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj,si,sj);
                    break;
                case CONVEXPOLYHEDRON: // box-convexpolyhedron
                    // Do convex/convex instead
                    this.convexConvex(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
                    break;
                case PARTICLE: // Particle vs box
                    this.convexParticle(result,sj,si.convexPolyhedronRepresentation,xj,xi,qj,qi,bj,bi,si,sj);
                    break;
                case HEIGHTFIELD: // Box vs heightfield
                    this.convexHeightfield(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
                    break;
                default:
                    warn("Collision between Shape.BOX and "+sj.type+" not implemented yet.");
                    break;
                }

            } else if(si.type===types.CONVEXPOLYHEDRON){

                switch(sj.type){
                case types.CONVEXPOLYHEDRON: // convex polyhedron - convex polyhedron
                    this.convexConvex(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case types.PARTICLE: // particle-convex
                    this.convexParticle(result,sj,si,xj,xi,qj,qi,bj,bi);
                    break;
                default:
                    warn("Collision between Shape.types.CONVEXPOLYHEDRON and "+sj.type+" not implemented yet.");
                    break;
                }

            } else if(si.type===types.HEIGHTFIELD){

                switch(sj.type){
                case types.SPHERE: // heightfield/sphere
                    this.sphereHeightfield(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                default:
                    warn("Collision between Shape.types.HEIGHTFIELD and "+sj.type+" not implemented yet.");
                    break;
                }

            } else if(si.type === PARTICLE){

                // Particle!
                switch(sj.type){
                case types.PLANE: // Particle vs plane
                    this.particlePlane(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case types.SPHERE: // Particle vs sphere
                    this.particleSphere(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                case types.BOX: // Particle vs box
                    this.convexParticle(result,si,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj);
                    break;
                case types.CONVEXPOLYHEDRON: // particle-convex
                    this.convexParticle(result,si,sj,xi,xj,qi,qj,bi,bj);
                    break;
                default:
                    console.warn("Collision between Particle and "+sj.type+" not implemented yet.");
                    break;
                }
            }
        }

        // Swap back if we swapped bodies in the beginning
        for(var i=0, Nresults=result.length; swapped && i!==Nresults; i++){
            this.swapResult(result[i]);
        }
    };
    */

    Narrowphase.prototype[Shape.types.BOX | Shape.types.BOX] =
    Narrowphase.prototype.boxBox = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        this.convexConvex(result,si.convexPolyhedronRepresentation,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj,si,sj);
    };

    Narrowphase.prototype[Shape.types.BOX | Shape.types.CONVEXPOLYHEDRON] =
    Narrowphase.prototype.boxConvex = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        this.convexConvex(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
    };

    Narrowphase.prototype[Shape.types.BOX | Shape.types.PARTICLE] =
    Narrowphase.prototype.boxParticle = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        this.convexParticle(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
    };

    /**
     * @method sphereSphere
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.SPHERE] =
    Narrowphase.prototype.sphereSphere = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        // We will have only one contact in this case
        var r = this.makeResult(bi,bj,si,sj);

        // Contact normal
        bj.position.vsub(xi, r.ni);
        r.ni.normalize();

        // Contact point locations
        r.ri.copy(r.ni);
        r.rj.copy(r.ni);
        r.ri.mult(si.radius, r.ri);
        r.rj.mult(-sj.radius, r.rj);
        result.push(r);
    };

    var point_on_plane_to_sphere = new Vec3();
    var plane_to_sphere_ortho = new Vec3();

    /**
     * @method spherePlane
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.SPHERE | Shape.types.PLANE] =
    Narrowphase.prototype.spherePlane = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        // We will have one contact in this case
        var r = this.makeResult(bi,bj,si,sj);

        // Contact normal
        r.ni.set(0,0,1);
        qj.vmult(r.ni, r.ni);
        r.ni.negate(r.ni); // body i is the sphere, flip normal
        r.ni.normalize(); // Needed?

        // Vector from sphere center to contact point
        r.ni.mult(si.radius, r.ri);

        // Project down sphere on plane
        xi.vsub(xj, point_on_plane_to_sphere);
        r.ni.mult(r.ni.dot(point_on_plane_to_sphere), plane_to_sphere_ortho);
        point_on_plane_to_sphere.vsub(plane_to_sphere_ortho,r.rj); // The sphere position projected to plane

        if(plane_to_sphere_ortho.norm2() <= si.radius * si.radius){
            result.push(r);

            // Make it relative to the body
            var ri = r.ri;
            var rj = r.rj;
            ri.vadd(xi, ri);
            ri.vsub(bi.position, ri);
            rj.vadd(xj, rj);
            rj.vsub(bj.position, rj);
        }
    };

    // See http://bulletphysics.com/Bullet/BulletFull/SphereTriangleDetector_8cpp_source.html
    var pointInPolygon_edge = new Vec3();
    var pointInPolygon_edge_x_normal = new Vec3();
    var pointInPolygon_vtp = new Vec3();
    function pointInPolygon(verts, normal, p){
        var positiveResult = null;
        var N = verts.length;
        for(var i=0; i!==N; i++){
            var v = verts[i];

            // Get edge to the next vertex
            var edge = pointInPolygon_edge;
            verts[(i+1) % (N)].vsub(v,edge);

            // Get cross product between polygon normal and the edge
            var edge_x_normal = pointInPolygon_edge_x_normal;
            //var edge_x_normal = new Vec3();
            edge.cross(normal,edge_x_normal);

            // Get vector between point and current vertex
            var vertex_to_p = pointInPolygon_vtp;
            p.vsub(v,vertex_to_p);

            // This dot product determines which side of the edge the point is
            var r = edge_x_normal.dot(vertex_to_p);

            // If all such dot products have same sign, we are inside the polygon.
            if(positiveResult===null || (r>0 && positiveResult===true) || (r<=0 && positiveResult===false)){
                if(positiveResult===null){
                    positiveResult = r>0;
                }
                continue;
            } else {
                return false; // Encountered some other sign. Exit.
            }
        }

        // If we got here, all dot products were of the same sign.
        return true;
    }

    var box_to_sphere = new Vec3();
    var sphereBox_ns = new Vec3();
    var sphereBox_ns1 = new Vec3();
    var sphereBox_ns2 = new Vec3();
    var sphereBox_sides = [new Vec3(),new Vec3(),new Vec3(),new Vec3(),new Vec3(),new Vec3()];
    var sphereBox_sphere_to_corner = new Vec3();
    var sphereBox_side_ns = new Vec3();
    var sphereBox_side_ns1 = new Vec3();
    var sphereBox_side_ns2 = new Vec3();

    /**
     * @method sphereBox
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.SPHERE | Shape.types.BOX] =
    Narrowphase.prototype.sphereBox = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        var v3pool = this.v3pool;

        // we refer to the box as body j
        var sides = sphereBox_sides;
        xi.vsub(xj,box_to_sphere);
        sj.getSideNormals(sides,qj);
        var R =     si.radius;
        var penetrating_sides = [];

        // Check side (plane) intersections
        var found = false;

        // Store the resulting side penetration info
        var side_ns = sphereBox_side_ns;
        var side_ns1 = sphereBox_side_ns1;
        var side_ns2 = sphereBox_side_ns2;
        var side_h = null;
        var side_penetrations = 0;
        var side_dot1 = 0;
        var side_dot2 = 0;
        var side_distance = null;
        for(var idx=0,nsides=sides.length; idx!==nsides && found===false; idx++){
            // Get the plane side normal (ns)
            var ns = sphereBox_ns;
            ns.copy(sides[idx]);

            var h = ns.norm();
            ns.normalize();

            // The normal/distance dot product tells which side of the plane we are
            var dot = box_to_sphere.dot(ns);

            if(dot<h+R && dot>0){
                // Intersects plane. Now check the other two dimensions
                var ns1 = sphereBox_ns1;
                var ns2 = sphereBox_ns2;
                ns1.copy(sides[(idx+1)%3]);
                ns2.copy(sides[(idx+2)%3]);
                var h1 = ns1.norm();
                var h2 = ns2.norm();
                ns1.normalize();
                ns2.normalize();
                var dot1 = box_to_sphere.dot(ns1);
                var dot2 = box_to_sphere.dot(ns2);
                if(dot1<h1 && dot1>-h1 && dot2<h2 && dot2>-h2){
                    var dist = Math.abs(dot-h-R);
                    if(side_distance===null || dist < side_distance){
                        side_distance = dist;
                        side_dot1 = dot1;
                        side_dot2 = dot2;
                        side_h = h;
                        side_ns.copy(ns);
                        side_ns1.copy(ns1);
                        side_ns2.copy(ns2);
                        side_penetrations++;
                    }
                }
            }
        }
        if(side_penetrations){
            found = true;
            var r = this.makeResult(bi,bj,si,sj);
            side_ns.mult(-R,r.ri); // Sphere r
            r.ni.copy(side_ns);
            r.ni.negate(r.ni); // Normal should be out of sphere
            side_ns.mult(side_h,side_ns);
            side_ns1.mult(side_dot1,side_ns1);
            side_ns.vadd(side_ns1,side_ns);
            side_ns2.mult(side_dot2,side_ns2);
            side_ns.vadd(side_ns2,r.rj);

            // Make relative to bodies
            r.ri.vadd(xi, r.ri);
            r.ri.vsub(bi.position, r.ri);
            r.rj.vadd(xj, r.rj);
            r.rj.vsub(bj.position, r.rj);

            result.push(r);
        }

        // Check corners
        var rj = v3pool.get();
        var sphere_to_corner = sphereBox_sphere_to_corner;
        for(var j=0; j!==2 && !found; j++){
            for(var k=0; k!==2 && !found; k++){
                for(var l=0; l!==2 && !found; l++){
                    rj.set(0,0,0);
                    if(j){
                        rj.vadd(sides[0],rj);
                    } else {
                        rj.vsub(sides[0],rj);
                    }
                    if(k){
                        rj.vadd(sides[1],rj);
                    } else {
                        rj.vsub(sides[1],rj);
                    }
                    if(l){
                        rj.vadd(sides[2],rj);
                    } else {
                        rj.vsub(sides[2],rj);
                    }

                    // World position of corner
                    xj.vadd(rj,sphere_to_corner);
                    sphere_to_corner.vsub(xi,sphere_to_corner);

                    if(sphere_to_corner.norm2() < R*R){
                        found = true;
                        var r = this.makeResult(bi,bj,si,sj);
                        r.ri.copy(sphere_to_corner);
                        r.ri.normalize();
                        r.ni.copy(r.ri);
                        r.ri.mult(R,r.ri);
                        r.rj.copy(rj);

                        // Make relative to bodies
                        r.ri.vadd(xi, r.ri);
                        r.ri.vsub(bi.position, r.ri);
                        r.rj.vadd(xj, r.rj);
                        r.rj.vsub(bj.position, r.rj);

                        result.push(r);
                    }
                }
            }
        }
        v3pool.release(rj);
        rj = null;

        // Check edges
        var edgeTangent = v3pool.get();
        var edgeCenter = v3pool.get();
        var r = v3pool.get(); // r = edge center to sphere center
        var orthogonal = v3pool.get();
        var dist = v3pool.get();
        var Nsides = sides.length;
        for(var j=0; j!==Nsides && !found; j++){
            for(var k=0; k!==Nsides && !found; k++){
                if(j%3 !== k%3){
                    // Get edge tangent
                    sides[k].cross(sides[j],edgeTangent);
                    edgeTangent.normalize();
                    sides[j].vadd(sides[k], edgeCenter);
                    r.copy(xi);
                    r.vsub(edgeCenter,r);
                    r.vsub(xj,r);
                    var orthonorm = r.dot(edgeTangent); // distance from edge center to sphere center in the tangent direction
                    edgeTangent.mult(orthonorm,orthogonal); // Vector from edge center to sphere center in the tangent direction

                    // Find the third side orthogonal to this one
                    var l = 0;
                    while(l===j%3 || l===k%3){
                        l++;
                    }

                    // vec from edge center to sphere projected to the plane orthogonal to the edge tangent
                    dist.copy(xi);
                    dist.vsub(orthogonal,dist);
                    dist.vsub(edgeCenter,dist);
                    dist.vsub(xj,dist);

                    // Distances in tangent direction and distance in the plane orthogonal to it
                    var tdist = Math.abs(orthonorm);
                    var ndist = dist.norm();

                    if(tdist < sides[l].norm() && ndist<R){
                        found = true;
                        var res = this.makeResult(bi,bj,si,sj);
                        edgeCenter.vadd(orthogonal,res.rj); // box rj
                        res.rj.copy(res.rj);
                        dist.negate(res.ni);
                        res.ni.normalize();

                        res.ri.copy(res.rj);
                        res.ri.vadd(xj,res.ri);
                        res.ri.vsub(xi,res.ri);
                        res.ri.normalize();
                        res.ri.mult(R,res.ri);

                        // Make relative to bodies
                        res.ri.vadd(xi, res.ri);
                        res.ri.vsub(bi.position, res.ri);
                        res.rj.vadd(xj, res.rj);
                        res.rj.vsub(bj.position, res.rj);

                        result.push(res);
                    }
                }
            }
        }
        v3pool.release(edgeTangent,edgeCenter,r,orthogonal,dist);
    };

    var convex_to_sphere = new Vec3();
    var sphereConvex_edge = new Vec3();
    var sphereConvex_edgeUnit = new Vec3();
    var sphereConvex_sphereToCorner = new Vec3();
    var sphereConvex_worldCorner = new Vec3();
    var sphereConvex_worldNormal = new Vec3();
    var sphereConvex_worldPoint = new Vec3();
    var sphereConvex_worldSpherePointClosestToPlane = new Vec3();
    var sphereConvex_penetrationVec = new Vec3();
    var sphereConvex_sphereToWorldPoint = new Vec3();

    /**
     * @method sphereConvex
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.SPHERE | Shape.types.CONVEXPOLYHEDRON] =
    Narrowphase.prototype.sphereConvex = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        var v3pool = this.v3pool;
        xi.vsub(xj,convex_to_sphere);
        var normals = sj.faceNormals;
        var faces = sj.faces;
        var verts = sj.vertices;
        var R =     si.radius;
        var penetrating_sides = [];

        // Check corners
        for(var i=0; i!==verts.length; i++){
            var v = verts[i];

            // World position of corner
            var worldCorner = sphereConvex_worldCorner;
            qj.vmult(v,worldCorner);
            xj.vadd(worldCorner,worldCorner);
            var sphere_to_corner = sphereConvex_sphereToCorner;
            worldCorner.vsub(xi, sphere_to_corner);
            if(sphere_to_corner.norm2() < R * R){
                found = true;
                var r = this.makeResult(bi,bj,si,sj);
                r.ri.copy(sphere_to_corner);
                r.ri.normalize();
                r.ni.copy(r.ri);
                r.ri.mult(R,r.ri);
                worldCorner.vsub(xj,r.rj);

                // Should be relative to the body.
                r.ri.vadd(xi, r.ri);
                r.ri.vsub(bi.position, r.ri);

                // Should be relative to the body.
                r.rj.vadd(xj, r.rj);
                r.rj.vsub(bj.position, r.rj);

                result.push(r);
                return;
            }
        }

        // Check side (plane) intersections
        var found = false;
        for(var i=0, nfaces=faces.length; i!==nfaces && found===false; i++){
            var normal = normals[i];
            var face = faces[i];

            // Get world-transformed normal of the face
            var worldNormal = sphereConvex_worldNormal;
            qj.vmult(normal,worldNormal);

            // Get a world vertex from the face
            var worldPoint = sphereConvex_worldPoint;
            qj.vmult(verts[face[0]],worldPoint);
            worldPoint.vadd(xj,worldPoint);

            // Get a point on the sphere, closest to the face normal
            var worldSpherePointClosestToPlane = sphereConvex_worldSpherePointClosestToPlane;
            worldNormal.mult(-R, worldSpherePointClosestToPlane);
            xi.vadd(worldSpherePointClosestToPlane, worldSpherePointClosestToPlane);

            // Vector from a face point to the closest point on the sphere
            var penetrationVec = sphereConvex_penetrationVec;
            worldSpherePointClosestToPlane.vsub(worldPoint,penetrationVec);

            // The penetration. Negative value means overlap.
            var penetration = penetrationVec.dot(worldNormal);

            var worldPointToSphere = sphereConvex_sphereToWorldPoint;
            xi.vsub(worldPoint, worldPointToSphere);

            if(penetration < 0 && worldPointToSphere.dot(worldNormal)>0){
                // Intersects plane. Now check if the sphere is inside the face polygon
                var faceVerts = []; // Face vertices, in world coords
                for(var j=0, Nverts=face.length; j!==Nverts; j++){
                    var worldVertex = v3pool.get();
                    qj.vmult(verts[face[j]], worldVertex);
                    xj.vadd(worldVertex,worldVertex);
                    faceVerts.push(worldVertex);
                }

                if(pointInPolygon(faceVerts,worldNormal,xi)){ // Is the sphere center in the face polygon?
                    found = true;
                    var r = this.makeResult(bi,bj,si,sj);

                    worldNormal.mult(-R, r.ri); // Contact offset, from sphere center to contact
                    worldNormal.negate(r.ni); // Normal pointing out of sphere

                    var penetrationVec2 = v3pool.get();
                    worldNormal.mult(-penetration, penetrationVec2);
                    var penetrationSpherePoint = v3pool.get();
                    worldNormal.mult(-R, penetrationSpherePoint);

                    //xi.vsub(xj).vadd(penetrationSpherePoint).vadd(penetrationVec2 , r.rj);
                    xi.vsub(xj,r.rj);
                    r.rj.vadd(penetrationSpherePoint,r.rj);
                    r.rj.vadd(penetrationVec2 , r.rj);

                    // Should be relative to the body.
                    r.rj.vadd(xj, r.rj);
                    r.rj.vsub(bj.position, r.rj);

                    // Should be relative to the body.
                    r.ri.vadd(xi, r.ri);
                    r.ri.vsub(bi.position, r.ri);

                    v3pool.release(penetrationVec2);
                    v3pool.release(penetrationSpherePoint);

                    result.push(r);

                    // Release world vertices
                    for(var j=0, Nfaceverts=faceVerts.length; j!==Nfaceverts; j++){
                        v3pool.release(faceVerts[j]);
                    }

                    return; // We only expect *one* face contact
                } else {
                    // Edge?
                    for(var j=0; j!==face.length; j++){

                        // Get two world transformed vertices
                        var v1 = v3pool.get();
                        var v2 = v3pool.get();
                        qj.vmult(verts[face[(j+1)%face.length]], v1);
                        qj.vmult(verts[face[(j+2)%face.length]], v2);
                        xj.vadd(v1, v1);
                        xj.vadd(v2, v2);

                        // Construct edge vector
                        var edge = sphereConvex_edge;
                        v2.vsub(v1,edge);

                        // Construct the same vector, but normalized
                        var edgeUnit = sphereConvex_edgeUnit;
                        edge.unit(edgeUnit);

                        // p is xi projected onto the edge
                        var p = v3pool.get();
                        var v1_to_xi = v3pool.get();
                        xi.vsub(v1, v1_to_xi);
                        var dot = v1_to_xi.dot(edgeUnit);
                        edgeUnit.mult(dot, p);
                        p.vadd(v1, p);

                        // Compute a vector from p to the center of the sphere
                        var xi_to_p = v3pool.get();
                        p.vsub(xi, xi_to_p);

                        // Collision if the edge-sphere distance is less than the radius
                        // AND if p is in between v1 and v2
                        if(dot > 0 && dot*dot<edge.norm2() && xi_to_p.norm2() < R*R){ // Collision if the edge-sphere distance is less than the radius
                            // Edge contact!
                            var r = this.makeResult(bi,bj,si,sj);
                            p.vsub(xj,r.rj);

                            p.vsub(xi,r.ni);
                            r.ni.normalize();

                            r.ni.mult(R,r.ri);

                            // Should be relative to the body.
                            r.rj.vadd(xj, r.rj);
                            r.rj.vsub(bj.position, r.rj);

                            // Should be relative to the body.
                            r.ri.vadd(xi, r.ri);
                            r.ri.vsub(bi.position, r.ri);

                            result.push(r);

                            // Release world vertices
                            for(var j=0, Nfaceverts=faceVerts.length; j!==Nfaceverts; j++){
                                v3pool.release(faceVerts[j]);
                            }

                            v3pool.release(v1);
                            v3pool.release(v2);
                            v3pool.release(p);
                            v3pool.release(xi_to_p);
                            v3pool.release(v1_to_xi);

                            return;
                        }

                        v3pool.release(v1);
                        v3pool.release(v2);
                        v3pool.release(p);
                        v3pool.release(xi_to_p);
                        v3pool.release(v1_to_xi);
                    }
                }

                // Release world vertices
                for(var j=0, Nfaceverts=faceVerts.length; j!==Nfaceverts; j++){
                    v3pool.release(faceVerts[j]);
                }
            }
        }
    };

    var planeBox_normal = new Vec3();
    var plane_to_corner = new Vec3();

    /**
     * @method planeBox
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.PLANE | Shape.types.BOX] =
    Narrowphase.prototype.planeBox = function(result,si,sj,xi,xj,qi,qj,bi,bj){
        this.planeConvex(result,si,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj);
    };

    var planeConvex_v = new Vec3();
    var planeConvex_normal = new Vec3();
    var planeConvex_relpos = new Vec3();
    var planeConvex_projected = new Vec3();

    /**
     * @method planeConvex
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.PLANE | Shape.types.CONVEXPOLYHEDRON] =
    Narrowphase.prototype.planeConvex = function(
        result,
        planeShape,
        convexShape,
        planePosition,
        convexPosition,
        planeQuat,
        convexQuat,
        planeBody,
        convexBody
    ){
        // Simply return the points behind the plane.
        var worldVertex = planeConvex_v,
            worldNormal = planeConvex_normal;
        worldNormal.set(0,0,1);
        planeQuat.vmult(worldNormal,worldNormal); // Turn normal according to plane orientation

        var relpos = planeConvex_relpos;
        for(var i = 0; i !== convexShape.vertices.length; i++){

            // Get world convex vertex
            worldVertex.copy(convexShape.vertices[i]);
            convexQuat.vmult(worldVertex, worldVertex);
            convexPosition.vadd(worldVertex, worldVertex);
            worldVertex.vsub(planePosition, relpos);

            var dot = worldNormal.dot(relpos);
            if(dot <= 0.0){

                var r = this.makeResult(planeBody, convexBody, planeShape, convexShape);

                // Get vertex position projected on plane
                var projected = planeConvex_projected;
                worldNormal.mult(worldNormal.dot(relpos),projected);
                worldVertex.vsub(projected, projected);
                projected.vsub(planePosition, r.ri); // From plane to vertex projected on plane

                r.ni.copy(worldNormal); // Contact normal is the plane normal out from plane

                // rj is now just the vector from the convex center to the vertex
                worldVertex.vsub(convexPosition, r.rj);

                // Make it relative to the body
                r.ri.vadd(planePosition, r.ri);
                r.ri.vsub(planeBody.position, r.ri);
                r.rj.vadd(convexPosition, r.rj);
                r.rj.vsub(convexBody.position, r.rj);

                result.push(r);
            }
        }
    };

    var convexConvex_sepAxis = new Vec3();
    var convexConvex_q = new Vec3();

    /**
     * @method convexConvex
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.CONVEXPOLYHEDRON] =
    Narrowphase.prototype.convexConvex = function(result,si,sj,xi,xj,qi,qj,bi,bj,rsi,rsj){
        var sepAxis = convexConvex_sepAxis;
        if(si.findSeparatingAxis(sj,xi,qi,xj,qj,sepAxis)){
            var res = [];
            var q = convexConvex_q;
            si.clipAgainstHull(xi,qi,sj,xj,qj,sepAxis,-100,100,res);
            for(var j = 0; j !== res.length; j++){
                var r = this.makeResult(bi,bj,si,sj,rsi,rsj),
                    ri = r.ri,
                    rj = r.rj;
                sepAxis.negate(r.ni);
                res[j].normal.negate(q);
                q.mult(res[j].depth, q);
                res[j].point.vadd(q, ri);
                rj.copy(res[j].point);

                // Contact points are in world coordinates. Transform back to relative
                ri.vsub(xi,ri);
                rj.vsub(xj,rj);

                // Make relative to bodies
                ri.vadd(xi, ri);
                ri.vsub(bi.position, ri);
                rj.vadd(xj, rj);
                rj.vsub(bj.position, rj);

                result.push(r);
            }
        }
    };

    var particlePlane_normal = new Vec3();
    var particlePlane_relpos = new Vec3();
    var particlePlane_projected = new Vec3();

    /**
     * @method particlePlane
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.PLANE | Shape.types.PARTICLE] =
    Narrowphase.prototype.planeParticle = function(result,sj,si,xj,xi,qj,qi,bj,bi){
        var normal = particlePlane_normal;
        normal.set(0,0,1);
        bj.quaternion.vmult(normal,normal); // Turn normal according to plane orientation
        var relpos = particlePlane_relpos;
        xi.vsub(bj.position,relpos);
        var dot = normal.dot(relpos);
        if(dot <= 0.0){
            var r = this.makeResult(bi,bj,si,sj);
            r.ni.copy(normal); // Contact normal is the plane normal
            r.ni.negate(r.ni);
            r.ri.set(0,0,0); // Center of particle

            // Get particle position projected on plane
            var projected = particlePlane_projected;
            normal.mult(normal.dot(xi),projected);
            xi.vsub(projected,projected);
            //projected.vadd(bj.position,projected);

            // rj is now the projected world position minus plane position
            r.rj.copy(projected);
            result.push(r);
        }
    };

    var particleSphere_normal = new Vec3();

    /**
     * @method particleSphere
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.PARTICLE | Shape.types.SPHERE] =
    Narrowphase.prototype.sphereParticle = function(result,sj,si,xj,xi,qj,qi,bj,bi){
        // The normal is the unit vector from sphere center to particle center
        var normal = particleSphere_normal;
        normal.set(0,0,1);
        xi.vsub(xj,normal);
        var lengthSquared = normal.norm2();

        if(lengthSquared <= sj.radius * sj.radius){
            var r = this.makeResult(bi,bj,si,sj);
            normal.normalize();
            r.rj.copy(normal);
            r.rj.mult(sj.radius,r.rj);
            r.ni.copy(normal); // Contact normal
            r.ni.negate(r.ni);
            r.ri.set(0,0,0); // Center of particle
            result.push(r);
        }
    };

    // WIP
    var cqj = new Quaternion();
    var convexParticle_local = new Vec3();
    var convexParticle_normal = new Vec3();
    var convexParticle_penetratedFaceNormal = new Vec3();
    var convexParticle_vertexToParticle = new Vec3();
    var convexParticle_worldPenetrationVec = new Vec3();

    /**
     * @method convexParticle
     * @param  {Array}      result
     * @param  {Shape}      si
     * @param  {Shape}      sj
     * @param  {Vec3}       xi
     * @param  {Vec3}       xj
     * @param  {Quaternion} qi
     * @param  {Quaternion} qj
     * @param  {Body}       bi
     * @param  {Body}       bj
     */
    Narrowphase.prototype[Shape.types.PARTICLE | Shape.types.CONVEXPOLYHEDRON] =
    Narrowphase.prototype.convexParticle = function(result,sj,si,xj,xi,qj,qi,bj,bi){
        var penetratedFaceIndex = -1;
        var penetratedFaceNormal = convexParticle_penetratedFaceNormal;
        var worldPenetrationVec = convexParticle_worldPenetrationVec;
        var minPenetration = null;
        var numDetectedFaces = 0;

        // Convert particle position xi to local coords in the convex
        var local = convexParticle_local;
        local.copy(xi);
        local.vsub(xj,local); // Convert position to relative the convex origin
        qj.conjugate(cqj);
        cqj.vmult(local,local);

        if(sj.pointIsInside(local)){

            if(sj.worldVerticesNeedsUpdate){
                sj.computeWorldVertices(xj,qj);
            }
            if(sj.worldFaceNormalsNeedsUpdate){
                sj.computeWorldFaceNormals(qj);
            }

            // For each world polygon in the polyhedra
            for(var i=0,nfaces=sj.faces.length; i!==nfaces; i++){

                // Construct world face vertices
                var verts = [ sj.worldVertices[ sj.faces[i][0] ] ];
                var normal = sj.worldFaceNormals[i];

                // Check how much the particle penetrates the polygon plane.
                xi.vsub(verts[0],convexParticle_vertexToParticle);
                var penetration = -normal.dot(convexParticle_vertexToParticle);
                if(minPenetration===null || Math.abs(penetration)<Math.abs(minPenetration)){
                    minPenetration = penetration;
                    penetratedFaceIndex = i;
                    penetratedFaceNormal.copy(normal);
                    numDetectedFaces++;
                }
            }

            if(penetratedFaceIndex!==-1){
                // Setup contact
                var r = this.makeResult(bi,bj,si,sj);
                penetratedFaceNormal.mult(minPenetration, worldPenetrationVec);

                // rj is the particle position projected to the face
                worldPenetrationVec.vadd(xi,worldPenetrationVec);
                worldPenetrationVec.vsub(xj,worldPenetrationVec);
                r.rj.copy(worldPenetrationVec);
                //var projectedToFace = xi.vsub(xj).vadd(worldPenetrationVec);
                //projectedToFace.copy(r.rj);

                //qj.vmult(r.rj,r.rj);
                penetratedFaceNormal.negate( r.ni ); // Contact normal
                r.ri.set(0,0,0); // Center of particle

                var ri = r.ri,
                    rj = r.rj;

                // Make relative to bodies
                ri.vadd(xi, ri);
                ri.vsub(bi.position, ri);
                rj.vadd(xj, rj);
                rj.vsub(bj.position, rj);

                result.push(r);
            } else {
                console.warn("Point found inside convex, but did not find penetrating face!");
            }
        }
    };

    Narrowphase.prototype[Shape.types.BOX | Shape.types.HEIGHTFIELD] =
    Narrowphase.prototype.boxHeightfield = function (result,si,sj,xi,xj,qi,qj,bi,bj){
        this.convexHeightfield(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj);
    };

    var convexHeightfield_tmp1 = new Vec3();
    var convexHeightfield_tmp2 = new Vec3();

    /**
     * @method sphereHeightfield
     */
    Narrowphase.prototype[Shape.types.CONVEXPOLYHEDRON | Shape.types.HEIGHTFIELD] =
    Narrowphase.prototype.convexHeightfield = function (
        result,
        convexShape,
        hfShape,
        convexPos,
        hfPos,
        convexQuat,
        hfQuat,
        convexBody,
        hfBody
    ){
        var data = hfShape.data,
            w = hfShape.elementSize,
            radius = convexShape.boundingSphereRadius,
            worldPillarOffset = convexHeightfield_tmp2;

        // Get sphere position to heightfield local!
        var localConvexPos = convexHeightfield_tmp1;
        Transform.pointToLocalFrame(hfPos, hfQuat, convexPos, localConvexPos);

        // Get the index of the data points to test against
        var iMinX = Math.floor((localConvexPos.x - radius) / w) - 1,
            iMaxX = Math.ceil((localConvexPos.x + radius) / w) + 1,
            iMinY = Math.floor((localConvexPos.y - radius) / w) - 1,
            iMaxY = Math.ceil((localConvexPos.y + radius) / w) + 1;

        // Bail out if we are out of the terrain
        if(iMaxX < 0 || iMaxY < 0 || iMinX > data.length || iMaxY > data[0].length){
            return;
        }

        // Clamp index to edges
        if(iMinX < 0){ iMinX = 0; }
        if(iMaxX < 0){ iMaxX = 0; }
        if(iMinY < 0){ iMinY = 0; }
        if(iMaxY < 0){ iMaxY = 0; }
        if(iMinX >= data.length){ iMinX = data.length - 1; }
        if(iMaxX >= data.length){ iMaxX = data.length - 1; }
        if(iMaxY >= data[0].length){ iMaxY = data[0].length - 1; }
        if(iMinY >= data[0].length){ iMinY = data[0].length - 1; }

        var minMax = [];
        hfShape.getRectMinMax(iMinX, iMinY, iMaxX, iMaxY, minMax);
        var min = minMax[0];
        var max = minMax[1];

        // Bail out if we're cant touch the bounding height box
        if(localConvexPos.z - radius > max || localConvexPos.z + radius < min){
            return;
        }

        for(var i = iMinX; i < iMaxX; i++){
            for(var j = iMinY; j < iMaxY; j++){

                // Lower triangle
                hfShape.getConvexTrianglePillar(i, j, false);
                Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
                this.convexConvex(result, convexShape, hfShape.pillarConvex, convexPos, worldPillarOffset, convexQuat, hfQuat, convexBody, hfBody);

                // Upper triangle
                hfShape.getConvexTrianglePillar(i, j, true);
                Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
                this.convexConvex(result, convexShape, hfShape.pillarConvex, convexPos, worldPillarOffset, convexQuat, hfQuat, convexBody, hfBody);
            }
        }
    };

    var sphereHeightfield_tmp1 = new Vec3();
    var sphereHeightfield_tmp2 = new Vec3();

    /**
     * @method sphereHeightfield
     */
    Narrowphase.prototype[Shape.types.SPHERE | Shape.types.HEIGHTFIELD] =
    Narrowphase.prototype.sphereHeightfield = function (
        result,
        sphereShape,
        hfShape,
        spherePos,
        hfPos,
        sphereQuat,
        hfQuat,
        sphereBody,
        hfBody
    ){
        var data = hfShape.data,
            radius = sphereShape.radius,
            w = hfShape.elementSize,
            worldPillarOffset = sphereHeightfield_tmp2;

        // Get sphere position to heightfield local!
        var localSpherePos = sphereHeightfield_tmp1;
        Transform.pointToLocalFrame(hfPos, hfQuat, spherePos, localSpherePos);

        // Get the index of the data points to test against
        var iMinX = Math.floor((localSpherePos.x - radius) / w) - 1,
            iMaxX = Math.ceil((localSpherePos.x + radius) / w) + 1,
            iMinY = Math.floor((localSpherePos.y - radius) / w) - 1,
            iMaxY = Math.ceil((localSpherePos.y + radius) / w) + 1;

        // Bail out if we are out of the terrain
        if(iMaxX < 0 || iMaxY < 0 || iMinX > data.length || iMaxY > data[0].length){
            return;
        }

        // Clamp index to edges
        if(iMinX < 0){ iMinX = 0; }
        if(iMaxX < 0){ iMaxX = 0; }
        if(iMinY < 0){ iMinY = 0; }
        if(iMaxY < 0){ iMaxY = 0; }
        if(iMinX >= data.length){ iMinX = data.length - 1; }
        if(iMaxX >= data.length){ iMaxX = data.length - 1; }
        if(iMaxY >= data[0].length){ iMaxY = data[0].length - 1; }
        if(iMinY >= data[0].length){ iMinY = data[0].length - 1; }

        var minMax = [];
        hfShape.getRectMinMax(iMinX, iMinY, iMaxX, iMaxY, minMax);
        var min = minMax[0];
        var max = minMax[1];

        // Bail out if we're cant touch the bounding height box
        if(localSpherePos.z - radius > max || localSpherePos.z + radius < min){
            return;
        }

        for(var i = iMinX; i < iMaxX; i++){
            for(var j = iMinY; j < iMaxY; j++){

                var numContactsBefore = result.length;

                // Lower triangle
                hfShape.getConvexTrianglePillar(i, j, false);
                Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
                this.sphereConvex(result, sphereShape, hfShape.pillarConvex, spherePos, worldPillarOffset, sphereQuat, hfQuat, sphereBody, hfBody);

                // Upper triangle
                hfShape.getConvexTrianglePillar(i, j, true);
                Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
                this.sphereConvex(result, sphereShape, hfShape.pillarConvex, spherePos, worldPillarOffset, sphereQuat, hfQuat, sphereBody, hfBody);

                var numContacts = result.length - numContactsBefore;

                if(numContacts > 2){
                    return;
                }
                /*
                // Skip all but 1
                for (var k = 0; k < numContacts - 1; k++) {
                    result.pop();
                }
                */
            }
        }
    };