solve method

@override void solve()

Solve the constraint. This is usually called iteratively.

Implementation

@override
void solve(){
  double rvn=
      ax!*(l2.x-l1.x)+ay!*(l2.y-l1.y)+az!*(l2.z-l1.z)+
      t2x!*a2.x-t1x!*a1.x+t2y!*a2.y-t1y!*a1.y+t2z!*a2.z-t1z!*a1.z;

  // motor part
  double newMotorImpulse;
  if(enableMotor){
    newMotorImpulse=(rvn-motorSpeed!)*invMotorDenom!;
    double oldMotorImpulse=motorImpulse;
    motorImpulse+=newMotorImpulse;
    if(motorImpulse>maxMotorImpulse!){
      motorImpulse=maxMotorImpulse!;
    }
    else if(motorImpulse<-maxMotorImpulse!){
      motorImpulse=-maxMotorImpulse!;
    }
    newMotorImpulse=motorImpulse-oldMotorImpulse;
    rvn-=newMotorImpulse*motorDenom!;
  }
  else {
    newMotorImpulse=0;
  }

  // limit part
  double newLimitImpulse;
  if(limitState!=2){
    newLimitImpulse=(rvn-limitVelocity!-limitImpulse*cfm!)*invDenom!;
    double oldLimitImpulse=limitImpulse;
    limitImpulse+=newLimitImpulse;
    if(limitImpulse*limitState<0){
      limitImpulse=0;
    }
    newLimitImpulse=limitImpulse-oldLimitImpulse;
  }
  else {
    newLimitImpulse=0;
  }

  double totalImpulse=newLimitImpulse+newMotorImpulse;
  l1.x+=totalImpulse*l1x!;
  l1.y+=totalImpulse*l1y!;
  l1.z+=totalImpulse*l1z!;
  a1.x+=totalImpulse*a1x!;
  a1.y+=totalImpulse*a1y!;
  a1.z+=totalImpulse*a1z!;
  l2.x-=totalImpulse*l2x!;
  l2.y-=totalImpulse*l2y!;
  l2.z-=totalImpulse*l2z!;
  a2.x-=totalImpulse*a2x!;
  a2.y-=totalImpulse*a2y!;
  a2.z-=totalImpulse*a2z!;
}