/* * matrix inverse code for any square matrix using LU decomposition * inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix * ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf * @param m, input 4x4 matrix * @param inv, Output inverted 4x4 matrix * @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) { float *L, *U, *P; bool ret = true; L = new float[n * n]; U = new float[n * n]; P = new float[n * n]; mat_LU_decompose(A, L, U, P, n); float *L_inv = new float[n * n]; float *U_inv = new float[n * n]; memset(L_inv, 0, n * n * sizeof(float)); mat_forward_sub(L, L_inv, n); memset(U_inv, 0, n * n * sizeof(float)); 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); //check sanity of results for (uint8_t i = 0; i < n; i++) { for (uint8_t j = 0; j < n; j++) { if (!PX4_ISFINITE(inv_pivoted[i * n + j])) { ret = false; } } } memcpy(inv, inv_pivoted, n * n * sizeof(float)); //free memory delete[] inv_pivoted; delete[] inv_unpivoted; delete[] P; delete[] U_inv; delete[] L_inv; return ret; }
/* * matrix inverse code for any square matrix using LU decomposition * inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix * ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf * @param m, input 4x4 matrix * @param inv, Output inverted 4x4 matrix * @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) { float *L, *U, *P; bool ret = true; L = new float[n*n]; U = new float[n*n]; P = new float[n*n]; mat_LU_decompose(A,L,U,P,n); float *L_inv = new float[n*n]; float *U_inv = new float[n*n]; memset(L_inv,0,n*n*sizeof(float)); mat_forward_sub(L,L_inv,n); memset(U_inv,0,n*n*sizeof(float)); mat_back_sub(U,U_inv,n); // decomposed matrices no loger required free(L); free(U); float *inv_unpivoted = mat_mul(U_inv,L_inv,n); float *inv_pivoted = mat_mul(inv_unpivoted, P, n); //check sanity of results for(uint8_t i = 0; i < n; i++) { for(uint8_t j = 0; j < n; j++) { if(isnan(inv_pivoted[i*n+j]) || isinf(inv_pivoted[i*n+j])){ ret = false; } } } memcpy(inv,inv_pivoted,n*n*sizeof(float)); //free memory free(inv_pivoted); free(inv_unpivoted); free(P); free(U_inv); free(L_inv); return ret; }