Commit fac89758 authored by ndtrung's avatar ndtrung
Browse files

Checked for zero inertia moments in a simpler way, similar to what is done in mq_to_omega()

parent bc5c2676

src/math_extra.cpp

+1 −1
Original line number Diff line number Diff line
@@ -258,7 +258,7 @@ void no_squish_rotate(int k, double *p, double *q, double *inertia, 
  // obtain phi, cosines and sines



  phi = p[0]*kq[0] + p[1]*kq[1] + p[2]*kq[2] + p[3]*kq[3];

  if (fabs(inertia[k-1]) < 1e-250) phi *= 0.0;

  if (inertia[k-1] == 0.0) phi = 0.0;

  else phi /= 4.0 * inertia[k-1];

  c_phi = cos(dt * phi);

  s_phi = sin(dt * phi);

CRA Git | Maintained and supported by SUSTech CRA and CCSE