*/
public void testConjugateGradients2() {
double epsilon = 1e-7;
Solver s = new ConjugateGradients();
Body b1 = new Body("default");
Body b2 = new Body("default");
Solver.NCPConstraint c1 = new NCPConstraint();
Solver.NCPConstraint c2 = new NCPConstraint();
Vector3 va =new Vector3(1,0,0);
Vector3 vb =new Vector3(-1,0,0);
Vector3 z = new Vector3(0,0,0);
c1.assign(b1,b2,
va,z,vb,z,
va,z,vb,z,
0,0,null,1, 0);
c2.assign(b1,b2,
va,z,vb,z,
va,z,vb,z,
0,0,null,1, 0);
//This is the system
//
// [ 1 0 0 0 0 0 -1 0 0 0 0 0] [1 0 0 0 0 0 -1 0 0 0 0 0 ]^T x + 1 = 0
// [ 1 0 0 0 0 0 -1 0 0 0 0 0] [1 0 0 0 0 0 -1 0 0 0 0 0 ] =>
//
// 2 2 x1 + 1 = 0
// 2 2 x2 + 1 = 0
//
// with a least squares solution solution x1 = -1/4 and x2 = -1/4
//list of bodies and constraints
List<Body> bodies = new ArrayList<Body>();
List<NCPConstraint> constraints = new ArrayList<NCPConstraint>();
bodies.add(b1); bodies.add(b2); constraints.add(c1); constraints.add(c2);
//run the solver
s.solve(constraints, bodies, 0.0);
System.out.println("("+c1.lambda+","+c2.lambda+")");
//expect solution (1/4,1/4)
assertTrue( Math.abs(-0.25 - c1.lambda) < epsilon );