diff --git a/src/xrt/state_trackers/oxr/oxr_instance.c b/src/xrt/state_trackers/oxr/oxr_instance.c index 466c66ebe..208eaf038 100644 --- a/src/xrt/state_trackers/oxr/oxr_instance.c +++ b/src/xrt/state_trackers/oxr/oxr_instance.c @@ -49,9 +49,6 @@ DEBUG_GET_ONCE_BOOL_OPTION(debug_spaces, "OXR_DEBUG_SPACES", false) DEBUG_GET_ONCE_BOOL_OPTION(debug_bindings, "OXR_DEBUG_BINDINGS", false) DEBUG_GET_ONCE_BOOL_OPTION(lifecycle_verbose, "OXR_LIFECYCLE_VERBOSE", false) -DEBUG_GET_ONCE_FLOAT_OPTION(tracking_origin_offset_x, "OXR_TRACKING_ORIGIN_OFFSET_X", 0.0f) -DEBUG_GET_ONCE_FLOAT_OPTION(tracking_origin_offset_y, "OXR_TRACKING_ORIGIN_OFFSET_Y", 0.0f) -DEBUG_GET_ONCE_FLOAT_OPTION(tracking_origin_offset_z, "OXR_TRACKING_ORIGIN_OFFSET_Z", 0.0f) static XrResult oxr_instance_destroy(struct oxr_logger *log, struct oxr_handle_base *hb) @@ -302,13 +299,6 @@ oxr_instance_create(struct oxr_logger *log, return ret; } - struct xrt_vec3 global_tracking_origin_offset = {debug_get_float_option_tracking_origin_offset_x(), - debug_get_float_option_tracking_origin_offset_y(), - debug_get_float_option_tracking_origin_offset_z()}; - - u_builder_setup_tracking_origins(dev, GET_XDEV_BY_ROLE(sys, left), GET_XDEV_BY_ROLE(sys, right), - &global_tracking_origin_offset); - // Sets the enabled extensions, this is where we should do any extra validation. inst->extensions = *extensions; diff --git a/src/xrt/state_trackers/oxr/oxr_objects.h b/src/xrt/state_trackers/oxr/oxr_objects.h index cb4a1964a..540407d2f 100644 --- a/src/xrt/state_trackers/oxr/oxr_objects.h +++ b/src/xrt/state_trackers/oxr/oxr_objects.h @@ -764,73 +764,28 @@ oxr_space_reference_create(struct oxr_logger *log, const XrReferenceSpaceCreateInfo *createInfo, struct oxr_space **out_space); -/*! - * Transforms a relation given in pure global space into the oxr_space @p spc. - * If @p apply_space_pose is true, the pose offset of @p spc will be included in @p out_relation. - */ -XRT_CHECK_RESULT bool -oxr_space_pure_relation_in_space(struct oxr_logger *log, - XrTime time, - struct xrt_space_relation *relation, - struct oxr_space *spc, - bool apply_space_pose, - struct xrt_space_relation *out_relation); - -/*! - * Transforms a pose given in pure global space into a relation in the oxr_space @p spc. - * If @p apply_space_pose is true, the pose offset of @p spc will be included in @p out_relation. - */ -XRT_CHECK_RESULT bool -oxr_space_pure_pose_in_space(struct oxr_logger *log, - XrTime time, - struct xrt_pose *pose, - struct oxr_space *spc, - bool apply_space_pose, - struct xrt_space_relation *out_relation); - -/*! - * Transforms a relation in an given oxr_space @p spc into pure global space, taking the pose offset of @p spc into - * account. - */ -XRT_CHECK_RESULT bool -oxr_space_pure_relation_from_space(struct oxr_logger *log, - XrTime time, - struct xrt_space_relation *relation, - struct oxr_space *spc, - struct xrt_space_relation *out_relation); - -/*! - * Transforms a posen in a given oxr_space @p spc into a relation in "pure" global space, taking the pose offset of @p - * spc into account. - */ -XRT_CHECK_RESULT bool -oxr_space_pure_pose_from_space(struct oxr_logger *log, - XrTime time, - struct xrt_pose *pose, - struct oxr_space *spc, - struct xrt_space_relation *out_relation); - -/*! - * Returns the pure relation in global space of an oxr_space, meaning the tracking_origin offsets are already applied - * and sets @p out_xdev to the device the space is associated with. - * - * @todo: This function currently assumes all reference spaces are associated with the HMD. - */ -XRT_CHECK_RESULT bool -oxr_space_get_pure_relation(struct oxr_logger *log, - struct oxr_space *spc, - XrTime time, - struct xrt_space_relation *out_relation); - XrResult oxr_space_locate( struct oxr_logger *log, struct oxr_space *spc, struct oxr_space *baseSpc, XrTime time, XrSpaceLocation *location); -XRT_CHECK_RESULT bool -is_local_space_set_up(struct oxr_session *sess); - -XRT_CHECK_RESULT bool -global_to_local_space(struct oxr_logger *log, struct oxr_session *sess, XrTime time, struct xrt_space_relation *rel); +/*! + * Locate the @ref xrt_device in the given base space, useful for implementing + * hand tracking location look ups and the like. + * + * @param log Logging struct. + * @param xdev Device to locate in the base space. + * @param baseSpc Base space where the device is to be located. + * @param[in] time Time in OpenXR domain. + * @param[out] out_relation Returns T_base_xdev, aka xdev in base space. + * + * @return Any errors, XR_SUCCESS, pose might not be valid on XR_SUCCESS. + */ +XRT_CHECK_RESULT XrResult +oxr_space_locate_device(struct oxr_logger *log, + struct xrt_device *xdev, + struct oxr_space *baseSpc, + XrTime time, + struct xrt_space_relation *out_relation); /* @@ -996,22 +951,6 @@ oxr_xdev_find_input(struct xrt_device *xdev, enum xrt_input_name name, struct xr bool oxr_xdev_find_output(struct xrt_device *xdev, enum xrt_output_name name, struct xrt_output **out_output); -void -oxr_xdev_get_relation_chain(struct oxr_logger *log, - struct oxr_instance *inst, - struct xrt_device *xdev, - enum xrt_input_name name, - XrTime at_time, - struct xrt_relation_chain *xrc); - -void -oxr_xdev_get_space_relation(struct oxr_logger *log, - struct oxr_instance *inst, - struct xrt_device *xdev, - enum xrt_input_name name, - XrTime at_time, - struct xrt_space_relation *out_relation); - /*! * Returns the hand tracking value of the named input from the device. * Does NOT apply tracking origin offset to each joint. @@ -1024,6 +963,7 @@ oxr_xdev_get_hand_tracking_at(struct oxr_logger *log, XrTime at_time, struct xrt_hand_joint_set *out_value); + /* * * OpenGL, located in various files. @@ -1995,6 +1935,13 @@ struct oxr_space //! Which sub action path is this? struct oxr_subaction_paths subaction_paths; + + struct + { + struct xrt_space *xs; + struct xrt_device *xdev; + enum xrt_input_name name; + } action; }; /*! diff --git a/src/xrt/state_trackers/oxr/oxr_session.c b/src/xrt/state_trackers/oxr/oxr_session.c index 9491d09de..81ba5258c 100644 --- a/src/xrt/state_trackers/oxr/oxr_session.c +++ b/src/xrt/state_trackers/oxr/oxr_session.c @@ -343,7 +343,8 @@ oxr_session_locate_views(struct oxr_logger *log, const uint64_t xdisplay_time = time_state_ts_to_monotonic_ns(sess->sys->inst->timekeeping, viewLocateInfo->displayTime); - struct xrt_space_relation head_relation = XRT_SPACE_RELATION_ZERO; + // The head pose as in the xdev's space, aka XRT_INPUT_GENERIC_HEAD_POSE. + struct xrt_space_relation T_xdev_head = XRT_SPACE_RELATION_ZERO; struct xrt_fov fovs[2] = {0}; struct xrt_pose poses[2] = {0}; @@ -352,39 +353,34 @@ oxr_session_locate_views(struct oxr_logger *log, &default_eye_relation, // xdisplay_time, // 2, // - &head_relation, // + &T_xdev_head, // fovs, // poses); - - struct xrt_space_relation pure_head_relation; - - // head_relation is in xdev space. Bring it into pure global space by applying tracking origin offset. - struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_relation(&xrc, &head_relation); - m_relation_chain_push_pose_if_not_identity(&xrc, &xdev->tracking_origin->offset); - m_relation_chain_resolve(&xrc, &pure_head_relation); - - // Clear here and filled in loop. - viewState->viewStateFlags = 0; - - struct xrt_space_relation head_relation_in_base_space; - if (!oxr_space_pure_relation_in_space(log, viewLocateInfo->displayTime, &pure_head_relation, baseSpc, true, - &head_relation_in_base_space)) { - for (uint32_t i = 0; i < view_count; i++) { - OXR_XRT_POSE_TO_XRPOSEF(XRT_POSE_IDENTITY, views[i].pose); - } - + // The xdev pose in the base space. + struct xrt_space_relation T_base_xdev = XRT_SPACE_RELATION_ZERO; + XrResult ret = oxr_space_locate_device( // + log, // + xdev, // + baseSpc, // + viewLocateInfo->displayTime, // + &T_base_xdev); // + if (ret != XR_SUCCESS || T_base_xdev.relation_flags == 0) { if (print) { oxr_slog(&slog, "\n\tReturning invalid poses"); oxr_log_slog(log, &slog); } else { oxr_slog_cancel(&slog); } - - return XR_SUCCESS; + return ret; } + struct xrt_space_relation T_base_head; + struct xrt_relation_chain xrc = {0}; + m_relation_chain_push_relation(&xrc, &T_xdev_head); + m_relation_chain_push_relation(&xrc, &T_base_xdev); + m_relation_chain_resolve(&xrc, &T_base_head); + if (print) { for (uint32_t i = 0; i < view_count; i++) { char tmp[32]; @@ -392,8 +388,8 @@ oxr_session_locate_views(struct oxr_logger *log, oxr_pp_fov_indented_as_object(&slog, &fovs[i], tmp); oxr_pp_pose_indented_as_object(&slog, &poses[i], tmp); } - oxr_pp_relation_indented(&slog, &head_relation, "xdev.head_relation"); - oxr_pp_relation_indented(&slog, &head_relation_in_base_space, "head_relation_in_base_space"); + oxr_pp_relation_indented(&slog, &T_xdev_head, "T_xdev_head"); + oxr_pp_relation_indented(&slog, &T_base_xdev, "T_base_xdev"); } for (uint32_t i = 0; i < view_count; i++) { @@ -407,7 +403,7 @@ oxr_session_locate_views(struct oxr_logger *log, struct xrt_space_relation result = {0}; struct xrt_relation_chain xrc = {0}; m_relation_chain_push_pose_if_not_identity(&xrc, &view_pose); - m_relation_chain_push_relation(&xrc, &head_relation_in_base_space); + m_relation_chain_push_relation(&xrc, &T_base_head); m_relation_chain_resolve(&xrc, &result); OXR_XRT_POSE_TO_XRPOSEF(result.pose, views[i].pose); @@ -942,23 +938,31 @@ oxr_session_hand_joints(struct oxr_logger *log, oxr_xdev_get_hand_tracking_at(log, sess->sys->inst, xdev, name, at_time, &value); - struct xrt_space_relation pure_hand_relation = value.hand_pose; + // The hand pose is returned in the xdev's space. + struct xrt_space_relation T_xdev_hand = value.hand_pose; + + // Get the xdev's pose in the base space. + struct xrt_space_relation T_base_xdev = XRT_SPACE_RELATION_ZERO; + + XrResult ret = oxr_space_locate_device(log, xdev, baseSpc, at_time, &T_base_xdev); + if (ret != XR_SUCCESS) { + // Error printed logged oxr_space_locate_device + return ret; + } + if (T_base_xdev.relation_flags == 0) { + locations->isActive = false; + return XR_SUCCESS; + } + + // Get the hands pose in the base space. + struct xrt_space_relation T_base_hand; struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_relation(&xrc, &pure_hand_relation); - m_relation_chain_push_pose_if_not_identity(&xrc, &xdev->tracking_origin->offset); - m_relation_chain_resolve(&xrc, &pure_hand_relation); - - struct xrt_space_relation hand_pose_in_base_space; - bool has_hand_pose_in_base_sapce = oxr_space_pure_relation_in_space( // - log, // - at_time, // - &pure_hand_relation, // - baseSpc, // - true, // - &hand_pose_in_base_space); // + m_relation_chain_push_relation(&xrc, &T_xdev_hand); + m_relation_chain_push_relation(&xrc, &T_base_xdev); + m_relation_chain_resolve(&xrc, &T_base_hand); // Can we not relate to this space or did we not get values? - if (!has_hand_pose_in_base_sapce || !value.is_active) { + if (T_base_hand.relation_flags == 0 || !value.is_active) { locations->isActive = false; // Loop over all joints and zero flags. @@ -986,7 +990,7 @@ oxr_session_hand_joints(struct oxr_logger *log, struct xrt_space_relation result; struct xrt_relation_chain chain = {0}; m_relation_chain_push_relation(&chain, &r); - m_relation_chain_push_relation(&chain, &hand_pose_in_base_space); + m_relation_chain_push_relation(&chain, &T_base_hand); m_relation_chain_resolve(&chain, &result); xrt_to_xr_pose(&result.pose, &locations->jointLocations[i].pose); diff --git a/src/xrt/state_trackers/oxr/oxr_session_frame_end.c b/src/xrt/state_trackers/oxr/oxr_session_frame_end.c index 6b7ce8039..683c0eb6f 100644 --- a/src/xrt/state_trackers/oxr/oxr_session_frame_end.c +++ b/src/xrt/state_trackers/oxr/oxr_session_frame_end.c @@ -858,11 +858,12 @@ handle_space(struct oxr_logger *log, uint64_t timestamp, struct xrt_pose *out_pose) { - struct xrt_pose pose = *pose_ptr; + // Aka T_offset_layer + struct xrt_pose T_space_layer = *pose_ptr; - // The pose might be valid for OpenXR, but not good enough for math. - if (!math_quat_validate(&pose.orientation)) { - math_quat_normalize(&pose.orientation); + // The T_space_layer might be valid for OpenXR, but not good enough for math. + if (!math_quat_validate(&T_space_layer.orientation)) { + math_quat_normalize(&T_space_layer.orientation); } /* @@ -871,27 +872,32 @@ handle_space(struct oxr_logger *log, if (spc->space_type == OXR_SPACE_TYPE_REFERENCE_VIEW) { struct xrt_space_relation rel; struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_pose(&xrc, &pose); - m_relation_chain_push_pose_if_not_identity(&xrc, &spc->pose); + m_relation_chain_push_pose(&xrc, &T_space_layer); // T_offset_layer + m_relation_chain_push_pose_if_not_identity(&xrc, &spc->pose); // T_space_offset m_relation_chain_resolve(&xrc, &rel); *out_pose = rel.pose; return true; } - struct xrt_space_relation rel; - if (!oxr_space_pure_pose_from_space(log, timestamp, &pose, spc, &rel)) { + // The compositor doesn't know about spaces, so we want the space in the xdev's "space". + struct xrt_device *head_xdev = GET_XDEV_BY_ROLE(sess->sys, head); + struct xrt_space_relation T_space_xdev = XRT_SPACE_RELATION_ZERO; + + XrResult ret = oxr_space_locate_device(log, head_xdev, spc, timestamp, &T_space_xdev); + if (ret != XR_SUCCESS) { + return false; + } + if (T_space_xdev.relation_flags == 0) { return false; } - - // The compositor doesn't know about tracking origins, transform into the "raw" HMD tracking space. - struct xrt_device *head_xdev = GET_XDEV_BY_ROLE(sess->sys, head); + struct xrt_space_relation T_xdev_layer; struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_relation(&xrc, &rel); - m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &head_xdev->tracking_origin->offset); - m_relation_chain_resolve(&xrc, &rel); + m_relation_chain_push_pose_if_not_identity(&xrc, &T_space_layer); + m_relation_chain_push_inverted_relation(&xrc, &T_space_xdev); // T_xdev_space + m_relation_chain_resolve(&xrc, &T_xdev_layer); - *out_pose = rel.pose; + *out_pose = T_xdev_layer.pose; return true; } diff --git a/src/xrt/state_trackers/oxr/oxr_space.c b/src/xrt/state_trackers/oxr/oxr_space.c index 272bdbd18..0a0a63036 100644 --- a/src/xrt/state_trackers/oxr/oxr_space.c +++ b/src/xrt/state_trackers/oxr/oxr_space.c @@ -8,12 +8,11 @@ */ -#include -#include -#include +#include "xrt/xrt_space.h" #include "math/m_api.h" #include "math/m_space.h" + #include "util/u_debug.h" #include "util/u_misc.h" @@ -25,8 +24,16 @@ #include "oxr_pretty_print.h" #include "oxr_conversions.h" +#include +#include +#include + -const struct xrt_pose origin = XRT_POSE_IDENTITY; +/* + * + * Helper functions. + * + */ static XrResult check_reference_space_type(struct oxr_logger *log, XrReferenceSpaceType type) @@ -48,11 +55,102 @@ check_reference_space_type(struct oxr_logger *log, XrReferenceSpaceType type) } } + +/* + * + * To xrt_space functions. + * + */ + +static XrResult +get_xrt_space_action(struct oxr_logger *log, struct oxr_space *spc, struct xrt_space **out_xspace) +{ + + struct oxr_action_input *input = NULL; + + XrResult ret = oxr_action_get_pose_input(spc->sess, spc->act_key, &spc->subaction_paths, &input); + if (ret != XR_SUCCESS) { + return ret; + } + + // Clear the cache. + if (input == NULL) { + xrt_space_reference(&spc->action.xs, NULL); + spc->action.name = 0; + spc->action.xdev = NULL; + return XR_SUCCESS; + } + + struct xrt_device *xdev = input->xdev; + enum xrt_input_name name = input->input->name; + + assert(xdev != NULL); + assert(name != 0); + + if (xdev != spc->action.xdev || name != spc->action.name) { + xrt_space_reference(&spc->action.xs, NULL); + + xrt_result_t xret = xrt_space_overseer_create_pose_space( // + spc->sess->sys->xso, // + xdev, // + name, // + &spc->action.xs); // + if (xret != XRT_SUCCESS) { + oxr_warn(log, "Failed to create pose space"); + } else { + spc->action.xdev = xdev; + spc->action.name = name; + } + } + + *out_xspace = spc->action.xs; + + return XR_SUCCESS; +} + +static XrResult +get_xrt_space(struct oxr_logger *log, struct oxr_space *spc, struct xrt_space **out_xspace) +{ + assert(out_xspace != NULL); + assert(*out_xspace == NULL); + + struct xrt_space *xspace = NULL; + switch (spc->space_type) { + case OXR_SPACE_TYPE_ACTION: return get_xrt_space_action(log, spc, out_xspace); + case OXR_SPACE_TYPE_REFERENCE_VIEW: xspace = spc->sess->sys->xso->semantic.view; break; + case OXR_SPACE_TYPE_REFERENCE_LOCAL: xspace = spc->sess->sys->xso->semantic.local; break; + case OXR_SPACE_TYPE_REFERENCE_STAGE: xspace = spc->sess->sys->xso->semantic.stage; break; + case OXR_SPACE_TYPE_REFERENCE_UNBOUNDED_MSFT: xspace = spc->sess->sys->xso->semantic.unbounded; break; + case OXR_SPACE_TYPE_REFERENCE_COMBINED_EYE_VARJO: xspace = NULL; break; + } + + if (xspace == NULL) { + return oxr_error(log, XR_ERROR_RUNTIME_FAILURE, "Reference space without internal semantic space!"); + } + + *out_xspace = xspace; + + return XR_SUCCESS; +} + + +/* + * + * Space creation and destroy functions. + * + */ + static XrResult oxr_space_destroy(struct oxr_logger *log, struct oxr_handle_base *hb) { struct oxr_space *spc = (struct oxr_space *)hb; + + xrt_space_reference(&spc->action.xs, NULL); + spc->action.xdev = NULL; + spc->action.name = 0; + free(spc); + return XR_SUCCESS; } @@ -110,253 +208,20 @@ oxr_space_reference_create(struct oxr_logger *log, return XR_SUCCESS; } -static void -print_pose(struct oxr_session *sess, const char *prefix, struct xrt_pose *pose); - -static bool -set_up_local_space(struct oxr_logger *log, struct oxr_session *sess, XrTime time) -{ - struct xrt_device *head_xdev = GET_XDEV_BY_ROLE(sess->sys, head); - struct xrt_space_relation head_relation; - oxr_xdev_get_space_relation(log, sess->sys->inst, head_xdev, XRT_INPUT_GENERIC_HEAD_POSE, time, &head_relation); - - if ((head_relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) == 0) { - return false; - } - if (!is_local_space_set_up(sess)) { - sess->local_space_pure_relation = head_relation; - - // take only head rotation around y axis - // https://stackoverflow.com/a/5783030 - sess->local_space_pure_relation.pose.orientation.x = 0; - sess->local_space_pure_relation.pose.orientation.z = 0; - math_quat_normalize(&sess->local_space_pure_relation.pose.orientation); - - print_pose(sess, "local space updated", &head_relation.pose); - - //! @todo: Handle relation velocities if necessary - } - return true; -} - -XRT_CHECK_RESULT bool -is_local_space_set_up(struct oxr_session *sess) -{ - return (sess->local_space_pure_relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0; -} - - -/*! - * Returns the pure relation in global space of an oxr_space, meaning the tracking_origin offsets are already applied. +/* + * + * OpenXR API functions. * - * @todo: Until a proper reference space system is implemented, the xdev assigned to the head role should be used as @p - * ref_xdev for consistency. - */ -XRT_CHECK_RESULT static bool -oxr_space_ref_get_pure_relation(struct oxr_logger *log, - struct oxr_session *sess, - enum oxr_space_type space_type, - struct xrt_device *ref_xdev, - XrTime time, - struct xrt_space_relation *out_relation) -{ - switch (space_type) { - case OXR_SPACE_TYPE_REFERENCE_LOCAL: { - if (!is_local_space_set_up(sess)) { - if (!set_up_local_space(log, sess, time)) { - return false; - } - } - - *out_relation = sess->local_space_pure_relation; - return true; - } - case OXR_SPACE_TYPE_REFERENCE_STAGE: { - //! @todo: stage space origin assumed to be the same as HMD xdev space origin for now. - m_space_relation_ident(out_relation); - return true; - } - case OXR_SPACE_TYPE_REFERENCE_VIEW: { - oxr_xdev_get_space_relation(log, sess->sys->inst, ref_xdev, XRT_INPUT_GENERIC_HEAD_POSE, time, - out_relation); - return true; - } - - case OXR_SPACE_TYPE_REFERENCE_UNBOUNDED_MSFT: - case OXR_SPACE_TYPE_REFERENCE_COMBINED_EYE_VARJO: - // not implemented - return oxr_error(log, false, "Reference Space type %d not implemented!", space_type); - case OXR_SPACE_TYPE_ACTION: return oxr_error(log, false, "Space is not a reference space!"); - } - return true; -} - -XRT_CHECK_RESULT bool -oxr_space_pure_relation_in_space(struct oxr_logger *log, - XrTime time, - struct xrt_space_relation *relation, - struct oxr_space *spc, - bool apply_space_pose, - struct xrt_space_relation *out_relation) -{ - struct xrt_space_relation pure_space_relation; - if (!oxr_space_get_pure_relation(log, spc, time, &pure_space_relation)) { - return false; - } - - struct xrt_relation_chain xrc = {0}; - - m_relation_chain_push_relation(&xrc, relation); - m_relation_chain_push_inverted_relation(&xrc, &pure_space_relation); - - if (apply_space_pose) { - m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &spc->pose); - } - - m_relation_chain_resolve(&xrc, out_relation); - return true; -} - -XRT_CHECK_RESULT bool -oxr_space_pure_pose_in_space(struct oxr_logger *log, - XrTime time, - struct xrt_pose *pose, - struct oxr_space *spc, - bool apply_space_pose, - struct xrt_space_relation *out_relation) -{ - struct xrt_space_relation rel; - m_space_relation_from_pose(pose, &rel); - return oxr_space_pure_relation_in_space(log, time, &rel, spc, apply_space_pose, out_relation); -} - -XRT_CHECK_RESULT bool -oxr_space_pure_relation_from_space(struct oxr_logger *log, - XrTime time, - struct xrt_space_relation *relation, - struct oxr_space *spc, - struct xrt_space_relation *out_relation) -{ - struct xrt_space_relation pure_space_relation; - if (!oxr_space_get_pure_relation(log, spc, time, &pure_space_relation)) { - return false; - } - - struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_relation(&xrc, relation); - m_relation_chain_push_pose_if_not_identity(&xrc, &spc->pose); - m_relation_chain_push_relation(&xrc, &pure_space_relation); - m_relation_chain_resolve(&xrc, out_relation); - return true; -} - -XRT_CHECK_RESULT bool -oxr_space_pure_pose_from_space(struct oxr_logger *log, - XrTime time, - struct xrt_pose *pose, - struct oxr_space *spc, - struct xrt_space_relation *out_relation) -{ - struct xrt_space_relation rel; - m_space_relation_from_pose(pose, &rel); - return oxr_space_pure_relation_from_space(log, time, &rel, spc, out_relation); -} - -XRT_CHECK_RESULT bool -oxr_space_get_pure_relation(struct oxr_logger *log, - struct oxr_space *spc, - XrTime time, - struct xrt_space_relation *out_relation) -{ - if (oxr_space_type_is_reference(spc->space_type)) { - struct xrt_device *head_xdev = GET_XDEV_BY_ROLE(spc->sess->sys, head); - return oxr_space_ref_get_pure_relation(log, spc->sess, spc->space_type, head_xdev, time, out_relation); - } - if (spc->space_type == OXR_SPACE_TYPE_ACTION) { - struct oxr_action_input *input = NULL; - oxr_action_get_pose_input(spc->sess, spc->act_key, &spc->subaction_paths, &input); - - // If the input isn't active. - if (input == NULL) { - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return false; - } - - oxr_xdev_get_space_relation(log, spc->sess->sys->inst, input->xdev, input->input->name, time, - out_relation); - - return true; - } - - return oxr_error(log, false, "Unknown space type"); -} - -XRT_CHECK_RESULT bool -global_to_local_space(struct oxr_logger *log, struct oxr_session *sess, XrTime time, struct xrt_space_relation *rel) -{ - if (!is_local_space_set_up(sess)) { - if (!set_up_local_space(log, sess, time)) { - return false; - } - } - - struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_relation(&xrc, rel); - m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &sess->local_space_pure_relation.pose); - m_relation_chain_resolve(&xrc, rel); - - return true; -} - -/*! - * This returns only the relation between two directly-associated spaces without - * the app given offset pose for baseSpc applied. */ -XRT_CHECK_RESULT static bool -get_pure_space_relation(struct oxr_logger *log, - struct oxr_space *spc, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation) -{ - struct xrt_space_relation space_pure_relation; - if (!oxr_space_get_pure_relation(log, spc, time, &space_pure_relation)) { - return false; - } - - struct xrt_space_relation base_space_pure_relation; - if (!oxr_space_get_pure_relation(log, baseSpc, time, &base_space_pure_relation)) { - return false; - } - - struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_relation(&xrc, &space_pure_relation); - m_relation_chain_push_inverted_relation(&xrc, &base_space_pure_relation); - m_relation_chain_resolve(&xrc, out_relation); - - return true; -} - -static void -print_pose(struct oxr_session *sess, const char *prefix, struct xrt_pose *pose) -{ - if (!sess->sys->inst->debug_spaces) { - return; - } - - struct xrt_vec3 *p = &pose->position; - struct xrt_quat *q = &pose->orientation; - - U_LOG_D("%s (%f, %f, %f) (%f, %f, %f, %f)", prefix, p->x, p->y, p->z, q->x, q->y, q->z, q->w); -} XrResult oxr_space_locate( struct oxr_logger *log, struct oxr_space *spc, struct oxr_space *baseSpc, XrTime time, XrSpaceLocation *location) { struct oxr_sink_logger slog = {0}; - bool print = spc->sess->sys->inst->debug_spaces; + struct oxr_system *sys = spc->sess->sys; + bool print = sys->inst->debug_spaces; if (print) { oxr_pp_space_indented(&slog, spc, "space"); oxr_pp_space_indented(&slog, baseSpc, "baseSpace"); @@ -365,10 +230,44 @@ oxr_space_locate( // Used in a lot of places. XrSpaceVelocity *vel = OXR_GET_OUTPUT_FROM_CHAIN(location->next, XR_TYPE_SPACE_VELOCITY, XrSpaceVelocity); - // Get the pure space relation. - struct xrt_space_relation pure; - bool has_pure_relation = get_pure_space_relation(log, spc, baseSpc, time, &pure); - if (!has_pure_relation) { + + /* + * Seek knowledge about the spaces from the space overseer. + */ + + struct xrt_space *xtarget = NULL; + struct xrt_space *xbase = NULL; + XrResult ret; + + ret = get_xrt_space(log, spc, &xtarget); + // Make sure not to overwrite error return + if (ret == XR_SUCCESS) { + ret = get_xrt_space(log, baseSpc, &xbase); + } + + // Only fill this out if the above succeeded. + struct xrt_space_relation result = XRT_SPACE_RELATION_ZERO; + if (xtarget != NULL && xbase != NULL) { + // Convert at_time to monotonic and give to device. + uint64_t at_timestamp_ns = time_state_ts_to_monotonic_ns(sys->inst->timekeeping, time); + + // Ask the space overseer to locate the spaces. + xrt_space_overseer_locate_space( // + sys->xso, // + xbase, // + &baseSpc->pose, // + at_timestamp_ns, // + xtarget, // + &spc->pose, // + &result); // + } + + + /* + * Validate results + */ + + if (result.relation_flags == 0) { location->locationFlags = 0; OXR_XRT_POSE_TO_XRPOSEF(XRT_POSE_IDENTITY, location->pose); @@ -386,7 +285,7 @@ oxr_space_locate( oxr_slog_cancel(&slog); } - return XR_SUCCESS; + return ret; // Return any error. } @@ -394,14 +293,6 @@ oxr_space_locate( * Combine and copy */ - // Combine space and base space poses with pure relation - struct xrt_space_relation result; - struct xrt_relation_chain xrc = {0}; - m_relation_chain_push_pose_if_not_identity(&xrc, &spc->pose); - m_relation_chain_push_relation(&xrc, &pure); - m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &baseSpc->pose); - m_relation_chain_resolve(&xrc, &result); - OXR_XRT_POSE_TO_XRPOSEF(result.pose, location->pose); location->locationFlags = xrt_to_xr_space_location_flags(result.relation_flags); @@ -432,7 +323,6 @@ oxr_space_locate( */ if (print) { - oxr_pp_pose_indented_as_object(&slog, (struct xrt_pose *)&pure.pose, "pure"); oxr_pp_relation_indented(&slog, &result, "relation"); oxr_log_slog(log, &slog); } else { @@ -441,3 +331,42 @@ oxr_space_locate( return oxr_session_success_result(spc->sess); } + + +/* + * + * 'Exported' functions. + * + */ + +XrResult +oxr_space_locate_device(struct oxr_logger *log, + struct xrt_device *xdev, + struct oxr_space *baseSpc, + XrTime time, + struct xrt_space_relation *out_relation) +{ + struct oxr_system *sys = baseSpc->sess->sys; + + struct xrt_space *xbase = NULL; + XrResult ret; + + ret = get_xrt_space(log, baseSpc, &xbase); + if (xbase == NULL) { + return ret; + } + + // Convert at_time to monotonic and give to device. + uint64_t at_timestamp_ns = time_state_ts_to_monotonic_ns(sys->inst->timekeeping, time); + + // Ask the space overseer to locate the spaces. + xrt_space_overseer_locate_device( // + sys->xso, // + xbase, // + &baseSpc->pose, // + at_timestamp_ns, // + xdev, // + out_relation); // + + return ret; +} diff --git a/src/xrt/state_trackers/oxr/oxr_xdev.c b/src/xrt/state_trackers/oxr/oxr_xdev.c index d23f10db0..151ad4e6e 100644 --- a/src/xrt/state_trackers/oxr/oxr_xdev.c +++ b/src/xrt/state_trackers/oxr/oxr_xdev.c @@ -78,25 +78,6 @@ oxr_xdev_find_output(struct xrt_device *xdev, enum xrt_output_name name, struct return false; } -void -oxr_xdev_get_relation_chain(struct oxr_logger *log, - struct oxr_instance *inst, - struct xrt_device *xdev, - enum xrt_input_name name, - XrTime at_time, - struct xrt_relation_chain *xrc) -{ - // Convert at_time to monotonic and give to device. - uint64_t at_timestamp_ns = time_state_ts_to_monotonic_ns(inst->timekeeping, at_time); - - struct xrt_space_relation *rel = m_relation_chain_reserve(xrc); - - xrt_device_get_tracked_pose(xdev, name, at_timestamp_ns, rel); - - // Add in the offset from the tracking system. - m_relation_chain_push_pose(xrc, &xdev->tracking_origin->offset); -} - void oxr_xdev_get_hand_tracking_at(struct oxr_logger *log, struct oxr_instance *inst, @@ -117,16 +98,3 @@ oxr_xdev_get_hand_tracking_at(struct oxr_logger *log, *out_value = value; } - -void -oxr_xdev_get_space_relation(struct oxr_logger *log, - struct oxr_instance *inst, - struct xrt_device *xdev, - enum xrt_input_name name, - XrTime at_time, - struct xrt_space_relation *out_relation) -{ - struct xrt_relation_chain xrc = {0}; - oxr_xdev_get_relation_chain(log, inst, xdev, name, at_time, &xrc); - m_relation_chain_resolve(&xrc, out_relation); -}