preSolve method
Prepare for solving the constraint
Implementation
@override
void preSolve(double timeStep,double invTimeStep ){
ax1=limitMotor1.axis.x;
ay1=limitMotor1.axis.y;
az1=limitMotor1.axis.z;
ax2=limitMotor2.axis.x;
ay2=limitMotor2.axis.y;
az2=limitMotor2.axis.z;
ax3=limitMotor3.axis.x;
ay3=limitMotor3.axis.y;
az3=limitMotor3.axis.z;
lowerLimit1=limitMotor1.lowerLimit;
upperLimit1=limitMotor1.upperLimit;
motorSpeed1=limitMotor1.motorSpeed;
maxMotorForce1=limitMotor1.maxMotorForce;
enableMotor1=maxMotorForce1!>0;
lowerLimit2=limitMotor2.lowerLimit;
upperLimit2=limitMotor2.upperLimit;
motorSpeed2=limitMotor2.motorSpeed;
maxMotorForce2=limitMotor2.maxMotorForce;
enableMotor2=maxMotorForce2!>0;
lowerLimit3=limitMotor3.lowerLimit;
upperLimit3=limitMotor3.upperLimit;
motorSpeed3=limitMotor3.motorSpeed;
maxMotorForce3=limitMotor3.maxMotorForce;
enableMotor3=maxMotorForce3!>0;
Float32List ti1 = i1.storage;
Float32List ti2 = i2.storage;
i1e00=ti1[0];
i1e01=ti1[1];
i1e02=ti1[2];
i1e10=ti1[3];
i1e11=ti1[4];
i1e12=ti1[5];
i1e20=ti1[6];
i1e21=ti1[7];
i1e22=ti1[8];
i2e00=ti2[0];
i2e01=ti2[1];
i2e02=ti2[2];
i2e10=ti2[3];
i2e11=ti2[4];
i2e12=ti2[5];
i2e20=ti2[6];
i2e21=ti2[7];
i2e22=ti2[8];
int frequency1=limitMotor1.frequency;
int frequency2=limitMotor2.frequency;
int frequency3=limitMotor3.frequency;
bool enableSpring1=frequency1>0;
bool enableSpring2=frequency2>0;
bool enableSpring3=frequency3>0;
bool enableLimit1=lowerLimit1!<=upperLimit1!;
bool enableLimit2=lowerLimit2!<=upperLimit2!;
bool enableLimit3=lowerLimit3!<=upperLimit3!;
double angle1=limitMotor1.angle;
if(enableLimit1){
if(lowerLimit1==upperLimit1){
if(limitState1!=0){
limitState1=0;
limitImpulse1=0;
}
limitVelocity1=lowerLimit1!-angle1;
}
else if(angle1<lowerLimit1!){
if(limitState1!=-1){
limitState1=-1;
limitImpulse1=0;
}
limitVelocity1=lowerLimit1!-angle1;
}
else if(angle1>upperLimit1!){
if(limitState1!=1){
limitState1=1;
limitImpulse1=0;
}
limitVelocity1=upperLimit1!-angle1;
}
else{
limitState1=2;
limitImpulse1=0;
limitVelocity1=0;
}
if(!enableSpring1){
if(limitVelocity1>0.02){
limitVelocity1 = limitVelocity1-0.02;
}
else if(limitVelocity1<-0.02){
limitVelocity1 = limitVelocity1+0.02;
}
else {
limitVelocity1=0;
}
}
}
else{
limitState1=2;
limitImpulse1=0;
}
double angle2=limitMotor2.angle;
if(enableLimit2){
if(lowerLimit2==upperLimit2){
if(limitState2!=0){
limitState2=0;
limitImpulse2=0;
}
limitVelocity2=lowerLimit2!-angle2;
}
else if(angle2<lowerLimit2!){
if(limitState2!=-1){
limitState2=-1;
limitImpulse2=0;
}
limitVelocity2=lowerLimit2!-angle2;
}
else if(angle2>upperLimit2!){
if(limitState2!=1){
limitState2=1;
limitImpulse2=0;
}
limitVelocity2=upperLimit2!-angle2;
}
else{
limitState2=2;
limitImpulse2=0;
limitVelocity2=0;
}
if(!enableSpring2){
if(limitVelocity2>0.02){
limitVelocity2=limitVelocity2-0.02;
}
else if(limitVelocity2<-0.02){
limitVelocity2=limitVelocity2+0.02;
}
else {
limitVelocity2=0;
}
}
}
else{
limitState2=2;
limitImpulse2=0;
}
var angle3=limitMotor3.angle;
if(enableLimit3){
if(lowerLimit3==upperLimit3){
if(limitState3!=0){
limitState3=0;
limitImpulse3=0;
}
limitVelocity3=lowerLimit3!-angle3;
}
else if(angle3<lowerLimit3!){
if(limitState3!=-1){
limitState3=-1;
limitImpulse3=0;
}
limitVelocity3=lowerLimit3!-angle3;
}
else if(angle3>upperLimit3!){
if(limitState3!=1){
limitState3=1;
limitImpulse3=0;
}
limitVelocity3=upperLimit3!-angle3;
}
else{
limitState3=2;
limitImpulse3=0;
limitVelocity3=0;
}
if(!enableSpring3){
if(limitVelocity3>0.02){
limitVelocity3=limitVelocity3-0.02;
}
else if(limitVelocity3<-0.02){
limitVelocity3=limitVelocity3+0.02;
}
else {
limitVelocity3=0;
}
}
}
else{
limitState3=2;
limitImpulse3=0;
}
if(enableMotor1&&(limitState1!=0||enableSpring1)){
maxMotorImpulse1=maxMotorForce1!*timeStep;
}
else{
motorImpulse1=0;
maxMotorImpulse1=0;
}
if(enableMotor2&&(limitState2!=0||enableSpring2)){
maxMotorImpulse2=maxMotorForce2!*timeStep;
}
else{
motorImpulse2=0;
maxMotorImpulse2=0;
}
if(enableMotor3&&(limitState3!=0||enableSpring3)){
maxMotorImpulse3=maxMotorForce3!*timeStep;
}
else{
motorImpulse3=0;
maxMotorImpulse3=0;
}
// build jacobians
a1x1=ax1!*i1e00!+ay1!*i1e01!+az1!*i1e02!;
a1y1=ax1!*i1e10!+ay1!*i1e11!+az1!*i1e12!;
a1z1=ax1!*i1e20!+ay1!*i1e21!+az1!*i1e22!;
a2x1=ax1!*i2e00!+ay1!*i2e01!+az1!*i2e02!;
a2y1=ax1!*i2e10!+ay1!*i2e11!+az1!*i2e12!;
a2z1=ax1!*i2e20!+ay1!*i2e21!+az1!*i2e22!;
a1x2=ax2!*i1e00!+ay2!*i1e01!+az2!*i1e02!;
a1y2=ax2!*i1e10!+ay2!*i1e11!+az2!*i1e12!;
a1z2=ax2!*i1e20!+ay2!*i1e21!+az2!*i1e22!;
a2x2=ax2!*i2e00!+ay2!*i2e01!+az2!*i2e02!;
a2y2=ax2!*i2e10!+ay2!*i2e11!+az2!*i2e12!;
a2z2=ax2!*i2e20!+ay2!*i2e21!+az2!*i2e22!;
a1x3=ax3!*i1e00!+ay3!*i1e01!+az3!*i1e02!;
a1y3=ax3!*i1e10!+ay3!*i1e11!+az3!*i1e12!;
a1z3=ax3!*i1e20!+ay3!*i1e21!+az3!*i1e22!;
a2x3=ax3!*i2e00!+ay3!*i2e01!+az3!*i2e02!;
a2y3=ax3!*i2e10!+ay3!*i2e11!+az3!*i2e12!;
a2z3=ax3!*i2e20!+ay3!*i2e21!+az3!*i2e22!;
// build an impulse matrix
k00=ax1!*(a1x1!+a2x1!)+ay1!*(a1y1!+a2y1!)+az1!*(a1z1!+a2z1!);
k01=ax1!*(a1x2!+a2x2!)+ay1!*(a1y2!+a2y2!)+az1!*(a1z2!+a2z2!);
k02=ax1!*(a1x3!+a2x3!)+ay1!*(a1y3!+a2y3!)+az1!*(a1z3!+a2z3!);
k10=ax2!*(a1x1!+a2x1!)+ay2!*(a1y1!+a2y1!)+az2!*(a1z1!+a2z1!);
k11=ax2!*(a1x2!+a2x2!)+ay2!*(a1y2!+a2y2!)+az2!*(a1z2!+a2z2!);
k12=ax2!*(a1x3!+a2x3!)+ay2!*(a1y3!+a2y3!)+az2!*(a1z3!+a2z3!);
k20=ax3!*(a1x1!+a2x1!)+ay3!*(a1y1!+a2y1!)+az3!*(a1z1!+a2z1!);
k21=ax3!*(a1x2!+a2x2!)+ay3!*(a1y2!+a2y2!)+az3!*(a1z2!+a2z2!);
k22=ax3!*(a1x3!+a2x3!)+ay3!*(a1y3!+a2y3!)+az3!*(a1z3!+a2z3!);
kv00=k00;
kv11=k11;
kv22=k22;
dv00=1/kv00!;
dv11=1/kv11!;
dv22=1/kv22!;
if(enableSpring1&&limitState1!=2){
double omega=6.2831853*frequency1;
double k=omega*omega*timeStep;
double dmp=invTimeStep/(k+2*limitMotor1.dampingRatio*omega);
cfm1=kv00!*dmp;
limitVelocity1 = limitVelocity1*k*dmp;
}
else{
cfm1=0;
limitVelocity1 = limitVelocity1*invTimeStep*0.05;
}
if(enableSpring2&&limitState2!=2){
double omega=6.2831853*frequency2;
double k=omega*omega*timeStep;
double dmp=invTimeStep/(k+2*limitMotor2.dampingRatio*omega);
cfm2=kv11!*dmp;
limitVelocity2 = limitVelocity2*k*dmp;
}
else{
cfm2=0;
limitVelocity2 = limitVelocity2*invTimeStep*0.05;
}
if(enableSpring3&&limitState3!=2){
double omega=6.2831853*frequency3;
double k=omega*omega*timeStep;
double dmp=invTimeStep/(k+2*limitMotor3.dampingRatio*omega);
cfm3=kv22!*dmp;
limitVelocity3 = limitVelocity3*k*dmp;
}
else{
cfm3=0;
limitVelocity3 = limitVelocity3*invTimeStep*0.05;
}
k00=k00!+cfm1!;
k11=k11!+cfm2!;
k22=k22!+cfm3!;
double inv=1/(
k00!*(k11!*k22!-k21!*k12!)+
k10!*(k21!*k02!-k01!*k22!)+
k20!*(k01!*k12!-k11!*k02!)
);
d00=(k11!*k22!-k12!*k21!)*inv;
d01=(k02!*k21!-k01!*k22!)*inv;
d02=(k01!*k12!-k02!*k11!)*inv;
d10=(k12!*k20!-k10!*k22!)*inv;
d11=(k00!*k22!-k02!*k20!)*inv;
d12=(k02!*k10!-k00!*k12!)*inv;
d20=(k10!*k21!-k11!*k20!)*inv;
d21=(k01!*k20!-k00!*k21!)*inv;
d22=(k00!*k11!-k01!*k10!)*inv;
limitImpulse1*=0.95;
motorImpulse1*=0.95;
limitImpulse2*=0.95;
motorImpulse2*=0.95;
limitImpulse3*=0.95;
motorImpulse3*=0.95;
double totalImpulse1=limitImpulse1+motorImpulse1;
double totalImpulse2=limitImpulse2+motorImpulse2;
double totalImpulse3=limitImpulse3+motorImpulse3;
a1.x+=totalImpulse1*a1x1!+totalImpulse2*a1x2!+totalImpulse3*a1x3!;
a1.y+=totalImpulse1*a1y1!+totalImpulse2*a1y2!+totalImpulse3*a1y3!;
a1.z+=totalImpulse1*a1z1!+totalImpulse2*a1z2!+totalImpulse3*a1z3!;
a2.x-=totalImpulse1*a2x1!+totalImpulse2*a2x2!+totalImpulse3*a2x3!;
a2.y-=totalImpulse1*a2y1!+totalImpulse2*a2y2!+totalImpulse3*a2y3!;
a2.z-=totalImpulse1*a2z1!+totalImpulse2*a2z2!+totalImpulse3*a2z3!;
}