var EventEmitter = require('../events/EventEmitter');
module.exports = Solver;
/**
* Base class for constraint solvers.
* @class Solver
* @constructor
* @extends EventEmitter
*/
function Solver(options,type){
options = options || {};
EventEmitter.call(this);
this.type = type;
/**
* Current equations in the solver.
*
* @property equations
* @type {Array}
*/
this.equations = [];
/**
* Function that is used to sort all equations before each solve.
* @property equationSortFunction
* @type {function|boolean}
*/
this.equationSortFunction = options.equationSortFunction || false;
}
Solver.prototype = new EventEmitter();
Solver.prototype.constructor = Solver;
/**
* Method to be implemented in each subclass
* @method solve
* @param {Number} dt
* @param {World} world
*/
Solver.prototype.solve = function(/*dt,world*/){
throw new Error("Solver.solve should be implemented by subclasses!");
};
/**
* Sort all equations using the .equationSortFunction. Should be called by subclasses before solving.
* @method sortEquations
*/
Solver.prototype.sortEquations = function(){
if(this.equationSortFunction){
this.equations.sort(this.equationSortFunction);
}
};
/**
* Add an equation to be solved.
*
* @method addEquation
* @param {Equation} eq
*/
Solver.prototype.addEquation = function(eq){
if(eq.enabled){
this.equations.push(eq);
}
};
/**
* Add equations. Same as .addEquation, but this time the argument is an array of Equations
*
* @method addEquations
* @param {Array} eqs
*/
Solver.prototype.addEquations = function(eqs){
for(var i=0, N=eqs.length; i!==N; i++){
var eq = eqs[i];
if(eq.enabled){
this.equations.push(eq);
}
}
};
/**
* Remove an equation.
*
* @method removeEquation
* @param {Equation} eq
*/
Solver.prototype.removeEquation = function(eq){
var i = this.equations.indexOf(eq);
if(i !== -1){
this.equations.splice(i,1);
}
};
/**
* Remove all currently added equations.
*
* @method removeAllEquations
*/
Solver.prototype.removeAllEquations = function(){
this.equations.length=0;
};
/**
* Gauss-Seidel solver.
* @property GS
* @type {Number}
* @static
*/
Solver.GS = 1;Gauss Seidel Solver
var Solver = require('./Solver')
, FrictionEquation = require('../equations/FrictionEquation');
module.exports = GSSolver;
/**
* Iterative Gauss-Seidel constraint equation solver.
*
* @class GSSolver
* @constructor
* @extends Solver
* @param {Object} [options]
* @param {Number} [options.iterations=10]
* @param {Number} [options.tolerance=0]
*/
function GSSolver(options){
Solver.call(this,options,Solver.GS);
options = options || {};
/**
* The max number of iterations to do when solving. More gives better results, but is more expensive.
* @property iterations
* @type {Number}
*/
this.iterations = options.iterations || 10;
/**
* The error tolerance, per constraint. If the total error is below this limit, the solver will stop iterating. Set to zero for as good solution as possible, but to something larger than zero to make computations faster.
* @property tolerance
* @type {Number}
* @default 1e-7
*/
this.tolerance = options.tolerance !== undefined ? options.tolerance : 1e-7;
/**
* Number of solver iterations that are used to approximate normal forces used for friction (F_friction = mu * F_normal). These friction forces will override any other friction forces that are set. If you set frictionIterations = 0, then this feature will be disabled.
*
* Use only frictionIterations > 0 if the approximated normal force (F_normal = mass * gravity) is not good enough. Examples of where it can happen is in space games where gravity is zero, or in tall stacks where the normal force is large at bottom but small at top.
*
* @property frictionIterations
* @type {Number}
* @default 0
*/
this.frictionIterations = options.frictionIterations !== undefined ? 0 : options.frictionIterations;
/**
* The number of iterations that were made during the last solve. If .tolerance is zero, this value will always be equal to .iterations, but if .tolerance is larger than zero, and the solver can quit early, then this number will be somewhere between 1 and .iterations.
* @property {Number} usedIterations
*/
this.usedIterations = 0;
}
GSSolver.prototype = new Solver();
GSSolver.prototype.constructor = GSSolver;
/**
* Solve the system of equations
* @method solve
* @param {Number} h Time step
* @param {World} world World to solve
*/
GSSolver.prototype.solve = function(h, world){
this.sortEquations();
var iter = 0,
maxIter = this.iterations,
maxFrictionIter = this.frictionIterations,
equations = this.equations,
Neq = equations.length,
tolSquared = Math.pow(this.tolerance * Neq, 2),
bodies = world.bodies,
Nbodies = bodies.length;
this.usedIterations = 0;
if(Neq){
for(var i=0; i!==Nbodies; i++){
var b = bodies[i];
// Update solve mass
b.updateSolveMassProperties();
}
}
for(var i=0; i!==Neq; i++){
var c = equations[i];
c.lambda = 0;
if(c.timeStep !== h || c.needsUpdate){
c.timeStep = h;
c.update();
}
c.B = c.computeB(c.a,c.b,h);
c.invC = c.computeInvC(c.epsilon);
c.maxForceDt = c.maxForce * h;
c.minForceDt = c.minForce * h;
}
var c, deltalambdaTot, i, j;
if(Neq !== 0){
for(i=0; i!==Nbodies; i++){
var b = bodies[i];
// Reset vlambda
b.resetConstraintVelocity();
}
if(maxFrictionIter){
// Iterate over contact equations to get normal forces
for(iter=0; iter!==maxFrictionIter; iter++){
// Accumulate the total error for each iteration.
deltalambdaTot = 0.0;
for(j=0; j!==Neq; j++){
c = equations[j];
var deltalambda = iterateEquation(c,h);
deltalambdaTot += Math.abs(deltalambda);
}
this.usedIterations++;
// If the total error is small enough - stop iterate
if(deltalambdaTot*deltalambdaTot <= tolSquared){
break;
}
}
updateMultipliers(equations, 1/h);
// Set computed friction force
for(j=0; j!==Neq; j++){
var eq = equations[j];
if(eq instanceof FrictionEquation){
var f = 0.0;
for(var k=0; k!==eq.contactEquations.length; k++){
f += eq.contactEquations[k].multiplier;
}
f *= eq.frictionCoefficient / eq.contactEquations.length;
eq.maxForce = f;
eq.minForce = -f;
eq.maxForceDt = f * h;
eq.minForceDt = -f * h;
}
}
}
// Iterate over all equations
for(iter=0; iter!==maxIter; iter++){
// Accumulate the total error for each iteration.
deltalambdaTot = 0.0;
for(j=0; j!==Neq; j++){
c = equations[j];
var deltalambda = iterateEquation(c,h);
deltalambdaTot += Math.abs(deltalambda);
}
this.usedIterations++;
// If the total error is small enough - stop iterate
if(deltalambdaTot*deltalambdaTot < tolSquared){
break;
}
}
// Add result to velocity
for(i=0; i!==Nbodies; i++){
bodies[i].addConstraintVelocity();
}
updateMultipliers(equations, 1/h);
}
};
// Sets the .multiplier property of each equation
function updateMultipliers(equations, invDt){
var l = equations.length;
while(l--){
var eq = equations[l];
eq.multiplier = eq.lambda * invDt;
}
}
function iterateEquation(eq){
// Compute iteration
var B = eq.B,
eps = eq.epsilon,
invC = eq.invC,
lambdaj = eq.lambda,
GWlambda = eq.computeGWlambda(),
maxForce_dt = eq.maxForceDt,
minForce_dt = eq.minForceDt;
var deltalambda = invC * ( B - GWlambda - eps * lambdaj );
// Clamp if we are not within the min/max interval
var lambdaj_plus_deltalambda = lambdaj + deltalambda;
if(lambdaj_plus_deltalambda < minForce_dt){
deltalambda = minForce_dt - lambdaj;
} else if(lambdaj_plus_deltalambda > maxForce_dt){
deltalambda = maxForce_dt - lambdaj;
}
eq.lambda += deltalambda;
eq.addToWlambda(deltalambda);
return deltalambda;
}
Comments