Babylon.js/Oimo.js

/**
 * OimoPhysics DEV 1.1.0a
 * @author Saharan / http://el-ement.com/
 * 
 * Oimo.js 2014
 * @author LoTh / http://3dflashlo.wordpress.com/
 */
 
var OIMO = { REVISION: 'DEV.1.1.2a' };

OIMO.SHAPE_SPHERE = 0x1;
OIMO.SHAPE_BOX = 0x2;

OIMO.WORLD_SCALE = 1;
OIMO.INV_SCALE = 1;

OIMO.TO_RAD = Math.PI / 180;
OIMO.TwoPI = 2.0 * Math.PI;

// Global identification of next shape.
// This will be incremented every time a shape is created.
OIMO.nextID = 0;

var OIMO_ARRAY_TYPE;
if(!OIMO_ARRAY_TYPE) { OIMO_ARRAY_TYPE = typeof Float32Array !== 'undefined' ? Float32Array : Array; }
/**
* The class of physical computing world. 
* You must be added to the world physical all computing objects
* @author saharan
*/
OIMO.World = function(TimeStep, BroadPhaseType, Iterations, NoStat){

    // The time between each step
    this.timeStep= TimeStep || 1/60;
    // The number of iterations for constraint solvers.
    this.numIterations = Iterations || 8;
     // It is a wide-area collision judgment that is used in order to reduce as much as possible a detailed collision judgment.
    var broadPhaseType = BroadPhaseType || 2;
    switch(broadPhaseType){
        case 1: this.broadPhase=new OIMO.BruteForceBroadPhase(); break;
        case 2: default: this.broadPhase=new OIMO.SAPBroadPhase(); break;
        case 3: this.broadPhase=new OIMO.DBVTBroadPhase(); break;
    }
    // Whether the constraints randomizer is enabled or not.
    this.enableRandomizer = true;

    this.isNoStat = NoStat || false;


    // The rigid body list
    this.rigidBodies=null;
    // number of rigid body
    this.numRigidBodies=0;
    // The contact list
    this.contacts=null;
    this.unusedContacts=null;
    // The number of contact
    this.numContacts=0;
    // The number of contact points
    this.numContactPoints=0;
    //  The joint list
    this.joints=null;
    // The number of joints.
    this.numJoints=0;
    // The number of simulation islands.
    this.numIslands=0;
    
   
    // The gravity in the world.
    this.gravity=new OIMO.Vec3(0,-9.80665,0);

    // This is the detailed information of the performance. 
    this.performance = null;
    if(!this.isNoStat) this.performance = new OIMO.Performance(this);

    var numShapeTypes=3;
    this.detectors=[];
    this.detectors.length = numShapeTypes;
    var i=numShapeTypes;
    while(i--){
        this.detectors[i]=[];
        this.detectors[i].length = numShapeTypes;
    }
    this.detectors[OIMO.SHAPE_SPHERE][OIMO.SHAPE_SPHERE]=new OIMO.SphereSphereCollisionDetector();
    this.detectors[OIMO.SHAPE_SPHERE][OIMO.SHAPE_BOX]=new OIMO.SphereBoxCollisionDetector(false);
    this.detectors[OIMO.SHAPE_BOX][OIMO.SHAPE_SPHERE]=new OIMO.SphereBoxCollisionDetector(true);
    this.detectors[OIMO.SHAPE_BOX][OIMO.SHAPE_BOX]=new OIMO.BoxBoxCollisionDetector();
 
    this.randX=65535;
    this.randA=98765;
    this.randB=123456789;
    this.maxIslandRigidBodies=64;
    this.islandRigidBodies=[];
    this.islandRigidBodies.length = this.maxIslandRigidBodies;
    this.islandStack=[];
    this.islandStack.length = this.maxIslandRigidBodies;
    this.maxIslandConstraints=128;
    this.islandConstraints=[];
    this.islandConstraints.length = this.maxIslandConstraints;

};

OIMO.World.prototype = {
    constructor: OIMO.World,
    /**
    * Reset the randomizer and remove all rigid bodies, shapes, joints and any object from the world.
	*/
    clear:function(){
        this.randX=65535;
        while(this.joints!==null){
            this.removeJoint(this.joints);
        }
        while(this.contacts!==null){
            this.removeContact(this.contacts);
        }
        while(this.rigidBodies!==null){
            this.removeRigidBody(this.rigidBodies);
        }
        OIMO.nextID=0;
    },
    /**
    * I'll add a rigid body to the world. 
    * Rigid body that has been added will be the operands of each step.
    * @param  rigidBody  Rigid body that you want to add
    */
    addRigidBody:function(rigidBody){
        if(rigidBody.parent){
            throw new Error("It is not possible to be added to more than one world one of the rigid body");
        }
        rigidBody.parent=this;
        rigidBody.awake();
        for(var shape=rigidBody.shapes; shape!==null; shape=shape.next){
            this.addShape(shape);
        }
        if(this.rigidBodies!==null)(this.rigidBodies.prev=rigidBody).next=this.rigidBodies;
        this.rigidBodies = rigidBody;
        this.numRigidBodies++;
    },
    /**
    * I will remove the rigid body from the world. 
    * Rigid body that has been deleted is excluded from the calculation on a step-by-step basis.
    * @param  rigidBody  Rigid body to be removed
    */
    removeRigidBody:function(rigidBody){
        var remove=rigidBody;
        if(remove.parent!==this)return;
        remove.awake();
        var js=remove.jointLink;
        while(js!=null){
	        var joint=js.joint;
	        js=js.next;
	        this.removeJoint(joint);
        }
        for(var shape=rigidBody.shapes; shape!==null; shape=shape.next){
            this.removeShape(shape);
        }
        var prev=remove.prev;
        var next=remove.next;
        if(prev!==null) prev.next=next;
        if(next!==null) next.prev=prev;
        if(this.rigidBodies==remove) this.rigidBodies=next;
        remove.prev=null;
        remove.next=null;
        remove.parent=null;
        this.numRigidBodies--;
    },
    getByName:function(name){
        var result = null;
        var body=this.rigidBodies;
        while(body!==null){
            if(body.name!== " " && body.name === name) result = body;
            body=body.next;
        }
        var joint=this.joints;
        while(joint!==null){
            if(joint.name!== "" && joint.name === name) result = joint;
            joint=joint.next;
        }
        return result;
    },

    /**
    * I'll add a shape to the world..
    * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world, 
    * Shape will be added to the world automatically, please do not call from outside this method.
    * @param  shape  Shape you want to add
    */
    addShape:function(shape){
        if(!shape.parent || !shape.parent.parent){
            throw new Error("It is not possible to be added alone to shape world");
        }
        shape.proxy = this.broadPhase.createProxy(shape);
        shape.updateProxy();
        this.broadPhase.addProxy(shape.proxy);
    },

    /**
    * I will remove the shape from the world.
    * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world, 
    * Shape will be added to the world automatically, please do not call from outside this method.
    * @param  shape  Shape you want to delete
    */
    removeShape:function(shape){
        this.broadPhase.removeProxy(shape.proxy);
        shape.proxy=null;
    },

    /**
    * I'll add a joint to the world. 
    * Joint that has been added will be the operands of each step.
    * @param  shape Joint to be added
    */
    addJoint:function(joint){
        if(joint.parent){
            throw new Error("It is not possible to be added to more than one world one of the joint");
        }
        if(this.joints!=null)(this.joints.prev=joint).next=this.joints;
        this.joints=joint;
        joint.parent=this;
        this.numJoints++;
        joint.awake();
        joint.attach();
    },

    /**
    * I will remove the joint from the world. 
    * Joint that has been added will be the operands of each step.
    * @param  shape Joint to be deleted
    */
    removeJoint:function(joint){
        var remove=joint;
        var prev=remove.prev;
        var next=remove.next;
        if(prev!==null)prev.next=next;
        if(next!==null)next.prev=prev;
        if(this.joints==remove)this.joints=next;
        remove.prev=null;
        remove.next=null;
        this.numJoints--;
        remove.awake();
        remove.detach();
        remove.parent=null;
    },

    worldscale:function(scale){
        OIMO.WORLD_SCALE = scale || 100;
        OIMO.INV_SCALE = 1/OIMO.WORLD_SCALE;
    },

    /**
    * I will proceed only time step seconds time of World.
    */
    step:function(){
        var time0, time1, time2, time3;

        if(!this.isNoStat) time0=Date.now();

        var body=this.rigidBodies;

        while(body!==null){
            body.addedToIsland=false;
            if(body.sleeping){
                if( body.linearVelocity.testZero() || body.position.testDiff(body.sleepPosition) || body.orientation.testDiff(body.sleepOrientation)){ body.awake(); } // awake the body
                /*var lv=body.linearVelocity;
                var av=body.linearVelocity;
                var p=body.position;
                var sp=body.sleepPosition;
                var o=body.orientation;
                var so=body.sleepOrientation;
               
                if( lv.x!==0 || lv.y!==0 || lv.z!==0 || av.x!==0 || av.y!==0 || av.z!==0 ||
                p.x!==sp.x || p.y!==sp.y || p.z!==sp.z ||
                o.s!==so.s || o.x!==so.x || o.y!==so.y || o.z!==so.z
                ){ body.awake(); }*/
            }
            body=body.next;
        }

        

        //------------------------------------------------------
        //   UPDATE CONTACT
        //------------------------------------------------------

        // broad phase
        if(!this.isNoStat) time1=Date.now();

        this.broadPhase.detectPairs();
        var pairs=this.broadPhase.pairs;
        var numPairs=this.broadPhase.numPairs;
        var i = numPairs;
        //do{
        while(i--){
        //for(var i=0, l=numPairs; i<l; i++){
            var pair=pairs[i];
            var s1;
            var s2;
            if(pair.shape1.id<pair.shape2.id){
                s1=pair.shape1;
                s2=pair.shape2;
            }else{
                s1=pair.shape2;
                s2=pair.shape1;
            }
            var link;
            if(s1.numContacts<s2.numContacts){
                link=s1.contactLink;
            }else{
                link=s2.contactLink;
            }
            var exists=false;
            while(link){
                var contact=link.contact;
                if(contact.shape1==s1&&contact.shape2==s2){
                    contact.persisting=true;
                    exists=true;// contact already exists
                    break;
                }
                link=link.next;
            }
            if(!exists){
                this.addContact(s1,s2);
            }
        }// while(i-- >0);

        if(!this.isNoStat){
            time2=Date.now();
            this.performance.broadPhaseTime=time2-time1;
        }

        // update & narrow phase
        this.numContactPoints=0;
        contact=this.contacts;
        while(contact!==null){
            if(!contact.persisting){
                var aabb1=contact.shape1.aabb;
                var aabb2=contact.shape2.aabb;
                if(
	                aabb1.minX>aabb2.maxX || aabb1.maxX<aabb2.minX ||
	                aabb1.minY>aabb2.maxY || aabb1.maxY<aabb2.minY ||
	                aabb1.minZ>aabb2.maxZ || aabb1.maxZ<aabb2.minZ
                ){
                    var next=contact.next;
                    this.removeContact(contact);
                    contact=next;
                    continue;
                }
            }
            var b1=contact.body1;
            var b2=contact.body2;
            if(b1.isDynamic && !b1.sleeping || b2.isDynamic && !b2.sleeping){
                contact.updateManifold();
            }
            this.numContactPoints+=contact.manifold.numPoints;
            contact.persisting=false;
            contact.constraint.addedToIsland=false;
            contact=contact.next;
        }

        if(!this.isNoStat){
            time3=Date.now();
            this.performance.narrowPhaseTime=time3-time2;
        }

        //------------------------------------------------------
        //   SOLVE ISLANDS
        //------------------------------------------------------

        var invTimeStep=1/this.timeStep;
        //var body;
        var joint;
        var constraint;
        var num;
        for(joint=this.joints; joint!==null; joint=joint.next){
            joint.addedToIsland=false;
        }
        // expand island buffers
        if(this.maxIslandRigidBodies<this.numRigidBodies){
            this.maxIslandRigidBodies=this.numRigidBodies<<1;
            //this.maxIslandRigidBodies=this.numRigidBodies*2;

            this.islandRigidBodies=[];
            this.islandStack=[];
            this.islandRigidBodies.length = this.maxIslandRigidBodies;
            this.islandStack.length = this.maxIslandRigidBodies;
        }
        var numConstraints=this.numJoints+this.numContacts;
        if(this.maxIslandConstraints<numConstraints){
            this.maxIslandConstraints=numConstraints<<1;
            //this.maxIslandConstraints=numConstraints*2;
            this.islandConstraints=[];
            this.islandConstraints.length = this.maxIslandConstraints;
        }
        time1=Date.now();
        this.numIslands=0;
        // build and solve simulation islands
        for(var base=this.rigidBodies; base!==null; base=base.next){
            if(base.addedToIsland || base.isStatic || base.sleeping){
                continue;// ignore
            }
            if(base.isLonely()){// update single body
                if(base.isDynamic){
                    base.linearVelocity.addTime(this.gravity, this.timeStep);
                    /*base.linearVelocity.x+=this.gravity.x*this.timeStep;
                    base.linearVelocity.y+=this.gravity.y*this.timeStep;
                    base.linearVelocity.z+=this.gravity.z*this.timeStep;*/
                }
                if(this.calSleep(base)){
                    base.sleepTime+=this.timeStep;
                if(base.sleepTime>0.5){
                    base.sleep();
                }else{
                    base.updatePosition(this.timeStep);
                }
                }else{
                    base.sleepTime=0;
                    base.updatePosition(this.timeStep);
                }
                this.numIslands++;
                continue;
            }
            var islandNumRigidBodies=0;
            var islandNumConstraints=0;
            var stackCount=1;
            // add rigid body to stack
            this.islandStack[0]=base;
            base.addedToIsland=true;
            // build an island
            do{
                // get rigid body from stack
                body=this.islandStack[--stackCount];
                this.islandStack[stackCount]=null;
                body.sleeping=false;
                // add rigid body to the island
                this.islandRigidBodies[islandNumRigidBodies++]=body;
                if(body.isStatic){
                continue;
                }
                // search connections
                for(var cs=body.contactLink; cs!==null; cs=cs.next){
                    var contact=cs.contact;
                    constraint=contact.constraint;
                    if(constraint.addedToIsland||!contact.touching){
                        continue;// ignore
                    }
                    // add constraint to the island
                    this.islandConstraints[islandNumConstraints++]=constraint;
                    constraint.addedToIsland=true;
                    var next=cs.body;
                    if(next.addedToIsland){
                    continue;
                    }
                    // add rigid body to stack
                    this.islandStack[stackCount++]=next;
                    next.addedToIsland=true;
                }
                for(var js=body.jointLink; js!==null; js=js.next){
                    constraint=js.joint;
                    if(constraint.addedToIsland){
                        continue;// ignore
                    }
                    // add constraint to the island
                    this.islandConstraints[islandNumConstraints++]=constraint;
                    constraint.addedToIsland=true;
                    next=js.body;
                    if(next.addedToIsland || !next.isDynamic){
                    continue;
                    }
                    // add rigid body to stack
                    this.islandStack[stackCount++]=next;
                    next.addedToIsland=true;
                }
            }while(stackCount!=0);

            // update velocities
            var gVel = new OIMO.Vec3().addTime(this.gravity, this.timeStep);
            /*var gx=this.gravity.x*this.timeStep;
            var gy=this.gravity.y*this.timeStep;
            var gz=this.gravity.z*this.timeStep;*/
            var j = islandNumRigidBodies;
            while (j--){
            //or(var j=0, l=islandNumRigidBodies; j<l; j++){
                body=this.islandRigidBodies[j];
                if(body.isDynamic){
                    body.linearVelocity.addEqual(gVel);
                    /*body.linearVelocity.x+=gx;
                    body.linearVelocity.y+=gy;
                    body.linearVelocity.z+=gz;*/
                }
            }

            // randomizing order
            if(this.enableRandomizer){
                //for(var j=1, l=islandNumConstraints; j<l; j++){
                j = islandNumConstraints;
                while(j--){ if(j!==0){     
                        var swap=(this.randX=(this.randX*this.randA+this.randB&0x7fffffff))/2147483648.0*j|0;
                        constraint=this.islandConstraints[j];
                        this.islandConstraints[j]=this.islandConstraints[swap];
                        this.islandConstraints[swap]=constraint;
                    }
                }
            }

            // solve contraints
            j = islandNumConstraints;
            while(j--){
            //for(j=0, l=islandNumConstraints; j<l; j++){
                this.islandConstraints[j].preSolve(this.timeStep,invTimeStep);// pre-solve
            }
            var k = this.numIterations;
            while(k--){
            //for(var k=0, l=this.numIterations; k<l; k++){
                j = islandNumConstraints;
                while(j--){
                //for(j=0, m=islandNumConstraints; j<m; j++){
                    this.islandConstraints[j].solve();// main-solve
                }
            }
            j = islandNumConstraints;
            while(j--){
            //for(j=0, l=islandNumConstraints; j<l; j++){
                this.islandConstraints[j].postSolve();// post-solve
                this.islandConstraints[j]=null;// gc
            }

            // sleeping check
            var sleepTime=10;
            j = islandNumRigidBodies;
            while(j--){
            //for(j=0, l=islandNumRigidBodies;j<l;j++){
                body=this.islandRigidBodies[j];
                if(this.calSleep(body)){
                    body.sleepTime+=this.timeStep;
                    if(body.sleepTime<sleepTime)sleepTime=body.sleepTime;
                }else{
                    body.sleepTime=0;
                    sleepTime=0;
                    continue;
                }
            }
            if(sleepTime>0.5){
                // sleep the island
                j = islandNumRigidBodies;
                while(j--){
                //for(j=0, l=islandNumRigidBodies;j<l;j++){
                    this.islandRigidBodies[j].sleep();
                    this.islandRigidBodies[j]=null;// gc
                }
            }else{
                // update positions
                j = islandNumRigidBodies;
                while(j--){
                //for(j=0, l=islandNumRigidBodies;j<l;j++){
                    this.islandRigidBodies[j].updatePosition(this.timeStep);
                    this.islandRigidBodies[j]=null;// gc
                }
            }
            this.numIslands++;
        }

        if(!this.isNoStat){
            time2=Date.now();
            this.performance.solvingTime=time2-time1;

            //------------------------------------------------------
            //   END SIMULATION
            //------------------------------------------------------

            time2=Date.now();
            // fps update
            this.performance.upfps();

            this.performance.totalTime=time2-time0;
            this.performance.updatingTime=this.performance.totalTime-(this.performance.broadPhaseTime+this.performance.narrowPhaseTime+this.performance.solvingTime);
        }
    },
    addContact:function(s1,s2){
        var newContact;
        if(this.unusedContacts!==null){
            newContact=this.unusedContacts;
            this.unusedContacts=this.unusedContacts.next;
        }else{
            newContact = new OIMO.Contact();
        }
        newContact.attach(s1,s2);
        newContact.detector=this.detectors[s1.type][s2.type];
        if(this.contacts)(this.contacts.prev=newContact).next=this.contacts;
        this.contacts=newContact;
        this.numContacts++;
    },
    removeContact:function(contact){
        var prev=contact.prev;
        var next=contact.next;
        if(next)next.prev=prev;
        if(prev)prev.next=next;
        if(this.contacts==contact)this.contacts=next;
        contact.prev=null;
        contact.next=null;
        contact.detach();
        contact.next=this.unusedContacts;
        this.unusedContacts=contact;
        this.numContacts--;
    },
    checkContact:function(name1, name2){
        var n1, n2;
        var contact = this.contacts;
        while(contact!==null){
            n1 = contact.body1.name || ' ';
            n2 = contact.body2.name || ' ';
            if((n1==name1 && n2==name2) || (n2==name1 && n1==name2)){ if(contact.touching) return true; else return false;}
            else contact = contact.next;
        }
        return false;
    },
    calSleep:function(body){
        if(!body.allowSleep)return false;
        if(body.linearVelocity.len()>0.04)return false;
        if(body.angularVelocity.len()>0.25)return false;
        /*var v=body.linearVelocity;
        if(v.x*v.x+v.y*v.y+v.z*v.z>0.04){return false;}
        v=body.angularVelocity;
        if(v.x*v.x+v.y*v.y+v.z*v.z>0.25){return false;}*/
        return true;
    }
}
/**
* The class of rigid body. 
* Rigid body has the shape of a single or multiple collision processing, 
* I can set the parameters individually.
* @author saharan
*/
OIMO.RigidBody = function(X,Y,Z,Rad,Ax,Ay,Az){
    var rad = Rad || 0;
    var ax = Ax || 0;
    var ay = Ay || 0;
    var az = Az || 0; 
    var x = X || 0;
    var y = Y || 0;
    var z = Z || 0;
    
    this.name = " ";
    // It is a kind of rigid body that represents the dynamic rigid body.
    this.BODY_DYNAMIC=0x1;
    // It is a kind of rigid body that represents the static rigid body.
    this.BODY_STATIC=0x2;
    // The maximum number of shapes that can be added to a one rigid.
    this.MAX_SHAPES=64;

    this.prev=null;
    this.next=null;

    // I represent the kind of rigid body.
    // Please do not change from the outside this variable. 
    // If you want to change the type of rigid body, always 
    // Please specify the type you want to set the arguments of setupMass method.
    this.type=0;

    this.massInfo= new OIMO.MassInfo();

    // It is the world coordinate of the center of gravity.
    this.position=new OIMO.Vec3(x,y,z); 


    this.newPosition = new OIMO.Vec3(0,0,0);
    this.controlPos = false;
    this.newOrientation = new OIMO.Quat();
    this.newRotation = new OIMO.Vec3(0,0,0);
    this.currentRotation = new OIMO.Vec3(0,0,0);
    this.controlRot = false;
    this.controlRotInTime = false;

    this.orientation = this.rotationAxisToQuad(rad,ax,ay,az);
    /*var len=ax*ax+ay*ay+az*az; 
    if(len>0){
        len=1/Math.sqrt(len);
        ax*=len;
        ay*=len;
        az*=len;
    }
    var sin=Math.sin(rad*0.5);
    var cos=Math.cos(rad*0.5);*/
    // It is a quaternion that represents the attitude.
    //this.orientation = new OIMO.Quat(cos,sin*ax,sin*ay,sin*az);
    // Is the translational velocity.
    this.linearVelocity = new OIMO.Vec3();
    // Is the angular velocity.
    this.angularVelocity = new OIMO.Vec3();

    // return matrix for three.js
    this.matrix = new OIMO.Mat44();

    //--------------------------------------------
    //  Please do not change from the outside this variables.
    //--------------------------------------------

    // It is a world that rigid body has been added.
    this.parent=null;
    this.contactLink=null;
    this.numContacts=0;

    // An array of shapes that are included in the rigid body.
    this.shapes=null;
    // The number of shapes that are included in the rigid body.
    this.numShapes=0;

    // It is the link array of joint that is connected to the rigid body.
    this.jointLink=null;
    // The number of joints that are connected to the rigid body.
    this.numJoints=0;

    // It is the world coordinate of the center of gravity in the sleep just before.
    this.sleepPosition=new OIMO.Vec3();
    // It is a quaternion that represents the attitude of sleep just before.
    this.sleepOrientation=new OIMO.Quat();
    // I will show this rigid body to determine whether it is a rigid body static.
    this.isStatic=false;
    // I indicates that this rigid body to determine whether it is a rigid body dynamic. 
    this.isDynamic=false;
    // It is a rotation matrix representing the orientation.
    this.rotation=new OIMO.Mat33();

    //--------------------------------------------
    // It will be recalculated automatically from the shape, which is included.
    //--------------------------------------------
    
    // This is the weight. 
    this.mass=NaN;
    // It is the reciprocal of the mass.
    this.inverseMass=NaN;
    // It is the inverse of the inertia tensor in the world system.
    this.inverseInertia=new OIMO.Mat33();
    // It is the inertia tensor in the initial state.
    this.localInertia=new OIMO.Mat33();
    // It is the inverse of the inertia tensor in the initial state.
    this.inverseLocalInertia=new OIMO.Mat33();


    // I indicates rigid body whether it has been added to the simulation Island.
    this.addedToIsland=false;
    // It shows how to sleep rigid body.
    this.allowSleep=true;
    // This is the time from when the rigid body at rest.
    this.sleepTime=0;
    // I shows rigid body to determine whether it is a sleep state.
    this.sleeping=false;
}

OIMO.RigidBody.prototype = {

    constructor: OIMO.RigidBody,
    /**
    * I'll add a shape to rigid body.  
    * If you add a shape, please call the setupMass method to step up to the start of the next.
    * @param   shape shape to Add 
    */
    addShape:function(shape){
        if(shape.parent){
            throw new Error("It is not possible that you add to the multi-rigid body the shape of one");
        }
        if(this.shapes!=null)(this.shapes.prev=shape).next=this.shapes;
        this.shapes=shape;
        shape.parent=this;
        if(this.parent)this.parent.addShape(shape);
        this.numShapes++;
    },
    /**
    * I will delete the shape from the rigid body. 
    * If you delete a shape, please call the setupMass method to step up to the start of the next. 
    * @param   shape shape to Delete 
    */
    removeShape:function(shape){
        var remove=shape;
        if(remove.parent!=this)return;
        var prev=remove.prev;
        var next=remove.next;
        if(prev!=null)prev.next=next;
        if(next!=null)next.prev=prev;
        if(this.shapes==remove)this.shapes=next;
        remove.prev=null;
        remove.next=null;
        remove.parent=null;
        if(this.parent)this.parent.removeShape(remove);
        this.numShapes--;
    },
    /**
    * Calulates mass datas(center of gravity, mass, moment inertia, etc...).
    * If the parameter type is set to BODY_STATIC, the rigid body will be fixed to the space.
    * If the parameter adjustPosition is set to true, the shapes' relative positions and
    * the rigid body's position will be adjusted to the center of gravity.
    * @param   type
    * @param   adjustPosition
    */
    setupMass:function(Type,AdjustPosition){
        var adjustPosition = ( AdjustPosition !== undefined ) ? AdjustPosition : true;
        var type = Type || this.BODY_DYNAMIC;
        //var te = this.localInertia.elements;
        this.type=type;
        this.isDynamic=type==this.BODY_DYNAMIC;
        this.isStatic=type==this.BODY_STATIC;
        this.mass=0;
        this.localInertia.init(0,0,0,0,0,0,0,0,0);
        var te = this.localInertia.elements;
        //
        var tmpM=new OIMO.Mat33();
        var tmpV=new OIMO.Vec3();
        for(var shape=this.shapes;shape!=null;shape=shape.next){
            shape.calculateMassInfo(this.massInfo);
            var shapeMass=this.massInfo.mass;
            var relX=shape.relativePosition.x;
            var relY=shape.relativePosition.y;
            var relZ=shape.relativePosition.z;
            /*tmpV.x+=relX*shapeMass;
            tmpV.y+=relY*shapeMass;
            tmpV.z+=relZ*shapeMass;*/
            tmpV.addScale(shape.relativePosition, shapeMass);
            this.mass+=shapeMass;
            this.rotateInertia(shape.relativeRotation,this.massInfo.inertia,tmpM);
            this.localInertia.addEqual(tmpM);

            // add offset inertia
            te[0]+=shapeMass*(relY*relY+relZ*relZ);
            te[4]+=shapeMass*(relX*relX+relZ*relZ);
            te[8]+=shapeMass*(relX*relX+relY*relY);
            var xy=shapeMass*relX*relY;
            var yz=shapeMass*relY*relZ;
            var zx=shapeMass*relZ*relX;
            te[1]-=xy;
            te[3]-=xy;
            te[2]-=yz;
            te[6]-=yz;
            te[5]-=zx;
            te[7]-=zx;
        }
        this.inverseMass=1/this.mass;
        tmpV.scaleEqual(this.inverseMass);
        if(adjustPosition){
            this.position.addEqual(tmpV);
            for(shape=this.shapes;shape!=null;shape=shape.next){
                shape.relativePosition.subEqual(tmpV);
            }
            // subtract offset inertia
            relX=tmpV.x;
            relY=tmpV.y;
            relZ=tmpV.z;
            //var te = this.localInertia.elements;
            te[0]-=this.mass*(relY*relY+relZ*relZ);
            te[4]-=this.mass*(relX*relX+relZ*relZ);
            te[8]-=this.mass*(relX*relX+relY*relY);
            xy=this.mass*relX*relY;
            yz=this.mass*relY*relZ;
            zx=this.mass*relZ*relX;
            te[1]+=xy;
            te[3]+=xy;
            te[2]+=yz;
            te[6]+=yz;
            te[5]+=zx;
            te[7]+=zx;
        }
        this.inverseLocalInertia.invert(this.localInertia);
        if(type==this.BODY_STATIC){
            this.inverseMass=0;
            this.inverseLocalInertia.init(0,0,0,0,0,0,0,0,0);
        }
        this.syncShapes();
        this.awake();
    },
    /**
    * Awake the rigid body.
    */
    awake:function(){
        if(!this.allowSleep||!this.sleeping)return;
        this.sleeping=false;
        this.sleepTime=0;
        // awake connected constraints
        var cs=this.contactLink;
        while(cs!=null){
            cs.body.sleepTime=0;
            cs.body.sleeping=false;
            cs=cs.next;
        }
        var js=this.jointLink;
        while(js!=null){
            js.body.sleepTime=0;
            js.body.sleeping=false;
            js=js.next;
        }
        for(var shape=this.shapes;shape!=null;shape=shape.next){
            shape.updateProxy();
        }
    },
    /**
    * Sleep the rigid body.
    */
    sleep:function(){
        if(!this.allowSleep||this.sleeping)return;
        this.linearVelocity.init();
        this.angularVelocity.init();
        this.sleepPosition.copy(this.position);
        this.sleepOrientation.copy(this.orientation);
        /*this.linearVelocity.x=0;
        this.linearVelocity.y=0;
        this.linearVelocity.z=0;
        this.angularVelocity.x=0;
        this.angularVelocity.y=0;
        this.angularVelocity.z=0;
        this.sleepPosition.x=this.position.x;
        this.sleepPosition.y=this.position.y;
        this.sleepPosition.z=this.position.z;*/
        /*this.sleepOrientation.s=this.orientation.s;
        this.sleepOrientation.x=this.orientation.x;
        this.sleepOrientation.y=this.orientation.y;
        this.sleepOrientation.z=this.orientation.z;*/
        
        this.sleepTime=0;
        this.sleeping=true;
        for(var shape=this.shapes;shape!=null;shape=shape.next){
            shape.updateProxy();
        }
    },
    /**
    * Get whether the rigid body has not any connection with others.
    * @return
    */
    isLonely:function(){
        return this.numJoints==0&&this.numContacts==0;
    },

    /** 
    * The time integration of the motion of a rigid body, you can update the information such as the shape. 
    * This method is invoked automatically when calling the step of the World, 
    * There is no need to call from outside usually. 
    * @param  timeStep time 
    */

    updatePosition:function(timeStep){
        switch(this.type){
            case this.BODY_STATIC:
                this.linearVelocity.init();
                this.angularVelocity.init();
                // ONLY FOR TEST
                if(this.controlPos){
                    this.position.copy(this.newPosition);
                    this.controlPos = false;
                }
                if(this.controlRot){
                    this.orientation.copy(this.newOrientation);
                    this.controlRot = false;
                }
                /*this.linearVelocity.x=0;
                this.linearVelocity.y=0;
                this.linearVelocity.z=0;
                this.angularVelocity.x=0;
                this.angularVelocity.y=0;
                this.angularVelocity.z=0;*/
            break;
            case this.BODY_DYNAMIC:

                if(this.controlPos){
                    this.angularVelocity.init();
                    this.linearVelocity.init();
                    this.linearVelocity.x = (this.newPosition.x - this.position.x)/timeStep;
                    this.linearVelocity.y = (this.newPosition.y - this.position.y)/timeStep;
                    this.linearVelocity.z = (this.newPosition.z - this.position.z)/timeStep;
                    this.controlPos = false;
                }
                if(this.controlRot){
                    this.angularVelocity.init();
                    this.orientation.copy(this.newOrientation);

                    //var t=timeStep//*0.5;
                    //var q = new OIMO.Quat();
                    //q.sub(this.newOrientation, this.orientation);
                    //q.normalize(q);
                    /*q.s = (this.newOrientation.s - this.orientation.s)/t;
                    q.x = (this.newOrientation.x - this.orientation.x)/t;
                    q.y = (this.newOrientation.y - this.orientation.y)/t;
                    q.z = (this.newOrientation.z - this.orientation.z)/t;*/

                    //this.angularVelocity.applyQuaternion(q);
                    //this.angularVelocity.x = this.angularVelocity.x/t;
                    //this.angularVelocity.y = this.angularVelocity.y/t;
                    //this.angularVelocity.z = this.angularVelocity.z/t;
                    
                    this.controlRot = false;
                }

                this.position.addTime(this.linearVelocity, timeStep);
                this.orientation.addTime(this.angularVelocity, timeStep);

            break;
            default:
                throw new Error("Invalid type.");
        }
        this.syncShapes();
    },
    rotateInertia:function(rot,inertia,out){
        var tm1 = rot.elements;
        var tm2 = inertia.elements;

        var a0 = tm1[0], a3 = tm1[3], a6 = tm1[6];
        var a1 = tm1[1], a4 = tm1[4], a7 = tm1[7];
        var a2 = tm1[2], a5 = tm1[5], a8 = tm1[8];

        var b0 = tm2[0], b3 = tm2[3], b6 = tm2[6];
        var b1 = tm2[1], b4 = tm2[4], b7 = tm2[7];
        var b2 = tm2[2], b5 = tm2[5], b8 = tm2[8];
        
        var e00 = a0*b0 + a1*b3 + a2*b6;
        var e01 = a0*b1 + a1*b4 + a2*b7;
        var e02 = a0*b2 + a1*b5 + a2*b8;
        var e10 = a3*b0 + a4*b3 + a5*b6;
        var e11 = a3*b1 + a4*b4 + a5*b7;
        var e12 = a3*b2 + a4*b5 + a5*b8;
        var e20 = a6*b0 + a7*b3 + a8*b6;
        var e21 = a6*b1 + a7*b4 + a8*b7;
        var e22 = a6*b2 + a7*b5 + a8*b8;

        var oe = out.elements;
        oe[0] = e00*a0 + e01*a1 + e02*a2;
        oe[1] = e00*a3 + e01*a4 + e02*a5;
        oe[2] = e00*a6 + e01*a7 + e02*a8;
        oe[3] = e10*a0 + e11*a1 + e12*a2;
        oe[4] = e10*a3 + e11*a4 + e12*a5;
        oe[5] = e10*a6 + e11*a7 + e12*a8;
        oe[6] = e20*a0 + e21*a1 + e22*a2;
        oe[7] = e20*a3 + e21*a4 + e22*a5;
        oe[8] = e20*a6 + e21*a7 + e22*a8;
    },
    syncShapes:function(){
        var s=this.orientation.s;
        var x=this.orientation.x;
        var y=this.orientation.y;
        var z=this.orientation.z;
        var x2=2*x;
        var y2=2*y;
        var z2=2*z;
        var xx=x*x2;
        var yy=y*y2;
        var zz=z*z2;
        var xy=x*y2;
        var yz=y*z2;
        var xz=x*z2;
        var sx=s*x2;
        var sy=s*y2;
        var sz=s*z2;

        var tr = this.rotation.elements;
        tr[0]=1-yy-zz;
        tr[1]=xy-sz;
        tr[2]=xz+sy;
        tr[3]=xy+sz;
        tr[4]=1-xx-zz;
        tr[5]=yz-sx;
        tr[6]=xz-sy;
        tr[7]=yz+sx;
        tr[8]=1-xx-yy;

        this.rotateInertia(this.rotation,this.inverseLocalInertia,this.inverseInertia);
        for(var shape=this.shapes;shape!=null;shape=shape.next){
            //var relPos=shape.relativePosition;
            //var relRot=shape.relativeRotation;
            //var rot=shape.rotation;
            /*var lx=relPos.x;
            var ly=relPos.y;
            var lz=relPos.z;
            shape.position.x=this.position.x+lx*tr[0]+ly*tr[1]+lz*tr[2];
            shape.position.y=this.position.y+lx*tr[3]+ly*tr[4]+lz*tr[5];
            shape.position.z=this.position.z+lx*tr[6]+ly*tr[7]+lz*tr[8];*/

            shape.position.mul(this.position,shape.relativePosition,this.rotation);
            shape.rotation.mul(shape.relativeRotation,this.rotation);
            shape.updateProxy();
        }
    },
    applyImpulse:function(position,force){
        this.linearVelocity.addScale(force, this.inverseMass);
        /*this.linearVelocity.x+=force.x*this.inverseMass;
        this.linearVelocity.y+=force.y*this.inverseMass;
        this.linearVelocity.z+=force.z*this.inverseMass;*/
        var rel=new OIMO.Vec3();
        rel.sub(position,this.position).cross(rel,force).mulMat(this.inverseInertia,rel);
        this.angularVelocity.addEqual(rel);
        /*this.angularVelocity.x+=rel.x;
        this.angularVelocity.y+=rel.y;
        this.angularVelocity.z+=rel.z;*/
    },

    //---------------------------------------------
    //
    // FOR THREE JS
    //
    //---------------------------------------------

    rotationVectToQuad: function(rot){
        var r = OIMO.EulerToAxis( rot.x * OIMO.TO_RAD, rot.y * OIMO.TO_RAD, rot.z * OIMO.TO_RAD );
        return this.rotationAxisToQuad(r[0], r[1], r[2], r[3]);
    },

    rotationAxisToQuad: function(rad, ax, ay, az){ // in radian
        var len=ax*ax+ay*ay+az*az; 
        if(len>0){
            len=1/Math.sqrt(len);
            ax*=len;
            ay*=len;
            az*=len;
        }
        var sin=Math.sin(rad*0.5);
        var cos=Math.cos(rad*0.5);
        return new OIMO.Quat(cos,sin*ax,sin*ay,sin*az);
    },

    //---------------------------------------------
    // SET DYNAMIQUE POSITION AND ROTATION
    //---------------------------------------------

    setPosition:function(pos){
        this.newPosition.init(pos.x*OIMO.INV_SCALE,pos.y*OIMO.INV_SCALE,pos.z*OIMO.INV_SCALE);
        this.controlPos = true;
    },

    setQuaternion:function(q){ 
        //if(this.type == this.BODY_STATIC)this.orientation.init(q.w,q.x,q.y,q.z);

        this.newOrientation.init(q.w,q.x,q.y,q.z);
        this.controlRot = true;
    },

    setRotation:function(rot){ 
        this.newOrientation = this.rotationVectToQuad(rot);
        this.controlRot = true;
    },

    //---------------------------------------------
    // RESET DYNAMIQUE POSITION AND ROTATION
    //---------------------------------------------

    resetPosition:function(x,y,z){
        this.linearVelocity.init();
        this.angularVelocity.init();
        this.position.init(x*OIMO.INV_SCALE,y*OIMO.INV_SCALE,z*OIMO.INV_SCALE);
        this.awake();
    },
    resetQuaternion:function(q){
        this.angularVelocity.init();
        this.orientation = new OIMO.Quat(q.w,q.x,q.y,q.z);
        this.awake();
    },
    resetRotation:function(x,y,z){
        this.angularVelocity.init();
        this.orientation = this.rotationVectToQuad(new OIMO.Vec3(x,y,z));
        this.awake();
    },

    //---------------------------------------------
    // GET POSITION AND ROTATION
    //---------------------------------------------

    getPosition:function(){
        return new OIMO.Vec3().scale(this.position, OIMO.WORLD_SCALE);
    },
    getRotation:function(){
        return new OIMO.Euler().setFromRotationMatrix(this.rotation);
    },
    getQuaternion:function(){
        return new OIMO.Quaternion().setFromRotationMatrix(this.rotation);
    },
    getMatrix:function(){
        var m = this.matrix.elements;
        var r,p;
        if(!this.sleeping){
            // rotation matrix
            r = this.rotation.elements;
            m[0] = r[0]; m[1] = r[3]; m[2] = r[6];  m[3] = 0;
            m[4] = r[1]; m[5] = r[4]; m[6] = r[7];  m[7] = 0;
            m[8] = r[2]; m[9] = r[5]; m[10] = r[8]; m[11] = 0;

            // position matrix
            p = this.position;
            m[12] = p.x*OIMO.WORLD_SCALE;
            m[13] = p.y*OIMO.WORLD_SCALE;
            m[14] = p.z*OIMO.WORLD_SCALE;

            // sleep or not ?
            m[15] = 0;
        } else {
            m[15] = 1;
        }

        return m;
    }
}
/**
* The main class of body.
* is for simplify creation process and data access of rigidRody
* Rigid body has the shape of a single or multiple collision processing, 
* all setting in object
* 
* @author loth
*/
OIMO.Body = function(Obj){
    var obj = Obj || {};
    if(!obj.world) return;

    // the world where i am
    this.parent = obj.world;

    // Yep my name 
    this.name = obj.name || '';

    // I'm dynamique or not
    var move = obj.move || false;

    // I can sleep or not
    var noSleep  = obj.noSleep || false;
    
    // My start position
    var p = obj.pos || [0,0,0];
    p = p.map(function(x) { return x * OIMO.INV_SCALE; });

    // My size 
    var s = obj.size || [1,1,1];
    s = s.map(function(x) { return x * OIMO.INV_SCALE; });

    // My rotation in degre
    var rot = obj.rot || [0,0,0];
    rot = rot.map(function(x) { return x * OIMO.TO_RAD; });
    var r = [];
    for (var i=0; i<rot.length/3; i++){
        var tmp = OIMO.EulerToAxis(rot[i+0], rot[i+1], rot[i+2]);
        r.push(tmp[0]);  r.push(tmp[1]); r.push(tmp[2]); r.push(tmp[3]);
    }

    // My physics setting
    var sc = obj.sc || new OIMO.ShapeConfig();
    if(obj.config){
        // The density of the shape.
        sc.density = obj.config[0] || 1;
        // The coefficient of friction of the shape.
        sc.friction = obj.config[1] || 0.4;
        // The coefficient of restitution of the shape.
        sc.restitution = obj.config[2] || 0.2;
        // The bits of the collision groups to which the shape belongs.
        sc.belongsTo = obj.config[3] || 1;
        // The bits of the collision groups with which the shape collides.
        sc.collidesWith = obj.config[4] || 0xffffffff;
    }

    if(obj.massPos){
        obj.massPos = obj.massPos.map(function(x) { return x * OIMO.INV_SCALE; });
        sc.relativePosition.init(obj.massPos[0], obj.massPos[1], obj.massPos[2]);
    }
    if(obj.massRot){
        obj.massRot = obj.massRot.map(function(x) { return x * OIMO.TO_RAD; });
        sc.relativeRotation = OIMO.EulerToMatrix(obj.massRot[0], obj.massRot[1], obj.massRot[2]);
    }
    
    // My rigidbody
    this.body = new OIMO.RigidBody(p[0], p[1], p[2], r[0], r[1], r[2], r[3]);

    // My shapes
    var shapes = [];
    var type = obj.type || "box";
    if( typeof type === 'string' ) type = [type];// single shape

    var n, n2;
    for(var i=0; i<type.length; i++){
        n = i*3;
        n2 = i*4;
        switch(type[i]){
            case "sphere": shapes[i] = new OIMO.SphereShape(sc, s[n+0]); break;
            case "cylinder": shapes[i] = new OIMO.BoxShape(sc, s[n+0], s[n+1], s[n+2]); break; // fake cylinder
            case "box": shapes[i] = new OIMO.BoxShape(sc, s[n+0], s[n+1], s[n+2]); break;
        }
        this.body.addShape(shapes[i]);
        if(i>0){
            //shapes[i].position.init(p[0]+p[n+0], p[1]+p[n+1], p[2]+p[n+2] );
            shapes[i].relativePosition = new OIMO.Vec3( p[n+0], p[n+1], p[n+2] );
            if(r[n2+0]) shapes[i].relativeRotation = [ r[n2+0], r[n2+1], r[n2+2], r[n2+3] ];
        }
    } 
    
    // I'm static or i move
    if(move){
        if(obj.massPos || obj.massRot)this.body.setupMass(0x1, false);
        else this.body.setupMass(0x1, true);
        if(noSleep) this.body.allowSleep = false;
        else this.body.allowSleep = true;
    } else {
        this.body.setupMass(0x2);
    }
    
    this.body.name = this.name;
    this.sleeping = this.body.sleeping;

    // finaly add to physics world
    this.parent.addRigidBody(this.body);
}

OIMO.Body.prototype = {
    constructor: OIMO.Body,
    // SET
    setPosition:function(pos){
        this.body.setPosition(pos);
    },
    setQuaternion:function(q){
        this.body.setQuaternion(q);
    },
    setRotation:function(rot){
        this.body.setRotation(rot);
    },
    // GET
    getPosition:function(){
        return this.body.getPosition();
    },
    getRotation:function(){
        return this.body.getRotation();
    },
    getQuaternion:function(){
        return this.body.getQuaternion();
    },
    getMatrix:function(){
        return this.body.getMatrix();
    },
    getSleep:function(){
        return this.body.sleeping;
    },
    // RESET
    resetPosition:function(x,y,z){
        this.body.resetPosition(x,y,z);
    },
    resetRotation:function(x,y,z){
        this.body.resetRotation(x,y,z);
    },
    // force wakeup
    awake:function(){
        this.body.awake();
    },
    // remove rigidbody
    remove:function(){
        this.parent.removeRigidBody(this.body);
    },
    // test if this object hit another
    checkContact:function(name){
        this.parent.checkContact(this.name, name);
    }
}
/**
* The main class of link.
* is for simplify creation process and data access of Joint
* all setting in object
* 
* @author loth
*/
OIMO.Link = function(Obj){
    var obj = Obj || {};
    if(!obj.world) return;

    // the world where i am
    this.parent = obj.world;

    this.name = obj.name || '';
    var type = obj.type || "jointHinge";
    var axe1 = obj.axe1 || [1,0,0];
    var axe2 = obj.axe2 || [1,0,0];
    var pos1 = obj.pos1 || [0,0,0];
    var pos2 = obj.pos2 || [0,0,0];

    pos1 = pos1.map(function(x){ return x * OIMO.INV_SCALE; });
    pos2 = pos2.map(function(x){ return x * OIMO.INV_SCALE; });

    var min, max;
    if(type==="jointDistance"){
        min = obj.min || 0;
        max = obj.max || 10;
        min = min*OIMO.INV_SCALE;
        max = max*OIMO.INV_SCALE;
    }else{
        min = obj.min || 57.29578;
        max = obj.max || 0;
        min = min*OIMO.TO_RAD;
        max = max*OIMO.TO_RAD;
    }

    var limit = obj.limit || null;
    var spring = obj.spring || null;
    var motor = obj.motor || null;

    // joint setting
    var jc = new OIMO.JointConfig();
    jc.allowCollision = obj.collision || false;;
    jc.localAxis1.init(axe1[0], axe1[1], axe1[2]);
    jc.localAxis2.init(axe2[0], axe2[1], axe2[2]);
    jc.localAnchorPoint1.init(pos1[0], pos1[1], pos1[2]);
    jc.localAnchorPoint2.init(pos2[0], pos2[1], pos2[2]);
    if (typeof obj.body1 == 'string' || obj.body1 instanceof String) obj.body1 = obj.world.getByName(obj.body1);
    if (typeof obj.body2 == 'string' || obj.body2 instanceof String) obj.body2 = obj.world.getByName(obj.body2);
    jc.body1 = obj.body1;
    jc.body2 = obj.body2;

    
    switch(type){
        case "jointDistance": this.joint = new OIMO.DistanceJoint(jc, min, max); 
            if(spring !== null) this.joint.limitMotor.setSpring(spring[0], spring[1]);
            if(motor !== null) this.joint.limitMotor.setSpring(motor[0], motor[1]);
        break;
        case "jointHinge": this.joint = new OIMO.HingeJoint(jc, min, max);
            if(spring !== null) this.joint.limitMotor.setSpring(spring[0], spring[1]);// soften the joint ex: 100, 0.2
            if(motor !== null) this.joint.limitMotor.setSpring(motor[0], motor[1]);
        break;
        case "jointPrisme": this.joint = new OIMO.PrismaticJoint(jc, min, max); break;
        case "jointSlide": this.joint = new OIMO.SliderJoint(jc, min, max); break;
        case "jointBall": this.joint = new OIMO.BallAndSocketJoint(jc); break;
        case "jointWheel": this.joint = new OIMO.WheelJoint(jc);  
            if(limit !== null) this.joint.rotationalLimitMotor1.setLimit(limit[0], limit[1]);
            if(spring !== null) this.joint.rotationalLimitMotor1.setSpring(spring[0], spring[1]);
            if(motor !== null) this.joint.rotationalLimitMotor1.setSpring(motor[0], motor[1]);
        break;
    }

    this.joint.name = this.name;
    
    // finaly add to physics world
    this.parent.addJoint(this.joint);
}

OIMO.Link.prototype = {
    constructor: OIMO.Link,
    getPosition:function(){
        // array of two vect3 [point1, point2]
        return this.joint.getPosition();
    },
    getMatrix:function(){
        return this.joint.getMatrix();
    },
    // remove joint
    remove:function(){
        this.parent.removeJoint(this.joint);
    },
    // force wakeup linked body
    awake:function(){
        this.joint.awake();
    }
}
OIMO.Performance = function(world){
	this.parent = world;
	this.infos = new OIMO_ARRAY_TYPE(13);
	this.f = [0,0,0];

    this.fps=0;
    this.broadPhaseTime=0;
    this.narrowPhaseTime=0;
    this.solvingTime=0;
    this.updatingTime=0;
    this.totalTime=0;
}
OIMO.Performance.prototype = {
	upfps : function(){
		this.f[1] = Date.now();
        if (this.f[1]-1000>this.f[0]){ this.f[0]=this.f[1]; this.fps=this.f[2]; this.f[2]=0; } this.f[2]++;
	},
	show : function(){
		var info =[
            "Oimo.js DEV.1.1.2a<br><br>",
            "FPS: " + this.fps +" fps<br><br>",
            "rigidbody "+this.parent.numRigidBodies+"<br>",
            "contact &nbsp;&nbsp;"+this.parent.numContacts+"<br>",
            "paircheck "+this.parent.broadPhase.numPairChecks+"<br>",
            "contact &nbsp;&nbsp;"+this.parent.numContactPoints+"<br>",
            "island &nbsp;&nbsp;&nbsp;"+this.parent.numIslands +"<br><br>",
            "Time in ms <br>",
            "broad-phase &nbsp;"+this.broadPhaseTime + "<br>",
            "narrow-phase "+this.narrowPhaseTime + "<br>",
            "solving &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;"+this.solvingTime + "<br>",
            "updating &nbsp;&nbsp;&nbsp;&nbsp;"+this.updatingTime + "<br>",
            "total &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;"+this.totalTime
        ].join("\n");
        return info;
	},
	toArray : function(){
		this.infos[0] = this.parent.broadPhase.types;
	    this.infos[1] = this.parent.numRigidBodies;
	    this.infos[2] = this.parent.numContacts;
	    this.infos[3] = this.parent.broadPhase.numPairChecks;
	    this.infos[4] = this.parent.numContactPoints;
	    this.infos[5] = this.parent.numIslands;
	    this.infos[6] = this.broadPhaseTime;
	    this.infos[7] = this.narrowPhaseTime;
	    this.infos[8] = this.solvingTime;
	    this.infos[9] = this.updatingTime;
	    this.infos[10] = this.totalTime;
	    this.infos[11] = this.fps;
		return this.infos;
	}
}
OIMO.Mat44 = function(n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34, n41, n42, n43, n44){
    this.elements = new OIMO_ARRAY_TYPE(16);
    var te = this.elements;
    te[0] = ( n11 !== undefined ) ? n11 : 1; te[4] = n12 || 0; te[8] = n13 || 0; te[12] = n14 || 0;
    te[1] = n21 || 0; te[5] = ( n22 !== undefined ) ? n22 : 1; te[9] = n23 || 0; te[13] = n24 || 0;
    te[2] = n31 || 0; te[6] = n32 || 0; te[10] = ( n33 !== undefined ) ? n33 : 1; te[14] = n34 || 0;
    te[3] = n41 || 0; te[7] = n42 || 0; te[11] = n43 || 0; te[15] = ( n44 !== undefined ) ? n44 : 1;
};

OIMO.Mat44.prototype = {
    constructor: OIMO.Mat44,

    set: function ( n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34, n41, n42, n43, n44 ) {

        var te = this.elements;

        te[0] = n11; te[4] = n12; te[8] = n13; te[12] = n14;
        te[1] = n21; te[5] = n22; te[9] = n23; te[13] = n24;
        te[2] = n31; te[6] = n32; te[10] = n33; te[14] = n34;
        te[3] = n41; te[7] = n42; te[11] = n43; te[15] = n44;

        return this;
    }/*,
    extractRotation: function () {

        var v1 = new THREE.Vector3();

        return function ( m ) {

            var te = this.elements;
            var me = m.elements;

            var scaleX = 1 / v1.set( me[ 0 ], me[ 1 ], me[ 2 ] ).length();
            var scaleY = 1 / v1.set( me[ 4 ], me[ 5 ], me[ 6 ] ).length();
            var scaleZ = 1 / v1.set( me[ 8 ], me[ 9 ], me[ 10 ] ).length();

            te[ 0 ] = me[ 0 ] * scaleX;
            te[ 1 ] = me[ 1 ] * scaleX;
            te[ 2 ] = me[ 2 ] * scaleX;

            te[ 4 ] = me[ 4 ] * scaleY;
            te[ 5 ] = me[ 5 ] * scaleY;
            te[ 6 ] = me[ 6 ] * scaleY;

            te[ 8 ] = me[ 8 ] * scaleZ;
            te[ 9 ] = me[ 9 ] * scaleZ;
            te[ 10 ] = me[ 10 ] * scaleZ;

            return this;

        };

    }() */
}
OIMO.Mat33 = function(e00,e01,e02,e10,e11,e12,e20,e21,e22){
    this.elements = new OIMO_ARRAY_TYPE(9);
    var te = this.elements;

    this.init(
        ( e00 !== undefined ) ? e00 : 1, e01 || 0, e02 || 0,
        e10 || 0, ( e11 !== undefined ) ? e11 : 1, e12 || 0,
        e20 || 0, e21 || 0, ( e22 !== undefined ) ? e22 : 1
    );
};

OIMO.Mat33.prototype = {
    constructor: OIMO.Mat33,

    init: function(e00,e01,e02,e10,e11,e12,e20,e21,e22){
        var te = this.elements;
        te[0] = e00; te[1] = e01; te[2] = e02;
        te[3] = e10; te[4] = e11; te[5] = e12;
        te[6] = e20; te[7] = e21; te[8] = e22;
        return this;
    },
    add: function(m1,m2){
        var te = this.elements, tem1 = m1.elements, tem2 = m2.elements;
        te[0] = tem1[0] + tem2[0]; te[1] = tem1[1] + tem2[1]; te[2] = tem1[2] + tem2[2];
        te[3] = tem1[3] + tem2[3]; te[4] = tem1[4] + tem2[4]; te[5] = tem1[5] + tem2[5];
        te[6] = tem1[6] + tem2[6]; te[7] = tem1[7] + tem2[7]; te[8] = tem1[8] + tem2[8];
        return this;
    },
    addEqual: function(m){
        var te = this.elements, tem = m.elements;
        te[0] += tem[0]; te[1] += tem[1]; te[2] += tem[2];
        te[3] += tem[3]; te[4] += tem[4]; te[5] += tem[5];
        te[6] += tem[6]; te[7] += tem[7]; te[8] += tem[8];
        return this;
    },
    sub: function(m1,m2){
        var te = this.elements, tem1 = m1.elements, tem2 = m2.elements;
        te[0] = tem1[0] - tem2[0]; te[1] = tem1[1] - tem2[1]; te[2] = tem1[2] - tem2[2];
        te[3] = tem1[3] - tem2[3]; te[4] = tem1[4] - tem2[4]; te[5] = tem1[5] - tem2[5];
        te[6] = tem1[6] - tem2[6]; te[7] = tem1[7] - tem2[7]; te[8] = tem1[8] - tem2[8];
        return this;
    },
    subEqual:function(m){
        var te = this.elements, tem = m.elements;
        te[0] -= tem[0]; te[1] -= tem[1]; te[2] -= tem[2];
        te[3] -= tem[3]; te[4] -= tem[4]; te[5] -= tem[5];
        te[6] -= tem[6]; te[7] -= tem[7]; te[8] -= tem[8];
        return this;
    },
    scale: function(m,s){
        var te = this.elements, tm = m.elements;
        te[0] = tm[0] * s; te[1] = tm[1] * s; te[2] = tm[2] * s;
        te[3] = tm[3] * s; te[4] = tm[4] * s; te[5] = tm[5] * s;
        te[6] = tm[6] * s; te[7] = tm[7] * s; te[8] = tm[8] * s;
        return this;
    },
    scaleEqual: function(s){
        var te = this.elements;
        te[0] *= s; te[1] *= s; te[2] *= s;
        te[3] *= s; te[4] *= s; te[5] *= s;
        te[6] *= s; te[7] *= s; te[8] *= s;
        return this;
    },
    mul: function(m1,m2){
        var te = this.elements, tm1 = m1.elements, tm2 = m2.elements,
        a0 = tm1[0], a3 = tm1[3], a6 = tm1[6],
        a1 = tm1[1], a4 = tm1[4], a7 = tm1[7],
        a2 = tm1[2], a5 = tm1[5], a8 = tm1[8],
        b0 = tm2[0], b3 = tm2[3], b6 = tm2[6],
        b1 = tm2[1], b4 = tm2[4], b7 = tm2[7],
        b2 = tm2[2], b5 = tm2[5], b8 = tm2[8];
        te[0] = a0*b0 + a1*b3 + a2*b6;
        te[1] = a0*b1 + a1*b4 + a2*b7;
        te[2] = a0*b2 + a1*b5 + a2*b8;
        te[3] = a3*b0 + a4*b3 + a5*b6;
        te[4] = a3*b1 + a4*b4 + a5*b7;
        te[5] = a3*b2 + a4*b5 + a5*b8;
        te[6] = a6*b0 + a7*b3 + a8*b6;
        te[7] = a6*b1 + a7*b4 + a8*b7;
        te[8] = a6*b2 + a7*b5 + a8*b8;
        return this;
    },
    mulScale: function(m,sx,sy,sz,Prepend){
        var prepend = Prepend || false;
        var te = this.elements, tm = m.elements;
        if(prepend){
            te[0] = sx*tm[0]; te[1] = sx*tm[1]; te[2] = sx*tm[2];
            te[3] = sy*tm[3]; te[4] = sy*tm[4]; te[5] = sy*tm[5];
            te[6] = sz*tm[6]; te[7] = sz*tm[7]; te[8] = sz*tm[8];
        }else{
            te[0] = tm[0]*sx; te[1] = tm[1]*sy; te[2] = tm[2]*sz;
            te[3] = tm[3]*sx; te[4] = tm[4]*sy; te[5] = tm[5]*sz;
            te[6] = tm[6]*sx; te[7] = tm[7]*sy; te[8] = tm[8]*sz;
        }
        return this;
    },
    mulRotate: function(m,rad,ax,ay,az,Prepend){
        var prepend = Prepend || false;
        var s=Math.sin(rad);
        var c=Math.cos(rad);
        var c1=1-c;
        var r00=ax*ax*c1+c;
        var r01=ax*ay*c1-az*s;
        var r02=ax*az*c1+ay*s;
        var r10=ay*ax*c1+az*s;
        var r11=ay*ay*c1+c;
        var r12=ay*az*c1-ax*s;
        var r20=az*ax*c1-ay*s;
        var r21=az*ay*c1+ax*s;
        var r22=az*az*c1+c;

        var tm = m.elements;

        var a0 = tm[0], a3 = tm[3], a6 = tm[6];
        var a1 = tm[1], a4 = tm[4], a7 = tm[7];
        var a2 = tm[2], a5 = tm[5], a8 = tm[8];

        var te = this.elements;
        
        if(prepend){
            te[0]=r00*a0+r01*a3+r02*a6;
            te[1]=r00*a1+r01*a4+r02*a7;
            te[2]=r00*a2+r01*a5+r02*a8;
            te[3]=r10*a0+r11*a3+r12*a6;
            te[4]=r10*a1+r11*a4+r12*a7;
            te[5]=r10*a2+r11*a5+r12*a8;
            te[6]=r20*a0+r21*a3+r22*a6;
            te[7]=r20*a1+r21*a4+r22*a7;
            te[8]=r20*a2+r21*a5+r22*a8;
        }else{
            te[0]=a0*r00+a1*r10+a2*r20;
            te[1]=a0*r01+a1*r11+a2*r21;
            te[2]=a0*r02+a1*r12+a2*r22;
            te[3]=a3*r00+a4*r10+a5*r20;
            te[4]=a3*r01+a4*r11+a5*r21;
            te[5]=a3*r02+a4*r12+a5*r22;
            te[6]=a6*r00+a7*r10+a8*r20;
            te[7]=a6*r01+a7*r11+a8*r21;
            te[8]=a6*r02+a7*r12+a8*r22;
        }
        return this;
    },
    transpose: function(m){
        var te = this.elements, tm = m.elements;
        te[0] = tm[0]; te[1] = tm[3]; te[2] = tm[6];
        te[3] = tm[1]; te[4] = tm[4]; te[5] = tm[7];
        te[6] = tm[2]; te[7] = tm[5]; te[8] = tm[8];
        return this;
    },
    setQuat: function(q){
        var te = this.elements,
        x2=2*q.x,  y2=2*q.y,  z2=2*q.z,
        xx=q.x*x2, yy=q.y*y2, zz=q.z*z2,
        xy=q.x*y2, yz=q.y*z2, xz=q.x*z2,
        sx=q.s*x2, sy=q.s*y2, sz=q.s*z2;
        
        te[0]=1-yy-zz;
        te[1]=xy-sz;
        te[2]=xz+sy;
        te[3]=xy+sz;
        te[4]=1-xx-zz;
        te[5]=yz-sx;
        te[6]=xz-sy;
        te[7]=yz+sx;
        te[8]=1-xx-yy;
        return this;
    },
    invert: function(m){
        var te = this.elements, tm = m.elements,
        a0 = tm[0], a3 = tm[3], a6 = tm[6],
        a1 = tm[1], a4 = tm[4], a7 = tm[7],
        a2 = tm[2], a5 = tm[5], a8 = tm[8],
        b01 = a4*a8-a7*a5,
        b11 = a7*a2-a1*a8,
        b21 = a1*a5-a4*a2,
        dt= a0 * (b01) + a3 * (b11) + a6 * (b21);

        if(dt!=0){dt=1.0/dt;}
        te[0] = dt*b01;//(a4*a8 - a5*a7);
        te[1] = dt*b11;//(a2*a7 - a1*a8);
        te[2] = dt*b21;//(a1*a5 - a2*a4);
        te[3] = dt*(a5*a6 - a3*a8);
        te[4] = dt*(a0*a8 - a2*a6);
        te[5] = dt*(a2*a3 - a0*a5);
        te[6] = dt*(a3*a7 - a4*a6);
        te[7] = dt*(a1*a6 - a0*a7);
        te[8] = dt*(a0*a4 - a1*a3);
        return this;
    },
    copy: function(m){
        var te = this.elements, tem = m.elements;
        te[0] = tem[0]; te[1] = tem[1]; te[2] = tem[2];
        te[3] = tem[3]; te[4] = tem[4]; te[5] = tem[5];
        te[6] = tem[6]; te[7] = tem[7]; te[8] = tem[8];
        return this;
    },
    toEuler: function(){ // not work !!
        function clamp( x ) {
            return Math.min( Math.max( x, -1 ), 1 );
        }
        var te = this.elements;
        var m11 = te[0], m12 = te[3], m13 = te[6];
        var m21 = te[1], m22 = te[4], m23 = te[7];
        var m31 = te[2], m32 = te[5], m33 = te[8];

        var p = new OIMO.Vec3();
        var d = new OIMO.Quat();
        var s;

        p.y = Math.asin( clamp( m13 ) );

        if ( Math.abs( m13 ) < 0.99999 ) {
            p.x = Math.atan2( - m23, m33 );
            p.z = Math.atan2( - m12, m11 );
        } else {
            p.x = Math.atan2( m32, m22 );
            p.z = 0;
        }
        
        return p;
    },
    clone: function(){
        var te = this.elements;

        return new OIMO.Mat33(
            te[0], te[1], te[2],
            te[3], te[4], te[5],
            te[6], te[7], te[8]
        );
    },
    toString: function(){
        var te = this.elements;
        var text=
        "Mat33|"+te[0].toFixed(4)+", "+te[1].toFixed(4)+", "+te[2].toFixed(4)+"|\n"+
        "     |"+te[3].toFixed(4)+", "+te[4].toFixed(4)+", "+te[5].toFixed(4)+"|\n"+
        "     |"+te[6].toFixed(4)+", "+te[7].toFixed(4)+", "+te[8].toFixed(4)+"|" ;
        return text;
    }
};
OIMO.Quat = function( s, x, y, z){
    this.s=( s !== undefined ) ? s : 1;
    this.x=x || 0;
    this.y=y || 0;
    this.z=z || 0;
};

OIMO.Quat.prototype = {

    constructor: OIMO.Quat,

    init: function(s,x,y,z){
        this.s=( s !== undefined ) ? s : 1;
        this.x=x || 0;
        this.y=y || 0;
        this.z=z || 0;
        return this;
    },
    add: function(q1,q2){
        this.s=q1.s+q2.s;
        this.x=q1.x+q2.x;
        this.y=q1.y+q2.y;
        this.z=q1.z+q2.z;
        return this;
    },
    addTime: function(v,t){
        var x = v.x;
        var y = v.y;
        var z = v.z;
        var qs=this.s;
        var qx=this.x;
        var qy=this.y;
        var qz=this.z;
        t*=0.5;
        var ns=(-x*qx - y*qy - z*qz)*t;
        var nx=( x*qs + y*qz - z*qy)*t;
        var ny=(-x*qz + y*qs + z*qx)*t;
        var nz=( x*qy - y*qx + z*qs)*t;
        qs += ns;
        qx += nx;
        qy += ny;
        qz += nz;
        var s=1/Math.sqrt(qs*qs+qx*qx+qy*qy+qz*qz);
        this.s=qs*s;
        this.x=qx*s;
        this.y=qy*s;
        this.z=qz*s;
        return this;
    },
    sub: function(q1,q2){
        this.s=q1.s-q2.s;
        this.x=q1.x-q2.x;
        this.y=q1.y-q2.y;
        this.z=q1.z-q2.z;
        return this;
    },
    scale: function(q,s){
        this.s=q.s*s;
        this.x=q.x*s;
        this.y=q.y*s;
        this.z=q.z*s;
        return this;
    },
    mul: function(q1,q2){
        var ax = q1.x, ay = q1.y, az = q1.z, as = q1.s,
        bx = q2.x, by = q2.y, bz = q2.z, bs = q2.s;
        this.x = ax * bs + as * bx + ay * bz - az * by;
        this.y = ay * bs + as * by + az * bx - ax * bz;
        this.z = az * bs + as * bz + ax * by - ay * bx;
        this.s = as * bs - ax * bx - ay * by - az * bz;
        return this;
    },
    arc: function(v1,v2){
        var x1=v1.x;
        var y1=v1.y;
        var z1=v1.z;
        var x2=v2.x;
        var y2=v2.y;
        var z2=v2.z;
        var d=x1*x2+y1*y2+z1*z2;
        if(d==-1){
        x2=y1*x1-z1*z1;
        y2=-z1*y1-x1*x1;
        z2=x1*z1+y1*y1;
        d=1/Math.sqrt(x2*x2+y2*y2+z2*z2);
        this.s=0;
        this.x=x2*d;
        this.y=y2*d;
        this.z=z2*d;
        return this;
        }
        var cx=y1*z2-z1*y2;
        var cy=z1*x2-x1*z2;
        var cz=x1*y2-y1*x2;
        this.s=Math.sqrt((1+d)*0.5);
        d=0.5/this.s;
        this.x=cx*d;
        this.y=cy*d;
        this.z=cz*d;
        return this;
    },
    normalize: function(q){
        var len=Math.sqrt(q.s*q.s+q.x*q.x+q.y*q.y+q.z*q.z);
        if(len>0){len=1/len;}
        this.s=q.s*len;
        this.x=q.x*len;
        this.y=q.y*len;
        this.z=q.z*len;
        return this;
    },
    invert: function(q){
        this.s=q.s;
        this.x=-q.x;
        this.y=-q.y;
        this.z=-q.z;
        return this;
    },
    length: function(){
        return Math.sqrt(this.s*this.s+this.x*this.x+this.y*this.y+this.z*this.z);
    },
    
    copy: function(q){
        this.s=q.s;
        this.x=q.x;
        this.y=q.y;
        this.z=q.z;
        return this;
    },
    testDiff: function(q){
        if(this.s!==q.s || this.x!==q.x || this.y!==q.y || this.z!==q.z) return true;
        else return false;
    },
    clone: function(q){
        return new OIMO.Quat(this.s,this.x,this.y,this.z);
    },
    toString: function(){
        return"Quat["+this.s.toFixed(4)+", ("+this.x.toFixed(4)+", "+this.y.toFixed(4)+", "+this.z.toFixed(4)+")]";
    }
}


// for three easy export
OIMO.Quaternion = function ( x, y, z, w ) {
    this.x = x || 0;
    this.y = y || 0;
    this.z = z || 0;
    this.w = ( w !== undefined ) ? w : 1;
};

OIMO.Quaternion.prototype = {

    constructor: OIMO.Quaternion,
    setFromRotationMatrix: function ( m ) {

        // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

        // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)

        var te = m.elements,

            m11 = te[ 0 ], m12 = te[ 1 ], m13 = te[ 2 ],
            m21 = te[ 3 ], m22 = te[ 4 ], m23 = te[ 5 ],
            m31 = te[ 6 ], m32 = te[ 7 ], m33 = te[ 8 ],

            trace = m11 + m22 + m33,
            s;

        if ( trace > 0 ) {

            s = 0.5 / Math.sqrt( trace + 1.0 );

            this.w = 0.25 / s;
            this.x = ( m32 - m23 ) * s;
            this.y = ( m13 - m31 ) * s;
            this.z = ( m21 - m12 ) * s;

        } else if ( m11 > m22 && m11 > m33 ) {

            s = 2.0 * Math.sqrt( 1.0 + m11 - m22 - m33 );

            this.w = ( m32 - m23 ) / s;
            this.x = 0.25 * s;
            this.y = ( m12 + m21 ) / s;
            this.z = ( m13 + m31 ) / s;

        } else if ( m22 > m33 ) {

            s = 2.0 * Math.sqrt( 1.0 + m22 - m11 - m33 );

            this.w = ( m13 - m31 ) / s;
            this.x = ( m12 + m21 ) / s;
            this.y = 0.25 * s;
            this.z = ( m23 + m32 ) / s;

        } else {

            s = 2.0 * Math.sqrt( 1.0 + m33 - m11 - m22 );

            this.w = ( m21 - m12 ) / s;
            this.x = ( m13 + m31 ) / s;
            this.y = ( m23 + m32 ) / s;
            this.z = 0.25 * s;

        }

        //this.onChangeCallback();

        return this;

    }
}
OIMO.Vec3 = function(x,y,z){
    this.x=x || 0;
    this.y=y || 0;
    this.z=z || 0;
};

OIMO.Vec3.prototype = {

    constructor: OIMO.Vec3,

    init: function(x,y,z){
        this.x=x || 0;
        this.y=y || 0;
        this.z=z || 0;
        return this;
    },
    set: function(x,y,z){
        this.x=x;
        this.y=y;
        this.z=z;
        return this;
    },
    add: function(v1,v2){
        this.x=v1.x+v2.x;
        this.y=v1.y+v2.y;
        this.z=v1.z+v2.z;
        return this;
    },
    addEqual: function(v){
        this.x+=v.x;
        this.y+=v.y;
        this.z+=v.z;
        return this;
    },
    addTime: function(v, t){
        this.x+=v.x*t;
        this.y+=v.y*t;
        this.z+=v.z*t;
        return this;
    },
    sub: function(v1,v2){
        this.x=v1.x-v2.x;
        this.y=v1.y-v2.y;
        this.z=v1.z-v2.z;
        return this;
    },
    subEqual: function(v){
        this.x-=v.x;
        this.y-=v.y;
        this.z-=v.z;
        return this;
    },
    addScale: function(v,s){
        this.x+=v.x*s;
        this.y+=v.y*s;
        this.z+=v.z*s;
        return this;
    },
    scale: function(v,s){
        this.x=v.x*s;
        this.y=v.y*s;
        this.z=v.z*s;
        return this;
    },
    scaleEqual: function(s){
        this.x*=s;
        this.y*=s;
        this.z*=s;
        return this;
    },
    dot: function(v){
        return this.x*v.x+this.y*v.y+this.z*v.z;
    },
    cross: function(v1,v2){
        var ax = v1.x, ay = v1.y, az = v1.z, 
        bx = v2.x, by = v2.y, bz = v2.z;
        this.x = ay * bz - az * by;
        this.y = az * bx - ax * bz;
        this.z = ax * by - ay * bx;
        return this;
    },
    mul: function(o, v, m){
        var te = m.elements;
        this.x=o.x+v.x*te[0]+v.y*te[1]+v.z*te[2];
        this.y=o.y+v.x*te[3]+v.y*te[4]+v.z*te[5];
        this.z=o.z+v.x*te[6]+v.y*te[7]+v.z*te[8];
        return this;
    },
    mulMat: function(m,v){
        var te = m.elements;
        var x=te[0]*v.x+te[1]*v.y+te[2]*v.z;
        var y=te[3]*v.x+te[4]*v.y+te[5]*v.z;
        var z=te[6]*v.x+te[7]*v.y+te[8]*v.z;
        this.x=x;
        this.y=y;
        this.z=z;
        return this;
    },
    normalize: function(v){
        var x = v.x, y = v.y, z = v.z;
        var l = x*x + y*y + z*z;
        if (l > 0) {
            l = 1 / Math.sqrt(l);
            this.x = x*l;
            this.y = y*l;
            this.z = z*l;
        }
        return this;
    },
    invert: function(v){
        this.x=-v.x;
        this.y=-v.y;
        this.z=-v.z;
        return this;
    },
    length: function(){
        var x = this.x, y = this.y, z = this.z;
        return Math.sqrt(x*x + y*y + z*z);
    },
    len: function(){
        var x = this.x, y = this.y, z = this.z;
        return x*x + y*y + z*z;
    },
    copy: function(v){
        this.x=v.x;
        this.y=v.y;
        this.z=v.z;
        return this;
    },
    applyQuaternion: function ( q ) {

        var x = this.x;
        var y = this.y;
        var z = this.z;

        var qx = q.x;
        var qy = q.y;
        var qz = q.z;
        var qs = q.s;

        // calculate quat * vector

        var ix =  qs * x + qy * z - qz * y;
        var iy =  qs * y + qz * x - qx * z;
        var iz =  qs * z + qx * y - qy * x;
        var iw = - qx * x - qy * y - qz * z;

        // calculate result * inverse quat

        this.x = ix * qs + iw * - qx + iy * - qz - iz * - qy;
        this.y = iy * qs + iw * - qy + iz * - qx - ix * - qz;
        this.z = iz * qs + iw * - qz + ix * - qy - iy * - qx;

        return this;

    },
    testZero: function(){
        if(this.x!==0 || this.y!==0 || this.z!==0) return true;
        else return false;
    },
    testDiff: function(v){
        if(this.x!==v.x || this.y!==v.y || this.z!==v.z) return true;
        else return false;
    },
    clone: function(){
        return new OIMO.Vec3(this.x,this.y,this.z);
    },
    toString: function(){
        return"Vec3["+this.x.toFixed(4)+", "+this.y.toFixed(4)+", "+this.z.toFixed(4)+"]";
    }
}
OIMO.Euler = function ( x, y, z, order ) {

	this._x = x || 0;
	this._y = y || 0;
	this._z = z || 0;
	this._order = order || OIMO.Euler.DefaultOrder;

};

OIMO.Euler.RotationOrders = [ 'XYZ', 'YZX', 'ZXY', 'XZY', 'YXZ', 'ZYX' ];

OIMO.Euler.DefaultOrder = 'XYZ';

OIMO.clamp =  function ( x, a, b ) {
	return ( x < a ) ? a : ( ( x > b ) ? b : x );
}

OIMO.Euler.prototype = {

	constructor: OIMO.Euler,

	_x: 0, _y: 0, _z: 0, _order: OIMO.Euler.DefaultOrder,

	get x () {

		return this._x;

	},

	set x ( value ) {

		this._x = value;
		this.onChangeCallback();

	},

	get y () {

		return this._y;

	},

	set y ( value ) {

		this._y = value;
		this.onChangeCallback();

	},

	get z () {

		return this._z;

	},

	set z ( value ) {

		this._z = value;
		this.onChangeCallback();

	},

	get order () {

		return this._order;

	},

	set order ( value ) {

		this._order = value;
		this.onChangeCallback();

	},

	set: function ( x, y, z, order ) {

		this._x = x;
		this._y = y;
		this._z = z;
		this._order = order || this._order;

		this.onChangeCallback();

		return this;

	},

	copy: function ( euler ) {

		this._x = euler._x;
		this._y = euler._y;
		this._z = euler._z;
		this._order = euler._order;

		this.onChangeCallback();

		return this;

	},

	setFromRotationMatrix: function ( m, order ) {

		var clamp = OIMO.clamp;

		// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)

		var te = m.elements;
		/*var m11 = te[ 0 ], m12 = te[ 4 ], m13 = te[ 8 ];
		var m21 = te[ 1 ], m22 = te[ 5 ], m23 = te[ 9 ];
		var m31 = te[ 2 ], m32 = te[ 6 ], m33 = te[ 10 ];*/

		var m11 = te[ 0 ], m12 = te[ 1 ], m13 = te[ 2 ],
            m21 = te[ 3 ], m22 = te[ 4 ], m23 = te[ 5 ],
            m31 = te[ 6 ], m32 = te[ 7 ], m33 = te[ 8 ];

		order = order || this._order;

		if ( order === 'XYZ' ) {

			this._y = Math.asin( clamp( m13, - 1, 1 ) );

			if ( Math.abs( m13 ) < 0.99999 ) {

				this._x = Math.atan2( - m23, m33 );
				this._z = Math.atan2( - m12, m11 );

			} else {

				this._x = Math.atan2( m32, m22 );
				this._z = 0;

			}

		} else if ( order === 'YXZ' ) {

			this._x = Math.asin( - clamp( m23, - 1, 1 ) );

			if ( Math.abs( m23 ) < 0.99999 ) {

				this._y = Math.atan2( m13, m33 );
				this._z = Math.atan2( m21, m22 );

			} else {

				this._y = Math.atan2( - m31, m11 );
				this._z = 0;

			}

		} else if ( order === 'ZXY' ) {

			this._x = Math.asin( clamp( m32, - 1, 1 ) );

			if ( Math.abs( m32 ) < 0.99999 ) {

				this._y = Math.atan2( - m31, m33 );
				this._z = Math.atan2( - m12, m22 );

			} else {

				this._y = 0;
				this._z = Math.atan2( m21, m11 );

			}

		} else if ( order === 'ZYX' ) {

			this._y = Math.asin( - clamp( m31, - 1, 1 ) );

			if ( Math.abs( m31 ) < 0.99999 ) {

				this._x = Math.atan2( m32, m33 );
				this._z = Math.atan2( m21, m11 );

			} else {

				this._x = 0;
				this._z = Math.atan2( - m12, m22 );

			}

		} else if ( order === 'YZX' ) {

			this._z = Math.asin( clamp( m21, - 1, 1 ) );

			if ( Math.abs( m21 ) < 0.99999 ) {

				this._x = Math.atan2( - m23, m22 );
				this._y = Math.atan2( - m31, m11 );

			} else {

				this._x = 0;
				this._y = Math.atan2( m13, m33 );

			}

		} else if ( order === 'XZY' ) {

			this._z = Math.asin( - clamp( m12, - 1, 1 ) );

			if ( Math.abs( m12 ) < 0.99999 ) {

				this._x = Math.atan2( m32, m22 );
				this._y = Math.atan2( m13, m11 );

			} else {

				this._x = Math.atan2( - m23, m33 );
				this._y = 0;

			}

		} else {

			console.warn( 'THREE.Euler: .setFromRotationMatrix() given unsupported order: ' + order )

		}

		this._order = order;

		this.onChangeCallback();

		return this;

	},

	setFromQuaternion: function ( q, order, update ) {

		var clamp = OIMO.clamp;

		// q is assumed to be normalized

		// http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m

		var sqx = q.x * q.x;
		var sqy = q.y * q.y;
		var sqz = q.z * q.z;
		var sqw = q.s * q.s;

		order = order || this._order;

		if ( order === 'XYZ' ) {

			this._x = Math.atan2( 2 * ( q.x * q.s - q.y * q.z ), ( sqw - sqx - sqy + sqz ) );
			this._y = Math.asin(  clamp( 2 * ( q.x * q.z + q.y * q.s ), - 1, 1 ) );
			this._z = Math.atan2( 2 * ( q.z * q.s - q.x * q.y ), ( sqw + sqx - sqy - sqz ) );

		} else if ( order ===  'YXZ' ) {

			this._x = Math.asin(  clamp( 2 * ( q.x * q.s - q.y * q.z ), - 1, 1 ) );
			this._y = Math.atan2( 2 * ( q.x * q.z + q.y * q.s ), ( sqw - sqx - sqy + sqz ) );
			this._z = Math.atan2( 2 * ( q.x * q.y + q.z * q.s ), ( sqw - sqx + sqy - sqz ) );

		} else if ( order === 'ZXY' ) {

			this._x = Math.asin(  clamp( 2 * ( q.x * q.s + q.y * q.z ), - 1, 1 ) );
			this._y = Math.atan2( 2 * ( q.y * q.s - q.z * q.x ), ( sqw - sqx - sqy + sqz ) );
			this._z = Math.atan2( 2 * ( q.z * q.s - q.x * q.y ), ( sqw - sqx + sqy - sqz ) );

		} else if ( order === 'ZYX' ) {

			this._x = Math.atan2( 2 * ( q.x * q.s + q.z * q.y ), ( sqw - sqx - sqy + sqz ) );
			this._y = Math.asin(  clamp( 2 * ( q.y * q.s - q.x * q.z ), - 1, 1 ) );
			this._z = Math.atan2( 2 * ( q.x * q.y + q.z * q.s ), ( sqw + sqx - sqy - sqz ) );

		} else if ( order === 'YZX' ) {

			this._x = Math.atan2( 2 * ( q.x * q.s - q.z * q.y ), ( sqw - sqx + sqy - sqz ) );
			this._y = Math.atan2( 2 * ( q.y * q.s - q.x * q.z ), ( sqw + sqx - sqy - sqz ) );
			this._z = Math.asin(  clamp( 2 * ( q.x * q.y + q.z * q.s ), - 1, 1 ) );

		} else if ( order === 'XZY' ) {

			this._x = Math.atan2( 2 * ( q.x * q.s + q.y * q.z ), ( sqw - sqx + sqy - sqz ) );
			this._y = Math.atan2( 2 * ( q.x * q.z + q.y * q.s ), ( sqw + sqx - sqy - sqz ) );
			this._z = Math.asin(  clamp( 2 * ( q.z * q.s - q.x * q.y ), - 1, 1 ) );

		} else {

			console.warn( 'OIMO.Euler: .setFromQuaternion() given unsupported order: ' + order )

		}

		this._order = order;

		if ( update !== false ) this.onChangeCallback();

		return this;

	},

	reorder: function () {

		// WARNING: this discards revolution information -bhouston

		var q = new OIMO.Quat();

		return function ( newOrder ) {

			q.setFromEuler( this );
			this.setFromQuaternion( q, newOrder );

		};


	}(),

	equals: function ( euler ) {

		return ( euler._x === this._x ) && ( euler._y === this._y ) && ( euler._z === this._z ) && ( euler._order === this._order );

	},

	fromArray: function ( array ) {

		this._x = array[ 0 ];
		this._y = array[ 1 ];
		this._z = array[ 2 ];
		if ( array[ 3 ] !== undefined ) this._order = array[ 3 ];

		this.onChangeCallback();

		return this;

	},

	toArray: function () {

		return [ this._x, this._y, this._z, this._order ];

	},

	onChange: function ( callback ) {

		this.onChangeCallback = callback;

		return this;

	},

	onChangeCallback: function () {},

	clone: function () {

		return new OIMO.Euler( this._x, this._y, this._z, this._order );

	}

};

OIMO.EulerToAxis = function( ox, oy, oz ){// angles in radians
    var c1 = Math.cos(oy*0.5);//heading
    var s1 = Math.sin(oy*0.5);
    var c2 = Math.cos(oz*0.5);//altitude
    var s2 = Math.sin(oz*0.5);
    var c3 = Math.cos(ox*0.5);//bank
    var s3 = Math.sin(ox*0.5);
    var c1c2 = c1*c2;
    var s1s2 = s1*s2;
    var w =c1c2*c3 - s1s2*s3;
    var x =c1c2*s3 + s1s2*c3;
    var y =s1*c2*c3 + c1*s2*s3;
    var z =c1*s2*c3 - s1*c2*s3;
    var angle = 2 * Math.acos(w);
    var norm = x*x+y*y+z*z;
    if (norm < 0.001) {
        x=1;
        y=z=0;
    } else {
        norm = Math.sqrt(norm);
        x /= norm;
        y /= norm;
        z /= norm;
    }
    return [angle, x, y, z];
}

OIMO.EulerToMatrix = function( ox, oy, oz ) {// angles in radians
    var ch = Math.cos(oy);//heading
    var sh = Math.sin(oy);
    var ca = Math.cos(oz);//altitude
    var sa = Math.sin(oz);
    var cb = Math.cos(ox);//bank
    var sb = Math.sin(ox);
    var mtx = new OIMO.Mat33();

    var te = mtx.elements;
    te[0] = ch * ca;
    te[1] = sh*sb - ch*sa*cb;
    te[2] = ch*sa*sb + sh*cb;
    te[3] = sa;
    te[4] = ca*cb;
    te[5] = -ca*sb;
    te[6] = -sh*ca;
    te[7] = sh*sa*cb + ch*sb;
    te[8] = -sh*sa*sb + ch*cb;
    return mtx;
}

OIMO.MatrixToEuler = function(mtx){// angles in radians
    var te = mtx.elements;
    var x, y, z;
    if (te[3] > 0.998) { // singularity at north pole
        y = Math.atan2(te[2],te[8]);
        z = Math.PI/2;
        x = 0;
    } else if (te[3] < -0.998) { // singularity at south pole
        y = Math.atan2(te[2],te[8]);
        z = -Math.PI/2;
        x = 0;
    } else {
        y = Math.atan2(-te[6],te[0]);
        x = Math.atan2(-te[5],te[4]);
        z = Math.asin(te[3]);
    }
    return [x, y, z];
}

OIMO.unwrapDegrees = function (r) {
    r = r % 360;
    if (r > 180) r -= 360;
    if (r < -180) r += 360;
    return r;
}

OIMO.unwrapRadian = function(r){
    r = r % OIMO.TwoPI;
    if (r > Math.PI) r -= OIMO.TwoPI;
    if (r < -Math.PI) r += OIMO.TwoPI;
    return r;
}
OIMO.Distance3d = function(p1, p2){
    var xd = p2[0]-p1[0];
    var yd = p2[1]-p1[1];
    var zd = p2[2]-p1[2];
    return Math.sqrt(xd*xd + yd*yd + zd*zd);
}
/**
* The base class of all type of the constraints.
* @author saharan
*/
OIMO.Constraint = function(){
    // The parent world of the constraint.
    this.parent=null;
    // The first body of the constraint.
    this.body1=null;
    // The second body of the constraint.
    this.body2=null;
    // Internal
    this.addedToIsland=false;
}

OIMO.Constraint.prototype = {
    constructor: OIMO.Constraint,
    /**
    * Prepare for solving the constraint.
    * @param   timeStep
    * @param   invTimeStep
    */
    preSolve:function(timeStep,invTimeStep){
        throw new Error("Inheritance error.");
    },
    /**
    * Solve the constraint.
    * This is usually called iteratively.
    */
    solve:function(){
        throw new Error("Inheritance error.");
    },
    /**
    * Do the post-processing.
    */
    postSolve:function(){
        throw new Error("Inheritance error.");
    }
}
/**
* Joints are used to constrain the motion between two rigid bodies.
* @author saharan
*/
OIMO.Joint = function(config){
    OIMO.Constraint.call( this );

    this.name = "";
    // Distance joint
    this.JOINT_DISTANCE=0x1;
    // Ball-and-socket joint
    this.JOINT_BALL_AND_SOCKET=0x2;
    //  Hinge joint
    this.JOINT_HINGE=0x3;
    //  Wheel joint
    this.JOINT_WHEEL=0x4;
    //  Slider joint
    this.JOINT_SLIDER=0x5;
    //  Prismatic joint
    this.JOINT_PRISMATIC=0x6;

    // The type of the joint.
    this.type=0;
    //  The previous joint in the world.
    this.prev=null;
    // The next joint in the world.
    this.next=null;

    this.body1=config.body1;
    this.body2=config.body2;

    // The anchor point on the first rigid body in local coordinate system.
    this.localAnchorPoint1=new OIMO.Vec3().copy(config.localAnchorPoint1);
    // The anchor point on the second rigid body in local coordinate system.
    this.localAnchorPoint2=new OIMO.Vec3().copy(config.localAnchorPoint2);
    // The anchor point on the first rigid body in world coordinate system relative to the body's origin.
    this.relativeAnchorPoint1=new OIMO.Vec3();
    // The anchor point on the second rigid body in world coordinate system relative to the body's origin.
    this.relativeAnchorPoint2=new OIMO.Vec3();
    //  The anchor point on the first rigid body in world coordinate system.
    this.anchorPoint1=new OIMO.Vec3();
    // The anchor point on the second rigid body in world coordinate system.
    this.anchorPoint2=new OIMO.Vec3();
    //  Whether allow collision between connected rigid bodies or not.
    this.allowCollision=config.allowCollision;

    this.b1Link=new OIMO.JointLink(this);
    this.b2Link=new OIMO.JointLink(this);

    this.matrix = new OIMO.Mat44();
}
OIMO.Joint.prototype = Object.create( OIMO.Constraint.prototype );
/**
* Update all the anchor points.
*/
OIMO.Joint.prototype.updateAnchorPoints = function () {
    var p1=this.body1.position;
    var p2=this.body2.position;

    var tr1 = this.body1.rotation.elements;
    var tr2 = this.body2.rotation.elements;

    var l1x=this.localAnchorPoint1.x;
    var l1y=this.localAnchorPoint1.y;
    var l1z=this.localAnchorPoint1.z;
    var l2x=this.localAnchorPoint2.x;
    var l2y=this.localAnchorPoint2.y;
    var l2z=this.localAnchorPoint2.z;

    var r1x=l1x*tr1[0]+l1y*tr1[1]+l1z*tr1[2];
    var r1y=l1x*tr1[3]+l1y*tr1[4]+l1z*tr1[5];
    var r1z=l1x*tr1[6]+l1y*tr1[7]+l1z*tr1[8];
    var r2x=l2x*tr2[0]+l2y*tr2[1]+l2z*tr2[2];
    var r2y=l2x*tr2[3]+l2y*tr2[4]+l2z*tr2[5];
    var r2z=l2x*tr2[6]+l2y*tr2[7]+l2z*tr2[8];
    this.relativeAnchorPoint1.x=r1x;
    this.relativeAnchorPoint1.y=r1y;
    this.relativeAnchorPoint1.z=r1z;
    this.relativeAnchorPoint2.x=r2x;
    this.relativeAnchorPoint2.y=r2y;
    this.relativeAnchorPoint2.z=r2z;
    var p1x=r1x+p1.x;
    var p1y=r1y+p1.y;
    var p1z=r1z+p1.z;
    var p2x=r2x+p2.x;
    var p2y=r2y+p2.y;
    var p2z=r2z+p2.z;
    this.anchorPoint1.x=p1x;
    this.anchorPoint1.y=p1y;
    this.anchorPoint1.z=p1z;
    this.anchorPoint2.x=p2x;
    this.anchorPoint2.y=p2y;
    this.anchorPoint2.z=p2z;
}
/**
* Attach the joint from the bodies.
*/
OIMO.Joint.prototype.attach = function () {
    this.b1Link.body=this.body2;
    this.b2Link.body=this.body1;
    if(this.body1.jointLink!=null)(this.b1Link.next=this.body1.jointLink).prev=this.b1Link;
    else this.b1Link.next=null;
    this.body1.jointLink=this.b1Link;
    this.body1.numJoints++;
    if(this.body2.jointLink!=null)(this.b2Link.next=this.body2.jointLink).prev=this.b2Link;
    else this.b2Link.next=null;
    this.body2.jointLink=this.b2Link;
    this.body2.numJoints++;
}
OIMO.Joint.prototype.detach = function () {
    var prev=this.b1Link.prev;
    var next=this.b1Link.next;
    if(prev!=null)prev.next=next;
    if(next!=null)next.prev=prev;
    if(this.body1.jointLink==this.b1Link)this.body1.jointLink=next;
    this.b1Link.prev=null;
    this.b1Link.next=null;
    this.b1Link.body=null;
    this.body1.numJoints--;

    prev=this.b2Link.prev;
    next=this.b2Link.next;
    if(prev!=null)prev.next=next;
    if(next!=null)next.prev=prev;
    if(this.body2.jointLink==this.b2Link)this.body2.jointLink=next;
    this.b2Link.prev=null;
    this.b2Link.next=null;
    this.b2Link.body=null;
    this.body2.numJoints--;

    this.b1Link.body=null;
    this.b2Link.body=null;
}
/**
* Awake the bodies.
*/
OIMO.Joint.prototype.awake = function () {
    this.body1.awake();
    this.body2.awake();
}
OIMO.Joint.prototype.preSolve = function (timeStep,invTimeStep) {
}
OIMO.Joint.prototype.solve = function () {
}
OIMO.Joint.prototype.postSolve = function () {
}


/**
* Three js add
*/
OIMO.Joint.prototype.getPosition = function () {
    var p1 = new OIMO.Vec3().scale(this.anchorPoint1, OIMO.WORLD_SCALE);
    var p2 = new OIMO.Vec3().scale(this.anchorPoint2, OIMO.WORLD_SCALE);
    return [p1, p2];
}

OIMO.Joint.prototype.getMatrix = function () {
    var m = this.matrix.elements;
    var p1 = this.anchorPoint1;
    var p2 = this.anchorPoint2;
    m[0] = p1.x * OIMO.WORLD_SCALE;
    m[1] = p1.y * OIMO.WORLD_SCALE;
    m[2] = p1.z * OIMO.WORLD_SCALE;
    m[3] = 0;

    m[4] = p2.x * OIMO.WORLD_SCALE;
    m[5] = p2.y * OIMO.WORLD_SCALE;
    m[6] = p2.z * OIMO.WORLD_SCALE;
    m[7] = 0;

    return m;
}
/**
* A joint configuration holds all configuration data for constructing a joint.
* Joint configurations can be reused safely.
* @author saharan
*/
OIMO.JointConfig = function(){
	// The first rigid body of the joint.
    this.body1 = null;
    // The second rigid body of the joint.
    this.body2 = null;
    // The anchor point on the first rigid body in local coordinate system.
    this.localAnchorPoint1=new OIMO.Vec3();
    //  The anchor point on the second rigid body in local coordinate system.
    this.localAnchorPoint2=new OIMO.Vec3();
    // The axis in the first body's coordinate system.
	// his property is available in some joints.
    this.localAxis1=new OIMO.Vec3();
    // The axis in the second body's coordinate system.
	// This property is available in some joints.
    this.localAxis2=new OIMO.Vec3();
    //  Whether allow collision between connected rigid bodies or not.
    this.allowCollision=false;
}
/**
* A link list of joints.
* @author saharan
*/
OIMO.JointLink = function(joint){
	// The previous joint link.
    this.prev = null;
    // The next joint link.
    this.next = null;
    // The other rigid body connected to the joint.
    this.body = null;
    // The joint of the link.
    this.joint = joint;
}
/**
* An information of limit and motor.
* @author saharan
*/
OIMO.LimitMotor = function(axis,fixed){
    // The axis of the constraint.
    this.axis=axis;
    // The current angle for rotational constraints.
    this.angle=0;
    // The lower limit. Set lower > upper to disable.
    if(fixed)this.lowerLimit=0;
    else this.lowerLimit=1;
    //  The upper limit. Set lower > upper to disable.
    this.upperLimit=0;
    // The target motor speed.
    this.motorSpeed=0;
    // The maximum motor force or torque. Set 0 to disable.
    this.maxMotorForce=0;
    // The frequency of the spring. Set 0 to disable.
    this.frequency=0;
    // The damping ratio of the spring. Set 0 for no damping, 1 for critical damping.
    this.dampingRatio=0;
}

OIMO.LimitMotor.prototype = {
    constructor: OIMO.LimitMotor,
    /**
    * Set limit data into this constraint.
    * @param   lowerLimit
    * @param   upperLimit
    */
    setLimit:function(lowerLimit,upperLimit){
        this.lowerLimit=lowerLimit;
        this.upperLimit=upperLimit;
    },
    /**
    * Set motor data into this constraint.
    * @param   motorSpeed
    * @param   maxMotorForce
    */
    setMotor:function(motorSpeed,maxMotorForce){
        this.motorSpeed=motorSpeed;
        this.maxMotorForce=maxMotorForce;
    },
    /**
    * Set spring data into this constraint.
    * @param   frequency
    * @param   dampingRatio
    */
    setSpring:function(frequency,dampingRatio){
        this.frequency=frequency;
        this.dampingRatio=dampingRatio;
    }
}
/**
* A ball-and-socket joint limits relative translation on two anchor points on rigid bodies.
* @author saharan
*/
OIMO.BallAndSocketJoint = function(config){
    OIMO.Joint.call( this, config);
    this.type=this.JOINT_BALL_AND_SOCKET;
    
    this.lc=new OIMO.LinearConstraint(this);
}
OIMO.BallAndSocketJoint.prototype = Object.create( OIMO.Joint.prototype );
OIMO.BallAndSocketJoint.prototype.preSolve = function (timeStep,invTimeStep) {
    this.updateAnchorPoints();
    this.lc.preSolve(timeStep,invTimeStep);
}
OIMO.BallAndSocketJoint.prototype.solve = function () {
    this.lc.solve();
}
OIMO.BallAndSocketJoint.prototype.postSolve = function () {
}
/**
* A distance joint limits the distance between two anchor points on rigid bodies.
* @author saharan
*/
OIMO.DistanceJoint = function(config,minDistance,maxDistance){
    OIMO.Joint.call( this, config);
    this.type=this.JOINT_DISTANCE;
    
    this.normal=new OIMO.Vec3();
    // The limit and motor information of the joint.
    this.limitMotor=new OIMO.LimitMotor(this.normal,true);
    this.limitMotor.lowerLimit=minDistance;
    this.limitMotor.upperLimit=maxDistance;
    this.t=new OIMO.TranslationalConstraint(this,this.limitMotor);
}
OIMO.DistanceJoint.prototype = Object.create( OIMO.Joint.prototype );
OIMO.DistanceJoint.prototype.preSolve = function (timeStep,invTimeStep) {
    this.updateAnchorPoints();
    var nx=this.anchorPoint2.x-this.anchorPoint1.x;
    var ny=this.anchorPoint2.y-this.anchorPoint1.y;
    var nz=this.anchorPoint2.z-this.anchorPoint1.z;
    var len=Math.sqrt(nx*nx+ny*ny+nz*nz);
    if(len>0)len=1/len;
    this.normal.init(nx*len,ny*len,nz*len);
    this.t.preSolve(timeStep,invTimeStep);
}
OIMO.DistanceJoint.prototype.solve = function () {
    this.t.solve();
}
OIMO.DistanceJoint.prototype.postSolve = function () {
}
/**
* A hinge joint allows only for relative rotation of rigid bodies along the axis.
* @author saharan
*/
OIMO.HingeJoint = function(config,lowerAngleLimit,upperAngleLimit){
    OIMO.Joint.call( this, config);
    this.type=this.JOINT_HINGE;

    // The axis in the first body's coordinate system.
    this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1);
    // The axis in the second body's coordinate system.
    this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2);
    var len;
    this.localAxis1X=this.localAxis1.x;
    this.localAxis1Y=this.localAxis1.y;
    this.localAxis1Z=this.localAxis1.z;

    this.localAngAxis1X=this.localAxis1Y*this.localAxis1X-this.localAxis1Z*this.localAxis1Z;
    this.localAngAxis1Y=-this.localAxis1Z*this.localAxis1Y-this.localAxis1X*this.localAxis1X;
    this.localAngAxis1Z=this.localAxis1X*this.localAxis1Z+this.localAxis1Y*this.localAxis1Y;

    len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z);
    this.localAngAxis1X*=len;
    this.localAngAxis1Y*=len;
    this.localAngAxis1Z*=len;

    this.localAxis2X=this.localAxis2.x;
    this.localAxis2Y=this.localAxis2.y;
    this.localAxis2Z=this.localAxis2.z;

    // make angle axis 2
    var arc=new OIMO.Mat33().setQuat(new OIMO.Quat().arc(this.localAxis1,this.localAxis2));
    var tarc = arc.elements;
    this.localAngAxis2X=this.localAngAxis1X*tarc[0]+this.localAngAxis1Y*tarc[1]+this.localAngAxis1Z*tarc[2];
    this.localAngAxis2Y=this.localAngAxis1X*tarc[3]+this.localAngAxis1Y*tarc[4]+this.localAngAxis1Z*tarc[5];
    this.localAngAxis2Z=this.localAngAxis1X*tarc[6]+this.localAngAxis1Y*tarc[7]+this.localAngAxis1Z*tarc[8];

    this.nor=new OIMO.Vec3();
    this.tan=new OIMO.Vec3();
    this.bin=new OIMO.Vec3();
    // The rotational limit and motor information of the joint.
    this.limitMotor=new OIMO.LimitMotor(this.nor,false);
    this.limitMotor.lowerLimit=lowerAngleLimit;
    this.limitMotor.upperLimit=upperAngleLimit;

    this.lc=new OIMO.LinearConstraint(this);
    this.r3=new OIMO.Rotational3Constraint(this,this.limitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true));
}
OIMO.HingeJoint.prototype = Object.create( OIMO.Joint.prototype );
OIMO.HingeJoint.prototype.preSolve = function (timeStep,invTimeStep) {
    var tmpM;
    var tmp1X;
    var tmp1Y;
    var tmp1Z;

    this.updateAnchorPoints();

    tmpM=this.body1.rotation.elements;
    var axis1X=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2];
    var axis1Y=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5];
    var axis1Z=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8];
    var angAxis1X=this.localAngAxis1X*tmpM[0]+this.localAngAxis1Y*tmpM[1]+this.localAngAxis1Z*tmpM[2];
    var angAxis1Y=this.localAngAxis1X*tmpM[3]+this.localAngAxis1Y*tmpM[4]+this.localAngAxis1Z*tmpM[5];
    var angAxis1Z=this.localAngAxis1X*tmpM[6]+this.localAngAxis1Y*tmpM[7]+this.localAngAxis1Z*tmpM[8];
    tmpM=this.body2.rotation.elements;
    var axis2X=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2];
    var axis2Y=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5];
    var axis2Z=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8];
    var angAxis2X=this.localAngAxis2X*tmpM[0]+this.localAngAxis2Y*tmpM[1]+this.localAngAxis2Z*tmpM[2];
    var angAxis2Y=this.localAngAxis2X*tmpM[3]+this.localAngAxis2Y*tmpM[4]+this.localAngAxis2Z*tmpM[5];
    var angAxis2Z=this.localAngAxis2X*tmpM[6]+this.localAngAxis2Y*tmpM[7]+this.localAngAxis2Z*tmpM[8];
    var nx=axis1X*this.body2.inverseMass+axis2X*this.body1.inverseMass;
    var ny=axis1Y*this.body2.inverseMass+axis2Y*this.body1.inverseMass;
    var nz=axis1Z*this.body2.inverseMass+axis2Z*this.body1.inverseMass;
    tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz);
    if(tmp1X>0)tmp1X=1/tmp1X;
    nx*=tmp1X;
    ny*=tmp1X;
    nz*=tmp1X;
    var tx=ny*nx-nz*nz;
    var ty=-nz*ny-nx*nx;
    var tz=nx*nz+ny*ny;
    tmp1X=1/Math.sqrt(tx*tx+ty*ty+tz*tz);
    tx*=tmp1X;
    ty*=tmp1X;
    tz*=tmp1X;
    var bx=ny*tz-nz*ty;
    var by=nz*tx-nx*tz;
    var bz=nx*ty-ny*tx;

    this.nor.init(nx,ny,nz);
    this.tan.init(tx,ty,tz);
    this.bin.init(bx,by,bz);

    // ----------------------------------------------
    //            calculate hinge angle
    // ----------------------------------------------
            
    if(
        nx*(angAxis1Y*angAxis2Z-angAxis1Z*angAxis2Y)+
        ny*(angAxis1Z*angAxis2X-angAxis1X*angAxis2Z)+
        nz*(angAxis1X*angAxis2Y-angAxis1Y*angAxis2X)<0
    ){
        this.limitMotor.angle=-this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z);
    }else{
        this.limitMotor.angle=this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z);
    }

    // angular error
    tmp1X=axis1Y*axis2Z-axis1Z*axis2Y;
    tmp1Y=axis1Z*axis2X-axis1X*axis2Z;
    tmp1Z=axis1X*axis2Y-axis1Y*axis2X;

    this.r3.limitMotor2.angle=tx*tmp1X+ty*tmp1Y+tz*tmp1Z;
    this.r3.limitMotor3.angle=bx*tmp1X+by*tmp1Y+bz*tmp1Z;
    
    this.r3.preSolve(timeStep,invTimeStep);
    this.lc.preSolve(timeStep,invTimeStep);
}
OIMO.HingeJoint.prototype.solve = function () {
    this.r3.solve();
    this.lc.solve();
}
OIMO.HingeJoint.prototype.postSolve = function () {
}
OIMO.HingeJoint.prototype.acosClamp = function(cos){
    if(cos>1)return 0;
    else if(cos<-1)return Math.PI;
    else return Math.acos(cos);
}
/**
* A prismatic joint allows only for relative translation of rigid bodies along the axis.
* @author saharan
*/
OIMO.PrismaticJoint = function(config,lowerTranslation,upperTranslation){
    OIMO.Joint.call( this, config);
    this.type=this.JOINT_PRISMATIC;

    // The axis in the first body's coordinate system.
    this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1);
    // The axis in the second body's coordinate system.
    this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2);
    this.localAxis1X=this.localAxis1.x;
    this.localAxis1Y=this.localAxis1.y;
    this.localAxis1Z=this.localAxis1.z;
    this.localAxis2X=this.localAxis2.x;
    this.localAxis2Y=this.localAxis2.y;
    this.localAxis2Z=this.localAxis2.z;
    
    this.nor=new OIMO.Vec3();
    this.tan=new OIMO.Vec3();
    this.bin=new OIMO.Vec3();
    this.ac=new OIMO.AngularConstraint(this,new OIMO.Quat().arc(this.localAxis1,this.localAxis2));
    // The translational limit and motor information of the joint.
    this.limitMotor=new OIMO.LimitMotor(this.nor,true);
    this.limitMotor.lowerLimit=lowerTranslation;
    this.limitMotor.upperLimit=upperTranslation;
    this.t3=new OIMO.Translational3Constraint(this,this.limitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true));
}
OIMO.PrismaticJoint.prototype = Object.create( OIMO.Joint.prototype );
OIMO.PrismaticJoint.prototype.preSolve = function (timeStep,invTimeStep) {
    var tmpM;
    var tmp1X;
    var tmp1Y;
    var tmp1Z;
    this.updateAnchorPoints();

    tmpM=this.body1.rotation.elements;
    var axis1X=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2];
    var axis1Y=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5];
    var axis1Z=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8];
    tmpM=this.body2.rotation.elements;
    var axis2X=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2];
    var axis2Y=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5];
    var axis2Z=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8];

    var nx=axis1X*this.body2.inverseMass+axis2X*this.body1.inverseMass;
    var ny=axis1Y*this.body2.inverseMass+axis2Y*this.body1.inverseMass;
    var nz=axis1Z*this.body2.inverseMass+axis2Z*this.body1.inverseMass;
    tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz);
    if(tmp1X>0)tmp1X=1/tmp1X;
    nx*=tmp1X;
    ny*=tmp1X;
    nz*=tmp1X;
    var tx=ny*nx-nz*nz;
    var ty=-nz*ny-nx*nx;
    var tz=nx*nz+ny*ny;
    tmp1X=1/Math.sqrt(tx*tx+ty*ty+tz*tz);
    tx*=tmp1X;
    ty*=tmp1X;
    tz*=tmp1X;
    var bx=ny*tz-nz*ty;
    var by=nz*tx-nx*tz;
    var bz=nx*ty-ny*tx;

    this.nor.init(nx,ny,nz);
    this.tan.init(tx,ty,tz);
    this.bin.init(bx,by,bz);
    
    this.ac.preSolve(timeStep,invTimeStep);
    this.t3.preSolve(timeStep,invTimeStep);
}
OIMO.PrismaticJoint.prototype.solve = function () {
    this.ac.solve();
    this.t3.solve();
}
OIMO.PrismaticJoint.prototype.postSolve = function () {
}
/**
* A slider joint allows for relative translation and relative rotation between two rigid bodies along the axis.
* @author saharan
*/
OIMO.SliderJoint = function(config,lowerTranslation,upperTranslation){
    OIMO.Joint.call( this, config);
    this.type=this.JOINT_SLIDER;

    // The first axis in local coordinate system.
    this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1);
    // The second axis in local coordinate system.
    this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2);

    var len;
    this.localAxis1X=this.localAxis1.x;
    this.localAxis1Y=this.localAxis1.y;
    this.localAxis1Z=this.localAxis1.z;

    this.localAngAxis1X=this.localAxis1Y*this.localAxis1X-this.localAxis1Z*this.localAxis1Z;
    this.localAngAxis1Y=-this.localAxis1Z*this.localAxis1Y-this.localAxis1X*this.localAxis1X;
    this.localAngAxis1Z=this.localAxis1X*this.localAxis1Z+this.localAxis1Y*this.localAxis1Y;

    len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z);
    this.localAngAxis1X*=len;
    this.localAngAxis1Y*=len;
    this.localAngAxis1Z*=len;

    this.localAxis2X=this.localAxis2.x;
    this.localAxis2Y=this.localAxis2.y;
    this.localAxis2Z=this.localAxis2.z;

    // make angle axis 2
    var arc=new OIMO.Mat33().setQuat(new OIMO.Quat().arc(this.localAxis1,this.localAxis2));
    var tarc = arc.elements;
    this.localAngAxis2X=this.localAngAxis1X*tarc[0]+this.localAngAxis1Y*tarc[1]+this.localAngAxis1Z*tarc[2];
    this.localAngAxis2Y=this.localAngAxis1X*tarc[3]+this.localAngAxis1Y*tarc[4]+this.localAngAxis1Z*tarc[5];
    this.localAngAxis2Z=this.localAngAxis1X*tarc[6]+this.localAngAxis1Y*tarc[7]+this.localAngAxis1Z*tarc[8];
    
    this.nor=new OIMO.Vec3();
    this.tan=new OIMO.Vec3();
    this.bin=new OIMO.Vec3();
    // The limit and motor for the rotation
    this.rotationalLimitMotor=new OIMO.LimitMotor(this.nor,false);
    this.r3=new OIMO.Rotational3Constraint(this,this.rotationalLimitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true));
    // The limit and motor for the translation.
    this.translationalLimitMotor=new OIMO.LimitMotor(this.nor,true);
    this.translationalLimitMotor.lowerLimit=lowerTranslation;
    this.translationalLimitMotor.upperLimit=upperTranslation;
    this.t3=new OIMO.Translational3Constraint(this,this.translationalLimitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true));
}
OIMO.SliderJoint.prototype = Object.create( OIMO.Joint.prototype );
OIMO.SliderJoint.prototype.preSolve = function (timeStep,invTimeStep) {
    var tmpM;
    var tmp1X;
    var tmp1Y;
    var tmp1Z;
    this.updateAnchorPoints();

    tmpM=this.body1.rotation.elements;
    var axis1X=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2];
    var axis1Y=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5];
    var axis1Z=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8];
    var angAxis1X=this.localAngAxis1X*tmpM[0]+this.localAngAxis1Y*tmpM[1]+this.localAngAxis1Z*tmpM[2];
    var angAxis1Y=this.localAngAxis1X*tmpM[3]+this.localAngAxis1Y*tmpM[4]+this.localAngAxis1Z*tmpM[5];
    var angAxis1Z=this.localAngAxis1X*tmpM[6]+this.localAngAxis1Y*tmpM[7]+this.localAngAxis1Z*tmpM[8];
    tmpM=this.body2.rotation.elements;
    var axis2X=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2];
    var axis2Y=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5];
    var axis2Z=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8];
    var angAxis2X=this.localAngAxis2X*tmpM[0]+this.localAngAxis2Y*tmpM[1]+this.localAngAxis2Z*tmpM[2];
    var angAxis2Y=this.localAngAxis2X*tmpM[3]+this.localAngAxis2Y*tmpM[4]+this.localAngAxis2Z*tmpM[5];
    var angAxis2Z=this.localAngAxis2X*tmpM[6]+this.localAngAxis2Y*tmpM[7]+this.localAngAxis2Z*tmpM[8];

    var nx=axis1X*this.body2.inverseMass+axis2X*this.body1.inverseMass;
    var ny=axis1Y*this.body2.inverseMass+axis2Y*this.body1.inverseMass;
    var nz=axis1Z*this.body2.inverseMass+axis2Z*this.body1.inverseMass;
    tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz);
    if(tmp1X>0)tmp1X=1/tmp1X;
    nx*=tmp1X;
    ny*=tmp1X;
    nz*=tmp1X;
    var tx=ny*nx-nz*nz;
    var ty=-nz*ny-nx*nx;
    var tz=nx*nz+ny*ny;
    tmp1X=1/Math.sqrt(tx*tx+ty*ty+tz*tz);
    tx*=tmp1X;
    ty*=tmp1X;
    tz*=tmp1X;
    var bx=ny*tz-nz*ty;
    var by=nz*tx-nx*tz;
    var bz=nx*ty-ny*tx;
    this.nor.init(nx,ny,nz);
    this.tan.init(tx,ty,tz);
    this.bin.init(bx,by,bz);

    // ----------------------------------------------
    //            calculate hinge angle
    // ----------------------------------------------

            
    if(
        nx*(angAxis1Y*angAxis2Z-angAxis1Z*angAxis2Y)+
        ny*(angAxis1Z*angAxis2X-angAxis1X*angAxis2Z)+
        nz*(angAxis1X*angAxis2Y-angAxis1Y*angAxis2X)<0
        ){
        this.rotationalLimitMotor.angle=-this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z);
    }else{
        this.rotationalLimitMotor.angle=this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z);
    }

    // angular error
    tmp1X=axis1Y*axis2Z-axis1Z*axis2Y;
    tmp1Y=axis1Z*axis2X-axis1X*axis2Z;
    tmp1Z=axis1X*axis2Y-axis1Y*axis2X;

    this.r3.limitMotor2.angle=tx*tmp1X+ty*tmp1Y+tz*tmp1Z;
    this.r3.limitMotor3.angle=bx*tmp1X+by*tmp1Y+bz*tmp1Z;
    
    this.r3.preSolve(timeStep,invTimeStep);
    this.t3.preSolve(timeStep,invTimeStep);
}
OIMO.SliderJoint.prototype.solve = function () {
    this.r3.solve();
    this.t3.solve();
}
OIMO.SliderJoint.prototype.postSolve = function () {
}
OIMO.SliderJoint.prototype.acosClamp = function(cos){
    if(cos>1)return 0;
    else if(cos<-1)return Math.PI;
    else return Math.acos(cos);
}
/**
* A wheel joint allows for relative rotation between two rigid bodies along two axes.
* The wheel joint also allows for relative translation for the suspension.
*/
OIMO.WheelJoint = function(config){
    OIMO.Joint.call( this, config);
    this.type=this.JOINT_WHEEL;

    // The first axis in local coordinate system.
    this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1);
    // The second axis in local coordinate system.
    this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2);

    var len;
    this.localAxis1X=this.localAxis1.x;
    this.localAxis1Y=this.localAxis1.y;
    this.localAxis1Z=this.localAxis1.z;
    this.localAxis2X=this.localAxis2.x;
    this.localAxis2Y=this.localAxis2.y;
    this.localAxis2Z=this.localAxis2.z;
    var dot=this.localAxis1X*this.localAxis2X+this.localAxis1Y*this.localAxis2Y+this.localAxis1Z*this.localAxis2Z;
    if(dot>-1&&dot<1){
        this.localAngAxis1X=this.localAxis2X-dot*this.localAxis1X;
        this.localAngAxis1Y=this.localAxis2Y-dot*this.localAxis1Y;
        this.localAngAxis1Z=this.localAxis2Z-dot*this.localAxis1Z;
        this.localAngAxis2X=this.localAxis1X-dot*this.localAxis2X;
        this.localAngAxis2Y=this.localAxis1Y-dot*this.localAxis2Y;
        this.localAngAxis2Z=this.localAxis1Z-dot*this.localAxis2Z;
        len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z);
        this.localAngAxis1X*=len;
        this.localAngAxis1Y*=len;
        this.localAngAxis1Z*=len;
        len=1/Math.sqrt(this.localAngAxis2X*this.localAngAxis2X+this.localAngAxis2Y*this.localAngAxis2Y+this.localAngAxis2Z*this.localAngAxis2Z);
        this.localAngAxis2X*=len;
        this.localAngAxis2Y*=len;
        this.localAngAxis2Z*=len;
    }else{
        this.localAngAxis1X=this.localAxis1Y*this.localAxis1X-this.localAxis1Z*this.localAxis1Z;
        this.localAngAxis1Y=-this.localAxis1Z*this.localAxis1Y-this.localAxis1X*this.localAxis1X;
        this.localAngAxis1Z=this.localAxis1X*this.localAxis1Z+this.localAxis1Y*this.localAxis1Y;
        len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z);
        this.localAngAxis1X*=len;
        this.localAngAxis1Y*=len;
        this.localAngAxis1Z*=len;
        var arc=new OIMO.Mat33().setQuat(new OIMO.Quat().arc(this.localAxis1,this.localAxis2));
        var tarc = arc.elements;
        this.localAngAxis2X=this.localAngAxis1X*tarc[0]+this.localAngAxis1Y*tarc[1]+this.localAngAxis1Z*tarc[2];
        this.localAngAxis2Y=this.localAngAxis1X*tarc[3]+this.localAngAxis1Y*tarc[4]+this.localAngAxis1Z*tarc[5];
        this.localAngAxis2Z=this.localAngAxis1X*tarc[6]+this.localAngAxis1Y*tarc[7]+this.localAngAxis1Z*tarc[8];
    }

    this.nor=new OIMO.Vec3();
    this.tan=new OIMO.Vec3();
    this.bin=new OIMO.Vec3();

    // The translational limit and motor information of the joint.
    this.translationalLimitMotor=new OIMO.LimitMotor(this.tan,true);
    this.translationalLimitMotor.frequency=8;
    this.translationalLimitMotor.dampingRatio=1;
    // The first rotational limit and motor information of the joint.
    this.rotationalLimitMotor1=new OIMO.LimitMotor(this.tan,false);
    // The second rotational limit and motor information of the joint.
    this.rotationalLimitMotor2=new OIMO.LimitMotor(this.bin,false);

    this.t3=new OIMO.Translational3Constraint(this,new OIMO.LimitMotor(this.nor,true),this.translationalLimitMotor,new OIMO.LimitMotor(this.bin,true));
    this.t3.weight=1;
    this.r3=new OIMO.Rotational3Constraint(this,new OIMO.LimitMotor(this.nor,true),this.rotationalLimitMotor1,this.rotationalLimitMotor2);
}
OIMO.WheelJoint.prototype = Object.create( OIMO.Joint.prototype );
OIMO.WheelJoint.prototype.preSolve = function (timeStep,invTimeStep) {
    var tmpM;
    var tmp1X;
    var tmp1Y;
    var tmp1Z;
    this.updateAnchorPoints();
    tmpM=this.body1.rotation.elements;
    var x1=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2];
    var y1=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5];
    var z1=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8];
    var angAxis1X=this.localAngAxis1X*tmpM[0]+this.localAngAxis1Y*tmpM[1]+this.localAngAxis1Z*tmpM[2];
    var angAxis1Y=this.localAngAxis1X*tmpM[3]+this.localAngAxis1Y*tmpM[4]+this.localAngAxis1Z*tmpM[5];
    var angAxis1Z=this.localAngAxis1X*tmpM[6]+this.localAngAxis1Y*tmpM[7]+this.localAngAxis1Z*tmpM[8];
    tmpM=this.body2.rotation.elements;
    var x2=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2];
    var y2=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5];
    var z2=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8];
    var angAxis2X=this.localAngAxis2X*tmpM[0]+this.localAngAxis2Y*tmpM[1]+this.localAngAxis2Z*tmpM[2];
    var angAxis2Y=this.localAngAxis2X*tmpM[3]+this.localAngAxis2Y*tmpM[4]+this.localAngAxis2Z*tmpM[5];
    var angAxis2Z=this.localAngAxis2X*tmpM[6]+this.localAngAxis2Y*tmpM[7]+this.localAngAxis2Z*tmpM[8];
    
    this.r3.limitMotor1.angle=x1*x2+y1*y2+z1*z2;
    if( x1*(angAxis1Y*z2-angAxis1Z*y2)+ y1*(angAxis1Z*x2-angAxis1X*z2)+ z1*(angAxis1X*y2-angAxis1Y*x2)<0 ){
        this.rotationalLimitMotor1.angle=-this.acosClamp(angAxis1X*x2+angAxis1Y*y2+angAxis1Z*z2);
    }else{
        this.rotationalLimitMotor1.angle=this.acosClamp(angAxis1X*x2+angAxis1Y*y2+angAxis1Z*z2);
    }
    if( x2*(angAxis2Y*z1-angAxis2Z*y1)+ y2*(angAxis2Z*x1-angAxis2X*z1)+ z2*(angAxis2X*y1-angAxis2Y*x1)<0 ){
        this.rotationalLimitMotor2.angle=this.acosClamp(angAxis2X*x1+angAxis2Y*y1+angAxis2Z*z1);
    }else{
        this.rotationalLimitMotor2.angle=-this.acosClamp(angAxis2X*x1+angAxis2Y*y1+angAxis2Z*z1);
    }

    var nx=y2*z1-z2*y1;
    var ny=z2*x1-x2*z1;
    var nz=x2*y1-y2*x1;

    tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz);
    if(tmp1X>0)tmp1X=1/tmp1X;
    nx*=tmp1X;
    ny*=tmp1X;
    nz*=tmp1X;

    var tx=ny*z2-nz*y2;
    var ty=nz*x2-nx*z2;
    var tz=nx*y2-ny*x2;

    tmp1X=Math.sqrt(tx*tx+ty*ty+tz*tz);
    if(tmp1X>0)tmp1X=1/tmp1X;
    tx*=tmp1X;
    ty*=tmp1X;
    tz*=tmp1X;

    var bx=y1*nz-z1*ny;
    var by=z1*nx-x1*nz;
    var bz=x1*ny-y1*nx;

    tmp1X=Math.sqrt(bx*bx+by*by+bz*bz);
    if(tmp1X>0)tmp1X=1/tmp1X;
    bx*=tmp1X;
    by*=tmp1X;
    bz*=tmp1X;

    this.nor.init(nx,ny,nz);
    this.tan.init(tx,ty,tz);
    this.bin.init(bx,by,bz);
    
    this.r3.preSolve(timeStep,invTimeStep);
    this.t3.preSolve(timeStep,invTimeStep);
}
OIMO.WheelJoint.prototype.solve = function () {
    this.r3.solve();
    this.t3.solve();
}
OIMO.WheelJoint.prototype.postSolve = function () {
}
OIMO.WheelJoint.prototype.acosClamp = function(cos){
    if(cos>1)return 0;
    else if(cos<-1)return Math.PI;
    else return Math.acos(cos);
}
/**
* An angular constraint for all axes for various joints.
* @author saharan
*/
OIMO.AngularConstraint = function(joint,targetOrientation){
    this.i1e00=NaN;
    this.i1e01=NaN;
    this.i1e02=NaN;
    this.i1e10=NaN;
    this.i1e11=NaN;
    this.i1e12=NaN;
    this.i1e20=NaN;
    this.i1e21=NaN;
    this.i1e22=NaN;
    this.i2e00=NaN;
    this.i2e01=NaN;
    this.i2e02=NaN;
    this.i2e10=NaN;
    this.i2e11=NaN;
    this.i2e12=NaN;
    this.i2e20=NaN;
    this.i2e21=NaN;
    this.i2e22=NaN;
    this.d00=NaN;
    this.d01=NaN;
    this.d02=NaN;
    this.d10=NaN;
    this.d11=NaN;
    this.d12=NaN;
    this.d20=NaN;
    this.d21=NaN;
    this.d22=NaN;
    this.ax=NaN;
    this.ay=NaN;
    this.az=NaN;
    this.velx=NaN;
    this.vely=NaN;
    this.velz=NaN;

    this.joint=joint;
    this.targetOrientation=new OIMO.Quat().invert(targetOrientation);
    this.relativeOrientation=new OIMO.Quat();
    this.b1=joint.body1;
    this.b2=joint.body2;
    this.a1=this.b1.angularVelocity;
    this.a2=this.b2.angularVelocity;
    this.i1=this.b1.inverseInertia;
    this.i2=this.b2.inverseInertia;
    this.impx=0;
    this.impy=0;
    this.impz=0;
}
OIMO.AngularConstraint.prototype = {
    constructor: OIMO.AngularConstraint,

    preSolve:function(timeStep,invTimeStep){
        var ti1 = this.i1.elements;
        var ti2 = this.i2.elements;
        this.i1e00=ti1[0];
        this.i1e01=ti1[1];
        this.i1e02=ti1[2];
        this.i1e10=ti1[3];
        this.i1e11=ti1[4];
        this.i1e12=ti1[5];
        this.i1e20=ti1[6];
        this.i1e21=ti1[7];
        this.i1e22=ti1[8];

        this.i2e00=ti2[0];
        this.i2e01=ti2[1];
        this.i2e02=ti2[2];
        this.i2e10=ti2[3];
        this.i2e11=ti2[4];
        this.i2e12=ti2[5];
        this.i2e20=ti2[6];
        this.i2e21=ti2[7];
        this.i2e22=ti2[8];

        var v00=this.i1e00+this.i2e00;
        var v01=this.i1e01+this.i2e01;
        var v02=this.i1e02+this.i2e02;
        var v10=this.i1e10+this.i2e10;
        var v11=this.i1e11+this.i2e11;
        var v12=this.i1e12+this.i2e12;
        var v20=this.i1e20+this.i2e20;
        var v21=this.i1e21+this.i2e21;
        var v22=this.i1e22+this.i2e22;
        var inv=1/(
        v00*(v11*v22-v21*v12)+
        v10*(v21*v02-v01*v22)+
        v20*(v01*v12-v11*v02)
        );
        this.d00=(v11*v22-v12*v21)*inv;
        this.d01=(v02*v21-v01*v22)*inv;
        this.d02=(v01*v12-v02*v11)*inv;
        this.d10=(v12*v20-v10*v22)*inv;
        this.d11=(v00*v22-v02*v20)*inv;
        this.d12=(v02*v10-v00*v12)*inv;
        this.d20=(v10*v21-v11*v20)*inv;
        this.d21=(v01*v20-v00*v21)*inv;
        this.d22=(v00*v11-v01*v10)*inv;
        
        this.relativeOrientation.invert(this.b1.orientation);// error = b2 - b1 - target
        this.relativeOrientation.mul(this.targetOrientation,this.relativeOrientation);
        this.relativeOrientation.mul(this.b2.orientation,this.relativeOrientation);
        inv=this.relativeOrientation.s*2;
        this.velx=this.relativeOrientation.x*inv;
        this.vely=this.relativeOrientation.y*inv;
        this.velz=this.relativeOrientation.z*inv;
        var len=Math.sqrt(this.velx*this.velx+this.vely*this.vely+this.velz*this.velz);
        if(len>0.02){
            len=(0.02-len)/len*invTimeStep*0.05;
            this.velx*=len;
            this.vely*=len;
            this.velz*=len;
        }else{
            this.velx=0;
            this.vely=0;
            this.velz=0;
        }
        this.a1.x+=this.impx*this.i1e00+this.impy*this.i1e01+this.impz*this.i1e02;
        this.a1.y+=this.impx*this.i1e10+this.impy*this.i1e11+this.impz*this.i1e12;
        this.a1.z+=this.impx*this.i1e20+this.impy*this.i1e21+this.impz*this.i1e22;
        this.a2.x-=this.impx*this.i2e00+this.impy*this.i2e01+this.impz*this.i2e02;
        this.a2.y-=this.impx*this.i2e10+this.impy*this.i2e11+this.impz*this.i2e12;
        this.a2.z-=this.impx*this.i2e20+this.impy*this.i2e21+this.impz*this.i2e22;
    },
    solve:function(){
        var rvx=this.a2.x-this.a1.x-this.velx;
        var rvy=this.a2.y-this.a1.y-this.vely;
        var rvz=this.a2.z-this.a1.z-this.velz;
        var nimpx=rvx*this.d00+rvy*this.d01+rvz*this.d02;
        var nimpy=rvx*this.d10+rvy*this.d11+rvz*this.d12;
        var nimpz=rvx*this.d20+rvy*this.d21+rvz*this.d22;
        this.impx+=nimpx;
        this.impy+=nimpy;
        this.impz+=nimpz;
        this.a1.x+=nimpx*this.i1e00+nimpy*this.i1e01+nimpz*this.i1e02;
        this.a1.y+=nimpx*this.i1e10+nimpy*this.i1e11+nimpz*this.i1e12;
        this.a1.z+=nimpx*this.i1e20+nimpy*this.i1e21+nimpz*this.i1e22;
        this.a2.x-=nimpx*this.i2e00+nimpy*this.i2e01+nimpz*this.i2e02;
        this.a2.y-=nimpx*this.i2e10+nimpy*this.i2e11+nimpz*this.i2e12;
        this.a2.z-=nimpx*this.i2e20+nimpy*this.i2e21+nimpz*this.i2e22;
    }
}
/**
* A linear constraint for all axes for various joints.
* @author saharan
*/
OIMO.LinearConstraint = function(joint){
    this.m1=NaN;
    this.m2=NaN;
    this.i1e00=NaN;
    this.i1e01=NaN;
    this.i1e02=NaN;
    this.i1e10=NaN;
    this.i1e11=NaN;
    this.i1e12=NaN;
    this.i1e20=NaN;
    this.i1e21=NaN;
    this.i1e22=NaN;
    this.i2e00=NaN;
    this.i2e01=NaN;
    this.i2e02=NaN;
    this.i2e10=NaN;
    this.i2e11=NaN;
    this.i2e12=NaN;
    this.i2e20=NaN;
    this.i2e21=NaN;
    this.i2e22=NaN;
    this.d00=NaN;
    this.d01=NaN;
    this.d02=NaN;
    this.d10=NaN;
    this.d11=NaN;
    this.d12=NaN;
    this.d20=NaN;
    this.d21=NaN;
    this.d22=NaN;
    this.r1x=NaN;
    this.r1y=NaN;
    this.r1z=NaN;
    this.r2x=NaN;
    this.r2y=NaN;
    this.r2z=NaN;
    this.ax1x=NaN;
    this.ax1y=NaN;
    this.ax1z=NaN;
    this.ay1x=NaN;
    this.ay1y=NaN;
    this.ay1z=NaN;
    this.az1x=NaN;
    this.az1y=NaN;
    this.az1z=NaN;
    this.ax2x=NaN;
    this.ax2y=NaN;
    this.ax2z=NaN;
    this.ay2x=NaN;
    this.ay2y=NaN;
    this.ay2z=NaN;
    this.az2x=NaN;
    this.az2y=NaN;
    this.az2z=NaN;
    this.vel=NaN;
    this.velx=NaN;
    this.vely=NaN;
    this.velz=NaN;


    this.joint=joint;
    this.r1=joint.relativeAnchorPoint1;
    this.r2=joint.relativeAnchorPoint2;
    this.p1=joint.anchorPoint1;
    this.p2=joint.anchorPoint2;
    this.b1=joint.body1;
    this.b2=joint.body2;
    this.l1=this.b1.linearVelocity;
    this.l2=this.b2.linearVelocity;
    this.a1=this.b1.angularVelocity;
    this.a2=this.b2.angularVelocity;
    this.i1=this.b1.inverseInertia;
    this.i2=this.b2.inverseInertia;
    this.impx=0;
    this.impy=0;
    this.impz=0;
}

OIMO.LinearConstraint.prototype = {
    constructor: OIMO.LinearConstraint,

    preSolve:function(timeStep,invTimeStep){
        this.r1x=this.r1.x;
        this.r1y=this.r1.y;
        this.r1z=this.r1.z;
        this.r2x=this.r2.x;
        this.r2y=this.r2.y;
        this.r2z=this.r2.z;
        this.m1=this.b1.inverseMass;
        this.m2=this.b2.inverseMass;

        var ti1 = this.i1.elements;
        var ti2 = this.i2.elements;
        this.i1e00=ti1[0];
        this.i1e01=ti1[1];
        this.i1e02=ti1[2];
        this.i1e10=ti1[3];
        this.i1e11=ti1[4];
        this.i1e12=ti1[5];
        this.i1e20=ti1[6];
        this.i1e21=ti1[7];
        this.i1e22=ti1[8];

        this.i2e00=ti2[0];
        this.i2e01=ti2[1];
        this.i2e02=ti2[2];
        this.i2e10=ti2[3];
        this.i2e11=ti2[4];
        this.i2e12=ti2[5];
        this.i2e20=ti2[6];
        this.i2e21=ti2[7];
        this.i2e22=ti2[8];

        this.ax1x=this.r1z*this.i1e01+-this.r1y*this.i1e02;
        this.ax1y=this.r1z*this.i1e11+-this.r1y*this.i1e12;
        this.ax1z=this.r1z*this.i1e21+-this.r1y*this.i1e22;
        this.ay1x=-this.r1z*this.i1e00+this.r1x*this.i1e02;
        this.ay1y=-this.r1z*this.i1e10+this.r1x*this.i1e12;
        this.ay1z=-this.r1z*this.i1e20+this.r1x*this.i1e22;
        this.az1x=this.r1y*this.i1e00+-this.r1x*this.i1e01;
        this.az1y=this.r1y*this.i1e10+-this.r1x*this.i1e11;
        this.az1z=this.r1y*this.i1e20+-this.r1x*this.i1e21;
        this.ax2x=this.r2z*this.i2e01+-this.r2y*this.i2e02;
        this.ax2y=this.r2z*this.i2e11+-this.r2y*this.i2e12;
        this.ax2z=this.r2z*this.i2e21+-this.r2y*this.i2e22;
        this.ay2x=-this.r2z*this.i2e00+this.r2x*this.i2e02;
        this.ay2y=-this.r2z*this.i2e10+this.r2x*this.i2e12;
        this.ay2z=-this.r2z*this.i2e20+this.r2x*this.i2e22;
        this.az2x=this.r2y*this.i2e00+-this.r2x*this.i2e01;
        this.az2y=this.r2y*this.i2e10+-this.r2x*this.i2e11;
        this.az2z=this.r2y*this.i2e20+-this.r2x*this.i2e21;

        // calculate point-to-point mass matrix
        // from impulse equation
        // 
        // M = ([/m] - [r^][/I][r^]) ^ -1
        // 
        // where
        // 
        // [/m] = |1/m, 0, 0|
        //        |0, 1/m, 0|
        //        |0, 0, 1/m|
        // 
        // [r^] = |0, -rz, ry|
        //        |rz, 0, -rx|
        //        |-ry, rx, 0|
        // 
        // [/I] = Inverted moment inertia

        var k00=this.m1+this.m2;
        var k01=0;
        var k02=0;
        var k10=0;
        var k11=k00;
        var k12=0;
        var k20=0;
        var k21=0;
        var k22=k00;

        k00+=this.i1e11*this.r1z*this.r1z-(this.i1e21+this.i1e12)*this.r1y*this.r1z+this.i1e22*this.r1y*this.r1y;
        k01+=(this.i1e20*this.r1y+this.i1e12*this.r1x)*this.r1z-this.i1e10*this.r1z*this.r1z-this.i1e22*this.r1x*this.r1y;
        k02+=(this.i1e10*this.r1y-this.i1e11*this.r1x)*this.r1z-this.i1e20*this.r1y*this.r1y+this.i1e21*this.r1x*this.r1y;
        k10+=(this.i1e02*this.r1y+this.i1e21*this.r1x)*this.r1z-this.i1e01*this.r1z*this.r1z-this.i1e22*this.r1x*this.r1y;
        k11+=this.i1e00*this.r1z*this.r1z-(this.i1e20+this.i1e02)*this.r1x*this.r1z+this.i1e22*this.r1x*this.r1x;
        k12+=(this.i1e01*this.r1x-this.i1e00*this.r1y)*this.r1z-this.i1e21*this.r1x*this.r1x+this.i1e20*this.r1x*this.r1y;
        k20+=(this.i1e01*this.r1y-this.i1e11*this.r1x)*this.r1z-this.i1e02*this.r1y*this.r1y+this.i1e12*this.r1x*this.r1y;
        k21+=(this.i1e10*this.r1x-this.i1e00*this.r1y)*this.r1z-this.i1e12*this.r1x*this.r1x+this.i1e02*this.r1x*this.r1y;
        k22+=this.i1e00*this.r1y*this.r1y-(this.i1e10+this.i1e01)*this.r1x*this.r1y+this.i1e11*this.r1x*this.r1x;

        k00+=this.i2e11*this.r2z*this.r2z-(this.i2e21+this.i2e12)*this.r2y*this.r2z+this.i2e22*this.r2y*this.r2y;
        k01+=(this.i2e20*this.r2y+this.i2e12*this.r2x)*this.r2z-this.i2e10*this.r2z*this.r2z-this.i2e22*this.r2x*this.r2y;
        k02+=(this.i2e10*this.r2y-this.i2e11*this.r2x)*this.r2z-this.i2e20*this.r2y*this.r2y+this.i2e21*this.r2x*this.r2y;
        k10+=(this.i2e02*this.r2y+this.i2e21*this.r2x)*this.r2z-this.i2e01*this.r2z*this.r2z-this.i2e22*this.r2x*this.r2y;
        k11+=this.i2e00*this.r2z*this.r2z-(this.i2e20+this.i2e02)*this.r2x*this.r2z+this.i2e22*this.r2x*this.r2x;
        k12+=(this.i2e01*this.r2x-this.i2e00*this.r2y)*this.r2z-this.i2e21*this.r2x*this.r2x+this.i2e20*this.r2x*this.r2y;
        k20+=(this.i2e01*this.r2y-this.i2e11*this.r2x)*this.r2z-this.i2e02*this.r2y*this.r2y+this.i2e12*this.r2x*this.r2y;
        k21+=(this.i2e10*this.r2x-this.i2e00*this.r2y)*this.r2z-this.i2e12*this.r2x*this.r2x+this.i2e02*this.r2x*this.r2y;
        k22+=this.i2e00*this.r2y*this.r2y-(this.i2e10+this.i2e01)*this.r2x*this.r2y+this.i2e11*this.r2x*this.r2x;

        var inv=1/(
        k00*(k11*k22-k21*k12)+
        k10*(k21*k02-k01*k22)+
        k20*(k01*k12-k11*k02)
        );
        this.d00=(k11*k22-k12*k21)*inv;
        this.d01=(k02*k21-k01*k22)*inv;
        this.d02=(k01*k12-k02*k11)*inv;
        this.d10=(k12*k20-k10*k22)*inv;
        this.d11=(k00*k22-k02*k20)*inv;
        this.d12=(k02*k10-k00*k12)*inv;
        this.d20=(k10*k21-k11*k20)*inv;
        this.d21=(k01*k20-k00*k21)*inv;
        this.d22=(k00*k11-k01*k10)*inv;

        this.velx=this.p2.x-this.p1.x;
        this.vely=this.p2.y-this.p1.y;
        this.velz=this.p2.z-this.p1.z;
        var len=Math.sqrt(this.velx*this.velx+this.vely*this.vely+this.velz*this.velz);
        if(len>0.005){
            len=(0.005-len)/len*invTimeStep*0.05;
            this.velx*=len;
            this.vely*=len;
            this.velz*=len;
        }else{
            this.velx=0;
            this.vely=0;
            this.velz=0;
        }

        this.impx*=0.95;
        this.impy*=0.95;
        this.impz*=0.95;
        
        this.l1.x+=this.impx*this.m1;
        this.l1.y+=this.impy*this.m1;
        this.l1.z+=this.impz*this.m1;
        this.a1.x+=this.impx*this.ax1x+this.impy*this.ay1x+this.impz*this.az1x;
        this.a1.y+=this.impx*this.ax1y+this.impy*this.ay1y+this.impz*this.az1y;
        this.a1.z+=this.impx*this.ax1z+this.impy*this.ay1z+this.impz*this.az1z;
        this.l2.x-=this.impx*this.m2;
        this.l2.y-=this.impy*this.m2;
        this.l2.z-=this.impz*this.m2;
        this.a2.x-=this.impx*this.ax2x+this.impy*this.ay2x+this.impz*this.az2x;
        this.a2.y-=this.impx*this.ax2y+this.impy*this.ay2y+this.impz*this.az2y;
        this.a2.z-=this.impx*this.ax2z+this.impy*this.ay2z+this.impz*this.az2z;
    },
    solve:function(){
        var rvx=this.l2.x-this.l1.x+this.a2.y*this.r2z-this.a2.z*this.r2y-this.a1.y*this.r1z+this.a1.z*this.r1y-this.velx;
        var rvy=this.l2.y-this.l1.y+this.a2.z*this.r2x-this.a2.x*this.r2z-this.a1.z*this.r1x+this.a1.x*this.r1z-this.vely;
        var rvz=this.l2.z-this.l1.z+this.a2.x*this.r2y-this.a2.y*this.r2x-this.a1.x*this.r1y+this.a1.y*this.r1x-this.velz;
        var nimpx=rvx*this.d00+rvy*this.d01+rvz*this.d02;
        var nimpy=rvx*this.d10+rvy*this.d11+rvz*this.d12;
        var nimpz=rvx*this.d20+rvy*this.d21+rvz*this.d22;
        this.impx+=nimpx;
        this.impy+=nimpy;
        this.impz+=nimpz;
        this.l1.x+=nimpx*this.m1;
        this.l1.y+=nimpy*this.m1;
        this.l1.z+=nimpz*this.m1;
        this.a1.x+=nimpx*this.ax1x+nimpy*this.ay1x+nimpz*this.az1x;
        this.a1.y+=nimpx*this.ax1y+nimpy*this.ay1y+nimpz*this.az1y;
        this.a1.z+=nimpx*this.ax1z+nimpy*this.ay1z+nimpz*this.az1z;
        this.l2.x-=nimpx*this.m2;
        this.l2.y-=nimpy*this.m2;
        this.l2.z-=nimpz*this.m2;
        this.a2.x-=nimpx*this.ax2x+nimpy*this.ay2x+nimpz*this.az2x;
        this.a2.y-=nimpx*this.ax2y+nimpy*this.ay2y+nimpz*this.az2y;
        this.a2.z-=nimpx*this.ax2z+nimpy*this.ay2z+nimpz*this.az2z;
    }
}
/**
* A three-axis rotational constraint for various joints.
* @author saharan
*/
OIMO.Rotational3Constraint = function(joint,limitMotor1,limitMotor2,limitMotor3){
    this.cfm1=NaN;
    this.cfm2=NaN;
    this.cfm3=NaN;
    this.i1e00=NaN;
    this.i1e01=NaN;
    this.i1e02=NaN;
    this.i1e10=NaN;
    this.i1e11=NaN;
    this.i1e12=NaN;
    this.i1e20=NaN;
    this.i1e21=NaN;
    this.i1e22=NaN;
    this.i2e00=NaN;
    this.i2e01=NaN;
    this.i2e02=NaN;
    this.i2e10=NaN;
    this.i2e11=NaN;
    this.i2e12=NaN;
    this.i2e20=NaN;
    this.i2e21=NaN;
    this.i2e22=NaN;
    this.ax1=NaN;
    this.ay1=NaN;
    this.az1=NaN;
    this.ax2=NaN;
    this.ay2=NaN;
    this.az2=NaN;
    this.ax3=NaN;
    this.ay3=NaN;
    this.az3=NaN;

    this.a1x1=NaN; // jacoians
    this.a1y1=NaN;
    this.a1z1=NaN;
    this.a2x1=NaN;
    this.a2y1=NaN;
    this.a2z1=NaN;
    this.a1x2=NaN;
    this.a1y2=NaN;
    this.a1z2=NaN;
    this.a2x2=NaN;
    this.a2y2=NaN;
    this.a2z2=NaN;
    this.a1x3=NaN;
    this.a1y3=NaN;
    this.a1z3=NaN;
    this.a2x3=NaN;
    this.a2y3=NaN;
    this.a2z3=NaN;

    this.lowerLimit1=NaN;
    this.upperLimit1=NaN;
    this.limitVelocity1=NaN;
    this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: free
    this.enableMotor1=false;
    this.motorSpeed1=NaN;
    this.maxMotorForce1=NaN;
    this.maxMotorImpulse1=NaN;
    this.lowerLimit2=NaN;
    this.upperLimit2=NaN;
    this.limitVelocity2=NaN;
    this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: free
    this.enableMotor2=false;
    this.motorSpeed2=NaN;
    this.maxMotorForce2=NaN;
    this.maxMotorImpulse2=NaN;
    this.lowerLimit3=NaN;
    this.upperLimit3=NaN;
    this.limitVelocity3=NaN;
    this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: free
    this.enableMotor3=false;
    this.motorSpeed3=NaN;
    this.maxMotorForce3=NaN;
    this.maxMotorImpulse3=NaN;

    this.k00=NaN; // K = J*M*JT
    this.k01=NaN;
    this.k02=NaN;
    this.k10=NaN;
    this.k11=NaN;
    this.k12=NaN;
    this.k20=NaN;
    this.k21=NaN;
    this.k22=NaN;

    this.kv00=NaN; // diagonals without CFMs
    this.kv11=NaN;
    this.kv22=NaN;

    this.dv00=NaN; // ...inverted
    this.dv11=NaN;
    this.dv22=NaN;

    this.d00=NaN;  // K^-1
    this.d01=NaN;
    this.d02=NaN;
    this.d10=NaN;
    this.d11=NaN;
    this.d12=NaN;
    this.d20=NaN;
    this.d21=NaN;
    this.d22=NaN;

    this.limitMotor1=limitMotor1;
    this.limitMotor2=limitMotor2;
    this.limitMotor3=limitMotor3;
    this.b1=joint.body1;
    this.b2=joint.body2;
    this.a1=this.b1.angularVelocity;
    this.a2=this.b2.angularVelocity;
    this.i1=this.b1.inverseInertia;
    this.i2=this.b2.inverseInertia;
    this.limitImpulse1=0;
    this.motorImpulse1=0;
    this.limitImpulse2=0;
    this.motorImpulse2=0;
    this.limitImpulse3=0;
    this.motorImpulse3=0;
}
OIMO.Rotational3Constraint.prototype = {
    constructor: OIMO.Rotational3Constraint,

    preSolve:function(timeStep,invTimeStep){
        this.ax1=this.limitMotor1.axis.x;
        this.ay1=this.limitMotor1.axis.y;
        this.az1=this.limitMotor1.axis.z;
        this.ax2=this.limitMotor2.axis.x;
        this.ay2=this.limitMotor2.axis.y;
        this.az2=this.limitMotor2.axis.z;
        this.ax3=this.limitMotor3.axis.x;
        this.ay3=this.limitMotor3.axis.y;
        this.az3=this.limitMotor3.axis.z;
        this.lowerLimit1=this.limitMotor1.lowerLimit;
        this.upperLimit1=this.limitMotor1.upperLimit;
        this.motorSpeed1=this.limitMotor1.motorSpeed;
        this.maxMotorForce1=this.limitMotor1.maxMotorForce;
        this.enableMotor1=this.maxMotorForce1>0;
        this.lowerLimit2=this.limitMotor2.lowerLimit;
        this.upperLimit2=this.limitMotor2.upperLimit;
        this.motorSpeed2=this.limitMotor2.motorSpeed;
        this.maxMotorForce2=this.limitMotor2.maxMotorForce;
        this.enableMotor2=this.maxMotorForce2>0;
        this.lowerLimit3=this.limitMotor3.lowerLimit;
        this.upperLimit3=this.limitMotor3.upperLimit;
        this.motorSpeed3=this.limitMotor3.motorSpeed;
        this.maxMotorForce3=this.limitMotor3.maxMotorForce;
        this.enableMotor3=this.maxMotorForce3>0;

        var ti1 = this.i1.elements;
        var ti2 = this.i2.elements;
        this.i1e00=ti1[0];
        this.i1e01=ti1[1];
        this.i1e02=ti1[2];
        this.i1e10=ti1[3];
        this.i1e11=ti1[4];
        this.i1e12=ti1[5];
        this.i1e20=ti1[6];
        this.i1e21=ti1[7];
        this.i1e22=ti1[8];

        this.i2e00=ti2[0];
        this.i2e01=ti2[1];
        this.i2e02=ti2[2];
        this.i2e10=ti2[3];
        this.i2e11=ti2[4];
        this.i2e12=ti2[5];
        this.i2e20=ti2[6];
        this.i2e21=ti2[7];
        this.i2e22=ti2[8];

        var frequency1=this.limitMotor1.frequency;
        var frequency2=this.limitMotor2.frequency;
        var frequency3=this.limitMotor3.frequency;
        var enableSpring1=frequency1>0;
        var enableSpring2=frequency2>0;
        var enableSpring3=frequency3>0;
        var enableLimit1=this.lowerLimit1<=this.upperLimit1;
        var enableLimit2=this.lowerLimit2<=this.upperLimit2;
        var enableLimit3=this.lowerLimit3<=this.upperLimit3;
        var angle1=this.limitMotor1.angle;
        if(enableLimit1){
            if(this.lowerLimit1==this.upperLimit1){
                if(this.limitState1!=0){
                    this.limitState1=0;
                    this.limitImpulse1=0;
                }
                this.limitVelocity1=this.lowerLimit1-angle1;
            }else if(angle1<this.lowerLimit1){
                if(this.limitState1!=-1){
                    this.limitState1=-1;
                    this.limitImpulse1=0;
                }
                this.limitVelocity1=this.lowerLimit1-angle1;
            }else if(angle1>this.upperLimit1){
                if(this.limitState1!=1){
                    this.limitState1=1;
                    this.limitImpulse1=0;
                }
                this.limitVelocity1=this.upperLimit1-angle1;
            }else{
                this.limitState1=2;
                this.limitImpulse1=0;
                this.limitVelocity1=0;
            }
            if(!enableSpring1){
                if(this.limitVelocity1>0.02)this.limitVelocity1-=0.02;
                else if(this.limitVelocity1<-0.02)this.limitVelocity1+=0.02;
                else this.limitVelocity1=0;
            }
        }else{
            this.limitState1=2;
            this.limitImpulse1=0;
        }

        var angle2=this.limitMotor2.angle;
        if(enableLimit2){
            if(this.lowerLimit2==this.upperLimit2){
                if(this.limitState2!=0){
                    this.limitState2=0;
                    this.limitImpulse2=0;
                }
                this.limitVelocity2=this.lowerLimit2-angle2;
            }else if(angle2<this.lowerLimit2){
                if(this.limitState2!=-1){
                    this.limitState2=-1;
                    this.limitImpulse2=0;
                }
                this.limitVelocity2=this.lowerLimit2-angle2;
            }else if(angle2>this.upperLimit2){
                if(this.limitState2!=1){
                    this.limitState2=1;
                    this.limitImpulse2=0;
                }
                this.limitVelocity2=this.upperLimit2-angle2;
            }else{
                this.limitState2=2;
                this.limitImpulse2=0;
                this.limitVelocity2=0;
            }
            if(!enableSpring2){
                if(this.limitVelocity2>0.02)this.limitVelocity2-=0.02;
                else if(this.limitVelocity2<-0.02)this.limitVelocity2+=0.02;
                else this.limitVelocity2=0;
            }
        }else{
            this.limitState2=2;
            this.limitImpulse2=0;
        }

        var angle3=this.limitMotor3.angle;
        if(enableLimit3){
            if(this.lowerLimit3==this.upperLimit3){
                if(this.limitState3!=0){
                    this.limitState3=0;
                    this.limitImpulse3=0;
                }
                this.limitVelocity3=this.lowerLimit3-angle3;
            }else if(angle3<this.lowerLimit3){
                if(this.limitState3!=-1){
                    this.limitState3=-1;
                    this.limitImpulse3=0;
                }
                this.limitVelocity3=this.lowerLimit3-angle3;
            }else if(angle3>this.upperLimit3){
                if(this.limitState3!=1){
                    this.limitState3=1;
                    this.limitImpulse3=0;
                }
                this.limitVelocity3=this.upperLimit3-angle3;
            }else{
                this.limitState3=2;
                this.limitImpulse3=0;
                this.limitVelocity3=0;
                }
            if(!enableSpring3){
                if(this.limitVelocity3>0.02)this.limitVelocity3-=0.02;
                else if(this.limitVelocity3<-0.02)this.limitVelocity3+=0.02;
                else this.limitVelocity3=0;
            }
        }else{
            this.limitState3=2;
            this.limitImpulse3=0;
        }

        if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){
            this.maxMotorImpulse1=this.maxMotorForce1*timeStep;
        }else{
            this.motorImpulse1=0;
            this.maxMotorImpulse1=0;
        }
        if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){
            this.maxMotorImpulse2=this.maxMotorForce2*timeStep;
        }else{
            this.motorImpulse2=0;
            this.maxMotorImpulse2=0;
        }
        if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){
            this.maxMotorImpulse3=this.maxMotorForce3*timeStep;
        }else{
            this.motorImpulse3=0;
            this.maxMotorImpulse3=0;
        }

        // build jacobians
        this.a1x1=this.ax1*this.i1e00+this.ay1*this.i1e01+this.az1*this.i1e02;
        this.a1y1=this.ax1*this.i1e10+this.ay1*this.i1e11+this.az1*this.i1e12;
        this.a1z1=this.ax1*this.i1e20+this.ay1*this.i1e21+this.az1*this.i1e22;
        this.a2x1=this.ax1*this.i2e00+this.ay1*this.i2e01+this.az1*this.i2e02;
        this.a2y1=this.ax1*this.i2e10+this.ay1*this.i2e11+this.az1*this.i2e12;
        this.a2z1=this.ax1*this.i2e20+this.ay1*this.i2e21+this.az1*this.i2e22;

        this.a1x2=this.ax2*this.i1e00+this.ay2*this.i1e01+this.az2*this.i1e02;
        this.a1y2=this.ax2*this.i1e10+this.ay2*this.i1e11+this.az2*this.i1e12;
        this.a1z2=this.ax2*this.i1e20+this.ay2*this.i1e21+this.az2*this.i1e22;
        this.a2x2=this.ax2*this.i2e00+this.ay2*this.i2e01+this.az2*this.i2e02;
        this.a2y2=this.ax2*this.i2e10+this.ay2*this.i2e11+this.az2*this.i2e12;
        this.a2z2=this.ax2*this.i2e20+this.ay2*this.i2e21+this.az2*this.i2e22;

        this.a1x3=this.ax3*this.i1e00+this.ay3*this.i1e01+this.az3*this.i1e02;
        this.a1y3=this.ax3*this.i1e10+this.ay3*this.i1e11+this.az3*this.i1e12;
        this.a1z3=this.ax3*this.i1e20+this.ay3*this.i1e21+this.az3*this.i1e22;
        this.a2x3=this.ax3*this.i2e00+this.ay3*this.i2e01+this.az3*this.i2e02;
        this.a2y3=this.ax3*this.i2e10+this.ay3*this.i2e11+this.az3*this.i2e12;
        this.a2z3=this.ax3*this.i2e20+this.ay3*this.i2e21+this.az3*this.i2e22;

        // build an impulse matrix
        this.k00=this.ax1*(this.a1x1+this.a2x1)+this.ay1*(this.a1y1+this.a2y1)+this.az1*(this.a1z1+this.a2z1);
        this.k01=this.ax1*(this.a1x2+this.a2x2)+this.ay1*(this.a1y2+this.a2y2)+this.az1*(this.a1z2+this.a2z2);
        this.k02=this.ax1*(this.a1x3+this.a2x3)+this.ay1*(this.a1y3+this.a2y3)+this.az1*(this.a1z3+this.a2z3);
        this.k10=this.ax2*(this.a1x1+this.a2x1)+this.ay2*(this.a1y1+this.a2y1)+this.az2*(this.a1z1+this.a2z1);
        this.k11=this.ax2*(this.a1x2+this.a2x2)+this.ay2*(this.a1y2+this.a2y2)+this.az2*(this.a1z2+this.a2z2);
        this.k12=this.ax2*(this.a1x3+this.a2x3)+this.ay2*(this.a1y3+this.a2y3)+this.az2*(this.a1z3+this.a2z3);
        this.k20=this.ax3*(this.a1x1+this.a2x1)+this.ay3*(this.a1y1+this.a2y1)+this.az3*(this.a1z1+this.a2z1);
        this.k21=this.ax3*(this.a1x2+this.a2x2)+this.ay3*(this.a1y2+this.a2y2)+this.az3*(this.a1z2+this.a2z2);
        this.k22=this.ax3*(this.a1x3+this.a2x3)+this.ay3*(this.a1y3+this.a2y3)+this.az3*(this.a1z3+this.a2z3);

        this.kv00=this.k00;
        this.kv11=this.k11;
        this.kv22=this.k22;
        this.dv00=1/this.kv00;
        this.dv11=1/this.kv11;
        this.dv22=1/this.kv22;

        if(enableSpring1&&this.limitState1!=2){
            var omega=6.2831853*frequency1;
            var k=omega*omega*timeStep;
            var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega);
            this.cfm1=this.kv00*dmp;
            this.limitVelocity1*=k*dmp;
        }else{
            this.cfm1=0;
            this.limitVelocity1*=invTimeStep*0.05;
        }

        if(enableSpring2&&this.limitState2!=2){
            omega=6.2831853*frequency2;
            k=omega*omega*timeStep;
            dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega);
            this.cfm2=this.kv11*dmp;
            this.limitVelocity2*=k*dmp;
        }else{
            this.cfm2=0;
            this.limitVelocity2*=invTimeStep*0.05;
        }

        if(enableSpring3&&this.limitState3!=2){
            omega=6.2831853*frequency3;
            k=omega*omega*timeStep;
            dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega);
            this.cfm3=this.kv22*dmp;
            this.limitVelocity3*=k*dmp;
        }else{
            this.cfm3=0;
            this.limitVelocity3*=invTimeStep*0.05;
        }

        this.k00+=this.cfm1;
        this.k11+=this.cfm2;
        this.k22+=this.cfm3;

        var inv=1/(
        this.k00*(this.k11*this.k22-this.k21*this.k12)+
        this.k10*(this.k21*this.k02-this.k01*this.k22)+
        this.k20*(this.k01*this.k12-this.k11*this.k02)
        );
        this.d00=(this.k11*this.k22-this.k12*this.k21)*inv;
        this.d01=(this.k02*this.k21-this.k01*this.k22)*inv;
        this.d02=(this.k01*this.k12-this.k02*this.k11)*inv;
        this.d10=(this.k12*this.k20-this.k10*this.k22)*inv;
        this.d11=(this.k00*this.k22-this.k02*this.k20)*inv;
        this.d12=(this.k02*this.k10-this.k00*this.k12)*inv;
        this.d20=(this.k10*this.k21-this.k11*this.k20)*inv;
        this.d21=(this.k01*this.k20-this.k00*this.k21)*inv;
        this.d22=(this.k00*this.k11-this.k01*this.k10)*inv;
        
        this.limitImpulse1*=0.95;
        this.motorImpulse1*=0.95;
        this.limitImpulse2*=0.95;
        this.motorImpulse2*=0.95;
        this.limitImpulse3*=0.95;
        this.motorImpulse3*=0.95;
        var totalImpulse1=this.limitImpulse1+this.motorImpulse1;
        var totalImpulse2=this.limitImpulse2+this.motorImpulse2;
        var totalImpulse3=this.limitImpulse3+this.motorImpulse3;
        this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3;
        this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3;
        this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3;
        this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3;
        this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3;
        this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3;
    },
    solve_:function(){
        var rvx=this.a2.x-this.a1.x;
        var rvy=this.a2.y-this.a1.y;
        var rvz=this.a2.z-this.a1.z;

        this.limitVelocity3=30;
        var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1-this.limitVelocity1;
        var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2-this.limitVelocity2;
        var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3-this.limitVelocity3;

        var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02;
        var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12;
        var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22;

        this.limitImpulse1+=dLimitImpulse1;
        this.limitImpulse2+=dLimitImpulse2;
        this.limitImpulse3+=dLimitImpulse3;

        this.a1.x+=dLimitImpulse1*this.a1x1+dLimitImpulse2*this.a1x2+dLimitImpulse3*this.a1x3;
        this.a1.y+=dLimitImpulse1*this.a1y1+dLimitImpulse2*this.a1y2+dLimitImpulse3*this.a1y3;
        this.a1.z+=dLimitImpulse1*this.a1z1+dLimitImpulse2*this.a1z2+dLimitImpulse3*this.a1z3;
        this.a2.x-=dLimitImpulse1*this.a2x1+dLimitImpulse2*this.a2x2+dLimitImpulse3*this.a2x3;
        this.a2.y-=dLimitImpulse1*this.a2y1+dLimitImpulse2*this.a2y2+dLimitImpulse3*this.a2y3;
        this.a2.z-=dLimitImpulse1*this.a2z1+dLimitImpulse2*this.a2z2+dLimitImpulse3*this.a2z3;
    },
    solve:function(){
        var rvx=this.a2.x-this.a1.x;
        var rvy=this.a2.y-this.a1.y;
        var rvz=this.a2.z-this.a1.z;

        var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1;
        var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2;
        var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3;

        var oldMotorImpulse1=this.motorImpulse1;
        var oldMotorImpulse2=this.motorImpulse2;
        var oldMotorImpulse3=this.motorImpulse3;

        var dMotorImpulse1=0;
        var dMotorImpulse2=0;
        var dMotorImpulse3=0;

        if(this.enableMotor1){
            dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00;
            this.motorImpulse1+=dMotorImpulse1;
            if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse
            this.motorImpulse1=this.maxMotorImpulse1;
            }else if(this.motorImpulse1<-this.maxMotorImpulse1){
            this.motorImpulse1=-this.maxMotorImpulse1;
            }
            dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1;
        }
        if(this.enableMotor2){
            dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11;
            this.motorImpulse2+=dMotorImpulse2;
            if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse
                this.motorImpulse2=this.maxMotorImpulse2;
            }else if(this.motorImpulse2<-this.maxMotorImpulse2){
                this.motorImpulse2=-this.maxMotorImpulse2;
            }
            dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2;
        }
        if(this.enableMotor3){
            dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22;
            this.motorImpulse3+=dMotorImpulse3;
            if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse
                this.motorImpulse3=this.maxMotorImpulse3;
            }else if(this.motorImpulse3<-this.maxMotorImpulse3){
                this.motorImpulse3=-this.maxMotorImpulse3;
            }
            dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3;
        }

        // apply motor impulse to relative velocity
        rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02;
        rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12;
        rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22;

        // subtract target velocity and applied impulse
        rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1;
        rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2;
        rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3;

        var oldLimitImpulse1=this.limitImpulse1;
        var oldLimitImpulse2=this.limitImpulse2;
        var oldLimitImpulse3=this.limitImpulse3;

        var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02;
        var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12;
        var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22;

        this.limitImpulse1+=dLimitImpulse1;
        this.limitImpulse2+=dLimitImpulse2;
        this.limitImpulse3+=dLimitImpulse3;

        // clamp
        var clampState=0;
        if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){
            dLimitImpulse1=-oldLimitImpulse1;
            rvn2+=dLimitImpulse1*this.k10;
            rvn3+=dLimitImpulse1*this.k20;
            clampState|=1;
        }
        if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){
            dLimitImpulse2=-oldLimitImpulse2;
            rvn1+=dLimitImpulse2*this.k01;
            rvn3+=dLimitImpulse2*this.k21;
            clampState|=2;
        }
        if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){
            dLimitImpulse3=-oldLimitImpulse3;
            rvn1+=dLimitImpulse3*this.k02;
            rvn2+=dLimitImpulse3*this.k12;
            clampState|=4;
        }

        // update un-clamped impulse
        // TODO: isolate division
        var det;
        switch(clampState){
            case 1: // update 2 3
            det=1/(this.k11*this.k22-this.k12*this.k21);
            dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det;
            dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det;
            break;
            case 2: // update 1 3
            det=1/(this.k00*this.k22-this.k02*this.k20);
            dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det;
            dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det;
            break;
            case 3: // update 3
            dLimitImpulse3=rvn3/this.k22;
            break;
            case 4: // update 1 2
            det=1/(this.k00*this.k11-this.k01*this.k10);
            dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det;
            dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det;
            break;
            case 5: // update 2
            dLimitImpulse2=rvn2/this.k11;
            break;
            case 6: // update 1
            dLimitImpulse1=rvn1/this.k00;
            break;
        }

        this.limitImpulse1=dLimitImpulse1+oldLimitImpulse1;
        this.limitImpulse2=dLimitImpulse2+oldLimitImpulse2;
        this.limitImpulse3=dLimitImpulse3+oldLimitImpulse3;

        var dImpulse1=dMotorImpulse1+dLimitImpulse1;
        var dImpulse2=dMotorImpulse2+dLimitImpulse2;
        var dImpulse3=dMotorImpulse3+dLimitImpulse3;

        // apply impulse
        this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3;
        this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3;
        this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3;
        this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3;
        this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3;
        this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3;
        rvx=this.a2.x-this.a1.x;
        rvy=this.a2.y-this.a1.y;
        rvz=this.a2.z-this.a1.z;

        rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2;
    }
}
/**
* A rotational constraint for various joints.
* @author saharan
*/
OIMO.RotationalConstraint = function(joint,limitMotor){
    this.cfm=NaN;
    this.i1e00=NaN;
    this.i1e01=NaN;
    this.i1e02=NaN;
    this.i1e10=NaN;
    this.i1e11=NaN;
    this.i1e12=NaN;
    this.i1e20=NaN;
    this.i1e21=NaN;
    this.i1e22=NaN;
    this.i2e00=NaN;
    this.i2e01=NaN;
    this.i2e02=NaN;
    this.i2e10=NaN;
    this.i2e11=NaN;
    this.i2e12=NaN;
    this.i2e20=NaN;
    this.i2e21=NaN;
    this.i2e22=NaN;
    this.motorDenom=NaN;
    this.invMotorDenom=NaN;
    this.invDenom=NaN;
    this.ax=NaN;
    this.ay=NaN;
    this.az=NaN;
    this.a1x=NaN;
    this.a1y=NaN;
    this.a1z=NaN;
    this.a2x=NaN;
    this.a2y=NaN;
    this.a2z=NaN;
    this.enableLimit=false;
    this.lowerLimit=NaN;
    this.upperLimit=NaN;
    this.limitVelocity=NaN;
    this.limitState=0; // -1: at lower, 0: locked, 1: at upper, 2: free
    this.enableMotor=false;
    this.motorSpeed=NaN;
    this.maxMotorForce=NaN;
    this.maxMotorImpulse=NaN;

    this.limitMotor=limitMotor;
    this.b1=joint.body1;
    this.b2=joint.body2;
    this.a1=this.b1.angularVelocity;
    this.a2=this.b2.angularVelocity;
    this.i1=this.b1.inverseInertia;
    this.i2=this.b2.inverseInertia;
    this.limitImpulse=0;
    this.motorImpulse=0;
}
OIMO.RotationalConstraint.prototype = {
    constructor: OIMO.RotationalConstraint,

    preSolve:function(timeStep,invTimeStep){
        this.ax=this.limitMotor.axis.x;
        this.ay=this.limitMotor.axis.y;
        this.az=this.limitMotor.axis.z;
        this.lowerLimit=this.limitMotor.lowerLimit;
        this.upperLimit=this.limitMotor.upperLimit;
        this.motorSpeed=this.limitMotor.motorSpeed;
        this.maxMotorForce=this.limitMotor.maxMotorForce;
        this.enableMotor=this.maxMotorForce>0;

        var ti1 = this.i1.elements;
        var ti2 = this.i2.elements;
        this.i1e00=ti1[0];
        this.i1e01=ti1[1];
        this.i1e02=ti1[2];
        this.i1e10=ti1[3];
        this.i1e11=ti1[4];
        this.i1e12=ti1[5];
        this.i1e20=ti1[6];
        this.i1e21=ti1[7];
        this.i1e22=ti1[8];

        this.i2e00=ti2[0];
        this.i2e01=ti2[1];
        this.i2e02=ti2[2];
        this.i2e10=ti2[3];
        this.i2e11=ti2[4];
        this.i2e12=ti2[5];
        this.i2e20=ti2[6];
        this.i2e21=ti2[7];
        this.i2e22=ti2[8];

        var frequency=this.limitMotor.frequency;
        var enableSpring=frequency>0;
        var enableLimit=this.lowerLimit<=this.upperLimit;
        var angle=this.limitMotor.angle;
        if(enableLimit){
            if(this.lowerLimit==this.upperLimit){
                if(this.limitState!=0){
                    this.limitState=0;
                    this.limitImpulse=0;
                }
                this.limitVelocity=this.lowerLimit-angle;
            }else if(angle<this.lowerLimit){
                if(this.limitState!=-1){
                    this.limitState=-1;
                    this.limitImpulse=0;
                }
                this.limitVelocity=this.lowerLimit-angle;
            }else if(angle>this.upperLimit){
                if(this.limitState!=1){
                    this.limitState=1;
                    this.limitImpulse=0;
                }
                this.limitVelocity=this.upperLimit-angle;
            }else{
                this.limitState=2;
                this.limitImpulse=0;
                this.limitVelocity=0;
            }
            if(!enableSpring){
                if(this.limitVelocity>0.02)this.limitVelocity-=0.02;
                else if(this.limitVelocity<-0.02)this.limitVelocity+=0.02;
                else this.limitVelocity=0;
            }
        }else{
            this.limitState=2;
            this.limitImpulse=0;
        }
        if(this.enableMotor&&(this.limitState!=0||enableSpring)){
            this.maxMotorImpulse=this.maxMotorForce*timeStep;
        }else{
            this.motorImpulse=0;
            this.maxMotorImpulse=0;
        }

        this.a1x=this.ax*this.i1e00+this.ay*this.i1e01+this.az*this.i1e02;
        this.a1y=this.ax*this.i1e10+this.ay*this.i1e11+this.az*this.i1e12;
        this.a1z=this.ax*this.i1e20+this.ay*this.i1e21+this.az*this.i1e22;
        this.a2x=this.ax*this.i2e00+this.ay*this.i2e01+this.az*this.i2e02;
        this.a2y=this.ax*this.i2e10+this.ay*this.i2e11+this.az*this.i2e12;
        this.a2z=this.ax*this.i2e20+this.ay*this.i2e21+this.az*this.i2e22;
        this.motorDenom=this.ax*(this.a1x+this.a2x)+this.ay*(this.a1y+this.a2y)+this.az*(this.a1z+this.a2z);
        this.invMotorDenom=1/this.motorDenom;

        if(enableSpring&&this.limitState!=2){
            var omega=6.2831853*frequency;
            var k=omega*omega*timeStep;
            var dmp=invTimeStep/(k+2*this.limitMotor.dampingRatio*omega);
            this.cfm=this.motorDenom*dmp;
            this.limitVelocity*=k*dmp;
        }else{
            this.cfm=0;
            this.limitVelocity*=invTimeStep*0.05;
        }

        this.invDenom=1/(this.motorDenom+this.cfm);
        
        this.limitImpulse*=0.95;
        this.motorImpulse*=0.95;
        var totalImpulse=this.limitImpulse+this.motorImpulse;
        this.a1.x+=totalImpulse*this.a1x;
        this.a1.y+=totalImpulse*this.a1y;
        this.a1.z+=totalImpulse*this.a1z;
        this.a2.x-=totalImpulse*this.a2x;
        this.a2.y-=totalImpulse*this.a2y;
        this.a2.z-=totalImpulse*this.a2z;
    },
    solve:function(){
        var rvn=this.ax*(this.a2.x-this.a1.x)+this.ay*(this.a2.y-this.a1.y)+this.az*(this.a2.z-this.a1.z);

        // motor part
        var newMotorImpulse;
        if(this.enableMotor){
            newMotorImpulse=(rvn-this.motorSpeed)*this.invMotorDenom;
            var oldMotorImpulse=this.motorImpulse;
            this.motorImpulse+=newMotorImpulse;
            if(this.motorImpulse>this.maxMotorImpulse)this.motorImpulse=this.maxMotorImpulse;
            else if(this.motorImpulse<-this.maxMotorImpulse)this.motorImpulse=-this.maxMotorImpulse;
            newMotorImpulse=this.motorImpulse-oldMotorImpulse;
            rvn-=newMotorImpulse*this.motorDenom;
        }else newMotorImpulse=0;

        // limit part
        var newLimitImpulse;
        if(this.limitState!=2){
            newLimitImpulse=(rvn-this.limitVelocity-this.limitImpulse*this.cfm)*this.invDenom;
            var oldLimitImpulse=this.limitImpulse;
            this.limitImpulse+=newLimitImpulse;
            if(this.limitImpulse*this.limitState<0)this.limitImpulse=0;
            newLimitImpulse=this.limitImpulse-oldLimitImpulse;
        }else newLimitImpulse=0;

        var totalImpulse=newLimitImpulse+newMotorImpulse;
        this.a1.x+=totalImpulse*this.a1x;
        this.a1.y+=totalImpulse*this.a1y;
        this.a1.z+=totalImpulse*this.a1z;
        this.a2.x-=totalImpulse*this.a2x;
        this.a2.y-=totalImpulse*this.a2y;
        this.a2.z-=totalImpulse*this.a2z;
    }
}
/**
* A three-axis translational constraint for various joints.
* @author saharan
*/
OIMO.Translational3Constraint = function(joint,limitMotor1,limitMotor2,limitMotor3){
    this.m1=NaN;
    this.m2=NaN;
    this.i1e00=NaN;
    this.i1e01=NaN;
    this.i1e02=NaN;
    this.i1e10=NaN;
    this.i1e11=NaN;
    this.i1e12=NaN;
    this.i1e20=NaN;
    this.i1e21=NaN;
    this.i1e22=NaN;
    this.i2e00=NaN;
    this.i2e01=NaN;
    this.i2e02=NaN;
    this.i2e10=NaN;
    this.i2e11=NaN;
    this.i2e12=NaN;
    this.i2e20=NaN;
    this.i2e21=NaN;
    this.i2e22=NaN;
    this.ax1=NaN;
    this.ay1=NaN;
    this.az1=NaN;
    this.ax2=NaN;
    this.ay2=NaN;
    this.az2=NaN;
    this.ax3=NaN;
    this.ay3=NaN;
    this.az3=NaN;
    this.r1x=NaN;
    this.r1y=NaN;
    this.r1z=NaN;
    this.r2x=NaN;
    this.r2y=NaN;
    this.r2z=NaN;
    this.t1x1=NaN;// jacobians
    this.t1y1=NaN;
    this.t1z1=NaN;
    this.t2x1=NaN;
    this.t2y1=NaN;
    this.t2z1=NaN;
    this.l1x1=NaN;
    this.l1y1=NaN;
    this.l1z1=NaN;
    this.l2x1=NaN;
    this.l2y1=NaN;
    this.l2z1=NaN;
    this.a1x1=NaN;
    this.a1y1=NaN;
    this.a1z1=NaN;
    this.a2x1=NaN;
    this.a2y1=NaN;
    this.a2z1=NaN;
    this.t1x2=NaN;
    this.t1y2=NaN;
    this.t1z2=NaN;
    this.t2x2=NaN;
    this.t2y2=NaN;
    this.t2z2=NaN;
    this.l1x2=NaN;
    this.l1y2=NaN;
    this.l1z2=NaN;
    this.l2x2=NaN;
    this.l2y2=NaN;
    this.l2z2=NaN;
    this.a1x2=NaN;
    this.a1y2=NaN;
    this.a1z2=NaN;
    this.a2x2=NaN;
    this.a2y2=NaN;
    this.a2z2=NaN;
    this.t1x3=NaN;
    this.t1y3=NaN;
    this.t1z3=NaN;
    this.t2x3=NaN;
    this.t2y3=NaN;
    this.t2z3=NaN;
    this.l1x3=NaN;
    this.l1y3=NaN;
    this.l1z3=NaN;
    this.l2x3=NaN;
    this.l2y3=NaN;
    this.l2z3=NaN;
    this.a1x3=NaN;
    this.a1y3=NaN;
    this.a1z3=NaN;
    this.a2x3=NaN;
    this.a2y3=NaN;
    this.a2z3=NaN;
    this.lowerLimit1=NaN;
    this.upperLimit1=NaN;
    this.limitVelocity1=NaN;
    this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited
    this.enableMotor1=false;
    this.motorSpeed1=NaN;
    this.maxMotorForce1=NaN;
    this.maxMotorImpulse1=NaN;
    this.lowerLimit2=NaN;
    this.upperLimit2=NaN;
    this.limitVelocity2=NaN;
    this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited
    this.enableMotor2=false;
    this.motorSpeed2=NaN;
    this.maxMotorForce2=NaN;
    this.maxMotorImpulse2=NaN;
    this.lowerLimit3=NaN;
    this.upperLimit3=NaN;
    this.limitVelocity3=NaN;
    this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited
    this.enableMotor3=false;
    this.motorSpeed3=NaN;
    this.maxMotorForce3=NaN;
    this.maxMotorImpulse3=NaN;
    this.k00=NaN; // K = J*M*JT
    this.k01=NaN;
    this.k02=NaN;
    this.k10=NaN;
    this.k11=NaN;
    this.k12=NaN;
    this.k20=NaN;
    this.k21=NaN;
    this.k22=NaN;
    this.kv00=NaN; // diagonals without CFMs
    this.kv11=NaN;
    this.kv22=NaN;
    this.dv00=NaN; // ...inverted
    this.dv11=NaN;
    this.dv22=NaN;
    this.d00=NaN; // K^-1
    this.d01=NaN;
    this.d02=NaN;
    this.d10=NaN;
    this.d11=NaN;
    this.d12=NaN;
    this.d20=NaN;
    this.d21=NaN;
    this.d22=NaN;

    this.limitMotor1=limitMotor1;
    this.limitMotor2=limitMotor2;
    this.limitMotor3=limitMotor3;
    this.b1=joint.body1;
    this.b2=joint.body2;
    this.p1=joint.anchorPoint1;
    this.p2=joint.anchorPoint2;
    this.r1=joint.relativeAnchorPoint1;
    this.r2=joint.relativeAnchorPoint2;
    this.l1=this.b1.linearVelocity;
    this.l2=this.b2.linearVelocity;
    this.a1=this.b1.angularVelocity;
    this.a2=this.b2.angularVelocity;
    this.i1=this.b1.inverseInertia;
    this.i2=this.b2.inverseInertia;
    this.limitImpulse1=0;
    this.motorImpulse1=0;
    this.limitImpulse2=0;
    this.motorImpulse2=0;
    this.limitImpulse3=0;
    this.motorImpulse3=0;
    this.cfm1=0;// Constraint Force Mixing
    this.cfm2=0;
    this.cfm3=0;
    this.weight=-1;
}
OIMO.Translational3Constraint.prototype = {
    constructor: OIMO.Translational3Constraint,

    preSolve:function(timeStep,invTimeStep){
        this.ax1=this.limitMotor1.axis.x;
        this.ay1=this.limitMotor1.axis.y;
        this.az1=this.limitMotor1.axis.z;
        this.ax2=this.limitMotor2.axis.x;
        this.ay2=this.limitMotor2.axis.y;
        this.az2=this.limitMotor2.axis.z;
        this.ax3=this.limitMotor3.axis.x;
        this.ay3=this.limitMotor3.axis.y;
        this.az3=this.limitMotor3.axis.z;
        this.lowerLimit1=this.limitMotor1.lowerLimit;
        this.upperLimit1=this.limitMotor1.upperLimit;
        this.motorSpeed1=this.limitMotor1.motorSpeed;
        this.maxMotorForce1=this.limitMotor1.maxMotorForce;
        this.enableMotor1=this.maxMotorForce1>0;
        this.lowerLimit2=this.limitMotor2.lowerLimit;
        this.upperLimit2=this.limitMotor2.upperLimit;
        this.motorSpeed2=this.limitMotor2.motorSpeed;
        this.maxMotorForce2=this.limitMotor2.maxMotorForce;
        this.enableMotor2=this.maxMotorForce2>0;
        this.lowerLimit3=this.limitMotor3.lowerLimit;
        this.upperLimit3=this.limitMotor3.upperLimit;
        this.motorSpeed3=this.limitMotor3.motorSpeed;
        this.maxMotorForce3=this.limitMotor3.maxMotorForce;
        this.enableMotor3=this.maxMotorForce3>0;
        this.m1=this.b1.inverseMass;
        this.m2=this.b2.inverseMass;

        var ti1 = this.i1.elements;
        var ti2 = this.i2.elements;
        this.i1e00=ti1[0];
        this.i1e01=ti1[1];
        this.i1e02=ti1[2];
        this.i1e10=ti1[3];
        this.i1e11=ti1[4];
        this.i1e12=ti1[5];
        this.i1e20=ti1[6];
        this.i1e21=ti1[7];
        this.i1e22=ti1[8];

        this.i2e00=ti2[0];
        this.i2e01=ti2[1];
        this.i2e02=ti2[2];
        this.i2e10=ti2[3];
        this.i2e11=ti2[4];
        this.i2e12=ti2[5];
        this.i2e20=ti2[6];
        this.i2e21=ti2[7];
        this.i2e22=ti2[8];

        var dx=this.p2.x-this.p1.x;
        var dy=this.p2.y-this.p1.y;
        var dz=this.p2.z-this.p1.z;
        var d1=dx*this.ax1+dy*this.ay1+dz*this.az1;
        var d2=dx*this.ax2+dy*this.ay2+dz*this.az2;
        var d3=dx*this.ax3+dy*this.ay3+dz*this.az3;
        var frequency1=this.limitMotor1.frequency;
        var frequency2=this.limitMotor2.frequency;
        var frequency3=this.limitMotor3.frequency;
        var enableSpring1=frequency1>0;
        var enableSpring2=frequency2>0;
        var enableSpring3=frequency3>0;
        var enableLimit1=this.lowerLimit1<=this.upperLimit1;
        var enableLimit2=this.lowerLimit2<=this.upperLimit2;
        var enableLimit3=this.lowerLimit3<=this.upperLimit3;

        // for stability
        if(enableSpring1&&d1>20||d1<-20){
            enableSpring1=false;
        }
        if(enableSpring2&&d2>20||d2<-20){
            enableSpring2=false;
        }
        if(enableSpring3&&d3>20||d3<-20){
            enableSpring3=false;
        }

        if(enableLimit1){
            if(this.lowerLimit1==this.upperLimit1){
                if(this.limitState1!=0){
                    this.limitState1=0;
                    this.limitImpulse1=0;
                }
                this.limitVelocity1=this.lowerLimit1-d1;
                if(!enableSpring1)d1=this.lowerLimit1;
            }else if(d1<this.lowerLimit1){
                if(this.limitState1!=-1){
                    this.limitState1=-1;
                    this.limitImpulse1=0;
                }
                this.limitVelocity1=this.lowerLimit1-d1;
                if(!enableSpring1)d1=this.lowerLimit1;
            }else if(d1>this.upperLimit1){
                if(this.limitState1!=1){
                    this.limitState1=1;
                    this.limitImpulse1=0;
                }
                this.limitVelocity1=this.upperLimit1-d1;
                if(!enableSpring1)d1=this.upperLimit1;
            }else{
                this.limitState1=2;
                this.limitImpulse1=0;
                this.limitVelocity1=0;
            }
            if(!enableSpring1){
                if(this.limitVelocity1>0.005)this.limitVelocity1-=0.005;
                else if(this.limitVelocity1<-0.005)this.limitVelocity1+=0.005;
                else this.limitVelocity1=0;
            }
        }else{
            this.limitState1=2;
            this.limitImpulse1=0;
        }

        if(enableLimit2){
            if(this.lowerLimit2==this.upperLimit2){
                if(this.limitState2!=0){
                    this.limitState2=0;
                    this.limitImpulse2=0;
                }
                this.limitVelocity2=this.lowerLimit2-d2;
                if(!enableSpring2)d2=this.lowerLimit2;
            }else if(d2<this.lowerLimit2){
                if(this.limitState2!=-1){
                    this.limitState2=-1;
                    this.limitImpulse2=0;
                }
                this.limitVelocity2=this.lowerLimit2-d2;
                if(!enableSpring2)d2=this.lowerLimit2;
            }else if(d2>this.upperLimit2){
                if(this.limitState2!=1){
                    this.limitState2=1;
                    this.limitImpulse2=0;
                }
                this.limitVelocity2=this.upperLimit2-d2;
                if(!enableSpring2)d2=this.upperLimit2;
            }else{
                this.limitState2=2;
                this.limitImpulse2=0;
                this.limitVelocity2=0;
            }
            if(!enableSpring2){
                if(this.limitVelocity2>0.005)this.limitVelocity2-=0.005;
                else if(this.limitVelocity2<-0.005)this.limitVelocity2+=0.005;
                else this.limitVelocity2=0;
            }
        }else{
            this.limitState2=2;
            this.limitImpulse2=0;
        }

        if(enableLimit3){
            if(this.lowerLimit3==this.upperLimit3){
                if(this.limitState3!=0){
                    this.limitState3=0;
                    this.limitImpulse3=0;
                }
                this.limitVelocity3=this.lowerLimit3-d3;
                if(!enableSpring3)d3=this.lowerLimit3;
                }else if(d3<this.lowerLimit3){
                if(this.limitState3!=-1){
                    this.limitState3=-1;
                    this.limitImpulse3=0;
                }
                this.limitVelocity3=this.lowerLimit3-d3;
                if(!enableSpring3)d3=this.lowerLimit3;
            }else if(d3>this.upperLimit3){
                if(this.limitState3!=1){
                    this.limitState3=1;
                    this.limitImpulse3=0;
                }
                this.limitVelocity3=this.upperLimit3-d3;
                if(!enableSpring3)d3=this.upperLimit3;
            }else{
                this.limitState3=2;
                this.limitImpulse3=0;
                this.limitVelocity3=0;
            }
            if(!enableSpring3){
                if(this.limitVelocity3>0.005)this.limitVelocity3-=0.005;
                else if(this.limitVelocity3<-0.005)this.limitVelocity3+=0.005;
                else this.limitVelocity3=0;
            }
        }else{
            this.limitState3=2;
            this.limitImpulse3=0;
        }

        if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){
            this.maxMotorImpulse1=this.maxMotorForce1*timeStep;
        }else{
            this.motorImpulse1=0;
            this.maxMotorImpulse1=0;
        }

        if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){
            this.maxMotorImpulse2=this.maxMotorForce2*timeStep;
        }else{
            this.motorImpulse2=0;
            this.maxMotorImpulse2=0;
        }

        if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){
            this.maxMotorImpulse3=this.maxMotorForce3*timeStep;
        }else{
            this.motorImpulse3=0;
            this.maxMotorImpulse3=0;
        }
        
        var rdx=d1*this.ax1+d2*this.ax2+d3*this.ax2;
        var rdy=d1*this.ay1+d2*this.ay2+d3*this.ay2;
        var rdz=d1*this.az1+d2*this.az2+d3*this.az2;
        var w1=this.m2/(this.m1+this.m2);
        if(this.weight>=0)w1=this.weight; // use given weight
        var w2=1-w1;
        this.r1x=this.r1.x+rdx*w1;
        this.r1y=this.r1.y+rdy*w1;
        this.r1z=this.r1.z+rdz*w1;
        this.r2x=this.r2.x-rdx*w2;
        this.r2y=this.r2.y-rdy*w2;
        this.r2z=this.r2.z-rdz*w2;

        // build jacobians
        this.t1x1=this.r1y*this.az1-this.r1z*this.ay1;
        this.t1y1=this.r1z*this.ax1-this.r1x*this.az1;
        this.t1z1=this.r1x*this.ay1-this.r1y*this.ax1;
        this.t2x1=this.r2y*this.az1-this.r2z*this.ay1;
        this.t2y1=this.r2z*this.ax1-this.r2x*this.az1;
        this.t2z1=this.r2x*this.ay1-this.r2y*this.ax1;
        this.l1x1=this.ax1*this.m1;
        this.l1y1=this.ay1*this.m1;
        this.l1z1=this.az1*this.m1;
        this.l2x1=this.ax1*this.m2;
        this.l2y1=this.ay1*this.m2;
        this.l2z1=this.az1*this.m2;
        this.a1x1=this.t1x1*this.i1e00+this.t1y1*this.i1e01+this.t1z1*this.i1e02;
        this.a1y1=this.t1x1*this.i1e10+this.t1y1*this.i1e11+this.t1z1*this.i1e12;
        this.a1z1=this.t1x1*this.i1e20+this.t1y1*this.i1e21+this.t1z1*this.i1e22;
        this.a2x1=this.t2x1*this.i2e00+this.t2y1*this.i2e01+this.t2z1*this.i2e02;
        this.a2y1=this.t2x1*this.i2e10+this.t2y1*this.i2e11+this.t2z1*this.i2e12;
        this.a2z1=this.t2x1*this.i2e20+this.t2y1*this.i2e21+this.t2z1*this.i2e22;

        this.t1x2=this.r1y*this.az2-this.r1z*this.ay2;
        this.t1y2=this.r1z*this.ax2-this.r1x*this.az2;
        this.t1z2=this.r1x*this.ay2-this.r1y*this.ax2;
        this.t2x2=this.r2y*this.az2-this.r2z*this.ay2;
        this.t2y2=this.r2z*this.ax2-this.r2x*this.az2;
        this.t2z2=this.r2x*this.ay2-this.r2y*this.ax2;
        this.l1x2=this.ax2*this.m1;
        this.l1y2=this.ay2*this.m1;
        this.l1z2=this.az2*this.m1;
        this.l2x2=this.ax2*this.m2;
        this.l2y2=this.ay2*this.m2;
        this.l2z2=this.az2*this.m2;
        this.a1x2=this.t1x2*this.i1e00+this.t1y2*this.i1e01+this.t1z2*this.i1e02;
        this.a1y2=this.t1x2*this.i1e10+this.t1y2*this.i1e11+this.t1z2*this.i1e12;
        this.a1z2=this.t1x2*this.i1e20+this.t1y2*this.i1e21+this.t1z2*this.i1e22;
        this.a2x2=this.t2x2*this.i2e00+this.t2y2*this.i2e01+this.t2z2*this.i2e02;
        this.a2y2=this.t2x2*this.i2e10+this.t2y2*this.i2e11+this.t2z2*this.i2e12;
        this.a2z2=this.t2x2*this.i2e20+this.t2y2*this.i2e21+this.t2z2*this.i2e22;

        this.t1x3=this.r1y*this.az3-this.r1z*this.ay3;
        this.t1y3=this.r1z*this.ax3-this.r1x*this.az3;
        this.t1z3=this.r1x*this.ay3-this.r1y*this.ax3;
        this.t2x3=this.r2y*this.az3-this.r2z*this.ay3;
        this.t2y3=this.r2z*this.ax3-this.r2x*this.az3;
        this.t2z3=this.r2x*this.ay3-this.r2y*this.ax3;
        this.l1x3=this.ax3*this.m1;
        this.l1y3=this.ay3*this.m1;
        this.l1z3=this.az3*this.m1;
        this.l2x3=this.ax3*this.m2;
        this.l2y3=this.ay3*this.m2;
        this.l2z3=this.az3*this.m2;
        this.a1x3=this.t1x3*this.i1e00+this.t1y3*this.i1e01+this.t1z3*this.i1e02;
        this.a1y3=this.t1x3*this.i1e10+this.t1y3*this.i1e11+this.t1z3*this.i1e12;
        this.a1z3=this.t1x3*this.i1e20+this.t1y3*this.i1e21+this.t1z3*this.i1e22;
        this.a2x3=this.t2x3*this.i2e00+this.t2y3*this.i2e01+this.t2z3*this.i2e02;
        this.a2y3=this.t2x3*this.i2e10+this.t2y3*this.i2e11+this.t2z3*this.i2e12;
        this.a2z3=this.t2x3*this.i2e20+this.t2y3*this.i2e21+this.t2z3*this.i2e22;

        // build an impulse matrix
        var m12=this.m1+this.m2;
        this.k00=(this.ax1*this.ax1+this.ay1*this.ay1+this.az1*this.az1)*m12;
        this.k01=(this.ax1*this.ax2+this.ay1*this.ay2+this.az1*this.az2)*m12;
        this.k02=(this.ax1*this.ax3+this.ay1*this.ay3+this.az1*this.az3)*m12;
        this.k10=(this.ax2*this.ax1+this.ay2*this.ay1+this.az2*this.az1)*m12;
        this.k11=(this.ax2*this.ax2+this.ay2*this.ay2+this.az2*this.az2)*m12;
        this.k12=(this.ax2*this.ax3+this.ay2*this.ay3+this.az2*this.az3)*m12;
        this.k20=(this.ax3*this.ax1+this.ay3*this.ay1+this.az3*this.az1)*m12;
        this.k21=(this.ax3*this.ax2+this.ay3*this.ay2+this.az3*this.az2)*m12;
        this.k22=(this.ax3*this.ax3+this.ay3*this.ay3+this.az3*this.az3)*m12;

        this.k00+=this.t1x1*this.a1x1+this.t1y1*this.a1y1+this.t1z1*this.a1z1;
        this.k01+=this.t1x1*this.a1x2+this.t1y1*this.a1y2+this.t1z1*this.a1z2;
        this.k02+=this.t1x1*this.a1x3+this.t1y1*this.a1y3+this.t1z1*this.a1z3;
        this.k10+=this.t1x2*this.a1x1+this.t1y2*this.a1y1+this.t1z2*this.a1z1;
        this.k11+=this.t1x2*this.a1x2+this.t1y2*this.a1y2+this.t1z2*this.a1z2;
        this.k12+=this.t1x2*this.a1x3+this.t1y2*this.a1y3+this.t1z2*this.a1z3;
        this.k20+=this.t1x3*this.a1x1+this.t1y3*this.a1y1+this.t1z3*this.a1z1;
        this.k21+=this.t1x3*this.a1x2+this.t1y3*this.a1y2+this.t1z3*this.a1z2;
        this.k22+=this.t1x3*this.a1x3+this.t1y3*this.a1y3+this.t1z3*this.a1z3;

        this.k00+=this.t2x1*this.a2x1+this.t2y1*this.a2y1+this.t2z1*this.a2z1;
        this.k01+=this.t2x1*this.a2x2+this.t2y1*this.a2y2+this.t2z1*this.a2z2;
        this.k02+=this.t2x1*this.a2x3+this.t2y1*this.a2y3+this.t2z1*this.a2z3;
        this.k10+=this.t2x2*this.a2x1+this.t2y2*this.a2y1+this.t2z2*this.a2z1;
        this.k11+=this.t2x2*this.a2x2+this.t2y2*this.a2y2+this.t2z2*this.a2z2;
        this.k12+=this.t2x2*this.a2x3+this.t2y2*this.a2y3+this.t2z2*this.a2z3;
        this.k20+=this.t2x3*this.a2x1+this.t2y3*this.a2y1+this.t2z3*this.a2z1;
        this.k21+=this.t2x3*this.a2x2+this.t2y3*this.a2y2+this.t2z3*this.a2z2;
        this.k22+=this.t2x3*this.a2x3+this.t2y3*this.a2y3+this.t2z3*this.a2z3;

        this.kv00=this.k00;
        this.kv11=this.k11;
        this.kv22=this.k22;

        this.dv00=1/this.kv00;
        this.dv11=1/this.kv11;
        this.dv22=1/this.kv22;

        if(enableSpring1&&this.limitState1!=2){
            var omega=6.2831853*frequency1;
            var k=omega*omega*timeStep;
            var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega);
            this.cfm1=this.kv00*dmp;
            this.limitVelocity1*=k*dmp;
        }else{
            this.cfm1=0;
            this.limitVelocity1*=invTimeStep*0.05;
        }
        if(enableSpring2&&this.limitState2!=2){
            omega=6.2831853*frequency2;
            k=omega*omega*timeStep;
            dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega);
            this.cfm2=this.kv11*dmp;
            this.limitVelocity2*=k*dmp;
        }else{
            this.cfm2=0;
            this.limitVelocity2*=invTimeStep*0.05;
        }
        if(enableSpring3&&this.limitState3!=2){
            omega=6.2831853*frequency3;
            k=omega*omega*timeStep;
            dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega);
            this.cfm3=this.kv22*dmp;
            this.limitVelocity3*=k*dmp;
        }else{
            this.cfm3=0;
            this.limitVelocity3*=invTimeStep*0.05;
        }
        this.k00+=this.cfm1;
        this.k11+=this.cfm2;
        this.k22+=this.cfm3;

        var inv=1/(
        this.k00*(this.k11*this.k22-this.k21*this.k12)+
        this.k10*(this.k21*this.k02-this.k01*this.k22)+
        this.k20*(this.k01*this.k12-this.k11*this.k02)
        );
        this.d00=(this.k11*this.k22-this.k12*this.k21)*inv;
        this.d01=(this.k02*this.k21-this.k01*this.k22)*inv;
        this.d02=(this.k01*this.k12-this.k02*this.k11)*inv;
        this.d10=(this.k12*this.k20-this.k10*this.k22)*inv;
        this.d11=(this.k00*this.k22-this.k02*this.k20)*inv;
        this.d12=(this.k02*this.k10-this.k00*this.k12)*inv;
        this.d20=(this.k10*this.k21-this.k11*this.k20)*inv;
        this.d21=(this.k01*this.k20-this.k00*this.k21)*inv;
        this.d22=(this.k00*this.k11-this.k01*this.k10)*inv;

        // warm starting
        var totalImpulse1=this.limitImpulse1+this.motorImpulse1;
        var totalImpulse2=this.limitImpulse2+this.motorImpulse2;
        var totalImpulse3=this.limitImpulse3+this.motorImpulse3;
        this.l1.x+=totalImpulse1*this.l1x1+totalImpulse2*this.l1x2+totalImpulse3*this.l1x3;
        this.l1.y+=totalImpulse1*this.l1y1+totalImpulse2*this.l1y2+totalImpulse3*this.l1y3;
        this.l1.z+=totalImpulse1*this.l1z1+totalImpulse2*this.l1z2+totalImpulse3*this.l1z3;
        this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3;
        this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3;
        this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3;
        this.l2.x-=totalImpulse1*this.l2x1+totalImpulse2*this.l2x2+totalImpulse3*this.l2x3;
        this.l2.y-=totalImpulse1*this.l2y1+totalImpulse2*this.l2y2+totalImpulse3*this.l2y3;
        this.l2.z-=totalImpulse1*this.l2z1+totalImpulse2*this.l2z2+totalImpulse3*this.l2z3;
        this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3;
        this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3;
        this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3;
    },

    solve:function(){
        var rvx=this.l2.x-this.l1.x+this.a2.y*this.r2z-this.a2.z*this.r2y-this.a1.y*this.r1z+this.a1.z*this.r1y;
        var rvy=this.l2.y-this.l1.y+this.a2.z*this.r2x-this.a2.x*this.r2z-this.a1.z*this.r1x+this.a1.x*this.r1z;
        var rvz=this.l2.z-this.l1.z+this.a2.x*this.r2y-this.a2.y*this.r2x-this.a1.x*this.r1y+this.a1.y*this.r1x;
        var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1;
        var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2;
        var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3;
        var oldMotorImpulse1=this.motorImpulse1;
        var oldMotorImpulse2=this.motorImpulse2;
        var oldMotorImpulse3=this.motorImpulse3;
        var dMotorImpulse1=0;
        var dMotorImpulse2=0;
        var dMotorImpulse3=0;
        if(this.enableMotor1){
            dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00;
            this.motorImpulse1+=dMotorImpulse1;
            if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse
                this.motorImpulse1=this.maxMotorImpulse1;
            }else if(this.motorImpulse1<-this.maxMotorImpulse1){
                this.motorImpulse1=-this.maxMotorImpulse1;
            }
            dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1;
        }
        if(this.enableMotor2){
            dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11;
            this.motorImpulse2+=dMotorImpulse2;
            if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse
                this.motorImpulse2=this.maxMotorImpulse2;
            }else if(this.motorImpulse2<-this.maxMotorImpulse2){
                this.motorImpulse2=-this.maxMotorImpulse2;
            }
            dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2;
        }
        if(this.enableMotor3){
            dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22;
            this.motorImpulse3+=dMotorImpulse3;
            if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse
                this.motorImpulse3=this.maxMotorImpulse3;
            }else if(this.motorImpulse3<-this.maxMotorImpulse3){
                this.motorImpulse3=-this.maxMotorImpulse3;
            }
            dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3;
        }

        // apply motor impulse to relative velocity
        rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02;
        rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12;
        rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22;

        // subtract target velocity and applied impulse
        rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1;
        rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2;
        rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3;

        var oldLimitImpulse1=this.limitImpulse1;
        var oldLimitImpulse2=this.limitImpulse2;
        var oldLimitImpulse3=this.limitImpulse3;

        var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02;
        var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12;
        var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22;

        this.limitImpulse1+=dLimitImpulse1;
        this.limitImpulse2+=dLimitImpulse2;
        this.limitImpulse3+=dLimitImpulse3;

        // clamp
        var clampState=0;
        if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){
            dLimitImpulse1=-oldLimitImpulse1;
            rvn2+=dLimitImpulse1*this.k10;
            rvn3+=dLimitImpulse1*this.k20;
            clampState|=1;
        }
        if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){
            dLimitImpulse2=-oldLimitImpulse2;
            rvn1+=dLimitImpulse2*this.k01;
            rvn3+=dLimitImpulse2*this.k21;
            clampState|=2;
        }
        if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){
            dLimitImpulse3=-oldLimitImpulse3;
            rvn1+=dLimitImpulse3*this.k02;
            rvn2+=dLimitImpulse3*this.k12;
            clampState|=4;
        }

        // update un-clamped impulse
        // TODO: isolate division
        var det;
        switch(clampState){
            case 1:// update 2 3
            det=1/(this.k11*this.k22-this.k12*this.k21);
            dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det;
            dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det;
            break;
            case 2:// update 1 3
            det=1/(this.k00*this.k22-this.k02*this.k20);
            dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det;
            dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det;
            break;
            case 3:// update 3
            dLimitImpulse3=rvn3/this.k22;
            break;
            case 4:// update 1 2
            det=1/(this.k00*this.k11-this.k01*this.k10);
            dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det;
            dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det;
            break;
            case 5:// update 2
            dLimitImpulse2=rvn2/this.k11;
            break;
            case 6:// update 1
            dLimitImpulse1=rvn1/this.k00;
            break;
        }

        this.limitImpulse1=oldLimitImpulse1+dLimitImpulse1;
        this.limitImpulse2=oldLimitImpulse2+dLimitImpulse2;
        this.limitImpulse3=oldLimitImpulse3+dLimitImpulse3;

        var dImpulse1=dMotorImpulse1+dLimitImpulse1;
        var dImpulse2=dMotorImpulse2+dLimitImpulse2;
        var dImpulse3=dMotorImpulse3+dLimitImpulse3;

        // apply impulse
        this.l1.x+=dImpulse1*this.l1x1+dImpulse2*this.l1x2+dImpulse3*this.l1x3;
        this.l1.y+=dImpulse1*this.l1y1+dImpulse2*this.l1y2+dImpulse3*this.l1y3;
        this.l1.z+=dImpulse1*this.l1z1+dImpulse2*this.l1z2+dImpulse3*this.l1z3;
        this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3;
        this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3;
        this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3;
        this.l2.x-=dImpulse1*this.l2x1+dImpulse2*this.l2x2+dImpulse3*this.l2x3;
        this.l2.y-=dImpulse1*this.l2y1+dImpulse2*this.l2y2+dImpulse3*this.l2y3;
        this.l2.z-=dImpulse1*this.l2z1+dImpulse2*this.l2z2+dImpulse3*this.l2z3;
        this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3;
        this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3;
        this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3;
    }
}
/**
* A translational constraint for various joints.
* @author saharan
*/
OIMO.TranslationalConstraint = function(joint,limitMotor){
    this.cfm=NaN;
    this.m1=NaN;
    this.m2=NaN;
    this.i1e00=NaN;
    this.i1e01=NaN;
    this.i1e02=NaN;
    this.i1e10=NaN;
    this.i1e11=NaN;
    this.i1e12=NaN;
    this.i1e20=NaN;
    this.i1e21=NaN;
    this.i1e22=NaN;
    this.i2e00=NaN;
    this.i2e01=NaN;
    this.i2e02=NaN;
    this.i2e10=NaN;
    this.i2e11=NaN;
    this.i2e12=NaN;
    this.i2e20=NaN;
    this.i2e21=NaN;
    this.i2e22=NaN;
    this.motorDenom=NaN;
    this.invMotorDenom=NaN;
    this.invDenom=NaN;
    this.ax=NaN;
    this.ay=NaN;
    this.az=NaN;
    this.r1x=NaN;
    this.r1y=NaN;
    this.r1z=NaN;
    this.r2x=NaN;
    this.r2y=NaN;
    this.r2z=NaN;
    this.t1x=NaN;
    this.t1y=NaN;
    this.t1z=NaN;
    this.t2x=NaN;
    this.t2y=NaN;
    this.t2z=NaN;
    this.l1x=NaN;
    this.l1y=NaN;
    this.l1z=NaN;
    this.l2x=NaN;
    this.l2y=NaN;
    this.l2z=NaN;
    this.a1x=NaN;
    this.a1y=NaN;
    this.a1z=NaN;
    this.a2x=NaN;
    this.a2y=NaN;
    this.a2z=NaN;
    this.lowerLimit=NaN;
    this.upperLimit=NaN;
    this.limitVelocity=NaN;
    this.limitState=0; // -1: at lower, 0: locked, 1: at upper, 2: free
    this.enableMotor=false;
    this.motorSpeed=NaN;
    this.maxMotorForce=NaN;
    this.maxMotorImpulse=NaN;

    this.limitMotor=limitMotor;
    this.b1=joint.body1;
    this.b2=joint.body2;
    this.p1=joint.anchorPoint1;
    this.p2=joint.anchorPoint2;
    this.r1=joint.relativeAnchorPoint1;
    this.r2=joint.relativeAnchorPoint2;
    this.l1=this.b1.linearVelocity;
    this.l2=this.b2.linearVelocity;
    this.a1=this.b1.angularVelocity;
    this.a2=this.b2.angularVelocity;
    this.i1=this.b1.inverseInertia;
    this.i2=this.b2.inverseInertia;
    this.limitImpulse=0;
    this.motorImpulse=0;
}
OIMO.TranslationalConstraint.prototype = {
    constructor: OIMO.TranslationalConstraint,

    preSolve:function(timeStep,invTimeStep){
        this.ax=this.limitMotor.axis.x;
        this.ay=this.limitMotor.axis.y;
        this.az=this.limitMotor.axis.z;
        this.lowerLimit=this.limitMotor.lowerLimit;
        this.upperLimit=this.limitMotor.upperLimit;
        this.motorSpeed=this.limitMotor.motorSpeed;
        this.maxMotorForce=this.limitMotor.maxMotorForce;
        this.enableMotor=this.maxMotorForce>0;
        this.m1=this.b1.inverseMass;
        this.m2=this.b2.inverseMass;

        var ti1 = this.i1.elements;
        var ti2 = this.i2.elements;
        this.i1e00=ti1[0];
        this.i1e01=ti1[1];
        this.i1e02=ti1[2];
        this.i1e10=ti1[3];
        this.i1e11=ti1[4];
        this.i1e12=ti1[5];
        this.i1e20=ti1[6];
        this.i1e21=ti1[7];
        this.i1e22=ti1[8];

        this.i2e00=ti2[0];
        this.i2e01=ti2[1];
        this.i2e02=ti2[2];
        this.i2e10=ti2[3];
        this.i2e11=ti2[4];
        this.i2e12=ti2[5];
        this.i2e20=ti2[6];
        this.i2e21=ti2[7];
        this.i2e22=ti2[8];

        var dx=this.p2.x-this.p1.x;
        var dy=this.p2.y-this.p1.y;
        var dz=this.p2.z-this.p1.z;
        var d=dx*this.ax+dy*this.ay+dz*this.az;
        var frequency=this.limitMotor.frequency;
        var enableSpring=frequency>0;
        var enableLimit=this.lowerLimit<=this.upperLimit;
        if(enableSpring&&d>20||d<-20){
            enableSpring=false;
        }

        if(enableLimit){
            if(this.lowerLimit==this.upperLimit){
                if(this.limitState!=0){
                    this.limitState=0;
                    this.limitImpulse=0;
                }
                this.limitVelocity=this.lowerLimit-d;
                if(!enableSpring)d=this.lowerLimit;
            }else if(d<this.lowerLimit){
                if(this.limitState!=-1){
                    this.limitState=-1;
                    this.limitImpulse=0;
                }
                this.limitVelocity=this.lowerLimit-d;
                if(!enableSpring)d=this.lowerLimit;
            }else if(d>this.upperLimit){
                if(this.limitState!=1){
                    this.limitState=1;
                    this.limitImpulse=0;
                }
                this.limitVelocity=this.upperLimit-d;
                if(!enableSpring)d=this.upperLimit;
            }else{
                this.limitState=2;
                this.limitImpulse=0;
                this.limitVelocity=0;
            }
            if(!enableSpring){
                if(this.limitVelocity>0.005)this.limitVelocity-=0.005;
                else if(this.limitVelocity<-0.005)this.limitVelocity+=0.005;
                else this.limitVelocity=0;
            }
        }else{
            this.limitState=2;
            this.limitImpulse=0;
        }

        if(this.enableMotor&&(this.limitState!=0||enableSpring)){
            this.maxMotorImpulse=this.maxMotorForce*timeStep;
        }else{
            this.motorImpulse=0;
            this.maxMotorImpulse=0;
        }

        var rdx=d*this.ax;
        var rdy=d*this.ay;
        var rdz=d*this.az;
        var w1=this.m1/(this.m1+this.m2);
        var w2=1-w1;
        this.r1x=this.r1.x+rdx*w1;
        this.r1y=this.r1.y+rdy*w1;
        this.r1z=this.r1.z+rdz*w1;
        this.r2x=this.r2.x-rdx*w2;
        this.r2y=this.r2.y-rdy*w2;
        this.r2z=this.r2.z-rdz*w2;

        this.t1x=this.r1y*this.az-this.r1z*this.ay;
        this.t1y=this.r1z*this.ax-this.r1x*this.az;
        this.t1z=this.r1x*this.ay-this.r1y*this.ax;
        this.t2x=this.r2y*this.az-this.r2z*this.ay;
        this.t2y=this.r2z*this.ax-this.r2x*this.az;
        this.t2z=this.r2x*this.ay-this.r2y*this.ax;
        this.l1x=this.ax*this.m1;
        this.l1y=this.ay*this.m1;
        this.l1z=this.az*this.m1;
        this.l2x=this.ax*this.m2;
        this.l2y=this.ay*this.m2;
        this.l2z=this.az*this.m2;
        this.a1x=this.t1x*this.i1e00+this.t1y*this.i1e01+this.t1z*this.i1e02;
        this.a1y=this.t1x*this.i1e10+this.t1y*this.i1e11+this.t1z*this.i1e12;
        this.a1z=this.t1x*this.i1e20+this.t1y*this.i1e21+this.t1z*this.i1e22;
        this.a2x=this.t2x*this.i2e00+this.t2y*this.i2e01+this.t2z*this.i2e02;
        this.a2y=this.t2x*this.i2e10+this.t2y*this.i2e11+this.t2z*this.i2e12;
        this.a2z=this.t2x*this.i2e20+this.t2y*this.i2e21+this.t2z*this.i2e22;
        this.motorDenom=
        this.m1+this.m2+
            this.ax*(this.a1y*this.r1z-this.a1z*this.r1y+this.a2y*this.r2z-this.a2z*this.r2y)+
            this.ay*(this.a1z*this.r1x-this.a1x*this.r1z+this.a2z*this.r2x-this.a2x*this.r2z)+
            this.az*(this.a1x*this.r1y-this.a1y*this.r1x+this.a2x*this.r2y-this.a2y*this.r2x);

        this.invMotorDenom=1/this.motorDenom;

        if(enableSpring&&this.limitState!=2){
            var omega=6.2831853*frequency;
            var k=omega*omega*timeStep;
            var dmp=invTimeStep/(k+2*this.limitMotor.dampingRatio*omega);
            this.cfm=this.motorDenom*dmp;
            this.limitVelocity*=k*dmp;
        }else{
            this.cfm=0;
            this.limitVelocity*=invTimeStep*0.05;
        }

        this.invDenom=1/(this.motorDenom+this.cfm);

        var totalImpulse=this.limitImpulse+this.motorImpulse;
        this.l1.x+=totalImpulse*this.l1x;
        this.l1.y+=totalImpulse*this.l1y;
        this.l1.z+=totalImpulse*this.l1z;
        this.a1.x+=totalImpulse*this.a1x;
        this.a1.y+=totalImpulse*this.a1y;
        this.a1.z+=totalImpulse*this.a1z;
        this.l2.x-=totalImpulse*this.l2x;
        this.l2.y-=totalImpulse*this.l2y;
        this.l2.z-=totalImpulse*this.l2z;
        this.a2.x-=totalImpulse*this.a2x;
        this.a2.y-=totalImpulse*this.a2y;
        this.a2.z-=totalImpulse*this.a2z;
    },
    solve:function(){
        var rvn=
            this.ax*(this.l2.x-this.l1.x)+this.ay*(this.l2.y-this.l1.y)+this.az*(this.l2.z-this.l1.z)+
            this.t2x*this.a2.x-this.t1x*this.a1.x+this.t2y*this.a2.y-this.t1y*this.a1.y+this.t2z*this.a2.z-this.t1z*this.a1.z;

        // motor part
        var newMotorImpulse;
        if(this.enableMotor){
            newMotorImpulse=(rvn-this.motorSpeed)*this.invMotorDenom;
            var oldMotorImpulse=this.motorImpulse;
            this.motorImpulse+=newMotorImpulse;
            if(this.motorImpulse>this.maxMotorImpulse)this.motorImpulse=this.maxMotorImpulse;
            else if(this.motorImpulse<-this.maxMotorImpulse)this.motorImpulse=-this.maxMotorImpulse;
            newMotorImpulse=this.motorImpulse-oldMotorImpulse;
            rvn-=newMotorImpulse*this.motorDenom;
        }else newMotorImpulse=0;

        // limit part
        var newLimitImpulse;
        if(this.limitState!=2){
            newLimitImpulse=(rvn-this.limitVelocity-this.limitImpulse*this.cfm)*this.invDenom;
            var oldLimitImpulse=this.limitImpulse;
            this.limitImpulse+=newLimitImpulse;
            if(this.limitImpulse*this.limitState<0)this.limitImpulse=0;
            newLimitImpulse=this.limitImpulse-oldLimitImpulse;
        }else newLimitImpulse=0;
        
        var totalImpulse=newLimitImpulse+newMotorImpulse;
        this.l1.x+=totalImpulse*this.l1x;
        this.l1.y+=totalImpulse*this.l1y;
        this.l1.z+=totalImpulse*this.l1z;
        this.a1.x+=totalImpulse*this.a1x;
        this.a1.y+=totalImpulse*this.a1y;
        this.a1.z+=totalImpulse*this.a1z;
        this.l2.x-=totalImpulse*this.l2x;
        this.l2.y-=totalImpulse*this.l2y;
        this.l2.z-=totalImpulse*this.l2z;
        this.a2.x-=totalImpulse*this.a2x;
        this.a2.y-=totalImpulse*this.a2y;
        this.a2.z-=totalImpulse*this.a2z;
    }
}
/**
* A contact is a pair of shapes whose axis-aligned bounding boxes are overlapping.
* @author saharan
*/
OIMO.Contact = function(){
    // The first shape.
    this.shape1=null;
    // The second shape.
    this.shape2=null;
    // The first rigid body.
    this.body1=null;
    // The second rigid body.
    this.body2=null;
    // The previous contact in the world.
    this.prev=null;
    // The next contact in the world.
    this.next=null;
    // Internal
    this.persisting=false;
    // Whether both the rigid bodies are sleeping or not.
    this.sleeping=false;
    // The collision detector between two shapes.
    this.detector=null;
    // The contact constraint of the contact.
    this.constraint=null;
    // Whether the shapes are touching or not.
    this.touching=false;

    this.b1Link=new OIMO.ContactLink(this);
    this.b2Link=new OIMO.ContactLink(this);
    this.s1Link=new OIMO.ContactLink(this);
    this.s2Link=new OIMO.ContactLink(this);
    // The contact manifold of the contact.
    this.manifold=new OIMO.ContactManifold();
    this.buffer=[];// vector 4
    this.buffer.length = 4;
    this.buffer[0]=new OIMO.ImpulseDataBuffer();
    this.buffer[1]=new OIMO.ImpulseDataBuffer();
    this.buffer[2]=new OIMO.ImpulseDataBuffer();
    this.buffer[3]=new OIMO.ImpulseDataBuffer();
    this.points=this.manifold.points;
    this.constraint=new OIMO.ContactConstraint(this.manifold);
}
OIMO.Contact.prototype = {
    constructor: OIMO.Contact,

    mixRestitution:function(restitution1,restitution2){
        return Math.sqrt(restitution1*restitution2);
    },
    mixFriction:function(friction1,friction2){
        return Math.sqrt(friction1*friction2);
    },
    /**
    * Update the contact manifold.
    */
    updateManifold:function(){
        this.constraint.restitution=this.mixRestitution(this.shape1.restitution,this.shape2.restitution);
        this.constraint.friction=this.mixFriction(this.shape1.friction,this.shape2.friction);
        var numBuffers=this.manifold.numPoints;
        var i = numBuffers;
        while(i--){
        //for(var i=0;i<numBuffers;i++){
            var b=this.buffer[i];
            var p=this.points[i];
            b.lp1X=p.localPoint1.x;
            b.lp1Y=p.localPoint1.y;
            b.lp1Z=p.localPoint1.z;
            b.lp2X=p.localPoint2.x;
            b.lp2Y=p.localPoint2.y;
            b.lp2Z=p.localPoint2.z;
            b.impulse=p.normalImpulse;
        }
        this.manifold.numPoints=0;
        this.detector.detectCollision(this.shape1,this.shape2,this.manifold);
        var num=this.manifold.numPoints;
        if(num==0){
            this.touching=false;
            return;
        }
        this.touching=true;
        i = num;
        while(i--){
        //for(i=0; i<num; i++){
            p=this.points[i];
            var lp1x=p.localPoint1.x;
            var lp1y=p.localPoint1.y;
            var lp1z=p.localPoint1.z;
            var lp2x=p.localPoint2.x;
            var lp2y=p.localPoint2.y;
            var lp2z=p.localPoint2.z;
            var index=-1;
            var minDistance=0.0004;
            var j = numBuffers;
            while(j--){
            //for(var j=0;j<numBuffers;j++){
                b=this.buffer[j];
                var dx=b.lp1X-lp1x;
                var dy=b.lp1Y-lp1y;
                var dz=b.lp1Z-lp1z;
                var distance1=dx*dx+dy*dy+dz*dz;
                dx=b.lp2X-lp2x;
                dy=b.lp2Y-lp2y;
                dz=b.lp2Z-lp2z;
                var distance2=dx*dx+dy*dy+dz*dz;
                if(distance1<distance2){
                    if(distance1<minDistance){
                        minDistance=distance1;
                        index=j;
                    }
                }else{
                    if(distance2<minDistance){
                        minDistance=distance2;
                        index=j;
                    }
                }
            }
            if(index!=-1){
                var tmp=this.buffer[index];
                this.buffer[index]=this.buffer[--numBuffers];
                this.buffer[numBuffers]=tmp;
                p.normalImpulse=tmp.impulse;
                p.warmStarted=true;
            }else{
                p.normalImpulse=0;
                p.warmStarted=false;
            }
        }
    },
    /**
    * Attach the contact to the shapes.
    * @param   shape1
    * @param   shape2
    */
    attach:function(shape1,shape2){
        this.shape1=shape1;
        this.shape2=shape2;
        this.body1=shape1.parent;
        this.body2=shape2.parent;

        this.manifold.body1=this.body1;
        this.manifold.body2=this.body2;
        this.constraint.body1=this.body1;
        this.constraint.body2=this.body2;
        this.constraint.attach();

        this.s1Link.shape=shape2;
        this.s1Link.body=this.body2;
        this.s2Link.shape=shape1;
        this.s2Link.body=this.body1;

        if(shape1.contactLink!=null)(this.s1Link.next=shape1.contactLink).prev=this.s1Link;
        else this.s1Link.next=null;
        shape1.contactLink=this.s1Link;
        shape1.numContacts++;

        if(shape2.contactLink!=null)(this.s2Link.next=shape2.contactLink).prev=this.s2Link;
        else this.s2Link.next=null;
        shape2.contactLink=this.s2Link;
        shape2.numContacts++;

        this.b1Link.shape=shape2;
        this.b1Link.body=this.body2;
        this.b2Link.shape=shape1;
        this.b2Link.body=this.body1;

        if(this.body1.contactLink!=null)(this.b1Link.next=this.body1.contactLink).prev=this.b1Link;
        else this.b1Link.next=null;
        this.body1.contactLink=this.b1Link;
        this.body1.numContacts++;

        if(this.body2.contactLink!=null)(this.b2Link.next=this.body2.contactLink).prev=this.b2Link;
        else this.b2Link.next=null;
        this.body2.contactLink=this.b2Link;
        this.body2.numContacts++;

        this.prev=null;
        this.next=null;

        this.persisting=true;
        this.sleeping=this.body1.sleeping&&this.body2.sleeping;
        this.manifold.numPoints=0;
    },
    /**
    * Detach the contact from the shapes.
    */
    detach:function(){
        var prev=this.s1Link.prev;
        var next=this.s1Link.next;
        if(prev!==null)prev.next=next;
        if(next!==null)next.prev=prev;
        if(this.shape1.contactLink==this.s1Link)this.shape1.contactLink=next;
        this.s1Link.prev=null;
        this.s1Link.next=null;
        this.s1Link.shape=null;
        this.s1Link.body=null;
        this.shape1.numContacts--;

        prev=this.s2Link.prev;
        next=this.s2Link.next;
        if(prev!==null)prev.next=next;
        if(next!==null)next.prev=prev;
        if(this.shape2.contactLink==this.s2Link)this.shape2.contactLink=next;
        this.s2Link.prev=null;
        this.s2Link.next=null;
        this.s2Link.shape=null;
        this.s2Link.body=null;
        this.shape2.numContacts--;

        prev=this.b1Link.prev;
        next=this.b1Link.next;
        if(prev!==null)prev.next=next;
        if(next!==null)next.prev=prev;
        if(this.body1.contactLink==this.b1Link)this.body1.contactLink=next;
        this.b1Link.prev=null;
        this.b1Link.next=null;
        this.b1Link.shape=null;
        this.b1Link.body=null;
        this.body1.numContacts--;

        prev=this.b2Link.prev;
        next=this.b2Link.next;
        if(prev!==null)prev.next=next;
        if(next!==null)next.prev=prev;
        if(this.body2.contactLink==this.b2Link)this.body2.contactLink=next;
        this.b2Link.prev=null;
        this.b2Link.next=null;
        this.b2Link.shape=null;
        this.b2Link.body=null;
        this.body2.numContacts--;

        this.manifold.body1=null;
        this.manifold.body2=null;
        this.constraint.body1=null;
        this.constraint.body2=null;
        this.constraint.detach();

        this.shape1=null;
        this.shape2=null;
        this.body1=null;
        this.body2=null;
    }
}
/**
* ...
* @author saharan
*/
OIMO.ContactConstraint = function(manifold){
    OIMO.Constraint.call( this);
    // The contact manifold of the constraint.
    this.manifold=manifold;
    // The coefficient of restitution of the constraint.
    this.restitution=NaN;
    // The coefficient of friction of the constraint.
    this.friction=NaN;
    this.p1=null;
    this.p2=null;
    this.lv1=null;
    this.lv2=null;
    this.av1=null;
    this.av2=null;
    this.i1=null;
    this.i2=null;
    this.i1e00=NaN;
    this.i1e01=NaN;
    this.i1e02=NaN;
    this.i1e10=NaN;
    this.i1e11=NaN;
    this.i1e12=NaN;
    this.i1e20=NaN;
    this.i1e21=NaN;
    this.i1e22=NaN;
    this.i2e00=NaN;
    this.i2e01=NaN;
    this.i2e02=NaN;
    this.i2e10=NaN;
    this.i2e11=NaN;
    this.i2e12=NaN;
    this.i2e20=NaN;
    this.i2e21=NaN;
    this.i2e22=NaN;
    this.m1=NaN;
    this.m2=NaN;
    this.num=0;
    
    this.ps=manifold.points;
    this.cs=new OIMO.ContactPointDataBuffer();
    this.cs.next=new OIMO.ContactPointDataBuffer();
    this.cs.next.next=new OIMO.ContactPointDataBuffer();
    this.cs.next.next.next=new OIMO.ContactPointDataBuffer();
}
OIMO.ContactConstraint.prototype = Object.create( OIMO.Constraint.prototype );
/**
* Attach the constraint to the bodies.
*/
OIMO.ContactConstraint.prototype.attach = function(){
    this.p1=this.body1.position;
    this.p2=this.body2.position;
    this.lv1=this.body1.linearVelocity;
    this.av1=this.body1.angularVelocity;
    this.lv2=this.body2.linearVelocity;
    this.av2=this.body2.angularVelocity;
    this.i1=this.body1.inverseInertia;
    this.i2=this.body2.inverseInertia;
}
/**
* Detach the constraint from the bodies.
*/
OIMO.ContactConstraint.prototype.detach = function(){
    this.p1=null;
    this.p2=null;
    this.lv1=null;
    this.lv2=null;
    this.av1=null;
    this.av2=null;
    this.i1=null;
    this.i2=null;
}
OIMO.ContactConstraint.prototype.preSolve = function(timeStep,invTimeStep){
    this.m1=this.body1.inverseMass;
    this.m2=this.body2.inverseMass;

    var ti1 = this.i1.elements;
    var ti2 = this.i2.elements;
    this.i1e00=ti1[0];
    this.i1e01=ti1[1];
    this.i1e02=ti1[2];
    this.i1e10=ti1[3];
    this.i1e11=ti1[4];
    this.i1e12=ti1[5];
    this.i1e20=ti1[6];
    this.i1e21=ti1[7];
    this.i1e22=ti1[8];

    this.i2e00=ti2[0];
    this.i2e01=ti2[1];
    this.i2e02=ti2[2];
    this.i2e10=ti2[3];
    this.i2e11=ti2[4];
    this.i2e12=ti2[5];
    this.i2e20=ti2[6];
    this.i2e21=ti2[7];
    this.i2e22=ti2[8];

    var p1x=this.p1.x;
    var p1y=this.p1.y;
    var p1z=this.p1.z;
    var p2x=this.p2.x;
    var p2y=this.p2.y;
    var p2z=this.p2.z;
    var m1m2=this.m1+this.m2;
    this.num=this.manifold.numPoints;
    var c=this.cs;
    for(var i=0;i<this.num;i++){
    var p=this.ps[i];
    var tmp1X;
    var tmp1Y;
    var tmp1Z;
    var tmp2X;
    var tmp2Y;
    var tmp2Z;
    tmp1X=p.position.x;
    tmp1Y=p.position.y;
    tmp1Z=p.position.z;
    var rp1X=tmp1X-p1x;
    var rp1Y=tmp1Y-p1y;
    var rp1Z=tmp1Z-p1z;
    var rp2X=tmp1X-p2x;
    var rp2Y=tmp1Y-p2y;
    var rp2Z=tmp1Z-p2z;
    c.rp1X=rp1X;
    c.rp1Y=rp1Y;
    c.rp1Z=rp1Z;
    c.rp2X=rp2X;
    c.rp2Y=rp2Y;
    c.rp2Z=rp2Z;
    c.norImp=p.normalImpulse;
    c.tanImp=p.tangentImpulse;
    c.binImp=p.binormalImpulse;
    var norX=p.normal.x;
    var norY=p.normal.y;
    var norZ=p.normal.z;
    var rvX=(this.lv2.x+this.av2.y*rp2Z-this.av2.z*rp2Y)-(this.lv1.x+this.av1.y*rp1Z-this.av1.z*rp1Y);
    var rvY=(this.lv2.y+this.av2.z*rp2X-this.av2.x*rp2Z)-(this.lv1.y+this.av1.z*rp1X-this.av1.x*rp1Z);
    var rvZ=(this.lv2.z+this.av2.x*rp2Y-this.av2.y*rp2X)-(this.lv1.z+this.av1.x*rp1Y-this.av1.y*rp1X);
    var rvn=norX*rvX+norY*rvY+norZ*rvZ;
    var tanX=rvX-rvn*norX;
    var tanY=rvY-rvn*norY;
    var tanZ=rvZ-rvn*norZ;
    var len=tanX*tanX+tanY*tanY+tanZ*tanZ;
    if(len>0.04){
    len=1/Math.sqrt(len);
    }else{
    tanX=norY*norX-norZ*norZ;
    tanY=-norZ*norY-norX*norX;
    tanZ=norX*norZ+norY*norY;
    len=1/Math.sqrt(tanX*tanX+tanY*tanY+tanZ*tanZ);
    }
    tanX*=len;
    tanY*=len;
    tanZ*=len;
    var binX=norY*tanZ-norZ*tanY;
    var binY=norZ*tanX-norX*tanZ;
    var binZ=norX*tanY-norY*tanX;
    c.norX=norX;
    c.norY=norY;
    c.norZ=norZ;
    c.tanX=tanX;
    c.tanY=tanY;
    c.tanZ=tanZ;
    c.binX=binX;
    c.binY=binY;
    c.binZ=binZ;
    c.norU1X=norX*this.m1;
    c.norU1Y=norY*this.m1;
    c.norU1Z=norZ*this.m1;
    c.norU2X=norX*this.m2;
    c.norU2Y=norY*this.m2;
    c.norU2Z=norZ*this.m2;
    c.tanU1X=tanX*this.m1;
    c.tanU1Y=tanY*this.m1;
    c.tanU1Z=tanZ*this.m1;
    c.tanU2X=tanX*this.m2;
    c.tanU2Y=tanY*this.m2;
    c.tanU2Z=tanZ*this.m2;
    c.binU1X=binX*this.m1;
    c.binU1Y=binY*this.m1;
    c.binU1Z=binZ*this.m1;
    c.binU2X=binX*this.m2;
    c.binU2Y=binY*this.m2;
    c.binU2Z=binZ*this.m2;
    var norT1X=rp1Y*norZ-rp1Z*norY;
    var norT1Y=rp1Z*norX-rp1X*norZ;
    var norT1Z=rp1X*norY-rp1Y*norX;
    var norT2X=rp2Y*norZ-rp2Z*norY;
    var norT2Y=rp2Z*norX-rp2X*norZ;
    var norT2Z=rp2X*norY-rp2Y*norX;
    var tanT1X=rp1Y*tanZ-rp1Z*tanY;
    var tanT1Y=rp1Z*tanX-rp1X*tanZ;
    var tanT1Z=rp1X*tanY-rp1Y*tanX;
    var tanT2X=rp2Y*tanZ-rp2Z*tanY;
    var tanT2Y=rp2Z*tanX-rp2X*tanZ;
    var tanT2Z=rp2X*tanY-rp2Y*tanX;
    var binT1X=rp1Y*binZ-rp1Z*binY;
    var binT1Y=rp1Z*binX-rp1X*binZ;
    var binT1Z=rp1X*binY-rp1Y*binX;
    var binT2X=rp2Y*binZ-rp2Z*binY;
    var binT2Y=rp2Z*binX-rp2X*binZ;
    var binT2Z=rp2X*binY-rp2Y*binX;
    var norTU1X=norT1X*this.i1e00+norT1Y*this.i1e01+norT1Z*this.i1e02;
    var norTU1Y=norT1X*this.i1e10+norT1Y*this.i1e11+norT1Z*this.i1e12;
    var norTU1Z=norT1X*this.i1e20+norT1Y*this.i1e21+norT1Z*this.i1e22;
    var norTU2X=norT2X*this.i2e00+norT2Y*this.i2e01+norT2Z*this.i2e02;
    var norTU2Y=norT2X*this.i2e10+norT2Y*this.i2e11+norT2Z*this.i2e12;
    var norTU2Z=norT2X*this.i2e20+norT2Y*this.i2e21+norT2Z*this.i2e22;
    var tanTU1X=tanT1X*this.i1e00+tanT1Y*this.i1e01+tanT1Z*this.i1e02;
    var tanTU1Y=tanT1X*this.i1e10+tanT1Y*this.i1e11+tanT1Z*this.i1e12;
    var tanTU1Z=tanT1X*this.i1e20+tanT1Y*this.i1e21+tanT1Z*this.i1e22;
    var tanTU2X=tanT2X*this.i2e00+tanT2Y*this.i2e01+tanT2Z*this.i2e02;
    var tanTU2Y=tanT2X*this.i2e10+tanT2Y*this.i2e11+tanT2Z*this.i2e12;
    var tanTU2Z=tanT2X*this.i2e20+tanT2Y*this.i2e21+tanT2Z*this.i2e22;
    var binTU1X=binT1X*this.i1e00+binT1Y*this.i1e01+binT1Z*this.i1e02;
    var binTU1Y=binT1X*this.i1e10+binT1Y*this.i1e11+binT1Z*this.i1e12;
    var binTU1Z=binT1X*this.i1e20+binT1Y*this.i1e21+binT1Z*this.i1e22;
    var binTU2X=binT2X*this.i2e00+binT2Y*this.i2e01+binT2Z*this.i2e02;
    var binTU2Y=binT2X*this.i2e10+binT2Y*this.i2e11+binT2Z*this.i2e12;
    var binTU2Z=binT2X*this.i2e20+binT2Y*this.i2e21+binT2Z*this.i2e22;
    c.norT1X=norT1X;
    c.norT1Y=norT1Y;
    c.norT1Z=norT1Z;
    c.tanT1X=tanT1X;
    c.tanT1Y=tanT1Y;
    c.tanT1Z=tanT1Z;
    c.binT1X=binT1X;
    c.binT1Y=binT1Y;
    c.binT1Z=binT1Z;
    c.norT2X=norT2X;
    c.norT2Y=norT2Y;
    c.norT2Z=norT2Z;
    c.tanT2X=tanT2X;
    c.tanT2Y=tanT2Y;
    c.tanT2Z=tanT2Z;
    c.binT2X=binT2X;
    c.binT2Y=binT2Y;
    c.binT2Z=binT2Z;
    c.norTU1X=norTU1X;
    c.norTU1Y=norTU1Y;
    c.norTU1Z=norTU1Z;
    c.tanTU1X=tanTU1X;
    c.tanTU1Y=tanTU1Y;
    c.tanTU1Z=tanTU1Z;
    c.binTU1X=binTU1X;
    c.binTU1Y=binTU1Y;
    c.binTU1Z=binTU1Z;
    c.norTU2X=norTU2X;
    c.norTU2Y=norTU2Y;
    c.norTU2Z=norTU2Z;
    c.tanTU2X=tanTU2X;
    c.tanTU2Y=tanTU2Y;
    c.tanTU2Z=tanTU2Z;
    c.binTU2X=binTU2X;
    c.binTU2Y=binTU2Y;
    c.binTU2Z=binTU2Z;
    tmp1X=norT1X*this.i1e00+norT1Y*this.i1e01+norT1Z*this.i1e02;
    tmp1Y=norT1X*this.i1e10+norT1Y*this.i1e11+norT1Z*this.i1e12;
    tmp1Z=norT1X*this.i1e20+norT1Y*this.i1e21+norT1Z*this.i1e22;
    tmp2X=tmp1Y*rp1Z-tmp1Z*rp1Y;
    tmp2Y=tmp1Z*rp1X-tmp1X*rp1Z;
    tmp2Z=tmp1X*rp1Y-tmp1Y*rp1X;
    tmp1X=norT2X*this.i2e00+norT2Y*this.i2e01+norT2Z*this.i2e02;
    tmp1Y=norT2X*this.i2e10+norT2Y*this.i2e11+norT2Z*this.i2e12;
    tmp1Z=norT2X*this.i2e20+norT2Y*this.i2e21+norT2Z*this.i2e22;
    tmp2X+=tmp1Y*rp2Z-tmp1Z*rp2Y;
    tmp2Y+=tmp1Z*rp2X-tmp1X*rp2Z;
    tmp2Z+=tmp1X*rp2Y-tmp1Y*rp2X;
    var norDen=1/(m1m2+norX*tmp2X+norY*tmp2Y+norZ*tmp2Z);
    tmp1X=tanT1X*this.i1e00+tanT1Y*this.i1e01+tanT1Z*this.i1e02;
    tmp1Y=tanT1X*this.i1e10+tanT1Y*this.i1e11+tanT1Z*this.i1e12;
    tmp1Z=tanT1X*this.i1e20+tanT1Y*this.i1e21+tanT1Z*this.i1e22;
    tmp2X=tmp1Y*rp1Z-tmp1Z*rp1Y;
    tmp2Y=tmp1Z*rp1X-tmp1X*rp1Z;
    tmp2Z=tmp1X*rp1Y-tmp1Y*rp1X;
    tmp1X=tanT2X*this.i2e00+tanT2Y*this.i2e01+tanT2Z*this.i2e02;
    tmp1Y=tanT2X*this.i2e10+tanT2Y*this.i2e11+tanT2Z*this.i2e12;
    tmp1Z=tanT2X*this.i2e20+tanT2Y*this.i2e21+tanT2Z*this.i2e22;
    tmp2X+=tmp1Y*rp2Z-tmp1Z*rp2Y;
    tmp2Y+=tmp1Z*rp2X-tmp1X*rp2Z;
    tmp2Z+=tmp1X*rp2Y-tmp1Y*rp2X;
    var tanDen=1/(m1m2+tanX*tmp2X+tanY*tmp2Y+tanZ*tmp2Z);
    tmp1X=binT1X*this.i1e00+binT1Y*this.i1e01+binT1Z*this.i1e02;
    tmp1Y=binT1X*this.i1e10+binT1Y*this.i1e11+binT1Z*this.i1e12;
    tmp1Z=binT1X*this.i1e20+binT1Y*this.i1e21+binT1Z*this.i1e22;
    tmp2X=tmp1Y*rp1Z-tmp1Z*rp1Y;
    tmp2Y=tmp1Z*rp1X-tmp1X*rp1Z;
    tmp2Z=tmp1X*rp1Y-tmp1Y*rp1X;
    tmp1X=binT2X*this.i2e00+binT2Y*this.i2e01+binT2Z*this.i2e02;
    tmp1Y=binT2X*this.i2e10+binT2Y*this.i2e11+binT2Z*this.i2e12;
    tmp1Z=binT2X*this.i2e20+binT2Y*this.i2e21+binT2Z*this.i2e22;
    tmp2X+=tmp1Y*rp2Z-tmp1Z*rp2Y;
    tmp2Y+=tmp1Z*rp2X-tmp1X*rp2Z;
    tmp2Z+=tmp1X*rp2Y-tmp1Y*rp2X;
    var binDen=1/(m1m2+binX*tmp2X+binY*tmp2Y+binZ*tmp2Z);
    c.norDen=norDen;
    c.tanDen=tanDen;
    c.binDen=binDen;
    if(p.warmStarted){
    var norImp=p.normalImpulse;
    this.lv1.x+=c.norU1X*norImp;
    this.lv1.y+=c.norU1Y*norImp;
    this.lv1.z+=c.norU1Z*norImp;
    this.av1.x+=norTU1X*norImp;
    this.av1.y+=norTU1Y*norImp;
    this.av1.z+=norTU1Z*norImp;
    this.lv2.x-=c.norU2X*norImp;
    this.lv2.y-=c.norU2Y*norImp;
    this.lv2.z-=c.norU2Z*norImp;
    this.av2.x-=norTU2X*norImp;
    this.av2.y-=norTU2Y*norImp;
    this.av2.z-=norTU2Z*norImp;
    c.norImp=norImp;
    c.tanImp=0;
    c.binImp=0;
    rvn=0; // disable bouncing
    }else{
    c.norImp=0;
    c.tanImp=0;
    c.binImp=0;
    }
    if(rvn>-1){
    rvn=0; // disable bouncing
    }
    var norTar=this.restitution*-rvn;
    var sepV=-(p.penetration+0.005)*invTimeStep*0.05; // allow 0.5cm error
    if(norTar<sepV)norTar=sepV;
    c.norTar=norTar;
    c.last=i==this.num-1;
    c=c.next;
    }
}
OIMO.ContactConstraint.prototype.solve = function(){
    var lv1x=this.lv1.x;
    var lv1y=this.lv1.y;
    var lv1z=this.lv1.z;
    var lv2x=this.lv2.x;
    var lv2y=this.lv2.y;
    var lv2z=this.lv2.z;
    var av1x=this.av1.x;
    var av1y=this.av1.y;
    var av1z=this.av1.z;
    var av2x=this.av2.x;
    var av2y=this.av2.y;
    var av2z=this.av2.z;
    var c=this.cs;
    while(true){
        var oldImp1;
        var newImp1;
        var oldImp2;
        var newImp2;
        var rvn;
        var norImp=c.norImp;
        var tanImp=c.tanImp;
        var binImp=c.binImp;
        var max=-norImp*this.friction;
        var rvX=lv2x-lv1x;
        var rvY=lv2y-lv1y;
        var rvZ=lv2z-lv1z;
        rvn=
        rvX*c.tanX+rvY*c.tanY+rvZ*c.tanZ+
        av2x*c.tanT2X+av2y*c.tanT2Y+av2z*c.tanT2Z-
        av1x*c.tanT1X-av1y*c.tanT1Y-av1z*c.tanT1Z
        ;
        oldImp1=tanImp;
        newImp1=rvn*c.tanDen;
        tanImp+=newImp1;
        rvn=
        rvX*c.binX+rvY*c.binY+rvZ*c.binZ+
        av2x*c.binT2X+av2y*c.binT2Y+av2z*c.binT2Z-
        av1x*c.binT1X-av1y*c.binT1Y-av1z*c.binT1Z
        ;
        oldImp2=binImp;
        newImp2=rvn*c.binDen;
        binImp+=newImp2;

        // cone friction clamp
        var len=tanImp*tanImp+binImp*binImp;
        if(len>max*max){
            len=max/Math.sqrt(len);
            tanImp*=len;
            binImp*=len;
        }
        newImp1=tanImp-oldImp1;
        newImp2=binImp-oldImp2;

        lv1x+=c.tanU1X*newImp1+c.binU1X*newImp2;
        lv1y+=c.tanU1Y*newImp1+c.binU1Y*newImp2;
        lv1z+=c.tanU1Z*newImp1+c.binU1Z*newImp2;
        av1x+=c.tanTU1X*newImp1+c.binTU1X*newImp2;
        av1y+=c.tanTU1Y*newImp1+c.binTU1Y*newImp2;
        av1z+=c.tanTU1Z*newImp1+c.binTU1Z*newImp2;
        lv2x-=c.tanU2X*newImp1+c.binU2X*newImp2;
        lv2y-=c.tanU2Y*newImp1+c.binU2Y*newImp2;
        lv2z-=c.tanU2Z*newImp1+c.binU2Z*newImp2;
        av2x-=c.tanTU2X*newImp1+c.binTU2X*newImp2;
        av2y-=c.tanTU2Y*newImp1+c.binTU2Y*newImp2;
        av2z-=c.tanTU2Z*newImp1+c.binTU2Z*newImp2;

        // restitution part
        rvn=
        (lv2x-lv1x)*c.norX+(lv2y-lv1y)*c.norY+(lv2z-lv1z)*c.norZ+
        av2x*c.norT2X+av2y*c.norT2Y+av2z*c.norT2Z-
        av1x*c.norT1X-av1y*c.norT1Y-av1z*c.norT1Z;

        oldImp1=norImp;
        newImp1=(rvn-c.norTar)*c.norDen;
        norImp+=newImp1;
        if(norImp>0)norImp=0;
        newImp1=norImp-oldImp1;
        lv1x+=c.norU1X*newImp1;
        lv1y+=c.norU1Y*newImp1;
        lv1z+=c.norU1Z*newImp1;
        av1x+=c.norTU1X*newImp1;
        av1y+=c.norTU1Y*newImp1;
        av1z+=c.norTU1Z*newImp1;
        lv2x-=c.norU2X*newImp1;
        lv2y-=c.norU2Y*newImp1;
        lv2z-=c.norU2Z*newImp1;
        av2x-=c.norTU2X*newImp1;
        av2y-=c.norTU2Y*newImp1;
        av2z-=c.norTU2Z*newImp1;

        c.norImp=norImp;
        c.tanImp=tanImp;
        c.binImp=binImp;

        if(c.last)break;
        c=c.next;
    }
    this.lv1.x=lv1x;
    this.lv1.y=lv1y;
    this.lv1.z=lv1z;
    this.lv2.x=lv2x;
    this.lv2.y=lv2y;
    this.lv2.z=lv2z;
    this.av1.x=av1x;
    this.av1.y=av1y;
    this.av1.z=av1z;
    this.av2.x=av2x;
    this.av2.y=av2y;
    this.av2.z=av2z;
}
OIMO.ContactConstraint.prototype.postSolve = function(){
    var c=this.cs;
    var i = this.num;
    while(i--){
    //for(var i=0;i<this.num;i++){
        var p=this.ps[i];
        p.normal.x=c.norX;
        p.normal.y=c.norY;
        p.normal.z=c.norZ;
        p.tangent.x=c.tanX;
        p.tangent.y=c.tanY;
        p.tangent.z=c.tanZ;
        p.binormal.x=c.binX;
        p.binormal.y=c.binY;
        p.binormal.z=c.binZ;
        p.normalImpulse=c.norImp;
        p.tangentImpulse=c.tanImp;
        p.binormalImpulse=c.binImp;
        p.normalDenominator=c.norDen;
        p.tangentDenominator=c.tanDen;
        p.binormalDenominator=c.binDen;
        c=c.next;
    }
}
/**
* A link list of contacts.
* @author saharan
*/
OIMO.ContactLink = function(contact){
	// The previous contact link.
    this.prev=null;
    // The next contact link.
    this.next=null;
    // The shape of the contact.
    this.shape=null;
    // The other rigid body.
    this.body=null;
    // The contact of the link.
    this.contact = contact;
}
/**
* A contact manifold between two shapes.
* @author saharan
*/
OIMO.ContactManifold = function(){
    // The first rigid body.
    this.body1 = null;
    // The second rigid body.
    this.body2 = null;
    // The number of manifold points.
    this.numPoints = 0;
    // The manifold points.
    this.points = [];
    this.points.length = 4;
    this.points[0] = new OIMO.ManifoldPoint();
    this.points[1] = new OIMO.ManifoldPoint();
    this.points[2] = new OIMO.ManifoldPoint();
    this.points[3] = new OIMO.ManifoldPoint();
}
OIMO.ContactManifold.prototype = {
    constructor: OIMO.ContactManifold,
    /**
    * Reset the manifold.
    * @param   shape1
    * @param   shape2
    */
    reset:function(shape1,shape2){
        this.body1=shape1.parent;
        this.body2=shape2.parent;
        this.numPoints=0;
    },
    /**
    * Add a point into this manifold.
    * @param   x
    * @param   y
    * @param   z
    * @param   normalX
    * @param   normalY
    * @param   normalZ
    * @param   penetration
    * @param   flip
    */
    addPoint:function(x,y,z,normalX,normalY,normalZ,penetration,flip){
        var p=this.points[this.numPoints++];
        p.position.x=x;
        p.position.y=y;
        p.position.z=z;
        var r=this.body1.rotation;
        var rx=x-this.body1.position.x;
        var ry=y-this.body1.position.y;
        var rz=z-this.body1.position.z;

        var tr = r.elements;
        p.localPoint1.x=rx*tr[0]+ry*tr[3]+rz*tr[6];
        p.localPoint1.y=rx*tr[1]+ry*tr[4]+rz*tr[7];
        p.localPoint1.z=rx*tr[2]+ry*tr[5]+rz*tr[8];
        r=this.body2.rotation;
        rx=x-this.body2.position.x;
        ry=y-this.body2.position.y;
        rz=z-this.body2.position.z;
        p.localPoint2.x=rx*tr[0]+ry*tr[3]+rz*tr[6];
        p.localPoint2.y=rx*tr[1]+ry*tr[4]+rz*tr[7];
        p.localPoint2.z=rx*tr[2]+ry*tr[5]+rz*tr[8];

        p.normalImpulse=0;
        if(flip){
            p.normal.x=-normalX;
            p.normal.y=-normalY;
            p.normal.z=-normalZ;
        }else{
            p.normal.x=normalX;
            p.normal.y=normalY;
            p.normal.z=normalZ;
        }
        p.penetration=penetration;
        p.warmStarted=false;
    }
}
OIMO.ContactPointDataBuffer = function(){
    this.norX=NaN;
    this.norY=NaN;
    this.norZ=NaN;
    this.tanX=NaN;
    this.tanY=NaN;
    this.tanZ=NaN;
    this.binX=NaN;
    this.binY=NaN;
    this.binZ=NaN;
    
    this.rp1X=NaN;
    this.rp1Y=NaN;
    this.rp1Z=NaN;
    this.rp2X=NaN;
    this.rp2Y=NaN;
    this.rp2Z=NaN;
    
    this.norU1X=NaN;
    this.norU1Y=NaN;
    this.norU1Z=NaN;
    this.norU2X=NaN;
    this.norU2Y=NaN;
    this.norU2Z=NaN;
    this.tanU1X=NaN;
    this.tanU1Y=NaN;
    this.tanU1Z=NaN;
    this.tanU2X=NaN;
    this.tanU2Y=NaN;
    this.tanU2Z=NaN;
    this.binU1X=NaN;
    this.binU1Y=NaN;
    this.binU1Z=NaN;
    this.binU2X=NaN;
    this.binU2Y=NaN;
    this.binU2Z=NaN;
    
    this.norT1X=NaN;
    this.norT1Y=NaN;
    this.norT1Z=NaN;
    this.norT2X=NaN;
    this.norT2Y=NaN;
    this.norT2Z=NaN;
    this.tanT1X=NaN;
    this.tanT1Y=NaN;
    this.tanT1Z=NaN;
    this.tanT2X=NaN;
    this.tanT2Y=NaN;
    this.tanT2Z=NaN;
    this.binT1X=NaN;
    this.binT1Y=NaN;
    this.binT1Z=NaN;
    this.binT2X=NaN;
    this.binT2Y=NaN;
    this.binT2Z=NaN;
    
    this.norTU1X=NaN;
    this.norTU1Y=NaN;
    this.norTU1Z=NaN;
    this.norTU2X=NaN;
    this.norTU2Y=NaN;
    this.norTU2Z=NaN;
    this.tanTU1X=NaN;
    this.tanTU1Y=NaN;
    this.tanTU1Z=NaN;
    this.tanTU2X=NaN;
    this.tanTU2Y=NaN;
    this.tanTU2Z=NaN;
    this.binTU1X=NaN;
    this.binTU1Y=NaN;
    this.binTU1Z=NaN;
    this.binTU2X=NaN;
    this.binTU2Y=NaN;
    this.binTU2Z=NaN;
    
    this.norImp=NaN;
    this.tanImp=NaN;
    this.binImp=NaN;
    this.norDen=NaN;
    this.tanDen=NaN;
    this.binDen=NaN;
    this.norTar=NaN;
    this.next=null;
    this.last=false;
}
OIMO.ImpulseDataBuffer = function(){
    this.lp1X=NaN;
    this.lp1Y=NaN;
    this.lp1Z=NaN;
    this.lp2X=NaN;
    this.lp2Y=NaN;
    this.lp2Z=NaN;
    this.impulse=NaN;
}
/**
* The class holds details of the contact point.
* @author saharan
*/
OIMO.ManifoldPoint = function(){
    // Whether this manifold point is persisting or not.
    this.warmStarted=false;
    //  The position of this manifold point.
    this.position=new OIMO.Vec3();
    // The position in the first shape's coordinate.
    this.localPoint1=new OIMO.Vec3();
    //  The position in the second shape's coordinate.
    this.localPoint2=new OIMO.Vec3();
    // The normal vector of this manifold point.
    this.normal=new OIMO.Vec3();
    // The tangent vector of this manifold point.
    this.tangent=new OIMO.Vec3();
    // The binormal vector of this manifold point.
    this.binormal=new OIMO.Vec3();
    // The impulse in normal direction.
    this.normalImpulse=0;
    // The impulse in tangent direction.
    this.tangentImpulse=0;
    // The impulse in binormal direction.
    this.binormalImpulse=0;
    // The denominator in normal direction.
    this.normalDenominator=0;
    // The denominator in tangent direction.
    this.tangentDenominator=0;
    // The denominator in binormal direction.
    this.binormalDenominator=0;
    // The depth of penetration.
    this.penetration=0;
}
/**
* This class holds mass information of a shape.
* @author saharan
*/
OIMO.MassInfo = function(){
	// Mass of the shape.
    this.mass = 0;
    // The moment inertia of the shape.
    this.inertia = new OIMO.Mat33();
}
/**
 * A shape is used to detect collisions of rigid bodies.
 * @author saharan
 */
OIMO.Shape = function(config){
    // The global identification of the shape.
    // This value should be unique to the shape.
    this.id = OIMO.nextID++;
    // The previous shape in parent rigid body.
    this.prev = null;
    // The next shape in parent rigid body.
    this.next = null;
    // The type of the shape.
    this.type = 0;

    // The proxy of the shape.
    // This is used for broad-phase collision detection.
    this.proxy = null;
    // The parent rigid body of the shape.
    this.parent = null;
    // The linked list of the contacts with the shape.
    this.contactLink = null;
    // The number of the contacts with the shape.
    this.numContacts=0;

    // The center of gravity of the shape in world coordinate system.
    this.position = new OIMO.Vec3();
    // The rotation matrix of the shape in world coordinate system
    this.rotation = new OIMO.Mat33();

    // The position of the shape in parent's coordinate system.
    this.relativePosition = new OIMO.Vec3().copy(config.relativePosition);
    // The rotation matrix of the shape in parent's coordinate system.
    this.relativeRotation = new OIMO.Mat33().copy(config.relativeRotation);

    // The axis-aligned bounding box of the shape.
    this.aabb = new OIMO.AABB();

    // The density of the shape.
    this.density = config.density;
    // The coefficient of friction of the shape.
    this.friction = config.friction;
    // The coefficient of restitution of the shape.
    this.restitution = config.restitution;
    // The bits of the collision groups to which the shape belongs.
    this.belongsTo = config.belongsTo;
    // The bits of the collision groups with which the shape collides.
    this.collidesWith = config.collidesWith;
}

OIMO.Shape.prototype = {
    constructor: OIMO.Shape,

    /**
    * Calculate the mass information of the shape.
    * @param   out
    */
    calculateMassInfo:function(out){
        throw new Error("Inheritance error.");
    },

    /**
    * Update the proxy of the shape.
    */
    updateProxy:function(){
        throw new Error("Inheritance error.");
    }
}
/**
 * A shape configuration holds common configuration data for constructing a shape.
 * Shape configurations can be reused safely.
 * @author saharan
 */

OIMO.ShapeConfig = function(){
	// The position of the shape in parent's coordinate system.
    this.relativePosition = new OIMO.Vec3();
    // The rotation matrix of the shape in parent's coordinate system.
    this.relativeRotation = new OIMO.Mat33();
    // The coefficient of friction of the shape.
    this.friction = 0.4;
    // The coefficient of restitution of the shape.
    this.restitution = 0.2;
    // The density of the shape.
    this.density = 1;
    // The bits of the collision groups to which the shape belongs.
    this.belongsTo = 1;
    // The bits of the collision groups with which the shape collides.
    this.collidesWith = 0xffffffff;
}
/**
* A box shape.
* @author saharan
* @author loth
*/
OIMO.BoxShape = function(config,Width,Height,Depth){
    OIMO.Shape.call( this, config );
    // The width of the box.
    this.width = Width;
    // The height of the box.
    this.height = Height;
    // The depth of the box.
    this.depth = Depth;
    // The half-width of the box.
    this.halfWidth = Width*0.5;
    // The half-height of the box.
    this.halfHeight = Height*0.5;
    // The half-depth of the box.
    this.halfDepth = Depth*0.5;

    this.dimentions = new OIMO_ARRAY_TYPE(18);
    this.elements = new OIMO_ARRAY_TYPE(24);
    this.type = OIMO.SHAPE_BOX;
}
OIMO.BoxShape.prototype = Object.create( OIMO.Shape.prototype );
OIMO.BoxShape.prototype.calculateMassInfo = function(out){
    var mass=this.width*this.height*this.depth*this.density;
    out.mass=mass;
    out.inertia.init(
        mass*(this.height*this.height+this.depth*this.depth)/12,0,0,
        0,mass*(this.width*this.width+this.depth*this.depth)/12,0,
        0,0,mass*(this.width*this.width+this.height*this.height)/12
    );
}
OIMO.BoxShape.prototype.updateProxy = function(){
    var te = this.rotation.elements;
    var di = this.dimentions;
    // Width
    di[0]=te[0];
    di[1]=te[3];
    di[2]=te[6];
    // Height
    di[3]=te[1];
    di[4]=te[4];
    di[5]=te[7];
    // Depth
    di[6]=te[2];
    di[7]=te[5];
    di[8]=te[8];
    // halp Width
    di[9]=te[0]*this.halfWidth;
    di[10]=te[3]*this.halfWidth;
    di[11]=te[6]*this.halfWidth;
    // halp Height
    di[12]=te[1]*this.halfHeight;
    di[13]=te[4]*this.halfHeight;
    di[14]=te[7]*this.halfHeight;
    // halp Depth
    di[15]=te[2]*this.halfDepth;
    di[16]=te[5]*this.halfDepth;
    di[17]=te[8]*this.halfDepth;

    var wx=di[9];
    var wy=di[10];
    var wz=di[11];
    var hx=di[12];
    var hy=di[13];
    var hz=di[14];
    var dx=di[15];
    var dy=di[16];
    var dz=di[17];

    var x=this.position.x;
    var y=this.position.y;
    var z=this.position.z;

    var v=this.elements;
    //v1
    v[0]=x+wx+hx+dx;
    v[1]=y+wy+hy+dy;
    v[2]=z+wz+hz+dz;
    //v2
    v[3]=x+wx+hx-dx;
    v[4]=y+wy+hy-dy;
    v[5]=z+wz+hz-dz;
    //v3
    v[6]=x+wx-hx+dx;
    v[7]=y+wy-hy+dy;
    v[8]=z+wz-hz+dz;
    //v4
    v[9]=x+wx-hx-dx;
    v[10]=y+wy-hy-dy;
    v[11]=z+wz-hz-dz;
    //v5
    v[12]=x-wx+hx+dx;
    v[13]=y-wy+hy+dy;
    v[14]=z-wz+hz+dz;
    //v6
    v[15]=x-wx+hx-dx;
    v[16]=y-wy+hy-dy;
    v[17]=z-wz+hz-dz;
    //v7
    v[18]=x-wx-hx+dx;
    v[19]=y-wy-hy+dy;
    v[20]=z-wz-hz+dz;
    //v8
    v[21]=x-wx-hx-dx;
    v[22]=y-wy-hy-dy;
    v[23]=z-wz-hz-dz;

    var w = (di[9]<0) ? -di[9] : di[9]; 
    var h = (di[10]<0) ? -di[10] : di[10];
    var d = (di[11]<0) ? -di[11] : di[11];

    w = (di[12]<0) ? w-di[12] : w+di[12]; 
    h = (di[13]<0) ? h-di[13] : h+di[13];
    d = (di[14]<0) ? d-di[14] : d+di[14];

    w = (di[15]<0) ? w-di[15] : w+di[15]; 
    h = (di[16]<0) ? h-di[16] : h+di[16];
    d = (di[17]<0) ? d-di[17] : d+di[17];
    
    this.aabb.init(
        this.position.x-w-0.005,this.position.x+w+0.005,
        this.position.y-h-0.005,this.position.y+h+0.005,
        this.position.z-d-0.005,this.position.z+d+0.005
    );
    if(this.proxy!==null){
        this.proxy.update();
    }
}
/**
 * A sphere shape.
 * @author saharan
 */
OIMO.SphereShape = function(config,radius){
    OIMO.Shape.call( this, config);
    // The radius of the shape.
    this.radius = radius;
    this.type = OIMO.SHAPE_SPHERE;
}
OIMO.SphereShape.prototype = Object.create( OIMO.Shape.prototype );
OIMO.SphereShape.prototype.calculateMassInfo = function(out){
    var mass = 4/3*Math.PI*this.radius*this.radius*this.radius*this.density;
    out.mass = mass;
    var inertia = mass*this.radius*this.radius*2/5;
    out.inertia.init( inertia,0,0,0,inertia,0,0,0,inertia );
}
OIMO.SphereShape.prototype.updateProxy = function(){
    this.aabb.init(
        this.position.x-this.radius-0.005,this.position.x+this.radius+0.005,
        this.position.y-this.radius-0.005,this.position.y+this.radius+0.005,
        this.position.z-this.radius-0.005,this.position.z+this.radius+0.005
    );
    if(this.proxy!==null){ this.proxy.update(); }
}
OIMO.CollisionDetector = function(){
    this.flip = false;
}

OIMO.CollisionDetector.prototype = {
    constructor: OIMO.CollisionDetector,

    detectCollision:function(shape1,shape2,manifold){
        throw new Error("Inheritance error.");
    }
}
/**
 * A collision detector which detects collisions between two boxes.
 * @author saharan
 */
OIMO.BoxBoxCollisionDetector = function(){
    OIMO.CollisionDetector.call( this );
    this.clipVertices1=new OIMO_ARRAY_TYPE(24); // 8 vertices x,y,z
    this.clipVertices2=new OIMO_ARRAY_TYPE(24);
    this.used=new OIMO_ARRAY_TYPE(8);
    
    this.INF = 1/0;
}
OIMO.BoxBoxCollisionDetector.prototype = Object.create( OIMO.CollisionDetector.prototype );
OIMO.BoxBoxCollisionDetector.prototype.detectCollision = function(shape1,shape2,manifold){
    // What you are doing 
    // · I to prepare a separate axis of the fifteen 
    //-Six in each of three normal vectors of the xyz direction of the box both 
    // · Remaining nine 3x3 a vector perpendicular to the side of the box 2 and the side of the box 1 
    // · Calculate the depth to the separation axis 

    // Calculates the distance using the inner product and put the amount of embedment 
    // · However a vertical separation axis and side to weight a little to avoid vibration 
    // And end when there is a separate axis that is remote even one 
    // · I look for separation axis with little to dent most 
    // Men and if separation axis of the first six - end collision 
    // Heng If it separate axis of nine other - side collision 
    // Heng - case of a side collision 
    // · Find points of two sides on which you made ​​the separation axis 

    // Calculates the point of closest approach of a straight line consisting of separate axis points obtained, and the collision point 
    //-Surface - the case of the plane crash 
    //-Box A, box B and the other a box of better made ​​a separate axis 
    // • The surface A and the plane that made the separation axis of the box A, and B to the surface the face of the box B close in the opposite direction to the most isolated axis 

    // When viewed from the front surface A, and the cut part exceeding the area of the surface A is a surface B 
    //-Plane B becomes the 3-8 triangle, I a candidate for the collision point the vertex of surface B 
    // • If more than one candidate 5 exists, scraping up to four 

    // For potential collision points of all, to examine the distance between the surface A 
    // • If you were on the inside surface of A, and the collision point

    var b1;
    var b2;
    if(shape1.id<shape2.id){
        b1=(shape1);
        b2=(shape2);
    }else{
        b1=(shape2);
        b2=(shape1);
    }
    var V1 = b1.elements;
    var V2 = b2.elements;

    var D1 = b1.dimentions;
    var D2 = b2.dimentions;

    var p1=b1.position;
    var p2=b2.position;
    var p1x=p1.x;
    var p1y=p1.y;
    var p1z=p1.z;
    var p2x=p2.x;
    var p2y=p2.y;
    var p2z=p2.z;
    // diff
    var dx=p2x-p1x;
    var dy=p2y-p1y;
    var dz=p2z-p1z;
    // distance
    var w1=b1.halfWidth;
    var h1=b1.halfHeight;
    var d1=b1.halfDepth;
    var w2=b2.halfWidth;
    var h2=b2.halfHeight;
    var d2=b2.halfDepth;
    // direction

    // ----------------------------
    // 15 separating axes
    // 1~6: face
    // 7~f: edge
    // http://marupeke296.com/COL_3D_No13_OBBvsOBB.html
    // ----------------------------
    
    var a1x=D1[0];
    var a1y=D1[1];
    var a1z=D1[2];
    var a2x=D1[3];
    var a2y=D1[4];
    var a2z=D1[5];
    var a3x=D1[6];
    var a3y=D1[7];
    var a3z=D1[8];
    var d1x=D1[9];
    var d1y=D1[10];
    var d1z=D1[11];
    var d2x=D1[12];
    var d2y=D1[13];
    var d2z=D1[14];
    var d3x=D1[15];
    var d3y=D1[16];
    var d3z=D1[17];

    var a4x=D2[0];
    var a4y=D2[1];
    var a4z=D2[2];
    var a5x=D2[3];
    var a5y=D2[4];
    var a5z=D2[5];
    var a6x=D2[6];
    var a6y=D2[7];
    var a6z=D2[8];
    var d4x=D2[9];
    var d4y=D2[10];
    var d4z=D2[11];
    var d5x=D2[12];
    var d5y=D2[13];
    var d5z=D2[14];
    var d6x=D2[15];
    var d6y=D2[16];
    var d6z=D2[17];
    
    var a7x=a1y*a4z-a1z*a4y;
    var a7y=a1z*a4x-a1x*a4z;
    var a7z=a1x*a4y-a1y*a4x;
    var a8x=a1y*a5z-a1z*a5y;
    var a8y=a1z*a5x-a1x*a5z;
    var a8z=a1x*a5y-a1y*a5x;
    var a9x=a1y*a6z-a1z*a6y;
    var a9y=a1z*a6x-a1x*a6z;
    var a9z=a1x*a6y-a1y*a6x;
    var aax=a2y*a4z-a2z*a4y;
    var aay=a2z*a4x-a2x*a4z;
    var aaz=a2x*a4y-a2y*a4x;
    var abx=a2y*a5z-a2z*a5y;
    var aby=a2z*a5x-a2x*a5z;
    var abz=a2x*a5y-a2y*a5x;
    var acx=a2y*a6z-a2z*a6y;
    var acy=a2z*a6x-a2x*a6z;
    var acz=a2x*a6y-a2y*a6x;
    var adx=a3y*a4z-a3z*a4y;
    var ady=a3z*a4x-a3x*a4z;
    var adz=a3x*a4y-a3y*a4x;
    var aex=a3y*a5z-a3z*a5y;
    var aey=a3z*a5x-a3x*a5z;
    var aez=a3x*a5y-a3y*a5x;
    var afx=a3y*a6z-a3z*a6y;
    var afy=a3z*a6x-a3x*a6z;
    var afz=a3x*a6y-a3y*a6x;
    // right or left flags
    var right1;
    var right2;
    var right3;
    var right4;
    var right5;
    var right6;
    var right7;
    var right8;
    var right9;
    var righta;
    var rightb;
    var rightc;
    var rightd;
    var righte;
    var rightf;
    // overlapping distances
    var overlap1;
    var overlap2;
    var overlap3;
    var overlap4;
    var overlap5;
    var overlap6;
    var overlap7;
    var overlap8;
    var overlap9;
    var overlapa;
    var overlapb;
    var overlapc;
    var overlapd;
    var overlape;
    var overlapf;
    // invalid flags
    var invalid7=false;
    var invalid8=false;
    var invalid9=false;
    var invalida=false;
    var invalidb=false;
    var invalidc=false;
    var invalidd=false;
    var invalide=false;
    var invalidf=false;
    // temporary variables
    var len;
    var len1;
    var len2;
    var dot1;
    var dot2;
    var dot3;
    // try axis 1
    len=a1x*dx+a1y*dy+a1z*dz;
    right1=len>0;
    if(!right1)len=-len;
    len1=w1;
    dot1=a1x*a4x+a1y*a4y+a1z*a4z;
    dot2=a1x*a5x+a1y*a5y+a1z*a5z;
    dot3=a1x*a6x+a1y*a6y+a1z*a6z;
    if(dot1<0)dot1=-dot1;
    if(dot2<0)dot2=-dot2;
    if(dot3<0)dot3=-dot3;
    len2=dot1*w2+dot2*h2+dot3*d2;
    overlap1=len-len1-len2;
    if(overlap1>0)return;
    // try axis 2
    len=a2x*dx+a2y*dy+a2z*dz;
    right2=len>0;
    if(!right2)len=-len;
    len1=h1;
    dot1=a2x*a4x+a2y*a4y+a2z*a4z;
    dot2=a2x*a5x+a2y*a5y+a2z*a5z;
    dot3=a2x*a6x+a2y*a6y+a2z*a6z;
    if(dot1<0)dot1=-dot1;
    if(dot2<0)dot2=-dot2;
    if(dot3<0)dot3=-dot3;
    len2=dot1*w2+dot2*h2+dot3*d2;
    overlap2=len-len1-len2;
    if(overlap2>0)return;
    // try axis 3
    len=a3x*dx+a3y*dy+a3z*dz;
    right3=len>0;
    if(!right3)len=-len;
    len1=d1;
    dot1=a3x*a4x+a3y*a4y+a3z*a4z;
    dot2=a3x*a5x+a3y*a5y+a3z*a5z;
    dot3=a3x*a6x+a3y*a6y+a3z*a6z;
    if(dot1<0)dot1=-dot1;
    if(dot2<0)dot2=-dot2;
    if(dot3<0)dot3=-dot3;
    len2=dot1*w2+dot2*h2+dot3*d2;
    overlap3=len-len1-len2;
    if(overlap3>0)return;
    // try axis 4
    len=a4x*dx+a4y*dy+a4z*dz;
    right4=len>0;
    if(!right4)len=-len;
    dot1=a4x*a1x+a4y*a1y+a4z*a1z;
    dot2=a4x*a2x+a4y*a2y+a4z*a2z;
    dot3=a4x*a3x+a4y*a3y+a4z*a3z;
    if(dot1<0)dot1=-dot1;
    if(dot2<0)dot2=-dot2;
    if(dot3<0)dot3=-dot3;
    len1=dot1*w1+dot2*h1+dot3*d1;
    len2=w2;
    overlap4=(len-len1-len2)*1.0;
    if(overlap4>0)return;
    // try axis 5
    len=a5x*dx+a5y*dy+a5z*dz;
    right5=len>0;
    if(!right5)len=-len;
    dot1=a5x*a1x+a5y*a1y+a5z*a1z;
    dot2=a5x*a2x+a5y*a2y+a5z*a2z;
    dot3=a5x*a3x+a5y*a3y+a5z*a3z;
    if(dot1<0)dot1=-dot1;
    if(dot2<0)dot2=-dot2;
    if(dot3<0)dot3=-dot3;
    len1=dot1*w1+dot2*h1+dot3*d1;
    len2=h2;
    overlap5=(len-len1-len2)*1.0;
    if(overlap5>0)return;
    // try axis 6
    len=a6x*dx+a6y*dy+a6z*dz;
    right6=len>0;
    if(!right6)len=-len;
    dot1=a6x*a1x+a6y*a1y+a6z*a1z;
    dot2=a6x*a2x+a6y*a2y+a6z*a2z;
    dot3=a6x*a3x+a6y*a3y+a6z*a3z;
    if(dot1<0)dot1=-dot1;
    if(dot2<0)dot2=-dot2;
    if(dot3<0)dot3=-dot3;
    len1=dot1*w1+dot2*h1+dot3*d1;
    len2=d2;
    overlap6=(len-len1-len2)*1.0;
    if(overlap6>0)return;
    // try axis 7
    len=a7x*a7x+a7y*a7y+a7z*a7z;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        a7x*=len;
        a7y*=len;
        a7z*=len;
        len=a7x*dx+a7y*dy+a7z*dz;
        right7=len>0;
        if(!right7)len=-len;
        dot1=a7x*a2x+a7y*a2y+a7z*a2z;
        dot2=a7x*a3x+a7y*a3y+a7z*a3z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*h1+dot2*d1;
        dot1=a7x*a5x+a7y*a5y+a7z*a5z;
        dot2=a7x*a6x+a7y*a6y+a7z*a6z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*h2+dot2*d2;
        overlap7=len-len1-len2;
        if(overlap7>0)return;
    }else{
        right7=false;
        overlap7=0;
        invalid7=true;
    }
    // try axis 8
    len=a8x*a8x+a8y*a8y+a8z*a8z;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        a8x*=len;
        a8y*=len;
        a8z*=len;
        len=a8x*dx+a8y*dy+a8z*dz;
        right8=len>0;
        if(!right8)len=-len;
        dot1=a8x*a2x+a8y*a2y+a8z*a2z;
        dot2=a8x*a3x+a8y*a3y+a8z*a3z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*h1+dot2*d1;
        dot1=a8x*a4x+a8y*a4y+a8z*a4z;
        dot2=a8x*a6x+a8y*a6y+a8z*a6z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*w2+dot2*d2;
        overlap8=len-len1-len2;
        if(overlap8>0)return;
    }else{
        right8=false;
        overlap8=0;
        invalid8=true;
    }
    // try axis 9
    len=a9x*a9x+a9y*a9y+a9z*a9z;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        a9x*=len;
        a9y*=len;
        a9z*=len;
        len=a9x*dx+a9y*dy+a9z*dz;
        right9=len>0;
        if(!right9)len=-len;
        dot1=a9x*a2x+a9y*a2y+a9z*a2z;
        dot2=a9x*a3x+a9y*a3y+a9z*a3z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*h1+dot2*d1;
        dot1=a9x*a4x+a9y*a4y+a9z*a4z;
        dot2=a9x*a5x+a9y*a5y+a9z*a5z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*w2+dot2*h2;
        overlap9=len-len1-len2;
        if(overlap9>0)return;
    }else{
        right9=false;
        overlap9=0;
        invalid9=true;
    }
    // try axis 10
    len=aax*aax+aay*aay+aaz*aaz;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        aax*=len;
        aay*=len;
        aaz*=len;
        len=aax*dx+aay*dy+aaz*dz;
        righta=len>0;
        if(!righta)len=-len;
        dot1=aax*a1x+aay*a1y+aaz*a1z;
        dot2=aax*a3x+aay*a3y+aaz*a3z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*w1+dot2*d1;
        dot1=aax*a5x+aay*a5y+aaz*a5z;
        dot2=aax*a6x+aay*a6y+aaz*a6z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*h2+dot2*d2;
        overlapa=len-len1-len2;
        if(overlapa>0)return;
    }else{
        righta=false;
        overlapa=0;
        invalida=true;
    }
    // try axis 11
    len=abx*abx+aby*aby+abz*abz;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        abx*=len;
        aby*=len;
        abz*=len;
        len=abx*dx+aby*dy+abz*dz;
        rightb=len>0;
        if(!rightb)len=-len;
        dot1=abx*a1x+aby*a1y+abz*a1z;
        dot2=abx*a3x+aby*a3y+abz*a3z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*w1+dot2*d1;
        dot1=abx*a4x+aby*a4y+abz*a4z;
        dot2=abx*a6x+aby*a6y+abz*a6z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*w2+dot2*d2;
        overlapb=len-len1-len2;
        if(overlapb>0)return;
    }else{
        rightb=false;
        overlapb=0;
        invalidb=true;
    }
    // try axis 12
    len=acx*acx+acy*acy+acz*acz;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        acx*=len;
        acy*=len;
        acz*=len;
        len=acx*dx+acy*dy+acz*dz;
        rightc=len>0;
        if(!rightc)len=-len;
        dot1=acx*a1x+acy*a1y+acz*a1z;
        dot2=acx*a3x+acy*a3y+acz*a3z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*w1+dot2*d1;
        dot1=acx*a4x+acy*a4y+acz*a4z;
        dot2=acx*a5x+acy*a5y+acz*a5z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*w2+dot2*h2;
        overlapc=len-len1-len2;
        if(overlapc>0)return;
    }else{
        rightc=false;
        overlapc=0;
        invalidc=true;
    }
    // try axis 13
    len=adx*adx+ady*ady+adz*adz;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        adx*=len;
        ady*=len;
        adz*=len;
        len=adx*dx+ady*dy+adz*dz;
        rightd=len>0;
        if(!rightd)len=-len;
        dot1=adx*a1x+ady*a1y+adz*a1z;
        dot2=adx*a2x+ady*a2y+adz*a2z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*w1+dot2*h1;
        dot1=adx*a5x+ady*a5y+adz*a5z;
        dot2=adx*a6x+ady*a6y+adz*a6z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*h2+dot2*d2;
        overlapd=len-len1-len2;
        if(overlapd>0)return;
    }else{
        rightd=false;
        overlapd=0;
        invalidd=true;
    }
    // try axis 14
    len=aex*aex+aey*aey+aez*aez;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        aex*=len;
        aey*=len;
        aez*=len;
        len=aex*dx+aey*dy+aez*dz;
        righte=len>0;
        if(!righte)len=-len;
        dot1=aex*a1x+aey*a1y+aez*a1z;
        dot2=aex*a2x+aey*a2y+aez*a2z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*w1+dot2*h1;
        dot1=aex*a4x+aey*a4y+aez*a4z;
        dot2=aex*a6x+aey*a6y+aez*a6z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*w2+dot2*d2;
        overlape=len-len1-len2;
        if(overlape>0)return;
    }else{
        righte=false;
        overlape=0;
        invalide=true;
    }
    // try axis 15
    len=afx*afx+afy*afy+afz*afz;
    if(len>1e-5){
        len=1/Math.sqrt(len);
        afx*=len;
        afy*=len;
        afz*=len;
        len=afx*dx+afy*dy+afz*dz;
        rightf=len>0;
        if(!rightf)len=-len;
        dot1=afx*a1x+afy*a1y+afz*a1z;
        dot2=afx*a2x+afy*a2y+afz*a2z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len1=dot1*w1+dot2*h1;
        dot1=afx*a4x+afy*a4y+afz*a4z;
        dot2=afx*a5x+afy*a5y+afz*a5z;
        if(dot1<0)dot1=-dot1;
        if(dot2<0)dot2=-dot2;
        len2=dot1*w2+dot2*h2;
        overlapf=len-len1-len2;
        if(overlapf>0)return;
    }else{
        rightf=false;
        overlapf=0;
        invalidf=true;
    }
    // boxes are overlapping
    var depth=overlap1;
    var depth2=overlap1;
    var minIndex=0;
    var right=right1;
    if(overlap2>depth2){
        depth=overlap2;
        depth2=overlap2;
        minIndex=1;
        right=right2;
    }
    if(overlap3>depth2){
        depth=overlap3;
        depth2=overlap3;
        minIndex=2;
        right=right3;
    }
    if(overlap4>depth2){
        depth=overlap4;
        depth2=overlap4;
        minIndex=3;
        right=right4;
    }
    if(overlap5>depth2){
        depth=overlap5;
        depth2=overlap5;
        minIndex=4;
        right=right5;
    }
    if(overlap6>depth2){
        depth=overlap6;
        depth2=overlap6;
        minIndex=5;
        right=right6;
    }
    if(overlap7-0.01>depth2&&!invalid7){
        depth=overlap7;
        depth2=overlap7-0.01;
        minIndex=6;
        right=right7;
    }
    if(overlap8-0.01>depth2&&!invalid8){
        depth=overlap8;
        depth2=overlap8-0.01;
        minIndex=7;
        right=right8;
    }
    if(overlap9-0.01>depth2&&!invalid9){
        depth=overlap9;
        depth2=overlap9-0.01;
        minIndex=8;
        right=right9;
    }
    if(overlapa-0.01>depth2&&!invalida){
        depth=overlapa;
        depth2=overlapa-0.01;
        minIndex=9;
        right=righta;
    }
    if(overlapb-0.01>depth2&&!invalidb){
        depth=overlapb;
        depth2=overlapb-0.01;
        minIndex=10;
        right=rightb;
    }
    if(overlapc-0.01>depth2&&!invalidc){
        depth=overlapc;
        depth2=overlapc-0.01;
        minIndex=11;
        right=rightc;
    }
    if(overlapd-0.01>depth2&&!invalidd){
        depth=overlapd;
        depth2=overlapd-0.01;
        minIndex=12;
        right=rightd;
    }
    if(overlape-0.01>depth2&&!invalide){
        depth=overlape;
        depth2=overlape-0.01;
        minIndex=13;
        right=righte;
    }
    if(overlapf-0.01>depth2&&!invalidf){
        depth=overlapf;
        minIndex=14;
        right=rightf;
    }
    // normal
    var nx=0;
    var ny=0;
    var nz=0;
    // edge line or face side normal
    var n1x=0;
    var n1y=0;
    var n1z=0;
    var n2x=0;
    var n2y=0;
    var n2z=0;
    // center of current face
    var cx=0;
    var cy=0;
    var cz=0;
    // face side
    var s1x=0;
    var s1y=0;
    var s1z=0;
    var s2x=0;
    var s2y=0;
    var s2z=0;
    // swap b1 b2
    var swap=false;

    //_______________________________________

    if(minIndex==0){// b1.x * b2
        if(right){
            cx=p1x+d1x; cy=p1y+d1y;  cz=p1z+d1z;
            nx=a1x; ny=a1y; nz=a1z;
        }else{
            cx=p1x-d1x; cy=p1y-d1y; cz=p1z-d1z;
            nx=-a1x; ny=-a1y; nz=-a1z;
        }
        s1x=d2x; s1y=d2y; s1z=d2z;
        n1x=-a2x; n1y=-a2y; n1z=-a2z;
        s2x=d3x; s2y=d3y; s2z=d3z;
        n2x=-a3x; n2y=-a3y; n2z=-a3z;
    }
    else if(minIndex==1){// b1.y * b2
        if(right){
            cx=p1x+d2x; cy=p1y+d2y; cz=p1z+d2z;
            nx=a2x; ny=a2y; nz=a2z;
        }else{
            cx=p1x-d2x; cy=p1y-d2y; cz=p1z-d2z;
            nx=-a2x; ny=-a2y; nz=-a2z;
        }
        s1x=d1x; s1y=d1y; s1z=d1z;
        n1x=-a1x; n1y=-a1y; n1z=-a1z;
        s2x=d3x; s2y=d3y; s2z=d3z;
        n2x=-a3x; n2y=-a3y; n2z=-a3z;
    }
    else if(minIndex==2){// b1.z * b2
        if(right){
            cx=p1x+d3x; cy=p1y+d3y; cz=p1z+d3z;
            nx=a3x; ny=a3y; nz=a3z;
        }else{
            cx=p1x-d3x; cy=p1y-d3y; cz=p1z-d3z;
            nx=-a3x; ny=-a3y; nz=-a3z;
        }
        s1x=d1x; s1y=d1y; s1z=d1z;
        n1x=-a1x; n1y=-a1y; n1z=-a1z;
        s2x=d2x; s2y=d2y; s2z=d2z;
        n2x=-a2x; n2y=-a2y; n2z=-a2z;
    }
    else if(minIndex==3){// b2.x * b1
        swap=true;
        if(!right){
            cx=p2x+d4x; cy=p2y+d4y; cz=p2z+d4z;
            nx=a4x; ny=a4y; nz=a4z;
        }else{
            cx=p2x-d4x; cy=p2y-d4y; cz=p2z-d4z;
            nx=-a4x; ny=-a4y; nz=-a4z;
        }
        s1x=d5x; s1y=d5y; s1z=d5z;
        n1x=-a5x; n1y=-a5y; n1z=-a5z;
        s2x=d6x; s2y=d6y; s2z=d6z;
        n2x=-a6x; n2y=-a6y; n2z=-a6z;
    }
    else if(minIndex==4){// b2.y * b1
        swap=true;
        if(!right){
            cx=p2x+d5x; cy=p2y+d5y; cz=p2z+d5z;
            nx=a5x; ny=a5y; nz=a5z;
        }else{
            cx=p2x-d5x; cy=p2y-d5y; cz=p2z-d5z;
            nx=-a5x; ny=-a5y; nz=-a5z;
        }
        s1x=d4x; s1y=d4y; s1z=d4z;
        n1x=-a4x; n1y=-a4y; n1z=-a4z;
        s2x=d6x; s2y=d6y; s2z=d6z;
        n2x=-a6x; n2y=-a6y; n2z=-a6z;
    }
    else if(minIndex==5){// b2.z * b1
        swap=true;
        if(!right){
            cx=p2x+d6x; cy=p2y+d6y; cz=p2z+d6z;
            nx=a6x; ny=a6y; nz=a6z;
        }else{
            cx=p2x-d6x; cy=p2y-d6y; cz=p2z-d6z;
            nx=-a6x; ny=-a6y; nz=-a6z;
        }
        s1x=d4x; s1y=d4y; s1z=d4z;
        n1x=-a4x; n1y=-a4y; n1z=-a4z;
        s2x=d5x; s2y=d5y; s2z=d5z;
        n2x=-a5x; n2y=-a5y; n2z=-a5z;
    }
    else if(minIndex==6){// b1.x * b2.x
        nx=a7x; ny=a7y; nz=a7z;
        n1x=a1x; n1y=a1y; n1z=a1z;
        n2x=a4x; n2y=a4y; n2z=a4z;
    }
    else if(minIndex==7){// b1.x * b2.y
        nx=a8x; ny=a8y; nz=a8z;
        n1x=a1x; n1y=a1y; n1z=a1z;
        n2x=a5x; n2y=a5y; n2z=a5z;
    }
    else if(minIndex==8){// b1.x * b2.z
        nx=a9x; ny=a9y; nz=a9z;
        n1x=a1x; n1y=a1y; n1z=a1z;
        n2x=a6x; n2y=a6y; n2z=a6z;
    }
    else if(minIndex==9){// b1.y * b2.x
        nx=aax; ny=aay; nz=aaz;
        n1x=a2x; n1y=a2y; n1z=a2z;
        n2x=a4x; n2y=a4y; n2z=a4z
    }
    else if(minIndex==10){// b1.y * b2.y
        nx=abx; ny=aby; nz=abz;
        n1x=a2x; n1y=a2y; n1z=a2z;
        n2x=a5x; n2y=a5y; n2z=a5z;
    }
    else if(minIndex==11){// b1.y * b2.z
        nx=acx; ny=acy; nz=acz;
        n1x=a2x; n1y=a2y; n1z=a2z;
        n2x=a6x; n2y=a6y; n2z=a6z;
    }
    else if(minIndex==12){// b1.z * b2.x
        nx=adx;  ny=ady; nz=adz;
        n1x=a3x; n1y=a3y; n1z=a3z;
        n2x=a4x; n2y=a4y; n2z=a4z;
    }
    else if(minIndex==13){// b1.z * b2.y
        nx=aex; ny=aey; nz=aez;
        n1x=a3x; n1y=a3y; n1z=a3z;
        n2x=a5x; n2y=a5y; n2z=a5z;
    }
    else if(minIndex==14){// b1.z * b2.z
        nx=afx; ny=afy; nz=afz;
        n1x=a3x; n1y=a3y; n1z=a3z;
        n2x=a6x; n2y=a6y; n2z=a6z;
    }

    //__________________________________________

    var v;
    if(minIndex>5){
        if(!right){
            nx=-nx; ny=-ny; nz=-nz;
        }
        var distance;
        var maxDistance;
        var vx;
        var vy;
        var vz;
        var v1x;
        var v1y;
        var v1z;
        var v2x;
        var v2y;
        var v2z;
        //vertex1;
        v1x=V1[0]; v1y=V1[1]; v1z=V1[2];
        maxDistance=nx*v1x+ny*v1y+nz*v1z;
        //vertex2;
        vx=V1[3]; vy=V1[4]; vz=V1[5];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance>maxDistance){
            maxDistance=distance;
            v1x=vx; v1y=vy; v1z=vz;
        }
        //vertex3;
        vx=V1[6]; vy=V1[7]; vz=V1[8];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance>maxDistance){
            maxDistance=distance;
            v1x=vx; v1y=vy; v1z=vz;
        }
        //vertex4;
        vx=V1[9]; vy=V1[10]; vz=V1[11];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance>maxDistance){
            maxDistance=distance;
            v1x=vx; v1y=vy; v1z=vz;
        }
        //vertex5;
        vx=V1[12]; vy=V1[13]; vz=V1[14];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance>maxDistance){
            maxDistance=distance;
            v1x=vx; v1y=vy; v1z=vz;
        }
        //vertex6;
        vx=V1[15]; vy=V1[16]; vz=V1[17];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance>maxDistance){
            maxDistance=distance;
            v1x=vx; v1y=vy; v1z=vz;
        }
        //vertex7;
        vx=V1[18]; vy=V1[19]; vz=V1[20];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance>maxDistance){
            maxDistance=distance;
            v1x=vx; v1y=vy; v1z=vz;
        }
        //vertex8;
        vx=V1[21]; vy=V1[22]; vz=V1[23];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance>maxDistance){
            maxDistance=distance;
            v1x=vx; v1y=vy; v1z=vz;
        }
        //vertex1;
        v2x=V2[0]; v2y=V2[1]; v2z=V2[2];
        maxDistance=nx*v2x+ny*v2y+nz*v2z;
        //vertex2;
        vx=V2[3]; vy=V2[4]; vz=V2[5];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance<maxDistance){
            maxDistance=distance;
            v2x=vx; v2y=vy; v2z=vz;
        }
        //vertex3;
        vx=V2[6]; vy=V2[7]; vz=V2[8];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance<maxDistance){
            maxDistance=distance;
            v2x=vx; v2y=vy; v2z=vz;
        }
        //vertex4;
        vx=V2[9]; vy=V2[10]; vz=V2[11];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance<maxDistance){
            maxDistance=distance;
            v2x=vx; v2y=vy; v2z=vz;
        }
        //vertex5;
        vx=V2[12]; vy=V2[13]; vz=V2[14];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance<maxDistance){
            maxDistance=distance;
            v2x=vx; v2y=vy; v2z=vz;
        }
        //vertex6;
        vx=V2[15]; vy=V2[16]; vz=V2[17];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance<maxDistance){
            maxDistance=distance;
            v2x=vx; v2y=vy; v2z=vz;
        }
        //vertex7;
        vx=V2[18]; vy=V2[19]; vz=V2[20];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance<maxDistance){
            maxDistance=distance;
            v2x=vx; v2y=vy; v2z=vz;
        }
        //vertex8;
        vx=V2[21]; vy=V2[22]; vz=V2[23];
        distance=nx*vx+ny*vy+nz*vz;
        if(distance<maxDistance){
            maxDistance=distance;
            v2x=vx; v2y=vy; v2z=vz;
        }
        vx=v2x-v1x; vy=v2y-v1y; vz=v2z-v1z;
        dot1=n1x*n2x+n1y*n2y+n1z*n2z;
        var t=(vx*(n1x-n2x*dot1)+vy*(n1y-n2y*dot1)+vz*(n1z-n2z*dot1))/(1-dot1*dot1);
        manifold.addPoint(v1x+n1x*t+nx*depth*0.5,v1y+n1y*t+ny*depth*0.5,v1z+n1z*t+nz*depth*0.5,nx,ny,nz,depth,false);
        return;
    }
    // now detect face-face collision...
    // target quad
    var q1x;
    var q1y;
    var q1z;
    var q2x;
    var q2y;
    var q2z;
    var q3x;
    var q3y;
    var q3z;
    var q4x;
    var q4y;
    var q4z;
    // search support face and vertex
    var minDot=1;
    var dot=0;
    var minDotIndex=0;
    if(swap){
        dot=a1x*nx+a1y*ny+a1z*nz;
        if(dot<minDot){
            minDot=dot;
            minDotIndex=0;
        }
        if(-dot<minDot){
            minDot=-dot;
            minDotIndex=1;
        }
        dot=a2x*nx+a2y*ny+a2z*nz;
        if(dot<minDot){
            minDot=dot;
            minDotIndex=2;
        }
        if(-dot<minDot){
            minDot=-dot;
            minDotIndex=3;
        }
        dot=a3x*nx+a3y*ny+a3z*nz;
        if(dot<minDot){
            minDot=dot;
            minDotIndex=4;
        }
        if(-dot<minDot){
            minDot=-dot;
            minDotIndex=5;
        }

        if(minDotIndex==0){// x+ face
            q1x=V1[0]; q1y=V1[1]; q1z=V1[2];//vertex1
            q2x=V1[6]; q2y=V1[7]; q2z=V1[8];//vertex3
            q3x=V1[9]; q3y=V1[10]; q3z=V1[11];//vertex4
            q4x=V1[3]; q4y=V1[4]; q4z=V1[5];//vertex2
        }
        else if(minDotIndex==1){// x- face
            q1x=V1[15]; q1y=V1[16]; q1z=V1[17];//vertex6
            q2x=V1[21]; q2y=V1[22]; q2z=V1[23];//vertex8
            q3x=V1[18]; q3y=V1[19]; q3z=V1[20];//vertex7
            q4x=V1[12]; q4y=V1[13]; q4z=V1[14];//vertex5
        }
        else if(minDotIndex==2){// y+ face
            q1x=V1[12]; q1y=V1[13]; q1z=V1[14];//vertex5
            q2x=V1[0]; q2y=V1[1]; q2z=V1[2];//vertex1
            q3x=V1[3]; q3y=V1[4]; q3z=V1[5];//vertex2
            q4x=V1[15]; q4y=V1[16]; q4z=V1[17];//vertex6
        }
        else if(minDotIndex==3){// y- face
            q1x=V1[21]; q1y=V1[22]; q1z=V1[23];//vertex8
            q2x=V1[9]; q2y=V1[10]; q2z=V1[11];//vertex4
            q3x=V1[6]; q3y=V1[7]; q3z=V1[8];//vertex3
            q4x=V1[18]; q4y=V1[19]; q4z=V1[20];//vertex7
        }
        else if(minDotIndex==4){// z+ face
            q1x=V1[12]; q1y=V1[13]; q1z=V1[14];//vertex5
            q2x=V1[18]; q2y=V1[19]; q2z=V1[20];//vertex7
            q3x=V1[6]; q3y=V1[7]; q3z=V1[8];//vertex3
            q4x=V1[0]; q4y=V1[1]; q4z=V1[2];//vertex1
        }
        else if(minDotIndex==5){// z- face
            q1x=V1[3]; q1y=V1[4]; q1z=V1[5];//vertex2
            q2x=V1[6]; q2y=V1[7]; q2z=V1[8];//vertex4
            q3x=V1[21]; q3y=V1[22]; q3z=V1[23];//vertex8
            q4x=V1[15]; q4y=V1[16]; q4z=V1[17];//vertex6
        }

    }else{
        dot=a4x*nx+a4y*ny+a4z*nz;
        if(dot<minDot){
            minDot=dot;
            minDotIndex=0;
        }
        if(-dot<minDot){
            minDot=-dot;
            minDotIndex=1;
        }
        dot=a5x*nx+a5y*ny+a5z*nz;
        if(dot<minDot){
            minDot=dot;
            minDotIndex=2;
        }
        if(-dot<minDot){
            minDot=-dot;
            minDotIndex=3;
        }
        dot=a6x*nx+a6y*ny+a6z*nz;
        if(dot<minDot){
            minDot=dot;
            minDotIndex=4;
        }
        if(-dot<minDot){
            minDot=-dot;
            minDotIndex=5;
        }

        //______________________________________________________

        if(minDotIndex==0){// x+ face
            q1x=V2[0]; q1y=V2[1]; q1z=V2[2];//vertex1
            q2x=V2[6]; q2y=V2[7]; q2z=V2[8];//vertex3
            q3x=V2[9]; q3y=V2[10]; q3z=V2[11];//vertex4
            q4x=V2[3]; q4y=V2[4]; q4z=V2[5];//vertex2
        }
        else if(minDotIndex==1){// x- face
            q1x=V2[15]; q1y=V2[16]; q1z=V2[17];//vertex6
            q2x=V2[21]; q2y=V2[22]; q2z=V2[23]; //vertex8
            q3x=V2[18]; q3y=V2[19]; q3z=V2[20];//vertex7
            q4x=V2[12]; q4y=V2[13]; q4z=V2[14];//vertex5
        }
        else if(minDotIndex==2){// y+ face
            q1x=V2[12]; q1y=V2[13]; q1z=V2[14];//vertex5
            q2x=V2[0]; q2y=V2[1]; q2z=V2[2];//vertex1
            q3x=V2[3]; q3y=V2[4]; q3z=V2[5];//vertex2
            q4x=V2[15]; q4y=V2[16]; q4z=V2[17];//vertex6
        }
        else if(minDotIndex==3){// y- face
            q1x=V2[21]; q1y=V2[22]; q1z=V2[23];//vertex8
            q2x=V2[9]; q2y=V2[10]; q2z=V2[11];//vertex4
            q3x=V2[6]; q3y=V2[7]; q3z=V2[8];//vertex3
            q4x=V2[18]; q4y=V2[19]; q4z=V2[20];//vertex7
        }
        else if(minDotIndex==4){// z+ face
            q1x=V2[12]; q1y=V2[13]; q1z=V2[14];//vertex5
            q2x=V2[18]; q2y=V2[19]; q2z=V2[20];//vertex7
            q3x=V2[6]; q3y=V2[7]; q3z=V2[8];//vertex3
            q4x=V2[0]; q4y=V2[1]; q4z=V2[2];//vertex1
        }
        else if(minDotIndex==5){// z- face
            q1x=V2[3]; q1y=V2[4]; q1z=V2[5];//vertex2
            q2x=V2[9]; q2y=V2[10]; q2z=V2[11];//vertex4
            q3x=V2[21]; q3y=V2[22]; q3z=V2[23];//vertex8
            q4x=V2[15]; q4y=V2[16]; q4z=V2[17];//vertex6
        }
  
    }
    // clip vertices
    var numClipVertices;
    var numAddedClipVertices;
    var index;
    var x1;
    var y1;
    var z1;
    var x2;
    var y2;
    var z2;
    this.clipVertices1[0]=q1x;
    this.clipVertices1[1]=q1y;
    this.clipVertices1[2]=q1z;
    this.clipVertices1[3]=q2x;
    this.clipVertices1[4]=q2y;
    this.clipVertices1[5]=q2z;
    this.clipVertices1[6]=q3x;
    this.clipVertices1[7]=q3y;
    this.clipVertices1[8]=q3z;
    this.clipVertices1[9]=q4x;
    this.clipVertices1[10]=q4y;
    this.clipVertices1[11]=q4z;
    numAddedClipVertices=0;
    x1=this.clipVertices1[9];
    y1=this.clipVertices1[10];
    z1=this.clipVertices1[11];
    dot1=(x1-cx-s1x)*n1x+(y1-cy-s1y)*n1y+(z1-cz-s1z)*n1z;

    //var i = 4;
    //while(i--){
    for(var i=0;i<4;i++){
        index=i*3;
        x2=this.clipVertices1[index];
        y2=this.clipVertices1[index+1];
        z2=this.clipVertices1[index+2];
        dot2=(x2-cx-s1x)*n1x+(y2-cy-s1y)*n1y+(z2-cz-s1z)*n1z;
        if(dot1>0){
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices2[index]=x2;
                this.clipVertices2[index+1]=y2;
                this.clipVertices2[index+2]=z2;
            }else{
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices2[index]=x1+(x2-x1)*t;
                this.clipVertices2[index+1]=y1+(y2-y1)*t;
                this.clipVertices2[index+2]=z1+(z2-z1)*t;
            }
        }else{
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices2[index]=x1+(x2-x1)*t;
                this.clipVertices2[index+1]=y1+(y2-y1)*t;
                this.clipVertices2[index+2]=z1+(z2-z1)*t;
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices2[index]=x2;
                this.clipVertices2[index+1]=y2;
                this.clipVertices2[index+2]=z2;
            }
        }
        x1=x2;
        y1=y2;
        z1=z2;
        dot1=dot2;
    }

    numClipVertices=numAddedClipVertices;
    if(numClipVertices==0)return;
    numAddedClipVertices=0;
    index=(numClipVertices-1)*3;
    x1=this.clipVertices2[index];
    y1=this.clipVertices2[index+1];
    z1=this.clipVertices2[index+2];
    dot1=(x1-cx-s2x)*n2x+(y1-cy-s2y)*n2y+(z1-cz-s2z)*n2z;

    //i = numClipVertices;
    //while(i--){
    for(i=0;i<numClipVertices;i++){
        index=i*3;
        x2=this.clipVertices2[index];
        y2=this.clipVertices2[index+1];
        z2=this.clipVertices2[index+2];
        dot2=(x2-cx-s2x)*n2x+(y2-cy-s2y)*n2y+(z2-cz-s2z)*n2z;
        if(dot1>0){
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices1[index]=x2;
                this.clipVertices1[index+1]=y2;
                this.clipVertices1[index+2]=z2;
            }else{
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices1[index]=x1+(x2-x1)*t;
                this.clipVertices1[index+1]=y1+(y2-y1)*t;
                this.clipVertices1[index+2]=z1+(z2-z1)*t;
            }
        }else{
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices1[index]=x1+(x2-x1)*t;
                this.clipVertices1[index+1]=y1+(y2-y1)*t;
                this.clipVertices1[index+2]=z1+(z2-z1)*t;
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices1[index]=x2;
                this.clipVertices1[index+1]=y2;
                this.clipVertices1[index+2]=z2;
            }
        }
        x1=x2;
        y1=y2;
        z1=z2;
        dot1=dot2;
    }

    numClipVertices=numAddedClipVertices;
    if(numClipVertices==0)return;
    numAddedClipVertices=0;
    index=(numClipVertices-1)*3;
    x1=this.clipVertices1[index];
    y1=this.clipVertices1[index+1];
    z1=this.clipVertices1[index+2];
    dot1=(x1-cx+s1x)*-n1x+(y1-cy+s1y)*-n1y+(z1-cz+s1z)*-n1z;

    //i = numClipVertices;
    //while(i--){
    for(i=0;i<numClipVertices;i++){
        index=i*3;
        x2=this.clipVertices1[index];
        y2=this.clipVertices1[index+1];
        z2=this.clipVertices1[index+2];
        dot2=(x2-cx+s1x)*-n1x+(y2-cy+s1y)*-n1y+(z2-cz+s1z)*-n1z;
        if(dot1>0){
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices2[index]=x2;
                this.clipVertices2[index+1]=y2;
                this.clipVertices2[index+2]=z2;
            }else{
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices2[index]=x1+(x2-x1)*t;
                this.clipVertices2[index+1]=y1+(y2-y1)*t;
                this.clipVertices2[index+2]=z1+(z2-z1)*t;
            }
        }else{
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices2[index]=x1+(x2-x1)*t;
                this.clipVertices2[index+1]=y1+(y2-y1)*t;
                this.clipVertices2[index+2]=z1+(z2-z1)*t;
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices2[index]=x2;
                this.clipVertices2[index+1]=y2;
                this.clipVertices2[index+2]=z2;
            }
        }
        x1=x2;
        y1=y2;
        z1=z2;
        dot1=dot2;
    }

    numClipVertices=numAddedClipVertices;
    if(numClipVertices==0)return;
    numAddedClipVertices=0;
    index=(numClipVertices-1)*3;
    x1=this.clipVertices2[index];
    y1=this.clipVertices2[index+1];
    z1=this.clipVertices2[index+2];
    dot1=(x1-cx+s2x)*-n2x+(y1-cy+s2y)*-n2y+(z1-cz+s2z)*-n2z;

    //i = numClipVertices;
    //while(i--){
    for(i=0;i<numClipVertices;i++){
        index=i*3;
        x2=this.clipVertices2[index];
        y2=this.clipVertices2[index+1];
        z2=this.clipVertices2[index+2];
        dot2=(x2-cx+s2x)*-n2x+(y2-cy+s2y)*-n2y+(z2-cz+s2z)*-n2z;
        if(dot1>0){
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices1[index]=x2;
                this.clipVertices1[index+1]=y2;
                this.clipVertices1[index+2]=z2;
            }else{
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices1[index]=x1+(x2-x1)*t;
                this.clipVertices1[index+1]=y1+(y2-y1)*t;
                this.clipVertices1[index+2]=z1+(z2-z1)*t;
            }
        }else{
            if(dot2>0){
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                t=dot1/(dot1-dot2);
                this.clipVertices1[index]=x1+(x2-x1)*t;
                this.clipVertices1[index+1]=y1+(y2-y1)*t;
                this.clipVertices1[index+2]=z1+(z2-z1)*t;
                index=numAddedClipVertices*3;
                numAddedClipVertices++;
                this.clipVertices1[index]=x2;
                this.clipVertices1[index+1]=y2;
                this.clipVertices1[index+2]=z2;
            }
        }
        x1=x2;
        y1=y2;
        z1=z2;
        dot1=dot2;
    }

    numClipVertices=numAddedClipVertices;
    if(swap){
        var tb=b1;
        b1=b2;
        b2=tb;
    }
    if(numClipVertices==0)return;
    var flipped=b1!=shape1;
    if(numClipVertices>4){
        x1=(q1x+q2x+q3x+q4x)*0.25;
        y1=(q1y+q2y+q3y+q4y)*0.25;
        z1=(q1z+q2z+q3z+q4z)*0.25;
        n1x=q1x-x1;
        n1y=q1y-y1;
        n1z=q1z-z1;
        n2x=q2x-x1;
        n2y=q2y-y1;
        n2z=q2z-z1;
        var index1=0;
        var index2=0;
        var index3=0;
        var index4=0;
        var maxDot=-this.INF;
        minDot=this.INF;

        //i = numClipVertices;
        //while(i--){
        for(i=0;i<numClipVertices;i++){
            this.used[i]=false;
            index=i*3;
            x1=this.clipVertices1[index];
            y1=this.clipVertices1[index+1];
            z1=this.clipVertices1[index+2];
            dot=x1*n1x+y1*n1y+z1*n1z;
            if(dot<minDot){
                minDot=dot;
                index1=i;
            }
            if(dot>maxDot){
                maxDot=dot;
                index3=i;
            }
        }

        this.used[index1]=true;
        this.used[index3]=true;
        maxDot=-this.INF;
        minDot=this.INF;

        //i = numClipVertices;
        //while(i--){
        for(i=0;i<numClipVertices;i++){
            if(this.used[i])continue;
            index=i*3;
            x1=this.clipVertices1[index];
            y1=this.clipVertices1[index+1];
            z1=this.clipVertices1[index+2];
            dot=x1*n2x+y1*n2y+z1*n2z;
            if(dot<minDot){
                minDot=dot;
                index2=i;
            }
            if(dot>maxDot){
                maxDot=dot;
                index4=i;
            }
        }

        index=index1*3;
        x1=this.clipVertices1[index];
        y1=this.clipVertices1[index+1];
        z1=this.clipVertices1[index+2];
        dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;
        if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);
        
        index=index2*3;
        x1=this.clipVertices1[index];
        y1=this.clipVertices1[index+1];
        z1=this.clipVertices1[index+2];
        dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;
        if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);
        
        index=index3*3;
        x1=this.clipVertices1[index];
        y1=this.clipVertices1[index+1];
        z1=this.clipVertices1[index+2];
        dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;
        if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);
        
        index=index4*3;
        x1=this.clipVertices1[index];
        y1=this.clipVertices1[index+1];
        z1=this.clipVertices1[index+2];
        dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;
        if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);
        
    }else{
        //i = numClipVertices;
        //while(i--){
        for(i=0;i<numClipVertices;i++){
            index=i*3;
            x1=this.clipVertices1[index];
            y1=this.clipVertices1[index+1];
            z1=this.clipVertices1[index+2];
            dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;
            if(dot<0)manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);
        }
    }

}
/**
 * A collision detector which detects collisions between sphere and box.
 * @author saharan
 */
OIMO.SphereBoxCollisionDetector = function(flip){
    OIMO.CollisionDetector.call( this );
    this.flip=flip;
}
OIMO.SphereBoxCollisionDetector.prototype = Object.create( OIMO.CollisionDetector.prototype );
OIMO.SphereBoxCollisionDetector.prototype.detectCollision = function(shape1,shape2,manifold){
    var s;
    var b;
    if(this.flip){
        s=(shape2);
        b=(shape1);
    }else{
        s=(shape1);
        b=(shape2);
    }

    var D = b.dimentions;

    var ps=s.position;
    var psx=ps.x;
    var psy=ps.y;
    var psz=ps.z;
    var pb=b.position;
    var pbx=pb.x;
    var pby=pb.y;
    var pbz=pb.z;
    var rad=s.radius;

    var hw=b.halfWidth;
    var hh=b.halfHeight;
    var hd=b.halfDepth;

    var dx=psx-pbx;
    var dy=psy-pby;
    var dz=psz-pbz;
    var sx=D[0]*dx+D[1]*dy+D[2]*dz;
    var sy=D[3]*dx+D[4]*dy+D[5]*dz;
    var sz=D[6]*dx+D[7]*dy+D[8]*dz;
    var cx;
    var cy;
    var cz;
    var len;
    var invLen;
    var overlap=0;
    if(sx>hw){
        sx=hw;
    }else if(sx<-hw){
        sx=-hw;
    }else{
        overlap=1;
    }
    if(sy>hh){
        sy=hh;
    }else if(sy<-hh){
        sy=-hh;
    }else{
        overlap|=2;
    }
    if(sz>hd){
        sz=hd;
    }else if(sz<-hd){
        sz=-hd;
    }else{
        overlap|=4;
    }
    if(overlap==7){
        // center of sphere is in the box
        if(sx<0){
            dx=hw+sx;
        }else{
            dx=hw-sx;
        }
        if(sy<0){
            dy=hh+sy;
        }else{
            dy=hh-sy;
        }
        if(sz<0){
            dz=hd+sz;
        }else{
            dz=hd-sz;
        }
        if(dx<dy){
            if(dx<dz){
                len=dx-hw;
            if(sx<0){
                sx=-hw;
                dx=D[0];
                dy=D[1];
                dz=D[2];
            }else{
                sx=hw;
                dx=-D[0];
                dy=-D[1];
                dz=-D[2];
            }
        }else{
            len=dz-hd;
            if(sz<0){
                sz=-hd;
                dx=D[6];
                dy=D[7];
                dz=D[8];
            }else{
                sz=hd;
                dx=-D[6];
                dy=-D[7];
                dz=-D[8];
            }
        }
        }else{
            if(dy<dz){
                len=dy-hh;
                if(sy<0){
                    sy=-hh;
                    dx=D[3];
                    dy=D[4];
                    dz=D[5];
                }else{
                    sy=hh;
                    dx=-D[3];
                    dy=-D[4];
                    dz=-D[5];
                }
            }else{
                len=dz-hd;
                if(sz<0){
                    sz=-hd;
                    dx=D[6];
                    dy=D[7];
                    dz=D[8];
                }else{
                    sz=hd;
                    dx=-D[6];
                    dy=-D[7];
                    dz=-D[8];
            }
        }
    }
    cx=pbx+sx*D[0]+sy*D[3]+sz*D[6];
    cy=pby+sx*D[1]+sy*D[4]+sz*D[7];
    cz=pbz+sx*D[2]+sy*D[5]+sz*D[8];
    manifold.addPoint(psx+rad*dx,psy+rad*dy,psz+rad*dz,dx,dy,dz,len-rad,this.flip);
    }else{
        cx=pbx+sx*D[0]+sy*D[3]+sz*D[6];
        cy=pby+sx*D[1]+sy*D[4]+sz*D[7];
        cz=pbz+sx*D[2]+sy*D[5]+sz*D[8];
        dx=cx-ps.x;
        dy=cy-ps.y;
        dz=cz-ps.z;
        len=dx*dx+dy*dy+dz*dz;
        if(len>0&&len<rad*rad){
            len=Math.sqrt(len);
            invLen=1/len;
            dx*=invLen;
            dy*=invLen;
            dz*=invLen;
            manifold.addPoint(psx+rad*dx,psy+rad*dy,psz+rad*dz,dx,dy,dz,len-rad,this.flip);
        }
    }

}
/**
 * A collision detector which detects collisions between two spheres.
 * @author saharan
 */
OIMO.SphereSphereCollisionDetector = function(){
    OIMO.CollisionDetector.call( this );
}
OIMO.SphereSphereCollisionDetector.prototype = Object.create( OIMO.CollisionDetector.prototype );
OIMO.SphereSphereCollisionDetector.prototype.detectCollision = function(shape1,shape2,manifold){
    var s1=(shape1);
    var s2=(shape2);
    var p1=s1.position;
    var p2=s2.position;
    var dx=p2.x-p1.x;
    var dy=p2.y-p1.y;
    var dz=p2.z-p1.z;
    var len=dx*dx+dy*dy+dz*dz;
    var r1=s1.radius;
    var r2=s2.radius;
    var rad=r1+r2;
    if(len>0&&len<rad*rad){
        len=Math.sqrt(len);
        var invLen=1/len;
        dx*=invLen;
        dy*=invLen;
        dz*=invLen;
        manifold.addPoint(p1.x+dx*r1,p1.y+dy*r1,p1.z+dz*r1,dx,dy,dz,len-rad,false);
    }
}
/**
* An axis-aligned bounding box.
* @author saharan
*/
OIMO.AABB = function(minX,maxX,minY,maxY,minZ,maxZ){
    this.minX=minX || 0;
    this.maxX=maxX || 0;
    this.minY=minY || 0;
    this.maxY=maxY || 0;
    this.minZ=minZ || 0;
    this.maxZ=maxZ || 0;
}

OIMO.AABB.prototype = {

    constructor: OIMO.AABB,

    init:function(minX,maxX,minY,maxY,minZ,maxZ){
        this.minX=minX;
        this.maxX=maxX;
        this.minY=minY;
        this.maxY=maxY;
        this.minZ=minZ;
        this.maxZ=maxZ;
    },
    /**
    * Set this AABB to the combined AABB of aabb1 and aabb2.
    * @param   aabb1
    * @param   aabb2
    */
    combine:function(aabb1,aabb2){
        this.minX = (aabb1.minX<aabb2.minX) ? aabb1.minX : aabb2.minX;
        this.maxX = (aabb1.maxX>aabb2.maxX) ? aabb1.maxX : aabb2.maxX;
        this.minY = (aabb1.minY<aabb2.minY) ? aabb1.minY : aabb2.minY;
        this.maxY = (aabb1.maxY>aabb2.maxY) ? aabb1.maxY : aabb2.maxY;
        this.minZ = (aabb1.minZ<aabb2.minZ) ? aabb1.minZ : aabb2.minZ;
        this.maxZ = (aabb1.maxZ>aabb2.maxZ) ? aabb1.maxZ : aabb2.maxZ;
        /*
        var margin=0;
        this.minX-=margin;
        this.minY-=margin;
        this.minZ-=margin;
        this.maxX+=margin;
        this.maxY+=margin;
        this.maxZ+=margin;
        */
    },
    /**
    * Get the surface area.
    * @return
    */
    surfaceArea:function(){
        var h=this.maxY-this.minY;
        var d=this.maxZ-this.minZ;
        return 2*((this.maxX-this.minX)*(h+d)+h*d);
    },
    /**
    * Get whether the AABB intersects with the point or not.
    * @param   x
    * @param   y
    * @param   z
    * @return
    */
    intersectsWithPoint:function(x,y,z){
        return x>=this.minX&&x<=this.maxX&&y>=this.minY&&y<=this.maxY&&z>=this.minZ&&z<=this.maxZ;
    }
}
/**
* A proxy is used for broad-phase collecting pairs that can be colliding.
*/
OIMO.Proxy = function(shape){
	// The parent shape.
    this.shape=shape;
    // The axis-aligned bounding box.
    this.aabb=shape.aabb;
};

OIMO.Proxy.prototype = {

    constructor: OIMO.Proxy,
    
    /**
	* Update the proxy.
	*/
    update:function(){
        throw new Error("Inheritance error.");
    }
}
/**
* A basic implementation of proxies.
* @author saharan
*/
OIMO.BasicProxy = function(shape){
    OIMO.Proxy.call( this, shape );
}
OIMO.BasicProxy.prototype = Object.create( OIMO.Proxy.prototype );
OIMO.BasicProxy.prototype.update = function () {
}
/**
* The broad-phase is used for collecting all possible pairs for collision.
*/

OIMO.BroadPhase = function(){
    this.types = 0x0;
    this.numPairChecks=0;
    this.numPairs=0;
    
    this.bufferSize=256;
    this.pairs=[];// vector
    this.pairs.length = this.bufferSize;
    var i = this.bufferSize;
    while(i--){
        this.pairs[i] = new OIMO.Pair();
    }
}

OIMO.BroadPhase.prototype = {

    constructor: OIMO.BroadPhase,
    /**
    * Create a new proxy.
    * @param   shape
    * @return
    */
    createProxy:function(shape){
        throw new Error("Inheritance error.");
    },
    /**
    * Add the proxy into the broad-phase.
    * @param   proxy
    */
    addProxy:function(proxy){
        throw new Error("Inheritance error.");
    },
    /**
    * Remove the proxy from the broad-phase.
    * @param   proxy
    */
    removeProxy:function(proxy){
        throw new Error("Inheritance error.");
    },
    /**
    * Returns whether the pair is available or not.
    * @param   s1
    * @param   s2
    * @return
    */
    isAvailablePair:function(s1,s2){
        var b1=s1.parent;
        var b2=s2.parent;
        if( b1==b2 || // same parents 
            (!b1.isDynamic&&!b2.isDynamic) || // static or kinematic object
            (s1.belongsTo&s2.collidesWith)==0 ||
             (s2.belongsTo&s1.collidesWith)==0 // collision filtering
        ){ return false; }
        var js;
        if(b1.numJoints<b2.numJoints) js=b1.jointLink;
        else js=b2.jointLink;
        while(js!=null){
           var joint=js.joint;
           if( !joint.allowCollision && (joint.body1==b1&&joint.body2==b2 || joint.body1==b2&&joint.body2==b1) ){ return false; }
           js=js.next;
        }
        return true;
    },
    // Detect overlapping pairs.
    detectPairs:function(){
        while(this.numPairs>0){
            var pair=this.pairs[--this.numPairs];
            pair.shape1=null;
            pair.shape2=null;
        }
        this.numPairChecks=0;
        this.collectPairs();
    },
    collectPairs:function(){
        throw new Error("Inheritance error.");
    },
    addPair:function(s1,s2){
        if(this.numPairs==this.bufferSize){ // expand pair buffer
            var newBufferSize=this.bufferSize<<1;
            //var newBufferSize=this.bufferSize*2;
            var newPairs=[];// vector
            newPairs.length = this.bufferSize;
            //var i = this.bufferSize;
            //var j;
            //while(i--){
            for(var i=0, j=this.bufferSize;i<j;i++){
                newPairs[i]=this.pairs[i];
            }
            i = this.newBufferSize;
            //j = this.bufferSize;
            //while(i-- >= j){
            for(i=this.bufferSize, j=newBufferSize;i<j;i++){
                newPairs[i]=new OIMO.Pair();
            }
            this.pairs=newPairs;
            this.bufferSize=newBufferSize;
        }
        var pair=this.pairs[this.numPairs++];
        pair.shape1=s1;
        pair.shape2=s2;
    }
}
/**
* A broad-phase algorithm with brute-force search.
* This always checks for all possible pairs.
*/
OIMO.BruteForceBroadPhase = function(){
    OIMO.BroadPhase.call( this);
    this.types = 0x1;
    this.numProxies=0;
    this.maxProxies = 256;
    this.proxies = [];// Vector !
    this.proxies.length = 256;
}

OIMO.BruteForceBroadPhase.prototype = Object.create( OIMO.BroadPhase.prototype );

OIMO.BruteForceBroadPhase.prototype.createProxy = function (shape) {
    return new OIMO.BasicProxy(shape);
}
OIMO.BruteForceBroadPhase.prototype.addProxy = function (proxy) {
    if(this.numProxies==this.maxProxies){
        //this.maxProxies<<=1;
        this.maxProxies*=2;
        var newProxies=[];
        newProxies.length = this.maxProxies;
        var i = this.numProxies;
        while(i--){
        //for(var i=0, l=this.numProxies;i<l;i++){
            newProxies[i]=this.proxies[i];
        }
        this.proxies=newProxies;
    }
    this.proxies[this.numProxies++]=proxy;
}
OIMO.BruteForceBroadPhase.prototype.removeProxy = function (proxy) {
    var i = this.numProxies;
    while(i--){
    //for(var i=0, l=this.numProxies;i<l;i++){
        if(this.proxies[i]==proxy){
            this.proxies[i]=this.proxies[--this.numProxies];
            this.proxies[this.numProxies]=null;
            return;
        }
    }
}
OIMO.BruteForceBroadPhase.prototype.collectPairs = function () {
    this.numPairChecks=this.numProxies*(this.numProxies-1)>>1;
    //this.numPairChecks=this.numProxies*(this.numProxies-1)*0.5;
        var i = this.numProxies;
        while(i--){
        //for(var i=0, l=this.numProxies;i<l;i++){
            var p1=this.proxies[i];
            var b1=p1.aabb;
            var s1=p1.shape;
            var j = this.numProxies;
            while(j--){ if(j!==i){
            //for(var j=i+1, m=this.numProxies;j<m;j++){
                var p2=this.proxies[j];
                var b2=p2.aabb;
                var s2=p2.shape;
                if(b1.maxX<b2.minX||b1.minX>b2.maxX|| b1.maxY<b2.minY||b1.minY>b2.maxY|| b1.maxZ<b2.minZ||b1.minZ>b2.maxZ|| !this.isAvailablePair(s1,s2) ){
                    continue;
                }
                    this.addPair(s1,s2);
                }}
        }
}
/**
* A pair of shapes that may collide.
* @author saharan
*/
OIMO.Pair = function(){
	// The first shape.
    this.shape1=null;
    // The second shape.
    this.shape2=null;
};
/**
* A projection axis for sweep and prune broad-phase.
* @author saharan
*/
OIMO.SAPAxis = function(){
    this.numElements=0;
    this.bufferSize=256;
    this.elements=[];
    this.elements.length = this.bufferSize;
    this.stack=new OIMO_ARRAY_TYPE(64);
};

OIMO.SAPAxis.prototype = {

    constructor: OIMO.SAPAxis,

    addElements:function(min,max){
        if(this.numElements+2>=this.bufferSize){
            //this.bufferSize<<=1;
            this.bufferSize*=2;
            var newElements=[];
            var i = this.numElements;
            while(i--){
            //for(var i=0, l=this.numElements; i<l; i++){
                newElements[i]=this.elements[i];
            }
        }
        this.elements[this.numElements++]=min;
        this.elements[this.numElements++]=max;
    },
    removeElements:function(min,max){
        var minIndex=-1;
        var maxIndex=-1;
        for(var i=0, l=this.numElements; i<l; i++){
            var e=this.elements[i];
            if(e==min||e==max){
                if(minIndex==-1){
                    minIndex=i;
                }else{
                    maxIndex=i;
                break;
                }
            }
        }
        for(i=minIndex+1, l=maxIndex; i<l; i++){
            this.elements[i-1]=this.elements[i];
        }
        for(i=maxIndex+1, l=this.numElements; i<l; i++){
            this.elements[i-2]=this.elements[i];
        }

        this.elements[--this.numElements]=null;
        this.elements[--this.numElements]=null;
    },
    sort:function(){
        var count=0;
        var threshold=1;
        while((this.numElements>>threshold)!=0)threshold++;
        threshold=threshold*this.numElements>>2;
        count=0;
        var giveup=false;
        var elements=this.elements;
        for(var i=1, l=this.numElements; i<l; i++){ // try insertion sort
            var tmp=elements[i];
            var pivot=tmp.value;
            var tmp2=elements[i-1];
            if(tmp2.value>pivot){
                var j=i;
                do{
                    elements[j]=tmp2;
                    if(--j==0)break;
                    tmp2=elements[j-1];
                }while(tmp2.value>pivot);
                elements[j]=tmp;
                count+=i-j;
                if(count>threshold){
                    giveup=true; // stop and use quick sort
                    break;
                }
            }
        }
        if(!giveup)return;
        count=2;var stack=this.stack;
        stack[0]=0;
        stack[1]=this.numElements-1;
        while(count>0){
            var right=stack[--count];
            var left=stack[--count];
            var diff=right-left;
            if(diff>16){  // quick sort
                //var mid=left+(diff>>1);
                var mid=left+(Math.floor(diff*0.5));
                tmp=elements[mid];
                elements[mid]=elements[right];
                elements[right]=tmp;
                pivot=tmp.value;
                i=left-1;
                j=right;
                while(true){
                    var ei;
                    var ej;
                    do{ ei=elements[++i]; }while(ei.value<pivot);
                    do{ ej=elements[--j]; }while(pivot<ej.value&&j!=left);
                    if(i>=j)break;
                    elements[i]=ej;
                    elements[j]=ei;
                }

                elements[right]=elements[i];
                elements[i]=tmp;
                if(i-left>right-i){
                    stack[count++]=left;
                    stack[count++]=i-1;
                    stack[count++]=i+1;
                    stack[count++]=right;
                }else{
                    stack[count++]=i+1;
                    stack[count++]=right;
                    stack[count++]=left;
                    stack[count++]=i-1;
                }
            }else{
                for(i=left+1;i<=right;i++){
                    tmp=elements[i];
                    pivot=tmp.value;
                    tmp2=elements[i-1];
                    if(tmp2.value>pivot){
                        j=i;
                        do{
                            elements[j]=tmp2;
                            if(--j==0)break;
                            tmp2=elements[j-1];
                        }while(tmp2.value>pivot);
                        elements[j]=tmp;
                    }
                }
            }
        }
    },
    calculateTestCount:function(){
        var num=1;
        var sum=0;
        for(var i=1, l=this.numElements; i<l; i++){
            if(this.elements[i].max){
                num--;
            }else{
                sum+=num;
                num++;
            }
        }
        return sum;
    }
}
/**
* A broad-phase collision detection algorithm using sweep and prune.
* @author saharan
*/
OIMO.SAPBroadPhase = function(){
    OIMO.BroadPhase.call( this);
    this.types = 0x2;

    this.numElementsD = 0;
    this.numElementsS = 0;
    // dynamic proxies
    this.axesD = [];// vector !
    // static or sleeping proxies
    this.axesS = [];// vector !
    this.axesD.length = 3;
    this.axesS.length = 3;

    this.axesD[0]=new OIMO.SAPAxis();
    this.axesD[1]=new OIMO.SAPAxis();
    this.axesD[2]=new OIMO.SAPAxis();
    this.axesS[0]=new OIMO.SAPAxis();
    this.axesS[1]=new OIMO.SAPAxis();
    this.axesS[2]=new OIMO.SAPAxis();
    this.index1=0;
    this.index2=1;
}

OIMO.SAPBroadPhase.prototype = Object.create( OIMO.BroadPhase.prototype );

OIMO.SAPBroadPhase.prototype.createProxy = function (shape) {
    return new OIMO.SAPProxy(this,shape);
}

OIMO.SAPBroadPhase.prototype.addProxy = function (proxy) {
    var p=(proxy);
    if(p.isDynamic()){
        this.axesD[0].addElements(p.min[0],p.max[0]);
        this.axesD[1].addElements(p.min[1],p.max[1]);
        this.axesD[2].addElements(p.min[2],p.max[2]);
        p.belongsTo=1;
        this.numElementsD+=2;
        }else{
        this.axesS[0].addElements(p.min[0],p.max[0]);
        this.axesS[1].addElements(p.min[1],p.max[1]);
        this.axesS[2].addElements(p.min[2],p.max[2]);
        p.belongsTo=2;
        this.numElementsS+=2;
    }
}

OIMO.SAPBroadPhase.prototype.removeProxy = function (proxy) {
    var p=(proxy);
    if(p.belongsTo==0)return;
    switch(p.belongsTo){
        case 1:
        this.axesD[0].removeElements(p.min[0],p.max[0]);
        this.axesD[1].removeElements(p.min[1],p.max[1]);
        this.axesD[2].removeElements(p.min[2],p.max[2]);
        this.numElementsD-=2;
        break;
        case 2:
        this.axesS[0].removeElements(p.min[0],p.max[0]);
        this.axesS[1].removeElements(p.min[1],p.max[1]);
        this.axesS[2].removeElements(p.min[2],p.max[2]);
        this.numElementsS-=2;
        break;
    }
    p.belongsTo=0;
}

OIMO.SAPBroadPhase.prototype.collectPairs = function () {
    if(this.numElementsD==0)return;
    var axis1=this.axesD[this.index1];
    var axis2=this.axesD[this.index2];
    axis1.sort();
    axis2.sort();
    var count1=axis1.calculateTestCount();
    var count2=axis2.calculateTestCount();
    var elementsD;
    var elementsS;
    if(count1<=count2){// select the best axis
        axis2=this.axesS[this.index1];
        axis2.sort();
        elementsD=axis1.elements;
        elementsS=axis2.elements;
    }else{
        axis1=this.axesS[this.index2];
        axis1.sort();
        elementsD=axis2.elements;
        elementsS=axis1.elements;
        this.index1^=this.index2;
        this.index2^=this.index1;
        this.index1^=this.index2;
    }
    var activeD;
    var activeS;
    var p=0;
    var q=0;
    while(p<this.numElementsD){
        var e1;
        var dyn;
        if(q==this.numElementsS){
            e1=elementsD[p];
            dyn=true;
            p++;
        }else{
            var d=elementsD[p];
            var s=elementsS[q];
            if(d.value<s.value){
                e1=d;
                dyn=true;
                p++;
            }else{
                e1=s;
                dyn=false;
                q++;
            }
        }
        if(!e1.max){
            var s1=e1.proxy.shape;var min1=e1.min1.value;var max1=e1.max1.value;var min2=e1.min2.value;var max2=e1.max2.value;
            for(var e2=activeD;e2!=null;e2=e2.pair){// test for dynamic
                var s2=e2.proxy.shape;
                this.numPairChecks++;
                if( min1>e2.max1.value||max1<e2.min1.value|| min2>e2.max2.value||max2<e2.min2.value|| !this.isAvailablePair(s1,s2) ){ continue; }
                this.addPair(s1,s2);
            }
            if(dyn){
                for(e2=activeS;e2!=null;e2=e2.pair){// test for static
                    s2=e2.proxy.shape;
                    this.numPairChecks++;
                    if( min1>e2.max1.value||max1<e2.min1.value|| min2>e2.max2.value||max2<e2.min2.value|| !this.isAvailablePair(s1,s2) ){ continue; }
                    this.addPair(s1,s2);
                }
                e1.pair=activeD;
                activeD=e1;
            }else{
                e1.pair=activeS;
                activeS=e1;
            }
        }else{
            var min=e1.pair;
            if(dyn){
                if(min==activeD){
                    activeD=activeD.pair;
                    continue;
                }else{
                    e1=activeD;
                }
            }else{
                if(min==activeS){
                    activeS=activeS.pair;
                    continue;
                }else{
                    e1=activeS;
                }
            }
            do{
                e2=e1.pair;
                if(e2===min){
                    e1.pair=e2.pair;
                    break;
                }
                e1=e2;
            }while(e1!==null);
        }
    }
    this.index2=(this.index1|this.index2)^3;
}
/**
* An element of proxies.
* @author saharan
*/
OIMO.SAPElement = function(proxy,max){
    // The parent proxy
    this.proxy=proxy;
	// The pair element.
    this.pair = null;
    // The minimum element on other axis.
    this.min1 = null;
    // The maximum element on other axis.
    this.max1 = null;
    // The minimum element on other axis.
    this.min2 = null;
    // The maximum element on other axis.
    this.max2 = null;
    // Whether the element has maximum value or not.
    this.max = max;
    // The value of the element.
    this.value = 0;
};
/**
* A proxy for sweep and prune broad-phase.
* @author saharan
*/
OIMO.SAPProxy = function(sap,shape){
    OIMO.Proxy.call( this, shape);
    // Type of the axis to which the proxy belongs to. [0:none, 1:dynamic, 2:static]
    this.belongsTo = 0;
    // The maximum elements on each axis.
    this.max = [];
    // The minimum elements on each axis.
    this.min = [];
    
    this.sap=sap;
    this.min[0]=new OIMO.SAPElement(this,false);
    this.max[0]=new OIMO.SAPElement(this,true);
    this.min[1]=new OIMO.SAPElement(this,false);
    this.max[1]=new OIMO.SAPElement(this,true);
    this.min[2]=new OIMO.SAPElement(this,false);
    this.max[2]=new OIMO.SAPElement(this,true);
    this.max[0].pair=this.min[0];
    this.max[1].pair=this.min[1];
    this.max[2].pair=this.min[2];
    this.min[0].min1=this.min[1];
    this.min[0].max1=this.max[1];
    this.min[0].min2=this.min[2];
    this.min[0].max2=this.max[2];
    this.min[1].min1=this.min[0];
    this.min[1].max1=this.max[0];
    this.min[1].min2=this.min[2];
    this.min[1].max2=this.max[2];
    this.min[2].min1=this.min[0];
    this.min[2].max1=this.max[0];
    this.min[2].min2=this.min[1];
    this.min[2].max2=this.max[1];
};
OIMO.SAPProxy.prototype = Object.create( OIMO.Proxy.prototype );
/**
* Returns whether the proxy is dynamic or not.
* @return
*/
OIMO.SAPProxy.prototype.isDynamic = function () {
    var body=this.shape.parent;
    return body.isDynamic&&!body.sleeping;
}
OIMO.SAPProxy.prototype.update = function () {
    this.min[0].value=this.aabb.minX;
    this.max[0].value=this.aabb.maxX;
    this.min[1].value=this.aabb.minY;
    this.max[1].value=this.aabb.maxY;
    this.min[2].value=this.aabb.minZ;
    this.max[2].value=this.aabb.maxZ;
    if(this.belongsTo==1&&!this.isDynamic()||this.belongsTo==2&&this.isDynamic()){
        this.sap.removeProxy(this);
        this.sap.addProxy(this);
    }
}
/**
* A dynamic bounding volume tree for the broad-phase algorithm.
* @author saharan
*/
OIMO.DBVT = function(){
    // The root of the tree.
    this.root=null;
    this.freeNodes=[];// vector
    this.freeNodes.length = 16384;
    this.numFreeNodes=0;
    this.aabb=new OIMO.AABB();
}

OIMO.DBVT.prototype = {
    constructor: OIMO.DBVT,
    /**
    * Move a leaf.
    * @param   leaf
    */
    moveLeaf:function(leaf){
        this.deleteLeaf(leaf);
        this.insertLeaf(leaf);
    },
    /**
    * Insert a leaf to the tree.
    * @param   node
    */
    insertLeaf:function(leaf){
        if(this.root==null){
            this.root=leaf;
            return;
        }
        var lb=leaf.aabb;
        var sibling=this.root;
        var oldArea;
        var newArea;
        while(sibling.proxy==null){ // descend the node to search the best pair
            var c1=sibling.child1;
            var c2=sibling.child2;
            var b=sibling.aabb;
            var c1b=c1.aabb;
            var c2b=c2.aabb;
            oldArea=b.surfaceArea();
            this.aabb.combine(lb,b);
            newArea=this.aabb.surfaceArea();
            var creatingCost=newArea*2;
            var incrementalCost=(newArea-oldArea)*2; // cost of creating a new pair with the node
            var discendingCost1=incrementalCost;
            this.aabb.combine(lb,c1b);
            if(c1.proxy!=null){
                // leaf cost = area(combined aabb)
                discendingCost1+=this.aabb.surfaceArea();
            }else{
                // node cost = area(combined aabb) - area(old aabb)
                discendingCost1+=this.aabb.surfaceArea()-c1b.surfaceArea();
            }
            var discendingCost2=incrementalCost;
            this.aabb.combine(lb,c2b);
            if(c2.proxy!=null){
                // leaf cost = area(combined aabb)
                discendingCost2+=this.aabb.surfaceArea();
            }else{
                // node cost = area(combined aabb) - area(old aabb)
                discendingCost2+=this.aabb.surfaceArea()-c2b.surfaceArea();
            }
            if(discendingCost1<discendingCost2){
                if(creatingCost<discendingCost1){
                    break;// stop descending
                }else{
                    sibling=c1;// descend into first child
                }
            }else{
                if(creatingCost<discendingCost2){
                    break;// stop descending
                }else{
                    sibling=c2;// descend into second child
                }
            }
        }
        var oldParent=sibling.parent;
        var newParent;
        if(this.numFreeNodes>0){
            newParent=this.freeNodes[--this.numFreeNodes];
        }else{
            newParent=new OIMO.DBVTNode();
        }
        newParent.parent=oldParent;
        newParent.child1=leaf;
        newParent.child2=sibling;
        newParent.aabb.combine(leaf.aabb,sibling.aabb);
        newParent.height=sibling.height+1;
        sibling.parent=newParent;
        leaf.parent=newParent;
        if(sibling==this.root){
            // replace root
            this.root=newParent;
        }else{
            // replace child
            if(oldParent.child1==sibling){
                oldParent.child1=newParent;
            }else{
                oldParent.child2=newParent;
            }
        }
        // update whole tree
        do{
            newParent=this.balance(newParent);
            this.fix(newParent);
            newParent=newParent.parent;
        }while(newParent!=null);
    },
    getBalance:function(node){
        if(node.proxy!=null)return 0;
        return node.child1.height-node.child2.height;
    },
    print:function(node,indent,text){
        var hasChild=node.proxy==null;
        if(hasChild)text=this.print(node.child1,indent+1,text);
        for(var i=indent*2;i>=0;i--){
            text+=" ";
        }
        text+=(hasChild?this.getBalance(node):"["+node.proxy.aabb.minX+"]")+"\n";
        if(hasChild)text=this.print(node.child2,indent+1,text);
        return text;
    },
    /**
    * Delete a leaf from the tree.
    * @param   node
    */
    deleteLeaf:function(leaf){
        if(leaf==this.root){
            this.root=null;
            return;
        }
        var parent=leaf.parent;
        var sibling;
        if(parent.child1==leaf){
            sibling=parent.child2;
        }else{
            sibling=parent.child1;
        }
        if(parent==this.root){
            this.root=sibling;
            sibling.parent=null;
            return;
        }
        var grandParent=parent.parent;
        sibling.parent=grandParent;
        if(grandParent.child1==parent){
            grandParent.child1=sibling;
        }else{
            grandParent.child2=sibling;
        }
        if(this.numFreeNodes<16384){
            this.freeNodes[this.numFreeNodes++]=parent;
        }
        do{
            grandParent=this.balance(grandParent);
            this.fix(grandParent);
            grandParent=grandParent.parent;
        }while(grandParent!=null);
    },
    balance:function(node){
        var nh=node.height;
        if(nh<2){
            return node;
        }
        var p=node.parent;
        var l=node.child1;
        var r=node.child2;
        var lh=l.height;
        var rh=r.height;
        var balance=lh-rh;
        var t;// for bit operation

        //          [ N ]
        //         /     \
        //    [ L ]       [ R ]
        //     / \         / \
        // [L-L] [L-R] [R-L] [R-R]

        // Is the tree balanced?
        if(balance>1){
            var ll=l.child1;
            var lr=l.child2;
            var llh=ll.height;
            var lrh=lr.height;

            // Is L-L higher than L-R?
            if(llh>lrh){
                // set N to L-R
                l.child2=node;
                node.parent=l;

                //          [ L ]
                //         /     \
                //    [L-L]       [ N ]
                //     / \         / \
                // [...] [...] [ L ] [ R ]
                
                // set L-R
                node.child1=lr;
                lr.parent=node;

                //          [ L ]
                //         /     \
                //    [L-L]       [ N ]
                //     / \         / \
                // [...] [...] [L-R] [ R ]
                
                // fix bounds and heights
                node.aabb.combine(lr.aabb,r.aabb);
                t=lrh-rh;
                node.height=lrh-(t&t>>31)+1;
                l.aabb.combine(ll.aabb,node.aabb);
                t=llh-nh;
                l.height=llh-(t&t>>31)+1;
            }else{
                // set N to L-L
                l.child1=node;
                node.parent=l;

                //          [ L ]
                //         /     \
                //    [ N ]       [L-R]
                //     / \         / \
                // [ L ] [ R ] [...] [...]
                
                // set L-L
                node.child1=ll;
                ll.parent=node;

                //          [ L ]
                //         /     \
                //    [ N ]       [L-R]
                //     / \         / \
                // [L-L] [ R ] [...] [...]
                
                // fix bounds and heights
                node.aabb.combine(ll.aabb,r.aabb);
                t=llh-rh;
                node.height=llh-(t&t>>31)+1;

                l.aabb.combine(node.aabb,lr.aabb);
                t=nh-lrh;
                l.height=nh-(t&t>>31)+1;
            }
            // set new parent of L
            if(p!=null){
                if(p.child1==node){
                    p.child1=l;
                }else{
                    p.child2=l;
                }
            }else{
                this.root=l;
            }
            l.parent=p;
            return l;
        }else if(balance<-1){
            var rl=r.child1;
            var rr=r.child2;
            var rlh=rl.height;
            var rrh=rr.height;

            // Is R-L higher than R-R?
            if(rlh>rrh){
                // set N to R-R
                r.child2=node;
                node.parent=r;

                //          [ R ]
                //         /     \
                //    [R-L]       [ N ]
                //     / \         / \
                // [...] [...] [ L ] [ R ]
                
                // set R-R
                node.child2=rr;
                rr.parent=node;

                //          [ R ]
                //         /     \
                //    [R-L]       [ N ]
                //     / \         / \
                // [...] [...] [ L ] [R-R]
                
                // fix bounds and heights
                node.aabb.combine(l.aabb,rr.aabb);
                t=lh-rrh;
                node.height=lh-(t&t>>31)+1;
                r.aabb.combine(rl.aabb,node.aabb);
                t=rlh-nh;
                r.height=rlh-(t&t>>31)+1;
            }else{
                // set N to R-L
                r.child1=node;
                node.parent=r;
                //          [ R ]
                //         /     \
                //    [ N ]       [R-R]
                //     / \         / \
                // [ L ] [ R ] [...] [...]
                
                // set R-L
                node.child2=rl;
                rl.parent=node;

                //          [ R ]
                //         /     \
                //    [ N ]       [R-R]
                //     / \         / \
                // [ L ] [R-L] [...] [...]
                
                // fix bounds and heights
                node.aabb.combine(l.aabb,rl.aabb);
                t=lh-rlh;
                node.height=lh-(t&t>>31)+1;
                r.aabb.combine(node.aabb,rr.aabb);
                t=nh-rrh;
                r.height=nh-(t&t>>31)+1;
            }
            // set new parent of R
            if(p!=null){
                if(p.child1==node){
                    p.child1=r;
                }else{
                    p.child2=r;
                }
            }else{
                this.root=r;
            }
            r.parent=p;
            return r;
        }
        return node;
    },
    fix:function(node){
        var c1=node.child1;
        var c2=node.child2;
        node.aabb.combine(c1.aabb,c2.aabb);
        var h1=c1.height;
        var h2=c2.height;
        if(h1<h2){
            node.height=h2+1;
        }else{
            node.height=h1+1;
        }
    }
}
/**
* A broad-phase algorithm using dynamic bounding volume tree.
* @author saharan
*/
OIMO.DBVTBroadPhase = function(){
    OIMO.BroadPhase.call( this);
    this.types = 0x3;

    this.numLeaves = 0;
    this.maxLeaves = 0;
    this.tree=new OIMO.DBVT();
    this.maxStack=256;

    this.stack=[];// vector
    this.stack.length = this.maxStack;
    this.maxLeaves=256;
    this.leaves=[];// vector
    this.leaves.length = this.maxLeaves;
}
OIMO.DBVTBroadPhase.prototype = Object.create( OIMO.BroadPhase.prototype );
OIMO.DBVTBroadPhase.prototype.createProxy = function (shape) {
    return new OIMO.DBVTProxy(shape);
}
OIMO.DBVTBroadPhase.prototype.addProxy = function (proxy) {
    var p=(proxy);
    this.tree.insertLeaf(p.leaf);
    if(this.numLeaves==this.maxLeaves){
    //this.maxLeaves<<=1;
    this.maxLeaves*=2;
    var newLeaves=[];// vector
    newLeaves.length = this.maxLeaves;
    for(var i=0;i<this.numLeaves;i++){
    newLeaves[i]=this.leaves[i];
    }
    this.leaves=newLeaves;
    }
    this.leaves[this.numLeaves++]=p.leaf;
}
OIMO.DBVTBroadPhase.prototype.removeProxy = function (proxy) {
    var p=(proxy);
    this.tree.deleteLeaf(p.leaf);
    for(var i=0;i<this.numLeaves;i++){
    if(this.leaves[i]==p.leaf){
    this.leaves[i]=this.leaves[--this.numLeaves];
    this.leaves[this.numLeaves]=null;
    return;
    }
    }
}
OIMO.DBVTBroadPhase.prototype.collectPairs = function () {
    if(this.numLeaves<2)return;
    for(var i=0;i<this.numLeaves;i++){
    var leaf=this.leaves[i];
    var trueB=leaf.proxy.aabb;
    var leafB=leaf.aabb;
    if(
    trueB.minX<leafB.minX||trueB.maxX>leafB.maxX||
    trueB.minY<leafB.minY||trueB.maxY>leafB.maxY||
    trueB.minZ<leafB.minZ||trueB.maxZ>leafB.maxZ
    ){// the leaf needs correcting
    var margin=0.1;
    this.tree.deleteLeaf(leaf);
    leafB.minX=trueB.minX-margin;
    leafB.maxX=trueB.maxX+margin;
    leafB.minY=trueB.minY-margin;
    leafB.maxY=trueB.maxY+margin;
    leafB.minZ=trueB.minZ-margin;
    leafB.maxZ=trueB.maxZ+margin;
    this.tree.insertLeaf(leaf);
    this.collide(leaf,this.tree.root);
    }
    }
}
OIMO.DBVTBroadPhase.prototype.collide = function (node1,node2) {
    var stackCount=2;
    this.stack[0]=node1;
    this.stack[1]=node2;
    while(stackCount>0){
    var n1=this.stack[--stackCount];
    var n2=this.stack[--stackCount];
    var l1=n1.proxy!=null;
    var l2=n2.proxy!=null;
    this.numPairChecks++;
    if(l1&&l2){
    var s1=n1.proxy.shape;
    var s2=n2.proxy.shape;
    var b1=s1.aabb;
    var b2=s2.aabb;
    if(
    s1==s2||
    b1.maxX<b2.minX||b1.minX>b2.maxX||
    b1.maxY<b2.minY||b1.minY>b2.maxY||
    b1.maxZ<b2.minZ||b1.minZ>b2.maxZ||
    !this.isAvailablePair(s1,s2)
    ){
    continue;
    }
    this.addPair(s1,s2);
    }else{
    b1=n1.aabb;
    b2=n2.aabb;
    if(
    b1.maxX<b2.minX||b1.minX>b2.maxX||
    b1.maxY<b2.minY||b1.minY>b2.maxY||
    b1.maxZ<b2.minZ||b1.minZ>b2.maxZ
    ){
    continue;
    }
    if(stackCount+4>=this.maxStack){// expand the stack
    //this.maxStack<<=1;
    this.maxStack*=2;
    var newStack=[];// vector
    newStack.length = this.maxStack;
    for(var i=0;i<stackCount;i++){
    newStack[i]=this.stack[i];
    }
    this.stack=newStack;
    }
    if(l2||!l1&&(n1.aabb.surfaceArea()>n2.aabb.surfaceArea())){
    this.stack[stackCount++]=n1.child1;
    this.stack[stackCount++]=n2;
    this.stack[stackCount++]=n1.child2;
    this.stack[stackCount++]=n2;
    }else{
    this.stack[stackCount++]=n1;
    this.stack[stackCount++]=n2.child1;
    this.stack[stackCount++]=n1;
    this.stack[stackCount++]=n2.child2;
    }
    }
    }
}
/**
* A node of the dynamic bounding volume tree.
* @author saharan
*/
OIMO.DBVTNode = function(){
	// The first child node of this node.
    this.child1=null;
    // The second child node of this node.
    this.child2=null;
    //  The parent node of this tree.
    this.parent=null;
    // The proxy of this node. This has no value if this node is not leaf.
    this.proxy=null;
    // The maximum distance from leaf nodes.
    this.height=0;
    // The AABB of this node.
    this.aabb=new OIMO.AABB();
}
/**
* A proxy for dynamic bounding volume tree broad-phase.
* @author saharan
*/
OIMO.DBVTProxy = function(shape){
    OIMO.Proxy.call( this, shape);
    // The leaf of the proxy.
    this.leaf = new OIMO.DBVTNode();
    this.leaf.proxy = this;
}
OIMO.DBVTProxy.prototype = Object.create( OIMO.Proxy.prototype );
OIMO.DBVTProxy.prototype.update = function () {
}