diff --git a/src/xrt/auxiliary/util/u_hand_tracking.c b/src/xrt/auxiliary/util/u_hand_tracking.c index 3de9ceccb..c59adbf35 100644 --- a/src/xrt/auxiliary/util/u_hand_tracking.c +++ b/src/xrt/auxiliary/util/u_hand_tracking.c @@ -4,6 +4,7 @@ * @file * @brief Hand Tracking API interface. * @author Christoph Haag + * @author Daniel Willmott * @ingroup aux_tracking */ @@ -20,224 +21,6 @@ #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) { @@ -284,489 +67,6 @@ u_hand_joint_is_thumb(enum xrt_hand_joint joint) 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, diff --git a/src/xrt/auxiliary/util/u_hand_tracking.h b/src/xrt/auxiliary/util/u_hand_tracking.h index b70ed776a..9facd9041 100644 --- a/src/xrt/auxiliary/util/u_hand_tracking.h +++ b/src/xrt/auxiliary/util/u_hand_tracking.h @@ -154,63 +154,6 @@ u_hand_joint_is_distal(enum xrt_hand_joint joint); bool u_hand_joint_is_thumb(enum xrt_hand_joint joint); -/*! - * Initializes a hand tracking set with default data. - * - * @ingroup aux_util - */ -void -u_hand_joints_init_default_set(struct u_hand_tracking *set, - enum xrt_hand hand, - enum u_hand_tracking_model model, - float scale); - -/*! - * Helper function using hand_relation and hand_offset to transform joint - * locations from an xrt_hand_tracking data in hand space - * to an xrt_hand_joint_set in global space. - * - * @ingroup aux_util - */ -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); - - -/* - * - * Curl model specific functions - * - */ - -/*! - * @ingroup aux_util - */ -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); - -/*! - * @ingroup aux_util - */ -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); - -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); /*! * Simple helper function for positioning hands on Valve Index controllers. *