AP_Math: added matrixN for soaring controller

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);

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];
+};

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];
 };