mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-01 12:46:12 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

aux/util: Remove old hand tracking code

Browse source
This commit is contained in:
danwillm 2022-08-01 17:39:35 +01:00 committed by Moses Turner
parent 9dc8632b0f
commit 9e7f814328
2 changed files with 1 additions and 758 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

702
src/xrt/auxiliary/util/u_hand_tracking.c
View file

@ -4,6 +4,7 @@
* @file
* @brief Hand Tracking API interface.
* @author Christoph Haag <christoph.haag@collabora.com>
* @author Daniel Willmott <web@dan-w.com>
* @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, &param->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,

57
src/xrt/auxiliary/util/u_hand_tracking.h
View file

@ -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.
*