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.