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;

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