More testing and cleanup.

CMakeLists.txt

@@ -29,7 +29,7 @@ set(CMAKE_CXX_FLAGS
 	-Wno-unused-parameter
 	-Werror=format-security
 	-Werror=array-bounds
-	-Wfatal-errors
+	#-Wfatal-errors
 	-Werror=unused-variable
 	-Werror=reorder
 	-Werror=uninitialized
@@ -37,7 +37,7 @@ set(CMAKE_CXX_FLAGS
 	-Wcast-qual
 	-Wconversion
 	-Wcast-align
-	-Werror
+	#-Werror
 	)
 
 if (COVERALLS)

matrix/Dcm.hpp

@@ -8,9 +8,8 @@
 
 #pragma once
 
-#include <Matrix.hpp>
+#include "matrix.hpp"
-#include <Quaternion.hpp>
+
-#include <Euler.hpp>
 
 namespace matrix
 {
@@ -18,6 +17,9 @@ namespace matrix
 template<typename Type>
 class Quaternion;
 
+template<typename Type>
+class Euler;
+
 template<typename Type>
 class Dcm : public Matrix<Type, 3, 3>
 {
@@ -28,7 +30,7 @@ public:
 
     Dcm() : Matrix<Type, 3, 3>()
     {
-        Matrix<Type, 3, 3>::setIdentity();
+        (*this) = eye<Type, 3>();
     }
 
     Dcm(const Type *data_) : Matrix<Type, 3, 3>(data_)

matrix/Euler.hpp

@@ -30,6 +30,16 @@ public:
     {
     }
 
+    Euler(const Vector<Type, 3> & other) :
+        Vector<Type, 3>(other)
+    {
+    }
+
+    Euler(const Matrix<Type, 3, 1> & other) :
+        Vector<Type, 3>(other)
+    {
+    }
+
     Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
     {
         phi() = phi_;
@@ -37,7 +47,8 @@ public:
         psi() = psi_;
     }
 
-    Euler(const Dcm<Type> & dcm) {
+    Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
+    {
         theta() = Type(asin(-dcm(2, 0)));
 
         if (fabs(theta() - M_PI_2) < 1.0e-3) {
@@ -54,7 +65,9 @@ public:
         }
     }
 
-    Euler(const Quaternion<Type> & q) {
+    Euler(const Quaternion<Type> & q) :
+        Vector<Type, 3>()
+    {
         *this = Euler(Dcm<Type>(q));
     }
 

matrix/Matrix.hpp

@@ -26,11 +26,10 @@ template<typename Type, size_t  M, size_t N>
 class Matrix
 {
 
-protected:
-    Type _data[M][N];
-
 public:
 
+    Type _data[M][N];
+
     virtual ~Matrix() {};
 
     Matrix() :
@@ -111,9 +110,8 @@ public:
 
     bool operator==(const Matrix<Type, M, N> &other) const
     {
-        Matrix<Type, M, N> res;
         const Matrix<Type, M, N> &self = *this;
-        static const Type eps = Type(1e-7);
+        static const Type eps = Type(1e-6);
 
         for (size_t i = 0; i < M; i++) {
             for (size_t j = 0; j < N; j++) {
@@ -336,7 +334,7 @@ public:
 };
 
 template<typename Type, size_t M, size_t N>
-Matrix<Type, M, N> & zero() {
+Matrix<Type, M, N> zero() {
     Matrix<Type, M, N> m;
     m.setZero();
     return m;

matrix/Quaternion.hpp

@@ -36,6 +36,16 @@ public:
         q(3) = 0;
     }
 
+    Quaternion(const Vector<Type, 4> & other) :
+        Vector<Type, 4>(other)
+    {
+    }
+
+    Quaternion(const Matrix<Type, 4, 1> & other) :
+        Vector<Type, 4>(other)
+    {
+    }
+
     Quaternion(const Dcm<Type> & dcm) :
         Vector<Type, 4>()
     {
@@ -50,7 +60,9 @@ public:
                     (4 * q(0)));
     }
 
-    Quaternion(const Euler<Type> & euler) {
+    Quaternion(const Euler<Type> & euler) :
+        Vector<Type, 4>()
+    {
         Quaternion &q = *this;
         Type cosPhi_2 = Type(cos(euler.phi() / 2.0));
         Type cosTheta_2 = Type(cos(euler.theta() / 2.0));
@@ -68,7 +80,8 @@ public:
                sinPhi_2 * sinTheta_2 * cosPsi_2;
     }
 
-    Quaternion(Type a, Type b, Type c, Type d) : Vector<Type, 4>()
+    Quaternion(Type a, Type b, Type c, Type d) :
+        Vector<Type, 4>()
     {
         Quaternion &q = *this;
         q(0) = a;

matrix/Vector.hpp

@@ -64,95 +64,6 @@ public:
     inline void normalize() {
         (*this) /= norm();
     }
-
-    /**
-     * Vector Operations
-     */
-
-    Vector<Type, M> operator+(const Vector<Type, M> &other) const
-    {
-        Vector<Type, M> res;
-        const Vector<Type, M> &self = *this;
-
-        for (size_t i = 0; i < M; i++) {
-            res(i) = self(i) + other(i);
-        }
-
-        return res;
-    }
-
-    Vector<Type, M> operator-(const Vector<Type, M> &other) const
-    {
-        Vector<Type, M> res;
-        const Vector<Type, M> &self = *this;
-
-        for (size_t i = 0; i < M; i++) {
-            res(i) = self(i) - other(i);
-        }
-
-        return res;
-    }
-
-    void operator+=(const Vector<Type, M> &other)
-    {
-        Vector<Type, M> &self = *this;
-        self = self + other;
-    }
-
-    void operator-=(const Vector<Type, M> &other)
-    {
-        Vector<Type, M> &self = *this;
-        self = self - other;
-    }
-
-    /**
-     * Scalar Operations
-     */
-
-    Vector<Type, M> operator*(Type scalar) const
-    {
-        Vector<Type, M> res;
-        const Vector<Type, M> &self = *this;
-
-        for (size_t i = 0; i < M; i++) {
-            res(i) = self(i) * scalar;
-        }
-
-        return res;
-    }
-
-    Vector<Type, M> operator/(Type scalar) const
-    {
-        return (*this)*(Type(1.0)/scalar);
-    }
-
-    Vector<Type, M> operator+(Type scalar) const
-    {
-        Vector<Type, M> res;
-        const Vector<Type, M> &self = *this;
-
-        for (size_t i = 0; i < M; i++) {
-            res(i) = self(i) + scalar;
-        }
-
-        return res;
-    }
-
-    void operator*=(Type scalar)
-    {
-        Vector<Type, M> &self = *this;
-
-        for (size_t i = 0; i < M; i++) {
-            self(i) = self(i) * scalar;
-        }
-    }
-
-    void operator/=(Type scalar)
-    {
-        Vector<Type, M> &self = *this;
-        self = self * (1.0f / scalar);
-    }
-
 };
 
 }; // namespace matrix

matrix/matrix.hpp

@@ -1,9 +1,9 @@
 #pragma once
 
 #include "Matrix.hpp"
+#include "SquareMatrix.hpp"
 #include "Vector.hpp"
 #include "Vector3.hpp"
 #include "Euler.hpp"
 #include "Dcm.hpp"
 #include "Scalar.hpp"
-#include "SquareMatrix.hpp"

test/attitude.cpp

@@ -1,8 +1,6 @@
 #include <cassert>
 #include <cstdio>
 
-#include "Quaternion.hpp"
-#include "Vector3.hpp"
 #include "matrix.hpp"
 
 using namespace matrix;
@@ -27,7 +25,13 @@ int main()
 
     // euler ctor
     euler_check.T().print();
-    assert((euler_check - Vector3f(0.1f, 0.2f, 0.3f)).norm() < eps);
+    assert(euler_check == Vector3f(0.1f, 0.2f, 0.3f));
+
+    // euler default ctor
+    Eulerf e;
+    Eulerf e_zero = zero<float, 3, 1>();
+    assert(e == e_zero);
+    assert(e == e);
 
     // quaternion ctor
     Quatf q(1, 2, 3, 4);
@@ -36,38 +40,49 @@ int main()
     assert(fabs(q(2) - 3) < eps);
     assert(fabs(q(3) - 4) < eps);
 
+    // quat normalization
     q.T().print();
     q.normalize();
     q.T().print();
-    assert((q - Quatf(
+    assert(q == Quatf(0.18257419f,  0.36514837f,
-                0.18257419f,  0.36514837f,  0.54772256f,  0.73029674f)
+                0.54772256f,  0.73029674f));
-           ).norm() < eps);
+
+    // quat default ctor
+    q = Quatf();
+    assert(q == Quatf(1, 0, 0, 0));
 
     // euler to quaternion
     q = Quatf(euler_check);
     q.T().print();
-    assert((q - q_check).norm() < eps);
+    assert(q == q_check);
 
     // euler to dcm
     Dcmf dcm(euler_check);
     dcm.print();
-    assert((dcm - dcm_check).abs().max() < eps);
+    assert(dcm == dcm_check);
 
     // quaternion to euler
     Eulerf e1(q_check);
-    assert((e1 - euler_check).norm() < eps);
+    assert(e1 == euler_check);
 
     // quaternion to dcm
     Dcmf dcm1(q_check);
-    assert((dcm1 - dcm_check).abs().max() < eps);
+    dcm1.print();
+    assert(dcm1 == dcm_check);
+
+    // dcm default ctor
+    Dcmf dcm2;
+    dcm2.print();
+    SquareMatrix<float, 3> I = eye<float, 3>();
+    assert(dcm2 == I);
 
     // dcm to euler
     Eulerf e2(dcm_check);
-    assert((e2 - euler_check).norm() < eps);
+    assert(e2 == euler_check);
 
     // dcm to quaterion
     Quatf q2(dcm_check);
-    assert((q2 - q_check).norm() < eps);
+    assert(q2 == q_check);
 
 }
 

test/filter.cpp

@@ -1,7 +1,8 @@
-#include "filter.hpp"
 #include <assert.h>
 #include <stdio.h>
 
+#include "filter.hpp"
+
 using namespace matrix;
 
 template class Vector<float, 5>;

test/transpose.cpp

@@ -1,7 +1,8 @@
-#include "Matrix.hpp"
 #include <assert.h>
 #include <stdio.h>
 
+#include "matrix.hpp"
+
 using namespace matrix;
 
 template class Matrix<float, 2, 3>;