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 /common/math_util.c | |
| 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>
Diffstat (limited to 'common/math_util.c')
| -rw-r--r-- | common/math_util.c | 57 |
1 files changed, 57 insertions, 0 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; +} |