commander calibration routines initialize everything

src/lib/mathlib/math/matrix_alg.cpp

@@ -256,135 +256,3 @@ bool mat_inverse(float *A, float *inv, uint8_t n)
 	delete[] L_inv;
 	return ret;
 }
-
-bool inverse4x4(float m[], float invOut[])
-{
-	float inv[16], det;
-	uint8_t i;
-
-	inv[0] = m[5]  * m[10] * m[15] -
-		 m[5]  * m[11] * m[14] -
-		 m[9]  * m[6]  * m[15] +
-		 m[9]  * m[7]  * m[14] +
-		 m[13] * m[6]  * m[11] -
-		 m[13] * m[7]  * m[10];
-
-	inv[4] = -m[4]  * m[10] * m[15] +
-		 m[4]  * m[11] * m[14] +
-		 m[8]  * m[6]  * m[15] -
-		 m[8]  * m[7]  * m[14] -
-		 m[12] * m[6]  * m[11] +
-		 m[12] * m[7]  * m[10];
-
-	inv[8] = m[4]  * m[9] * m[15] -
-		 m[4]  * m[11] * m[13] -
-		 m[8]  * m[5] * m[15] +
-		 m[8]  * m[7] * m[13] +
-		 m[12] * m[5] * m[11] -
-		 m[12] * m[7] * m[9];
-
-	inv[12] = -m[4]  * m[9] * m[14] +
-		  m[4]  * m[10] * m[13] +
-		  m[8]  * m[5] * m[14] -
-		  m[8]  * m[6] * m[13] -
-		  m[12] * m[5] * m[10] +
-		  m[12] * m[6] * m[9];
-
-	inv[1] = -m[1]  * m[10] * m[15] +
-		 m[1]  * m[11] * m[14] +
-		 m[9]  * m[2] * m[15] -
-		 m[9]  * m[3] * m[14] -
-		 m[13] * m[2] * m[11] +
-		 m[13] * m[3] * m[10];
-
-	inv[5] = m[0]  * m[10] * m[15] -
-		 m[0]  * m[11] * m[14] -
-		 m[8]  * m[2] * m[15] +
-		 m[8]  * m[3] * m[14] +
-		 m[12] * m[2] * m[11] -
-		 m[12] * m[3] * m[10];
-
-	inv[9] = -m[0]  * m[9] * m[15] +
-		 m[0]  * m[11] * m[13] +
-		 m[8]  * m[1] * m[15] -
-		 m[8]  * m[3] * m[13] -
-		 m[12] * m[1] * m[11] +
-		 m[12] * m[3] * m[9];
-
-	inv[13] = m[0]  * m[9] * m[14] -
-		  m[0]  * m[10] * m[13] -
-		  m[8]  * m[1] * m[14] +
-		  m[8]  * m[2] * m[13] +
-		  m[12] * m[1] * m[10] -
-		  m[12] * m[2] * m[9];
-
-	inv[2] = m[1]  * m[6] * m[15] -
-		 m[1]  * m[7] * m[14] -
-		 m[5]  * m[2] * m[15] +
-		 m[5]  * m[3] * m[14] +
-		 m[13] * m[2] * m[7] -
-		 m[13] * m[3] * m[6];
-
-	inv[6] = -m[0]  * m[6] * m[15] +
-		 m[0]  * m[7] * m[14] +
-		 m[4]  * m[2] * m[15] -
-		 m[4]  * m[3] * m[14] -
-		 m[12] * m[2] * m[7] +
-		 m[12] * m[3] * m[6];
-
-	inv[10] = m[0]  * m[5] * m[15] -
-		  m[0]  * m[7] * m[13] -
-		  m[4]  * m[1] * m[15] +
-		  m[4]  * m[3] * m[13] +
-		  m[12] * m[1] * m[7] -
-		  m[12] * m[3] * m[5];
-
-	inv[14] = -m[0]  * m[5] * m[14] +
-		  m[0]  * m[6] * m[13] +
-		  m[4]  * m[1] * m[14] -
-		  m[4]  * m[2] * m[13] -
-		  m[12] * m[1] * m[6] +
-		  m[12] * m[2] * m[5];
-
-	inv[3] = -m[1] * m[6] * m[11] +
-		 m[1] * m[7] * m[10] +
-		 m[5] * m[2] * m[11] -
-		 m[5] * m[3] * m[10] -
-		 m[9] * m[2] * m[7] +
-		 m[9] * m[3] * m[6];
-
-	inv[7] = m[0] * m[6] * m[11] -
-		 m[0] * m[7] * m[10] -
-		 m[4] * m[2] * m[11] +
-		 m[4] * m[3] * m[10] +
-		 m[8] * m[2] * m[7] -
-		 m[8] * m[3] * m[6];
-
-	inv[11] = -m[0] * m[5] * m[11] +
-		  m[0] * m[7] * m[9] +
-		  m[4] * m[1] * m[11] -
-		  m[4] * m[3] * m[9] -
-		  m[8] * m[1] * m[7] +
-		  m[8] * m[3] * m[5];
-
-	inv[15] = m[0] * m[5] * m[10] -
-		  m[0] * m[6] * m[9] -
-		  m[4] * m[1] * m[10] +
-		  m[4] * m[2] * m[9] +
-		  m[8] * m[1] * m[6] -
-		  m[8] * m[2] * m[5];
-
-	det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
-
-	if (fabsf(det) < 1.1755e-38f) {
-		return false;
-	}
-
-	det = 1.0f / det;
-
-	for (i = 0; i < 16; i++) {
-		invOut[i] = inv[i] * det;
-	}
-
-	return true;
-}

src/lib/mathlib/math/matrix_alg.h

@@ -47,4 +47,3 @@
 
 float *mat_mul(float *A, float *B, uint8_t n);
 bool mat_inverse(float *A, float *inv, uint8_t n);
-bool inverse4x4(float m[], float invOut[]);