ConjugateGradients.java :  » Java-3D » jinngine » jinngine » physics » solver » Java Open Source

 Java Open Source » Java 3D » jinngine
 jinngine » jinngine » physics » solver » ConjugateGradients.java ``````/** * Copyright (c) 2008-2010 Morten Silcowitz. * * This file is part of the Jinngine physics library * * Jinngine is published under the GPL license, available * at http://www.gnu.org/copyleft/gpl.html. */ package jinngine.physics.solver; import java.util.List; import jinngine.math.Vector3; import jinngine.physics.Body; /** * Preconditioned Conjugate Gradients solver. Solves the given list of constraints, * as if they described a plain symmetric linear system of equations, not a NCP problem. * Phased in another way, CG can solve the NCP, if l=-infinity and u=infinity for all constraints. */ public class ConjugateGradients implements Solver { int maxIterations = 0; @Override public void setMaximumIterations(int n) { maxIterations = n; } @Override public double solve(List constraints, List bodies, double epsilon) { maxIterations = constraints.size(); //System.out.println("*) CG "+ n+"x" + n+" system"); //Conjugate Gradients double delta_new=0, delta_old=0, delta_zero=0; double delta_low=Double.POSITIVE_INFINITY; double delta_best=delta_low; //double epsilon = 1e-6; double division_epsilon = 1e-12; int iterations=0; //reset auxiliary deltas for (Body b: bodies) { b.auxDeltav.assignZero(); b.auxDeltaOmega.assignZero(); } //We solve Ax+b = 0 => r = -b-Ax //r = b-Ax //d = M^(-1)r //delta_new = rTr for (NCPConstraint ci: constraints) { ci.residual = -ci.b - ci.j1.dot(ci.body1.deltavelocity.add(ci.body1.externaldeltavelocity)) - ci.j2.dot(ci.body1.deltaomega.add(ci.body1.externaldeltaomega)) - ci.j3.dot(ci.body2.deltavelocity.add(ci.body2.externaldeltavelocity)) - ci.j4.dot(ci.body2.deltaomega.add(ci.body2.externaldeltaomega)) - ci.lambda*ci.damper; //d = M^-1 r ci.d = ci.residual / (ci.diagonal+ci.damper); //TODO remove if (Math.abs(ci.diagonal)<1e-13) { System.exit(0); } //reflect d in the delta velocities Vector3.add( ci.body1.auxDeltav, ci.b1.multiply(ci.d)); Vector3.add( ci.body1.auxDeltaOmega, ci.b2.multiply(ci.d)); Vector3.add( ci.body2.auxDeltav, ci.b3.multiply(ci.d)); Vector3.add( ci.body2.auxDeltaOmega, ci.b4.multiply(ci.d)); delta_new += ci.residual * ci.d; //System.out.println("initial residuals "+ci.residual + " b=" + ci.b); } delta_old = delta_new; delta_zero = delta_new; delta_best = delta_new; //System.out.println(""+FischerNewtonConjugateGradients.fischerMerit(errormeassure, bodies)); //CG iterations while (iterations < maxIterations && delta_new>epsilon*epsilon*delta_zero && delta_new > epsilon ) { //System.out.println("cg iterate"); //q = Ad //alpha = delta_new/dTq double dTq = 0; for (NCPConstraint ci: constraints) { ci.q = ci.j1.dot(ci.body1.auxDeltav) + ci.j2.dot(ci.body1.auxDeltaOmega) + ci.j3.dot(ci.body2.auxDeltav) + ci.j4.dot(ci.body2.auxDeltaOmega) + ci.d * ci.damper; dTq += ci.d*ci.q; } if (Math.abs(dTq) delta_old && iterations > 25) { // System.out.println("rising error"); // break; // } if (delta_new > 100*delta_zero) { //System.out.println("error larger then start"); break; } double beta = delta_new/delta_old; //d = s + beta d for (Body b: bodies) { // d = beta d Vector3.multiply( b.auxDeltav, beta); Vector3.multiply( b.auxDeltaOmega, beta); } for (NCPConstraint ci: constraints) { // d = d + r //ci.d = ci.residual + beta* ci.d; ci.d = ci.s + beta* ci.d; //reflect d in the delta velocities Vector3.add( ci.body1.auxDeltav, ci.b1.multiply(ci.s)); Vector3.add( ci.body1.auxDeltaOmega, ci.b2.multiply(ci.s)); Vector3.add( ci.body2.auxDeltav, ci.b3.multiply(ci.s)); Vector3.add( ci.body2.auxDeltaOmega, ci.b4.multiply(ci.s)); //if best updated, set the best vector if (best_updated) ci.bestdlambda = ci.dlambda; } iterations += 1; //System.out.println(""+FischerNewtonConjugateGradients.fischerMerit(errormeassure, bodies)); //System.out.println("iteration " + iterations +", delta_new=" + delta_new +", alpha=" +alpha +", beta=" + beta); } //CG iterations //if solution was accepted //if (delta_best < epsilon) { //apply the lambda values to the final velocities for (NCPConstraint ci: constraints) { ci.lambda += ci.bestdlambda; //reflect in delta velocities Vector3.add( ci.body1.deltavelocity, ci.b1.multiply(ci.bestdlambda)); Vector3.add( ci.body1.deltaomega, ci.b2.multiply(ci.bestdlambda)); Vector3.add( ci.body2.deltavelocity, ci.b3.multiply(ci.bestdlambda)); Vector3.add( ci.body2.deltaomega, ci.b4.multiply(ci.bestdlambda)); //reset ci.dlambda = 0; ci.bestdlambda = 0; } //} //if (delta_best>1 && delta_best < 100000) //System.out.println("iterations: " + iterations +" best is "+ delta_best); //System.out.println("delta_best="+delta_best+ "iterations "+iterations); return (iterations+1); } } ``````