solve method
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!;
}