From 3b3e2c01f87c4ab875bf8e0774d2c6642305b451 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sat, 9 Jan 2021 23:39:13 +0530 Subject: [PATCH] AP_Math: templatize matrix_alg methods --- libraries/AP_Math/AP_Math.h | 13 ++- .../AP_Math/{matrix_alg.cpp => matrix_alg.h} | 104 ++++++++++-------- libraries/AP_Math/polyfit.cpp | 4 +- libraries/AP_Math/polyfit.h | 4 +- 4 files changed, 69 insertions(+), 56 deletions(-) rename libraries/AP_Math/{matrix_alg.cpp => matrix_alg.h} (83%) diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 2280aca771..6c55d62cc0 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -83,17 +83,22 @@ template float safe_sqrt(const T v); // invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular -bool inverse3x3(const float m[], float invOut[]) WARN_IF_UNUSED; +template +bool inverse3x3(const T m[], T invOut[]) WARN_IF_UNUSED; // invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular -bool inverse4x4(const float m[],float invOut[]) WARN_IF_UNUSED; +template +bool inverse4x4(const T m[],T invOut[]) WARN_IF_UNUSED; // matrix multiplication of two NxN matrices -float *mat_mul(const float *A, const float *B, uint8_t n); +template +float *mat_mul(const T *A, const T *B, uint8_t n); // matrix algebra -bool inverse(const float x[], float y[], uint16_t dim) WARN_IF_UNUSED; +template +bool inverse(const T x[], T y[], uint16_t dim) WARN_IF_UNUSED; +#include "matrix_alg.h" /* * Constrain an angle to be within the range: -180 to 180 degrees. The second * parameter changes the units. Default: 1 == degrees, 10 == dezi, diff --git a/libraries/AP_Math/matrix_alg.cpp b/libraries/AP_Math/matrix_alg.h similarity index 83% rename from libraries/AP_Math/matrix_alg.cpp rename to libraries/AP_Math/matrix_alg.h index 4ef741494a..29ec0ee25f 100644 --- a/libraries/AP_Math/matrix_alg.cpp +++ b/libraries/AP_Math/matrix_alg.h @@ -16,7 +16,7 @@ * with this program. If not, see . */ #pragma GCC optimize("O2") - +#pragma once #include #include @@ -24,9 +24,8 @@ #include #endif -#include -extern const AP_HAL::HAL& hal; +// extern const AP_HAL::HAL& hal; //TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations @@ -38,11 +37,11 @@ extern const AP_HAL::HAL& hal; * @param n, dimemsion of square matrices * @returns multiplied matrix i.e. A*B */ - -float* mat_mul(const float *A, const float *B, uint8_t n) +template +T* matrix_multiply(const T *A, const T *B, uint8_t n) { - float* ret = new float[n*n]; - memset(ret,0.0f,n*n*sizeof(float)); + T* ret = new T[n*n]; + memset(ret,0.0f,n*n*sizeof(T)); for(uint8_t i = 0; i < n; i++) { for(uint8_t j = 0; j < n; j++) { @@ -54,9 +53,10 @@ float* mat_mul(const float *A, const float *B, uint8_t n) return ret; } -static inline void swap(float &a, float &b) +template +static inline void swap(T &a, T &b) { - float c; + T c; c = a; a = b; b = c; @@ -70,20 +70,26 @@ static inline void swap(float &a, float &b) * @param n, dimenstion of square matrix * @returns false = matrix is Singular or non positive definite, true = matrix inversion successful */ - -static void mat_pivot(const float* A, float* pivot, uint8_t n) +template +static void mat_pivot(const T* A, T* pivot, uint8_t n) { for(uint8_t i = 0;i(i==j); + pivot[i*n+j] = static_cast(i==j); } } for(uint8_t i = 0;i < n; i++) { uint8_t max_j = i; for(uint8_t j=i;j fabsf(A[max_j*n + i])) { - max_j = j; + if (std::is_same::value) { + if(fabs(A[j*n + i]) > fabs(A[max_j*n + i])) { + max_j = j; + } + } else { + if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) { + max_j = j; + } } } @@ -102,8 +108,8 @@ static void mat_pivot(const float* A, float* pivot, uint8_t n) * @param out, Output inverted lower triangular matrix * @param n, dimension of matrix */ - -static void mat_forward_sub(const float *L, float *out, uint8_t n) +template +static void mat_forward_sub(const T *L, T *out, uint8_t n) { // Forward substitution solve LY = I for(int i = 0; i < n; i++) { @@ -124,8 +130,8 @@ static void mat_forward_sub(const float *L, float *out, uint8_t n) * @param out, Output inverted upper triangular matrix * @param n, dimension of matrix */ - -static void mat_back_sub(const float *U, float *out, uint8_t n) +template +static void mat_back_sub(const T *U, T *out, uint8_t n) { // Backward Substitution solve UY = I for(int i = n-1; i >= 0; i--) { @@ -147,15 +153,15 @@ static void mat_back_sub(const float *U, float *out, uint8_t n) * @param out, Output inverted upper triangular matrix * @param n, dimension of matrix */ - -static void mat_LU_decompose(const float* A, float* L, float* U, float *P, uint8_t n) +template +static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n) { - memset(L,0,n*n*sizeof(float)); - memset(U,0,n*n*sizeof(float)); - memset(P,0,n*n*sizeof(float)); + memset(L,0,n*n*sizeof(T)); + memset(U,0,n*n*sizeof(T)); + memset(P,0,n*n*sizeof(T)); mat_pivot(A,P,n); - float *APrime = mat_mul(P,A,n); + T *APrime = matrix_multiply(P,A,n); for(uint8_t i = 0; i < n; i++) { L[i*n + i] = 1; } @@ -188,30 +194,31 @@ static void mat_LU_decompose(const float* A, float* L, float* U, float *P, uint8 * @param n, dimension of square matrix * @returns false = matrix is Singular, true = matrix inversion successful */ -static bool mat_inverse(const float* A, float* inv, uint8_t n) +template +static bool mat_inverse(const T* A, T* inv, uint8_t n) { - float *L, *U, *P; + T *L, *U, *P; bool ret = true; - L = new float[n*n]; - U = new float[n*n]; - P = new float[n*n]; + L = new T[n*n]; + U = new T[n*n]; + P = new T[n*n]; mat_LU_decompose(A,L,U,P,n); - float *L_inv = new float[n*n]; - float *U_inv = new float[n*n]; + T *L_inv = new T[n*n]; + T *U_inv = new T[n*n]; - memset(L_inv,0,n*n*sizeof(float)); + memset(L_inv,0,n*n*sizeof(T)); mat_forward_sub(L,L_inv,n); - memset(U_inv,0,n*n*sizeof(float)); + memset(U_inv,0,n*n*sizeof(T)); mat_back_sub(U,U_inv,n); // decomposed matrices no longer required delete[] L; delete[] U; - float *inv_unpivoted = mat_mul(U_inv,L_inv,n); - float *inv_pivoted = mat_mul(inv_unpivoted, P, n); + T *inv_unpivoted = matrix_multiply(U_inv,L_inv,n); + T *inv_pivoted = matrix_multiply(inv_unpivoted, P, n); //check sanity of results for(uint8_t i = 0; i < n; i++) { @@ -221,7 +228,7 @@ static bool mat_inverse(const float* A, float* inv, uint8_t n) } } } - memcpy(inv,inv_pivoted,n*n*sizeof(float)); + memcpy(inv,inv_pivoted,n*n*sizeof(T)); //free memory delete[] inv_pivoted; @@ -239,19 +246,19 @@ static bool mat_inverse(const float* A, float* inv, uint8_t n) * @param invOut, Output inverted 4x4 matrix * @returns false = matrix is Singular, true = matrix inversion successful */ - -bool inverse3x3(const float m[], float invOut[]) +template +bool inverse3x3(const T m[], T invOut[]) { - float inv[9]; + T inv[9]; // computes the inverse of a matrix m - float det = m[0] * (m[4] * m[8] - m[7] * m[5]) - + T det = m[0] * (m[4] * m[8] - m[7] * m[5]) - m[1] * (m[3] * m[8] - m[5] * m[6]) + m[2] * (m[3] * m[7] - m[4] * m[6]); if (is_zero(det) || isinf(det)) { return false; } - float invdet = 1 / det; + T invdet = 1 / det; inv[0] = (m[4] * m[8] - m[7] * m[5]) * invdet; inv[1] = (m[2] * m[7] - m[1] * m[8]) * invdet; @@ -278,17 +285,17 @@ bool inverse3x3(const float m[], float invOut[]) * @param invOut, Output inverted 4x4 matrix * @returns false = matrix is Singular, true = matrix inversion successful */ - -bool inverse4x4(const float m[],float invOut[]) +template +bool inverse4x4(const T m[],T invOut[]) { - float inv[16], det; + T inv[16], det; uint8_t i; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL //disable FE_INEXACT detection as it fails on mac os runs int old = fedisableexcept(FE_INEXACT | FE_OVERFLOW); if (old < 0) { - hal.console->printf("inverse4x4(): warning: error on disabling FE_OVERFLOW floating point exception\n"); + // hal.console->printf("inverse4x4(): warning: error on disabling FE_OVERFLOW floating point exception\n"); } #endif @@ -417,7 +424,7 @@ bool inverse4x4(const float m[],float invOut[]) #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (old >= 0 && feenableexcept(old) < 0) { - hal.console->printf("inverse4x4(): warning: error on restoring floating exception mask\n"); + // hal.console->printf("inverse4x4(): warning: error on restoring floating exception mask\n"); } #endif @@ -432,11 +439,12 @@ bool inverse4x4(const float m[],float invOut[]) * @param n, dimension of square matrix * @returns false = matrix is Singular, true = matrix inversion successful */ -bool inverse(const float x[], float y[], uint16_t dim) +template +bool inverse(const T x[], T y[], uint16_t dim) { switch(dim){ case 3: return inverse3x3(x,y); case 4: return inverse4x4(x,y); - default: return mat_inverse(x,y,dim); + default: return mat_inverse(x,y,(uint8_t)dim); } } diff --git a/libraries/AP_Math/polyfit.cpp b/libraries/AP_Math/polyfit.cpp index b313f8cab0..15bd9d2228 100644 --- a/libraries/AP_Math/polyfit.cpp +++ b/libraries/AP_Math/polyfit.cpp @@ -9,7 +9,7 @@ template void PolyFit::update(float x, float y) { - float temp = 1; + double temp = 1; for (int8_t i = 2*(order-1); i >= 0; i--) { int8_t k = (i::update(float x, float y) template bool PolyFit::get_polynomial(float res[order]) const { - float inv_mat[order][order]; + double inv_mat[order][order]; if (!inverse(&mat[0][0], &inv_mat[0][0], order)) { return false; } diff --git a/libraries/AP_Math/polyfit.h b/libraries/AP_Math/polyfit.h index 1228eb085e..3797609427 100644 --- a/libraries/AP_Math/polyfit.h +++ b/libraries/AP_Math/polyfit.h @@ -17,7 +17,7 @@ public: bool get_polynomial(float res[order]) const; private: - float mat[order][order]; - float vec[order]; + double mat[order][order]; + double vec[order]; };