From fac897587ff4fe5ac5a0ca99d10dbe60edcfe699 Mon Sep 17 00:00:00 2001 From: ndtrung Date: Sat, 30 May 2020 07:42:34 -0500 Subject: [PATCH] Checked for zero inertia moments in a simpler way, similar to what is done in mq_to_omega() --- src/math_extra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/math_extra.cpp b/src/math_extra.cpp index e600177eb8..749736ef27 100644 --- a/src/math_extra.cpp +++ b/src/math_extra.cpp @@ -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);