preSolve method

@override void preSolve(double timeStep, double invTimeStep)

Prepare for solving the constraint

Implementation

@override
void preSolve(double timeStep,double invTimeStep ){
  m1 = body1!.inverseMass;
  m2 = body2!.inverseMass;

  double m1m2 = m1! + m2!;

  num = manifold.numPoints.toDouble();

  ContactPointDataBuffer? c = cs;
  ManifoldPoint p;
  double rvn, len, norImp, norTar, sepV;
  Matrix3 i1, i2;

  for(int i=0; i < num; i++){
    p = ps[i];

    tmpP1.sub2( p.position, p1! );
    tmpP2.sub2( p.position, p2! );

    tmpC1.cross2(av1!, tmpP1 );
    tmpC2.cross2(av2!, tmpP2 );

    c!.norImp = p.normalImpulse;
    c.tanImp = p.tangentImpulse;
    c.binImp = p.binormalImpulse;

    c.nor.setFrom( p.normal );

    tmp.setValues(
      ( lv2!.x + tmpC2.x ) - ( lv1!.x + tmpC1.x ),
      ( lv2!.y + tmpC2.y ) - ( lv1!.y + tmpC1.y ),
      ( lv2!.z + tmpC2.z ) - ( lv1!.z + tmpC1.z )
    );

    rvn = Math.dotVectors( c.nor, tmp );

    c.tan.setValues(
      tmp.x - rvn * c.nor.x,
      tmp.y - rvn * c.nor.y,
      tmp.z - rvn * c.nor.z
    );

    len = Math.dotVectors( c.tan, c.tan );

    if( len <= 0.04 ) {
      c.tan.tangent( c.nor );
    }

    c.tan.normalize();

    c.bin.cross2( c.nor, c.tan );

    c.norU1.scale2( c.nor, m1! );
    c.norU2.scale2( c.nor, m2! );

    c.tanU1.scale2( c.tan, m1! );
    c.tanU2.scale2( c.tan, m2! );

    c.binU1.scale2( c.bin, m1! );
    c.binU2.scale2( c.bin, m2! );

    c.norT1.cross2( tmpP1, c.nor );
    c.tanT1.cross2( tmpP1, c.tan );
    c.binT1.cross2( tmpP1, c.bin );

    c.norT2.cross2( tmpP2, c.nor );
    c.tanT2.cross2( tmpP2, c.tan );
    c.binT2.cross2( tmpP2, c.bin );

    i1 = this.i1!;
    i2 = this.i2!;

    c.norTU1..setFrom( c.norT1 )..applyMatrix3Transpose( i1 );
    c.tanTU1..setFrom( c.tanT1 )..applyMatrix3Transpose( i1 );
    c.binTU1..setFrom( c.binT1 )..applyMatrix3Transpose( i1 );

    c.norTU2..setFrom( c.norT2 )..applyMatrix3Transpose( i2 );
    c.tanTU2..setFrom( c.tanT2 )..applyMatrix3Transpose( i2 );
    c.binTU2..setFrom( c.binT2 )..applyMatrix3Transpose( i2 );

    tmpC1.cross2( c.norTU1, tmpP1 );
    tmpC2.cross2( c.norTU2, tmpP2 );
    tmp.add2( tmpC1, tmpC2 );
    c.norDen = 1 / ( m1m2 +Math.dotVectors( c.nor, tmp ));

    tmpC1.cross2( c.tanTU1, tmpP1 );
    tmpC2.cross2( c.tanTU2, tmpP2 );
    tmp.add2( tmpC1, tmpC2 );
    c.tanDen = 1 / ( m1m2 +Math.dotVectors( c.tan, tmp ));

    tmpC1.cross2( c.binTU1, tmpP1 );
    tmpC2.cross2( c.binTU2, tmpP2 );
    tmp.add2( tmpC1, tmpC2 );
    c.binDen = 1 / ( m1m2 +Math.dotVectors( c.bin, tmp ));

    if( p.warmStarted ){
      norImp = p.normalImpulse;
      lv1!.addScaled( c.norU1, norImp );
      av1!.addScaled( c.norTU1, norImp );
      lv2!.subScaledVector( c.norU2, norImp );
      av2!.subScaledVector( c.norTU2, norImp );
      c.norImp = norImp;
      c.tanImp = 0;
      c.binImp = 0;
      rvn = 0; // disable bouncing
    }
    else {
      c.norImp=0;
      c.tanImp=0;
      c.binImp=0;
    }

    if(rvn>-1) rvn=0; // disable bouncing

    norTar = restitution!*-rvn;
    sepV = -(p.penetration+0.005)*invTimeStep*0.05; // allow 0.5cm error
    if(norTar<sepV) norTar=sepV;
    c.norTar = norTar;
    c.last = i==num-1;
    c = c.next;
  }
}