preSolve method

  1. @override
void preSolve(
  1. double timeStep,
  2. double invTimeStep
)
override

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;
  m1=b1.inverseMass;
  m2=b2.inverseMass;

  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];

  double dx=p2.x-p1.x;
  double dy=p2.y-p1.y;
  double dz=p2.z-p1.z;
  double d1=dx*ax1!+dy*ay1!+dz*az1!;
  double d2=dx*ax2!+dy*ay2!+dz*az2!;
  double d3=dx*ax3!+dy*ay3!+dz*az3!;
  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!;

  // for stability
  if(enableSpring1&&d1>20||d1<-20){
    enableSpring1=false;
  }
  if(enableSpring2&&d2>20||d2<-20){
    enableSpring2=false;
  }
  if(enableSpring3&&d3>20||d3<-20){
    enableSpring3=false;
  }

  if(enableLimit1){
    if(lowerLimit1==upperLimit1){
      if(limitState1!=0){
        limitState1=0;
        limitImpulse1=0;
      }
      limitVelocity1=lowerLimit1!-d1;
      if(!enableSpring1)d1=lowerLimit1!;
    }
    else if(d1<lowerLimit1!){
      if(limitState1!=-1){
        limitState1=-1;
        limitImpulse1=0;
      }
      limitVelocity1=lowerLimit1!-d1;
      if(!enableSpring1)d1=lowerLimit1!;
    }
    else if(d1>upperLimit1!){
      if(limitState1!=1){
        limitState1=1;
        limitImpulse1=0;
      }
      limitVelocity1=upperLimit1!-d1;
      if(!enableSpring1)d1=upperLimit1!;
    }
    else{
      limitState1=2;
      limitImpulse1=0;
      limitVelocity1=0;
    }

    if(!enableSpring1){
      if(limitVelocity1!>0.005){
        limitVelocity1 = limitVelocity1!-0.005;
      }
      else if(limitVelocity1! < -0.005){
        limitVelocity1 = limitVelocity1!+0.005;
      }
      else{
        limitVelocity1=0;
      }
    }
  }
  else{
    limitState1=2;
    limitImpulse1=0;
  }

  if(enableLimit2){
    if(lowerLimit2==upperLimit2){
      if(limitState2!=0){
        limitState2=0;
        limitImpulse2=0;
      }
      limitVelocity2=lowerLimit2!-d2;
      if(!enableSpring2){
        d2=lowerLimit2!;
      }
    }
    else if(d2<lowerLimit2!){
      if(limitState2!=-1){
        limitState2=-1;
        limitImpulse2=0;
      }
      limitVelocity2=lowerLimit2!-d2;
      if(!enableSpring2){
        d2=lowerLimit2!;
      }
    }
    else if(d2>upperLimit2!){
      if(limitState2!=1){
        limitState2=1;
        limitImpulse2=0;
      }
      limitVelocity2=upperLimit2!-d2;
      if(!enableSpring2){
        d2=upperLimit2!;
      }
    }
    else{
      limitState2=2;
      limitImpulse2=0;
      limitVelocity2=0;
    }
    if(!enableSpring2){
      if(limitVelocity2! > 0.005){
        limitVelocity2 = limitVelocity2!-0.005;
      }
      else if(limitVelocity2! < -0.005){
        limitVelocity2 = limitVelocity2!+0.005;
      }
      else{
        limitVelocity2=0;
      }
    }
  }
  else{
    limitState2=2;
    limitImpulse2=0;
  }

  if(enableLimit3){
    if(lowerLimit3==upperLimit3){
      if(limitState3!=0){
        limitState3=0;
        limitImpulse3=0;
      }
      limitVelocity3=lowerLimit3!-d3;
      if(!enableSpring3){
        d3=lowerLimit3!;
      }
    }
    else if(d3<lowerLimit3!){
      if(limitState3!=-1){
        limitState3=-1;
        limitImpulse3=0;
      }
      limitVelocity3=lowerLimit3!-d3;
      if(!enableSpring3){
        d3=lowerLimit3!;
      }
    }
    else if(d3>upperLimit3!){
      if(limitState3!=1){
        limitState3=1;
        limitImpulse3=0;
      }
      limitVelocity3=upperLimit3!-d3;
      if(!enableSpring3)d3=upperLimit3!;
    }
    else{
      limitState3=2;
      limitImpulse3=0;
      limitVelocity3=0;
    }

    if(!enableSpring3){
      if(limitVelocity3! > 0.005){
        limitVelocity3 = limitVelocity3!-0.005;
      }
      else if(limitVelocity3! < -0.005){
        limitVelocity3 = limitVelocity3!+0.005;
      }
      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;
  }

  double rdx=d1*ax1!+d2*ax2!+d3*ax2!;
  double rdy=d1*ay1!+d2*ay2!+d3*ay2!;
  double rdz=d1*az1!+d2*az2!+d3*az2!;
  double w1=m2!/(m1!+m2!);
  if(weight>=0){
    w1=weight; // use given weight
  }
  double w2=1-w1;
  r1x=r1.x+rdx*w1;
  r1y=r1.y+rdy*w1;
  r1z=r1.z+rdz*w1;
  r2x=r2.x-rdx*w2;
  r2y=r2.y-rdy*w2;
  r2z=r2.z-rdz*w2;

  // build jacobians
  t1x1=r1y!*az1!-r1z!*ay1!;
  t1y1=r1z!*ax1!-r1x!*az1!;
  t1z1=r1x!*ay1!-r1y!*ax1!;
  t2x1=r2y!*az1!-r2z!*ay1!;
  t2y1=r2z!*ax1!-r2x!*az1!;
  t2z1=r2x!*ay1!-r2y!*ax1!;
  l1x1=ax1!*m1!;
  l1y1=ay1!*m1!;
  l1z1=az1!*m1!;
  l2x1=ax1!*m2!;
  l2y1=ay1!*m2!;
  l2z1=az1!*m2!;
  a1x1=t1x1!*i1e00!+t1y1!*i1e01!+t1z1!*i1e02!;
  a1y1=t1x1!*i1e10!+t1y1!*i1e11!+t1z1!*i1e12!;
  a1z1=t1x1!*i1e20!+t1y1!*i1e21!+t1z1!*i1e22!;
  a2x1=t2x1!*i2e00!+t2y1!*i2e01!+t2z1!*i2e02!;
  a2y1=t2x1!*i2e10!+t2y1!*i2e11!+t2z1!*i2e12!;
  a2z1=t2x1!*i2e20!+t2y1!*i2e21!+t2z1!*i2e22!;

  t1x2=r1y!*az2!-r1z!*ay2!;
  t1y2=r1z!*ax2!-r1x!*az2!;
  t1z2=r1x!*ay2!-r1y!*ax2!;
  t2x2=r2y!*az2!-r2z!*ay2!;
  t2y2=r2z!*ax2!-r2x!*az2!;
  t2z2=r2x!*ay2!-r2y!*ax2!;
  l1x2=ax2!*m1!;
  l1y2=ay2!*m1!;
  l1z2=az2!*m1!;
  l2x2=ax2!*m2!;
  l2y2=ay2!*m2!;
  l2z2=az2!*m2!;
  a1x2=t1x2!*i1e00!+t1y2!*i1e01!+t1z2!*i1e02!;
  a1y2=t1x2!*i1e10!+t1y2!*i1e11!+t1z2!*i1e12!;
  a1z2=t1x2!*i1e20!+t1y2!*i1e21!+t1z2!*i1e22!;
  a2x2=t2x2!*i2e00!+t2y2!*i2e01!+t2z2!*i2e02!;
  a2y2=t2x2!*i2e10!+t2y2!*i2e11!+t2z2!*i2e12!;
  a2z2=t2x2!*i2e20!+t2y2!*i2e21!+t2z2!*i2e22!;

  t1x3=r1y!*az3!-r1z!*ay3!;
  t1y3=r1z!*ax3!-r1x!*az3!;
  t1z3=r1x!*ay3!-r1y!*ax3!;
  t2x3=r2y!*az3!-r2z!*ay3!;
  t2y3=r2z!*ax3!-r2x!*az3!;
  t2z3=r2x!*ay3!-r2y!*ax3!;
  l1x3=ax3!*m1!;
  l1y3=ay3!*m1!;
  l1z3=az3!*m1!;
  l2x3=ax3!*m2!;
  l2y3=ay3!*m2!;
  l2z3=az3!*m2!;
  a1x3=t1x3!*i1e00!+t1y3!*i1e01!+t1z3!*i1e02!;
  a1y3=t1x3!*i1e10!+t1y3!*i1e11!+t1z3!*i1e12!;
  a1z3=t1x3!*i1e20!+t1y3!*i1e21!+t1z3!*i1e22!;
  a2x3=t2x3!*i2e00!+t2y3!*i2e01!+t2z3!*i2e02!;
  a2y3=t2x3!*i2e10!+t2y3!*i2e11!+t2z3!*i2e12!;
  a2z3=t2x3!*i2e20!+t2y3!*i2e21!+t2z3!*i2e22!;

  // build an impulse matrix
  double m12=m1!+m2!;
  k00=(ax1!*ax1!+ay1!*ay1!+az1!*az1!)*m12;
  k01=(ax1!*ax2!+ay1!*ay2!+az1!*az2!)*m12;
  k02=(ax1!*ax3!+ay1!*ay3!+az1!*az3!)*m12;
  k10=(ax2!*ax1!+ay2!*ay1!+az2!*az1!)*m12;
  k11=(ax2!*ax2!+ay2!*ay2!+az2!*az2!)*m12;
  k12=(ax2!*ax3!+ay2!*ay3!+az2!*az3!)*m12;
  k20=(ax3!*ax1!+ay3!*ay1!+az3!*az1!)*m12;
  k21=(ax3!*ax2!+ay3!*ay2!+az3!*az2!)*m12;
  k22=(ax3!*ax3!+ay3!*ay3!+az3!*az3!)*m12;

  k00= k00!+t1x1!*a1x1!+t1y1!*a1y1!+t1z1!*a1z1!;
  k01= k01!+t1x1!*a1x2!+t1y1!*a1y2!+t1z1!*a1z2!;
  k02= k02!+t1x1!*a1x3!+t1y1!*a1y3!+t1z1!*a1z3!;
  k10= k10!+t1x2!*a1x1!+t1y2!*a1y1!+t1z2!*a1z1!;
  k11= k11!+t1x2!*a1x2!+t1y2!*a1y2!+t1z2!*a1z2!;
  k12= k12!+t1x2!*a1x3!+t1y2!*a1y3!+t1z2!*a1z3!;
  k20= k20!+t1x3!*a1x1!+t1y3!*a1y1!+t1z3!*a1z1!;
  k21= k21!+t1x3!*a1x2!+t1y3!*a1y2!+t1z3!*a1z2!;
  k22= k22!+t1x3!*a1x3!+t1y3!*a1y3!+t1z3!*a1z3!;

  k00= k00!+t2x1!*a2x1!+t2y1!*a2y1!+t2z1!*a2z1!;
  k01= k01!+t2x1!*a2x2!+t2y1!*a2y2!+t2z1!*a2z2!;
  k02= k02!+t2x1!*a2x3!+t2y1!*a2y3!+t2z1!*a2z3!;
  k10= k10!+t2x2!*a2x1!+t2y2!*a2y1!+t2z2!*a2z1!;
  k11= k11!+t2x2!*a2x2!+t2y2!*a2y2!+t2z2!*a2z2!;
  k12= k12!+t2x2!*a2x3!+t2y2!*a2y3!+t2z2!*a2z3!;
  k20= k20!+t2x3!*a2x1!+t2y3!*a2y1!+t2z3!*a2z1!;
  k21= k21!+t2x3!*a2x2!+t2y3!*a2y2!+t2z3!*a2z2!;
  k22= k22!+t2x3!*a2x3!+t2y3!*a2y3!+t2z3!*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;

  // warm starting
  double totalImpulse1=limitImpulse1+motorImpulse1;
  double totalImpulse2=limitImpulse2+motorImpulse2;
  double totalImpulse3=limitImpulse3+motorImpulse3;

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