UPSTREAM: math: Add inverse matrix calculation

Add a slow inverse matrix calculation function.
It is needed to apply factory offset properly.

Also consider the NULL matrix the identity matrix.

BRANCH=smaug,cyan, samus
TEST=Unit test
BUG=chromium:517675, b:27849483

Change-Id: Ifa11954992e6f2fab02b4e92684e7b01bbaafe94
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/294594
Reviewed-by: Sheng-liang Song <ssl@chromium.org>
(cherry picked from commit 0e01759cedcd25868196508177791388b89450e5)
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/370517

4 files changed, 100 insertions, 5 deletions

diff --git a/common/math_util.c b/common/math_util.c
index 120d13d63f..4077590c1a 100644
--- a/common/math_util.c
+++ b/common/math_util.c
@@ -166,6 +166,13 @@ void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res)
 {
 	int64_t t[3];
 
+	if (R == NULL) {
+		if (v != res)
+			memcpy(res, v, sizeof(*v));
+		return;
+	}
+
+
 	/* Rotate */
 	t[0] =	(int64_t)v[0] * R[0][0] +
 		(int64_t)v[1] * R[1][0] +
@@ -182,3 +189,53 @@ void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res)
 	res[1] = t[1] >> FP_BITS;
 	res[2] = t[2] >> FP_BITS;
 }
+
+void rotate_inv(const vector_3_t v, const matrix_3x3_t R, vector_3_t res)
+{
+	int64_t t[3];
+	fp_t deter;
+
+	if (R == NULL) {
+		if (v != res)
+			memcpy(res, v, sizeof(*v));
+		return;
+	}
+
+	deter = fp_mul(R[0][0], (fp_mul(R[1][1], R[2][2]) -
+				 fp_mul(R[2][1], R[1][2]))) -
+		fp_mul(R[0][1], (fp_mul(R[1][0], R[2][2]) -
+				 fp_mul(R[1][2], R[2][0]))) +
+		fp_mul(R[0][2], (fp_mul(R[1][0], R[2][1]) -
+				 fp_mul(R[1][1], R[2][0])));
+
+	/*
+	 * invert the matrix: from
+	 * http://stackoverflow.com/questions/983999/
+	 * simple-3x3-matrix-inverse-code-c
+	 */
+	t[0] =  (int64_t)v[0] * (fp_mul(R[1][1], R[2][2]) -
+				 fp_mul(R[2][1], R[1][2])) -
+		(int64_t)v[1] * (fp_mul(R[1][0], R[2][2]) -
+				 fp_mul(R[1][2], R[2][0])) +
+		(int64_t)v[2] * (fp_mul(R[1][0], R[2][1]) -
+				 fp_mul(R[2][0], R[1][1]));
+
+	t[1] =	(int64_t)v[0] * (fp_mul(R[0][1], R[2][2]) -
+				 fp_mul(R[0][2], R[2][1])) * -1 +
+		(int64_t)v[1] * (fp_mul(R[0][0], R[2][2]) -
+				 fp_mul(R[0][2], R[2][0])) -
+		(int64_t)v[2] * (fp_mul(R[0][0], R[2][1]) -
+				 fp_mul(R[2][0], R[0][1]));
+
+	t[2] =	(int64_t)v[0] * (fp_mul(R[0][1], R[1][2]) -
+				 fp_mul(R[0][2], R[1][1])) -
+		(int64_t)v[1] * (fp_mul(R[0][0], R[1][2]) -
+				 fp_mul(R[1][0], R[0][2])) +
+		(int64_t)v[2] * (fp_mul(R[0][0], R[1][1]) -
+				 fp_mul(R[1][0], R[0][1]));
+
+	/* Scale by fixed point shift when writing back to result */
+	res[0] = fp_div(t[0], deter) >> FP_BITS;
+	res[1] = fp_div(t[1], deter) >> FP_BITS;
+	res[2] = fp_div(t[2], deter) >> FP_BITS;
+}
diff --git a/include/math_util.h b/include/math_util.h
index e7dcc6cbec..b891ee38c5 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -112,6 +112,13 @@ fp_t cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
  */
 void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res);
 
-
+/**
+ * Rotate vector v by rotation matrix R^-1.
+ *
+ * @param v Vector to be rotated.
+ * @param R Rotation matrix.
+ * @param res Resultant vector.
+ */
+void rotate_inv(const vector_3_t v, const matrix_3x3_t R, vector_3_t res);
 
 #endif /* __CROS_MATH_UTIL_H */
diff --git a/test/math_util.c b/test/math_util.c
index e563d8dd02..3296e8da07 100644
--- a/test/math_util.c
+++ b/test/math_util.c
@@ -45,11 +45,45 @@ static int test_acos(void)
 	return EC_SUCCESS;
 }
 
+
+const matrix_3x3_t test_matrices[] = {
+	{{ 0, FLOAT_TO_FP(-1), 0},
+	 {FLOAT_TO_FP(-1), 0, 0},
+	 { 0, 0, FLOAT_TO_FP(1)} },
+	{{ FLOAT_TO_FP(1), 0, FLOAT_TO_FP(5)},
+	 { FLOAT_TO_FP(2), FLOAT_TO_FP(1), FLOAT_TO_FP(6)},
+	 { FLOAT_TO_FP(3), FLOAT_TO_FP(4), 0} }
+};
+
+
+static int test_rotate(void)
+{
+	int i, j, k;
+	vector_3_t v = {1, 2, 3};
+	vector_3_t w;
+
+	for (i = 0; i < ARRAY_SIZE(test_matrices); i++) {
+		for (j = 0; j < 100; j += 10) {
+			for (k = X; k <= Z; k++) {
+				v[k] += j;
+				v[k] %= 7;
+			}
+
+			rotate(v, test_matrices[i], w);
+			rotate_inv(w, test_matrices[i], w);
+			for (k = X; k <= Z; k++)
+				TEST_ASSERT(v[k] == w[k]);
+		}
+	}
+	return EC_SUCCESS;
+}
+
 void run_test(void)
 {
 	test_reset();
 
 	RUN_TEST(test_acos);
+	RUN_TEST(test_rotate);
 
 	test_print_result();
 }
diff --git a/test/motion_lid.c b/test/motion_lid.c
index 18c4f64f33..d204ec4ab0 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -40,10 +40,7 @@ static int accel_init(const struct motion_sensor_t *s)
 
 static int accel_read(const struct motion_sensor_t *s, vector_3_t v)
 {
-	if (*s->rot_standard_ref != NULL)
-		rotate(s->xyz, *s->rot_standard_ref, v);
-	else if (s->xyz != v)
-		memcpy(v, s->xyz, sizeof(v));
+	rotate(s->xyz, *s->rot_standard_ref, v);
 	return EC_SUCCESS;
 }