From a1fa2a9de3cb093de256d02d297ad286e2dc9319 Mon Sep 17 00:00:00 2001
From: Andrey Kolobov <akolobov@microsoft.com>
Date: Mon, 27 Feb 2017 10:24:35 +1100
Subject: [PATCH] AP_Math: added matrixN for soaring controller

---
 libraries/AP_Math/matrixN.cpp | 60 +++++++++++++++++++++++++++++++++++
 libraries/AP_Math/matrixN.h   | 48 ++++++++++++++++++++++++++++
 libraries/AP_Math/vectorN.h   | 35 ++++++++++++++++++++
 3 files changed, 143 insertions(+)
 create mode 100644 libraries/AP_Math/matrixN.cpp
 create mode 100644 libraries/AP_Math/matrixN.h

diff --git a/libraries/AP_Math/matrixN.cpp b/libraries/AP_Math/matrixN.cpp
new file mode 100644
index 0000000000..aac3ca7382
--- /dev/null
+++ b/libraries/AP_Math/matrixN.cpp
@@ -0,0 +1,60 @@
+/*
+ *  N dimensional matrix operations
+ */
+
+#pragma GCC optimize("O3")
+
+#include "matrixN.h"
+
+
+// multiply two vectors to give a matrix, in-place
+template <typename T, uint8_t N>
+void MatrixN<T,N>::mult(const VectorN<T,N> &A, const VectorN<T,N> &B)
+{
+    for (uint8_t i = 0; i < N; i++) {
+        for (uint8_t j = 0; j < N; j++) {
+            v[i][j] = A[i] * B[j];
+        }
+    }
+}
+
+// subtract B from the matrix
+template <typename T, uint8_t N>
+MatrixN<T,N> &MatrixN<T,N>::operator -=(const MatrixN<T,N> &B)
+{
+    for (uint8_t i = 0; i < N; i++) {
+        for (uint8_t j = 0; j < N; j++) {
+            v[i][j] -= B.v[i][j];
+        }
+    }
+    return *this;
+}
+
+// add B to the matrix
+template <typename T, uint8_t N>
+MatrixN<T,N> &MatrixN<T,N>::operator +=(const MatrixN<T,N> &B)
+{
+    for (uint8_t i = 0; i < N; i++) {
+        for (uint8_t j = 0; j < N; j++) {
+            v[i][j] += B.v[i][j];
+        }
+    }
+    return *this;
+}
+
+// Matrix symmetry routine
+template <typename T, uint8_t N>
+void MatrixN<T,N>::force_symmetry(void)
+{
+    for (uint8_t i = 0; i < N; i++) {
+        for (uint8_t j = 0; j < (i - 1); j++) {
+            v[i][j] = (v[i][j] + v[j][i]) * 0.5;
+            v[j][i] = v[i][j];
+        }
+    }
+}
+
+template void MatrixN<float,4>::mult(const VectorN<float,4> &A, const VectorN<float,4> &B);
+template MatrixN<float,4> &MatrixN<float,4>::operator -=(const MatrixN<float,4> &B);
+template MatrixN<float,4> &MatrixN<float,4>::operator +=(const MatrixN<float,4> &B);
+template void MatrixN<float,4>::force_symmetry(void);
diff --git a/libraries/AP_Math/matrixN.h b/libraries/AP_Math/matrixN.h
new file mode 100644
index 0000000000..82605fa54b
--- /dev/null
+++ b/libraries/AP_Math/matrixN.h
@@ -0,0 +1,48 @@
+/*
+ *  N dimensional matrix operations
+ */
+
+#pragma once
+
+#include "math.h"
+#include <stdint.h>
+#include "vectorN.h"
+
+template <typename T, uint8_t N>
+class VectorN;
+
+
+template <typename T, uint8_t N>
+class MatrixN {
+  
+    friend class VectorN<T,N>;
+
+public:
+    // constructor from zeros
+    MatrixN<T,N>(void) {
+        memset(v, 0, sizeof(v));        
+    }
+
+    // constructor from 4 diagonals
+    MatrixN<T,N>(const float d[N]) {
+        memset(v, 0, sizeof(v));
+        for (uint8_t i = 0; i < N; i++) {
+            v[i][i] = d[i];
+        }
+    }
+
+    // multiply two vectors to give a matrix, in-place
+    void mult(const VectorN<T,N> &A, const VectorN<T,N> &B);
+
+    // subtract B from the matrix
+    MatrixN<T,N> &operator -=(const MatrixN<T,N> &B);
+
+    // add B to the matrix
+    MatrixN<T,N> &operator +=(const MatrixN<T,N> &B);
+    
+    // Matrix symmetry routine
+    void force_symmetry(void);
+
+private:
+    T v[N][N];
+};
diff --git a/libraries/AP_Math/vectorN.h b/libraries/AP_Math/vectorN.h
index eab36a2f5e..8204390d22 100644
--- a/libraries/AP_Math/vectorN.h
+++ b/libraries/AP_Math/vectorN.h
@@ -16,10 +16,20 @@
 
 #include <cmath>
 #include <string.h>
+#include "matrixN.h"
+
+#ifndef MATH_CHECK_INDEXES
+# define MATH_CHECK_INDEXES 0
+#endif
+
 #if MATH_CHECK_INDEXES
 #include <assert.h>
 #endif
 
+template <typename T, uint8_t N>
+class MatrixN;
+
+
 template <typename T, uint8_t N>
 class VectorN
 {
@@ -29,6 +39,11 @@ public:
         memset(_v, 0, sizeof(T)*N);
     }
 
+    // vector ctor
+    inline VectorN<T,N>(const T *v) {
+        memcpy(_v, v, sizeof(T)*N);
+    }
+    
     inline T & operator[](uint8_t i) {
 #if MATH_CHECK_INDEXES
         assert(i >= 0 && i < N);
@@ -134,6 +149,26 @@ public:
         return *this;
     }
 
+    // dot product
+    T operator *(const VectorN<T,N> &v) const {
+        float ret = 0;
+        for (uint8_t i=0; i<N; i++) {
+            ret += _v[i] * v._v[i];
+        }
+        return ret;
+    }
+    
+    // multiplication of a matrix by a vector, in-place
+    // C = A * B
+    void mult(const MatrixN<T,N> &A, const VectorN<T,N> &B) {
+        for (uint8_t i = 0; i < N; i++) {
+            _v[i] = 0;
+            for (uint8_t k = 0; k < N; k++) {
+                _v[i] += A.v[i][k] * B[k];
+            }
+        }
+    }
+
 private:
     T _v[N];
 };