From 12713105b8f6471d6ec0508812c6edd3f64b3037 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Wed, 4 Nov 2015 00:48:59 -0500
Subject: [PATCH] Fixed quaternion unit test.

---
 matrix/Dcm.hpp      |  1 +
 test/quaternion.cpp | 17 +++++++++--------
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp
index 05bcdcb37e..40de899688 100644
--- a/matrix/Dcm.hpp
+++ b/matrix/Dcm.hpp
@@ -28,6 +28,7 @@ public:
 
     Dcm() : Matrix<Type, 3, 3>()
     {
+        Matrix<Type, 3, 3>::setIdentity();
     }
 
     Dcm(const Quaternion<Type> & q) {
diff --git a/test/quaternion.cpp b/test/quaternion.cpp
index 35a2f6c7fc..92a727d126 100644
--- a/test/quaternion.cpp
+++ b/test/quaternion.cpp
@@ -21,17 +21,18 @@ int main()
 
     // test euler ctor
     q = Quatf(Eulerf(0.1f, 0.2f, 0.3f));
-    assert((q - Quatf(0.983347f, 0.034271f, 0.106021f, 0.143572f)).norm() < 1e-3);
+    assert((q - Quatf(0.983347f, 0.034271f, 0.106021f, 0.143572f)).norm() < 1e-5);
+
     // test dcm ctor
-    //q = Quatf(Dcmf());
-    //assert(q == Quatf(1.0f, 0.0f, 0.0f, 0.0f));
+    q = Quatf(Dcmf());
+    assert(q == Quatf(1.0f, 0.0f, 0.0f, 0.0f));
     // TODO test derivative
     // test accessors
-    //q(0) = 0.1f;
-    //q(1) = 0.2f;
-    //q(2) = 0.3f;
-    //q(3) = 0.4f;
-    //assert(q == Quatf(0.1f, 0.2f, 0.3f, 0.4f));
+    q(0) = 0.1f;
+    q(1) = 0.2f;
+    q(2) = 0.3f;
+    q(3) = 0.4f;
+    assert(q == Quatf(0.1f, 0.2f, 0.3f, 0.4f));
     return 0;
 }