solve method

@override void solve()

Solve the constraint. This is usually called iteratively.

Implementation

@override
void solve(){
  tmplv1.setFrom( lv1! );
  tmplv2.setFrom( lv2! );
  tmpav1.setFrom( av1! );
  tmpav2.setFrom( av2! );

  double oldImp1, newImp1, oldImp2, newImp2, rvn, norImp, tanImp, binImp, max, len;
  ContactPointDataBuffer c = cs;

  while(true){
    norImp = c.norImp;
    tanImp = c.tanImp;
    binImp = c.binImp;
    max = -norImp * friction!;

    tmp.sub2( tmplv2, tmplv1 );

    rvn = Math.dotVectors( tmp, c.tan ) + Math.dotVectors( tmpav2, c.tanT2 ) - Math.dotVectors( tmpav1, c.tanT1 );

    oldImp1 = tanImp;
    newImp1 = rvn*c.tanDen;
    tanImp += newImp1;

    rvn = Math.dotVectors( tmp, c.bin ) + Math.dotVectors( tmpav2, c.binT2 ) - Math.dotVectors( tmpav1, c.binT1 );

    oldImp2 = binImp;
    newImp2 = rvn*c.binDen;
    binImp += newImp2;

    // cone friction clamp
    len = tanImp*tanImp + binImp*binImp;
    if(len > max * max ){
      len = max/math.sqrt(len);
      tanImp *= len;
      binImp *= len;
    }

    newImp1 = tanImp-oldImp1;
    newImp2 = binImp-oldImp2;

    //
    tmp.setValues(
      c.tanU1.x*newImp1 + c.binU1.x*newImp2,
      c.tanU1.y*newImp1 + c.binU1.y*newImp2,
      c.tanU1.z*newImp1 + c.binU1.z*newImp2
    );

    tmplv1.add( tmp );

    tmp.setValues(
      c.tanTU1.x*newImp1 + c.binTU1.x*newImp2,
      c.tanTU1.y*newImp1 + c.binTU1.y*newImp2,
      c.tanTU1.z*newImp1 + c.binTU1.z*newImp2
    );

    tmpav1.add( tmp );

    tmp.setValues(
      c.tanU2.x*newImp1 + c.binU2.x*newImp2,
      c.tanU2.y*newImp1 + c.binU2.y*newImp2,
      c.tanU2.z*newImp1 + c.binU2.z*newImp2
    );

    tmplv2.sub( tmp );

    tmp.setValues(
        c.tanTU2.x*newImp1 + c.binTU2.x*newImp2,
        c.tanTU2.y*newImp1 + c.binTU2.y*newImp2,
        c.tanTU2.z*newImp1 + c.binTU2.z*newImp2
    );

    tmpav2.sub( tmp );

    // restitution part

    tmp.sub2( tmplv2, tmplv1 );

    rvn = Math.dotVectors( tmp, c.nor ) + Math.dotVectors( tmpav2, c.norT2 ) - Math.dotVectors( tmpav1, c.norT1 );

    oldImp1 = norImp;
    newImp1 = (rvn-c.norTar)*c.norDen;
    norImp += newImp1;
    if( norImp > 0 ){ norImp = 0;}

    newImp1 = norImp - oldImp1;

    tmplv1.addScaled( c.norU1, newImp1 );
    tmpav1.addScaled( c.norTU1, newImp1 );
    tmplv2.subScaledVector( c.norU2, newImp1 );
    tmpav2.subScaledVector( c.norTU2, newImp1 );

    c.norImp = norImp;
    c.tanImp = tanImp;
    c.binImp = binImp;

    if(c.last){break;}
    c = c.next!;
  }

  lv1!.setFrom( tmplv1 );
  lv2!.setFrom( tmplv2 );
  av1!.setFrom( tmpav1 );
  av2!.setFrom( tmpav2 );
}