API Docs for: 0.6.1
Show:

File: src/world/World.js

  1. /* global performance */
  2.  
  3. module.exports = World;
  4.  
  5. var Shape = require('../shapes/Shape');
  6. var Vec3 = require('../math/Vec3');
  7. var Quaternion = require('../math/Quaternion');
  8. var GSSolver = require('../solver/GSSolver');
  9. var Vec3Pool = require('../utils/Vec3Pool');
  10. var ContactEquation = require('../equations/ContactEquation');
  11. var FrictionEquation = require('../equations/FrictionEquation');
  12. var Narrowphase = require('./Narrowphase');
  13. var EventTarget = require('../utils/EventTarget');
  14. var ArrayCollisionMatrix = require('../collision/ArrayCollisionMatrix');
  15. var Material = require('../material/Material');
  16. var ContactMaterial = require('../material/ContactMaterial');
  17. var Body = require('../objects/Body');
  18. var TupleDictionary = require('../utils/TupleDictionary');
  19. var RaycastResult = require('../collision/RaycastResult');
  20. var AABB = require('../collision/AABB');
  21. var Ray = require('../collision/Ray');
  22. var NaiveBroadphase = require('../collision/NaiveBroadphase');
  23.  
  24. /**
  25. * The physics world
  26. * @class World
  27. * @constructor
  28. * @extends EventTarget
  29. */
  30. function World(){
  31. EventTarget.apply(this);
  32.  
  33. /**
  34. * 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.
  35. * @property {Number} dt
  36. */
  37. this.dt = -1;
  38.  
  39. /**
  40. * Makes bodies go to sleep when they've been inactive
  41. * @property allowSleep
  42. * @type {Boolean}
  43. */
  44. this.allowSleep = false;
  45.  
  46. /**
  47. * All the current contacts (instances of ContactEquation) in the world.
  48. * @property contacts
  49. * @type {Array}
  50. */
  51. this.contacts = [];
  52. this.frictionEquations = [];
  53.  
  54. /**
  55. * 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).
  56. * @property quatNormalizeSkip
  57. * @type {Number}
  58. */
  59. this.quatNormalizeSkip = 0;
  60.  
  61. /**
  62. * Set to true to use fast quaternion normalization. It is often enough accurate to use. If bodies tend to explode, set to false.
  63. * @property quatNormalizeFast
  64. * @type {Boolean}
  65. * @see Quaternion.normalizeFast
  66. * @see Quaternion.normalize
  67. */
  68. this.quatNormalizeFast = false;
  69.  
  70. /**
  71. * The wall-clock time since simulation start
  72. * @property time
  73. * @type {Number}
  74. */
  75. this.time = 0.0;
  76.  
  77. /**
  78. * Number of timesteps taken since start
  79. * @property stepnumber
  80. * @type {Number}
  81. */
  82. this.stepnumber = 0;
  83.  
  84. /// Default and last timestep sizes
  85. this.default_dt = 1/60;
  86.  
  87. this.nextId = 0;
  88. /**
  89. * @property gravity
  90. * @type {Vec3}
  91. */
  92. this.gravity = new Vec3();
  93.  
  94. /**
  95. * @property broadphase
  96. * @type {Broadphase}
  97. */
  98. this.broadphase = new NaiveBroadphase();
  99.  
  100. /**
  101. * @property bodies
  102. * @type {Array}
  103. */
  104. this.bodies = [];
  105.  
  106. /**
  107. * @property solver
  108. * @type {Solver}
  109. */
  110. this.solver = new GSSolver();
  111.  
  112. /**
  113. * @property constraints
  114. * @type {Array}
  115. */
  116. this.constraints = [];
  117.  
  118. /**
  119. * @property narrowphase
  120. * @type {Narrowphase}
  121. */
  122. this.narrowphase = new Narrowphase(this);
  123.  
  124. /**
  125. * @property {ArrayCollisionMatrix} collisionMatrix
  126. * @type {ArrayCollisionMatrix}
  127. */
  128. this.collisionMatrix = new ArrayCollisionMatrix();
  129.  
  130. /**
  131. * CollisionMatrix from the previous step.
  132. * @property {ArrayCollisionMatrix} collisionMatrixPrevious
  133. * @type {ArrayCollisionMatrix}
  134. */
  135. this.collisionMatrixPrevious = new ArrayCollisionMatrix();
  136.  
  137. /**
  138. * All added materials
  139. * @property materials
  140. * @type {Array}
  141. */
  142. this.materials = [];
  143.  
  144. /**
  145. * @property contactmaterials
  146. * @type {Array}
  147. */
  148. this.contactmaterials = [];
  149.  
  150. /**
  151. * Used to look up a ContactMaterial given two instances of Material.
  152. * @property {TupleDictionary} contactMaterialTable
  153. */
  154. this.contactMaterialTable = new TupleDictionary();
  155.  
  156. this.defaultMaterial = new Material("default");
  157.  
  158. /**
  159. * This contact material is used if no suitable contactmaterial is found for a contact.
  160. * @property defaultContactMaterial
  161. * @type {ContactMaterial}
  162. */
  163. this.defaultContactMaterial = new ContactMaterial(this.defaultMaterial, this.defaultMaterial, { friction: 0.3, restitution: 0.0 });
  164.  
  165. /**
  166. * @property doProfiling
  167. * @type {Boolean}
  168. */
  169. this.doProfiling = false;
  170.  
  171. /**
  172. * @property profile
  173. * @type {Object}
  174. */
  175. this.profile = {
  176. solve:0,
  177. makeContactConstraints:0,
  178. broadphase:0,
  179. integrate:0,
  180. narrowphase:0,
  181. };
  182.  
  183. /**
  184. * @property subsystems
  185. * @type {Array}
  186. */
  187. this.subsystems = [];
  188.  
  189. this.addBodyEvent = {
  190. type:"addBody",
  191. body : null,
  192. };
  193.  
  194. this.removeBodyEvent = {
  195. type:"removeBody",
  196. body : null,
  197. };
  198. }
  199. World.prototype = new EventTarget();
  200.  
  201. // Temp stuff
  202. var tmpAABB1 = new AABB();
  203. var tmpArray1 = [];
  204. var tmpRay = new Ray();
  205.  
  206. /**
  207. * Get the contact material between materials m1 and m2
  208. * @method getContactMaterial
  209. * @param {Material} m1
  210. * @param {Material} m2
  211. * @return {ContactMaterial} The contact material if it was found.
  212. */
  213. World.prototype.getContactMaterial = function(m1,m2){
  214. return this.contactMaterialTable.get(m1.id,m2.id); //this.contactmaterials[this.mats2cmat[i+j*this.materials.length]];
  215. };
  216.  
  217. /**
  218. * Get number of objects in the world.
  219. * @method numObjects
  220. * @return {Number}
  221. * @deprecated
  222. */
  223. World.prototype.numObjects = function(){
  224. return this.bodies.length;
  225. };
  226.  
  227. /**
  228. * Store old collision state info
  229. * @method collisionMatrixTick
  230. */
  231. World.prototype.collisionMatrixTick = function(){
  232. var temp = this.collisionMatrixPrevious;
  233. this.collisionMatrixPrevious = this.collisionMatrix;
  234. this.collisionMatrix = temp;
  235. this.collisionMatrix.reset();
  236. };
  237.  
  238. /**
  239. * Add a rigid body to the simulation.
  240. * @method add
  241. * @param {Body} body
  242. * @todo If the simulation has not yet started, why recrete and copy arrays for each body? Accumulate in dynamic arrays in this case.
  243. * @todo Adding an array of bodies should be possible. This would save some loops too
  244. * @deprecated Use .addBody instead
  245. */
  246. World.prototype.add = World.prototype.addBody = function(body){
  247. if(this.bodies.indexOf(body) !== -1){
  248. return;
  249. }
  250. body.index = this.bodies.length;
  251. this.bodies.push(body);
  252. body.world = this;
  253. body.initPosition.copy(body.position);
  254. body.initVelocity.copy(body.velocity);
  255. body.timeLastSleepy = this.time;
  256. if(body instanceof Body){
  257. body.initAngularVelocity.copy(body.angularVelocity);
  258. body.initQuaternion.copy(body.quaternion);
  259. }
  260. this.collisionMatrix.setNumObjects(this.bodies.length);
  261. this.addBodyEvent.body = body;
  262. this.dispatchEvent(this.addBodyEvent);
  263. };
  264.  
  265. /**
  266. * Add a constraint to the simulation.
  267. * @method addConstraint
  268. * @param {Constraint} c
  269. */
  270. World.prototype.addConstraint = function(c){
  271. this.constraints.push(c);
  272. };
  273.  
  274. /**
  275. * Removes a constraint
  276. * @method removeConstraint
  277. * @param {Constraint} c
  278. */
  279. World.prototype.removeConstraint = function(c){
  280. var idx = this.constraints.indexOf(c);
  281. if(idx!==-1){
  282. this.constraints.splice(idx,1);
  283. }
  284. };
  285.  
  286. /**
  287. * Raycast test
  288. * @method rayTest
  289. * @param {Vec3} from
  290. * @param {Vec3} to
  291. * @param {Function|RaycastResult} result
  292. * @deprecated Use .raycastAll, .raycastClosest or .raycastAny instead.
  293. */
  294. World.prototype.rayTest = function(from, to, result){
  295. if(result instanceof RaycastResult){
  296. // Do raycastclosest
  297. this.raycastClosest(from, to, {
  298. skipBackfaces: true
  299. }, result);
  300. } else {
  301. // Do raycastAll
  302. this.raycastAll(from, to, {
  303. skipBackfaces: true
  304. }, result);
  305. }
  306. };
  307.  
  308. /**
  309. * Ray cast against all bodies. The provided callback will be executed for each hit with a RaycastResult as single argument.
  310. * @method raycastAll
  311. * @param {Vec3} from
  312. * @param {Vec3} to
  313. * @param {Object} options
  314. * @param {number} [options.collisionFilterMask=-1]
  315. * @param {number} [options.collisionFilterGroup=-1]
  316. * @param {boolean} [options.skipBackfaces=false]
  317. * @param {boolean} [options.checkCollisionResponse=true]
  318. * @param {Function} callback
  319. * @return {boolean} True if any body was hit.
  320. */
  321. World.prototype.raycastAll = function(from, to, options, callback){
  322. options.mode = Ray.ALL;
  323. options.from = from;
  324. options.to = to;
  325. options.callback = callback;
  326. return tmpRay.intersectWorld(this, options);
  327. };
  328.  
  329. /**
  330. * Ray cast, and stop at the first result. Note that the order is random - but the method is fast.
  331. * @method raycastAny
  332. * @param {Vec3} from
  333. * @param {Vec3} to
  334. * @param {Object} options
  335. * @param {number} [options.collisionFilterMask=-1]
  336. * @param {number} [options.collisionFilterGroup=-1]
  337. * @param {boolean} [options.skipBackfaces=false]
  338. * @param {boolean} [options.checkCollisionResponse=true]
  339. * @param {RaycastResult} result
  340. * @return {boolean} True if any body was hit.
  341. */
  342. World.prototype.raycastAny = function(from, to, options, result){
  343. options.mode = Ray.ANY;
  344. options.from = from;
  345. options.to = to;
  346. options.result = result;
  347. return tmpRay.intersectWorld(this, options);
  348. };
  349.  
  350. /**
  351. * Ray cast, and return information of the closest hit.
  352. * @method raycastClosest
  353. * @param {Vec3} from
  354. * @param {Vec3} to
  355. * @param {Object} options
  356. * @param {number} [options.collisionFilterMask=-1]
  357. * @param {number} [options.collisionFilterGroup=-1]
  358. * @param {boolean} [options.skipBackfaces=false]
  359. * @param {boolean} [options.checkCollisionResponse=true]
  360. * @param {RaycastResult} result
  361. * @return {boolean} True if any body was hit.
  362. */
  363. World.prototype.raycastClosest = function(from, to, options, result){
  364. options.mode = Ray.CLOSEST;
  365. options.from = from;
  366. options.to = to;
  367. options.result = result;
  368. return tmpRay.intersectWorld(this, options);
  369. };
  370.  
  371. /**
  372. * Remove a rigid body from the simulation.
  373. * @method remove
  374. * @param {Body} body
  375. * @deprecated Use .removeBody instead
  376. */
  377. World.prototype.remove = function(body){
  378. body.world = null;
  379. var n = this.bodies.length-1,
  380. bodies = this.bodies,
  381. idx = bodies.indexOf(body);
  382. if(idx !== -1){
  383. bodies.splice(idx, 1); // Todo: should use a garbage free method
  384.  
  385. // Recompute index
  386. for(var i=0; i!==bodies.length; i++){
  387. bodies[i].index = i;
  388. }
  389.  
  390. this.collisionMatrix.setNumObjects(n);
  391. this.removeBodyEvent.body = body;
  392. this.dispatchEvent(this.removeBodyEvent);
  393. }
  394. };
  395.  
  396. /**
  397. * Remove a rigid body from the simulation.
  398. * @method removeBody
  399. * @param {Body} body
  400. */
  401. World.prototype.removeBody = World.prototype.remove;
  402.  
  403. /**
  404. * Adds a material to the World.
  405. * @method addMaterial
  406. * @param {Material} m
  407. * @todo Necessary?
  408. */
  409. World.prototype.addMaterial = function(m){
  410. this.materials.push(m);
  411. };
  412.  
  413. /**
  414. * Adds a contact material to the World
  415. * @method addContactMaterial
  416. * @param {ContactMaterial} cmat
  417. */
  418. World.prototype.addContactMaterial = function(cmat) {
  419.  
  420. // Add contact material
  421. this.contactmaterials.push(cmat);
  422.  
  423. // Add current contact material to the material table
  424. this.contactMaterialTable.set(cmat.materials[0].id,cmat.materials[1].id,cmat);
  425. };
  426.  
  427. // performance.now()
  428. if(typeof performance === 'undefined'){
  429. performance = {};
  430. }
  431. if(!performance.now){
  432. var nowOffset = Date.now();
  433. if (performance.timing && performance.timing.navigationStart){
  434. nowOffset = performance.timing.navigationStart;
  435. }
  436. performance.now = function(){
  437. return Date.now() - nowOffset;
  438. };
  439. }
  440.  
  441. var step_tmp1 = new Vec3();
  442.  
  443. /**
  444. * Step the physics world forward in time.
  445. *
  446. * 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.
  447. *
  448. * @method step
  449. * @param {Number} dt The fixed time step size to use.
  450. * @param {Number} [timeSinceLastCalled] The time elapsed since the function was last called.
  451. * @param {Number} [maxSubSteps=10] Maximum number of fixed steps to take per function call.
  452. *
  453. * @example
  454. * // fixed timestepping without interpolation
  455. * world.step(1/60);
  456. *
  457. * @see http://bulletphysics.org/mediawiki-1.5.8/index.php/Stepping_The_World
  458. */
  459. World.prototype.step = function(dt, timeSinceLastCalled, maxSubSteps){
  460. maxSubSteps = maxSubSteps || 10;
  461. timeSinceLastCalled = timeSinceLastCalled || 0;
  462.  
  463. if(timeSinceLastCalled === 0){ // Fixed, simple stepping
  464.  
  465. this.internalStep(dt);
  466.  
  467. // Increment time
  468. this.time += dt;
  469.  
  470. } else {
  471.  
  472. // Compute the number of fixed steps we should have taken since the last step
  473. var internalSteps = Math.floor((this.time + timeSinceLastCalled) / dt) - Math.floor(this.time / dt);
  474. internalSteps = Math.min(internalSteps,maxSubSteps);
  475.  
  476. // Do some fixed steps to catch up
  477. var t0 = performance.now();
  478. for(var i=0; i!==internalSteps; i++){
  479. this.internalStep(dt);
  480. if(performance.now() - t0 > dt * 1000){
  481. // We are slower than real-time. Better bail out.
  482. break;
  483. }
  484. }
  485.  
  486. // Increment internal clock
  487. this.time += timeSinceLastCalled;
  488.  
  489. // Compute "Left over" time step
  490. var h = this.time % dt;
  491. var h_div_dt = h / dt;
  492. var interpvelo = step_tmp1;
  493. var bodies = this.bodies;
  494.  
  495. for(var j=0; j !== bodies.length; j++){
  496. var b = bodies[j];
  497. if(b.type !== Body.STATIC && b.sleepState !== Body.SLEEPING){
  498.  
  499. // Interpolate
  500. b.position.vsub(b.previousPosition, interpvelo);
  501. interpvelo.scale(h_div_dt, interpvelo);
  502. b.position.vadd(interpvelo, b.interpolatedPosition);
  503.  
  504. // TODO: interpolate quaternion
  505. // b.interpolatedAngle = b.angle + (b.angle - b.previousAngle) * h_div_dt;
  506.  
  507. } else {
  508.  
  509. // For static bodies, just copy. Who else will do it?
  510. b.interpolatedPosition.copy(b.position);
  511. b.interpolatedQuaternion.copy(b.quaternion);
  512. }
  513. }
  514. }
  515. };
  516.  
  517. /**
  518. * Step the simulation
  519. * @method step
  520. * @param {Number} dt
  521. */
  522. var World_step_postStepEvent = {type:"postStep"}, // Reusable event objects to save memory
  523. World_step_preStepEvent = {type:"preStep"},
  524. World_step_collideEvent = {type:"collide", body:null, contact:null },
  525. World_step_oldContacts = [], // Pools for unused objects
  526. World_step_frictionEquationPool = [],
  527. World_step_p1 = [], // Reusable arrays for collision pairs
  528. World_step_p2 = [],
  529. World_step_gvec = new Vec3(), // Temporary vectors and quats
  530. World_step_vi = new Vec3(),
  531. World_step_vj = new Vec3(),
  532. World_step_wi = new Vec3(),
  533. World_step_wj = new Vec3(),
  534. World_step_t1 = new Vec3(),
  535. World_step_t2 = new Vec3(),
  536. World_step_rixn = new Vec3(),
  537. World_step_rjxn = new Vec3(),
  538. World_step_step_q = new Quaternion(),
  539. World_step_step_w = new Quaternion(),
  540. World_step_step_wq = new Quaternion(),
  541. invI_tau_dt = new Vec3();
  542. World.prototype.internalStep = function(dt){
  543. this.dt = dt;
  544.  
  545. var world = this,
  546. that = this,
  547. contacts = this.contacts,
  548. p1 = World_step_p1,
  549. p2 = World_step_p2,
  550. N = this.numObjects(),
  551. bodies = this.bodies,
  552. solver = this.solver,
  553. gravity = this.gravity,
  554. doProfiling = this.doProfiling,
  555. profile = this.profile,
  556. DYNAMIC = Body.DYNAMIC,
  557. profilingStart,
  558. constraints = this.constraints,
  559. frictionEquationPool = World_step_frictionEquationPool,
  560. gnorm = gravity.norm(),
  561. gx = gravity.x,
  562. gy = gravity.y,
  563. gz = gravity.z,
  564. i=0;
  565.  
  566. if(doProfiling){
  567. profilingStart = performance.now();
  568. }
  569.  
  570. // Add gravity to all objects
  571. for(i=0; i!==N; i++){
  572. var bi = bodies[i];
  573. if(bi.type & DYNAMIC){ // Only for dynamic bodies
  574. var f = bi.force, m = bi.mass;
  575. f.x += m*gx;
  576. f.y += m*gy;
  577. f.z += m*gz;
  578. }
  579. }
  580.  
  581. // Update subsystems
  582. for(var i=0, Nsubsystems=this.subsystems.length; i!==Nsubsystems; i++){
  583. this.subsystems[i].update();
  584. }
  585.  
  586. // Collision detection
  587. if(doProfiling){ profilingStart = performance.now(); }
  588. p1.length = 0; // Clean up pair arrays from last step
  589. p2.length = 0;
  590. this.broadphase.collisionPairs(this,p1,p2);
  591. if(doProfiling){ profile.broadphase = performance.now() - profilingStart; }
  592.  
  593. // Remove constrained pairs with collideConnected == false
  594. var Nconstraints = constraints.length;
  595. for(i=0; i!==Nconstraints; i++){
  596. var c = constraints[i];
  597. if(!c.collideConnected){
  598. for(var j = p1.length-1; j>=0; j-=1){
  599. if( (c.bodyA === p1[j] && c.bodyB === p2[j]) ||
  600. (c.bodyB === p1[j] && c.bodyA === p2[j])){
  601. p1.splice(j, 1);
  602. p2.splice(j, 1);
  603. }
  604. }
  605. }
  606. }
  607.  
  608. this.collisionMatrixTick();
  609.  
  610. // Generate contacts
  611. if(doProfiling){ profilingStart = performance.now(); }
  612. var oldcontacts = World_step_oldContacts;
  613. var NoldContacts = contacts.length;
  614.  
  615. for(i=0; i!==NoldContacts; i++){
  616. oldcontacts.push(contacts[i]);
  617. }
  618. contacts.length = 0;
  619.  
  620. // Transfer FrictionEquation from current list to the pool for reuse
  621. var NoldFrictionEquations = this.frictionEquations.length;
  622. for(i=0; i!==NoldFrictionEquations; i++){
  623. frictionEquationPool.push(this.frictionEquations[i]);
  624. }
  625. this.frictionEquations.length = 0;
  626.  
  627. this.narrowphase.getContacts(
  628. p1,
  629. p2,
  630. this,
  631. contacts,
  632. oldcontacts, // To be reused
  633. this.frictionEquations,
  634. frictionEquationPool
  635. );
  636.  
  637. if(doProfiling){
  638. profile.narrowphase = performance.now() - profilingStart;
  639. }
  640.  
  641. // Loop over all collisions
  642. if(doProfiling){
  643. profilingStart = performance.now();
  644. }
  645.  
  646. // Add all friction eqs
  647. for (var i = 0; i < this.frictionEquations.length; i++) {
  648. solver.addEquation(this.frictionEquations[i]);
  649. }
  650.  
  651. var ncontacts = contacts.length;
  652. for(var k=0; k!==ncontacts; k++){
  653.  
  654. // Current contact
  655. var c = contacts[k];
  656.  
  657. // Get current collision indeces
  658. var bi = c.bi,
  659. bj = c.bj,
  660. si = c.si,
  661. sj = c.sj;
  662.  
  663. // Get collision properties
  664. var cm;
  665. if(bi.material && bj.material){
  666. cm = this.getContactMaterial(bi.material,bj.material) || this.defaultContactMaterial;
  667. } else {
  668. cm = this.defaultContactMaterial;
  669. }
  670.  
  671. // c.enabled = bi.collisionResponse && bj.collisionResponse && si.collisionResponse && sj.collisionResponse;
  672.  
  673. var mu = cm.friction;
  674. // c.restitution = cm.restitution;
  675.  
  676. // If friction or restitution were specified in the material, use them
  677. if(bi.material && bj.material){
  678. if(bi.material.friction >= 0 && bj.material.friction >= 0){
  679. mu = bi.material.friction * bj.material.friction;
  680. }
  681.  
  682. if(bi.material.restitution >= 0 && bj.material.restitution >= 0){
  683. c.restitution = bi.material.restitution * bj.material.restitution;
  684. }
  685. }
  686.  
  687. // c.setSpookParams(
  688. // cm.contactEquationStiffness,
  689. // cm.contactEquationRelaxation,
  690. // dt
  691. // );
  692.  
  693. solver.addEquation(c);
  694.  
  695. // // Add friction constraint equation
  696. // if(mu > 0){
  697.  
  698. // // Create 2 tangent equations
  699. // var mug = mu * gnorm;
  700. // var reducedMass = (bi.invMass + bj.invMass);
  701. // if(reducedMass > 0){
  702. // reducedMass = 1/reducedMass;
  703. // }
  704. // var pool = frictionEquationPool;
  705. // var c1 = pool.length ? pool.pop() : new FrictionEquation(bi,bj,mug*reducedMass);
  706. // var c2 = pool.length ? pool.pop() : new FrictionEquation(bi,bj,mug*reducedMass);
  707. // this.frictionEquations.push(c1, c2);
  708.  
  709. // c1.bi = c2.bi = bi;
  710. // c1.bj = c2.bj = bj;
  711. // c1.minForce = c2.minForce = -mug*reducedMass;
  712. // c1.maxForce = c2.maxForce = mug*reducedMass;
  713.  
  714. // // Copy over the relative vectors
  715. // c1.ri.copy(c.ri);
  716. // c1.rj.copy(c.rj);
  717. // c2.ri.copy(c.ri);
  718. // c2.rj.copy(c.rj);
  719.  
  720. // // Construct tangents
  721. // c.ni.tangents(c1.t, c2.t);
  722.  
  723. // // Set spook params
  724. // c1.setSpookParams(cm.frictionEquationStiffness, cm.frictionEquationRelaxation, dt);
  725. // c2.setSpookParams(cm.frictionEquationStiffness, cm.frictionEquationRelaxation, dt);
  726.  
  727. // c1.enabled = c2.enabled = c.enabled;
  728.  
  729. // // Add equations to solver
  730. // solver.addEquation(c1);
  731. // solver.addEquation(c2);
  732. // }
  733.  
  734. if( bi.allowSleep &&
  735. bi.type === Body.DYNAMIC &&
  736. bi.sleepState === Body.SLEEPING &&
  737. bj.sleepState === Body.AWAKE &&
  738. bj.type !== Body.STATIC
  739. ){
  740. var speedSquaredB = bj.velocity.norm2() + bj.angularVelocity.norm2();
  741. var speedLimitSquaredB = Math.pow(bj.sleepSpeedLimit,2);
  742. if(speedSquaredB >= speedLimitSquaredB*2){
  743. bi._wakeUpAfterNarrowphase = true;
  744. }
  745. }
  746.  
  747. if( bj.allowSleep &&
  748. bj.type === Body.DYNAMIC &&
  749. bj.sleepState === Body.SLEEPING &&
  750. bi.sleepState === Body.AWAKE &&
  751. bi.type !== Body.STATIC
  752. ){
  753. var speedSquaredA = bi.velocity.norm2() + bi.angularVelocity.norm2();
  754. var speedLimitSquaredA = Math.pow(bi.sleepSpeedLimit,2);
  755. if(speedSquaredA >= speedLimitSquaredA*2){
  756. bj._wakeUpAfterNarrowphase = true;
  757. }
  758. }
  759.  
  760. // Now we know that i and j are in contact. Set collision matrix state
  761. this.collisionMatrix.set(bi, bj, true);
  762.  
  763. if (!this.collisionMatrixPrevious.get(bi, bj)) {
  764. // First contact!
  765. // 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.
  766. World_step_collideEvent.body = bj;
  767. World_step_collideEvent.contact = c;
  768. bi.dispatchEvent(World_step_collideEvent);
  769.  
  770. World_step_collideEvent.body = bi;
  771. bj.dispatchEvent(World_step_collideEvent);
  772. }
  773. }
  774. if(doProfiling){
  775. profile.makeContactConstraints = performance.now() - profilingStart;
  776. profilingStart = performance.now();
  777. }
  778.  
  779. // Wake up bodies
  780. for(i=0; i!==N; i++){
  781. var bi = bodies[i];
  782. if(bi._wakeUpAfterNarrowphase){
  783. bi.wakeUp();
  784. bi._wakeUpAfterNarrowphase = false;
  785. }
  786. }
  787.  
  788. // Add user-added constraints
  789. var Nconstraints = constraints.length;
  790. for(i=0; i!==Nconstraints; i++){
  791. var c = constraints[i];
  792. c.update();
  793. for(var j=0, Neq=c.equations.length; j!==Neq; j++){
  794. var eq = c.equations[j];
  795. solver.addEquation(eq);
  796. }
  797. }
  798.  
  799. // Solve the constrained system
  800. solver.solve(dt,this);
  801.  
  802. if(doProfiling){
  803. profile.solve = performance.now() - profilingStart;
  804. }
  805.  
  806. // Remove all contacts from solver
  807. solver.removeAllEquations();
  808.  
  809. // Apply damping, see http://code.google.com/p/bullet/issues/detail?id=74 for details
  810. var pow = Math.pow;
  811. for(i=0; i!==N; i++){
  812. var bi = bodies[i];
  813. if(bi.type & DYNAMIC){ // Only for dynamic bodies
  814. var ld = pow(1.0 - bi.linearDamping,dt);
  815. var v = bi.velocity;
  816. v.mult(ld,v);
  817. var av = bi.angularVelocity;
  818. if(av){
  819. var ad = pow(1.0 - bi.angularDamping,dt);
  820. av.mult(ad,av);
  821. }
  822. }
  823. }
  824.  
  825. this.dispatchEvent(World_step_preStepEvent);
  826.  
  827. // Invoke pre-step callbacks
  828. for(i=0; i!==N; i++){
  829. var bi = bodies[i];
  830. if(bi.preStep){
  831. bi.preStep.call(bi);
  832. }
  833. }
  834.  
  835. // Leap frog
  836. // vnew = v + h*f/m
  837. // xnew = x + h*vnew
  838. if(doProfiling){
  839. profilingStart = performance.now();
  840. }
  841. var q = World_step_step_q;
  842. var w = World_step_step_w;
  843. var wq = World_step_step_wq;
  844. var stepnumber = this.stepnumber;
  845. var DYNAMIC_OR_KINEMATIC = Body.DYNAMIC | Body.KINEMATIC;
  846. var quatNormalize = stepnumber % (this.quatNormalizeSkip+1) === 0;
  847. var quatNormalizeFast = this.quatNormalizeFast;
  848. var half_dt = dt * 0.5;
  849. var PLANE = Shape.types.PLANE,
  850. CONVEX = Shape.types.CONVEXPOLYHEDRON;
  851.  
  852. for(i=0; i!==N; i++){
  853. var b = bodies[i],
  854. force = b.force,
  855. tau = b.torque;
  856. if((b.type & DYNAMIC_OR_KINEMATIC) && b.sleepState !== Body.SLEEPING){ // Only for dynamic
  857. var velo = b.velocity,
  858. angularVelo = b.angularVelocity,
  859. pos = b.position,
  860. quat = b.quaternion,
  861. invMass = b.invMass,
  862. invInertia = b.invInertiaWorld;
  863.  
  864. velo.x += force.x * invMass * dt;
  865. velo.y += force.y * invMass * dt;
  866. velo.z += force.z * invMass * dt;
  867.  
  868. if(b.angularVelocity){
  869. invInertia.vmult(tau,invI_tau_dt);
  870. invI_tau_dt.mult(dt,invI_tau_dt);
  871. invI_tau_dt.vadd(angularVelo,angularVelo);
  872. }
  873.  
  874. // Use new velocity - leap frog
  875. pos.x += velo.x * dt;
  876. pos.y += velo.y * dt;
  877. pos.z += velo.z * dt;
  878.  
  879. if(b.angularVelocity){
  880. w.set(angularVelo.x, angularVelo.y, angularVelo.z, 0);
  881. w.mult(quat,wq);
  882. quat.x += half_dt * wq.x;
  883. quat.y += half_dt * wq.y;
  884. quat.z += half_dt * wq.z;
  885. quat.w += half_dt * wq.w;
  886. if(quatNormalize){
  887. if(quatNormalizeFast){
  888. quat.normalizeFast();
  889. } else {
  890. quat.normalize();
  891. }
  892. }
  893. }
  894.  
  895. if(b.aabb){
  896. b.aabbNeedsUpdate = true;
  897. }
  898.  
  899. // Update world inertia
  900. if(b.updateInertiaWorld){
  901. b.updateInertiaWorld();
  902. }
  903. }
  904. }
  905. this.clearForces();
  906.  
  907. this.broadphase.dirty = true;
  908.  
  909. if(doProfiling){
  910. profile.integrate = performance.now() - profilingStart;
  911. }
  912.  
  913. // Update world time
  914. this.time += dt;
  915. this.stepnumber += 1;
  916.  
  917. this.dispatchEvent(World_step_postStepEvent);
  918.  
  919. // Invoke post-step callbacks
  920. for(i=0; i!==N; i++){
  921. var bi = bodies[i];
  922. var postStep = bi.postStep;
  923. if(postStep){
  924. postStep.call(bi);
  925. }
  926. }
  927.  
  928. // Sleeping update
  929. if(this.allowSleep){
  930. for(i=0; i!==N; i++){
  931. bodies[i].sleepTick(this.time);
  932. }
  933. }
  934. };
  935.  
  936. /**
  937. * Sets all body forces in the world to zero.
  938. * @method clearForces
  939. */
  940. World.prototype.clearForces = function(){
  941. var bodies = this.bodies;
  942. var N = bodies.length;
  943. for(var i=0; i !== N; i++){
  944. var b = bodies[i],
  945. force = b.force,
  946. tau = b.torque;
  947.  
  948. b.force.set(0,0,0);
  949. b.torque.set(0,0,0);
  950. }
  951. };
  952.