solve method

@override void solve()

Solve the constraint. This is usually called iteratively.

Implementation

@override
void solve(){
  double rvx=l2.x-l1.x+a2.y*r2z!-a2.z*r2y!-a1.y*r1z!+a1.z*r1y!;
  double rvy=l2.y-l1.y+a2.z*r2x!-a2.x*r2z!-a1.z*r1x!+a1.x*r1z!;
  double rvz=l2.z-l1.z+a2.x*r2y!-a2.y*r2x!-a1.x*r1y!+a1.y*r1x!;
  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=oldLimitImpulse1+dLimitImpulse1;
  limitImpulse2=oldLimitImpulse2+dLimitImpulse2;
  limitImpulse3=oldLimitImpulse3+dLimitImpulse3;

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

  // apply impulse
  l1.x+=dImpulse1*l1x1!+dImpulse2*l1x2!+dImpulse3*l1x3!;
  l1.y+=dImpulse1*l1y1!+dImpulse2*l1y2!+dImpulse3*l1y3!;
  l1.z+=dImpulse1*l1z1!+dImpulse2*l1z2!+dImpulse3*l1z3!;
  a1.x+=dImpulse1*a1x1!+dImpulse2*a1x2!+dImpulse3*a1x3!;
  a1.y+=dImpulse1*a1y1!+dImpulse2*a1y2!+dImpulse3*a1y3!;
  a1.z+=dImpulse1*a1z1!+dImpulse2*a1z2!+dImpulse3*a1z3!;
  l2.x-=dImpulse1*l2x1!+dImpulse2*l2x2!+dImpulse3*l2x3!;
  l2.y-=dImpulse1*l2y1!+dImpulse2*l2y2!+dImpulse3*l2y3!;
  l2.z-=dImpulse1*l2z1!+dImpulse2*l2z2!+dImpulse3*l2z3!;
  a2.x-=dImpulse1*a2x1!+dImpulse2*a2x2!+dImpulse3*a2x3!;
  a2.y-=dImpulse1*a2y1!+dImpulse2*a2y2!+dImpulse3*a2y3!;
  a2.z-=dImpulse1*a2z1!+dImpulse2*a2z2!+dImpulse3*a2z3!;
}