solve_ method

void solve_()

Implementation

void solve_(){
  double rvx=a2.x-a1.x;
  double rvy=a2.y-a1.y;
  double rvz=a2.z-a1.z;

  limitVelocity3=30;
  double rvn1=rvx*ax1!+rvy*ay1!+rvz*az1!-limitVelocity1;
  double rvn2=rvx*ax2!+rvy*ay2!+rvz*az2!-limitVelocity2;
  double rvn3=rvx*ax3!+rvy*ay3!+rvz*az3!-limitVelocity3;

  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;

  a1.x+=dLimitImpulse1*a1x1!+dLimitImpulse2*a1x2!+dLimitImpulse3*a1x3!;
  a1.y+=dLimitImpulse1*a1y1!+dLimitImpulse2*a1y2!+dLimitImpulse3*a1y3!;
  a1.z+=dLimitImpulse1*a1z1!+dLimitImpulse2*a1z2!+dLimitImpulse3*a1z3!;
  a2.x-=dLimitImpulse1*a2x1!+dLimitImpulse2*a2x2!+dLimitImpulse3*a2x3!;
  a2.y-=dLimitImpulse1*a2y1!+dLimitImpulse2*a2y2!+dLimitImpulse3*a2y3!;
  a2.z-=dLimitImpulse1*a2z1!+dLimitImpulse2*a2z2!+dLimitImpulse3*a2z3!;
}