Added runge kutta integration.

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