From d60cce89779fb847ea886f98750c68ced6b3887c Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Thu, 23 Jul 2020 12:12:29 +0100 Subject: [PATCH] st/oxr: Use new xrt_space_graph --- src/xrt/state_trackers/oxr/oxr_objects.h | 42 +++++-------- src/xrt/state_trackers/oxr/oxr_session.c | 57 +++++++++-------- src/xrt/state_trackers/oxr/oxr_space.c | 47 +++++++++----- src/xrt/state_trackers/oxr/oxr_xdev.c | 79 +++++++----------------- 4 files changed, 99 insertions(+), 126 deletions(-) diff --git a/src/xrt/state_trackers/oxr/oxr_objects.h b/src/xrt/state_trackers/oxr/oxr_objects.h index 151cd889d..4880d77ee 100644 --- a/src/xrt/state_trackers/oxr/oxr_objects.h +++ b/src/xrt/state_trackers/oxr/oxr_objects.h @@ -625,14 +625,14 @@ void oxr_session_poll(struct oxr_logger *log, struct oxr_session *sess); /*! - * Get the view space position at the given time in relation to the + * Get the view space relation at the given time in relation to the * local or stage space. */ XrResult -oxr_session_get_view_pose_at(struct oxr_logger *, - struct oxr_session *sess, - XrTime at_time, - struct xrt_pose *); +oxr_session_get_view_relation_at(struct oxr_logger *, + struct oxr_session *sess, + XrTime at_time, + struct xrt_space_relation *out_relation); XrResult oxr_session_views(struct oxr_logger *log, @@ -882,31 +882,21 @@ oxr_xdev_find_output(struct xrt_device *xdev, enum xrt_output_name name, struct xrt_output **out_output); -/*! - * Returns the pose of the named input from the device, if the pose isn't valid - * uses the device offset instead. - */ void -oxr_xdev_get_pose_at(struct oxr_logger *log, - struct oxr_instance *inst, - struct xrt_device *xdev, - enum xrt_input_name name, - XrTime at_time, - uint64_t *out_pose_timestamp_ns, - struct xrt_space_relation *out_relation); - -/*! - * Returns the relation of the named input from the device, always ensures - * that position and orientation is valid by using the device offset. - */ -void -oxr_xdev_get_relation_at(struct oxr_logger *log, +oxr_xdev_get_space_graph(struct oxr_logger *log, struct oxr_instance *inst, struct xrt_device *xdev, enum xrt_input_name name, XrTime at_time, - uint64_t *out_relation_timestamp_ns, - struct xrt_space_relation *out_relation); + struct xrt_space_graph *xsg); + +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); /* @@ -1287,8 +1277,6 @@ struct oxr_session */ float ipd_meters; - float static_prediction_s; - /*! * To pipe swapchain creation to right code. */ diff --git a/src/xrt/state_trackers/oxr/oxr_session.c b/src/xrt/state_trackers/oxr/oxr_session.c index ba0c31670..7fb57cd2a 100644 --- a/src/xrt/state_trackers/oxr/oxr_session.c +++ b/src/xrt/state_trackers/oxr/oxr_session.c @@ -27,7 +27,7 @@ #include "math/m_api.h" #include "math/m_mathinclude.h" - +#include "math/m_space.h" #include "oxr_objects.h" #include "oxr_logger.h" @@ -43,9 +43,7 @@ #include -DEBUG_GET_ONCE_BOOL_OPTION(dynamic_prediction, "OXR_DYNAMIC_PREDICTION", true) DEBUG_GET_ONCE_NUM_OPTION(ipd, "OXR_DEBUG_IPD_MM", 63) -DEBUG_GET_ONCE_NUM_OPTION(prediction_ms, "OXR_DEBUG_PREDICTION_MS", 11) #define CALL_CHK(call) \ if ((call) == XRT_ERROR_IPC_FAILURE) { \ @@ -278,10 +276,10 @@ oxr_session_poll(struct oxr_logger *log, struct oxr_session *sess) } XrResult -oxr_session_get_view_pose_at(struct oxr_logger *log, - struct oxr_session *sess, - XrTime at_time, - struct xrt_pose *pose) +oxr_session_get_view_relation_at(struct oxr_logger *log, + struct oxr_session *sess, + XrTime at_time, + struct xrt_space_relation *out_relation) { // @todo This function needs to be massively expanded to support all // use cases this drive. The main use of this function is to get @@ -294,17 +292,16 @@ oxr_session_get_view_pose_at(struct oxr_logger *log, // get at least a slightly better position. struct xrt_device *xdev = GET_XDEV_BY_ROLE(sess->sys, head); - struct xrt_space_relation relation; - uint64_t timestamp; // Applies the offset in the function. - oxr_xdev_get_relation_at(log, sess->sys->inst, xdev, - XRT_INPUT_GENERIC_HEAD_POSE, at_time, - ×tamp, &relation); + struct xrt_space_graph xsg = {0}; + oxr_xdev_get_space_graph(log, sess->sys->inst, xdev, + XRT_INPUT_GENERIC_HEAD_POSE, at_time, &xsg); + m_space_graph_resolve(&xsg, out_relation); + +#if 0 // clang-format off - *pose = relation.pose; - bool valid_vel = (relation.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0; // clang-format on @@ -340,6 +337,7 @@ oxr_session_get_view_pose_at(struct oxr_logger *log, pose->orientation = predicted; } +#endif return oxr_session_success_result(sess); } @@ -420,8 +418,6 @@ oxr_session_views(struct oxr_logger *log, baseSpc->type, viewLocateInfo->displayTime, &pure_relation); - struct xrt_pose pure = pure_relation.pose; - // @todo the fov information that we get from xdev->hmd->views[i].fov is // not properly filled out in oh_device.c, fix before wasting time // on debugging weird rendering when adding stuff here. @@ -439,16 +435,26 @@ oxr_session_views(struct oxr_logger *log, xdev->get_view_pose(xdev, &eye_relation, i, &view_pose); // Do the magical space relation dance here. - math_pose_openxr_locate(&view_pose, &pure, &baseSpc->pose, - (struct xrt_pose *)&views[i].pose); + struct xrt_space_relation result = {0}; + struct xrt_space_graph xsg = {0}; + m_space_graph_add_pose_if_not_identity(&xsg, &view_pose); + m_space_graph_add_relation(&xsg, &pure_relation); + m_space_graph_add_pose_if_not_identity(&xsg, &baseSpc->pose); + m_space_graph_resolve(&xsg, &result); + union { + struct xrt_pose xrt; + struct XrPosef oxr; + } safe_copy_pose = {0}; + safe_copy_pose.xrt = result.pose; + views[i].pose = safe_copy_pose.oxr; // Copy the fov information directly from the device. union { struct xrt_fov xrt; XrFovf oxr; - } safe_copy = {0}; - safe_copy.xrt = xdev->hmd->views[i].fov; - views[i].fov = safe_copy.oxr; + } safe_copy_fov = {0}; + safe_copy_fov.xrt = xdev->hmd->views[i].fov; + views[i].fov = safe_copy_fov.oxr; struct xrt_pose *pose = (struct xrt_pose *)&views[i].pose; if (!math_quat_ensure_normalized(&pose->orientation)) { @@ -1328,13 +1334,12 @@ handle_space(struct oxr_logger *log, return false; } - uint64_t xdev_timestamp = 0; struct xrt_space_relation out_relation; - oxr_xdev_get_pose_at(log, sess->sys->inst, input->xdev, - input->input->name, timestamp, - &xdev_timestamp, &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; @@ -1995,8 +2000,6 @@ oxr_session_create(struct oxr_logger *log, } sess->ipd_meters = debug_get_num_option_ipd() / 1000.0f; - sess->static_prediction_s = - debug_get_num_option_prediction_ms() / 1000.0f; oxr_session_change_state(log, sess, XR_SESSION_STATE_IDLE); oxr_session_change_state(log, sess, XR_SESSION_STATE_READY); diff --git a/src/xrt/state_trackers/oxr/oxr_space.c b/src/xrt/state_trackers/oxr/oxr_space.c index eb175f89b..27161796a 100644 --- a/src/xrt/state_trackers/oxr/oxr_space.c +++ b/src/xrt/state_trackers/oxr/oxr_space.c @@ -13,6 +13,7 @@ #include #include "math/m_api.h" +#include "math/m_space.h" #include "util/u_debug.h" #include "util/u_misc.h" @@ -184,14 +185,13 @@ oxr_space_ref_relation(struct oxr_logger *log, XrTime time, struct xrt_space_relation *out_relation) { - math_relation_reset(out_relation); + m_space_relation_ident(out_relation); if (space == baseSpc) { - // math_relation_reset() sets to identity. + // m_space_relation_ident() sets to identity. } else if (space == XR_REFERENCE_SPACE_TYPE_VIEW) { - oxr_session_get_view_pose_at(log, sess, time, - &out_relation->pose); + oxr_session_get_view_relation_at(log, sess, time, out_relation); if (!ensure_initial_head_relation(sess, out_relation)) { out_relation->relation_flags = @@ -214,8 +214,7 @@ oxr_space_ref_relation(struct oxr_logger *log, return XR_SUCCESS; } } else if (baseSpc == XR_REFERENCE_SPACE_TYPE_VIEW) { - oxr_session_get_view_pose_at(log, sess, time, - &out_relation->pose); + oxr_session_get_view_relation_at(log, sess, time, out_relation); if (!ensure_initial_head_relation(sess, out_relation)) { out_relation->relation_flags = @@ -275,11 +274,27 @@ oxr_space_ref_relation(struct oxr_logger *log, 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_LINEAR_ACCELERATION_VALID_BIT | + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | + XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT; + + out_relation->relation_flags &= ~flags; + out_relation->linear_velocity = (struct xrt_vec3){0, 0, 0}; + out_relation->linear_acceleration = (struct xrt_vec3){0, 0, 0}; + out_relation->angular_velocity = (struct xrt_vec3){0, 0, 0}; + out_relation->angular_acceleration = (struct xrt_vec3){0, 0, 0}; +} + /*! * This returns only the relation between two spaces without any of the app * given relations applied, assumes that only one is a action space. */ -XrResult +static XrResult oxr_space_action_relation(struct oxr_logger *log, struct oxr_session *sess, struct oxr_space *spc, @@ -289,11 +304,8 @@ oxr_space_action_relation(struct oxr_logger *log, { struct oxr_action_input *input = NULL; struct oxr_space *act_spc, *ref_spc = NULL; - uint64_t timestamp = 0; bool invert = false; - - // Find the action space if (baseSpc->is_reference) { // Note spc, is assumed to be the action space. @@ -316,7 +328,7 @@ oxr_space_action_relation(struct oxr_logger *log, } // Reset so no relation is returned. - math_relation_reset(out_relation); + m_space_relation_ident(out_relation); //! @todo Can not relate to the view space right now. if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_VIEW) { @@ -334,9 +346,8 @@ oxr_space_action_relation(struct oxr_logger *log, return XR_SUCCESS; } - oxr_xdev_get_pose_at(log, sess->sys->inst, input->xdev, - input->input->name, at_time, ×tamp, - out_relation); + 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->pose); @@ -344,6 +355,8 @@ oxr_space_action_relation(struct oxr_logger *log, 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; @@ -471,7 +484,11 @@ oxr_space_locate(struct oxr_logger *log, // Combine space and base space poses with pure relation struct xrt_space_relation result; - math_relation_openxr_locate(&spc->pose, &pure, &baseSpc->pose, &result); + struct xrt_space_graph graph = {0}; + m_space_graph_add_pose_if_not_identity(&graph, &spc->pose); + m_space_graph_add_relation(&graph, &pure); + m_space_graph_add_inverted_pose_if_not_identity(&graph, &baseSpc->pose); + m_space_graph_resolve(&graph, &result); // Copy union { diff --git a/src/xrt/state_trackers/oxr/oxr_xdev.c b/src/xrt/state_trackers/oxr/oxr_xdev.c index fc07db1ab..944ff22b8 100644 --- a/src/xrt/state_trackers/oxr/oxr_xdev.c +++ b/src/xrt/state_trackers/oxr/oxr_xdev.c @@ -1,4 +1,4 @@ -// Copyright 2019, Collabora, Ltd. +// Copyright 2019-2020, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file @@ -8,7 +8,10 @@ */ #include "os/os_time.h" + #include "math/m_api.h" +#include "math/m_space.h" + #include "util/u_time.h" #include "util/u_misc.h" @@ -79,73 +82,35 @@ oxr_xdev_find_output(struct xrt_device *xdev, return false; } -static void -ensure_valid_position_and_orientation(struct xrt_space_relation *relation, - const struct xrt_pose *fallback) -{ - // clang-format off - bool valid_pos = (relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; - bool valid_ori = (relation->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0; - // clang-format on - - if (!valid_ori) { - relation->pose.orientation = fallback->orientation; - } - - if (!valid_pos) { - relation->pose.position = fallback->position; - } -} - void -oxr_xdev_get_relation_at(struct oxr_logger *log, +oxr_xdev_get_space_graph(struct oxr_logger *log, struct oxr_instance *inst, struct xrt_device *xdev, enum xrt_input_name name, XrTime at_time, - uint64_t *out_relation_timestamp_ns, - struct xrt_space_relation *out_relation) + struct xrt_space_graph *xsg) { - struct xrt_pose *offset = &xdev->tracking_origin->offset; + // Convert at_time to monotonic and give to device. + uint64_t at_timestamp_ns = + time_state_ts_to_monotonic_ns(inst->timekeeping, at_time); + uint64_t dummy = 0; - //! @todo Convert at_time to monotonic and give to device. - uint64_t at_timestamp_ns = os_monotonic_get_ns(); - (void)at_time; - - uint64_t relation_timestamp_ns = 0; - - struct xrt_space_relation relation; - U_ZERO(&relation); - - xrt_device_get_tracked_pose(xdev, name, at_timestamp_ns, - &relation_timestamp_ns, &relation); + struct xrt_space_relation *rel = m_space_graph_reserve(xsg); + xrt_device_get_tracked_pose(xdev, name, at_timestamp_ns, &dummy, rel); // Add in the offset from the tracking system. - math_relation_apply_offset(offset, &relation); - - // Always make those to base things valid. - ensure_valid_position_and_orientation(&relation, offset); - - *out_relation_timestamp_ns = time_state_monotonic_to_ts_ns( - inst->timekeeping, relation_timestamp_ns); - - *out_relation = relation; + m_space_graph_add_pose(xsg, &xdev->tracking_origin->offset); } void -oxr_xdev_get_pose_at(struct oxr_logger *log, - struct oxr_instance *inst, - struct xrt_device *xdev, - enum xrt_input_name name, - XrTime at_time, - uint64_t *out_pose_timestamp_ns, - struct xrt_space_relation *out_relation) +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_space_relation relation; - U_ZERO(&relation); - - oxr_xdev_get_relation_at(log, inst, xdev, name, at_time, - out_pose_timestamp_ns, &relation); - - *out_relation = relation; + struct xrt_space_graph xsg = {0}; + oxr_xdev_get_space_graph(log, inst, xdev, name, at_time, &xsg); + m_space_graph_resolve(&xsg, out_relation); }