From 9353e4cb647690b1820eb85c5b6ac055b9cb6377 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Wed, 6 Jul 2016 01:37:50 -0400
Subject: [PATCH] Fix bugs in axis angle.

---
 matrix/AxisAngle.hpp | 16 +++++++++++-----
 test/attitude.cpp    |  2 +-
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/matrix/AxisAngle.hpp b/matrix/AxisAngle.hpp
index 05951e24de..d21e893e6d 100644
--- a/matrix/AxisAngle.hpp
+++ b/matrix/AxisAngle.hpp
@@ -78,9 +78,15 @@ public:
         AxisAngle &v = *this;
         Type angle = (Type)2.0f*acosf(q(0));
         Type mag = sinf(angle/2.0f);
-        v(0) = angle*q(1)/mag;
-        v(1) = angle*q(2)/mag;
-        v(2) = angle*q(3)/mag;
+        if (fabs(angle) < 1e-10f) {
+            v(0) = 0;
+            v(1) = 0;
+            v(2) = 0;
+        } else {
+            v(0) = angle*q(1)/mag;
+            v(1) = angle*q(2)/mag;
+            v(2) = angle*q(3)/mag;
+        }
     }
 
     /**
@@ -95,7 +101,7 @@ public:
         Vector<Type, 3>()
     {
         AxisAngle &v = *this;
-        v = Quaternion<float>(dcm);
+        v = Quaternion<Type>(dcm);
     }
 
     /**
@@ -111,7 +117,7 @@ public:
         Vector<Type, 3>()
     {
         AxisAngle &v = *this;
-        v = Quaternion<float>(euler);
+        v = Quaternion<Type>(euler);
     }
 
     /**
diff --git a/test/attitude.cpp b/test/attitude.cpp
index e0a9642e3a..764f3143f7 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -259,7 +259,7 @@ int main()
     TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
 
     AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f));
-    TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 1.0f)));
+    TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f)));
 
     Dcmf dcm_aa_check = AxisAnglef(dcm_check);
     TEST(isEqual(dcm_aa_check, dcm_check));