From 0e14e1118327a7994ba87745ea0b5e6cb406e82f Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Mar 2016 06:35:13 -0400
Subject: [PATCH] Added support for attitude_estimator_q functions.

---
 matrix/Matrix.hpp         | 16 ++++++++++++++++
 matrix/Quaternion.hpp     |  6 ++++++
 matrix/Vector.hpp         |  4 ++--
 test/attitude.cpp         |  2 +-
 test/matrixAssignment.cpp |  4 ++++
 5 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 051056422a..83e3870757 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -342,6 +342,22 @@ public:
         }
     }
 
+    void setRow(size_t i, const Matrix<Type, N, 1> &row)
+    {
+        Matrix<Type, M, N> &self = *this;
+        for (size_t j = 0; j < N; j++) {
+            self(i, j) = row(j, 0);
+        }
+    }
+
+    void setCol(size_t j, const Matrix<Type, M, 1> &col)
+    {
+        Matrix<Type, M, N> &self = *this;
+        for (size_t i = 0; i < M; i++) {
+            self(i, j) = col(i, 1);
+        }
+    }
+
     void setZero()
     {
         memset(_data, 0, sizeof(_data));
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index e9cf25f2d8..7b845b1475 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -29,6 +29,11 @@ public:
     typedef Matrix<Type, 4, 1> Matrix41;
     typedef Matrix<Type, 3, 1> Matrix31;
 
+    Quaternion(const Type *data_) :
+        Vector<Type, 4>(data_)
+    {
+    }
+
     Quaternion() :
         Vector<Type, 4>()
     {
@@ -199,6 +204,7 @@ public:
 };
 
 typedef Quaternion<float> Quatf;
+typedef Quaternion<float> Quaternionf;
 
 } // namespace matrix
 
diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp
index 5a8718a7e2..0349060c06 100644
--- a/matrix/Vector.hpp
+++ b/matrix/Vector.hpp
@@ -66,8 +66,8 @@ public:
         return Type(sqrt(a.dot(a)));
     }
 
-    inline void normalize() {
-        (*this) /= norm();
+    Vector normalize() const {
+        return  (*this) / norm();
     }
 
     Vector pow(Type v) const {
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 9117df155d..74ad5379dc 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -49,7 +49,7 @@ int main()
     TEST(fabs(q(3) - 4) < eps);
 
     // quat normalization
-    q.normalize();
+    q = q.normalize();
     TEST(isEqual(q, Quatf(0.18257419f,  0.36514837f,
                           0.54772256f,  0.73029674f)));
 
diff --git a/test/matrixAssignment.cpp b/test/matrixAssignment.cpp
index 1484a31189..7ff4a41a78 100644
--- a/test/matrixAssignment.cpp
+++ b/test/matrixAssignment.cpp
@@ -77,6 +77,10 @@ int main()
     Matrix<float, 1, 1> m5 = s;
     TEST(fabs(m5(0,0) - s) < 1e-5);
 
+    Matrix<float, 2, 2> m6;
+    m6.setRow(0, Vector2f(1, 1));
+    m6.setCol(0, Vector2f(1, 1));
+
     return 0;
 }