From bc5c2676303e9027cd68ae56bf7f2718a6414ea8 Mon Sep 17 00:00:00 2001
From: Trung Nguyen <ndactrung@gmail.com>
Date: Mon, 25 May 2020 23:37:49 -0500
Subject: [PATCH] Reverted to using a threshold much smaller than 1e-6 for zero
 inertia moments

---
 src/math_extra.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/math_extra.cpp b/src/math_extra.cpp
index 0fe04b6d43..e600177eb8 100644
--- a/src/math_extra.cpp
+++ b/src/math_extra.cpp
@@ -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);