preSolve method

@override void preSolve(double timeStep, double invTimeStep)

Prepare for solving the constraint

Implementation

@override
void preSolve(double timeStep, double invTimeStep) {
  double inv, len;
  List<double> v;

  ii1 = i1.clone();
  ii2 = i2.clone();

  v = Matrix3.identity().add2(ii1!, ii2!).storage;
  inv = 1/( v[0]*(v[4]*v[8]-v[7]*v[5])  +  v[3]*(v[7]*v[2]-v[1]*v[8])  +  v[6]*(v[1]*v[5]-v[4]*v[2]) );
  dd = Matrix3(
      v[4]*v[8]-v[5]*v[7], v[2]*v[7]-v[1]*v[8], v[1]*v[5]-v[2]*v[4],
      v[5]*v[6]-v[3]*v[8], v[0]*v[8]-v[2]*v[6], v[2]*v[3]-v[0]*v[5],
      v[3]*v[7]-v[4]*v[6], v[1]*v[6]-v[0]*v[7], v[0]*v[4]-v[1]*v[3]
  )..multiplyScalar( inv );

  relativeOrientation.invert(b1.orientation).multiply(targetOrientation).multiply(b2.orientation);

  inv = relativeOrientation.w*2;

  vel..setFrom(relativeOrientation.toVector3())..scale(inv);

  len = vel.length;

  if( len > 0.02 ) {
    len = (0.02-len)/len*invTimeStep*0.05;
    vel.scale(len);
  }
  else{
    vel.setValues(0,0,0);
  }

  rn1..setFrom(imp)..applyMatrix3Transpose(ii1!);
  rn2..setFrom(imp)..applyMatrix3Transpose(ii2!);

  a1.add(rn1);
  a2.sub(rn2);
}