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

This commit is contained in:
ndtrung 2020-05-30 07:42:34 -05:00
parent bc5c267630
commit fac897587f
1 changed files with 1 additions and 1 deletions

View File

@ -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);