solve method

@override void solve()

Solve the constraint. This is usually called iteratively.

Implementation

@override
void solve(){
  double rvn=ax!*(a2.x-a1.x)+ay!*(a2.y-a1.y)+az!*(a2.z-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;
  a1.x+=totalImpulse*a1x!;
  a1.y+=totalImpulse*a1y!;
  a1.z+=totalImpulse*a1z!;
  a2.x-=totalImpulse*a2x!;
  a2.y-=totalImpulse*a2y!;
  a2.z-=totalImpulse*a2z!;
}