/*
* 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;
}
