ardupilot/libraries/AP_Math/matrixN.h

49 lines
938 B
C++

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