preSolve method

@override void preSolve(double timeStep, double invTimeStep)

Prepare for solving the constraint

Implementation

@override
void preSolve(double timeStep,double invTimeStep ) {
  updateAnchorPoints();

  ax1..setFrom( localAxis1 )..applyMatrix3Transpose( body1!.rotation );
  an1..setFrom( localAngle1 )..applyMatrix3Transpose( body1!.rotation );

  ax2..setFrom( localAxis2 )..applyMatrix3Transpose( body2!.rotation );
  an2..setFrom( localAngle2 )..applyMatrix3Transpose( body2!.rotation );

  // normal tangent binormal

  nor..setValues(
    ax1.x*body2!.inverseMass + ax2.x*body1!.inverseMass,
    ax1.y*body2!.inverseMass + ax2.y*body1!.inverseMass,
    ax1.z*body2!.inverseMass + ax2.z*body1!.inverseMass
  )..normalize();
  tan.tangent( nor ).normalize();
  bin.cross2( nor, tan );

  // calculate hinge angle
  tmp.cross2( an1, an2 );

  double limite = Math.acosClamp( Math.dotVectors( an1, an2 ) );

  if(Math.dotVectors( nor, tmp ) < 0 ){
    rotationalLimitMotor.angle = -limite;
  }
  else{
    rotationalLimitMotor.angle = limite;
  }
  // angular error
  tmp.cross2( ax1, ax2 );
  r3.limitMotor2.angle = Math.dotVectors( tan, tmp );
  r3.limitMotor3.angle = Math.dotVectors( bin, tmp );

  // preSolve

  r3.preSolve( timeStep, invTimeStep );
  t3.preSolve( timeStep, invTimeStep );
}