preSolve method

@override void preSolve(double timeStep, double invTimeStep)

Prepare for solving the constraint

Implementation

@override
void preSolve(double timeStep,double invTimeStep){
  ax=limitMotor.axis.x;
  ay=limitMotor.axis.y;
  az=limitMotor.axis.z;
  lowerLimit=limitMotor.lowerLimit;
  upperLimit=limitMotor.upperLimit;
  motorSpeed=limitMotor.motorSpeed;
  maxMotorForce=limitMotor.maxMotorForce;
  enableMotor=maxMotorForce!>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 frequency=limitMotor.frequency;
  bool enableSpring=frequency>0;
  bool enableLimit=lowerLimit! <= upperLimit!;
  double angle=limitMotor.angle;

  if(enableLimit){
    if(lowerLimit==upperLimit){
      if(limitState!=0){
        limitState=0;
        limitImpulse=0;
      }
      limitVelocity=lowerLimit!-angle;
    }
    else if(angle<lowerLimit!){
      if(limitState!=-1){
        limitState=-1;
        limitImpulse=0;
      }
      limitVelocity=lowerLimit!-angle;
    }
    else if(angle>upperLimit!){
      if(limitState!=1){
        limitState=1;
        limitImpulse=0;
      }
      limitVelocity=upperLimit!-angle;
    }
    else{
      limitState=2;
      limitImpulse=0;
      limitVelocity=0;
    }

    if(!enableSpring){
      if(limitVelocity! > 0.02){
        limitVelocity = limitVelocity!-0.02;
      }
      else if(limitVelocity! < -0.02){
        limitVelocity = limitVelocity!+0.02;
      }
      else{
        limitVelocity=0;
      }
    }
  }
  else{
    limitState=2;
    limitImpulse=0;
  }

  if(enableMotor&&(limitState!=0||enableSpring)){
    maxMotorImpulse=maxMotorForce!*timeStep;
  }
  else{
    motorImpulse=0;
    maxMotorImpulse=0;
  }

  a1x=ax!*i1e00!+ay!*i1e01!+az!*i1e02!;
  a1y=ax!*i1e10!+ay!*i1e11!+az!*i1e12!;
  a1z=ax!*i1e20!+ay!*i1e21!+az!*i1e22!;
  a2x=ax!*i2e00!+ay!*i2e01!+az!*i2e02!;
  a2y=ax!*i2e10!+ay!*i2e11!+az!*i2e12!;
  a2z=ax!*i2e20!+ay!*i2e21!+az!*i2e22!;
  motorDenom=ax!*(a1x!+a2x!)+ay!*(a1y!+a2y!)+az!*(a1z!+a2z!);
  invMotorDenom=1/motorDenom!;

  if(enableSpring&&limitState!=2){
    double omega=6.2831853*frequency;
    double k=omega*omega*timeStep;
    double dmp=invTimeStep/(k+2*limitMotor.dampingRatio*omega);
    cfm=motorDenom!*dmp;
    limitVelocity = limitVelocity!*k*dmp;
  }
  else{
    cfm=0;
    limitVelocity = limitVelocity!*invTimeStep*0.05;
  }

  invDenom=1/(motorDenom!+cfm!);

  limitImpulse*=0.95;
  motorImpulse*=0.95;
  double totalImpulse=limitImpulse+motorImpulse;
  a1.x+=totalImpulse*a1x!;
  a1.y+=totalImpulse*a1y!;
  a1.z+=totalImpulse*a1z!;
  a2.x-=totalImpulse*a2x!;
  a2.y-=totalImpulse*a2y!;
  a2.z-=totalImpulse*a2z!;
}