diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index 9c1c0cfee..03032efb4 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -268,6 +268,17 @@ void math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result); +/*! + * Compute quad layer model matrix from xrt_pose and xrt_vec2 size. + * + * @relates xrt_matrix_4x4 + * @ingroup aux_math + */ +void +math_matrix_4x4_quad_model(const struct xrt_pose *pose, + const struct xrt_vec2 *size, + 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 7c1aa4d3d..11916d14b 100644 --- a/src/xrt/auxiliary/math/m_base.cpp +++ b/src/xrt/auxiliary/math/m_base.cpp @@ -265,6 +265,22 @@ math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, map_matrix_4x4(*result) = transformation.matrix().inverse(); } +void +math_matrix_4x4_quad_model(const struct xrt_pose *pose, + const struct xrt_vec2 *size, + struct xrt_matrix_4x4 *result) +{ + Eigen::Vector3f position = copy(&pose->position); + Eigen::Quaternionf orientation = copy(&pose->orientation); + + auto scale = Eigen::Scaling(size->x, size->y, 1.0f); + + Eigen::Translation3f translation(position); + Eigen::Affine3f transformation = translation * orientation * scale; + + map_matrix_4x4(*result) = transformation.matrix(); +} + /* * * Exported pose functions.