diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index d91f5a473..702683445 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -95,6 +95,17 @@ math_vec3_validate(const struct xrt_vec3 *vec3); void math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut); +/*! + * Cross product of a vector. + * + * @relates xrt_vec3 + * @ingroup aux_math + */ +void +math_vec3_cross(const struct xrt_vec3 *l, + const struct xrt_vec3 *r, + struct xrt_vec3 *result); + /* * @@ -113,6 +124,19 @@ void math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat, struct xrt_quat *result); +/*! + * Create a rotation from two vectors plus x and z, by creating a rotation + * matrix by crossing z and x to get the y axis. + * + * @relates xrt_quat + * @relates xrt_vec3 + * @ingroup aux_math + */ +void +math_quat_from_plus_x_z(const struct xrt_vec3 *plus_x, + const struct xrt_vec3 *plus_z, + struct xrt_quat *result); + /*! * Check if this quat can be used in transformation operations. * @@ -230,6 +254,18 @@ math_pose_transform(const struct xrt_pose *transform, const struct xrt_pose *pose, struct xrt_pose *outPose); +/*! + * Apply a rigid-body transformation to a point. + * + * @relates xrt_pose + * @relates xrt_vec3 + * @ingroup aux_math + */ +void +math_pose_transform_point(const struct xrt_pose *transform, + const struct xrt_vec3 *point, + struct xrt_vec3 *out_point); + /*! * Combine the poses of the target and base space with the relative pose of * those spaces. In a way that OpenXR specifies in the function xrLocateSpace. diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp index c9c23e016..a98c8c61e 100644 --- a/src/xrt/auxiliary/math/m_base.cpp +++ b/src/xrt/auxiliary/math/m_base.cpp @@ -74,6 +74,14 @@ math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut) map_vec3(*inAndOut) += map_vec3(*additional); } +extern "C" void +math_vec3_cross(const struct xrt_vec3 *l, + const struct xrt_vec3 *r, + struct xrt_vec3 *result) +{ + map_vec3(*result) = map_vec3(*l).cross(map_vec3(*r)); +} + /* * @@ -81,7 +89,6 @@ math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut) * */ - extern "C" void math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat, struct xrt_quat *result) @@ -94,6 +101,29 @@ math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat, map_quat(*result) = q; } +extern "C" void +math_quat_from_plus_x_z(const struct xrt_vec3 *plus_x, + const struct xrt_vec3 *plus_z, + struct xrt_quat *result) +{ + xrt_vec3 plus_y; + math_vec3_cross(plus_z, plus_x, &plus_y); + + xrt_matrix_3x3 m = {{ + plus_x->x, + plus_y.x, + plus_z->x, + plus_x->y, + plus_y.y, + plus_z->y, + plus_x->z, + plus_y.z, + plus_z->z, + }}; + + math_quat_from_matrix_3x3(&m, result); +} + extern "C" bool math_quat_validate(const struct xrt_quat *quat) { @@ -227,6 +257,18 @@ math_pose_transform(const struct xrt_pose *transform, memcpy(outPose, &newPose, sizeof(xrt_pose)); } +extern "C" void +math_pose_transform_point(const struct xrt_pose *transform, + const struct xrt_vec3 *point, + struct xrt_vec3 *out_point) +{ + assert(transform != NULL); + assert(point != NULL); + assert(out_point != NULL); + + map_vec3(*out_point) = transform_point(*transform, *point); +} + extern "C" void math_pose_openxr_locate(const struct xrt_pose *space_pose, const struct xrt_pose *relative_pose,