|
|
|
@ -72,7 +72,7 @@ static inline void swap(float &a, float &b)
@@ -72,7 +72,7 @@ static inline void swap(float &a, float &b)
|
|
|
|
|
* @returns false = matrix is Singular or non positive definite, true = matrix inversion successful |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
void mat_pivot(float* A, float* pivot, uint8_t n) |
|
|
|
|
static void mat_pivot(float* A, float* pivot, uint8_t n) |
|
|
|
|
{ |
|
|
|
|
for(uint8_t i = 0;i<n;i++){ |
|
|
|
|
for(uint8_t j=0;j<n;j++) { |
|
|
|
@ -104,7 +104,7 @@ void mat_pivot(float* A, float* pivot, uint8_t n)
@@ -104,7 +104,7 @@ void mat_pivot(float* A, float* pivot, uint8_t n)
|
|
|
|
|
* @param n, dimension of matrix |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
void mat_forward_sub(float *L, float *out, uint8_t n) |
|
|
|
|
static void mat_forward_sub(float *L, float *out, uint8_t n) |
|
|
|
|
{ |
|
|
|
|
// Forward substitution solve LY = I
|
|
|
|
|
for(int i = 0; i < n; i++) { |
|
|
|
@ -126,7 +126,7 @@ void mat_forward_sub(float *L, float *out, uint8_t n)
@@ -126,7 +126,7 @@ void mat_forward_sub(float *L, float *out, uint8_t n)
|
|
|
|
|
* @param n, dimension of matrix |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
void mat_back_sub(float *U, float *out, uint8_t n) |
|
|
|
|
static void mat_back_sub(float *U, float *out, uint8_t n) |
|
|
|
|
{ |
|
|
|
|
// Backward Substitution solve UY = I
|
|
|
|
|
for(int i = n-1; i >= 0; i--) { |
|
|
|
@ -149,7 +149,7 @@ void mat_back_sub(float *U, float *out, uint8_t n)
@@ -149,7 +149,7 @@ void mat_back_sub(float *U, float *out, uint8_t n)
|
|
|
|
|
* @param n, dimension of matrix |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
void mat_LU_decompose(float* A, float* L, float* U, float *P, uint8_t n) |
|
|
|
|
static void mat_LU_decompose(float* A, float* L, float* U, float *P, uint8_t n) |
|
|
|
|
{ |
|
|
|
|
memset(L,0,n*n*sizeof(float)); |
|
|
|
|
memset(U,0,n*n*sizeof(float)); |
|
|
|
@ -189,7 +189,7 @@ void mat_LU_decompose(float* A, float* L, float* U, float *P, uint8_t n)
@@ -189,7 +189,7 @@ void mat_LU_decompose(float* A, float* L, float* U, float *P, uint8_t n)
|
|
|
|
|
* @param n, dimension of square matrix |
|
|
|
|
* @returns false = matrix is Singular, true = matrix inversion successful |
|
|
|
|
*/ |
|
|
|
|
bool mat_inverse(float* A, float* inv, uint8_t n) |
|
|
|
|
static bool mat_inverse(float* A, float* inv, uint8_t n) |
|
|
|
|
{ |
|
|
|
|
float *L, *U, *P; |
|
|
|
|
bool ret = true; |
|
|
|
|