Work on testing.

matrix/Dcm.hpp

@@ -31,6 +31,14 @@ public:
         Matrix<Type, 3, 3>::setIdentity();
     }
 
+    Dcm(const Type *data) : Matrix<Type, 3, 3>(data)
+    {
+    }
+
+    Dcm(const Matrix<Type, 3, 3> &other) : Matrix<Type, 3, 3>(other)
+    {
+    }
+
     Dcm(const Quaternion<Type> & q) {
         Dcm &dcm = *this;
         Type a = q(0);

matrix/Euler.hpp

@@ -42,11 +42,11 @@ public:
         Type phi = 0;
         Type psi = 0;
 
-        if (abs(theta - M_PI_2) < 1.0e-3) {
+        if (fabs(theta - M_PI_2) < 1.0e-3) {
             psi = atan2(dcm(1, 2) - dcm(0, 1),
                         dcm(0, 2) + dcm(1, 1));
 
-        } else if (abs(theta + M_PI_2) < 1.0e-3) {
+        } else if (fabs(theta + M_PI_2) < 1.0e-3) {
             psi = atan2(dcm(1, 2) - dcm(0, 1),
                         dcm(0, 2) + dcm(1, 1));
 

matrix/Matrix.hpp

@@ -293,6 +293,45 @@ public:
         }
     }
 
+    Matrix<Type, M, N> abs()
+    {
+        Matrix<Type, M, N> r;
+        for (int i=0; i<M; i++) {
+            for (int j=0; j<M; j++) {
+                r(i,j) = fabs((*this)(i,j));
+            }
+        }
+        return r;
+    }
+
+    Type max()
+    {
+        Type max = (*this)(0,0);
+        for (int i=0; i<M; i++) {
+            for (int j=0; j<M; j++) {
+                Type val = (*this)(i,j);
+                if (val > max) {
+                    max = val;
+                }
+            }
+        }
+        return max;
+    }
+
+    Type min()
+    {
+        Type min = (*this)(0,0);
+        for (int i=0; i<M; i++) {
+            for (int j=0; j<M; j++) {
+                Type val = (*this)(i,j);
+                if (val < min) {
+                    min = val;
+                }
+            }
+        }
+        return min;
+    }
+
 };
 
 template<typename Type, size_t M, size_t N>

matrix/Vector.hpp

@@ -47,20 +47,24 @@ public:
         return v(i, 0);
     }
 
-    Type dot(const Vector & b) {
-        Vector &a(*this);
+    Type dot(const Vector & b) const {
+        const Vector &a(*this);
         Type r = 0;
-        for (int i = 0; i<3; i++) {
+        for (int i = 0; i<M; i++) {
             r += a(i)*b(i);
         }
         return r;
     }
 
-    Type norm() {
-        Vector &a(*this);
+    Type norm() const {
+        const Vector &a(*this);
         return sqrt(a.dot(a));
     }
 
+    inline void normalize() {
+        (*this) /= norm();
+    }
+
     /**
      * Vector Operations
      */
@@ -131,6 +135,11 @@ public:
         return res;
     }
 
+    Vector<Type, M> operator/(Type scalar) const
+    {
+        return (*this)*(1.0/scalar);
+    }
+
     Vector<Type, M> operator+(Type scalar) const
     {
         Vector<Type, M> res;