diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index 684ebd1d9..a56285a74 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -375,84 +375,13 @@ 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. - * - * Performs roughly outPose = spacePose * relativePose * baseSpacePose^-1 - * - * OK if input and output are the same addresses. - * - * @relates xrt_pose - * @ingroup aux_math - */ -void -math_pose_openxr_locate(const struct xrt_pose *space_pose, - const struct xrt_pose *relative_pose, - const struct xrt_pose *base_space_pose, - struct xrt_pose *result); /* * - * Space relation functions + * Optics functions. * */ -/*! - * Reset a relation to zero velocity, located at origin, and all validity flags. - * - * @relates xrt_space_relation - * @ingroup aux_math - */ -void -math_relation_reset(struct xrt_space_relation *out); - -/*! - * Apply a static pose on top of an existing relation. - * - * Updates all valid pose and derivative fields. Does not modify the validity - * mask. Treats both position and orientation of transform as valid. - * - * @relates xrt_space_relation - * @see xrt_pose - * @ingroup aux_math - */ -void -math_relation_apply_offset(const struct xrt_pose *offset, - struct xrt_space_relation *in_out_relation); - -/*! - * Apply another step of space relation on top of an existing relation. - * - * Updates all valid pose and derivative fields, as well as the validity mask. - * - * @relates xrt_space_relation - * @ingroup aux_math - */ -void -math_relation_accumulate_relation( - const struct xrt_space_relation *additional_relation, - struct xrt_space_relation *in_out_relation); - -/*! - * Combine the poses of the target and base space with the relative relation of - * those spaces. In a way that OpenXR specifies in the function xrLocateSpace. - * - * Performs roughly `out_relation->pose = space_pose * relative_relation->pose * - * base_space_pose^-1` for the poses, and appropriate rotation - * - * OK if input and output are the same addresses. - * - * @relates xrt_space_relation - * @see xrt_pose - * @ingroup aux_math - */ -void -math_relation_openxr_locate(const struct xrt_pose *space_pose, - const struct xrt_space_relation *relative_relation, - const struct xrt_pose *base_space_pose, - struct xrt_space_relation *result); - /*! * Perform the computations from * "Computing Half-Fields-Of-View from Simpler Display Models", diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp index bd20622cf..b1a3ed35d 100644 --- a/src/xrt/auxiliary/math/m_base.cpp +++ b/src/xrt/auxiliary/math/m_base.cpp @@ -413,272 +413,3 @@ math_pose_transform_point(const struct xrt_pose *transform, 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, - const struct xrt_pose *base_space_pose, - struct xrt_pose *result) -{ - assert(space_pose != NULL); - assert(relative_pose != NULL); - assert(base_space_pose != NULL); - assert(result != NULL); - - // Compilers are slightly better optimizing - // if we copy the arguments in one go. - const auto bsp = *base_space_pose; - const auto rel = *relative_pose; - const auto spc = *space_pose; - struct xrt_pose pose; - - // Apply the invert of the base space to identity. - math_pose_invert(&bsp, &pose); - - // Apply the pure pose from the space relation. - math_pose_transform(&pose, &rel, &pose); - - // Apply the space pose. - math_pose_transform(&pose, &spc, &pose); - - *result = pose; -} - -/*! - * Return the result of rotating a derivative vector by a matrix. - * - * This is a differential transform. - */ -static inline Eigen::Vector3f -rotate_deriv(Eigen::Matrix3f const &rotation, - const xrt_vec3 &derivativeVector, - Eigen::Matrix3f const &rotationInverse) -{ - return ((rotation * map_vec3(derivativeVector)).transpose() * - rotationInverse) - .transpose(); -} - -#ifndef XRT_DOXYGEN - -#define MAKE_REL_FLAG_CHECK(NAME, MASK) \ - static inline bool NAME(xrt_space_relation_flags flags) \ - { \ - return ((flags & (MASK)) != 0); \ - } - -MAKE_REL_FLAG_CHECK(has_some_pose_component, - XRT_SPACE_RELATION_POSITION_VALID_BIT | - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) -MAKE_REL_FLAG_CHECK(has_position, XRT_SPACE_RELATION_POSITION_VALID_BIT) -MAKE_REL_FLAG_CHECK(has_orientation, XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) -MAKE_REL_FLAG_CHECK(has_lin_vel, XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) -MAKE_REL_FLAG_CHECK(has_ang_vel, XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) -MAKE_REL_FLAG_CHECK(has_lin_acc, - XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT) -MAKE_REL_FLAG_CHECK(has_ang_acc, - XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT) -MAKE_REL_FLAG_CHECK(has_some_derivative, - XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | - XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | - XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT | - XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT) - -#undef MAKE_REL_FLAG_CHECK - -#endif // !XRT_DOXYGEN - -enum accumulate_pose_flags -{ - OFFSET, - LEGACY, -}; - -/*! - * Apply a transform to a space relation. - */ -static inline void -transform_accumulate_pose(const xrt_pose &transform, - xrt_space_relation &relation, - enum accumulate_pose_flags accum_flags, - bool do_translation = true, - bool do_rotation = true) -{ - assert(do_translation || do_rotation); - - // Save the quat in case we are self-transforming. - Eigen::Quaternionf quat = orientation(transform); - - auto flags = relation.relation_flags; - // so code looks similar - auto in_out_relation = &relation; - - // transform (rotate and translate) the pose, if applicable. - if (has_some_pose_component(flags)) { - // Zero out transform parts we don't want to use, - // because math_pose_transform doesn't take flags. - xrt_pose transform_copy = transform; - if (!do_translation) { - position(transform_copy) = Eigen::Vector3f::Zero(); - } - if (!do_rotation) { - orientation(transform_copy) = - Eigen::Quaternionf::Identity(); - } - - //! @todo This is just a big hack. - if (accum_flags == OFFSET) { - math_pose_transform(&transform, &in_out_relation->pose, - &in_out_relation->pose); - } else { - math_pose_transform(&in_out_relation->pose, &transform, - &in_out_relation->pose); - } - } - - if (do_rotation && has_some_derivative(flags)) { - - // prepare matrices required for rotating derivatives from the - // saved quat. - Eigen::Matrix3f rot = quat.toRotationMatrix(); - Eigen::Matrix3f rotInverse = rot.inverse(); - - // Rotate derivatives, if applicable. - if (has_lin_vel(flags)) { - map_vec3(in_out_relation->linear_velocity) = - rotate_deriv(rot, in_out_relation->linear_velocity, - rotInverse); - } - - if (has_ang_vel(flags)) { - map_vec3(in_out_relation->angular_velocity) = - rotate_deriv(rot, in_out_relation->angular_velocity, - rotInverse); - } - - if (has_lin_acc(flags)) { - map_vec3(in_out_relation->linear_acceleration) = - rotate_deriv(rot, - in_out_relation->linear_acceleration, - rotInverse); - } - - if (has_ang_acc(flags)) { - map_vec3(in_out_relation->angular_acceleration) = - rotate_deriv(rot, - in_out_relation->angular_acceleration, - rotInverse); - } - } -} - -static const struct xrt_space_relation BLANK_RELATION = { - XRT_SPACE_RELATION_BITMASK_ALL, - {{0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 0.0f}}, - {0, 0, 0}, - {0, 0, 0}, - {0, 0, 0}, - {0, 0, 0}, -}; - -extern "C" void -math_relation_reset(struct xrt_space_relation *out) -{ - *out = BLANK_RELATION; -} - -extern "C" void -math_relation_apply_offset(const struct xrt_pose *offset, - struct xrt_space_relation *in_out_relation) -{ - assert(offset != nullptr); - assert(in_out_relation != nullptr); - - // No modifying the validity flags here. - transform_accumulate_pose(*offset, *in_out_relation, OFFSET); -} - -void -accumulate_transform(const struct xrt_pose *transform, - struct xrt_space_relation *in_out_relation) -{ - assert(transform != nullptr); - assert(in_out_relation != nullptr); - - // No modifying the validity flags here. - transform_accumulate_pose(*transform, *in_out_relation, LEGACY); -} - -extern "C" void -math_relation_accumulate_relation( - const struct xrt_space_relation *additional_relation, - struct xrt_space_relation *in_out_relation) -{ - assert(additional_relation != NULL); - assert(in_out_relation != NULL); - - // Update the flags. - xrt_space_relation_flags flags = (enum xrt_space_relation_flags)( - in_out_relation->relation_flags & - additional_relation->relation_flags); - in_out_relation->relation_flags = flags; - - if (has_some_pose_component(flags)) { - // First, just do the pose part (including rotating - // derivatives, if applicable). - transform_accumulate_pose( - additional_relation->pose, *in_out_relation, LEGACY, - has_position(flags), has_orientation(flags)); - } - - // Then, accumulate the derivatives, if required. - if (has_lin_vel(flags)) { - map_vec3(in_out_relation->linear_velocity) += - map_vec3(additional_relation->linear_velocity); - } - - if (has_ang_vel(flags)) { - map_vec3(in_out_relation->angular_velocity) += - map_vec3(additional_relation->angular_velocity); - } - - if (has_lin_acc(flags)) { - map_vec3(in_out_relation->linear_acceleration) += - map_vec3(additional_relation->linear_acceleration); - } - - if (has_ang_acc(flags)) { - map_vec3(in_out_relation->angular_acceleration) += - map_vec3(additional_relation->angular_acceleration); - } -} - -extern "C" void -math_relation_openxr_locate(const struct xrt_pose *space_pose, - const struct xrt_space_relation *relative_relation, - const struct xrt_pose *base_space_pose, - struct xrt_space_relation *result) -{ - assert(space_pose != NULL); - assert(relative_relation != NULL); - assert(base_space_pose != NULL); - assert(result != NULL); - - // Compilers are slightly better optimizing - // if we copy the arguments in one go. - const auto bsp = *base_space_pose; - const auto spc = *space_pose; - struct xrt_space_relation accumulating_relation = BLANK_RELATION; - - // Apply the invert of the base space to identity. - math_pose_invert(&bsp, &accumulating_relation.pose); - - // Apply the pure relation between spaces. - math_relation_accumulate_relation(relative_relation, - &accumulating_relation); - - // Apply the space pose. - accumulate_transform(&spc, &accumulating_relation); - - *result = accumulating_relation; -}