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<NCPConstraint> constraints, List<Body> 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)<division_epsilon) {
        //System.out.println("at solution, delta_new " + delta_new );
        if (iterations==0){
          delta_best = Double.POSITIVE_INFINITY;
        }
        break;
      }
      
      double alpha = delta_new/dTq;
      delta_old = delta_new;
      delta_new = 0;

      for (NCPConstraint ci: constraints) {
        //x = x + alpha d
        ci.dlambda += alpha*ci.d;

        //r = r-alpha q
        ci.residual -= alpha*ci.q;
        
        //s = M^(-1) r
        ci.s = ci.residual / (ci.diagonal+ci.damper);
                
        //delta_new = rTs
        delta_new += ci.residual*ci.s;
      }
      
      //keep track of best solution
      boolean best_updated = false;
      if (delta_new < delta_best) {
        delta_best = delta_new;
        best_updated = true;
      }

      if (Math.abs(delta_old)<division_epsilon) {
        //System.out.println("old delta too low");
        break;
      }
      
//      if (delta_new > 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);
  }

}
java2s.com  | Contact Us | Privacy Policy
Copyright 2009 - 12 Demo Source and Support. All rights reserved.
All other trademarks are property of their respective owners.