summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGwendal Grignou <gwendal@chromium.org>2018-12-19 09:08:12 -0800
committerchrome-bot <chrome-bot@chromium.org>2019-02-28 11:01:47 -0800
commit068c46513aac5ecfb06489191fad9fd60e5d750c (patch)
tree3241f63ceb44735e9482495b935f648c191562cf
parent265eceed674b7208fa303d0cfd5345b9113342da (diff)
downloadchrome-ec-068c46513aac5ecfb06489191fad9fd60e5d750c.tar.gz
math: Add/expose some vector operation
Expose dot procdut and cros product for vector operations. BUG=b:120346412 BRANCH=none TEST=compile, check unit tests. Change-Id: Ief50f31aa6105dc2f0d92caf8b7473a7e141eb45 Signed-off-by: Gwendal Grignou <gwendal@chromium.org> Reviewed-on: https://chromium-review.googlesource.com/1387923 Commit-Ready: ChromeOS CL Exonerator Bot <chromiumos-cl-exonerator@appspot.gserviceaccount.com>
-rw-r--r--common/math_util.c27
-rw-r--r--include/math_util.h18
2 files changed, 41 insertions, 4 deletions
diff --git a/common/math_util.c b/common/math_util.c
index 5e45f9efad..1a29c63d49 100644
--- a/common/math_util.c
+++ b/common/math_util.c
@@ -148,6 +148,28 @@ int vector_magnitude(const intv3_t v)
return int_sqrtf(sum);
}
+/* cross_product only works if the vectors magnitudes are around 1<<16. */
+void cross_product(const intv3_t v1, const intv3_t v2, intv3_t v)
+{
+ v[X] = (fp_inter_t)v1[Y] * v2[Z] - (fp_inter_t)v1[Z] * v2[Y];
+ v[Y] = (fp_inter_t)v1[Z] * v2[X] - (fp_inter_t)v1[X] * v2[Z];
+ v[Z] = (fp_inter_t)v1[X] * v2[Y] - (fp_inter_t)v1[Y] * v2[X];
+}
+
+fp_inter_t dot_product(const intv3_t v1, const intv3_t v2)
+{
+ return (fp_inter_t)v1[X] * v2[X] +
+ (fp_inter_t)v1[Y] * v2[Y] +
+ (fp_inter_t)v1[Z] * v2[Z];
+}
+
+void vector_scale(intv3_t v, fp_t s)
+{
+ v[X] = fp_mul(v[X], s);
+ v[Y] = fp_mul(v[Y], s);
+ v[Z] = fp_mul(v[Z], s);
+}
+
fp_t cosine_of_angle_diff(const intv3_t v1, const intv3_t v2)
{
fp_inter_t dotproduct;
@@ -157,10 +179,7 @@ fp_t cosine_of_angle_diff(const intv3_t v1, const intv3_t v2)
* Angle between two vectors is acos(A dot B / |A|*|B|). To return
* cosine of angle between vectors, then don't do acos operation.
*/
-
- dotproduct = (fp_inter_t)v1[0] * v2[0] +
- (fp_inter_t)v1[1] * v2[1] +
- (fp_inter_t)v1[2] * v2[2];
+ dotproduct = dot_product(v1, v2);
denominator = (fp_inter_t)vector_magnitude(v1) * vector_magnitude(v2);
diff --git a/include/math_util.h b/include/math_util.h
index 942aaf8908..8d779846b3 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -151,6 +151,24 @@ enum {
fp_t arc_cos(fp_t x);
/**
+ * Calculate the dot product of 2 vectors.
+ */
+fp_inter_t dot_product(const intv3_t v1, const intv3_t v2);
+
+/*
+ * Calculate the dot product of 2 vectors,
+ *
+ * Assume the result vector components fits in 32bit.
+ */
+void cross_product(const intv3_t v1, const intv3_t v2, intv3_t v);
+
+/**
+ * Scale a vector by fixed point constant.
+ */
+void vector_scale(intv3_t v, fp_t s);
+
+
+/**
* Find the cosine of the angle between two vectors.
*
* The implementation assumes no vector component is greater than