mirror of
https://github.com/wassname/phaser.git
synced 2026-08-11 11:23:06 +08:00
Started revamp of the Tilemap system. Also removed old 'Advanced Physics' and dropped in p2.js which is what I hope we'll eventually use.
This commit is contained in:
photonstorm
@@ -0,0 +1,42 @@
|
||||
module.exports = Constraint;
|
||||
|
||||
/**
|
||||
* Base constraint class.
|
||||
*
|
||||
* @class Constraint
|
||||
* @constructor
|
||||
* @author schteppe
|
||||
* @param {Body} bodyA
|
||||
* @param {Body} bodyB
|
||||
*/
|
||||
function Constraint(bodyA,bodyB){
|
||||
|
||||
/**
|
||||
* Equations to be solved in this constraint
|
||||
* @property equations
|
||||
* @type {Array}
|
||||
*/
|
||||
this.equations = [];
|
||||
|
||||
/**
|
||||
* First body participating in the constraint.
|
||||
* @property bodyA
|
||||
* @type {Body}
|
||||
*/
|
||||
this.bodyA = bodyA;
|
||||
|
||||
/**
|
||||
* Second body participating in the constraint.
|
||||
* @property bodyB
|
||||
* @type {Body}
|
||||
*/
|
||||
this.bodyB = bodyB;
|
||||
};
|
||||
|
||||
/**
|
||||
* To be implemented by subclasses. Should update the internal constraint parameters.
|
||||
* @method update
|
||||
*/
|
||||
/*Constraint.prototype.update = function(){
|
||||
throw new Error("method update() not implmemented in this Constraint subclass!");
|
||||
};*/
|
||||
@@ -0,0 +1,136 @@
|
||||
var Equation = require("./Equation"),
|
||||
vec2 = require('../math/vec2'),
|
||||
mat2 = require('../math/mat2');
|
||||
|
||||
module.exports = ContactEquation;
|
||||
|
||||
/**
|
||||
* Non-penetration constraint equation.
|
||||
*
|
||||
* @class ContactEquation
|
||||
* @constructor
|
||||
* @extends Equation
|
||||
* @param {Body} bi
|
||||
* @param {Body} bj
|
||||
*/
|
||||
function ContactEquation(bi,bj){
|
||||
Equation.call(this,bi,bj,0,1e6);
|
||||
this.ri = vec2.create();
|
||||
this.penetrationVec = vec2.create();
|
||||
this.rj = vec2.create();
|
||||
this.ni = vec2.create();
|
||||
this.rixn = 0;
|
||||
this.rjxn = 0;
|
||||
};
|
||||
ContactEquation.prototype = new Equation();
|
||||
ContactEquation.prototype.constructor = ContactEquation;
|
||||
ContactEquation.prototype.computeB = function(a,b,h){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
ri = this.ri,
|
||||
rj = this.rj,
|
||||
xi = bi.position,
|
||||
xj = bj.position;
|
||||
|
||||
var vi = bi.velocity,
|
||||
wi = bi.angularVelocity,
|
||||
fi = bi.force,
|
||||
taui = bi.angularForce;
|
||||
|
||||
var vj = bj.velocity,
|
||||
wj = bj.angularVelocity,
|
||||
fj = bj.force,
|
||||
tauj = bj.angularForce;
|
||||
|
||||
var penetrationVec = this.penetrationVec,
|
||||
invMassi = bi.invMass,
|
||||
invMassj = bj.invMass,
|
||||
invIi = bi.invInertia,
|
||||
invIj = bj.invInertia,
|
||||
n = this.ni;
|
||||
|
||||
// Caluclate cross products
|
||||
this.rixn = vec2.crossLength(ri,n);
|
||||
this.rjxn = vec2.crossLength(rj,n);
|
||||
|
||||
// Calculate q = xj+rj -(xi+ri) i.e. the penetration vector
|
||||
vec2.add(penetrationVec,xj,rj);
|
||||
vec2.sub(penetrationVec,penetrationVec,xi);
|
||||
vec2.sub(penetrationVec,penetrationVec,ri);
|
||||
|
||||
var Gq = vec2.dot(n,penetrationVec);
|
||||
|
||||
// Compute iteration
|
||||
var GW = vec2.dot(vj,n) - vec2.dot(vi,n) + wj * this.rjxn - wi * this.rixn;
|
||||
var GiMf = vec2.dot(fj,n)*invMassj - vec2.dot(fi,n)*invMassi + invIj*tauj*this.rjxn - invIi*taui*this.rixn;
|
||||
|
||||
var B = - Gq * a - GW * b - h*GiMf;
|
||||
|
||||
return B;
|
||||
};
|
||||
|
||||
// Compute C = GMG+eps in the SPOOK equation
|
||||
var computeC_tmp1 = vec2.create(),
|
||||
tmpMat1 = mat2.create(),
|
||||
tmpMat2 = mat2.create();
|
||||
ContactEquation.prototype.computeC = function(eps){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
n = this.ni,
|
||||
rixn = this.rixn,
|
||||
rjxn = this.rjxn,
|
||||
tmp = computeC_tmp1,
|
||||
imMat1 = tmpMat1,
|
||||
imMat2 = tmpMat2;
|
||||
|
||||
mat2.identity(imMat1);
|
||||
mat2.identity(imMat2);
|
||||
imMat1[0] = imMat1[3] = bi.invMass;
|
||||
imMat2[0] = imMat2[3] = bj.invMass;
|
||||
|
||||
var C = vec2.dot(n,vec2.transformMat2(tmp,n,imMat1)) + vec2.dot(n,vec2.transformMat2(tmp,n,imMat2)) + eps;
|
||||
//var C = bi.invMass + bj.invMass + eps;
|
||||
|
||||
C += bi.invInertia * this.rixn * this.rixn;
|
||||
C += bj.invInertia * this.rjxn * this.rjxn;
|
||||
|
||||
return C;
|
||||
};
|
||||
|
||||
ContactEquation.prototype.computeGWlambda = function(){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
n = this.ni,
|
||||
dot = vec2.dot;
|
||||
|
||||
return dot(n, bj.vlambda) + bj.wlambda * this.rjxn - dot(n, bi.vlambda) - bi.wlambda * this.rixn;
|
||||
};
|
||||
|
||||
var addToWlambda_temp = vec2.create();
|
||||
ContactEquation.prototype.addToWlambda = function(deltalambda){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
n = this.ni,
|
||||
temp = addToWlambda_temp,
|
||||
imMat1 = tmpMat1,
|
||||
imMat2 = tmpMat2;
|
||||
|
||||
mat2.identity(imMat1);
|
||||
mat2.identity(imMat2);
|
||||
imMat1[0] = imMat1[3] = bi.invMass;
|
||||
imMat2[0] = imMat2[3] = bj.invMass;
|
||||
|
||||
// Add to linear velocity
|
||||
//vec2.scale(temp,n,-bi.invMass*deltalambda);
|
||||
vec2.scale(temp,vec2.transformMat2(temp,n,imMat1),-deltalambda);
|
||||
vec2.add( bi.vlambda,bi.vlambda, temp );
|
||||
|
||||
//vec2.scale(temp,n,bj.invMass*deltalambda);
|
||||
vec2.scale(temp,vec2.transformMat2(temp,n,imMat2),deltalambda);
|
||||
vec2.add( bj.vlambda,bj.vlambda, temp);
|
||||
|
||||
// Add to angular velocity
|
||||
bi.wlambda -= bi.invInertia * this.rixn * deltalambda;
|
||||
bj.wlambda += bj.invInertia * this.rjxn * deltalambda;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
var Constraint = require('./Constraint')
|
||||
, ContactEquation = require('./ContactEquation')
|
||||
, vec2 = require('../math/vec2')
|
||||
|
||||
module.exports = DistanceConstraint;
|
||||
|
||||
/**
|
||||
* Constraint that tries to keep the distance between two bodies constant.
|
||||
*
|
||||
* @class DistanceConstraint
|
||||
* @constructor
|
||||
* @author schteppe
|
||||
* @param {Body} bodyA
|
||||
* @param {Body} bodyB
|
||||
* @param {number} dist The distance to keep between the bodies.
|
||||
* @param {number} maxForce
|
||||
* @extends {Constraint}
|
||||
*/
|
||||
function DistanceConstraint(bodyA,bodyB,distance,maxForce){
|
||||
Constraint.call(this,bodyA,bodyB);
|
||||
|
||||
this.distance = distance;
|
||||
|
||||
if(typeof(maxForce)==="undefined" ) {
|
||||
maxForce = 1e6;
|
||||
}
|
||||
|
||||
var normal = new ContactEquation(bodyA,bodyB); // Just in the normal direction
|
||||
|
||||
this.equations = [ normal ];
|
||||
|
||||
// Make the contact constraint bilateral
|
||||
this.setMaxForce(maxForce);
|
||||
}
|
||||
DistanceConstraint.prototype = new Constraint();
|
||||
|
||||
/**
|
||||
* Update the constraint equations. Should be done if any of the bodies changed position, before solving.
|
||||
* @method update
|
||||
*/
|
||||
DistanceConstraint.prototype.update = function(){
|
||||
var normal = this.equations[0],
|
||||
bodyA = this.bodyA,
|
||||
bodyB = this.bodyB,
|
||||
distance = this.distance;
|
||||
|
||||
vec2.sub(normal.ni, bodyB.position, bodyA.position);
|
||||
vec2.normalize(normal.ni,normal.ni);
|
||||
vec2.scale(normal.ri, normal.ni, distance*0.5);
|
||||
vec2.scale(normal.rj, normal.ni, -distance*0.5);
|
||||
};
|
||||
|
||||
DistanceConstraint.prototype.setMaxForce = function(f){
|
||||
var normal = this.equations[0];
|
||||
normal.minForce = -f;
|
||||
normal.maxForce = f;
|
||||
};
|
||||
|
||||
DistanceConstraint.prototype.getMaxForce = function(f){
|
||||
var normal = this.equations[0];
|
||||
return normal.maxForce;
|
||||
};
|
||||
@@ -0,0 +1,77 @@
|
||||
module.exports = Equation;
|
||||
|
||||
/**
|
||||
* Base class for constraint equations.
|
||||
* @class Equation
|
||||
* @constructor
|
||||
* @param {Body} bi First body participating in the equation
|
||||
* @param {Body} bj Second body participating in the equation
|
||||
* @param {number} minForce Minimum force to apply. Default: -1e6
|
||||
* @param {number} maxForce Maximum force to apply. Default: 1e6
|
||||
*/
|
||||
function Equation(bi,bj,minForce,maxForce){
|
||||
|
||||
/**
|
||||
* Minimum force to apply when solving
|
||||
* @property minForce
|
||||
* @type {Number}
|
||||
*/
|
||||
this.minForce = typeof(minForce)=="undefined" ? -1e6 : minForce;
|
||||
|
||||
/**
|
||||
* Max force to apply when solving
|
||||
* @property maxForce
|
||||
* @type {Number}
|
||||
*/
|
||||
this.maxForce = typeof(maxForce)=="undefined" ? 1e6 : maxForce;
|
||||
|
||||
/**
|
||||
* First body participating in the constraint
|
||||
* @property bi
|
||||
* @type {Body}
|
||||
*/
|
||||
this.bi = bi;
|
||||
|
||||
/**
|
||||
* Second body participating in the constraint
|
||||
* @property bj
|
||||
* @type {Body}
|
||||
*/
|
||||
this.bj = bj;
|
||||
|
||||
/**
|
||||
* The stiffness of this equation. Typically chosen to a large number (~1e7), but can be chosen somewhat freely to get a stable simulation.
|
||||
* @property stiffness
|
||||
* @type {Number}
|
||||
*/
|
||||
this.stiffness = 1e6;
|
||||
|
||||
/**
|
||||
* The number of time steps needed to stabilize the constraint equation. Typically between 3 and 5 time steps.
|
||||
* @property relaxation
|
||||
* @type {Number}
|
||||
*/
|
||||
this.relaxation = 4;
|
||||
|
||||
this.a = 0;
|
||||
this.b = 0;
|
||||
this.eps = 0;
|
||||
this.h = 0;
|
||||
this.updateSpookParams(1/60);
|
||||
};
|
||||
Equation.prototype.constructor = Equation;
|
||||
|
||||
/**
|
||||
* Update SPOOK parameters .a, .b and .eps according to the given time step. See equations 9, 10 and 11 in the <a href="http://www8.cs.umu.se/kurser/5DV058/VT09/lectures/spooknotes.pdf">SPOOK notes</a>.
|
||||
* @method updateSpookParams
|
||||
* @param {number} timeStep
|
||||
*/
|
||||
Equation.prototype.updateSpookParams = function(timeStep){
|
||||
var k = this.stiffness,
|
||||
d = this.relaxation,
|
||||
h = timeStep;
|
||||
this.a = 4.0 / (h * (1 + 4 * d));
|
||||
this.b = (4.0 * d) / (1 + 4 * d);
|
||||
this.eps = 4.0 / (h * h * k * (1 + 4 * d));
|
||||
this.h = timeStep;
|
||||
};
|
||||
@@ -0,0 +1,180 @@
|
||||
var mat2 = require('../math/mat2')
|
||||
, vec2 = require('../math/vec2')
|
||||
, Equation = require('./Equation')
|
||||
|
||||
module.exports = FrictionEquation;
|
||||
|
||||
// 3D cross product from glmatrix, until we get this to work...
|
||||
function cross(out, a, b) {
|
||||
var ax = a[0], ay = a[1], az = a[2],
|
||||
bx = b[0], by = b[1], bz = b[2];
|
||||
|
||||
out[0] = ay * bz - az * by;
|
||||
out[1] = az * bx - ax * bz;
|
||||
out[2] = ax * by - ay * bx;
|
||||
return out;
|
||||
};
|
||||
|
||||
var dot = vec2.dot;
|
||||
|
||||
/**
|
||||
* Constrains the slipping in a contact along a tangent
|
||||
*
|
||||
* @class FrictionEquation
|
||||
* @constructor
|
||||
* @param {Body} bi
|
||||
* @param {Body} bj
|
||||
* @param {Number} slipForce
|
||||
* @extends {Equation}
|
||||
*/
|
||||
function FrictionEquation(bi,bj,slipForce){
|
||||
Equation.call(this,bi,bj,-slipForce,slipForce);
|
||||
|
||||
/**
|
||||
* Relative vector from center of body i to the contact point, in world coords.
|
||||
* @property ri
|
||||
* @type {Float32Array}
|
||||
*/
|
||||
this.ri = vec2.create();
|
||||
|
||||
/**
|
||||
* Relative vector from center of body j to the contact point, in world coords.
|
||||
* @property rj
|
||||
* @type {Float32Array}
|
||||
*/
|
||||
this.rj = vec2.create();
|
||||
|
||||
/**
|
||||
* Tangent vector that the friction force will act along, in world coords.
|
||||
* @property t
|
||||
* @type {Float32Array}
|
||||
*/
|
||||
this.t = vec2.create();
|
||||
|
||||
this.rixt = 0;
|
||||
this.rjxt = 0;
|
||||
};
|
||||
FrictionEquation.prototype = new Equation();
|
||||
FrictionEquation.prototype.constructor = FrictionEquation;
|
||||
|
||||
/**
|
||||
* Set the slipping condition for the constraint. The friction force cannot be
|
||||
* larger than this value.
|
||||
* @method setSlipForce
|
||||
* @param {Number} slipForce
|
||||
*/
|
||||
FrictionEquation.prototype.setSlipForce = function(slipForce){
|
||||
this.maxForce = slipForce;
|
||||
this.minForce = -slipForce;
|
||||
};
|
||||
|
||||
var rixtVec = [0,0,0];
|
||||
var rjxtVec = [0,0,0];
|
||||
var ri3 = [0,0,0];
|
||||
var rj3 = [0,0,0];
|
||||
var t3 = [0,0,0];
|
||||
FrictionEquation.prototype.computeB = function(a,b,h){
|
||||
var a = this.a,
|
||||
b = this.b,
|
||||
bi = this.bi,
|
||||
bj = this.bj,
|
||||
ri = this.ri,
|
||||
rj = this.rj,
|
||||
t = this.t;
|
||||
|
||||
// Caluclate cross products
|
||||
ri3[0] = ri[0];
|
||||
ri3[1] = ri[1];
|
||||
rj3[0] = rj[0];
|
||||
rj3[1] = rj[1];
|
||||
t3[0] = t[0];
|
||||
t3[1] = t[1];
|
||||
cross(rixtVec, ri3, t3);//ri.cross(t,rixt);
|
||||
cross(rjxtVec, rj3, t3);//rj.cross(t,rjxt);
|
||||
this.rixt = rixtVec[2];
|
||||
this.rjxt = rjxtVec[2];
|
||||
|
||||
var GW = -dot(bi.velocity,t) + dot(bj.velocity,t) - this.rixt*bi.angularVelocity + this.rjxt*bj.angularVelocity; // eq. 40
|
||||
var GiMf = -dot(bi.force,t)*bi.invMass +dot(bj.force,t)*bj.invMass -this.rixt*bi.invInertia*bi.angularForce + this.rjxt*bj.invInertia*bj.angularForce;
|
||||
|
||||
var B = /* - Gq * a */ - GW * b - h*GiMf;
|
||||
|
||||
return B;
|
||||
};
|
||||
|
||||
// Compute C = G * iM * G' + eps
|
||||
//
|
||||
// G*iM*G' =
|
||||
//
|
||||
// [ iM1 ] [-t ]
|
||||
// [-t (-ri x t) t (rj x t)] * [ iI1 ] [-ri x t]
|
||||
// [ iM2 ] [t ]
|
||||
// [ iI2 ] [rj x t ]
|
||||
//
|
||||
// = (-t)*iM1*(-t) + (-ri x t)*iI1*(-ri x t) + t*iM2*t + (rj x t)*iI2*(rj x t)
|
||||
//
|
||||
// = t*iM1*t + (ri x t)*iI1*(ri x t) + t*iM2*t + (rj x t)*iI2*(rj x t)
|
||||
//
|
||||
var computeC_tmp1 = vec2.create(),
|
||||
tmpMat1 = mat2.create(),
|
||||
tmpMat2 = mat2.create();
|
||||
FrictionEquation.prototype.computeC = function(eps){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
t = this.t,
|
||||
C = 0.0,
|
||||
tmp = computeC_tmp1,
|
||||
imMat1 = tmpMat1,
|
||||
imMat2 = tmpMat2,
|
||||
dot = vec2.dot;
|
||||
|
||||
mat2.identity(imMat1);
|
||||
mat2.identity(imMat2);
|
||||
|
||||
imMat1[0] = imMat1[3] = bi.invMass;
|
||||
imMat2[0] = imMat2[3] = bj.invMass;
|
||||
|
||||
C = dot(t,vec2.transformMat2(tmp,t,imMat1)) + dot(t,vec2.transformMat2(tmp,t,imMat2)) + eps;
|
||||
|
||||
//C = bi.invMass + bj.invMass + eps;
|
||||
|
||||
C += bi.invInertia * this.rixt * this.rixt;
|
||||
C += bj.invInertia * this.rjxt * this.rjxt;
|
||||
|
||||
return C;
|
||||
};
|
||||
|
||||
FrictionEquation.prototype.computeGWlambda = function(){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
t = this.t,
|
||||
dot = vec2.dot;
|
||||
|
||||
return dot(t, bj.vlambda) + bj.wlambda * this.rjxt - bi.wlambda * this.rixt - dot(t, bi.vlambda);
|
||||
};
|
||||
|
||||
var FrictionEquation_addToWlambda_tmp = vec2.create();
|
||||
FrictionEquation.prototype.addToWlambda = function(deltalambda){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
t = this.t,
|
||||
tmp = FrictionEquation_addToWlambda_tmp,
|
||||
imMat1 = tmpMat1,
|
||||
imMat2 = tmpMat2;
|
||||
|
||||
mat2.identity(imMat1);
|
||||
mat2.identity(imMat2);
|
||||
imMat1[0] = imMat1[3] = bi.invMass;
|
||||
imMat2[0] = imMat2[3] = bj.invMass;
|
||||
|
||||
vec2.scale(tmp,vec2.transformMat2(tmp,t,imMat1),-deltalambda);
|
||||
//vec2.scale(tmp, t, -bi.invMass * deltalambda); //t.mult(invMassi * deltalambda, tmp);
|
||||
vec2.add(bi.vlambda, bi.vlambda, tmp); //bi.vlambda.vsub(tmp,bi.vlambda);
|
||||
|
||||
vec2.scale(tmp,vec2.transformMat2(tmp,t,imMat2),deltalambda);
|
||||
//vec2.scale(tmp, t, bj.invMass * deltalambda); //t.mult(invMassj * deltalambda, tmp);
|
||||
vec2.add(bj.vlambda, bj.vlambda, tmp); //bj.vlambda.vadd(tmp,bj.vlambda);
|
||||
|
||||
bi.wlambda -= bi.invInertia * this.rixt * deltalambda;
|
||||
bj.wlambda += bj.invInertia * this.rjxt * deltalambda;
|
||||
};
|
||||
@@ -0,0 +1,94 @@
|
||||
var Constraint = require('./Constraint')
|
||||
, ContactEquation = require('./ContactEquation')
|
||||
, RotationalVelocityEquation = require('./RotationalVelocityEquation')
|
||||
, vec2 = require('../math/vec2')
|
||||
|
||||
module.exports = PointToPointConstraint;
|
||||
|
||||
/**
|
||||
* Connects two bodies at given offset points
|
||||
* @class PointToPointConstraint
|
||||
* @constructor
|
||||
* @author schteppe
|
||||
* @param {Body} bodyA
|
||||
* @param {Float32Array} pivotA The point relative to the center of mass of bodyA which bodyA is constrained to.
|
||||
* @param {Body} bodyB Body that will be constrained in a similar way to the same point as bodyA. We will therefore get sort of a link between bodyA and bodyB. If not specified, bodyA will be constrained to a static point.
|
||||
* @param {Float32Array} pivotB See pivotA.
|
||||
* @param {Number} maxForce The maximum force that should be applied to constrain the bodies.
|
||||
* @extends {Constraint}
|
||||
* @todo Ability to specify world points
|
||||
*/
|
||||
function PointToPointConstraint(bodyA, pivotA, bodyB, pivotB, maxForce){
|
||||
Constraint.call(this,bodyA,bodyB);
|
||||
|
||||
maxForce = typeof(maxForce)!="undefined" ? maxForce : 1e7;
|
||||
|
||||
this.pivotA = pivotA;
|
||||
this.pivotB = pivotB;
|
||||
|
||||
// Equations to be fed to the solver
|
||||
var eqs = this.equations = [
|
||||
new ContactEquation(bodyA,bodyB), // Normal
|
||||
new ContactEquation(bodyA,bodyB), // Tangent
|
||||
];
|
||||
|
||||
var normal = eqs[0];
|
||||
var tangent = eqs[1];
|
||||
|
||||
tangent.minForce = normal.minForce = -maxForce;
|
||||
tangent.maxForce = normal.maxForce = maxForce;
|
||||
|
||||
this.motorEquation = null;
|
||||
}
|
||||
PointToPointConstraint.prototype = new Constraint();
|
||||
|
||||
PointToPointConstraint.prototype.update = function(){
|
||||
var bodyA = this.bodyA,
|
||||
bodyB = this.bodyB,
|
||||
pivotA = this.pivotA,
|
||||
pivotB = this.pivotB,
|
||||
eqs = this.equations,
|
||||
normal = eqs[0],
|
||||
tangent= eqs[1];
|
||||
|
||||
vec2.subtract(normal.ni, bodyB.position, bodyA.position);
|
||||
vec2.normalize(normal.ni,normal.ni);
|
||||
vec2.rotate(normal.ri, pivotA, bodyA.angle);
|
||||
vec2.rotate(normal.rj, pivotB, bodyB.angle);
|
||||
|
||||
vec2.rotate(tangent.ni, normal.ni, Math.PI / 2);
|
||||
vec2.copy(tangent.ri, normal.ri);
|
||||
vec2.copy(tangent.rj, normal.rj);
|
||||
};
|
||||
|
||||
/**
|
||||
* Enable the rotational motor
|
||||
* @method enableMotor
|
||||
*/
|
||||
PointToPointConstraint.prototype.enableMotor = function(){
|
||||
if(this.motorEquation) return;
|
||||
this.motorEquation = new RotationalVelocityEquation(this.bodyA,this.bodyB);
|
||||
this.equations.push(this.motorEquation);
|
||||
};
|
||||
|
||||
/**
|
||||
* Disable the rotational motor
|
||||
* @method disableMotor
|
||||
*/
|
||||
PointToPointConstraint.prototype.disableMotor = function(){
|
||||
if(!this.motorEquation) return;
|
||||
var i = this.equations.indexOf(this.motorEquation);
|
||||
this.motorEquation = null;
|
||||
this.equations.splice(i,1);
|
||||
};
|
||||
|
||||
/**
|
||||
* Set the speed of the rotational constraint motor
|
||||
* @method setMotorSpeed
|
||||
* @param {Number} speed
|
||||
*/
|
||||
PointToPointConstraint.prototype.setMotorSpeed = function(speed){
|
||||
if(!this.motorEquation) return;
|
||||
var i = this.equations.indexOf(this.motorEquation);
|
||||
this.equations[i].relativeVelocity = speed;
|
||||
};
|
||||
@@ -0,0 +1,83 @@
|
||||
var Constraint = require('./Constraint')
|
||||
, ContactEquation = require('./ContactEquation')
|
||||
, vec2 = require('../math/vec2')
|
||||
|
||||
module.exports = PrismaticConstraint;
|
||||
|
||||
/**
|
||||
* Constraint that only allows translation along a line between the bodies, no rotation
|
||||
*
|
||||
* @class PrismaticConstraint
|
||||
* @constructor
|
||||
* @author schteppe
|
||||
* @param {Body} bodyA
|
||||
* @param {Body} bodyB
|
||||
* @param {Object} options
|
||||
* @param {Number} options.maxForce
|
||||
* @param {Array} options.worldAxis
|
||||
* @param {Array} options.localAxisA
|
||||
* @param {Array} options.localAxisB
|
||||
* @extends {Constraint}
|
||||
*/
|
||||
function PrismaticConstraint(bodyA,bodyB,options){
|
||||
options = options || {};
|
||||
Constraint.call(this,bodyA,bodyB);
|
||||
|
||||
var maxForce = this.maxForce = typeof(options.maxForce)==="undefined" ? options.maxForce : 1e6;
|
||||
|
||||
// Equations to be fed to the solver
|
||||
var eqs = this.equations = [
|
||||
new ContactEquation(bodyA,bodyB), // Tangent for bodyA
|
||||
new ContactEquation(bodyB,bodyA), // Tangent for bodyB
|
||||
];
|
||||
|
||||
var tangentA = eqs[0],
|
||||
tangentB = eqs[1];
|
||||
|
||||
tangentA.minForce = tangentB.minForce = -maxForce;
|
||||
tangentA.maxForce = tangentB.maxForce = maxForce;
|
||||
|
||||
var worldAxis = vec2.create();
|
||||
if(options.worldAxis){
|
||||
vec2.copy(worldAxis, options.worldAxis);
|
||||
} else {
|
||||
vec2.sub(worldAxis, bodyB.position, bodyA.position);
|
||||
}
|
||||
vec2.normalize(worldAxis,worldAxis);
|
||||
|
||||
// Axis that is local in each body
|
||||
this.localAxisA = vec2.create();
|
||||
this.localAxisB = vec2.create();
|
||||
if(options.localAxisA) vec2.copy(this.localAxisA, options.localAxisA);
|
||||
else vec2.rotate(this.localAxisA, worldAxis, -bodyA.angle);
|
||||
|
||||
if(options.localAxisB) vec2.copy(this.localAxisB, options.localAxisB);
|
||||
else vec2.rotate(this.localAxisB, worldAxis, -bodyB.angle);
|
||||
}
|
||||
|
||||
PrismaticConstraint.prototype = new Constraint();
|
||||
|
||||
/**
|
||||
* Update the constraint equations. Should be done if any of the bodies changed position, before solving.
|
||||
* @method update
|
||||
*/
|
||||
PrismaticConstraint.prototype.update = function(){
|
||||
var tangentA = this.equations[0],
|
||||
tangentB = this.equations[1],
|
||||
bodyA = this.bodyA,
|
||||
bodyB = this.bodyB;
|
||||
|
||||
// Get tangent directions
|
||||
vec2.rotate(tangentA.ni, this.localAxisA, bodyA.angle - Math.PI/2);
|
||||
vec2.rotate(tangentB.ni, this.localAxisB, bodyB.angle + Math.PI/2);
|
||||
|
||||
// Get distance vector
|
||||
var dist = vec2.create();
|
||||
vec2.sub(dist, bodyB.position, bodyA.position);
|
||||
vec2.scale(tangentA.ri, tangentA.ni, -vec2.dot(tangentA.ni, dist));
|
||||
vec2.scale(tangentB.ri, tangentB.ni, vec2.dot(tangentB.ni, dist));
|
||||
vec2.add(tangentA.rj, tangentA.ri, dist);
|
||||
vec2.sub(tangentB.rj, tangentB.ri, dist);
|
||||
vec2.set(tangentA.ri, 0, 0);
|
||||
vec2.set(tangentB.ri, 0, 0);
|
||||
};
|
||||
@@ -0,0 +1,70 @@
|
||||
var Equation = require("./Equation"),
|
||||
vec2 = require('../math/vec2');
|
||||
|
||||
module.exports = RotationalVelocityEquation;
|
||||
|
||||
/**
|
||||
* Syncs rotational velocity of two bodies, or sets a relative velocity (motor).
|
||||
*
|
||||
* @class RotationalVelocityEquation
|
||||
* @constructor
|
||||
* @extends Equation
|
||||
* @param {Body} bi
|
||||
* @param {Body} bj
|
||||
*/
|
||||
function RotationalVelocityEquation(bi,bj){
|
||||
Equation.call(this,bi,bj,-1e6,1e6);
|
||||
this.relativeVelocity = 1;
|
||||
this.ratio = 1;
|
||||
};
|
||||
RotationalVelocityEquation.prototype = new Equation();
|
||||
RotationalVelocityEquation.prototype.constructor = RotationalVelocityEquation;
|
||||
RotationalVelocityEquation.prototype.computeB = function(a,b,h){
|
||||
var bi = this.bi,
|
||||
bj = this.bj,
|
||||
vi = bi.velocity,
|
||||
wi = bi.angularVelocity,
|
||||
taui = bi.angularForce,
|
||||
vj = bj.velocity,
|
||||
wj = bj.angularVelocity,
|
||||
tauj = bj.angularForce,
|
||||
invIi = bi.invInertia,
|
||||
invIj = bj.invInertia,
|
||||
Gq = 0,
|
||||
GW = this.ratio * wj - wi + this.relativeVelocity,
|
||||
GiMf = invIj*tauj - invIi*taui;
|
||||
|
||||
var B = - Gq * a - GW * b - h*GiMf;
|
||||
|
||||
return B;
|
||||
};
|
||||
|
||||
// Compute C = GMG+eps in the SPOOK equation
|
||||
RotationalVelocityEquation.prototype.computeC = function(eps){
|
||||
var bi = this.bi,
|
||||
bj = this.bj;
|
||||
|
||||
var C = bi.invInertia + bj.invInertia + eps;
|
||||
|
||||
return C;
|
||||
};
|
||||
var computeGWlambda_ulambda = vec2.create();
|
||||
RotationalVelocityEquation.prototype.computeGWlambda = function(){
|
||||
var bi = this.bi,
|
||||
bj = this.bj;
|
||||
|
||||
var GWlambda = bj.wlambda - bi.wlambda;
|
||||
|
||||
return GWlambda;
|
||||
};
|
||||
|
||||
var addToWlambda_temp = vec2.create();
|
||||
RotationalVelocityEquation.prototype.addToWlambda = function(deltalambda){
|
||||
var bi = this.bi,
|
||||
bj = this.bj;
|
||||
|
||||
// Add to angular velocity
|
||||
bi.wlambda -= bi.invInertia * deltalambda;
|
||||
bj.wlambda += bj.invInertia * deltalambda;
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user