mirror of
https://github.com/photonstorm/phaser
synced 2024-11-14 17:07:43 +00:00
823 lines
No EOL
23 KiB
TypeScript
823 lines
No EOL
23 KiB
TypeScript
/// <reference path="../math/Vec2.ts" />
|
|
/// <reference path="../math/Vec2Utils.ts" />
|
|
/// <reference path="AdvancedPhysics.ts" />
|
|
/// <reference path="Body.ts" />
|
|
/// <reference path="shapes/Shape.ts" />
|
|
/// <reference path="ContactSolver.ts" />
|
|
/// <reference path="Contact.ts" />
|
|
/// <reference path="Collision.ts" />
|
|
/// <reference path="joints/IJoint.ts" />
|
|
|
|
/**
|
|
* Phaser - Advanced Physics - Space
|
|
*
|
|
* Based on the work Ju Hyung Lee started in JS PhyRus.
|
|
*/
|
|
|
|
module Phaser.Physics {
|
|
|
|
export class Space {
|
|
|
|
constructor(manager: Phaser.Physics.AdvancedPhysics) {
|
|
|
|
this._manager = manager;
|
|
|
|
this.bodies = [];
|
|
this.bodyHash = {};
|
|
|
|
this.joints = [];
|
|
this.jointHash = {};
|
|
|
|
this.numContacts = 0;
|
|
this.contactSolvers = [];
|
|
|
|
this.gravity = this._manager.gravity;
|
|
this.damping = 0;
|
|
|
|
this._linTolSqr = Space.SLEEP_LINEAR_TOLERANCE * Space.SLEEP_LINEAR_TOLERANCE;
|
|
this._angTolSqr = Space.SLEEP_ANGULAR_TOLERANCE * Space.SLEEP_ANGULAR_TOLERANCE;
|
|
|
|
}
|
|
|
|
private _manager: Phaser.Physics.AdvancedPhysics;
|
|
|
|
// Delta Timer
|
|
private _delta: number;
|
|
private _deltaInv: number;
|
|
|
|
// Body array length
|
|
private _bl: number;
|
|
|
|
// Joints array length
|
|
private _jl: number;
|
|
|
|
// Contact Solvers array length
|
|
private _cl: number;
|
|
|
|
private _linTolSqr: number;
|
|
private _angTolSqr: number;
|
|
|
|
// Minimum sleep time (used in the sleep process solver)
|
|
private _minSleepTime: number;
|
|
|
|
private _positionSolved: bool;
|
|
|
|
private _shape1: IShape;
|
|
private _shape2: IShape;
|
|
private _contactsOk: bool;
|
|
private _jointsOk: bool;
|
|
|
|
private bodyHash;
|
|
private jointHash;
|
|
|
|
public static TIME_TO_SLEEP = 0.5;
|
|
public static SLEEP_LINEAR_TOLERANCE = 0.5;
|
|
public static SLEEP_ANGULAR_TOLERANCE = 2 * 0.017453292519943294444444444444444;
|
|
|
|
public bodies: Body[];
|
|
public joints: IJoint[];
|
|
public numContacts: number;
|
|
public contactSolvers: ContactSolver[];
|
|
public postSolve = null;
|
|
public gravity: Phaser.Vec2;
|
|
public damping: number;
|
|
public stepCount: number = 0;
|
|
|
|
public clear() {
|
|
|
|
AdvancedPhysics.shapeCounter = 0;
|
|
AdvancedPhysics.bodyCounter = 0;
|
|
AdvancedPhysics.jointCounter = 0;
|
|
|
|
for (var i = 0; i < this.bodies.length; i++)
|
|
{
|
|
if (this.bodies[i])
|
|
{
|
|
this.removeBody(this.bodies[i]);
|
|
}
|
|
}
|
|
|
|
this.bodies = [];
|
|
this.bodyHash = {};
|
|
|
|
this.joints = [];
|
|
this.jointHash = {};
|
|
|
|
this.contactSolvers = [];
|
|
|
|
this.stepCount = 0;
|
|
|
|
}
|
|
|
|
public addBody(body: Body) {
|
|
|
|
if (this.bodyHash[body.id] != undefined)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var index = this.bodies.push(body) - 1;
|
|
this.bodyHash[body.id] = index;
|
|
|
|
body.awake(true);
|
|
body.space = this;
|
|
body.cacheData('addBody');
|
|
|
|
}
|
|
|
|
public removeBody(body: Body) {
|
|
|
|
if (this.bodyHash[body.id] == undefined)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Remove linked joints
|
|
for (var i = 0; i < body.joints.length; i++)
|
|
{
|
|
if (body.joints[i])
|
|
{
|
|
this.removeJoint(body.joints[i]);
|
|
}
|
|
}
|
|
|
|
body.space = null;
|
|
|
|
var index = this.bodyHash[body.id];
|
|
delete this.bodyHash[body.id];
|
|
delete this.bodies[index];
|
|
|
|
}
|
|
|
|
public addJoint(joint: IJoint) {
|
|
|
|
if (this.jointHash[joint.id] != undefined)
|
|
{
|
|
return;
|
|
}
|
|
|
|
joint.body1.awake(true);
|
|
joint.body2.awake(true);
|
|
|
|
var index = this.joints.push(joint) - 1;
|
|
this.jointHash[joint.id] = index;
|
|
|
|
var index = joint.body1.joints.push(joint) - 1;
|
|
joint.body1.jointHash[joint.id] = index;
|
|
|
|
var index = joint.body2.joints.push(joint) - 1;
|
|
joint.body2.jointHash[joint.id] = index;
|
|
|
|
}
|
|
|
|
public removeJoint(joint: IJoint) {
|
|
|
|
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.joints[index];
|
|
|
|
var index = joint.body2.jointHash[joint.id];
|
|
delete joint.body2.jointHash[joint.id];
|
|
delete joint.body2.joints[index];
|
|
|
|
var index = this.jointHash[joint.id];
|
|
delete this.jointHash[joint.id];
|
|
delete this.joints[index];
|
|
|
|
}
|
|
|
|
public findShapeByPoint(p, refShape) {
|
|
|
|
var firstShape;
|
|
|
|
for (var i = 0; i < this.bodies.length; i++)
|
|
{
|
|
var body = this.bodies[i];
|
|
|
|
if (!body)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapes.length; j++)
|
|
{
|
|
var shape = body.shapes[j];
|
|
|
|
if (shape.pointQuery(p))
|
|
{
|
|
if (!refShape)
|
|
{
|
|
return shape;
|
|
}
|
|
|
|
if (!firstShape)
|
|
{
|
|
firstShape = shape;
|
|
}
|
|
|
|
if (shape == refShape)
|
|
{
|
|
refShape = null;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return firstShape;
|
|
}
|
|
|
|
public findBodyByPoint(p, refBody: Body) {
|
|
|
|
var firstBody;
|
|
|
|
for (var i = 0; i < this.bodies.length; i++)
|
|
{
|
|
var body = this.bodies[i];
|
|
|
|
if (!body)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapes.length; j++)
|
|
{
|
|
var shape = body.shapes[j];
|
|
|
|
if (shape.pointQuery(p))
|
|
{
|
|
if (!refBody)
|
|
{
|
|
return shape.body;
|
|
}
|
|
|
|
if (!firstBody)
|
|
{
|
|
firstBody = shape.body;
|
|
}
|
|
|
|
if (shape.body == refBody)
|
|
{
|
|
refBody = null;
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
return firstBody;
|
|
|
|
}
|
|
|
|
public shapeById(id) {
|
|
|
|
var shape;
|
|
|
|
for (var i = 0; i < this.bodies.length; i++)
|
|
{
|
|
var body: Body = this.bodies[i];
|
|
|
|
if (!body)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapes.length; j++)
|
|
{
|
|
if (body.shapes[j].id == id)
|
|
{
|
|
return body.shapes[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
public jointById(id) {
|
|
|
|
var index = this.jointHash[id];
|
|
|
|
if (index != undefined)
|
|
{
|
|
return this.joints[index];
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
public findVertexByPoint(p, minDist, refVertexId) {
|
|
|
|
var firstVertexId = -1;
|
|
|
|
refVertexId = refVertexId || -1;
|
|
|
|
for (var i = 0; i < this.bodies.length; i++)
|
|
{
|
|
var body = this.bodies[i];
|
|
|
|
if (!body)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapes.length; j++)
|
|
{
|
|
var shape = body.shapes[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;
|
|
|
|
}
|
|
|
|
public findEdgeByPoint(p, minDist, refEdgeId) {
|
|
|
|
var firstEdgeId = -1;
|
|
|
|
refEdgeId = refEdgeId || -1;
|
|
|
|
for (var i = 0; i < this.bodies.length; i++)
|
|
{
|
|
var body = this.bodies[i];
|
|
|
|
if (!body)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
for (var j = 0; j < body.shapes.length; j++)
|
|
{
|
|
var shape = body.shapes[j];
|
|
|
|
if (shape.type != AdvancedPhysics.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;
|
|
}
|
|
|
|
public findJointByPoint(p, minDist, refJointId) {
|
|
|
|
var firstJointId = -1;
|
|
|
|
var dsq = minDist * minDist;
|
|
|
|
refJointId = refJointId || -1;
|
|
|
|
for (var i = 0; i < this.joints.length; i++)
|
|
{
|
|
var joint = this.joints[i];
|
|
|
|
if (!joint)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var jointId = -1;
|
|
|
|
if (Phaser.Vec2Utils.distanceSq(p, joint.getWorldAnchor1()) < dsq)
|
|
{
|
|
jointId = (joint.id << 16 | 0);
|
|
}
|
|
else if (Phaser.Vec2Utils.distanceSq(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;
|
|
}
|
|
|
|
private findContactSolver(shape1:IShape, shape2:IShape):ContactSolver {
|
|
|
|
Manager.write('findContactSolver. Length: ' + this._cl);
|
|
|
|
for (var i = 0; i < this._cl; i++)
|
|
{
|
|
var contactSolver: ContactSolver = this.contactSolvers[i];
|
|
|
|
if (shape1 == contactSolver.shape1 && shape2 == contactSolver.shape2)
|
|
{
|
|
return contactSolver;
|
|
}
|
|
}
|
|
|
|
return null;
|
|
|
|
}
|
|
|
|
private genTemporalContactSolvers() {
|
|
|
|
Manager.write('genTemporalContactSolvers');
|
|
|
|
this._cl = 0;
|
|
this.contactSolvers.length = 0;
|
|
this.numContacts = 0;
|
|
|
|
for (var body1Index = 0; body1Index < this._bl; body1Index++)
|
|
{
|
|
if (!this.bodies[body1Index])
|
|
{
|
|
continue;
|
|
}
|
|
|
|
this.bodies[body1Index].stepCount = this.stepCount;
|
|
|
|
for (var body2Index = 0; body2Index < this._bl; body2Index++)
|
|
{
|
|
if (this.bodies[body1Index].inContact(this.bodies[body2Index]) == false)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
Manager.write('body1 and body2 intersect');
|
|
|
|
for (var i = 0; i < this.bodies[body1Index].shapesLength; i++)
|
|
{
|
|
for (var j = 0; j < this.bodies[body2Index].shapesLength; j++)
|
|
{
|
|
this._shape1 = this.bodies[body1Index].shapes[i];
|
|
this._shape2 = this.bodies[body2Index].shapes[j];
|
|
|
|
var contactArr = [];
|
|
|
|
if (!AdvancedPhysics.collision.collide(this._shape1, this._shape2, contactArr))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (this._shape1.type > this._shape2.type)
|
|
{
|
|
var temp = this._shape1;
|
|
this._shape1 = this._shape2;
|
|
this._shape2 = temp;
|
|
}
|
|
|
|
this.numContacts += contactArr.length;
|
|
|
|
// Result stored in this._contactSolver (see what we can do about generating some re-usable solvers)
|
|
var contactSolver: ContactSolver = this.findContactSolver(this._shape1, this._shape2);
|
|
|
|
Manager.write('findContactSolver result: ' + contactSolver);
|
|
|
|
if (contactSolver)
|
|
{
|
|
contactSolver.update(contactArr);
|
|
this.contactSolvers.push(contactSolver);
|
|
}
|
|
else
|
|
{
|
|
Manager.write('awake both bodies');
|
|
|
|
this.bodies[body1Index].awake(true);
|
|
this.bodies[body2Index].awake(true);
|
|
|
|
var newContactSolver = new ContactSolver(this._shape1, this._shape2);
|
|
newContactSolver.contacts = contactArr;
|
|
newContactSolver.elasticity = Math.max(this._shape1.elasticity, this._shape2.elasticity);
|
|
newContactSolver.friction = Math.sqrt(this._shape1.friction * this._shape2.friction);
|
|
|
|
this.contactSolvers.push(newContactSolver);
|
|
Manager.write('new contact solver');
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
this._cl = this.contactSolvers.length;
|
|
|
|
}
|
|
|
|
private initSolver(warmStarting) {
|
|
|
|
Manager.write('initSolver');
|
|
Manager.write('contactSolvers.length: ' + this._cl);
|
|
|
|
// Initialize contact solvers
|
|
for (var c = 0; c < this._cl; c++)
|
|
{
|
|
this.contactSolvers[c].initSolver(this._deltaInv);
|
|
|
|
// Warm starting (apply cached impulse)
|
|
if (warmStarting)
|
|
{
|
|
this.contactSolvers[c].warmStart();
|
|
}
|
|
|
|
}
|
|
|
|
// Initialize joint solver
|
|
for (var j = 0; j < this.joints.length; j++)
|
|
{
|
|
if (this.joints[j])
|
|
{
|
|
this.joints[j].initSolver(this._delta, warmStarting);
|
|
}
|
|
}
|
|
|
|
// Warm starting (apply cached impulse)
|
|
/*
|
|
if (warmStarting)
|
|
{
|
|
for (var c = 0; c < this._cl; c++)
|
|
{
|
|
this.contactSolvers[c].warmStart();
|
|
}
|
|
}
|
|
*/
|
|
|
|
}
|
|
|
|
private velocitySolver(iterations:number) {
|
|
|
|
Manager.write('velocitySolver, iterations: ' + iterations + ' csa len: ' + this._cl);
|
|
|
|
for (var i = 0; i < iterations; i++)
|
|
{
|
|
for (var j = 0; j < this._jl; j++)
|
|
{
|
|
if (this.joints[j])
|
|
{
|
|
this.joints[j].solveVelocityConstraints();
|
|
}
|
|
}
|
|
|
|
for (var c = 0; c < this._cl; c++)
|
|
{
|
|
this.contactSolvers[c].solveVelocityConstraints();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
private positionSolver(iterations:number):bool {
|
|
|
|
this._positionSolved = false;
|
|
|
|
for (var i = 0; i < iterations; i++)
|
|
{
|
|
this._contactsOk = true;
|
|
this._jointsOk = true;
|
|
|
|
for (var c = 0; c < this._cl; c++)
|
|
{
|
|
this._contactsOk = this.contactSolvers[c].solvePositionConstraints() && this._contactsOk;
|
|
}
|
|
|
|
for (var j = 0; j < this._jl; j++)
|
|
{
|
|
if (this.joints[j])
|
|
{
|
|
this._jointsOk = this.joints[j].solvePositionConstraints() && this._jointsOk;
|
|
}
|
|
}
|
|
|
|
if (this._contactsOk && this._jointsOk)
|
|
{
|
|
// exit early if the position errors are small
|
|
this._positionSolved = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return this._positionSolved;
|
|
|
|
}
|
|
|
|
// Step through the physics simulation
|
|
public step(dt: number, velocityIterations: number, positionIterations: number, warmStarting: bool, allowSleep: bool) {
|
|
|
|
Manager.clear();
|
|
Manager.write('Space step ' + this.stepCount);
|
|
|
|
this._delta = dt;
|
|
this._deltaInv = 1 / dt;
|
|
this._bl = this.bodies.length;
|
|
this._jl = this.joints.length;
|
|
|
|
this.stepCount++;
|
|
|
|
// 1) Generate Contact Solvers (into the this.contactSolvers array)
|
|
this.genTemporalContactSolvers();
|
|
|
|
Manager.dump("Contact Solvers", this.bodies[1]);
|
|
|
|
// 2) Initialize the Contact Solvers
|
|
this.initSolver(warmStarting);
|
|
|
|
Manager.dump("Init Solver", this.bodies[1]);
|
|
|
|
// 3) Intergrate velocity
|
|
for (var i = 0; i < this._bl; i++)
|
|
{
|
|
if (this.bodies[i] && this.bodies[i].isDynamic && this.bodies[i].isAwake)
|
|
{
|
|
this.bodies[i].updateVelocity(this.gravity, this._delta, this.damping);
|
|
}
|
|
}
|
|
|
|
Manager.dump("Update Velocity", this.bodies[1]);
|
|
|
|
// 4) Awaken bodies via joints
|
|
for (var j = 0; i < this._jl; j++)
|
|
{
|
|
if (!this.joints[j])
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// combine
|
|
var awake1 = this.joints[j].body1.isAwake && !this.joints[j].body1.isStatic;
|
|
var awake2 = this.joints[j].body2.isAwake && !this.joints[j].body2.isStatic;
|
|
|
|
if (awake1 ^ awake2)
|
|
{
|
|
if (!awake1)
|
|
{
|
|
this.joints[j].body1.awake(true);
|
|
}
|
|
|
|
if (!awake2)
|
|
{
|
|
this.joints[j].body2.awake(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 5) Iterative velocity constraints solver
|
|
this.velocitySolver(velocityIterations);
|
|
|
|
Manager.dump("Velocity Solvers", this.bodies[1]);
|
|
|
|
// 6) Integrate position
|
|
for (var i = 0; i < this._bl; i++)
|
|
{
|
|
if (this.bodies[i] && this.bodies[i].isDynamic && this.bodies[i].isAwake)
|
|
{
|
|
this.bodies[i].updatePosition(this._delta);
|
|
}
|
|
}
|
|
|
|
Manager.dump("Update Position", this.bodies[1]);
|
|
|
|
// 7) Process breakable joint
|
|
for (var i = 0; i < this._jl; i++)
|
|
{
|
|
if (this.joints[i] && this.joints[i].breakable && (this.joints[i].getReactionForce(this._deltaInv).lengthSq() >= this.joints[i].maxForce * this.joints[i].maxForce))
|
|
{
|
|
this.removeJoint(this.joints[i]);
|
|
}
|
|
}
|
|
|
|
// 8) Iterative position constraints solver (result stored in this._positionSolved)
|
|
this.positionSolver(positionIterations);
|
|
|
|
Manager.dump("Position Solver", this.bodies[1]);
|
|
|
|
// 9) Sync the Transforms
|
|
for (var i = 0; i < this._bl; i++)
|
|
{
|
|
if (this.bodies[i])
|
|
{
|
|
this.bodies[i].syncTransform();
|
|
}
|
|
}
|
|
|
|
Manager.dump("Sync Transform", this.bodies[1]);
|
|
|
|
// 10) Post solve collision callback
|
|
if (this.postSolve)
|
|
{
|
|
for (var i = 0; i < this._cl; i++)
|
|
{
|
|
this.postSolve(this.contactSolvers[i]);
|
|
}
|
|
}
|
|
|
|
// 11) Cache Body Data
|
|
for (var i = 0; i < this._bl; i++)
|
|
{
|
|
if (this.bodies[i] && this.bodies[i].isDynamic && this.bodies[i].isAwake)
|
|
{
|
|
this.bodies[i].cacheData('post solve collision callback');
|
|
}
|
|
}
|
|
|
|
Manager.dump("Cache Data", this.bodies[1]);
|
|
|
|
Manager.writeAll();
|
|
|
|
// 12) Process sleeping
|
|
if (allowSleep)
|
|
{
|
|
this._minSleepTime = 999999;
|
|
|
|
for (var i = 0; i < this._bl; i++)
|
|
{
|
|
if (!this.bodies[i] || this.bodies[i].isDynamic == false)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (this.bodies[i].angularVelocity * this.bodies[i].angularVelocity > this._angTolSqr || this.bodies[i].velocity.dot(this.bodies[i].velocity) > this._linTolSqr)
|
|
{
|
|
this.bodies[i].sleepTime = 0;
|
|
this._minSleepTime = 0;
|
|
}
|
|
else
|
|
{
|
|
this.bodies[i].sleepTime += this._delta;
|
|
this._minSleepTime = Math.min(this._minSleepTime, this.bodies[i].sleepTime);
|
|
}
|
|
}
|
|
|
|
if (this._positionSolved && this._minSleepTime >= Space.TIME_TO_SLEEP)
|
|
{
|
|
for (var i = 0; i < this._bl; i++)
|
|
{
|
|
if (this.bodies[i])
|
|
{
|
|
this.bodies[i].awake(false);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
} |