From 4840def44817ffeab3ecd2f9773c3ec0d20c8e67 Mon Sep 17 00:00:00 2001
From: Aidan Thornton <makosoft@gmail.com>
Date: Fri, 10 Jun 2022 18:57:47 +0100
Subject: [PATCH] a/math: add math_matrix_2x2_transform_vec2

---
 src/xrt/auxiliary/math/m_api.h    | 11 +++++++++++
 src/xrt/auxiliary/math/m_base.cpp | 11 +++++++++++
 2 files changed, 22 insertions(+)

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index b6444128d..a70115ac1 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -428,6 +428,17 @@ math_matrix_2x2_multiply(const struct xrt_matrix_2x2 *left,
                          const struct xrt_matrix_2x2 *right,
                          struct xrt_matrix_2x2 *result_out);
 
+/*!
+ * Transform a vec2 by a 2x2 matrix
+ *
+ * @see xrt_matrix_2x2
+ * @ingroup aux_math
+ */
+void
+math_matrix_2x2_transform_vec2(const struct xrt_matrix_2x2 *left,
+                               const struct xrt_vec2 *right,
+                               struct xrt_vec2 *result_out);
+
 /*!
  * Initialize a 3x3 matrix to the identity matrix
  *
diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp
index 40200b822..7fa997087 100644
--- a/src/xrt/auxiliary/math/m_base.cpp
+++ b/src/xrt/auxiliary/math/m_base.cpp
@@ -456,6 +456,17 @@ math_matrix_2x2_multiply(const struct xrt_matrix_2x2 *left,
 	*result_out = result;
 }
 
+extern "C" void
+math_matrix_2x2_transform_vec2(const struct xrt_matrix_2x2 *left,
+                               const struct xrt_vec2 *right,
+                               struct xrt_vec2 *result_out)
+{
+	const struct xrt_matrix_2x2 l = *left;
+	const struct xrt_vec2 r = *right;
+	struct xrt_vec2 result = {l.v[0] * r.x + l.v[1] * r.y, l.v[2] * r.x + l.v[3] * r.y};
+	*result_out = result;
+}
+
 extern "C" void
 math_matrix_3x3_identity(struct xrt_matrix_3x3 *mat)
 {