diff options
author | Gwendal Grignou <gwendal@chromium.org> | 2015-08-17 14:58:32 -0700 |
---|---|---|
committer | ChromeOS Commit Bot <chromeos-commit-bot@chromium.org> | 2015-08-24 19:08:20 +0000 |
commit | 0e01759cedcd25868196508177791388b89450e5 (patch) | |
tree | d361152082ad7a92a37d3e5fb9d7981202c40b19 | |
parent | de42bb285fa45a777ed7a27be9f4c99c8f606ed8 (diff) | |
download | chrome-ec-0e01759cedcd25868196508177791388b89450e5.tar.gz |
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
TEST=Unit test
BUG=chromium:517675
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>
-rw-r--r-- | common/math_util.c | 57 | ||||
-rw-r--r-- | include/math_util.h | 9 | ||||
-rw-r--r-- | test/math_util.c | 34 | ||||
-rw-r--r-- | test/motion_lid.c | 5 |
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 07283cf000..5412655ec3 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_EC_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; } |