diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index aafbd373a..c78664d79 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -1,4 +1,4 @@ -// Copyright 2019, Collabora, Ltd. +// Copyright 2019-2021, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file @@ -379,6 +379,16 @@ math_matrix_4x4_inverse_view_projection(const struct xrt_matrix_4x4 *view, * */ + +/*! + * Somewhat laboriously make an xrt_pose identity. + * + * @relates xrt_pose + * @ingroup aux_math + */ +void +math_pose_identity(struct xrt_pose *pose); + /*! * Check if this pose can be used in transformation operations. * diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp index f6040ccce..e75fc35ff 100644 --- a/src/xrt/auxiliary/math/m_base.cpp +++ b/src/xrt/auxiliary/math/m_base.cpp @@ -317,8 +317,8 @@ math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left, const struct x extern "C" void math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left, - const struct xrt_matrix_3x3 *right, - struct xrt_matrix_3x3 *result) + const struct xrt_matrix_3x3 *right, + struct xrt_matrix_3x3 *result) { result->v[0] = left->v[0] * right->v[0] + left->v[1] * right->v[3] + left->v[2] * right->v[6]; result->v[1] = left->v[0] * right->v[1] + left->v[1] * right->v[4] + left->v[2] * right->v[7]; @@ -419,6 +419,18 @@ math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose) orientation(*outPose) = newOrientation; } +extern "C" void +math_pose_identity(struct xrt_pose *pose) +{ + pose->position.x = 0.0; + pose->position.y = 0.0; + pose->position.z = 0.0; + pose->orientation.x = 0.0; + pose->orientation.y = 0.0; + pose->orientation.z = 0.0; + pose->orientation.w = 1.0; +} + /*! * Return the result of transforming a point by a pose/transform. */