237 lines
8.5 KiB
C++
237 lines
8.5 KiB
C++
#ifndef MATRIXOPERATIONS_H_
|
|
#define MATRIXOPERATIONS_H_
|
|
|
|
#include <fsfw/retval.h>
|
|
#include <stdint.h>
|
|
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <utility>
|
|
|
|
template <typename T1, typename T2 = T1, typename T3 = T2>
|
|
class MatrixOperations {
|
|
public:
|
|
// do not use with result == matrix1 or matrix2
|
|
static void multiply(const T1 *matrix1, const T2 *matrix2, T3 *result, uint8_t rows1,
|
|
uint8_t columns1, uint8_t columns2) {
|
|
if ((matrix1 == (T1 *)result) || (matrix2 == (T2 *)result)) {
|
|
// SHOULDDO find an implementation that is tolerant to this
|
|
return;
|
|
}
|
|
for (uint8_t resultColumn = 0; resultColumn < columns2; resultColumn++) {
|
|
for (uint8_t resultRow = 0; resultRow < rows1; resultRow++) {
|
|
result[resultColumn + columns2 * resultRow] = 0;
|
|
for (uint8_t i = 0; i < columns1; i++) {
|
|
result[resultColumn + columns2 * resultRow] +=
|
|
matrix1[i + resultRow * columns1] * matrix2[resultColumn + i * columns2];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static void transpose(const T1 *matrix, T2 *transposed, uint8_t size) {
|
|
uint8_t row, column;
|
|
transposed[0] = matrix[0];
|
|
for (column = 1; column < size; column++) {
|
|
transposed[column + size * column] = matrix[column + size * column];
|
|
for (row = 0; row < column; row++) {
|
|
T1 temp = matrix[column + size * row];
|
|
transposed[column + size * row] = matrix[row + size * column];
|
|
transposed[row + size * column] = temp;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Overload transpose to support non symmetrical matrices
|
|
// do not use with transposed == matrix && columns != rows
|
|
static void transpose(const T1 *matrix, T2 *transposed, uint8_t rows, uint8_t columns) {
|
|
uint8_t row, column;
|
|
transposed[0] = matrix[0];
|
|
if (matrix == transposed && columns == rows) {
|
|
transpose(matrix, transposed, rows);
|
|
} else if (matrix == transposed && columns != rows) {
|
|
// not permitted
|
|
return;
|
|
}
|
|
for (column = 0; column < columns; column++) {
|
|
for (row = 0; row < rows; row++) {
|
|
transposed[row + column * rows] = matrix[column + row * columns];
|
|
}
|
|
}
|
|
}
|
|
|
|
static void add(const T1 *matrix1, const T2 *matrix2, T3 *result, uint8_t rows, uint8_t columns) {
|
|
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++) {
|
|
for (uint8_t resultRow = 0; resultRow < rows; resultRow++) {
|
|
result[resultColumn + columns * resultRow] = matrix1[resultColumn + columns * resultRow] +
|
|
matrix2[resultColumn + columns * resultRow];
|
|
}
|
|
}
|
|
}
|
|
|
|
static void subtract(const T1 *matrix1, const T2 *matrix2, T3 *result, uint8_t rows,
|
|
uint8_t columns) {
|
|
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++) {
|
|
for (uint8_t resultRow = 0; resultRow < rows; resultRow++) {
|
|
result[resultColumn + columns * resultRow] = matrix1[resultColumn + columns * resultRow] -
|
|
matrix2[resultColumn + columns * resultRow];
|
|
}
|
|
}
|
|
}
|
|
|
|
static void addScalar(const T1 *matrix1, const T2 scalar, T3 *result, uint8_t rows,
|
|
uint8_t columns) {
|
|
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++) {
|
|
for (uint8_t resultRow = 0; resultRow < rows; resultRow++) {
|
|
result[resultColumn + columns * resultRow] =
|
|
matrix1[resultColumn + columns * resultRow] + scalar;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void multiplyScalar(const T1 *matrix1, const T2 scalar, T3 *result, uint8_t rows,
|
|
uint8_t columns) {
|
|
for (uint8_t resultColumn = 0; resultColumn < columns; resultColumn++) {
|
|
for (uint8_t resultRow = 0; resultRow < rows; resultRow++) {
|
|
result[resultColumn + columns * resultRow] =
|
|
matrix1[resultColumn + columns * resultRow] * scalar;
|
|
}
|
|
}
|
|
}
|
|
|
|
static bool isFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
|
|
for (uint8_t col = 0; col < cols; col++) {
|
|
for (uint8_t row = 0; row < rows; row++) {
|
|
if (not std::isfinite(inputMatrix[row * cols + cols])) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void writeSubmatrix(T1 *mainMatrix, T1 *subMatrix, uint8_t subRows, uint8_t subCols,
|
|
uint8_t mainRows, uint8_t mainCols, uint8_t startRow,
|
|
uint8_t startCol) {
|
|
if ((startRow + subRows > mainRows) or (startCol + subCols > mainCols)) {
|
|
return;
|
|
}
|
|
for (uint8_t row = 0; row < subRows; row++) {
|
|
for (uint8_t col = 0; col < subCols; col++) {
|
|
mainMatrix[(startRow + row) * mainCols + (startCol + col)] = subMatrix[row * subCols + col];
|
|
}
|
|
}
|
|
}
|
|
|
|
static ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
|
|
// Stopwatch stopwatch;
|
|
T1 matrix[size][size], identity[size][size];
|
|
// reformat array to matrix
|
|
for (uint8_t row = 0; row < size; row++) {
|
|
for (uint8_t col = 0; col < size; col++) {
|
|
matrix[row][col] = inputMatrix[row * size + col];
|
|
}
|
|
}
|
|
// init identity matrix
|
|
std::memset(identity, 0.0, sizeof(identity));
|
|
for (uint8_t diag = 0; diag < size; diag++) {
|
|
identity[diag][diag] = 1;
|
|
}
|
|
// gauss-jordan algo
|
|
// sort matrix such as no diag entry shall be 0
|
|
for (uint8_t row = 0; row < size; row++) {
|
|
if (matrix[row][row] == 0.0) {
|
|
bool swaped = false;
|
|
uint8_t rowIndex = 0;
|
|
while ((rowIndex < size) && !swaped) {
|
|
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
|
|
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
|
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
|
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
|
}
|
|
swaped = true;
|
|
}
|
|
rowIndex++;
|
|
}
|
|
if (!swaped) {
|
|
return returnvalue::FAILED; // matrix not invertible
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int row = 0; row < size; row++) {
|
|
if (matrix[row][row] == 0.0) {
|
|
uint8_t rowIndex;
|
|
if (row == 0) {
|
|
rowIndex = size - 1;
|
|
} else {
|
|
rowIndex = row - 1;
|
|
}
|
|
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
|
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
|
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
|
}
|
|
row--;
|
|
if (row < 0) {
|
|
return returnvalue::FAILED; // Matrix is not invertible
|
|
}
|
|
}
|
|
}
|
|
// remove non diag elements in matrix (jordan)
|
|
for (int row = 0; row < size; row++) {
|
|
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
|
|
if (row != rowIndex) {
|
|
double ratio = matrix[rowIndex][row] / matrix[row][row];
|
|
for (int colIndex = 0; colIndex < size; colIndex++) {
|
|
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
|
|
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// normalize rows in matrix (gauss)
|
|
for (int row = 0; row < size; row++) {
|
|
for (int col = 0; col < size; col++) {
|
|
identity[row][col] = identity[row][col] / matrix[row][row];
|
|
}
|
|
}
|
|
std::memcpy(inverse, identity, sizeof(identity));
|
|
return returnvalue::OK; // successful inversion
|
|
}
|
|
|
|
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
|
|
int i, j;
|
|
double determinant = 0;
|
|
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
|
|
{matrix[3], matrix[4], matrix[5]},
|
|
{matrix[6], matrix[7], matrix[8]}};
|
|
|
|
for (i = 0; i < 3; i++) {
|
|
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
|
|
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
|
|
}
|
|
for (i = 0; i < 3; i++) {
|
|
for (j = 0; j < 3; j++) {
|
|
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
|
|
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
|
|
determinant;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void skewMatrix(const T1 *vector, T2 *result) {
|
|
// Input Dimension [3], Output [3][3]
|
|
result[0] = 0;
|
|
result[1] = -vector[2];
|
|
result[2] = vector[1];
|
|
result[3] = vector[2];
|
|
result[4] = 0;
|
|
result[5] = -vector[0];
|
|
result[6] = -vector[1];
|
|
result[7] = vector[0];
|
|
result[8] = 0;
|
|
}
|
|
};
|
|
|
|
#endif /* MATRIXOPERATIONS_H_ */
|