Reverted to using a threshold much smaller than 1e-6 for zero inertia moments

This commit is contained in:
Trung Nguyen
2020-05-25 23:37:49 -05:00
parent 093f981719
commit bc5c267630

View File

@ -258,8 +258,8 @@ 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];
phi /= (4.0 * inertia[k-1]);
if (!std::isfinite(phi)) phi = 0.0;
if (fabs(inertia[k-1]) < 1e-250) phi *= 0.0;
else phi /= 4.0 * inertia[k-1];
c_phi = cos(dt * phi);
s_phi = sin(dt * phi);