#

API Docs for: 0.6.1
Show: Inherited Protected Private Deprecated

# File: src/world/World.js

    /* global performance */

    module.exports = World;

    var Shape = require('../shapes/Shape');
    var Vec3 = require('../math/Vec3');
    var Quaternion = require('../math/Quaternion');
    var GSSolver = require('../solver/GSSolver');
    var Vec3Pool = require('../utils/Vec3Pool');
    var ContactEquation = require('../equations/ContactEquation');
    var FrictionEquation = require('../equations/FrictionEquation');
    var Narrowphase = require('./Narrowphase');
    var EventTarget = require('../utils/EventTarget');
    var ArrayCollisionMatrix = require('../collision/ArrayCollisionMatrix');
    var Material = require('../material/Material');
    var ContactMaterial = require('../material/ContactMaterial');
    var Body = require('../objects/Body');
    var TupleDictionary = require('../utils/TupleDictionary');
    var RaycastResult = require('../collision/RaycastResult');
    var AABB = require('../collision/AABB');
    var Ray = require('../collision/Ray');
    var NaiveBroadphase = require('../collision/NaiveBroadphase');

    /**
     * The physics world
     * @class World
     * @constructor
     * @extends EventTarget
     */
    function World(){
        EventTarget.apply(this);

        /**
         * Currently / last used timestep. Is set to -1 if not available. This value is updated before each internal step, which means that it is "fresh" inside event callbacks.
         * @property {Number} dt
         */
        this.dt = -1;

        /**
         * Makes bodies go to sleep when they've been inactive
         * @property allowSleep
         * @type {Boolean}
         */
        this.allowSleep = false;

        /**
         * All the current contacts (instances of ContactEquation) in the world.
         * @property contacts
         * @type {Array}
         */
        this.contacts = [];
        this.frictionEquations = [];

        /**
         * How often to normalize quaternions. Set to 0 for every step, 1 for every second etc.. A larger value increases performance. If bodies tend to explode, set to a smaller value (zero to be sure nothing can go wrong).
         * @property quatNormalizeSkip
         * @type {Number}
         */
        this.quatNormalizeSkip = 0;

        /**
         * Set to true to use fast quaternion normalization. It is often enough accurate to use. If bodies tend to explode, set to false.
         * @property quatNormalizeFast
         * @type {Boolean}
         * @see Quaternion.normalizeFast
         * @see Quaternion.normalize
         */
        this.quatNormalizeFast = false;

        /**
         * The wall-clock time since simulation start
         * @property time
         * @type {Number}
         */
        this.time = 0.0;

        /**
         * Number of timesteps taken since start
         * @property stepnumber
         * @type {Number}
         */
        this.stepnumber = 0;

        /// Default and last timestep sizes
        this.default_dt = 1/60;

        this.nextId = 0;
        /**
         * @property gravity
         * @type {Vec3}
         */
        this.gravity = new Vec3();

        /**
         * @property broadphase
         * @type {Broadphase}
         */
        this.broadphase = new NaiveBroadphase();

        /**
         * @property bodies
         * @type {Array}
         */
        this.bodies = [];

        /**
         * @property solver
         * @type {Solver}
         */
        this.solver = new GSSolver();

        /**
         * @property constraints
         * @type {Array}
         */
        this.constraints = [];

        /**
         * @property narrowphase
         * @type {Narrowphase}
         */
        this.narrowphase = new Narrowphase(this);

        /**
         * @property {ArrayCollisionMatrix} collisionMatrix
    	 * @type {ArrayCollisionMatrix}
    	 */
    	this.collisionMatrix = new ArrayCollisionMatrix();

        /**
         * CollisionMatrix from the previous step.
         * @property {ArrayCollisionMatrix} collisionMatrixPrevious
    	 * @type {ArrayCollisionMatrix}
    	 */
    	this.collisionMatrixPrevious = new ArrayCollisionMatrix();

        /**
         * All added materials
         * @property materials
         * @type {Array}
         */
        this.materials = [];

        /**
         * @property contactmaterials
         * @type {Array}
         */
        this.contactmaterials = [];

        /**
         * Used to look up a ContactMaterial given two instances of Material.
         * @property {TupleDictionary} contactMaterialTable
         */
        this.contactMaterialTable = new TupleDictionary();

        this.defaultMaterial = new Material("default");

        /**
         * This contact material is used if no suitable contactmaterial is found for a contact.
         * @property defaultContactMaterial
         * @type {ContactMaterial}
         */
        this.defaultContactMaterial = new ContactMaterial(this.defaultMaterial, this.defaultMaterial, { friction: 0.3, restitution: 0.0 });

        /**
         * @property doProfiling
         * @type {Boolean}
         */
        this.doProfiling = false;

        /**
         * @property profile
         * @type {Object}
         */
        this.profile = {
            solve:0,
            makeContactConstraints:0,
            broadphase:0,
            integrate:0,
            narrowphase:0,
        };

        /**
         * @property subsystems
         * @type {Array}
         */
        this.subsystems = [];

        this.addBodyEvent = {
            type:"addBody",
            body : null,
        };

        this.removeBodyEvent = {
            type:"removeBody",
            body : null,
        };
    }
    World.prototype = new EventTarget();

    // Temp stuff
    var tmpAABB1 = new AABB();
    var tmpArray1 = [];
    var tmpRay = new Ray();

    /**
     * Get the contact material between materials m1 and m2
     * @method getContactMaterial
     * @param {Material} m1
     * @param {Material} m2
     * @return {ContactMaterial} The contact material if it was found.
     */
    World.prototype.getContactMaterial = function(m1,m2){
        return this.contactMaterialTable.get(m1.id,m2.id); //this.contactmaterials[this.mats2cmat[i+j*this.materials.length]];
    };

    /**
     * Get number of objects in the world.
     * @method numObjects
     * @return {Number}
     * @deprecated
     */
    World.prototype.numObjects = function(){
        return this.bodies.length;
    };

    /**
     * Store old collision state info
     * @method collisionMatrixTick
     */
    World.prototype.collisionMatrixTick = function(){
    	var temp = this.collisionMatrixPrevious;
    	this.collisionMatrixPrevious = this.collisionMatrix;
    	this.collisionMatrix = temp;
    	this.collisionMatrix.reset();
    };

    /**
     * Add a rigid body to the simulation.
     * @method add
     * @param {Body} body
     * @todo If the simulation has not yet started, why recrete and copy arrays for each body? Accumulate in dynamic arrays in this case.
     * @todo Adding an array of bodies should be possible. This would save some loops too
     * @deprecated Use .addBody instead
     */
    World.prototype.add = World.prototype.addBody = function(body){
        if(this.bodies.indexOf(body) !== -1){
            return;
        }
        body.index = this.bodies.length;
        this.bodies.push(body);
        body.world = this;
        body.initPosition.copy(body.position);
        body.initVelocity.copy(body.velocity);
        body.timeLastSleepy = this.time;
        if(body instanceof Body){
            body.initAngularVelocity.copy(body.angularVelocity);
            body.initQuaternion.copy(body.quaternion);
        }
    	this.collisionMatrix.setNumObjects(this.bodies.length);
        this.addBodyEvent.body = body;
        this.dispatchEvent(this.addBodyEvent);
    };

    /**
     * Add a constraint to the simulation.
     * @method addConstraint
     * @param {Constraint} c
     */
    World.prototype.addConstraint = function(c){
        this.constraints.push(c);
    };

    /**
     * Removes a constraint
     * @method removeConstraint
     * @param {Constraint} c
     */
    World.prototype.removeConstraint = function(c){
        var idx = this.constraints.indexOf(c);
        if(idx!==-1){
            this.constraints.splice(idx,1);
        }
    };

    /**
     * Raycast test
     * @method rayTest
     * @param {Vec3} from
     * @param {Vec3} to
     * @param {Function|RaycastResult} result
     * @deprecated Use .raycastAll, .raycastClosest or .raycastAny instead.
     */
    World.prototype.rayTest = function(from, to, result){
        if(result instanceof RaycastResult){
            // Do raycastclosest
            this.raycastClosest(from, to, {
                skipBackfaces: true
            }, result);
        } else {
            // Do raycastAll
            this.raycastAll(from, to, {
                skipBackfaces: true
            }, result);
        }
    };

    /**
     * Ray cast against all bodies. The provided callback will be executed for each hit with a RaycastResult as single argument.
     * @method raycastAll
     * @param  {Vec3} from
     * @param  {Vec3} to
     * @param  {Object} options
     * @param  {number} [options.collisionFilterMask=-1]
     * @param  {number} [options.collisionFilterGroup=-1]
     * @param  {boolean} [options.skipBackfaces=false]
     * @param  {boolean} [options.checkCollisionResponse=true]
     * @param  {Function} callback
     * @return {boolean} True if any body was hit.
     */
    World.prototype.raycastAll = function(from, to, options, callback){
        options.mode = Ray.ALL;
        options.from = from;
        options.to = to;
        options.callback = callback;
        return tmpRay.intersectWorld(this, options);
    };

    /**
     * Ray cast, and stop at the first result. Note that the order is random - but the method is fast.
     * @method raycastAny
     * @param  {Vec3} from
     * @param  {Vec3} to
     * @param  {Object} options
     * @param  {number} [options.collisionFilterMask=-1]
     * @param  {number} [options.collisionFilterGroup=-1]
     * @param  {boolean} [options.skipBackfaces=false]
     * @param  {boolean} [options.checkCollisionResponse=true]
     * @param  {RaycastResult} result
     * @return {boolean} True if any body was hit.
     */
    World.prototype.raycastAny = function(from, to, options, result){
        options.mode = Ray.ANY;
        options.from = from;
        options.to = to;
        options.result = result;
        return tmpRay.intersectWorld(this, options);
    };

    /**
     * Ray cast, and return information of the closest hit.
     * @method raycastClosest
     * @param  {Vec3} from
     * @param  {Vec3} to
     * @param  {Object} options
     * @param  {number} [options.collisionFilterMask=-1]
     * @param  {number} [options.collisionFilterGroup=-1]
     * @param  {boolean} [options.skipBackfaces=false]
     * @param  {boolean} [options.checkCollisionResponse=true]
     * @param  {RaycastResult} result
     * @return {boolean} True if any body was hit.
     */
    World.prototype.raycastClosest = function(from, to, options, result){
        options.mode = Ray.CLOSEST;
        options.from = from;
        options.to = to;
        options.result = result;
        return tmpRay.intersectWorld(this, options);
    };

    /**
     * Remove a rigid body from the simulation.
     * @method remove
     * @param {Body} body
     * @deprecated Use .removeBody instead
     */
    World.prototype.remove = function(body){
        body.world = null;
        var n = this.bodies.length-1,
            bodies = this.bodies,
            idx = bodies.indexOf(body);
        if(idx !== -1){
            bodies.splice(idx, 1); // Todo: should use a garbage free method

            // Recompute index
            for(var i=0; i!==bodies.length; i++){
                bodies[i].index = i;
            }

            this.collisionMatrix.setNumObjects(n);
            this.removeBodyEvent.body = body;
            this.dispatchEvent(this.removeBodyEvent);
        }
    };

    /**
     * Remove a rigid body from the simulation.
     * @method removeBody
     * @param {Body} body
     */
    World.prototype.removeBody = World.prototype.remove;

    /**
     * Adds a material to the World.
     * @method addMaterial
     * @param {Material} m
     * @todo Necessary?
     */
    World.prototype.addMaterial = function(m){
        this.materials.push(m);
    };

    /**
     * Adds a contact material to the World
     * @method addContactMaterial
     * @param {ContactMaterial} cmat
     */
    World.prototype.addContactMaterial = function(cmat) {

        // Add contact material
        this.contactmaterials.push(cmat);

        // Add current contact material to the material table
        this.contactMaterialTable.set(cmat.materials[0].id,cmat.materials[1].id,cmat);
    };

    // performance.now()
    if(typeof performance === 'undefined'){
        performance = {};
    }
    if(!performance.now){
        var nowOffset = Date.now();
        if (performance.timing && performance.timing.navigationStart){
            nowOffset = performance.timing.navigationStart;
        }
        performance.now = function(){
            return Date.now() - nowOffset;
        };
    }

    var step_tmp1 = new Vec3();

    /**
     * Step the physics world forward in time.
     *
     * There are two modes. The simple mode is fixed timestepping without interpolation. In this case you only use the first argument. The second case uses interpolation. In that you also provide the time since the function was last used, as well as the maximum fixed timesteps to take.
     *
     * @method step
     * @param {Number} dt                       The fixed time step size to use.
     * @param {Number} [timeSinceLastCalled]    The time elapsed since the function was last called.
     * @param {Number} [maxSubSteps=10]         Maximum number of fixed steps to take per function call.
     *
     * @example
     *     // fixed timestepping without interpolation
     *     world.step(1/60);
     *
     * @see http://bulletphysics.org/mediawiki-1.5.8/index.php/Stepping_The_World
     */
    World.prototype.step = function(dt, timeSinceLastCalled, maxSubSteps){
        maxSubSteps = maxSubSteps || 10;
        timeSinceLastCalled = timeSinceLastCalled || 0;

        if(timeSinceLastCalled === 0){ // Fixed, simple stepping

            this.internalStep(dt);

            // Increment time
            this.time += dt;

        } else {

            // Compute the number of fixed steps we should have taken since the last step
            var internalSteps = Math.floor((this.time + timeSinceLastCalled) / dt) - Math.floor(this.time / dt);
            internalSteps = Math.min(internalSteps,maxSubSteps);

            // Do some fixed steps to catch up
            var t0 = performance.now();
            for(var i=0; i!==internalSteps; i++){
                this.internalStep(dt);
                if(performance.now() - t0 > dt * 1000){
                    // We are slower than real-time. Better bail out.
                    break;
                }
            }

            // Increment internal clock
            this.time += timeSinceLastCalled;

            // Compute "Left over" time step
            var h = this.time % dt;
            var h_div_dt = h / dt;
            var interpvelo = step_tmp1;
            var bodies = this.bodies;

            for(var j=0; j !== bodies.length; j++){
                var b = bodies[j];
                if(b.type !== Body.STATIC && b.sleepState !== Body.SLEEPING){

                    // Interpolate
                    b.position.vsub(b.previousPosition, interpvelo);
                    interpvelo.scale(h_div_dt, interpvelo);
                    b.position.vadd(interpvelo, b.interpolatedPosition);

                    // TODO: interpolate quaternion
                    // b.interpolatedAngle = b.angle + (b.angle - b.previousAngle) * h_div_dt;

                } else {

                    // For static bodies, just copy. Who else will do it?
                    b.interpolatedPosition.copy(b.position);
                    b.interpolatedQuaternion.copy(b.quaternion);
                }
            }
        }
    };

    /**
     * Step the simulation
     * @method step
     * @param {Number} dt
     */
    var World_step_postStepEvent = {type:"postStep"}, // Reusable event objects to save memory
        World_step_preStepEvent = {type:"preStep"},
        World_step_collideEvent = {type:"collide", body:null, contact:null },
        World_step_oldContacts = [], // Pools for unused objects
        World_step_frictionEquationPool = [],
        World_step_p1 = [], // Reusable arrays for collision pairs
        World_step_p2 = [],
        World_step_gvec = new Vec3(), // Temporary vectors and quats
        World_step_vi = new Vec3(),
        World_step_vj = new Vec3(),
        World_step_wi = new Vec3(),
        World_step_wj = new Vec3(),
        World_step_t1 = new Vec3(),
        World_step_t2 = new Vec3(),
        World_step_rixn = new Vec3(),
        World_step_rjxn = new Vec3(),
        World_step_step_q = new Quaternion(),
        World_step_step_w = new Quaternion(),
        World_step_step_wq = new Quaternion(),
        invI_tau_dt = new Vec3();
    World.prototype.internalStep = function(dt){
        this.dt = dt;

        var world = this,
            that = this,
            contacts = this.contacts,
            p1 = World_step_p1,
            p2 = World_step_p2,
            N = this.numObjects(),
            bodies = this.bodies,
            solver = this.solver,
            gravity = this.gravity,
            doProfiling = this.doProfiling,
            profile = this.profile,
            DYNAMIC = Body.DYNAMIC,
            profilingStart,
            constraints = this.constraints,
            frictionEquationPool = World_step_frictionEquationPool,
            gnorm = gravity.norm(),
            gx = gravity.x,
            gy = gravity.y,
            gz = gravity.z,
            i=0;

        if(doProfiling){
            profilingStart = performance.now();
        }

        // Add gravity to all objects
        for(i=0; i!==N; i++){
            var bi = bodies[i];
            if(bi.type & DYNAMIC){ // Only for dynamic bodies
                var f = bi.force, m = bi.mass;
                f.x += m*gx;
                f.y += m*gy;
                f.z += m*gz;
            }
        }

        // Update subsystems
        for(var i=0, Nsubsystems=this.subsystems.length; i!==Nsubsystems; i++){
            this.subsystems[i].update();
        }

        // Collision detection
        if(doProfiling){ profilingStart = performance.now(); }
        p1.length = 0; // Clean up pair arrays from last step
        p2.length = 0;
        this.broadphase.collisionPairs(this,p1,p2);
        if(doProfiling){ profile.broadphase = performance.now() - profilingStart; }

        // Remove constrained pairs with collideConnected == false
        var Nconstraints = constraints.length;
        for(i=0; i!==Nconstraints; i++){
            var c = constraints[i];
            if(!c.collideConnected){
                for(var j = p1.length-1; j>=0; j-=1){
                    if( (c.bodyA === p1[j] && c.bodyB === p2[j]) ||
                        (c.bodyB === p1[j] && c.bodyA === p2[j])){
                        p1.splice(j, 1);
                        p2.splice(j, 1);
                    }
                }
            }
        }

        this.collisionMatrixTick();

        // Generate contacts
        if(doProfiling){ profilingStart = performance.now(); }
        var oldcontacts = World_step_oldContacts;
        var NoldContacts = contacts.length;

        for(i=0; i!==NoldContacts; i++){
            oldcontacts.push(contacts[i]);
        }
        contacts.length = 0;

        // Transfer FrictionEquation from current list to the pool for reuse
        var NoldFrictionEquations = this.frictionEquations.length;
        for(i=0; i!==NoldFrictionEquations; i++){
            frictionEquationPool.push(this.frictionEquations[i]);
        }
        this.frictionEquations.length = 0;

        this.narrowphase.getContacts(
            p1,
            p2,
            this,
            contacts,
            oldcontacts, // To be reused
            this.frictionEquations,
            frictionEquationPool
        );

        if(doProfiling){
            profile.narrowphase = performance.now() - profilingStart;
        }

        // Loop over all collisions
        if(doProfiling){
            profilingStart = performance.now();
        }

        // Add all friction eqs
        for (var i = 0; i < this.frictionEquations.length; i++) {
            solver.addEquation(this.frictionEquations[i]);
        }

        var ncontacts = contacts.length;
        for(var k=0; k!==ncontacts; k++){

            // Current contact
            var c = contacts[k];

            // Get current collision indeces
            var bi = c.bi,
                bj = c.bj,
                si = c.si,
                sj = c.sj;

            // Get collision properties
            var cm;
            if(bi.material && bj.material){
                cm = this.getContactMaterial(bi.material,bj.material) || this.defaultContactMaterial;
            } else {
                cm = this.defaultContactMaterial;
            }

            // c.enabled = bi.collisionResponse && bj.collisionResponse && si.collisionResponse && sj.collisionResponse;

            var mu = cm.friction;
            // c.restitution = cm.restitution;

            // If friction or restitution were specified in the material, use them
            if(bi.material && bj.material){
                if(bi.material.friction >= 0 && bj.material.friction >= 0){
                    mu = bi.material.friction * bj.material.friction;
                }

                if(bi.material.restitution >= 0 && bj.material.restitution >= 0){
                    c.restitution = bi.material.restitution * bj.material.restitution;
                }
            }

    		// c.setSpookParams(
      //           cm.contactEquationStiffness,
      //           cm.contactEquationRelaxation,
      //           dt
      //       );

    		solver.addEquation(c);

    		// // Add friction constraint equation
    		// if(mu > 0){

    		// 	// Create 2 tangent equations
    		// 	var mug = mu * gnorm;
    		// 	var reducedMass = (bi.invMass + bj.invMass);
    		// 	if(reducedMass > 0){
    		// 		reducedMass = 1/reducedMass;
    		// 	}
    		// 	var pool = frictionEquationPool;
    		// 	var c1 = pool.length ? pool.pop() : new FrictionEquation(bi,bj,mug*reducedMass);
    		// 	var c2 = pool.length ? pool.pop() : new FrictionEquation(bi,bj,mug*reducedMass);
    		// 	this.frictionEquations.push(c1, c2);

    		// 	c1.bi = c2.bi = bi;
    		// 	c1.bj = c2.bj = bj;
    		// 	c1.minForce = c2.minForce = -mug*reducedMass;
    		// 	c1.maxForce = c2.maxForce = mug*reducedMass;

    		// 	// Copy over the relative vectors
    		// 	c1.ri.copy(c.ri);
    		// 	c1.rj.copy(c.rj);
    		// 	c2.ri.copy(c.ri);
    		// 	c2.rj.copy(c.rj);

    		// 	// Construct tangents
    		// 	c.ni.tangents(c1.t, c2.t);

      //           // Set spook params
      //           c1.setSpookParams(cm.frictionEquationStiffness, cm.frictionEquationRelaxation, dt);
      //           c2.setSpookParams(cm.frictionEquationStiffness, cm.frictionEquationRelaxation, dt);

      //           c1.enabled = c2.enabled = c.enabled;

    		// 	// Add equations to solver
    		// 	solver.addEquation(c1);
    		// 	solver.addEquation(c2);
    		// }

            if( bi.allowSleep &&
                bi.type === Body.DYNAMIC &&
                bi.sleepState  === Body.SLEEPING &&
                bj.sleepState  === Body.AWAKE &&
                bj.type !== Body.STATIC
            ){
                var speedSquaredB = bj.velocity.norm2() + bj.angularVelocity.norm2();
                var speedLimitSquaredB = Math.pow(bj.sleepSpeedLimit,2);
                if(speedSquaredB >= speedLimitSquaredB*2){
                    bi._wakeUpAfterNarrowphase = true;
                }
            }

            if( bj.allowSleep &&
                bj.type === Body.DYNAMIC &&
                bj.sleepState  === Body.SLEEPING &&
                bi.sleepState  === Body.AWAKE &&
                bi.type !== Body.STATIC
            ){
                var speedSquaredA = bi.velocity.norm2() + bi.angularVelocity.norm2();
                var speedLimitSquaredA = Math.pow(bi.sleepSpeedLimit,2);
                if(speedSquaredA >= speedLimitSquaredA*2){
                    bj._wakeUpAfterNarrowphase = true;
                }
            }

            // Now we know that i and j are in contact. Set collision matrix state
    		this.collisionMatrix.set(bi, bj, true);

            if (!this.collisionMatrixPrevious.get(bi, bj)) {
                // First contact!
                // We reuse the collideEvent object, otherwise we will end up creating new objects for each new contact, even if there's no event listener attached.
                World_step_collideEvent.body = bj;
                World_step_collideEvent.contact = c;
                bi.dispatchEvent(World_step_collideEvent);

                World_step_collideEvent.body = bi;
                bj.dispatchEvent(World_step_collideEvent);
            }
        }
        if(doProfiling){
            profile.makeContactConstraints = performance.now() - profilingStart;
            profilingStart = performance.now();
        }

        // Wake up bodies
        for(i=0; i!==N; i++){
            var bi = bodies[i];
            if(bi._wakeUpAfterNarrowphase){
                bi.wakeUp();
                bi._wakeUpAfterNarrowphase = false;
            }
        }

        // Add user-added constraints
        var Nconstraints = constraints.length;
        for(i=0; i!==Nconstraints; i++){
            var c = constraints[i];
            c.update();
            for(var j=0, Neq=c.equations.length; j!==Neq; j++){
                var eq = c.equations[j];
                solver.addEquation(eq);
            }
        }

        // Solve the constrained system
        solver.solve(dt,this);

        if(doProfiling){
            profile.solve = performance.now() - profilingStart;
        }

        // Remove all contacts from solver
        solver.removeAllEquations();

        // Apply damping, see http://code.google.com/p/bullet/issues/detail?id=74 for details
        var pow = Math.pow;
        for(i=0; i!==N; i++){
            var bi = bodies[i];
            if(bi.type & DYNAMIC){ // Only for dynamic bodies
                var ld = pow(1.0 - bi.linearDamping,dt);
                var v = bi.velocity;
                v.mult(ld,v);
                var av = bi.angularVelocity;
                if(av){
                    var ad = pow(1.0 - bi.angularDamping,dt);
                    av.mult(ad,av);
                }
            }
        }

        this.dispatchEvent(World_step_preStepEvent);

        // Invoke pre-step callbacks
        for(i=0; i!==N; i++){
            var bi = bodies[i];
            if(bi.preStep){
                bi.preStep.call(bi);
            }
        }

        // Leap frog
        // vnew = v + h*f/m
        // xnew = x + h*vnew
        if(doProfiling){
            profilingStart = performance.now();
        }
        var q = World_step_step_q;
        var w = World_step_step_w;
        var wq = World_step_step_wq;
        var stepnumber = this.stepnumber;
        var DYNAMIC_OR_KINEMATIC = Body.DYNAMIC | Body.KINEMATIC;
        var quatNormalize = stepnumber % (this.quatNormalizeSkip+1) === 0;
        var quatNormalizeFast = this.quatNormalizeFast;
        var half_dt = dt * 0.5;
        var PLANE = Shape.types.PLANE,
            CONVEX = Shape.types.CONVEXPOLYHEDRON;

        for(i=0; i!==N; i++){
            var b = bodies[i],
                force = b.force,
                tau = b.torque;
            if((b.type & DYNAMIC_OR_KINEMATIC) && b.sleepState !== Body.SLEEPING){ // Only for dynamic
                var velo = b.velocity,
                    angularVelo = b.angularVelocity,
                    pos = b.position,
                    quat = b.quaternion,
                    invMass = b.invMass,
                    invInertia = b.invInertiaWorld;

                velo.x += force.x * invMass * dt;
                velo.y += force.y * invMass * dt;
                velo.z += force.z * invMass * dt;

                if(b.angularVelocity){
                    invInertia.vmult(tau,invI_tau_dt);
                    invI_tau_dt.mult(dt,invI_tau_dt);
                    invI_tau_dt.vadd(angularVelo,angularVelo);
                }

                // Use new velocity  - leap frog
                pos.x += velo.x * dt;
                pos.y += velo.y * dt;
                pos.z += velo.z * dt;

                if(b.angularVelocity){
                    w.set(angularVelo.x, angularVelo.y, angularVelo.z, 0);
                    w.mult(quat,wq);
                    quat.x += half_dt * wq.x;
                    quat.y += half_dt * wq.y;
                    quat.z += half_dt * wq.z;
                    quat.w += half_dt * wq.w;
                    if(quatNormalize){
                        if(quatNormalizeFast){
                            quat.normalizeFast();
                        } else {
                            quat.normalize();
                        }
                    }
                }

                if(b.aabb){
                    b.aabbNeedsUpdate = true;
                }

                // Update world inertia
                if(b.updateInertiaWorld){
                    b.updateInertiaWorld();
                }
            }
        }
        this.clearForces();

        this.broadphase.dirty = true;

        if(doProfiling){
            profile.integrate = performance.now() - profilingStart;
        }

        // Update world time
        this.time += dt;
        this.stepnumber += 1;

        this.dispatchEvent(World_step_postStepEvent);

        // Invoke post-step callbacks
        for(i=0; i!==N; i++){
            var bi = bodies[i];
            var postStep = bi.postStep;
            if(postStep){
                postStep.call(bi);
            }
        }

        // Sleeping update
        if(this.allowSleep){
            for(i=0; i!==N; i++){
                bodies[i].sleepTick(this.time);
            }
        }
    };

    /**
     * Sets all body forces in the world to zero.
     * @method clearForces
     */
    World.prototype.clearForces = function(){
        var bodies = this.bodies;
        var N = bodies.length;
        for(var i=0; i !== N; i++){
            var b = bodies[i],
                force = b.force,
                tau = b.torque;

            b.force.set(0,0,0);
            b.torque.set(0,0,0);
        }
    };