mirror of
https://github.com/wassname/phaser.git
synced 2026-07-11 00:40:20 +08:00
Richard Davey
798 lines
18 KiB
JavaScript
798 lines
18 KiB
JavaScript
/*
|
|
* Copyright (c) 2012 Ju Hyung Lee
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
|
|
* and associated documentation files (the "Software"), to deal in the Software without
|
|
* restriction, including without limitation the rights to use, copy, modify, merge, publish,
|
|
* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
|
|
* Software is furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all copies or
|
|
* substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
|
|
* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
|
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
|
|
* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
*/
|
|
|
|
function Space() {
|
|
this.bodyArr = [];
|
|
this.bodyHash = {};
|
|
|
|
this.jointArr = [];
|
|
this.jointHash = {};
|
|
|
|
this.numContacts = 0;
|
|
this.contactSolverArr = [];
|
|
|
|
this.postSolve = function(arb) {};
|
|
|
|
this.gravity = new vec2(0, 0);
|
|
this.damping = 0;
|
|
|
|
this.log = [];
|
|
}
|
|
|
|
Space.TIME_TO_SLEEP = 0.5;
|
|
Space.SLEEP_LINEAR_TOLERANCE = 0.5;
|
|
Space.SLEEP_ANGULAR_TOLERANCE = deg2rad(2);
|
|
|
|
Space.prototype.clear = function() {
|
|
Shape.id_counter = 0;
|
|
Body.id_counter = 0;
|
|
Joint.id_counter = 0;
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
if (this.bodyArr[i]) {
|
|
this.removeBody(this.bodyArr[i]);
|
|
}
|
|
}
|
|
|
|
this.bodyArr = [];
|
|
this.bodyHash = {};
|
|
|
|
this.jointArr = [];
|
|
this.jointHash = {};
|
|
|
|
this.contactSolverArr = [];
|
|
|
|
this.stepCount = 0;
|
|
}
|
|
|
|
Space.prototype.toJSON = function(key) {
|
|
var o_bodies = [];
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
if (this.bodyArr[i]) {
|
|
o_bodies.push(this.bodyArr[i].serialize());
|
|
}
|
|
}
|
|
|
|
var o_joints = [];
|
|
for (var i = 0; i < this.jointArr.length; i++) {
|
|
if (this.jointArr[i]) {
|
|
o_joints.push(this.jointHash[i].serialize());
|
|
}
|
|
}
|
|
|
|
return {
|
|
bodies: o_bodies,
|
|
joints: o_joints
|
|
};
|
|
}
|
|
|
|
Space.prototype.create = function(text) {
|
|
var config = JSON.parse(text);
|
|
|
|
this.clear();
|
|
|
|
for (var i = 0; i < config.bodies.length; i++) {
|
|
var config_body = config.bodies[i];
|
|
var type = {"static": Body.Static, "kinetic": Body.KINETIC, "dynamic": Body.DYNAMIC}[config_body.type];
|
|
var body = new Body(type, config_body.position.x, config_body.position.y, config_body.angle);
|
|
|
|
for (var j = 0; j < config_body.shapes.length; j++) {
|
|
var config_shape = config_body.shapes[j];
|
|
var shape;
|
|
|
|
switch (config_shape.type) {
|
|
case "ShapeCircle":
|
|
shape = new ShapeCircle(config_shape.center.x, config_shape.center.y, config_shape.radius);
|
|
break;
|
|
case "ShapeSegment":
|
|
shape = new ShapeSegment(config_shape.a, config_shape.b, config_shape.radius);
|
|
break;
|
|
case "ShapePoly":
|
|
shape = new ShapePoly(config_shape.verts);
|
|
break;
|
|
}
|
|
|
|
shape.e = config_shape.e;
|
|
shape.u = config_shape.u;
|
|
shape.density = config_shape.density;
|
|
|
|
body.addShape(shape);
|
|
}
|
|
|
|
body.resetMassData();
|
|
this.addBody(body);
|
|
}
|
|
|
|
for (var i = 0; i < config.joints.length; i++) {
|
|
var config_joint = config.joints[i];
|
|
var body1 = this.bodyArr[this.bodyHash[config_joint.body1]];
|
|
var body2 = this.bodyArr[this.bodyHash[config_joint.body2]];
|
|
var joint;
|
|
|
|
switch (config_joint.type) {
|
|
case "AngleJoint":
|
|
joint = new AngleJoint(body1, body2);
|
|
break;
|
|
case "RevoluteJoint":
|
|
joint = new RevoluteJoint(body1, body2, config_joint.anchor);
|
|
joint.enableLimit(config_joint.limitEnabled);
|
|
joint.setLimits(config_joint.limitLowerAngle, config_joint.limitUpperAngle);
|
|
joint.enableMotor(config_joint.motorEnabled);
|
|
joint.setMotorSpeed(config_joint.motorSpeed);
|
|
joint.setMaxMotorTorque(config_joint.maxMotorTorque);
|
|
break;
|
|
case "WeldJoint":
|
|
joint = new WeldJoint(body1, body2, config_joint.anchor);
|
|
joint.setSpringFrequencyHz(config_joint.frequencyHz);
|
|
joint.setSpringDampingRatio(config_joint.dampingRatio);
|
|
break;
|
|
case "WheelJoint":
|
|
joint = new WheelJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
|
|
joint.enableMotor(config_joint.motorEnabled);
|
|
joint.setMotorSpeed(config_joint.motorSpeed);
|
|
joint.setMaxMotorTorque(config_joint.maxMotorTorque);
|
|
break;
|
|
case "PrismaticJoint":
|
|
joint = new PrismaticJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
|
|
break;
|
|
case "DistanceJoint":
|
|
joint = new DistanceJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
|
|
joint.setSpringFrequencyHz(config_joint.frequencyHz);
|
|
joint.setSpringDampingRatio(config_joint.dampingRatio);
|
|
break;
|
|
case "RopeJoint":
|
|
joint = new RopeJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
|
|
break;
|
|
}
|
|
|
|
joint.collideConnected = config_joint.collideConnected;
|
|
joint.maxForce = config_joint.maxForce;
|
|
joint.breakable = config_joint.breakable;
|
|
|
|
this.addJoint(joint);
|
|
}
|
|
}
|
|
|
|
Space.prototype.addBody = function(body) {
|
|
if (this.bodyHash[body.id] != undefined) {
|
|
return;
|
|
}
|
|
|
|
var index = this.bodyArr.push(body) - 1;
|
|
this.bodyHash[body.id] = index;
|
|
|
|
body.awake(true);
|
|
body.space = this;
|
|
body.cacheData();
|
|
}
|
|
|
|
Space.prototype.removeBody = function(body) {
|
|
if (this.bodyHash[body.id] == undefined) {
|
|
return;
|
|
}
|
|
|
|
// Remove linked joint
|
|
for (var i = 0; i < body.jointArr.length; i++) {
|
|
if (body.jointArr[i]) {
|
|
this.removeJoint(body.jointArr[i]);
|
|
}
|
|
}
|
|
|
|
body.space = null;
|
|
|
|
var index = this.bodyHash[body.id];
|
|
delete this.bodyHash[body.id];
|
|
delete this.bodyArr[index];
|
|
}
|
|
|
|
Space.prototype.addJoint = function(joint) {
|
|
if (this.jointHash[joint.id] != undefined) {
|
|
return;
|
|
}
|
|
|
|
joint.body1.awake(true);
|
|
joint.body2.awake(true);
|
|
|
|
var index = this.jointArr.push(joint) - 1;
|
|
this.jointHash[joint.id] = index;
|
|
|
|
var index = joint.body1.jointArr.push(joint) - 1;
|
|
joint.body1.jointHash[joint.id] = index;
|
|
|
|
var index = joint.body2.jointArr.push(joint) - 1;
|
|
joint.body2.jointHash[joint.id] = index;
|
|
}
|
|
|
|
Space.prototype.removeJoint = function(joint) {
|
|
if (this.jointHash[joint.id] == undefined) {
|
|
return;
|
|
}
|
|
|
|
joint.body1.awake(true);
|
|
joint.body2.awake(true);
|
|
|
|
var index = joint.body1.jointHash[joint.id];
|
|
delete joint.body1.jointHash[joint.id];
|
|
delete joint.body1.jointArr[index];
|
|
|
|
var index = joint.body2.jointHash[joint.id];
|
|
delete joint.body2.jointHash[joint.id];
|
|
delete joint.body2.jointArr[index];
|
|
|
|
var index = this.jointHash[joint.id];
|
|
delete this.jointHash[joint.id];
|
|
delete this.jointArr[index];
|
|
}
|
|
|
|
Space.prototype.findShapeByPoint = function(p, refShape) {
|
|
var firstShape;
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapeArr.length; j++) {
|
|
var shape = body.shapeArr[j];
|
|
|
|
if (shape.pointQuery(p)) {
|
|
if (!refShape) {
|
|
return shape;
|
|
}
|
|
|
|
if (!firstShape) {
|
|
firstShape = shape;
|
|
}
|
|
|
|
if (shape == refShape) {
|
|
refShape = null;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return firstShape;
|
|
}
|
|
|
|
Space.prototype.findBodyByPoint = function(p, refBody) {
|
|
var firstBody;
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapeArr.length; j++) {
|
|
var shape = body.shapeArr[j];
|
|
|
|
if (shape.pointQuery(p)) {
|
|
if (!refBody) {
|
|
return shape.body;
|
|
}
|
|
|
|
if (!firstBody) {
|
|
firstBody = shape.body;
|
|
}
|
|
|
|
if (shape.body == refBody) {
|
|
refBody = null;
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
return firstBody;
|
|
}
|
|
|
|
// TODO: Replace this function to shape hashing
|
|
Space.prototype.shapeById = function(id) {
|
|
var shape;
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapeArr.length; j++) {
|
|
if (body.shapeArr[j].id == id) {
|
|
return body.shapeArr[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
Space.prototype.jointById = function(id) {
|
|
var index = this.jointHash[id];
|
|
if (index != undefined) {
|
|
return this.jointArr[index];
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
Space.prototype.findVertexByPoint = function(p, minDist, refVertexId) {
|
|
var firstVertexId = -1;
|
|
|
|
refVertexId = refVertexId || -1;
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapeArr.length; j++) {
|
|
var shape = body.shapeArr[j];
|
|
var index = shape.findVertexByPoint(p, minDist);
|
|
if (index != -1) {
|
|
var vertex = (shape.id << 16) | index;
|
|
if (refVertexId == -1) {
|
|
return vertex;
|
|
}
|
|
|
|
if (firstVertexId == -1) {
|
|
firstVertexId = vertex;
|
|
}
|
|
|
|
if (vertex == refVertexId) {
|
|
refVertexId = -1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return firstVertexId;
|
|
}
|
|
|
|
Space.prototype.findEdgeByPoint = function(p, minDist, refEdgeId) {
|
|
var firstEdgeId = -1;
|
|
|
|
refEdgeId = refEdgeId || -1;
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapeArr.length; j++) {
|
|
var shape = body.shapeArr[j];
|
|
if (shape.type != Shape.TYPE_POLY) {
|
|
continue;
|
|
}
|
|
|
|
var index = shape.findEdgeByPoint(p, minDist);
|
|
if (index != -1) {
|
|
var edge = (shape.id << 16) | index;
|
|
if (refEdgeId == -1) {
|
|
return edge;
|
|
}
|
|
|
|
if (firstEdgeId == -1) {
|
|
firstEdgeId = edge;
|
|
}
|
|
|
|
if (edge == refEdgeId) {
|
|
refEdgeId = -1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return firstEdgeId;
|
|
}
|
|
|
|
Space.prototype.findJointByPoint = function(p, minDist, refJointId) {
|
|
var firstJointId = -1;
|
|
|
|
var dsq = minDist * minDist;
|
|
|
|
refJointId = refJointId || -1;
|
|
|
|
for (var i = 0; i < this.jointArr.length; i++) {
|
|
var joint = this.jointArr[i];
|
|
if (!joint) {
|
|
continue;
|
|
}
|
|
|
|
var jointId = -1;
|
|
|
|
if (vec2.distsq(p, joint.getWorldAnchor1()) < dsq) {
|
|
jointId = (joint.id << 16 | 0);
|
|
}
|
|
else if (vec2.distsq(p, joint.getWorldAnchor2()) < dsq) {
|
|
jointId = (joint.id << 16 | 1);
|
|
}
|
|
|
|
if (jointId != -1) {
|
|
if (refJointId == -1) {
|
|
return jointId;
|
|
}
|
|
|
|
if (firstJointId == -1) {
|
|
firstJointId = jointId;
|
|
}
|
|
|
|
if (jointId == refJointId) {
|
|
refJointId = -1;
|
|
}
|
|
}
|
|
}
|
|
|
|
return firstJointId;
|
|
}
|
|
|
|
Space.prototype.findContactSolver = function(shape1, shape2) {
|
|
|
|
for (var i = 0; i < this.contactSolverArr.length; i++) {
|
|
var contactSolver = this.contactSolverArr[i];
|
|
if (shape1 == contactSolver.shape1 && shape2 == contactSolver.shape2) {
|
|
return contactSolver;
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
Space.dump = function (phase, body) {
|
|
|
|
var s = "\n\nPhase: " + phase + "\n";
|
|
s += "Position: " + body.p.toString() + "\n";
|
|
s += "Velocity: " + body.v.toString() + "\n";
|
|
s += "Angle: " + body.a + "\n";
|
|
s += "Force: " + body.f.toString() + "\n";
|
|
s += "Torque: " + body.t + "\n";
|
|
s += "Bounds: " + body.bounds.toString() + "\n";
|
|
s += "Shape ***\n";
|
|
s += "Vert 0: " + body.shapeArr[0].verts[0].toString() + "\n";
|
|
s += "Vert 1: " + body.shapeArr[0].verts[1].toString() + "\n";
|
|
s += "Vert 2: " + body.shapeArr[0].verts[2].toString() + "\n";
|
|
s += "Vert 3: " + body.shapeArr[0].verts[3].toString() + "\n";
|
|
s += "TVert 0: " + body.shapeArr[0].tverts[0].toString() + "\n";
|
|
s += "TVert 1: " + body.shapeArr[0].tverts[1].toString() + "\n";
|
|
s += "TVert 2: " + body.shapeArr[0].tverts[2].toString() + "\n";
|
|
s += "TVert 3: " + body.shapeArr[0].tverts[3].toString() + "\n";
|
|
s += "Plane 0: " + body.shapeArr[0].planes[0].n.toString() + "\n";
|
|
s += "Plane 1: " + body.shapeArr[0].planes[1].n.toString() + "\n";
|
|
s += "Plane 2: " + body.shapeArr[0].planes[2].n.toString() + "\n";
|
|
s += "Plane 3: " + body.shapeArr[0].planes[3].n.toString() + "\n";
|
|
s += "TPlane 0: " + body.shapeArr[0].tplanes[0].n.toString() + "\n";
|
|
s += "TPlane 1: " + body.shapeArr[0].tplanes[1].n.toString() + "\n";
|
|
s += "TPlane 2: " + body.shapeArr[0].tplanes[2].n.toString() + "\n";
|
|
s += "TPlane 3: " + body.shapeArr[0].tplanes[3].n.toString() + "\n";
|
|
|
|
this.log.push(s);
|
|
|
|
}
|
|
|
|
Space.prototype.genTemporalContactSolvers = function() {
|
|
|
|
var t0 = Date.now();
|
|
var newContactSolverArr = [];
|
|
|
|
this.numContacts = 0;
|
|
|
|
for (var body1_index = 0; body1_index < this.bodyArr.length; body1_index++) {
|
|
var body1 = this.bodyArr[body1_index];
|
|
if (!body1) {
|
|
continue;
|
|
}
|
|
|
|
body1.stepCount = this.stepCount;
|
|
|
|
for (var body2_index = 0; body2_index < this.bodyArr.length; body2_index++) {
|
|
var body2 = this.bodyArr[body2_index];
|
|
if (!body2) {
|
|
continue;
|
|
}
|
|
|
|
if (body1.stepCount == body2.stepCount) {
|
|
continue;
|
|
}
|
|
|
|
var active1 = body1.isAwake() && !body1.isStatic();
|
|
var active2 = body2.isAwake() && !body2.isStatic();
|
|
|
|
if (!active1 && !active2) {
|
|
continue;
|
|
}
|
|
|
|
if (!body1.isCollidable(body2)) {
|
|
continue;
|
|
}
|
|
|
|
if (!body1.bounds.intersectsBounds(body2.bounds)) {
|
|
continue;
|
|
}
|
|
|
|
for (var i = 0; i < body1.shapeArr.length; i++) {
|
|
for (var j = 0; j < body2.shapeArr.length; j++) {
|
|
var shape1 = body1.shapeArr[i];
|
|
var shape2 = body2.shapeArr[j];
|
|
|
|
var contactArr = [];
|
|
if (!collision.collide(shape1, shape2, contactArr)) {
|
|
continue;
|
|
}
|
|
|
|
if (shape1.type > shape2.type) {
|
|
var temp = shape1;
|
|
shape1 = shape2;
|
|
shape2 = temp;
|
|
}
|
|
|
|
this.numContacts += contactArr.length;
|
|
|
|
var contactSolver = this.findContactSolver(shape1, shape2);
|
|
|
|
if (contactSolver) {
|
|
contactSolver.update(contactArr);
|
|
newContactSolverArr.push(contactSolver);
|
|
}
|
|
else {
|
|
body1.awake(true);
|
|
body2.awake(true);
|
|
|
|
var newContactSolver = new ContactSolver(shape1, shape2);
|
|
newContactSolver.contactArr = contactArr;
|
|
newContactSolver.e = Math.max(shape1.e, shape2.e);
|
|
newContactSolver.u = Math.sqrt(shape1.u * shape2.u);
|
|
newContactSolverArr.push(newContactSolver);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
stats.timeCollision = Date.now() - t0;
|
|
|
|
return newContactSolverArr;
|
|
}
|
|
|
|
Space.prototype.initSolver = function(dt, dt_inv, warmStarting) {
|
|
|
|
var t0 = Date.now();
|
|
|
|
// Initialize contact solvers
|
|
for (var i = 0; i < this.contactSolverArr.length; i++) {
|
|
this.contactSolverArr[i].initSolver(dt_inv);
|
|
}
|
|
|
|
// Initialize joint solver
|
|
for (var i = 0; i < this.jointArr.length; i++) {
|
|
if (this.jointArr[i]) {
|
|
this.jointArr[i].initSolver(dt, warmStarting);
|
|
}
|
|
}
|
|
|
|
// Warm starting (apply cached impulse)
|
|
if (warmStarting) {
|
|
for (var i = 0; i < this.contactSolverArr.length; i++) {
|
|
this.contactSolverArr[i].warmStart();
|
|
}
|
|
}
|
|
|
|
stats.timeInitSolver = Date.now() - t0;
|
|
}
|
|
|
|
Space.prototype.velocitySolver = function(iteration) {
|
|
|
|
var t0 = Date.now();
|
|
|
|
for (var i = 0; i < iteration; i++) {
|
|
for (var j = 0; j < this.jointArr.length; j++) {
|
|
if (this.jointArr[j]) {
|
|
this.jointArr[j].solveVelocityConstraints();
|
|
}
|
|
}
|
|
|
|
for (var j = 0; j < this.contactSolverArr.length; j++) {
|
|
this.contactSolverArr[j].solveVelocityConstraints();
|
|
}
|
|
}
|
|
|
|
stats.timeVelocitySolver = Date.now() - t0;
|
|
}
|
|
|
|
Space.prototype.positionSolver = function(iteration) {
|
|
var t0 = Date.now();
|
|
|
|
var positionSolved = false;
|
|
|
|
stats.positionIterations = 0;
|
|
|
|
for (var i = 0; i < iteration; i++) {
|
|
var contactsOk = true;
|
|
var jointsOk = true;
|
|
|
|
for (var j = 0; j < this.contactSolverArr.length; j++) {
|
|
var contactOk = this.contactSolverArr[j].solvePositionConstraints();
|
|
contactsOk = contactOk && contactsOk;
|
|
}
|
|
|
|
for (var j = 0; j < this.jointArr.length; j++) {
|
|
if (this.jointArr[j]) {
|
|
var jointOk = this.jointArr[j].solvePositionConstraints();
|
|
jointsOk = jointOk && jointsOk;
|
|
}
|
|
}
|
|
|
|
if (contactsOk && jointsOk) {
|
|
// exit early if the position errors are small
|
|
positionSolved = true;
|
|
break;
|
|
}
|
|
|
|
stats.positionIterations++;
|
|
}
|
|
|
|
stats.timePositionSolver = Date.now() - t0;
|
|
|
|
return positionSolved;
|
|
}
|
|
|
|
Space.prototype.step = function(dt, vel_iteration, pos_iteration, warmStarting, allowSleep) {
|
|
|
|
var dt_inv = 1 / dt;
|
|
|
|
this.stepCount++;
|
|
|
|
// Generate contact & contactSolver
|
|
this.contactSolverArr = this.genTemporalContactSolvers();
|
|
|
|
// Initialize contacts & joints solver
|
|
this.initSolver(dt, dt_inv, warmStarting);
|
|
|
|
// Intergrate velocity
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
if (body.isDynamic() && body.isAwake()) {
|
|
body.updateVelocity(this.gravity, dt, this.damping);
|
|
}
|
|
}
|
|
|
|
for (var i = 0; i < this.jointArr.length; i++) {
|
|
var joint = this.jointArr[i];
|
|
if (!joint) {
|
|
continue;
|
|
}
|
|
|
|
var body1 = joint.body1;
|
|
var body2 = joint.body2;
|
|
|
|
var awake1 = body1.isAwake() && !body1.isStatic();
|
|
var awake2 = body2.isAwake() && !body2.isStatic();
|
|
|
|
if (awake1 ^ awake2) {
|
|
if (!awake1)
|
|
body1.awake(true);
|
|
if (!awake2)
|
|
body2.awake(true);
|
|
}
|
|
}
|
|
|
|
// Iterative velocity constraints solver
|
|
this.velocitySolver(vel_iteration);
|
|
|
|
// Intergrate position
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue
|
|
}
|
|
|
|
if (body.isDynamic() && body.isAwake()) {
|
|
body.updatePosition(dt);
|
|
}
|
|
}
|
|
|
|
// Process breakable joint
|
|
for (var i = 0; i < this.jointArr.length; i++) {
|
|
var joint = this.jointArr[i];
|
|
if (!joint) {
|
|
continue;
|
|
}
|
|
|
|
if (joint.breakable) {
|
|
if (joint.getReactionForce(dt_inv).lengthsq() >= joint.maxForce * joint.maxForce)
|
|
this.removeJoint(joint);
|
|
}
|
|
}
|
|
|
|
// Iterative position constraints solver
|
|
var positionSolved = this.positionSolver(pos_iteration);
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
body.syncTransform();
|
|
}
|
|
|
|
// Post solve collision callback
|
|
for (var i = 0; i < this.contactSolverArr.length; i++) {
|
|
var arb = this.contactSolverArr[i];
|
|
//this.postSolve(arb);
|
|
}
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
if (body.isDynamic() && body.isAwake()) {
|
|
body.cacheData();
|
|
}
|
|
}
|
|
|
|
// Process sleeping
|
|
if (allowSleep) {
|
|
var minSleepTime = 999999;
|
|
|
|
var linTolSqr = Space.SLEEP_LINEAR_TOLERANCE * Space.SLEEP_LINEAR_TOLERANCE;
|
|
var angTolSqr = Space.SLEEP_ANGULAR_TOLERANCE * Space.SLEEP_ANGULAR_TOLERANCE;
|
|
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
if (!body.isDynamic()) {
|
|
continue;
|
|
}
|
|
|
|
if (body.w * body.w > angTolSqr || body.v.dot(body.v) > linTolSqr) {
|
|
body.sleepTime = 0;
|
|
minSleepTime = 0;
|
|
}
|
|
else {
|
|
body.sleepTime += dt;
|
|
minSleepTime = Math.min(minSleepTime, body.sleepTime);
|
|
}
|
|
}
|
|
|
|
if (positionSolved && minSleepTime >= Space.TIME_TO_SLEEP) {
|
|
for (var i = 0; i < this.bodyArr.length; i++) {
|
|
var body = this.bodyArr[i];
|
|
if (!body) {
|
|
continue;
|
|
}
|
|
|
|
body.awake(false);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|