From ed0bd9c24424f6c74d3207d6cf898c34e7ee7116 Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Tue, 1 Mar 2022 02:55:43 +0100 Subject: [PATCH] st/oxr: rework spaces --- src/xrt/state_trackers/oxr/oxr_objects.h | 82 +++- src/xrt/state_trackers/oxr/oxr_session.c | 94 +--- .../oxr/oxr_session_frame_end.c | 53 +-- src/xrt/state_trackers/oxr/oxr_space.c | 409 ++++++++---------- 4 files changed, 264 insertions(+), 374 deletions(-) diff --git a/src/xrt/state_trackers/oxr/oxr_objects.h b/src/xrt/state_trackers/oxr/oxr_objects.h index f006a68ac..a311043eb 100644 --- a/src/xrt/state_trackers/oxr/oxr_objects.h +++ b/src/xrt/state_trackers/oxr/oxr_objects.h @@ -708,35 +708,77 @@ 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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +bool +oxr_space_get_pure_relation(struct oxr_logger *log, + struct oxr_space *spc, + XrTime time, + struct xrt_space_relation *out_relation, + struct xrt_device **out_xdev); + XrResult oxr_space_locate( struct oxr_logger *log, struct oxr_space *spc, struct oxr_space *baseSpc, XrTime time, XrSpaceLocation *location); -XrResult -oxr_space_ref_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct oxr_space *space, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation); - -XrResult -oxr_view_relation_ref_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct xrt_space_relation *view_relation, - struct xrt_device *view_xdev, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation); - bool -initial_head_relation_valid(struct oxr_session *sess); +is_local_space_set_up(struct oxr_session *sess); XrSpaceLocationFlags xrt_to_xr_space_location_flags(enum xrt_space_relation_flags relation_flags); bool -global_to_local_space(struct oxr_session *sess, struct xrt_space_relation *rel); +global_to_local_space(struct oxr_logger *log, struct oxr_session *sess, XrTime time, struct xrt_space_relation *rel); /* * @@ -1376,7 +1418,7 @@ struct oxr_session /*! initial relation of head in "global" space. * Used as reference for local space. */ - struct xrt_space_relation initial_head_relation; + struct xrt_space_relation local_space_pure_relation; }; /*! diff --git a/src/xrt/state_trackers/oxr/oxr_session.c b/src/xrt/state_trackers/oxr/oxr_session.c index 932797549..9a4a27bfc 100644 --- a/src/xrt/state_trackers/oxr/oxr_session.c +++ b/src/xrt/state_trackers/oxr/oxr_session.c @@ -399,16 +399,18 @@ oxr_session_locate_views(struct oxr_logger *log, fovs, // poses); - // head_relation is in xdev space. Bring it into global space by applying tracking origin offset. + + 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, &head_relation); + m_relation_chain_resolve(&xrc, &pure_head_relation); - // transform head_relation into base_space - struct xrt_space_relation base_spc_head_relation; - oxr_view_relation_ref_relation(log, sess, &head_relation, xdev, baseSpc, viewLocateInfo->displayTime, - &base_spc_head_relation); + struct xrt_space_relation head_relation_in_base_space; + oxr_space_pure_relation_in_space(log, viewLocateInfo->displayTime, &pure_head_relation, baseSpc, true, + &head_relation_in_base_space); // Clear here and filled in loop. viewState->viewStateFlags = 0; @@ -424,8 +426,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, &base_spc_head_relation); - m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &baseSpc->pose); + m_relation_chain_push_relation(&xrc, &head_relation_in_base_space); m_relation_chain_resolve(&xrc, &result); union { struct xrt_pose xrt; @@ -865,13 +866,21 @@ oxr_session_hand_joints(struct oxr_logger *log, struct xrt_device *xdev = hand_tracker->xdev; enum xrt_input_name name = hand_tracker->input_name; - struct xrt_pose *tracking_origin_offset = &xdev->tracking_origin->offset; - XrTime at_time = locateInfo->time; struct xrt_hand_joint_set value; 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; + 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; + oxr_space_pure_relation_in_space(log, at_time, &pure_hand_relation, baseSpc, true, &hand_pose_in_base_space); + + for (uint32_t i = 0; i < locations->jointCount; i++) { locations->jointLocations[i].locationFlags = xrt_to_xr_space_location_flags(value.values.hand_joint_set_default[i].relation.relation_flags); @@ -882,72 +891,9 @@ 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); - - - if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_STAGE) { - - m_relation_chain_push_relation(&chain, &value.hand_pose); - m_relation_chain_push_pose_if_not_identity(&chain, tracking_origin_offset); - - } else if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - - // for local space, first do stage space and transform - // result to local @todo: improve local space - m_relation_chain_push_relation(&chain, &value.hand_pose); - m_relation_chain_push_pose_if_not_identity(&chain, tracking_origin_offset); - - } else if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_VIEW) { - /*! @todo: testing, relating to view space unsupported - * in other parts of monado */ - - struct xrt_device *view_xdev = NULL; - - struct xrt_space_relation view_relation; - oxr_session_get_view_relation_at(log, sess, at_time, &view_relation, &view_xdev); - - m_relation_chain_push_relation(&chain, &value.hand_pose); - m_relation_chain_push_pose_if_not_identity(&chain, tracking_origin_offset); - - m_relation_chain_push_inverted_relation(&chain, &view_relation); - m_relation_chain_push_inverted_pose_if_not_identity(&chain, - &view_xdev->tracking_origin->offset); - - } else if (!baseSpc->is_reference) { - // action space - - struct oxr_action_input *input = NULL; - oxr_action_get_pose_input(log, sess, baseSpc->act_key, &baseSpc->subaction_paths, &input); - - // If the input isn't active. - if (input == NULL) { - locations->isActive = false; - return XR_SUCCESS; - } - - struct xrt_space_relation act_space_relation; - - oxr_xdev_get_space_relation(log, sess->sys->inst, input->xdev, input->input->name, at_time, - &act_space_relation); - - - m_relation_chain_push_relation(&chain, &value.hand_pose); - m_relation_chain_push_pose_if_not_identity(&chain, tracking_origin_offset); - - m_relation_chain_push_inverted_relation(&chain, &act_space_relation); - m_relation_chain_push_inverted_pose_if_not_identity(&chain, - &input->xdev->tracking_origin->offset); - } - - m_relation_chain_push_inverted_pose_if_not_identity(&chain, &baseSpc->pose); + m_relation_chain_push_relation(&chain, &hand_pose_in_base_space); m_relation_chain_resolve(&chain, &result); - if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - if (!global_to_local_space(sess, &result)) { - locations->isActive = false; - return XR_SUCCESS; - } - } - xrt_to_xr_pose(&result.pose, &locations->jointLocations[i].pose); if (vel) { 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 dbb99889e..3c482c027 100644 --- a/src/xrt/state_trackers/oxr/oxr_session_frame_end.c +++ b/src/xrt/state_trackers/oxr/oxr_session_frame_end.c @@ -804,55 +804,9 @@ handle_space(struct oxr_logger *log, math_quat_normalize(&pose.orientation); } - if (spc->is_reference && spc->type == XR_REFERENCE_SPACE_TYPE_VIEW) { - // The space might have a pose, transform that in as well. - math_pose_transform(&spc->pose, &pose, &pose); - } else if (spc->is_reference) { - // The space might have a pose, transform that in as well. - math_pose_transform(&spc->pose, &pose, &pose); - - // Remove the tracking system origin offset. - math_pose_transform(inv_offset, &pose, &pose); - - if (spc->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - if (!initial_head_relation_valid(sess)) { - return false; - } - math_pose_transform(&sess->initial_head_relation.pose, &pose, &pose); - } - - } else { - //! @todo Action space handling not very complete - - struct oxr_action_input *input = NULL; - - oxr_action_get_pose_input(log, sess, spc->act_key, &spc->subaction_paths, &input); - - // If the input isn't active. - if (input == NULL) { - //! @todo just don't render the quad here? - return false; - } - - - struct xrt_space_relation out_relation; - - oxr_xdev_get_space_relation(log, sess->sys->inst, input->xdev, input->input->name, timestamp, - &out_relation); - - struct xrt_pose device_pose = out_relation.pose; - - // The space might have a pose, transform that in as well. - math_pose_transform(&spc->pose, &device_pose, &device_pose); - - math_pose_transform(&device_pose, &pose, &pose); - - // Remove the tracking system origin offset. - math_pose_transform(inv_offset, &pose, &pose); - } - - - *out_pose = pose; + struct xrt_space_relation rel; + oxr_space_pure_pose_from_space(log, timestamp, &pose, spc, &rel); + *out_pose = rel.pose; return true; } @@ -948,7 +902,6 @@ submit_projection_layer(struct oxr_session *sess, fill_in_sub_image(scs[0], &proj->views[0].subImage, &data.stereo.l.sub); fill_in_sub_image(scs[1], &proj->views[1].subImage, &data.stereo.r.sub); - #ifdef XRT_FEATURE_OPENXR_LAYER_DEPTH const XrCompositionLayerDepthInfoKHR *d_l = OXR_GET_INPUT_FROM_CHAIN( &proj->views[0], XR_TYPE_COMPOSITION_LAYER_DEPTH_INFO_KHR, XrCompositionLayerDepthInfoKHR); diff --git a/src/xrt/state_trackers/oxr/oxr_space.c b/src/xrt/state_trackers/oxr/oxr_space.c index 648439601..69f773dd1 100644 --- a/src/xrt/state_trackers/oxr/oxr_space.c +++ b/src/xrt/state_trackers/oxr/oxr_space.c @@ -123,21 +123,30 @@ get_ref_space_type_short_str(struct oxr_space *spc) } } +static void +print_pose(struct oxr_session *sess, const char *prefix, struct xrt_pose *pose); + static bool -ensure_initial_head_relation(struct oxr_logger *log, struct oxr_session *sess, struct xrt_space_relation *head_relation) +set_up_local_space(struct oxr_logger *log, struct oxr_session *sess, XrTime time) { - if ((head_relation->relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) == 0) { + 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 (!initial_head_relation_valid(sess)) { - sess->initial_head_relation = *head_relation; + 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->initial_head_relation.pose.orientation.x = 0; - sess->initial_head_relation.pose.orientation.z = 0; - math_quat_normalize(&sess->initial_head_relation.pose.orientation); + 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 } @@ -145,224 +154,173 @@ ensure_initial_head_relation(struct oxr_logger *log, struct oxr_session *sess, s } bool -initial_head_relation_valid(struct oxr_session *sess) +is_local_space_set_up(struct oxr_session *sess) { - return sess->initial_head_relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT; + 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. + * + * @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. + */ +static bool +oxr_space_ref_get_pure_relation(struct oxr_logger *log, + struct oxr_session *sess, + XrReferenceSpaceType ref_type, + struct xrt_device *ref_xdev, + XrTime time, + struct xrt_space_relation *out_relation) +{ + switch (ref_type) { + case XR_REFERENCE_SPACE_TYPE_LOCAL: { + if (!is_local_space_set_up(sess)) { + if (!set_up_local_space(log, sess, time)) { + return false; + } + return false; + } + + *out_relation = sess->local_space_pure_relation; + return true; + } + case XR_REFERENCE_SPACE_TYPE_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 XR_REFERENCE_SPACE_TYPE_VIEW: { + oxr_xdev_get_space_relation(log, sess->sys->inst, ref_xdev, XRT_INPUT_GENERIC_HEAD_POSE, time, + out_relation); + return true; + } + + case XR_REFERENCE_SPACE_TYPE_UNBOUNDED_MSFT: + case XR_REFERENCE_SPACE_TYPE_COMBINED_EYE_VARJO: + // not implemented + return false; + case XR_REFERENCE_SPACE_TYPE_MAX_ENUM: return false; ; + } + return true; } bool -global_to_local_space(struct oxr_session *sess, struct xrt_space_relation *rel) +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) { - if (!initial_head_relation_valid(sess)) { + struct xrt_space_relation pure_space_relation; + struct xrt_device *xdev; + oxr_space_get_pure_relation(log, spc, time, &pure_space_relation, &xdev); + + 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; +} + +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); +} + +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; + struct xrt_device *xdev; + oxr_space_get_pure_relation(log, spc, time, &pure_space_relation, &xdev); + + struct xrt_relation_chain xrc = {0}; + m_relation_chain_push_relation(&xrc, &pure_space_relation); + m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &spc->pose); + m_relation_chain_push_relation(&xrc, relation); + m_relation_chain_resolve(&xrc, out_relation); + return true; +} + +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); +} + +bool +oxr_space_get_pure_relation(struct oxr_logger *log, + struct oxr_space *spc, + XrTime time, + struct xrt_space_relation *out_relation, + struct xrt_device **out_xdev) +{ + if (spc->is_reference) { + struct xrt_device *head_xdev = GET_XDEV_BY_ROLE(spc->sess->sys, head); + *out_xdev = head_xdev; + return oxr_space_ref_get_pure_relation(log, spc->sess, spc->type, head_xdev, time, out_relation); + } + + struct oxr_action_input *input = NULL; + oxr_action_get_pose_input(log, 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; + } + + *out_xdev = input->xdev; + oxr_xdev_get_space_relation(log, spc->sess->sys->inst, input->xdev, input->input->name, time, out_relation); + + return true; +} + +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; + } 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->initial_head_relation.pose); + m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &sess->local_space_pure_relation.pose); m_relation_chain_resolve(&xrc, rel); return true; } -/*! - * Transform @p view_relation given in global space into @p baseSpc without the app-given offset pose for @p baseSpc - * applied. - */ -XrResult -oxr_view_relation_ref_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct xrt_space_relation *view_relation, - struct xrt_device *view_xdev, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation) -{ - *out_relation = *view_relation; - - //! @todo: find a central place to set up local space - if (!ensure_initial_head_relation(log, sess, out_relation)) { - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return XR_SUCCESS; - } - - if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_STAGE) { - // device poses are already in stage = "global" space - } else if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - global_to_local_space(sess, out_relation); - } else if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_VIEW) { - // view relation in view space should be identity - m_space_relation_ident(out_relation); - } else { - OXR_WARN_ONCE(log, "unsupported base reference space in space_ref_relation"); - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return XR_SUCCESS; - } - - return XR_SUCCESS; -} - -static XrResult -oxr_view_ref_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation) -{ - struct xrt_device *view_xdev = NULL; - struct xrt_space_relation view_relation; - oxr_session_get_view_relation_at(log, sess, time, &view_relation, &view_xdev); - return oxr_view_relation_ref_relation(log, sess, &view_relation, view_xdev, baseSpc, time, out_relation); -} - -static XrResult -oxr_stage_ref_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation) -{ - if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - math_pose_invert(&sess->initial_head_relation.pose, &out_relation->pose); - } else { - OXR_WARN_ONCE(log, "unsupported base space in space_ref_relation"); - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return XR_SUCCESS; - } - return XR_SUCCESS; -} - -static XrResult -oxr_local_ref_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation) -{ - if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_STAGE) { - out_relation->pose = sess->initial_head_relation.pose; - } else { - OXR_WARN_ONCE(log, "unsupported base space in space_ref_relation"); - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return XR_SUCCESS; - } - return XR_SUCCESS; -} - -/*! - * This returns only the relation between two spaces without any of the app - * given relations applied, assumes that both spaces are reference spaces. - */ -XrResult -oxr_space_ref_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct oxr_space *space, - struct oxr_space *baseSpc, - XrTime time, - struct xrt_space_relation *out_relation) -{ - m_space_relation_ident(out_relation); - - if (space->type == baseSpc->type) { - // m_space_relation_ident() already set this to identity. - } else if (space->type == XR_REFERENCE_SPACE_TYPE_VIEW) { - oxr_view_ref_relation(log, sess, baseSpc, time, out_relation); - } else if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_VIEW) { - oxr_view_ref_relation(log, sess, space, time, out_relation); - //! @todo invert complete relation - math_pose_invert(&out_relation->pose, &out_relation->pose); - } else if (space->type == XR_REFERENCE_SPACE_TYPE_STAGE) { - oxr_stage_ref_relation(log, sess, baseSpc, time, out_relation); - } else if (space->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - oxr_local_ref_relation(log, sess, baseSpc, time, out_relation); - } else { - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return XR_SUCCESS; - } - - return XR_SUCCESS; -} - -static void -remove_angular_and_linear_stuff(struct xrt_space_relation *out_relation) -{ - const enum xrt_space_relation_flags flags = - XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT; - - out_relation->relation_flags &= ~flags; - out_relation->linear_velocity = (struct xrt_vec3){0, 0, 0}; - out_relation->angular_velocity = (struct xrt_vec3){0, 0, 0}; -} - -/*! - * This returns only the relation between two space without the app given offset pose for baseSpc applied, - * assumes that only one is a action space. - */ -static XrResult -oxr_space_action_relation(struct oxr_logger *log, - struct oxr_session *sess, - struct oxr_space *spc, - struct oxr_space *baseSpc, - XrTime at_time, - struct xrt_space_relation *out_relation) -{ - struct oxr_action_input *input = NULL; - struct oxr_space *act_spc, *ref_spc = NULL; - bool invert = false; - - // Find the action space - if (baseSpc->is_reference) { - // Note spc, is assumed to be the action space. - act_spc = spc; - ref_spc = baseSpc; - } - - // Find the action space. - if (spc->is_reference) { - // Note baseSpc, is assumed to be the action space. - act_spc = baseSpc; - ref_spc = spc; - invert = true; - } - - // Internal error check. - if (act_spc == NULL || act_spc->is_reference || ref_spc == NULL || !ref_spc->is_reference) { - return oxr_error(log, XR_ERROR_RUNTIME_FAILURE, "This is bad!"); - } - - // Reset so no relation is returned. - m_space_relation_ident(out_relation); - - //! @todo Can not relate to the view space right now. - if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_VIEW) { - //! @todo Error code? - OXR_WARN_ONCE(log, "relating to view space unsupported"); - return XR_SUCCESS; - } - - oxr_action_get_pose_input(log, sess, act_spc->act_key, &act_spc->subaction_paths, &input); - - // If the input isn't active. - if (input == NULL) { - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return XR_SUCCESS; - } - - oxr_xdev_get_space_relation(log, sess->sys->inst, input->xdev, input->input->name, at_time, out_relation); - - if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - global_to_local_space(sess, out_relation); - } - - if (invert) { - math_pose_invert(&out_relation->pose, &out_relation->pose); - // Remove this since we can't (for now) invert the derivatives. - remove_angular_and_linear_stuff(out_relation); - } - - return XR_SUCCESS; -} - /*! * This returns only the relation between two directly-associated spaces without * the app given offset pose for baseSpc applied. @@ -374,26 +332,19 @@ get_pure_space_relation(struct oxr_logger *log, XrTime time, struct xrt_space_relation *out_relation) { - struct oxr_session *sess = spc->sess; + struct xrt_space_relation space_pure_relation; + struct xrt_device *space_xdev; + oxr_space_get_pure_relation(log, spc, time, &space_pure_relation, &space_xdev); - if (spc->is_reference && baseSpc->is_reference) { - return oxr_space_ref_relation(log, sess, spc, baseSpc, time, out_relation); - } - /// @todo Deal with action to action by keeping a true_space that we can always go via. (poor mans space graph) - if (!spc->is_reference && !baseSpc->is_reference) { - // WARNING order not thought through here! - // struct xrt_pose pose1; - // struct xrt_pose pose2; - // get_pure_space_relation(log, session->true_space, baseSpc, - // time, &pose1); - // get_pure_space_relation(log, space, session->true_space, - // time, &pose2); - // math_pose_relate_2(&pose1, &pose2, out_pose); - out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; - return XR_SUCCESS; - } + struct xrt_space_relation base_space_pure_relation; + struct xrt_device *base_space_xdev; + oxr_space_get_pure_relation(log, baseSpc, time, &base_space_pure_relation, &base_space_xdev); + + 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); - oxr_space_action_relation(log, sess, spc, baseSpc, time, out_relation); return XR_SUCCESS; } @@ -468,8 +419,6 @@ oxr_space_locate( print_space("baseSpace", baseSpc); // Get the pure space relation. - //! @todo for longer paths in "space graph" than one edge, this will be - //! a loop. struct xrt_space_relation pure; XrResult ret = get_pure_space_relation(log, spc, baseSpc, time, &pure); if (ret != XR_SUCCESS) {