// Copyright 2019-2020, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Hand Tracking API interface. * @author Christoph Haag * @ingroup aux_tracking */ #include "u_hand_tracking.h" #include "math/m_mathinclude.h" #include "math/m_api.h" #include "math/m_space.h" #include "math/m_vec3.h" #include "math/m_api.h" #include "util/u_time.h" #define DEG_TO_RAD(DEG) (DEG * M_PI / 180.) #define VELOCIY_VALID_FLAGS \ (XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) #define POSE_VALID_FLAGS \ (XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | \ XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT) #define HAND_MAX_SPLAY_ANGLE 10.0f struct u_joint_curl_model { enum xrt_hand_joint joint_id; // offset from hand origin (palm) in hand coordinates struct xrt_vec3 position_offset; // rotation always added to this joint float axis_angle_offset[3]; // the length of the bone from this joint towards finger tips float bone_length; float radius; }; //! @todo: Make this tunable by configuration /* describes default configuration for a hand in rest position using the curl * model: Fingers are tracked with a singular curl value per finger. * * Coordinates are in "Hand coordinate system", i.e. a hand flat on a table has * y -> up, -z -> forward (direction of fingers), x -> right. * * Palm is always in the origin of the hand coordinate system. * * Finger Joints are rigidly connected to the bone towards the finger tips. * * metacarpal joints are connected to the wrist in the order * metacarpal, proximal, intermediate, distal, tip (thumb skips intermediate) * * Joint poses are calculated starting at the wrist. Iteratively joint poses are * calculated by rotating the joint by axis_angle_offset, then "following the * attached bone" to the next connected joint, and applying the next rotation * relative to the previous rotation. * * angles for left hand (right hand is mirrored), angles are clockwise. */ static struct u_joint_curl_model hand_joint_default_set_curl_model_defaults[XRT_HAND_JOINT_COUNT] = { // special cases: wrist and palm without bone lengths, offsets are // absolute, relative to hand origin (palm) [XRT_HAND_JOINT_PALM] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0, .radius = 0.018, .joint_id = XRT_HAND_JOINT_PALM}, [XRT_HAND_JOINT_WRIST] = {.position_offset = {.x = 0, .y = 0, .z = 0.07}, .axis_angle_offset = {0, 0, 0}, .bone_length = 0, .radius = 0.03, .joint_id = XRT_HAND_JOINT_WRIST}, // fingers // metacarpal bones are angled outwards a little, // proximal bones copmpensate most of it, making fingers parallel again // finger position offsets are relative to previous bone // previous bone of metacarpal: wrist [XRT_HAND_JOINT_LITTLE_METACARPAL] = {.position_offset = {.x = -0.03, .y = 0, .z = -0.037}, .axis_angle_offset = {0, DEG_TO_RAD(-25), 0}, .bone_length = 0.048, .radius = 0.015, .joint_id = XRT_HAND_JOINT_LITTLE_METACARPAL}, [XRT_HAND_JOINT_LITTLE_PROXIMAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, DEG_TO_RAD(20), 0}, .bone_length = 0.035, .radius = 0.01, .joint_id = XRT_HAND_JOINT_LITTLE_PROXIMAL}, [XRT_HAND_JOINT_LITTLE_INTERMEDIATE] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.028, .radius = 0.009, .joint_id = XRT_HAND_JOINT_LITTLE_INTERMEDIATE}, [XRT_HAND_JOINT_LITTLE_DISTAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.022, .radius = 0.009, .joint_id = XRT_HAND_JOINT_LITTLE_DISTAL}, [XRT_HAND_JOINT_LITTLE_TIP] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0, .radius = 0.012, .joint_id = XRT_HAND_JOINT_LITTLE_TIP}, [XRT_HAND_JOINT_RING_METACARPAL] = {.position_offset = {.x = -0.015, .y = 0, .z = -0.038}, .axis_angle_offset = {0, DEG_TO_RAD(-12), 0}, .bone_length = 0.052, .radius = 0.015, .joint_id = XRT_HAND_JOINT_RING_METACARPAL}, [XRT_HAND_JOINT_RING_PROXIMAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, DEG_TO_RAD(10), 0}, .bone_length = 0.040, .radius = 0.012, .joint_id = XRT_HAND_JOINT_RING_PROXIMAL}, [XRT_HAND_JOINT_RING_INTERMEDIATE] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.031, .radius = 0.01, .joint_id = XRT_HAND_JOINT_RING_INTERMEDIATE}, [XRT_HAND_JOINT_RING_DISTAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.023, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_RING_DISTAL}, [XRT_HAND_JOINT_RING_TIP] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0, .radius = 0.013f, .joint_id = XRT_HAND_JOINT_RING_TIP}, [XRT_HAND_JOINT_MIDDLE_METACARPAL] = {.position_offset = {.x = 0, .y = 0, .z = -0.039f}, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.045f, .radius = 0.012f, .joint_id = XRT_HAND_JOINT_MIDDLE_METACARPAL}, [XRT_HAND_JOINT_MIDDLE_PROXIMAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.042f, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_MIDDLE_PROXIMAL}, [XRT_HAND_JOINT_MIDDLE_INTERMEDIATE] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.033f, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_MIDDLE_INTERMEDIATE}, [XRT_HAND_JOINT_MIDDLE_DISTAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.024f, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_MIDDLE_DISTAL}, [XRT_HAND_JOINT_MIDDLE_TIP] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_MIDDLE_TIP}, [XRT_HAND_JOINT_INDEX_METACARPAL] = {.position_offset = {.x = 0.015, .y = 0, .z = -0.038}, .axis_angle_offset = {0, DEG_TO_RAD(12.0f), 0}, .bone_length = 0.05f, .radius = 0.012f, .joint_id = XRT_HAND_JOINT_INDEX_METACARPAL}, [XRT_HAND_JOINT_INDEX_PROXIMAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, DEG_TO_RAD(-10.0f), 0}, .bone_length = 0.040f, .radius = 0.011f, .joint_id = XRT_HAND_JOINT_INDEX_PROXIMAL}, [XRT_HAND_JOINT_INDEX_INTERMEDIATE] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.031f, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_INDEX_INTERMEDIATE}, [XRT_HAND_JOINT_INDEX_DISTAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.023f, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_INDEX_DISTAL}, [XRT_HAND_JOINT_INDEX_TIP] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0, .radius = 0.01f, .joint_id = XRT_HAND_JOINT_INDEX_TIP}, [XRT_HAND_JOINT_THUMB_METACARPAL] = {.position_offset = {.x = 0.03f, .y = 0, .z = -0.033f}, .axis_angle_offset = {0, DEG_TO_RAD(40), 0}, .bone_length = 0.037f, .radius = 0.0175f, .joint_id = XRT_HAND_JOINT_THUMB_METACARPAL}, [XRT_HAND_JOINT_THUMB_PROXIMAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, DEG_TO_RAD(-12), 0}, .bone_length = 0.038f, .radius = 0.017f, .joint_id = XRT_HAND_JOINT_THUMB_PROXIMAL}, // no intermediate [XRT_HAND_JOINT_THUMB_DISTAL] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0.028f, .radius = 0.017f, .joint_id = XRT_HAND_JOINT_THUMB_DISTAL}, [XRT_HAND_JOINT_THUMB_TIP] = {.position_offset = XRT_VEC3_ZERO, .axis_angle_offset = {0, 0, 0}, .bone_length = 0, .radius = 0.016f, .joint_id = XRT_HAND_JOINT_THUMB_TIP}}; inline static void quat_from_angle_vector_clockwise(float angle_rads, const struct xrt_vec3 *vector, struct xrt_quat *result) { math_quat_from_angle_vector(-angle_rads, vector, result); } bool u_hand_joint_is_metacarpal(enum xrt_hand_joint joint) { return joint == XRT_HAND_JOINT_LITTLE_METACARPAL || joint == XRT_HAND_JOINT_RING_METACARPAL || joint == XRT_HAND_JOINT_MIDDLE_METACARPAL || joint == XRT_HAND_JOINT_INDEX_METACARPAL || joint == XRT_HAND_JOINT_THUMB_METACARPAL; } bool u_hand_joint_is_proximal(enum xrt_hand_joint joint) { return joint == XRT_HAND_JOINT_LITTLE_PROXIMAL || joint == XRT_HAND_JOINT_RING_PROXIMAL || joint == XRT_HAND_JOINT_MIDDLE_PROXIMAL || joint == XRT_HAND_JOINT_INDEX_PROXIMAL || joint == XRT_HAND_JOINT_THUMB_PROXIMAL; } bool u_hand_joint_is_intermediate(enum xrt_hand_joint joint) { return joint == XRT_HAND_JOINT_LITTLE_INTERMEDIATE || joint == XRT_HAND_JOINT_RING_INTERMEDIATE || joint == XRT_HAND_JOINT_MIDDLE_INTERMEDIATE || joint == XRT_HAND_JOINT_INDEX_INTERMEDIATE; } bool u_hand_joint_is_distal(enum xrt_hand_joint joint) { return joint == XRT_HAND_JOINT_LITTLE_DISTAL || joint == XRT_HAND_JOINT_RING_DISTAL || joint == XRT_HAND_JOINT_MIDDLE_DISTAL || joint == XRT_HAND_JOINT_INDEX_DISTAL || joint == XRT_HAND_JOINT_THUMB_DISTAL; } bool u_hand_joint_is_tip(enum xrt_hand_joint joint) { return joint == XRT_HAND_JOINT_LITTLE_TIP || joint == XRT_HAND_JOINT_RING_TIP || joint == XRT_HAND_JOINT_MIDDLE_TIP || joint == XRT_HAND_JOINT_INDEX_TIP || joint == XRT_HAND_JOINT_THUMB_TIP; } bool u_hand_joint_is_thumb(enum xrt_hand_joint joint) { return joint == XRT_HAND_JOINT_THUMB_METACARPAL || joint == XRT_HAND_JOINT_THUMB_PROXIMAL || joint == XRT_HAND_JOINT_THUMB_DISTAL || joint == XRT_HAND_JOINT_THUMB_TIP; } static void scale_model_param(struct u_joint_curl_model *param, float scale) { param->bone_length *= scale; math_vec3_scalar_mul(scale, ¶m->position_offset); param->radius *= scale; } static void u_hand_joints_set_default_root_poses(struct u_hand_tracking *set) { const struct xrt_quat identity_quat = XRT_QUAT_IDENTITY; // wrist and palm mostly fixed poses set->joints.wrist.relation.pose = (struct xrt_pose){ .position = hand_joint_default_set_curl_model_defaults[XRT_HAND_JOINT_WRIST].position_offset, .orientation = identity_quat}; set->joints.wrist.relation.linear_velocity = (struct xrt_vec3)XRT_VEC3_ZERO; set->joints.wrist.relation.angular_velocity = (struct xrt_vec3)XRT_VEC3_ZERO; set->joints.wrist.relation.relation_flags = POSE_VALID_FLAGS | VELOCIY_VALID_FLAGS; set->joints.palm.relation.pose = (struct xrt_pose){ .position = hand_joint_default_set_curl_model_defaults[XRT_HAND_JOINT_PALM].position_offset, .orientation = identity_quat}; set->joints.palm.relation.linear_velocity = (struct xrt_vec3)XRT_VEC3_ZERO; set->joints.palm.relation.angular_velocity = (struct xrt_vec3)XRT_VEC3_ZERO; set->joints.palm.relation.relation_flags = POSE_VALID_FLAGS | VELOCIY_VALID_FLAGS; } static void u_hand_joints_fill_finger_value_by_curl(const float curl_value, const int joint_count, struct u_hand_tracking_finger_value *out_finger_value) { out_finger_value->splay = 0.0f; out_finger_value->joint_count = joint_count; for (int i = 0; i < joint_count; i++) out_finger_value->joint_curls[i] = curl_value; } static void u_hand_joints_set_hand_values_by_curl(const struct u_hand_tracking_curl_values *curl_values, struct u_hand_tracking_values *out_hand_tracking_values) { u_hand_joints_fill_finger_value_by_curl(curl_values->thumb, 4, &out_hand_tracking_values->thumb); u_hand_joints_fill_finger_value_by_curl(curl_values->index, 5, &out_hand_tracking_values->index); u_hand_joints_fill_finger_value_by_curl(curl_values->middle, 5, &out_hand_tracking_values->middle); u_hand_joints_fill_finger_value_by_curl(curl_values->ring, 5, &out_hand_tracking_values->ring); u_hand_joints_fill_finger_value_by_curl(curl_values->little, 5, &out_hand_tracking_values->little); } static void u_hand_joints_apply_splay(float splay_value, struct u_joint_space_relation *out_bone) { const struct xrt_vec3 y_axis = XRT_VEC3_UNIT_Y; struct xrt_quat splay_quat; quat_from_angle_vector_clockwise(splay_value * DEG_TO_RAD(HAND_MAX_SPLAY_ANGLE), &y_axis, &splay_quat); math_quat_rotate(&splay_quat, &out_bone->relation.pose.orientation, &out_bone->relation.pose.orientation); } static void u_hand_joints_update_finger(const struct u_hand_tracking *set, struct u_finger_joint_set *joint_set, enum xrt_hand hand, uint64_t at_timestamp_ns, const struct u_hand_tracking_finger_value *value) { struct u_joint_space_relation origin = set->joints.wrist; struct u_joint_space_relation *prev = &origin; for (int joint_num = 0; joint_num < joint_set->joint_count; joint_num++) { struct u_joint_space_relation *joint = &joint_set->joints[joint_num]; u_hand_joint_compute_next_by_curl(set, prev, hand, at_timestamp_ns, joint, value->joint_curls[joint_num]); // apply the splay to the proximal bone if (u_hand_joint_is_proximal(joint->joint_id)) u_hand_joints_apply_splay(value->splay, joint); prev = joint; } } void u_hand_joints_update(struct u_hand_tracking *set, enum xrt_hand hand, uint64_t at_timestamp_ns, const struct u_hand_tracking_values *values) { u_hand_joints_set_default_root_poses(set); u_hand_joints_update_finger(set, &set->joints.fingers[XRT_FINGER_LITTLE], hand, at_timestamp_ns, &values->little); u_hand_joints_update_finger(set, &set->joints.fingers[XRT_FINGER_RING], hand, at_timestamp_ns, &values->ring); u_hand_joints_update_finger(set, &set->joints.fingers[XRT_FINGER_MIDDLE], hand, at_timestamp_ns, &values->middle); u_hand_joints_update_finger(set, &set->joints.fingers[XRT_FINGER_INDEX], hand, at_timestamp_ns, &values->index); u_hand_joints_update_finger(set, &set->joints.fingers[XRT_FINGER_THUMB], hand, at_timestamp_ns, &values->thumb); set->model_data.finger_values = *values; set->timestamp_ns = at_timestamp_ns; } void u_hand_joint_compute_next_by_curl(const struct u_hand_tracking *set, const struct u_joint_space_relation *prev, enum xrt_hand hand, uint64_t at_timestamp_ns, struct u_joint_space_relation *out_joint, float curl_value) { struct xrt_space_relation old_relation = out_joint->relation; struct u_joint_curl_model prev_defaults = hand_joint_default_set_curl_model_defaults[prev->joint_id]; struct u_joint_curl_model defaults = hand_joint_default_set_curl_model_defaults[out_joint->joint_id]; scale_model_param(&prev_defaults, set->scale); scale_model_param(&defaults, set->scale); // prev joint pose is transformed to next joint pose by adding the bone // vector to the joint, and adding rotation based on finger curl struct xrt_pose pose = prev->relation.pose; // create bone vector with orientation of previous joint struct xrt_vec3 bone = {0, 0, -prev_defaults.bone_length}; math_quat_rotate_vec3(&pose.orientation, &bone, &bone); // translate the bone to the previous joint math_vec3_accum(&bone, &pose.position); // curl and bone length alone doesn't give us an actual hand shape. // rotate first finger joints "outwards" to create a hand shape. // the offset rotation should not rotate the curl rotation, it rotates // the joint "around the finger axis", before the curl rotation. //! @todo more axis rotations & make sure order is right //! @todo handle velocities const struct xrt_vec3 y_axis = XRT_VEC3_UNIT_Y; struct xrt_pose offset_pose = XRT_POSE_IDENTITY; if (hand == XRT_HAND_LEFT) { quat_from_angle_vector_clockwise(defaults.axis_angle_offset[1], &y_axis, &offset_pose.orientation); offset_pose.position = defaults.position_offset; } if (hand == XRT_HAND_RIGHT) { quat_from_angle_vector_clockwise(-defaults.axis_angle_offset[1], &y_axis, &offset_pose.orientation); offset_pose.position = (struct xrt_vec3){.x = defaults.position_offset.x * -1, .y = defaults.position_offset.y, .z = defaults.position_offset.z}; } math_pose_transform(&pose, &offset_pose, &pose); // proximal, intermediate, and distal joints (+ bones) will rotate //! @todo make this tunable // full curl angle for this joint at 1.0 value float full_curl_angle = 0; if (u_hand_joint_is_thumb(out_joint->joint_id)) { if (u_hand_joint_is_metacarpal(out_joint->joint_id)) { full_curl_angle = DEG_TO_RAD(10); } else if (u_hand_joint_is_proximal(out_joint->joint_id)) { full_curl_angle = DEG_TO_RAD(30); } else if (u_hand_joint_is_distal(out_joint->joint_id)) { full_curl_angle = DEG_TO_RAD(80); } } else { if (u_hand_joint_is_metacarpal(out_joint->joint_id)) { full_curl_angle = DEG_TO_RAD(0); } else if (u_hand_joint_is_proximal(out_joint->joint_id)) { full_curl_angle = DEG_TO_RAD(70); } else if (u_hand_joint_is_intermediate(out_joint->joint_id)) { full_curl_angle = DEG_TO_RAD(90); } else if (u_hand_joint_is_distal(out_joint->joint_id)) { full_curl_angle = DEG_TO_RAD(65); } } float curl_angle = curl_value * full_curl_angle; const struct xrt_vec3 x_axis = XRT_VEC3_UNIT_X; struct xrt_quat curl_rotation; math_quat_from_angle_vector(-curl_angle, &x_axis, &curl_rotation); math_quat_rotate(&pose.orientation, &curl_rotation, &pose.orientation); //! @todo: full relation with velocities out_joint->relation.pose = pose; double time_diff_s = time_ns_to_s(at_timestamp_ns - set->timestamp_ns); // linear velocity = // diff of current and old joint position, divided by timestamp diff out_joint->relation.linear_velocity = out_joint->relation.pose.position; math_vec3_subtract(&old_relation.pose.position, &out_joint->relation.linear_velocity); math_vec3_scalar_mul(1. / time_diff_s, &out_joint->relation.linear_velocity); if (time_diff_s > 0) { math_quat_finite_difference(&old_relation.pose.orientation, &out_joint->relation.pose.orientation, time_diff_s, &out_joint->relation.angular_velocity); } else { out_joint->relation.angular_velocity = (struct xrt_vec3)XRT_VEC3_ZERO; } out_joint->relation.relation_flags = POSE_VALID_FLAGS | VELOCIY_VALID_FLAGS; } void u_hand_joints_update_curl(struct u_hand_tracking *set, enum xrt_hand hand, uint64_t at_timestamp_ns, struct u_hand_tracking_curl_values *curl_values) { float curl_little = curl_values->little; float curl_ring = curl_values->ring; float curl_middle = curl_values->middle; float curl_index = curl_values->index; float curl_thumb = curl_values->thumb; //! @todo: full relations with velocities u_hand_joints_set_default_root_poses(set); struct u_joint_space_relation *prev = &set->joints.wrist; for (int joint_num = 0; joint_num < set->joints.fingers[XRT_FINGER_LITTLE].joint_count; joint_num++) { struct u_joint_space_relation *joint = &set->joints.fingers[XRT_FINGER_LITTLE].joints[joint_num]; u_hand_joint_compute_next_by_curl(set, prev, hand, at_timestamp_ns, joint, curl_little); prev = joint; } prev = &set->joints.wrist; for (int joint_num = 0; joint_num < set->joints.fingers[XRT_FINGER_RING].joint_count; joint_num++) { struct u_joint_space_relation *joint = &set->joints.fingers[XRT_FINGER_RING].joints[joint_num]; u_hand_joint_compute_next_by_curl(set, prev, hand, at_timestamp_ns, joint, curl_ring); prev = joint; } prev = &set->joints.wrist; for (int joint_num = 0; joint_num < set->joints.fingers[XRT_FINGER_MIDDLE].joint_count; joint_num++) { struct u_joint_space_relation *joint = &set->joints.fingers[XRT_FINGER_MIDDLE].joints[joint_num]; u_hand_joint_compute_next_by_curl(set, prev, hand, at_timestamp_ns, joint, curl_middle); prev = joint; } prev = &set->joints.wrist; for (int joint_num = 0; joint_num < set->joints.fingers[XRT_FINGER_INDEX].joint_count; joint_num++) { struct u_joint_space_relation *joint = &set->joints.fingers[XRT_FINGER_INDEX].joints[joint_num]; u_hand_joint_compute_next_by_curl(set, prev, hand, at_timestamp_ns, joint, curl_index); prev = joint; } prev = &set->joints.wrist; for (int joint_num = 0; joint_num < set->joints.fingers[XRT_FINGER_THUMB].joint_count; joint_num++) { struct u_joint_space_relation *joint = &set->joints.fingers[XRT_FINGER_THUMB].joints[joint_num]; u_hand_joint_compute_next_by_curl(set, prev, hand, at_timestamp_ns, joint, curl_thumb); prev = joint; } u_hand_joints_set_hand_values_by_curl(curl_values, &set->model_data.finger_values); set->timestamp_ns = at_timestamp_ns; } void u_hand_joints_init_default_set(struct u_hand_tracking *set, enum xrt_hand hand, enum u_hand_tracking_model model, float scale) { struct xrt_space_relation identity; m_space_relation_ident(&identity); *set = (struct u_hand_tracking){ .joints = {.palm = {.joint_id = XRT_HAND_JOINT_PALM, .relation = identity}, .wrist = {.joint_id = XRT_HAND_JOINT_WRIST, .relation = identity}, .fingers = {[XRT_FINGER_LITTLE] = {.joint_count = 5, .joints = { { .joint_id = XRT_HAND_JOINT_LITTLE_METACARPAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_LITTLE_PROXIMAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_LITTLE_INTERMEDIATE, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_LITTLE_DISTAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_LITTLE_TIP, .relation = identity, }, }}, [XRT_FINGER_RING] = {.joint_count = 5, .joints = { { .joint_id = XRT_HAND_JOINT_RING_METACARPAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_RING_PROXIMAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_RING_INTERMEDIATE, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_RING_DISTAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_RING_TIP, .relation = identity, }, }}, [XRT_FINGER_MIDDLE] = {.joint_count = 5, .joints = { { .joint_id = XRT_HAND_JOINT_MIDDLE_METACARPAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_MIDDLE_PROXIMAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_MIDDLE_INTERMEDIATE, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_MIDDLE_DISTAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_MIDDLE_TIP, .relation = identity, }, }}, [XRT_FINGER_INDEX] = {.joint_count = 5, .joints = { { .joint_id = XRT_HAND_JOINT_INDEX_METACARPAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_INDEX_PROXIMAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_INDEX_INTERMEDIATE, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_INDEX_DISTAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_INDEX_TIP, .relation = identity, }, }}, [XRT_FINGER_THUMB] = {.joint_count = 4, .joints = { { .joint_id = XRT_HAND_JOINT_THUMB_METACARPAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_THUMB_PROXIMAL, .relation = identity, }, // has no intermediate { .joint_id = XRT_HAND_JOINT_THUMB_DISTAL, .relation = identity, }, { .joint_id = XRT_HAND_JOINT_THUMB_TIP, .relation = identity, }, }}}}}; set->model = XRT_HAND_TRACKING_MODEL_INTRINSIC; set->scale = scale; struct u_hand_tracking_curl_values values = {0, 0, 0, 0, 0}; u_hand_joints_update_curl(set, hand, 0, &values); } static struct u_joint_space_relation * get_joint_data(struct u_hand_tracking *set, enum xrt_hand_joint joint_id) { switch (joint_id) { case XRT_HAND_JOINT_WRIST: return &set->joints.wrist; case XRT_HAND_JOINT_PALM: return &set->joints.palm; case XRT_HAND_JOINT_LITTLE_METACARPAL: return &set->joints.fingers[XRT_FINGER_LITTLE].joints[0]; case XRT_HAND_JOINT_LITTLE_PROXIMAL: return &set->joints.fingers[XRT_FINGER_LITTLE].joints[1]; case XRT_HAND_JOINT_LITTLE_INTERMEDIATE: return &set->joints.fingers[XRT_FINGER_LITTLE].joints[2]; case XRT_HAND_JOINT_LITTLE_DISTAL: return &set->joints.fingers[XRT_FINGER_LITTLE].joints[3]; case XRT_HAND_JOINT_LITTLE_TIP: return &set->joints.fingers[XRT_FINGER_LITTLE].joints[4]; case XRT_HAND_JOINT_RING_METACARPAL: return &set->joints.fingers[XRT_FINGER_RING].joints[0]; case XRT_HAND_JOINT_RING_PROXIMAL: return &set->joints.fingers[XRT_FINGER_RING].joints[1]; case XRT_HAND_JOINT_RING_INTERMEDIATE: return &set->joints.fingers[XRT_FINGER_RING].joints[2]; case XRT_HAND_JOINT_RING_DISTAL: return &set->joints.fingers[XRT_FINGER_RING].joints[3]; case XRT_HAND_JOINT_RING_TIP: return &set->joints.fingers[XRT_FINGER_RING].joints[4]; case XRT_HAND_JOINT_MIDDLE_METACARPAL: return &set->joints.fingers[XRT_FINGER_MIDDLE].joints[0]; case XRT_HAND_JOINT_MIDDLE_PROXIMAL: return &set->joints.fingers[XRT_FINGER_MIDDLE].joints[1]; case XRT_HAND_JOINT_MIDDLE_INTERMEDIATE: return &set->joints.fingers[XRT_FINGER_MIDDLE].joints[2]; case XRT_HAND_JOINT_MIDDLE_DISTAL: return &set->joints.fingers[XRT_FINGER_MIDDLE].joints[3]; case XRT_HAND_JOINT_MIDDLE_TIP: return &set->joints.fingers[XRT_FINGER_MIDDLE].joints[4]; case XRT_HAND_JOINT_INDEX_METACARPAL: return &set->joints.fingers[XRT_FINGER_INDEX].joints[0]; case XRT_HAND_JOINT_INDEX_PROXIMAL: return &set->joints.fingers[XRT_FINGER_INDEX].joints[1]; case XRT_HAND_JOINT_INDEX_INTERMEDIATE: return &set->joints.fingers[XRT_FINGER_INDEX].joints[2]; case XRT_HAND_JOINT_INDEX_DISTAL: return &set->joints.fingers[XRT_FINGER_INDEX].joints[3]; case XRT_HAND_JOINT_INDEX_TIP: return &set->joints.fingers[XRT_FINGER_INDEX].joints[4]; case XRT_HAND_JOINT_THUMB_METACARPAL: return &set->joints.fingers[XRT_FINGER_THUMB].joints[0]; case XRT_HAND_JOINT_THUMB_PROXIMAL: return &set->joints.fingers[XRT_FINGER_THUMB].joints[1]; // no intermediate for thumb case XRT_HAND_JOINT_THUMB_DISTAL: return &set->joints.fingers[XRT_FINGER_THUMB].joints[2]; case XRT_HAND_JOINT_THUMB_TIP: return &set->joints.fingers[XRT_FINGER_THUMB].joints[3]; case XRT_HAND_JOINT_MAX_ENUM: return NULL; } return NULL; } void u_hand_joints_set_out_data(struct u_hand_tracking *set, enum xrt_hand hand, const struct xrt_space_relation *hand_relation, const struct xrt_pose *hand_offset, struct xrt_hand_joint_set *out_value) { struct xrt_hand_joint_value *l = out_value->values.hand_joint_set_default; for (int i = 0; i < XRT_HAND_JOINT_COUNT; i++) { struct u_joint_space_relation *data = get_joint_data(set, i); l[i].relation.relation_flags |= data->relation.relation_flags; l[i].radius = hand_joint_default_set_curl_model_defaults[i].radius; struct xrt_relation_chain chain = {0}; m_relation_chain_push_relation(&chain, &data->relation); m_relation_chain_push_pose(&chain, hand_offset); m_relation_chain_resolve(&chain, &l[i].relation); // joint relations can not be "more valid" than the hand relation // after relation chain to make sure flags are not "upgraded" l[i].relation.relation_flags &= hand_relation->relation_flags; } out_value->hand_pose = *hand_relation; } void u_hand_joints_offset_valve_index_controller(enum xrt_hand hand, const struct xrt_vec3 *static_offset, struct xrt_pose *offset) { /* Controller space origin is at the very tip of the controller, * handle pointing forward at -z. * * Transform joints into controller space by rotating "outwards" around * -z "forward" by -75/75 deg. Then, rotate "forward" around x by 72 * deg. * * Then position everything at static_offset.. * * Now the hand points "through the strap" like at normal use. */ const struct xrt_vec3 x = XRT_VEC3_UNIT_X; const struct xrt_vec3 y = XRT_VEC3_UNIT_Y; const struct xrt_vec3 negative_z = {0, 0, -1}; float hand_on_handle_x_rotation = DEG_TO_RAD(-72); float hand_on_handle_y_rotation = 0; float hand_on_handle_z_rotation = 0; if (hand == XRT_HAND_LEFT) { hand_on_handle_z_rotation = DEG_TO_RAD(-75); } else if (hand == XRT_HAND_RIGHT) { hand_on_handle_z_rotation = DEG_TO_RAD(75); } struct xrt_quat hand_rotation_y = XRT_QUAT_IDENTITY; math_quat_from_angle_vector(hand_on_handle_y_rotation, &y, &hand_rotation_y); struct xrt_quat hand_rotation_z = XRT_QUAT_IDENTITY; math_quat_from_angle_vector(hand_on_handle_z_rotation, &negative_z, &hand_rotation_z); struct xrt_quat hand_rotation_x = XRT_QUAT_IDENTITY; math_quat_from_angle_vector(hand_on_handle_x_rotation, &x, &hand_rotation_x); struct xrt_quat hand_rotation; math_quat_rotate(&hand_rotation_x, &hand_rotation_z, &hand_rotation); struct xrt_pose hand_on_handle_pose = {.orientation = hand_rotation, .position = *static_offset}; *offset = hand_on_handle_pose; }