Added runge kutta integration.

CMakeLists.txt

@@ -37,7 +37,7 @@ set(CMAKE_CXX_FLAGS
 	-Wcast-qual
 	-Wconversion
 	-Wcast-align
-	#-Werror
+	-Werror
 	)
 
 if (COVERAGE)

matrix/Matrix.hpp

@@ -152,6 +152,21 @@ public:
         return res;
     }
 
+    // unary minus
+    Matrix<Type, M, N> operator-() 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);
+            }
+        }
+
+        return res;
+    }
+
     void operator+=(const Matrix<Type, M, N> &other)
     {
         Matrix<Type, M, N> &self = *this;
@@ -287,6 +302,22 @@ public:
         memset(_data, 0, sizeof(_data));
     }
 
+    void setAll(Type val)
+    {
+        Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                self(i, j) = val;
+            }
+        }
+    }
+
+    inline void setOne()
+    {
+        setAll(1);
+    }
+
     void setIdentity()
     {
         setZero();
@@ -369,12 +400,19 @@ public:
 };
 
 template<typename Type, size_t M, size_t N>
-Matrix<Type, M, N> zero() {
+Matrix<Type, M, N> zeros() {
     Matrix<Type, M, N> m;
     m.setZero();
     return m;
 }
 
+template<typename Type, size_t M, size_t N>
+Matrix<Type, M, N> ones() {
+    Matrix<Type, M, N> m;
+    m.setOne();
+    return m;
+}
+
 typedef Matrix<float, 3, 3> Matrix3f;
 
 }; // namespace matrix

matrix/Scalar.hpp

@@ -20,24 +20,31 @@ namespace matrix
 {
 
 template<typename Type>
-class Scalar : public Matrix<Type, 1, 1>
+class Scalar
 {
 public:
     virtual ~Scalar() {};
 
-    Scalar() : Matrix<Type, 1, 1>()
+    Scalar() : _value()
     {
     }
 
     Scalar(const Matrix<Type, 1, 1> & other)
     {
-        (*this)(0,0) = other(0,0);
+        _value = other(0,0);
+    }
+
+    Scalar(float other)
+    {
+        _value = other;
     }
 
     operator Type()
     {
-        return (*this)(0,0);
+        return _value;
     }
+private:
+    Type _value;
 
 };
 

matrix/Vector.hpp

@@ -67,9 +67,6 @@ public:
         (*this) /= norm();
     }
 
-    Type operator*(const MatrixM1 & b) const {
-        return (*this).dot(b);
-    }
 };
 
 }; // namespace matrix

matrix/integration.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix {
+
+template<typename Type, size_t M>
+int integrate_rk4(
+	Vector<Type, M> (*f)(Type, Vector<Type, M>),
+    Vector<Type, M> & y,
+    Type & t,
+	Type h
+	)
+{
+	// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
+	Vector<Type, M> k1, k2, k3, k4;
+   	k1 = f(t, y);
+   	k2 = f(t + h/2, y + k1*h/2);
+   	k3 = f(t + h/2, y + k2*h/2);
+   	k4 = f(t + h, y + k3*h);
+	y += (k1 + k2*2 + k3*2 + k4)*(h/6);
+	t += h;
+	return 0;
+}
+
+}; // namespace matrix

test/CMakeLists.txt

@@ -11,6 +11,7 @@ set(tests
 	vector3
 	attitude
 	filter
+	integration
 	squareMatrix
 	)
 

test/attitude.cpp

@@ -29,7 +29,7 @@ int main()
 
     // euler default ctor
     Eulerf e;
-    Eulerf e_zero = zero<float, 3, 1>();
+    Eulerf e_zero = zeros<float, 3, 1>();
     assert(e == e_zero);
     assert(e == e);
 

test/integration.cpp

@@ -0,0 +1,26 @@
+#include <assert.h>
+#include <stdio.h>
+
+#include <matrix/integration.hpp>
+
+using namespace matrix;
+
+Vector<float, 6> f(float t, Vector<float, 6> y);
+
+Vector<float, 6> f(float t, Vector<float, 6> y) {
+    return ones<float, 6, 1>();
+}
+
+int main()
+{
+    Vector<float, 6> y = ones<float, 6, 1>();
+    float h = 1;
+    float t = 1;
+    y.T().print();
+    integrate_rk4(f, y, t, h);
+    y.T().print();
+    assert(y == (ones<float, 6, 1>()*2));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/inverse.cpp

@@ -40,7 +40,7 @@ int main()
         assert(A_large == A_large_I);
     }
 
-    SquareMatrix<float, 3> zero_test = zero<float, 3, 3>();
+    SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
     inv(zero_test);
 
     return 0;

test/matrixAssignment.cpp

@@ -65,6 +65,7 @@ int main()
 
     Matrix3f m4(data);
 
+    assert(-m4 == m4*(-1));
 
     m4.swapCols(0, 2);
     assert(m4 == Matrix3f(data_col_02_swap));
@@ -72,6 +73,11 @@ int main()
     m4.swapRows(0, 2);
     assert(m4 == Matrix3f(data_row_02_swap));
     assert(fabs(m4.min() - 1) < 1e-5);
+
+    Scalar<float> s;
+    s = 1;
+    assert(fabs(s - 1) < 1e-5);
+
     return 0;
 }
 

test/vector.cpp

@@ -15,7 +15,6 @@ 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);