From 74a120f5544103c3bcdf9f2c3c61f41af7cf3032 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Wed, 30 Nov 2016 01:27:48 -0500
Subject: [PATCH] Handle some divide by zero edge cases on init.

---
 matrix/AxisAngle.hpp  | 16 ++++++++++------
 matrix/Quaternion.hpp | 22 ++++++++++++++--------
 test/attitude.cpp     |  4 ++++
 3 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/matrix/AxisAngle.hpp b/matrix/AxisAngle.hpp
index 8b25bedc48..b3ad7305e3 100644
--- a/matrix/AxisAngle.hpp
+++ b/matrix/AxisAngle.hpp
@@ -78,14 +78,14 @@ public:
         AxisAngle &v = *this;
         Type ang = (Type)2.0f*acosf(q(0));
         Type mag = sinf(ang/2.0f);
-        if (fabsf(ang) < 1e-10f) {
-            v(0) = 1e-10f;
-            v(1) = 0;
-            v(2) = 0;
-        } else {
+        if (fabsf(mag) > 0) {
             v(0) = ang*q(1)/mag;
             v(1) = ang*q(2)/mag;
             v(2) = ang*q(3)/mag;
+        } else {
+            v(0) = 0;
+            v(1) = 0;
+            v(2) = 0;
         }
     }
 
@@ -156,7 +156,11 @@ public:
 
 
     Vector<Type, 3> axis() {
-        return Vector<Type, 3>::unit();
+        if (Vector<Type, 3>::norm() > 0) {
+            return Vector<Type, 3>::unit();
+        } else {
+            return Vector3<Type>(1, 0, 0);
+        }
     }
 
     Type angle() {
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 2e0a768c8a..98f1efddcc 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -94,14 +94,20 @@ public:
         Vector<Type, 4>()
     {
         Quaternion &q = *this;
-        q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
-                                     dcm(1, 1) + dcm(2, 2)));
-        q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
-                    (Type(4) * q(0)));
-        q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
-                    (Type(4) * q(0)));
-        q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
-                    (Type(4) * q(0)));
+        q(0) = 1;
+        q(1) = 0;
+        q(2) = 0;
+        q(3) = 0;
+        if (((Type(1) + dcm(0, 0) + dcm(1, 1) + dcm(2, 2)) > 0) & (fabsf(q(0)) > 0) ) {
+            q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
+                                         dcm(1, 1) + dcm(2, 2)));
+            q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
+                        (Type(4) * q(0)));
+            q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
+                        (Type(4) * q(0)));
+            q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
+                        (Type(4) * q(0)));
+        }
     }
 
     /**
diff --git a/test/attitude.cpp b/test/attitude.cpp
index ea20c3cdcf..c2606b9b00 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -283,6 +283,10 @@ int main()
     AxisAnglef aa_data_init(aa_data);
     TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
 
+    AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f));
+    TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0)));
+    TEST(isEqualF(aa_norm_check.angle(), 0.0f));
+
     q = Quatf(-0.29555112749297824f, 0.25532186f,  0.51064372f,  0.76596558f);
     TEST(isEqual(q.imag(), Vector3f(0.25532186f,  0.51064372f,  0.76596558f)));