From 999268cbba2a17daa766e671c9025cc1e6f65d15 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Jan 2021 11:39:26 +1100 Subject: [PATCH] AP_Math: fixup matrix algorithms to be in cpp file this fixes an issue where optimize O2 was forced on any file that included AP_Math.h. It also fixes the test suite for matrix_alg, and fixes the type handling to be consistent --- libraries/AP_Math/AP_Math.h | 19 ++-- .../examples/matrix_alg/matrix_alg.cpp | 65 +++++------ libraries/AP_Math/examples/matrix_alg/wscript | 7 ++ .../AP_Math/{matrix_alg.h => matrix_alg.cpp} | 101 +++++++++++------- 4 files changed, 105 insertions(+), 87 deletions(-) create mode 100644 libraries/AP_Math/examples/matrix_alg/wscript rename libraries/AP_Math/{matrix_alg.h => matrix_alg.cpp} (82%) diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 6c55d62cc0..63ee3dd405 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -82,23 +82,18 @@ float safe_asin(const T v); template float safe_sqrt(const T v); -// invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular -template -bool inverse3x3(const T m[], T invOut[]) WARN_IF_UNUSED; - -// invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular -template -bool inverse4x4(const T m[],T invOut[]) WARN_IF_UNUSED; - // matrix multiplication of two NxN matrices template -float *mat_mul(const T *A, const T *B, uint8_t n); +void mat_mul(const T *A, const T *B, T *C, uint16_t n); -// matrix algebra +// matrix inverse template -bool inverse(const T x[], T y[], uint16_t dim) WARN_IF_UNUSED; +bool mat_inverse(const T *x, T *y, uint16_t dim) WARN_IF_UNUSED; + +// matrix identity +template +void mat_identity(T *x, uint16_t dim); -#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/examples/matrix_alg/matrix_alg.cpp b/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp index bcbdc0ff73..6670bb4581 100644 --- a/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp +++ b/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp @@ -5,10 +5,16 @@ #include #include #include + +void setup(); +void loop(); + const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #define MAT_ALG_ACCURACY 1e-4f +typedef float ftype; + static uint16_t get_random(void) { static uint32_t m_z = 1234; @@ -19,7 +25,7 @@ static uint16_t get_random(void) } -static void show_matrix(float *A, int n) { +static void show_matrix(ftype *A, int n) { for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) printf("%.10f ", A[i * n + j]); @@ -27,7 +33,7 @@ static void show_matrix(float *A, int n) { } } -static bool compare_mat(const float *A, const float *B, const uint8_t n) +static bool compare_mat(const ftype *A, const ftype *B, const uint8_t n) { for(uint8_t i = 0; i < n; i++) { for(uint8_t j = 0; j < n; j++) { @@ -42,22 +48,17 @@ static bool compare_mat(const float *A, const float *B, const uint8_t n) static void test_matrix_inverse(void) { //fast inverses - float test_mat[25],ident_mat[25]; - float *out_mat; + ftype test_mat[25],ident_mat[25]; + ftype out_mat[25], out_mat2[25], mat[25]; for(uint8_t i = 0;i<25;i++) { - test_mat[i] = pow(-1,i)*get_random()/0.7f; + test_mat[i] = powf(-1,i)*get_random()/0.7f; } - float mat[25]; //Test for 3x3 matrix - memset(ident_mat,0,sizeof(ident_mat)); - for(uint8_t i=0; i<3; i++) { - ident_mat[i*3+i] = 1.0f; - } - if(inverse(test_mat,mat,3)){ - out_mat = mat_mul(test_mat,mat,3); - inverse(mat,mat,3); + mat_identity(ident_mat, 3); + if (mat_inverse(test_mat,mat,3) && mat_inverse(mat, out_mat2, 3)) { + mat_mul(test_mat, mat, out_mat, 3); } else { hal.console->printf("3x3 Matrix is Singular!\n"); return; @@ -70,25 +71,22 @@ static void test_matrix_inverse(void) printf("\nInv(A) * A\n"); show_matrix(out_mat,3); printf("\n"); - //compare matrix - if(!compare_mat(test_mat,mat,3)) { + + // compare matrix + if (!compare_mat(test_mat, out_mat2, 3)) { printf("Test Failed!!\n"); return; } - if(!compare_mat(ident_mat,out_mat,3)) { + if (!compare_mat(ident_mat, out_mat, 3)) { printf("Identity output Test Failed!!\n"); return; } //Test for 4x4 matrix - memset(ident_mat,0,sizeof(ident_mat)); - for(uint8_t i=0; i<4; i++) { - ident_mat[i*4+i] = 1.0f; - } - if(inverse(test_mat,mat,4)){ - out_mat = mat_mul(test_mat,mat,4); - inverse(mat,mat,4); + mat_identity(ident_mat, 4); + if (mat_inverse(test_mat, mat, 4) && mat_inverse(mat, out_mat2, 4)){ + mat_mul(test_mat, mat, out_mat, 4); } else { hal.console->printf("4x4 Matrix is Singular!\n"); return; @@ -100,29 +98,22 @@ static void test_matrix_inverse(void) printf("\nInv(A) * A\n"); show_matrix(out_mat,4); printf("\n"); - if(!compare_mat(test_mat,mat,4)) { + if (!compare_mat(test_mat, out_mat2, 4)) { printf("Test Failed!!\n"); return; } - if(!compare_mat(ident_mat,out_mat,4)) { + if (!compare_mat(ident_mat,out_mat,4)) { printf("Identity output Test Failed!!\n"); return; } - - //Test for 5x5 matrix - memset(ident_mat,0,sizeof(ident_mat)); - for(uint8_t i=0; i<5; i++) { - ident_mat[i*5+i] = 1.0f; - } - if(inverse(test_mat,mat,5)) { - out_mat = mat_mul(test_mat,mat,5); - inverse(mat,mat,5); + mat_identity(ident_mat, 5); + if (mat_inverse(test_mat,mat,5) && mat_inverse(mat, out_mat2, 5)) { + mat_mul(test_mat, mat, out_mat, 5); } else { hal.console->printf("5x5 Matrix is Singular!\n"); return; - } printf("\n\n5x5 Test Matrix:\n"); @@ -132,11 +123,11 @@ static void test_matrix_inverse(void) printf("\nInv(A) * A\n"); show_matrix(out_mat,5); printf("\n"); - if(!compare_mat(test_mat,mat,5)) { + if (!compare_mat(test_mat, out_mat2, 5)) { printf("Test Failed!!\n"); return; } - if(!compare_mat(ident_mat,out_mat,5)) { + if (!compare_mat(ident_mat, out_mat, 5)) { printf("Identity output Test Failed!!\n"); return; } diff --git a/libraries/AP_Math/examples/matrix_alg/wscript b/libraries/AP_Math/examples/matrix_alg/wscript new file mode 100644 index 0000000000..719ec151ba --- /dev/null +++ b/libraries/AP_Math/examples/matrix_alg/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + bld.ap_example( + use='ap', + ) diff --git a/libraries/AP_Math/matrix_alg.h b/libraries/AP_Math/matrix_alg.cpp similarity index 82% rename from libraries/AP_Math/matrix_alg.h rename to libraries/AP_Math/matrix_alg.cpp index 29ec0ee25f..c2962a5b0f 100644 --- a/libraries/AP_Math/matrix_alg.h +++ b/libraries/AP_Math/matrix_alg.cpp @@ -15,20 +15,15 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . */ -#pragma GCC optimize("O2") -#pragma once + #include +#include "AP_Math.h" #include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #endif - -// extern const AP_HAL::HAL& hal; - -//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations - /* * Does matrix multiplication of two regular/square matrices * @@ -38,14 +33,14 @@ * @returns multiplied matrix i.e. A*B */ template -T* matrix_multiply(const T *A, const T *B, uint8_t n) +static T* matrix_multiply(const T *A, const T *B, uint16_t n) { 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++) { - for(uint8_t k = 0;k < n; k++) { + for(uint16_t i = 0; i < n; i++) { + for(uint16_t j = 0; j < n; j++) { + for(uint16_t k = 0;k < n; k++) { ret[i*n + j] += A[i*n + k] * B[k*n + j]; } } @@ -71,19 +66,19 @@ static inline void swap(T &a, T &b) * @returns false = matrix is Singular or non positive definite, true = matrix inversion successful */ template -static void mat_pivot(const T* A, T* pivot, uint8_t n) +static void mat_pivot(const T* A, T* pivot, uint16_t n) { - for(uint8_t i = 0;i(i==j); } } - for(uint8_t i = 0;i < n; i++) { - uint8_t max_j = i; - for(uint8_t j=i;j::value) { - if(fabs(A[j*n + i]) > fabs(A[max_j*n + i])) { + if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) { max_j = j; } } else { @@ -94,7 +89,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n) } if(max_j != i) { - for(uint8_t k = 0; k < n; k++) { + for(uint16_t k = 0; k < n; k++) { swap(pivot[i*n + k], pivot[max_j*n + k]); } } @@ -109,7 +104,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n) * @param n, dimension of matrix */ template -static void mat_forward_sub(const T *L, T *out, uint8_t n) +static void mat_forward_sub(const T *L, T *out, uint16_t n) { // Forward substitution solve LY = I for(int i = 0; i < n; i++) { @@ -131,7 +126,7 @@ static void mat_forward_sub(const T *L, T *out, uint8_t n) * @param n, dimension of matrix */ template -static void mat_back_sub(const T *U, T *out, uint8_t n) +static void mat_back_sub(const T *U, T *out, uint16_t n) { // Backward Substitution solve UY = I for(int i = n-1; i >= 0; i--) { @@ -154,7 +149,7 @@ static void mat_back_sub(const T *U, T *out, uint8_t n) * @param n, dimension of matrix */ template -static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n) +static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint16_t n) { memset(L,0,n*n*sizeof(T)); memset(U,0,n*n*sizeof(T)); @@ -162,20 +157,20 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n) mat_pivot(A,P,n); T *APrime = matrix_multiply(P,A,n); - for(uint8_t i = 0; i < n; i++) { + for(uint16_t i = 0; i < n; i++) { L[i*n + i] = 1; } - for(uint8_t i = 0; i < n; i++) { - for(uint8_t j = 0; j < n; j++) { + for(uint16_t i = 0; i < n; i++) { + for(uint16_t j = 0; j < n; j++) { if(j <= i) { U[j*n + i] = APrime[j*n + i]; - for(uint8_t k = 0; k < j; k++) { + for(uint16_t k = 0; k < j; k++) { U[j*n + i] -= L[j*n + k] * U[k*n + i]; } } if(j >= i) { L[j*n + i] = APrime[j*n + i]; - for(uint8_t k = 0; k < i; k++) { + for(uint16_t k = 0; k < i; k++) { L[j*n + i] -= L[j*n + k] * U[k*n + i]; } L[j*n + i] /= U[i*n + i]; @@ -195,7 +190,7 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n) * @returns false = matrix is Singular, true = matrix inversion successful */ template -static bool mat_inverse(const T* A, T* inv, uint8_t n) +static bool mat_inverseN(const T* A, T* inv, uint16_t n) { T *L, *U, *P; bool ret = true; @@ -221,8 +216,8 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n) T *inv_pivoted = matrix_multiply(inv_unpivoted, P, n); //check sanity of results - for(uint8_t i = 0; i < n; i++) { - for(uint8_t j = 0; j < n; j++) { + for(uint16_t i = 0; i < n; i++) { + for(uint16_t j = 0; j < n; j++) { if(isnan(inv_pivoted[i*n+j]) || isinf(inv_pivoted[i*n+j])){ ret = false; } @@ -247,7 +242,7 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n) * @returns false = matrix is Singular, true = matrix inversion successful */ template -bool inverse3x3(const T m[], T invOut[]) +static bool inverse3x3(const T m[], T invOut[]) { T inv[9]; // computes the inverse of a matrix m @@ -270,7 +265,7 @@ bool inverse3x3(const T m[], T invOut[]) inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet; inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet; - for(uint8_t i = 0; i < 9; i++){ + for(uint16_t i = 0; i < 9; i++){ invOut[i] = inv[i]; } @@ -286,10 +281,10 @@ bool inverse3x3(const T m[], T invOut[]) * @returns false = matrix is Singular, true = matrix inversion successful */ template -bool inverse4x4(const T m[],T invOut[]) +static bool inverse4x4(const T m[],T invOut[]) { T inv[16], det; - uint8_t i; + uint16_t i; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL //disable FE_INEXACT detection as it fails on mac os runs @@ -440,11 +435,41 @@ bool inverse4x4(const T m[],T invOut[]) * @returns false = matrix is Singular, true = matrix inversion successful */ template -bool inverse(const T x[], T y[], uint16_t dim) +bool mat_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,(uint8_t)dim); + case 3: return inverse3x3(x,y); + case 4: return inverse4x4(x,y); + default: return mat_inverseN(x,y,dim); } } + +template +void mat_mul(const T *A, const T *B, T *C, uint16_t n) +{ + memset(C, 0, sizeof(T)*n*n); + for(uint16_t i = 0; i < n; i++) { + for(uint16_t j = 0; j < n; j++) { + for(uint16_t k = 0;k < n; k++) { + C[i*n + j] += A[i*n + k] * B[k*n + j]; + } + } + } +} + +template +void mat_identity(T *A, uint16_t n) +{ + memset(A, 0, sizeof(T)*n*n); + for (uint16_t i=0; i(const float x[], float y[], uint16_t dim); +template void mat_mul(const float *A, const float *B, float *C, uint16_t n); +template void mat_identity(float x[], uint16_t dim); + +template bool mat_inverse(const double x[], double y[], uint16_t dim); +template void mat_mul(const double *A, const double *B, double *C, uint16_t n); +template void mat_identity(double x[], uint16_t dim);