mirror of https://github.com/rusefi/autopid.git
96 lines
2.0 KiB
C++
96 lines
2.0 KiB
C++
/*
|
|
* @file matrix_helper.h
|
|
*
|
|
* @date Sep 27, 2019
|
|
* @author andreika, (c) 2019
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "global.h"
|
|
|
|
|
|
template<typename FLOAT, int N>
|
|
class MatrixHelper {
|
|
public:
|
|
// Gauss-Jordan elimination method
|
|
static bool inverseMatrix(FLOAT dst[N][N], FLOAT src[N][N]) {
|
|
// a working copy of src (needs modification)
|
|
FLOAT tmp[N][N];
|
|
|
|
for (int i = 0; i < N; i++) {
|
|
for (int j = 0; j < N; j++) {
|
|
tmp[i][j] = src[i][j];
|
|
dst[i][j] = (i == j) ? 1.0 : 0; // fill as an identity matrix first
|
|
}
|
|
}
|
|
|
|
// determinant
|
|
FLOAT det = 1.0;
|
|
|
|
// for each pass, find the maximum element in the pivot column.
|
|
for (int pass = 0; pass < N; pass++) {
|
|
int row, maxRow = pass;
|
|
// find the largest element in the column
|
|
for (row = pass; row < N; row++) {
|
|
if (fabs(tmp[row][pass]) > fabs(tmp[maxRow][pass]))
|
|
maxRow = row;
|
|
}
|
|
// interchange the elements of these rows
|
|
if (maxRow != pass) {
|
|
for (int col = 0; col < N; col++) {
|
|
FLOAT temp = dst[pass][col];
|
|
dst[pass][col] = dst[maxRow][col];
|
|
dst[maxRow][col] = temp;
|
|
|
|
if (col >= pass) {
|
|
temp = tmp[pass][col];
|
|
tmp[pass][col] = tmp[maxRow][col];
|
|
tmp[maxRow][col] = temp;
|
|
}
|
|
}
|
|
}
|
|
|
|
// calculate the determinant as the product of the elements
|
|
FLOAT coef = tmp[pass][pass];
|
|
if (coef == 0.0)
|
|
return false;
|
|
det *= coef;
|
|
|
|
// normalize
|
|
for (int col = 0; col < N; col++) {
|
|
dst[pass][col] /= coef;
|
|
if (col >= pass)
|
|
tmp[pass][col] /= coef;
|
|
}
|
|
|
|
// add a multiple of the pivot row to each row
|
|
for (row = 0; row < N; row++) {
|
|
if (row != pass) {
|
|
coef = tmp[row][pass];
|
|
for (int col = 0; col < N; col++) {
|
|
dst[row][col] -= coef * dst[pass][col];
|
|
tmp[row][col] -= coef * tmp[pass][col];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static void multiplyMatrix(FLOAT dst[N][N], FLOAT src1[N][N], FLOAT src2[N][N]) {
|
|
for (int i = 0; i < N; i++) {
|
|
for (int j = 0; j < N; j++) {
|
|
dst[i][j] = 0;
|
|
for (int k = 0; k < N; k++) {
|
|
dst[i][j] += src1[i][k] * src2[k][j];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
|