diff options
Diffstat (limited to 'common/math_util.c')
| -rw-r--r-- | common/math_util.c | 92 |
1 files changed, 0 insertions, 92 deletions
diff --git a/common/math_util.c b/common/math_util.c index 56acec16dc..83bfc6cdfb 100644 --- a/common/math_util.c +++ b/common/math_util.c @@ -110,95 +110,3 @@ void rotate(const vector_3_t v, const matrix_3x3_t R, t[2] * R[2][2]; } -#ifdef CONFIG_ACCEL_CALIBRATE - -void matrix_multiply(matrix_3x3_t *m1, matrix_3x3_t *m2, matrix_3x3_t *res) -{ - (*res)[0][0] = (*m1)[0][0] * (*m2)[0][0] + (*m1)[0][1] * (*m2)[1][0] + - (*m1)[0][2] * (*m2)[2][0]; - (*res)[0][1] = (*m1)[0][0] * (*m2)[0][1] + (*m1)[0][1] * (*m2)[1][1] + - (*m1)[0][2] * (*m2)[2][1]; - (*res)[0][2] = (*m1)[0][0] * (*m2)[0][2] + (*m1)[0][1] * (*m2)[1][2] + - (*m1)[0][2] * (*m2)[2][2]; - - (*res)[1][0] = (*m1)[1][0] * (*m2)[0][0] + (*m1)[1][1] * (*m2)[1][0] + - (*m1)[1][2] * (*m2)[2][0]; - (*res)[1][1] = (*m1)[1][0] * (*m2)[0][1] + (*m1)[1][1] * (*m2)[1][1] + - (*m1)[1][2] * (*m2)[2][1]; - (*res)[1][2] = (*m1)[1][0] * (*m2)[0][2] + (*m1)[1][1] * (*m2)[1][2] + - (*m1)[1][2] * (*m2)[2][2]; - - (*res)[2][0] = (*m1)[2][0] * (*m2)[0][0] + (*m1)[2][1] * (*m2)[1][0] + - (*m1)[2][2] * (*m2)[2][0]; - (*res)[2][1] = (*m1)[2][0] * (*m2)[0][1] + (*m1)[2][1] * (*m2)[1][1] + - (*m1)[2][2] * (*m2)[2][1]; - (*res)[2][2] = (*m1)[2][0] * (*m2)[0][2] + (*m1)[2][1] * (*m2)[1][2] + - (*m1)[2][2] * (*m2)[2][2]; -} - -/** - * Find determinant of matrix. - * - * @param m Pointer to matrix to take determinant of. - * - * @return Determinant of matrix m. - */ -static float matrix_determinant(matrix_3x3_t *m) -{ - return (*m)[0][0]*(*m)[1][1]*(*m)[2][2] + - (*m)[1][0]*(*m)[2][1]*(*m)[0][2] + - (*m)[2][0]*(*m)[0][1]*(*m)[1][2] - - (*m)[0][0]*(*m)[2][1]*(*m)[1][2] - - (*m)[2][0]*(*m)[1][1]*(*m)[0][2] - - (*m)[1][0]*(*m)[0][1]*(*m)[2][2]; -} - -/** - * Find inverse of matrix. - * - * @param m Pointer to matrix to invert. - * @param res Pointer to resultant matrix. - */ -static int matrix_inverse(matrix_3x3_t *m, matrix_3x3_t *res) -{ - float det = matrix_determinant(m); - - /* - * If determinant is too close to zero, then input is not linearly - * independent enough, return an error. - */ - if (ABS(det) < 1e7) - return EC_ERROR_UNKNOWN; - - /* Find inverse of input matrix. */ - (*res)[0][0] = ((*m)[1][1]*(*m)[2][2] - (*m)[1][2]*(*m)[2][1]) / det; - (*res)[0][1] = ((*m)[0][2]*(*m)[2][1] - (*m)[0][1]*(*m)[2][2]) / det; - (*res)[0][2] = ((*m)[0][1]*(*m)[1][2] - (*m)[0][2]*(*m)[1][1]) / det; - - (*res)[1][0] = ((*m)[1][2]*(*m)[2][0] - (*m)[1][0]*(*m)[2][2]) / det; - (*res)[1][1] = ((*m)[0][0]*(*m)[2][2] - (*m)[0][2]*(*m)[2][0]) / det; - (*res)[1][2] = ((*m)[0][2]*(*m)[1][0] - (*m)[0][0]*(*m)[1][2]) / det; - - (*res)[2][0] = ((*m)[1][0]*(*m)[2][1] - (*m)[1][1]*(*m)[2][0]) / det; - (*res)[2][1] = ((*m)[0][1]*(*m)[2][0] - (*m)[0][0]*(*m)[2][1]) / det; - (*res)[2][2] = ((*m)[0][0]*(*m)[1][1] - (*m)[0][1]*(*m)[1][0]) / det; - - return EC_SUCCESS; -} - -int solve_rotation_matrix(matrix_3x3_t *in, matrix_3x3_t *out, matrix_3x3_t *R) -{ - static matrix_3x3_t i; - int ret; - - /* Calculate inverse of input matrix. */ - ret = matrix_inverse(in, &i); - if (ret != EC_SUCCESS) - return ret; - - /* Multiply inverse of in matrix by out matrix and store into R. */ - matrix_multiply(&i, out, R); - - return EC_SUCCESS; -} -#endif /* CONFIG_ACCEL_CALIBRATE */ |