From 73561006969737e0b02f223597c0865d4b90311e Mon Sep 17 00:00:00 2001
From: Moses Turner <moses@collabora.com>
Date: Fri, 13 Aug 2021 16:18:05 -0500
Subject: [PATCH] aux/math: Add m_vec3_project, m_vec3_orthonormalize,
 m_vec3_lerp, m_vec2_normalize, math_lerp

---
 src/xrt/auxiliary/math/m_api.h  |  6 ++++++
 src/xrt/auxiliary/math/m_vec2.h |  6 ++++++
 src/xrt/auxiliary/math/m_vec3.h | 21 +++++++++++++++++++++
 3 files changed, 33 insertions(+)

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index 0e23ab42c..65a4c1b3a 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -467,6 +467,12 @@ math_map_ranges(double value, double from_low, double from_high, double to_low,
 	return (value - from_low) * (to_high - to_low) / (from_high - from_low) + to_low;
 }
 
+static inline double
+math_lerp(double from, double to, double amount)
+{
+	return (from * (1.0 - amount)) + (to * (amount));
+}
+
 /*
  *
  * Optics functions.
diff --git a/src/xrt/auxiliary/math/m_vec2.h b/src/xrt/auxiliary/math/m_vec2.h
index 5bd54b7b7..d4132bcec 100644
--- a/src/xrt/auxiliary/math/m_vec2.h
+++ b/src/xrt/auxiliary/math/m_vec2.h
@@ -76,6 +76,12 @@ m_vec2_len(struct xrt_vec2 l)
 	return sqrtf(m_vec2_len_sqrd(l));
 }
 
+static inline void
+m_vec2_normalize(struct xrt_vec2 *inout)
+{
+	*inout = m_vec2_div_scalar(*inout, m_vec2_len(*inout));
+}
+
 static inline float
 m_vec2_dot(struct xrt_vec2 l, struct xrt_vec2 r)
 {
diff --git a/src/xrt/auxiliary/math/m_vec3.h b/src/xrt/auxiliary/math/m_vec3.h
index a2669c0ed..622077991 100644
--- a/src/xrt/auxiliary/math/m_vec3.h
+++ b/src/xrt/auxiliary/math/m_vec3.h
@@ -113,6 +113,27 @@ m_vec3_angle(struct xrt_vec3 l, struct xrt_vec3 r)
 	return acosf(dot / lengths);
 }
 
+static inline struct xrt_vec3
+m_vec3_project(struct xrt_vec3 project_this, struct xrt_vec3 onto_this)
+{
+
+	float amnt = (m_vec3_dot(project_this, onto_this) / m_vec3_len_sqrd(onto_this));
+
+	return m_vec3_mul_scalar(onto_this, amnt);
+}
+
+static inline struct xrt_vec3
+m_vec3_orthonormalize(struct xrt_vec3 leave_this_alone, struct xrt_vec3 change_this_one)
+{
+	return m_vec3_normalize(m_vec3_sub(change_this_one, m_vec3_project(change_this_one, leave_this_alone)));
+}
+
+static inline struct xrt_vec3
+m_vec3_lerp(struct xrt_vec3 from, struct xrt_vec3 to, float amount)
+{
+	// Recommend amount being in [0,1]
+	return m_vec3_add(m_vec3_mul_scalar(from, 1.0f - amount), m_vec3_mul_scalar(to, amount));
+}
 
 #ifdef __cplusplus
 }