solve method

@override void solve()

Solve the constraint. This is usually called iteratively.

Implementation

@override
void solve(){
  double rvx=a2.x-a1.x;
  double rvy=a2.y-a1.y;
  double rvz=a2.z-a1.z;

  double rvn1=rvx*ax1!+rvy*ay1!+rvz*az1!;
  double rvn2=rvx*ax2!+rvy*ay2!+rvz*az2!;
  double rvn3=rvx*ax3!+rvy*ay3!+rvz*az3!;

  double oldMotorImpulse1=motorImpulse1;
  double oldMotorImpulse2=motorImpulse2;
  double oldMotorImpulse3=motorImpulse3;

  double dMotorImpulse1=0;
  double dMotorImpulse2=0;
  double dMotorImpulse3=0;

  if(enableMotor1){
    dMotorImpulse1=(rvn1-motorSpeed1!)*dv00!;
    motorImpulse1+=dMotorImpulse1;
    if(motorImpulse1>maxMotorImpulse1!){ // clamp motor impulse
      motorImpulse1=maxMotorImpulse1!;
    }
    else if(motorImpulse1<-maxMotorImpulse1!){
      motorImpulse1=-maxMotorImpulse1!;
    }
    dMotorImpulse1=motorImpulse1-oldMotorImpulse1;
  }
  if(enableMotor2){
    dMotorImpulse2=(rvn2-motorSpeed2!)*dv11!;
    motorImpulse2+=dMotorImpulse2;
    if(motorImpulse2>maxMotorImpulse2!){ // clamp motor impulse
      motorImpulse2=maxMotorImpulse2!;
    }
    else if(motorImpulse2<-maxMotorImpulse2!){
      motorImpulse2=-maxMotorImpulse2!;
    }
    dMotorImpulse2=motorImpulse2-oldMotorImpulse2;
  }
  if(enableMotor3){
    dMotorImpulse3=(rvn3-motorSpeed3!)*dv22!;
    motorImpulse3+=dMotorImpulse3;
    if(motorImpulse3>maxMotorImpulse3!){ // clamp motor impulse
      motorImpulse3=maxMotorImpulse3!;
    }
    else if(motorImpulse3<-maxMotorImpulse3!){
      motorImpulse3=-maxMotorImpulse3!;
    }
    dMotorImpulse3=motorImpulse3-oldMotorImpulse3;
  }

  // apply motor impulse to relative velocity
  rvn1+=dMotorImpulse1*kv00!+dMotorImpulse2*k01!+dMotorImpulse3*k02!;
  rvn2+=dMotorImpulse1*k10!+dMotorImpulse2*kv11!+dMotorImpulse3*k12!;
  rvn3+=dMotorImpulse1*k20!+dMotorImpulse2*k21!+dMotorImpulse3*kv22!;

  // subtract target velocity and applied impulse
  rvn1-=limitVelocity1+limitImpulse1*cfm1!;
  rvn2-=limitVelocity2+limitImpulse2*cfm2!;
  rvn3-=limitVelocity3+limitImpulse3*cfm3!;

  double oldLimitImpulse1=limitImpulse1;
  double oldLimitImpulse2=limitImpulse2;
  double oldLimitImpulse3=limitImpulse3;

  double dLimitImpulse1=rvn1*d00!+rvn2*d01!+rvn3*d02!;
  double dLimitImpulse2=rvn1*d10!+rvn2*d11!+rvn3*d12!;
  double dLimitImpulse3=rvn1*d20!+rvn2*d21!+rvn3*d22!;

  limitImpulse1+=dLimitImpulse1;
  limitImpulse2+=dLimitImpulse2;
  limitImpulse3+=dLimitImpulse3;

  // clamp
  int clampState=0;
  if(limitState1==2||limitImpulse1*limitState1<0){
    dLimitImpulse1=-oldLimitImpulse1;
    rvn2+=dLimitImpulse1*k10!;
    rvn3+=dLimitImpulse1*k20!;
    clampState|=1;
  }
  if(limitState2==2||limitImpulse2*limitState2<0){
    dLimitImpulse2=-oldLimitImpulse2;
    rvn1+=dLimitImpulse2*k01!;
    rvn3+=dLimitImpulse2*k21!;
    clampState|=2;
  }
  if(limitState3==2||limitImpulse3*limitState3<0){
    dLimitImpulse3=-oldLimitImpulse3;
    rvn1+=dLimitImpulse3*k02!;
    rvn2+=dLimitImpulse3*k12!;
    clampState|=4;
  }

  // update un-clamped impulse
  // dart(todo) isolate division
  double det;
  switch(clampState){
    case 1: // update 2 3
      det=1/(k11!*k22!-k12!*k21!);
      dLimitImpulse2=(k22!*rvn2+-k12!*rvn3)*det;
      dLimitImpulse3=(-k21!*rvn2+k11!*rvn3)*det;
      break;
    case 2: // update 1 3
      det=1/(k00!*k22!-k02!*k20!);
      dLimitImpulse1=(k22!*rvn1+-k02!*rvn3)*det;
      dLimitImpulse3=(-k20!*rvn1+k00!*rvn3)*det;
      break;
    case 3: // update 3
      dLimitImpulse3=rvn3/k22!;
      break;
    case 4: // update 1 2
      det=1/(k00!*k11!-k01!*k10!);
      dLimitImpulse1=(k11!*rvn1+-k01!*rvn2)*det;
      dLimitImpulse2=(-k10!*rvn1+k00!*rvn2)*det;
      break;
    case 5: // update 2
      dLimitImpulse2=rvn2/k11!;
      break;
    case 6: // update 1
      dLimitImpulse1=rvn1/k00!;
      break;
  }

  limitImpulse1=dLimitImpulse1+oldLimitImpulse1;
  limitImpulse2=dLimitImpulse2+oldLimitImpulse2;
  limitImpulse3=dLimitImpulse3+oldLimitImpulse3;

  double dImpulse1=dMotorImpulse1+dLimitImpulse1;
  double dImpulse2=dMotorImpulse2+dLimitImpulse2;
  double dImpulse3=dMotorImpulse3+dLimitImpulse3;

  // apply impulse
  a1.x+=dImpulse1*a1x1!+dImpulse2*a1x2!+dImpulse3*a1x3!;
  a1.y+=dImpulse1*a1y1!+dImpulse2*a1y2!+dImpulse3*a1y3!;
  a1.z+=dImpulse1*a1z1!+dImpulse2*a1z2!+dImpulse3*a1z3!;
  a2.x-=dImpulse1*a2x1!+dImpulse2*a2x2!+dImpulse3*a2x3!;
  a2.y-=dImpulse1*a2y1!+dImpulse2*a2y2!+dImpulse3*a2y3!;
  a2.z-=dImpulse1*a2z1!+dImpulse2*a2z2!+dImpulse3*a2z3!;
  rvx=a2.x-a1.x;
  rvy=a2.y-a1.y;
  rvz=a2.z-a1.z;

  rvn2=rvx*ax2!+rvy*ay2!+rvz*az2!;
}