common: vec3: Add init/sub/add functions

Add convinience functions for initializing, adding, and subtracting
vec3.

BUG=None
BRANCH=None
TEST=buildall

Change-Id: I594db350863a8199eade15a38deb6c223e2ae1ac
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1869729
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
Tested-by: Jack Rosenthal <jrosenth@chromium.org>

2 files changed, 78 insertions, 0 deletions

diff --git a/common/vec3.c b/common/vec3.c
index 9a3561365a..29dc22f922 100644
--- a/common/vec3.c
+++ b/common/vec3.c
@@ -9,6 +9,13 @@
 #include "vec3.h"
 #include "util.h"
 
+void fpv3_init(fpv3_t v, fp_t x, fp_t y, fp_t z)
+{
+	v[X] = x;
+	v[Y] = y;
+	v[Z] = z;
+}
+
 void fpv3_scalar_mul(fpv3_t v, fp_t c)
 {
 	v[X] = fp_mul(v[X], c);
@@ -16,6 +23,20 @@ void fpv3_scalar_mul(fpv3_t v, fp_t c)
 	v[Z] = fp_mul(v[Z], c);
 }
 
+void fpv3_sub(fpv3_t out, const fpv3_t a, const fpv3_t b)
+{
+	out[X] = a[X] - b[X];
+	out[Y] = a[Y] - b[Y];
+	out[Z] = a[Z] - b[Z];
+}
+
+void fpv3_add(fpv3_t out, const fpv3_t a, const fpv3_t b)
+{
+	out[X] = a[X] + b[X];
+	out[Y] = a[Y] + b[Y];
+	out[Z] = a[Z] + b[Z];
+}
+
 fp_t fpv3_dot(const fpv3_t v, const fpv3_t w)
 {
 	return fp_mul(v[X], w[X]) + fp_mul(v[Y], w[Y]) + fp_mul(v[Z], w[Z]);
diff --git a/include/vec3.h b/include/vec3.h
index 39514157dd..3151ec35d5 100644
--- a/include/vec3.h
+++ b/include/vec3.h
@@ -12,8 +12,65 @@
 typedef float floatv3_t[3];
 typedef fp_t fpv3_t[3];
 
+/**
+ * Initialize a vector.
+ *
+ * @param v Pointer to the vector that will be initialized.
+ * @param x The value to use for the X component of v.
+ * @param y The value to use for the Y component of v.
+ * @param z The value to use for the Z component of v.
+ */
+void fpv3_init(fpv3_t v, fp_t x, fp_t y, fp_t z);
+
+/**
+ * Multiply components of the vector by a scalar.
+ *
+ * @param v Pointer to the vector that is modified.
+ * @param c Scalar value to multiply v by.
+ */
 void fpv3_scalar_mul(fpv3_t v, fp_t c);
+
+/**
+ * Subtract b from a and save the result.
+ *
+ * @param out Pointer to the vector that will be written to.
+ * @param a Pointer to the vector that is being subtracted from.
+ * @param b Pointer to the vector that is being subtracted.
+ */
+void fpv3_sub(fpv3_t out, const fpv3_t a, const fpv3_t b);
+
+/**
+ * Adds a and b then save the result.
+ *
+ * @param out Pointer to the vector that will be written to.
+ * @param a Pointer to the first vector being added.
+ * @param b Pointer to the second vector being added.
+ */
+void fpv3_add(fpv3_t out, const fpv3_t a, const fpv3_t b);
+
+/**
+ * Perform the dot product of two vectors.
+ *
+ * @param v Pointer to the first vector.
+ * @param w Pointer to the second vector.
+ * @return The dot product of v and w.
+ */
 fp_t fpv3_dot(const fpv3_t v, const fpv3_t w);
+
+/**
+ * Compute the length^2 of a vector.
+ *
+ * @param v Pointer to the vector in question.
+ * @return The length^2 of the vector.
+ */
 fp_t fpv3_norm_squared(const fpv3_t v);
+
+/**
+ * Compute the length of a vector.
+ *
+ * @param v Pointer to the vector in question.
+ * @return The length of the vector.
+ */
 fp_t fpv3_norm(const fpv3_t v);
+
 #endif  /* __CROS_EC_VEC_3_H */