From 7035490ed7d100ac06c409028084bc94a4e65b98 Mon Sep 17 00:00:00 2001
From: Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
Date: Tue, 8 Sep 2020 19:41:24 +0200
Subject: [PATCH] aux/math: Implement math_matrix_4x4_inverse_view_projection.

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

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index 0d4c2c72c..3a0c1a915 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -346,6 +346,18 @@ math_matrix_4x4_model(const struct xrt_pose *pose,
                       const struct xrt_vec3 *size,
                       struct xrt_matrix_4x4 *result);
 
+/*!
+ * Compute inverse view projection matrix,
+ * using only the starting 3x3 block of the view.
+ *
+ * @relates xrt_matrix_4x4
+ * @ingroup aux_math
+ */
+void
+math_matrix_4x4_inverse_view_projection(const struct xrt_matrix_4x4 *view,
+                                        const struct xrt_matrix_4x4 *projection,
+                                        struct xrt_matrix_4x4 *result);
+
 /*
  *
  * Pose functions.
diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp
index fe5845b81..33a4e1162 100644
--- a/src/xrt/auxiliary/math/m_base.cpp
+++ b/src/xrt/auxiliary/math/m_base.cpp
@@ -350,6 +350,18 @@ math_matrix_4x4_model(const struct xrt_pose *pose,
 	map_matrix_4x4(*result) = transformation.matrix();
 }
 
+void
+math_matrix_4x4_inverse_view_projection(const struct xrt_matrix_4x4 *view,
+                                        const struct xrt_matrix_4x4 *projection,
+                                        struct xrt_matrix_4x4 *result)
+{
+	Eigen::Matrix4f v = copy(view);
+	Eigen::Matrix4f v3 = Eigen::Matrix4f::Identity();
+	v3.block<3, 3>(0, 0) = v.block<3, 3>(0, 0);
+	Eigen::Matrix4f vp = copy(projection) * v3;
+	map_matrix_4x4(*result) = vp.inverse();
+}
+
 /*
  *
  * Exported pose functions.