Fix bugs in axis angle.

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);
     }
 
     /**

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));