preSolve method

@override void preSolve(double timeStep, double invTimeStep)

Prepare for solving the constraint

Implementation

@override
void preSolve(double timeStep, double invTimeStep ) {
  r1x = r1.x;
  r1y = r1.y;
  r1z = r1.z;

  r2x = r2.x;
  r2y = r2.y;
  r2z = r2.z;

  m1 = b1?.inverseMass;
  m2 = b2?.inverseMass;

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

  Float32List ii1 = this.ii1!.storage;
  Float32List ii2 = this.ii2!.storage;

  ax1x = r1z!*ii1[1]+-r1y!*ii1[2];
  ax1y = r1z!*ii1[4]+-r1y!*ii1[5];
  ax1z = r1z!*ii1[7]+-r1y!*ii1[8];
  ay1x = -r1z!*ii1[0]+r1x!*ii1[2];
  ay1y = -r1z!*ii1[3]+r1x!*ii1[5];
  ay1z = -r1z!*ii1[6]+r1x!*ii1[8];
  az1x = r1y!*ii1[0]+-r1x!*ii1[1];
  az1y = r1y!*ii1[3]+-r1x!*ii1[4];
  az1z = r1y!*ii1[6]+-r1x!*ii1[7];
  ax2x = r2z!*ii2[1]+-r2y!*ii2[2];
  ax2y = r2z!*ii2[4]+-r2y!*ii2[5];
  ax2z = r2z!*ii2[7]+-r2y!*ii2[8];
  ay2x = -r2z!*ii2[0]+r2x!*ii2[2];
  ay2y = -r2z!*ii2[3]+r2x!*ii2[5];
  ay2z = -r2z!*ii2[6]+r2x!*ii2[8];
  az2x = r2y!*ii2[0]+-r2x!*ii2[1];
  az2y = r2y!*ii2[3]+-r2x!*ii2[4];
  az2z = r2y!*ii2[6]+-r2x!*ii2[7];

  double rxx = m1!+m2!;

  Matrix3 kk = Matrix3( rxx, 0, 0,  0, rxx, 0,  0, 0, rxx );
  Float32List k = kk.storage;

  k[0] += ii1[4]*r1z!*r1z!-(ii1[7]+ii1[5])*r1y!*r1z!+ii1[8]*r1y!*r1y!;
  k[1] += (ii1[6]*r1y!+ii1[5]*r1x!)*r1z!-ii1[3]*r1z!*r1z!-ii1[8]*r1x!*r1y!;
  k[2] += (ii1[3]*r1y!-ii1[4]*r1x!)*r1z!-ii1[6]*r1y!*r1y!+ii1[7]*r1x!*r1y!;
  k[3] += (ii1[2]*r1y!+ii1[7]*r1x!)*r1z!-ii1[1]*r1z!*r1z!-ii1[8]*r1x!*r1y!;
  k[4] += ii1[0]*r1z!*r1z!-(ii1[6]+ii1[2])*r1x!*r1z!+ii1[8]*r1x!*r1x!;
  k[5] += (ii1[1]*r1x!-ii1[0]*r1y!)*r1z!-ii1[7]*r1x!*r1x!+ii1[6]*r1x!*r1y!;
  k[6] += (ii1[1]*r1y!-ii1[4]*r1x!)*r1z!-ii1[2]*r1y!*r1y!+ii1[5]*r1x!*r1y!;
  k[7] += (ii1[3]*r1x!-ii1[0]*r1y!)*r1z!-ii1[5]*r1x!*r1x!+ii1[2]*r1x!*r1y!;
  k[8] += ii1[0]*r1y!*r1y!-(ii1[3]+ii1[1])*r1x!*r1y!+ii1[4]*r1x!*r1x!;

  k[0] += ii2[4]*r2z!*r2z!-(ii2[7]+ii2[5])*r2y!*r2z!+ii2[8]*r2y!*r2y!;
  k[1] += (ii2[6]*r2y!+ii2[5]*r2x!)*r2z!-ii2[3]*r2z!*r2z!-ii2[8]*r2x!*r2y!;
  k[2] += (ii2[3]*r2y!-ii2[4]*r2x!)*r2z!-ii2[6]*r2y!*r2y!+ii2[7]*r2x!*r2y!;
  k[3] += (ii2[2]*r2y!+ii2[7]*r2x!)*r2z!-ii2[1]*r2z!*r2z!-ii2[8]*r2x!*r2y!;
  k[4] += ii2[0]*r2z!*r2z!-(ii2[6]+ii2[2])*r2x!*r2z!+ii2[8]*r2x!*r2x!;
  k[5] += (ii2[1]*r2x!-ii2[0]*r2y!)*r2z!-ii2[7]*r2x!*r2x!+ii2[6]*r2x!*r2y!;
  k[6] += (ii2[1]*r2y!-ii2[4]*r2x!)*r2z!-ii2[2]*r2y!*r2y!+ii2[5]*r2x!*r2y!;
  k[7] += (ii2[3]*r2x!-ii2[0]*r2y!)*r2z!-ii2[5]*r2x!*r2x!+ii2[2]*r2x!*r2y!;
  k[8] += ii2[0]*r2y!*r2y!-(ii2[3]+ii2[1])*r2x!*r2y!+ii2[4]*r2x!*r2x!;

  double inv=1/( k[0]*(k[4]*k[8]-k[7]*k[5]) + k[3]*(k[7]*k[2]-k[1]*k[8]) + k[6]*(k[1]*k[5]-k[4]*k[2]) );
  dd = Matrix3(
    k[4]*k[8]-k[5]*k[7], k[2]*k[7]-k[1]*k[8], k[1]*k[5]-k[2]*k[4],
    k[5]*k[6]-k[3]*k[8], k[0]*k[8]-k[2]*k[6], k[2]*k[3]-k[0]*k[5],
    k[3]*k[7]-k[4]*k[6], k[1]*k[6]-k[0]*k[7], k[0]*k[4]-k[1]*k[3]
  )..scale( inv );

  velx = p2.x-p1.x;
  vely = p2.y-p1.y;
  velz = p2.z-p1.z;
  var len = math.sqrt(velx!*velx!+vely!*vely!+velz!*velz!);
  if(len>0.005){
    len = (0.005-len)/len*invTimeStep*0.05;
    velx = velx!*len;
    vely = vely!*len;
    velz = velz!*len;
  }
  else{
    velx = 0;
    vely = 0;
    velz = 0;
  }

  impx *= 0.95;
  impy *= 0.95;
  impz *= 0.95;

  l1!.x += impx*m1!;
  l1!.y += impy*m1!;
  l1!.z += impz*m1!;
  a1!.x += impx*ax1x!+impy*ay1x!+impz*az1x!;
  a1!.y += impx*ax1y!+impy*ay1y!+impz*az1y!;
  a1!.z += impx*ax1z!+impy*ay1z!+impz*az1z!;
  l2!.x -= impx*m2!;
  l2!.y -= impy*m2!;
  l2!.z -= impz*m2!;
  a2!.x -= impx*ax2x!+impy*ay2x!+impz*az2x!;
  a2!.y -= impx*ax2y!+impy*ay2y!+impz*az2y!;
  a2!.z -= impx*ax2z!+impy*ay2z!+impz*az2z!;
}