From f5ef3985ba009245cee5e1825e54e1035e6a3d5f Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Fri, 13 Nov 2020 01:30:25 +0100 Subject: [PATCH] xrt: Add support for locating hand joints in action spaces --- src/xrt/auxiliary/util/u_hand_tracking.c | 27 ++++------ src/xrt/auxiliary/util/u_hand_tracking.h | 2 +- src/xrt/drivers/survive/survive_driver.c | 2 +- src/xrt/drivers/vive/vive_controller.c | 6 ++- src/xrt/include/xrt/xrt_defines.h | 12 +++-- src/xrt/include/xrt/xrt_device.h | 4 +- src/xrt/ipc/client/ipc_client_device.c | 2 +- src/xrt/ipc/server/ipc_server_client.c | 2 +- src/xrt/ipc/shared/proto.json | 2 +- src/xrt/state_trackers/oxr/oxr_objects.h | 2 +- src/xrt/state_trackers/oxr/oxr_session.c | 69 ++++++++++++++++++++---- src/xrt/state_trackers/oxr/oxr_xdev.c | 4 +- 12 files changed, 91 insertions(+), 43 deletions(-) diff --git a/src/xrt/auxiliary/util/u_hand_tracking.c b/src/xrt/auxiliary/util/u_hand_tracking.c index 772598452..965ee4057 100644 --- a/src/xrt/auxiliary/util/u_hand_tracking.c +++ b/src/xrt/auxiliary/util/u_hand_tracking.c @@ -668,10 +668,11 @@ u_hand_joints_set_out_data(struct u_hand_tracking *set, enum xrt_hand hand, struct xrt_space_relation *hand_relation, struct xrt_pose *hand_offset, - union xrt_hand_joint_set *out_value) + struct xrt_hand_joint_set *out_value) { - struct xrt_hand_joint_value *l = out_value->hand_joint_set_default; + struct xrt_hand_joint_value *l = + out_value->values.hand_joint_set_default; for (int i = 0; i < XRT_HAND_JOINT_COUNT; i++) { l[i].relation.relation_flags |= @@ -685,23 +686,13 @@ u_hand_joints_set_out_data(struct u_hand_tracking *set, struct u_joint_space_relation *data = get_joint_data(set, i); - // transform poses from "hand space" to "controller space in - // world space" - struct xrt_space_relation transformed; - - transformed.pose = data->relation.pose; - - //! @todo: transform velocities - math_pose_transform(hand_offset, &data->relation.pose, - &transformed.pose); - - math_pose_transform(&hand_relation->pose, &transformed.pose, - &transformed.pose); - - - //! @todo handle velocities - l[i].relation.pose = transformed.pose; + struct xrt_space_graph graph = {0}; + m_space_graph_add_relation(&graph, &data->relation); + m_space_graph_add_pose(&graph, hand_offset); + m_space_graph_resolve(&graph, &l[i].relation); } + + out_value->hand_origin = *hand_relation; } void diff --git a/src/xrt/auxiliary/util/u_hand_tracking.h b/src/xrt/auxiliary/util/u_hand_tracking.h index 21ecd07fd..238c5c7af 100644 --- a/src/xrt/auxiliary/util/u_hand_tracking.h +++ b/src/xrt/auxiliary/util/u_hand_tracking.h @@ -133,7 +133,7 @@ u_hand_joints_set_out_data(struct u_hand_tracking *set, enum xrt_hand hand, struct xrt_space_relation *hand_relation, struct xrt_pose *hand_offset, - union xrt_hand_joint_set *out_value); + struct xrt_hand_joint_set *out_value); /* diff --git a/src/xrt/drivers/survive/survive_driver.c b/src/xrt/drivers/survive/survive_driver.c index 1c9f6c35b..82379d0bc 100644 --- a/src/xrt/drivers/survive/survive_driver.c +++ b/src/xrt/drivers/survive/survive_driver.c @@ -454,7 +454,7 @@ static void survive_controller_get_hand_tracking(struct xrt_device *xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, - union xrt_hand_joint_set *out_value) + struct xrt_hand_joint_set *out_value) { struct survive_device *survive = (struct survive_device *)xdev; diff --git a/src/xrt/drivers/vive/vive_controller.c b/src/xrt/drivers/vive/vive_controller.c index ec3dacad7..bdd35c8ef 100644 --- a/src/xrt/drivers/vive/vive_controller.c +++ b/src/xrt/drivers/vive/vive_controller.c @@ -324,7 +324,7 @@ static void vive_controller_get_hand_tracking(struct xrt_device *xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, - union xrt_hand_joint_set *out_value) + struct xrt_hand_joint_set *out_value) { struct vive_controller_device *d = vive_controller_device(xdev); @@ -369,6 +369,10 @@ vive_controller_get_hand_tracking(struct xrt_device *xdev, struct xrt_space_relation controller_relation = { .pose = {.orientation = d->rot_filtered, .position = {0, 0, pivot_offset_z}}}; + controller_relation.relation_flags = + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT; struct xrt_vec3 static_offset = {0, 0, 0}; diff --git a/src/xrt/include/xrt/xrt_defines.h b/src/xrt/include/xrt/xrt_defines.h index 7b86f0f91..05deb17d6 100644 --- a/src/xrt/include/xrt/xrt_defines.h +++ b/src/xrt/include/xrt/xrt_defines.h @@ -663,9 +663,15 @@ enum xrt_finger * * @ingroup xrt_iface */ -union xrt_hand_joint_set { - struct xrt_hand_joint_value - hand_joint_set_default[XRT_HAND_JOINT_COUNT]; +struct xrt_hand_joint_set +{ + union { + struct xrt_hand_joint_value + hand_joint_set_default[XRT_HAND_JOINT_COUNT]; + } values; + + // in driver global space, without tracking_origin offset + struct xrt_space_relation hand_origin; }; /*! diff --git a/src/xrt/include/xrt/xrt_device.h b/src/xrt/include/xrt/xrt_device.h index 0a208fc67..f67edb359 100644 --- a/src/xrt/include/xrt/xrt_device.h +++ b/src/xrt/include/xrt/xrt_device.h @@ -305,7 +305,7 @@ struct xrt_device void (*get_hand_tracking)(struct xrt_device *xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, - union xrt_hand_joint_set *out_value); + struct xrt_hand_joint_set *out_value); /*! * Set a output value. * @@ -389,7 +389,7 @@ static inline void xrt_device_get_hand_tracking(struct xrt_device *xdev, enum xrt_input_name name, uint64_t requested_timestamp_ns, - union xrt_hand_joint_set *out_value) + struct xrt_hand_joint_set *out_value) { xdev->get_hand_tracking(xdev, name, requested_timestamp_ns, out_value); } diff --git a/src/xrt/ipc/client/ipc_client_device.c b/src/xrt/ipc/client/ipc_client_device.c index f7fc069e0..8cb24e23f 100644 --- a/src/xrt/ipc/client/ipc_client_device.c +++ b/src/xrt/ipc/client/ipc_client_device.c @@ -108,7 +108,7 @@ void ipc_client_device_get_hand_tracking(struct xrt_device *xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, - union xrt_hand_joint_set *out_value) + struct xrt_hand_joint_set *out_value) { struct ipc_client_device *icd = ipc_client_device(xdev); diff --git a/src/xrt/ipc/server/ipc_server_client.c b/src/xrt/ipc/server/ipc_server_client.c index 7f0033057..f6832609d 100644 --- a/src/xrt/ipc/server/ipc_server_client.c +++ b/src/xrt/ipc/server/ipc_server_client.c @@ -598,7 +598,7 @@ ipc_handle_device_get_hand_tracking(volatile struct ipc_client_state *ics, uint32_t id, enum xrt_input_name name, uint64_t at_timestamp, - union xrt_hand_joint_set *out_value) + struct xrt_hand_joint_set *out_value) { // To make the code a bit more readable. diff --git a/src/xrt/ipc/shared/proto.json b/src/xrt/ipc/shared/proto.json index bc7057c39..dbbcdcc95 100644 --- a/src/xrt/ipc/shared/proto.json +++ b/src/xrt/ipc/shared/proto.json @@ -187,7 +187,7 @@ {"name": "at_timestamp", "type": "uint64_t"} ], "out": [ - {"name": "value", "type": "union xrt_hand_joint_set"} + {"name": "value", "type": "struct xrt_hand_joint_set"} ] }, diff --git a/src/xrt/state_trackers/oxr/oxr_objects.h b/src/xrt/state_trackers/oxr/oxr_objects.h index 60c117d67..9c67921f0 100644 --- a/src/xrt/state_trackers/oxr/oxr_objects.h +++ b/src/xrt/state_trackers/oxr/oxr_objects.h @@ -942,7 +942,7 @@ oxr_xdev_get_hand_tracking_at(struct oxr_logger *log, struct xrt_device *xdev, enum xrt_input_name name, XrTime at_time, - union xrt_hand_joint_set *out_value); + struct xrt_hand_joint_set *out_value); /* * diff --git a/src/xrt/state_trackers/oxr/oxr_session.c b/src/xrt/state_trackers/oxr/oxr_session.c index 4d085a602..eee7e1871 100644 --- a/src/xrt/state_trackers/oxr/oxr_session.c +++ b/src/xrt/state_trackers/oxr/oxr_session.c @@ -2351,7 +2351,7 @@ oxr_session_hand_joints(struct oxr_logger *log, &xdev->tracking_origin->offset; XrTime at_time = locateInfo->time; - union xrt_hand_joint_set value; + struct xrt_hand_joint_set value; enum xrt_input_name name; if (hand_tracker->hand_joint_set == XR_HAND_JOINT_SET_DEFAULT_EXT) { @@ -2365,27 +2365,74 @@ oxr_session_hand_joints(struct oxr_logger *log, &value); for (uint32_t i = 0; i < locations->jointCount; i++) { - locations->jointLocations[i] - .locationFlags = xrt_to_xr_space_location_flags( - value.hand_joint_set_default[i].relation.relation_flags); + locations->jointLocations[i].locationFlags = + xrt_to_xr_space_location_flags( + value.values.hand_joint_set_default[i] + .relation.relation_flags); locations->jointLocations[i].radius = - value.hand_joint_set_default[i].radius; + value.values.hand_joint_set_default[i].radius; struct xrt_space_relation r = - value.hand_joint_set_default[i].relation; + value.values.hand_joint_set_default[i].relation; struct xrt_space_relation result; struct xrt_space_graph graph = {0}; m_space_graph_add_relation(&graph, &r); - m_space_graph_add_pose_if_not_identity(&graph, - tracking_origin_offset); + + + if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_STAGE) { + + m_space_graph_add_relation(&graph, &value.hand_origin); + m_space_graph_add_pose_if_not_identity( + &graph, 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_space_graph_add_relation(&graph, &value.hand_origin); + m_space_graph_add_pose_if_not_identity( + &graph, 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->sub_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_space_graph_add_relation(&graph, &value.hand_origin); + m_space_graph_add_pose_if_not_identity( + &graph, tracking_origin_offset); + + m_space_graph_add_inverted_relation( + &graph, &act_space_relation); + m_space_graph_add_inverted_pose_if_not_identity( + &graph, &input->xdev->tracking_origin->offset); + } + m_space_graph_add_inverted_pose_if_not_identity(&graph, &baseSpc->pose); m_space_graph_resolve(&graph, &result); - //! @todo need general handling of local space if (baseSpc->type == XR_REFERENCE_SPACE_TYPE_LOCAL) { - global_to_local_space(sess, &result); + if (!global_to_local_space(sess, &result)) { + locations->isActive = false; + return XR_SUCCESS; + } } xrt_to_xr_pose(&result.pose, @@ -2394,5 +2441,5 @@ oxr_session_hand_joints(struct oxr_logger *log, locations->isActive = true; - return true; + return XR_SUCCESS; } diff --git a/src/xrt/state_trackers/oxr/oxr_xdev.c b/src/xrt/state_trackers/oxr/oxr_xdev.c index 59c3aa86e..8086ae263 100644 --- a/src/xrt/state_trackers/oxr/oxr_xdev.c +++ b/src/xrt/state_trackers/oxr/oxr_xdev.c @@ -107,13 +107,13 @@ oxr_xdev_get_hand_tracking_at(struct oxr_logger *log, struct xrt_device *xdev, enum xrt_input_name name, XrTime at_time, - union xrt_hand_joint_set *out_value) + struct xrt_hand_joint_set *out_value) { //! @todo Convert at_time to monotonic and give to device. uint64_t at_timestamp_ns = os_monotonic_get_ns(); (void)at_time; - union xrt_hand_joint_set value; + struct xrt_hand_joint_set value; xrt_device_get_hand_tracking(xdev, name, at_timestamp_ns, &value);