var vec2 = require('../math/vec2')
, Solver = require('./Solver')
, Utils = require('../utils/Utils')
, 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 || 1e-7;
this.arrayStep = 30;
this.lambda = new Utils.ARRAY_TYPE(this.arrayStep);
this.Bs = new Utils.ARRAY_TYPE(this.arrayStep);
this.invCs = new Utils.ARRAY_TYPE(this.arrayStep);
/**
* Set to true to set all right hand side terms to zero when solving. Can be handy for a few applications.
* @property useZeroRHS
* @type {Boolean}
* @todo Remove, not used
*/
this.useZeroRHS = false;
/**
* 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;
function setArrayZero(array){
var l = array.length;
while(l--){
array[l] = +0.0;
}
}
/**
* 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 = world.bodies.length,
add = vec2.add,
set = vec2.set,
useZeroRHS = this.useZeroRHS,
lambda = this.lambda;
this.usedIterations = 0;
if(Neq){
for(var i=0; i!==Nbodies; i++){
var b = bodies[i];
// Update solve mass
b.updateSolveMassProperties();
}
}
// Things that does not change during iteration can be computed once
if(lambda.length < Neq){
lambda = this.lambda = new Utils.ARRAY_TYPE(Neq + this.arrayStep);
this.Bs = new Utils.ARRAY_TYPE(Neq + this.arrayStep);
this.invCs = new Utils.ARRAY_TYPE(Neq + this.arrayStep);
}
setArrayZero(lambda);
var invCs = this.invCs,
Bs = this.Bs,
lambda = this.lambda;
for(var i=0; i!==equations.length; i++){
var c = equations[i];
if(c.timeStep !== h || c.needsUpdate){
c.timeStep = h;
c.update();
}
Bs[i] = c.computeB(c.a,c.b,h);
invCs[i] = c.computeInvC(c.epsilon);
}
var q, B, 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 = GSSolver.iterateEquation(j,c,c.epsilon,Bs,invCs,lambda,useZeroRHS,h,iter);
deltalambdaTot += Math.abs(deltalambda);
}
this.usedIterations++;
// If the total error is small enough - stop iterate
if(deltalambdaTot*deltalambdaTot <= tolSquared){
break;
}
}
GSSolver.updateMultipliers(equations, lambda, 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;
}
}
}
// 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 = GSSolver.iterateEquation(j,c,c.epsilon,Bs,invCs,lambda,useZeroRHS,h,iter);
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();
}
GSSolver.updateMultipliers(equations, lambda, 1/h);
}
};
// Sets the .multiplier property of each equation
GSSolver.updateMultipliers = function(equations, lambda, invDt){
// Set the .multiplier property of each equation
var l = equations.length;
while(l--){
equations[l].multiplier = lambda[l] * invDt;
}
};
GSSolver.iterateEquation = function(j,eq,eps,Bs,invCs,lambda,useZeroRHS,dt,iter){
// Compute iteration
var B = Bs[j],
invC = invCs[j],
lambdaj = lambda[j],
GWlambda = eq.computeGWlambda();
var maxForce = eq.maxForce,
minForce = eq.minForce;
if(useZeroRHS){
B = 0;
}
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;
}
lambda[j] += deltalambda;
eq.addToWlambda(deltalambda);
return deltalambda;
};
