Merge ../Firmware/src/lib/matrix

CMakeLists.txt

@@ -50,13 +50,6 @@ if (COVERAGE)
 		)
 	include(Coveralls)
 	coveralls_turn_on_coverage()
-	add_custom_target(coverage
-		COMMAND lcov --capture --directory . --output-file coverage.info
-		COMMAND genhtml coverage.info --output-directory out
-		COMMAND x-www-browser out/index.html
-		WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
-		DEPENDS coveralls
-		)
 endif()
 
 string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")

matrix/Matrix.hpp

@@ -94,6 +94,20 @@ public:
         return res;
     }
 
+    Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i , j) = self(i, j)*other(i, j);
+            }
+        }
+
+        return res;
+    }
+
     Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
     {
         Matrix<Type, M, N> res;

matrix/Quaternion.hpp

@@ -25,6 +25,9 @@ class Quaternion : public Vector<Type, 4>
 public:
     virtual ~Quaternion() {};
 
+    typedef Matrix<Type, 4, 1> Matrix41;
+    typedef Matrix<Type, 3, 1> Matrix31;
+
     Quaternion() :
         Vector<Type, 4>()
     {
@@ -35,12 +38,7 @@ public:
         q(3) = 0;
     }
 
-    Quaternion(const Vector<Type, 4> & other) :
+    Quaternion(const Matrix41 & other) :
-        Vector<Type, 4>(other)
-    {
-    }
-
-    Quaternion(const Matrix<Type, 4, 1> & other) :
         Vector<Type, 4>(other)
     {
     }
@@ -100,7 +98,7 @@ public:
         return r;
     }
 
-    Matrix<Type, 4, 1> derivative(const Vector<Type, 3> & w) const {
+    Matrix41 derivative(const Matrix31 & w) const {
         const Quaternion &q = *this;
         Type dataQ[] = {
             q(0), -q(1), -q(2), -q(3),
@@ -111,9 +109,9 @@ public:
         Matrix<Type, 4, 4> Q(dataQ);
         Vector<Type, 4> v;
         v(0) = 0;
-        v(1) = w(0);
+        v(1) = w(0,0);
-        v(2) = w(1);
+        v(2) = w(1,0);
-        v(3) = w(2);
+        v(3) = w(2,0);
         return Q * v * Type(0.5);
     }
 };

matrix/SquareMatrix.hpp

@@ -79,7 +79,7 @@ SquareMatrix<Type, M> diag(Vector<Type, M> d) {
 }
 
 template<typename Type, size_t M>
-SquareMatrix<Type, M> expm(const SquareMatrix<Type, M> & A, size_t order=5)
+SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5)
 {
     SquareMatrix<Type, M> res;
     SquareMatrix<Type, M> A_pow = A;

matrix/Vector.hpp

@@ -21,42 +21,39 @@ class Vector : public Matrix<Type, M, 1>
 public:
     virtual ~Vector() {};
 
-    Vector() : Matrix<Type, M, 1>()
+    typedef Matrix<Type, M, 1> MatrixM1;
+
+    Vector() : MatrixM1()
     {
     }
 
-    Vector(const Vector<Type, M> & other) :
+    Vector(const MatrixM1 & other) :
-        Matrix<Type, M, 1>(other)
+        MatrixM1(other)
-    {
-    }
-
-    Vector(const Matrix<Type, M, 1> & other) :
-        Matrix<Type, M, 1>(other)
     {
     }
 
     Vector(const Type *data_) :
-        Matrix<Type, M, 1>(data_)
+        MatrixM1(data_)
     {
     }
 
     inline Type operator()(size_t i) const
     {
-        const Matrix<Type, M, 1> &v = *this;
+        const MatrixM1 &v = *this;
         return v(i, 0);
     }
 
     inline Type &operator()(size_t i)
     {
-        Matrix<Type, M, 1> &v = *this;
+        MatrixM1 &v = *this;
         return v(i, 0);
     }
 
-    Type dot(const Vector & b) const {
+    Type dot(const MatrixM1 & b) const {
         const Vector &a(*this);
         Type r = 0;
         for (int i = 0; i<M; i++) {
-            r += a(i)*b(i);
+            r += a(i)*b(i,0);
         }
         return r;
     }
@@ -69,6 +66,10 @@ public:
     inline void normalize() {
         (*this) /= norm();
     }
+
+    Type operator*(const MatrixM1 & b) const {
+        return (*this).dot(b);
+    }
 };
 
 }; // namespace matrix

matrix/Vector2.hpp

@@ -0,0 +1,65 @@
+/**
+ * @file Vector2.hpp
+ *
+ * 2D vector class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M>
+class Vector;
+
+template<typename Type>
+class Vector2 : public Vector<Type, 2>
+{
+public:
+
+    typedef Matrix<Type, 2, 1> Matrix21;
+
+    virtual ~Vector2() {};
+
+    Vector2() :
+        Vector<Type, 2>()
+    {
+    }
+
+    Vector2(const Matrix21 & other) :
+        Vector<Type, 2>(other)
+    {
+    }
+
+    Vector2(const Type *data_) :
+        Vector<Type, 2>(data_)
+    {
+    }
+
+    Vector2(Type x, Type y) : Vector<Type, 2>()
+    {
+        Vector2 &v(*this);
+        v(0) = x;
+        v(1) = y;
+    }
+
+    Type cross(const Matrix21 & b)  const {
+        const Vector2 &a(*this);
+        return a(0)*b(1, 0) - a(1)*b(0, 0);
+    }
+
+    Type operator%(const Matrix21 & b) const {
+        return (*this).cross(b);
+    }
+
+};
+
+typedef Vector2<float> Vector2f;
+
+}; // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Vector3.hpp

@@ -20,6 +20,9 @@ template<typename Type>
 class Vector3 : public Vector<Type, 3>
 {
 public:
+
+    typedef Matrix<Type, 3, 1> Matrix31;
+
     virtual ~Vector3() {};
 
     Vector3() :
@@ -27,12 +30,7 @@ public:
     {
     }
 
-    Vector3(const Vector<Type, 3> & other) :
+    Vector3(const Matrix31 & other) :
-        Vector<Type, 3>(other)
-    {
-    }
-
-    Vector3(const Matrix<Type, 3, 1> & other) :
         Vector<Type, 3>(other)
     {
     }
@@ -50,14 +48,19 @@ public:
         v(2) = z;
     }
 
-    Vector3 cross(const Vector3 & b)  const {
+    Vector3 cross(const Matrix31 & b)  const {
         const Vector3 &a(*this);
         Vector3 c;
-        c(0) = a(1)*b(2) - a(2)*b(1);
+        c(0) = a(1)*b(2,0) - a(2)*b(1,0);
-        c(1) = -a(0)*b(2) + a(2)*b(0);
+        c(1) = -a(0)*b(2,0) + a(2)*b(0,0);
-        c(2) = a(0)*b(1) - a(1)*b(0);
+        c(2) = a(0)*b(1,0) - a(1)*b(0,0);
         return c;
     }
+
+    Vector3 operator%(const Matrix31 & b) const {
+        return (*this).cross(b);
+    }
+
 };
 
 typedef Vector3<float> Vector3f;

matrix/math.hpp

@@ -3,6 +3,7 @@
 #include "Matrix.hpp"
 #include "SquareMatrix.hpp"
 #include "Vector.hpp"
+#include "Vector2.hpp"
 #include "Vector3.hpp"
 #include "Euler.hpp"
 #include "Dcm.hpp"

test/CMakeLists.txt

@@ -7,14 +7,29 @@ set(tests
 	matrixScalarMult
 	transpose
 	vector
+	vector2
 	vector3
 	attitude
 	filter
 	squareMatrix
 	)
 
-foreach(test ${tests})
+add_custom_target(test_build)
-	add_executable(${test}
+foreach(test_name ${tests})
-		${test}.cpp)
+	add_executable(${test_name}
-	add_test(${test} ${test})
+		${test_name}.cpp)
+	add_test(test_${test_name} ${test_name})
+	add_dependencies(test_build ${test_name})
 endforeach()
+
+if (COVERAGE)
+	add_custom_target(coverage
+		COMMAND lcov --capture --directory . --output-file coverage.info
+		COMMAND genhtml coverage.info --output-directory out
+		COMMAND x-www-browser out/index.html
+		WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+		DEPENDS coveralls test_build
+		)
+endif()
+
+

test/matrixMult.cpp

@@ -23,6 +23,12 @@ int main()
     R2 *= A_I;
     R2.print();
     assert(R2 == I);
+
+
+    Matrix3f A2 = eye<float, 3>()*2;
+    Matrix3f B = A2.emult(A2);
+    Matrix3f B_check = eye<float, 3>()*4;
+    assert(B == B_check);
     return 0;
 }
 

test/vector.cpp

@@ -15,6 +15,7 @@ int main()
     assert(fabs(v1.norm() - 7.416198487095663f) < 1e-5);
     Vector<float, 5> v2(data2);
     assert(fabs(v1.dot(v2) - 130.0f) < 1e-5);
+    assert(fabs(v1*v2 - 130.0f) < 1e-5);
     v2.normalize();
     Vector<float, 5> v3(v2);
     assert(v2 == v3);

test/vector2.cpp

@@ -0,0 +1,18 @@
+#include <assert.h>
+#include <stdio.h>
+
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+template class Vector<float, 3>;
+
+int main()
+{
+    Vector2f a(1, 0);
+    Vector2f b(0, 1);
+    assert (fabs(a % b - 1.0f) < 1e-5);
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */