matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019-2020, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Wrapper around Mercury's parametric hand code, used by Index and OpenGloves to simulate hand tracking.
6 * @author Christoph Haag <christoph.haag@collabora.com>
7 * @author Moshi Turner <moshiturner@protonmail.com>
8 * @author Daniel Willmott <web@dan-w.com>
9 * @ingroup aux_util
10 */
11
12#include "math/m_mathinclude.h"
13#include "util/u_hand_tracking.h"
14#include "xrt/xrt_defines.h"
15#include "u_hand_simulation.h"
16#include "math/m_api.h"
17#include "u_trace_marker.h"
18
19#define HAND_SIM_NUM_FINGERS 5
20
21// This is a lie for the thumb; we usually do the hidden metacarpal trick there
22#define HAND_SIM_NUM_JOINTS_IN_FINGER 5
23#define HAND_SIM_NUM_ORIENTATIONS_IN_FINGER 4
24
25struct translations55
26{
27 struct xrt_vec3 t[HAND_SIM_NUM_FINGERS][HAND_SIM_NUM_JOINTS_IN_FINGER];
28};
29
30struct orientations54
31{
32 struct xrt_quat q[HAND_SIM_NUM_FINGERS][HAND_SIM_NUM_ORIENTATIONS_IN_FINGER];
33};
34
35// For debugging.
36#if 0
37#include <iostream>
38#define assert_quat_length_1(q) \
39 { \
40 const T scale = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; \
41 if (abs(scale - T(1.0)) > 0.001) { \
42 std::cout << "Length bad! " << scale << std::endl; \
43 assert(false); \
44 }; \
45 }
46#else
47#define assert_quat_length_1(q)
48#endif
49
50static void
51eval_hand_set_rel_orientations(const struct u_hand_sim_hand *opt, struct orientations54 *rel_orientations)
52{
53
54// Thumb MCP hidden orientation
55#if 0
56 Vec2<T> mcp_root_swing;
57
58 mcp_root_swing.x = rad<T>((T)(-10));
59 mcp_root_swing.y = rad<T>((T)(-40));
60
61 T mcp_root_twist = rad<T>((T)(-80));
62
63 SwingTwistToQuaternion(mcp_root_swing, mcp_root_twist, rel_orientations.q[0][0]);
64
65 std::cout << "\n\n\n\nHIDDEN ORIENTATION\n";
66 std::cout << std::setprecision(100);
67 std::cout << rel_orientations.q[0][0].w << std::endl;
68 std::cout << rel_orientations.q[0][0].x << std::endl;
69 std::cout << rel_orientations.q[0][0].y << std::endl;
70 std::cout << rel_orientations.q[0][0].z << std::endl;
71#else
72 // This should be exactly equivalent to the above
73 rel_orientations->q[0][0].w = 0.716990172863006591796875f;
74 rel_orientations->q[0][0].x = 0.1541481912136077880859375f;
75 rel_orientations->q[0][0].y = -0.31655871868133544921875f;
76 rel_orientations->q[0][0].z = -0.6016261577606201171875f;
77#endif
78
79 // Thumb MCP orientation
80 math_quat_from_swing_twist(&opt->thumb.metacarpal.swing, //
81 opt->thumb.metacarpal.twist, //
82 &rel_orientations->q[0][1]);
83
84 // Thumb curls
85 struct xrt_vec2 thumb_swing0 = {opt->thumb.rotations[0], 0.f};
86 math_quat_from_swing(&thumb_swing0, &rel_orientations->q[0][2]);
87
88 struct xrt_vec2 thumb_swing1 = {opt->thumb.rotations[1], 0.f};
89 math_quat_from_swing(&thumb_swing1, &rel_orientations->q[0][3]);
90
91 // Finger orientations
92 for (int i = 0; i < 4; i++) {
93 math_quat_from_swing_twist(&opt->finger[i].metacarpal.swing, //
94 opt->finger[i].metacarpal.twist, //
95 &rel_orientations->q[i + 1][0]);
96
97 math_quat_from_swing(&opt->finger[i].proximal_swing, //
98 &rel_orientations->q[i + 1][1]);
99
100 struct xrt_vec2 finger_swing0 = {opt->finger[i].rotations[0], 0.f};
101 math_quat_from_swing(&finger_swing0, &rel_orientations->q[i + 1][2]);
102 struct xrt_vec2 finger_swing1 = {opt->finger[i].rotations[1], 0.f};
103 math_quat_from_swing(&finger_swing1, &rel_orientations->q[i + 1][3]);
104 }
105}
106
107static inline void
108eval_hand_set_rel_translations(const struct u_hand_sim_hand *opt, struct translations55 *rel_translations)
109{
110 // Basically, we're walking up rel_translations, writing strictly sequentially. Hopefully this is fast.
111
112
113 // Thumb metacarpal translation.
114 rel_translations->t[0][0] = (struct xrt_vec3){0.33097f, -0.1f, -0.25968f};
115
116 // Comes after the invisible joint.
117 rel_translations->t[0][1] = (struct xrt_vec3){0.f, 0.f, 0.f};
118 // prox, distal, tip
119 rel_translations->t[0][2] = (struct xrt_vec3){0.f, 0.f, -0.389626f};
120 rel_translations->t[0][3] = (struct xrt_vec3){0.f, 0.f, -0.311176f};
121 rel_translations->t[0][4] = (struct xrt_vec3){0.f, 0.f, -0.232195f};
122
123 // What's the best place to put this? Here works, but is there somewhere we could put it where it gets accessed
124 // faster?
125 float finger_joint_lengths[4][4] = {
126 {
127 -0.66f,
128 -0.365719f,
129 -0.231581f,
130 -0.201790f,
131 },
132 {
133 -0.645f,
134 -0.404486f,
135 -0.247749f,
136 -0.210121f,
137 },
138 {
139 -0.58f,
140 -0.365639f,
141 -0.225666f,
142 -0.187089f,
143 },
144 {
145 -0.52f,
146 -0.278197f,
147 -0.176178f,
148 -0.157566f,
149 },
150 };
151
152 // Index metacarpal
153 rel_translations->t[1][0] = (struct xrt_vec3){0.16926f, 0.f, -0.34437f};
154 // Middle
155 rel_translations->t[2][0] = (struct xrt_vec3){0.034639f, 0.01f, -0.35573f};
156 // Ring
157 rel_translations->t[3][0] = (struct xrt_vec3){-0.063625f, 0.005f, -0.34164f};
158 // Little
159 rel_translations->t[4][0] = (struct xrt_vec3){-0.1509f, -0.005f, -0.30373f};
160
161 // Index to little finger
162 for (int finger = 0; finger < 4; finger++) {
163 for (int i = 0; i < 4; i++) {
164 int bone = i + 1;
165 rel_translations->t[finger + 1][bone].x = 0.f;
166 rel_translations->t[finger + 1][bone].y = 0.f;
167 rel_translations->t[finger + 1][bone].z = finger_joint_lengths[finger][i];
168 }
169 }
170}
171
172void
173eval_hand_with_orientation(const struct u_hand_sim_hand *opt,
174 bool is_right,
175 struct translations55 *translations_absolute,
176 struct orientations54 *orientations_absolute)
177
178{
179 XRT_TRACE_MARKER();
180
181 struct translations55 rel_translations;
182 struct orientations54 rel_orientations;
183
184 eval_hand_set_rel_orientations(opt, &rel_orientations);
185
186 eval_hand_set_rel_translations(opt, &rel_translations);
187
188 struct xrt_quat orientation_root = XRT_QUAT_IDENTITY;
189
190 // Get each joint's tracking-relative orientation by rotating its parent-relative orientation by the
191 // tracking-relative orientation of its parent.
192 for (size_t finger = 0; finger < HAND_SIM_NUM_FINGERS; finger++) {
193 struct xrt_quat *last_orientation = &orientation_root;
194 for (size_t bone = 0; bone < HAND_SIM_NUM_ORIENTATIONS_IN_FINGER; bone++) {
195 struct xrt_quat *rel_orientation = &rel_orientations.q[finger][bone];
196 struct xrt_quat *out_orientation = &orientations_absolute->q[finger][bone];
197
198 math_quat_rotate(last_orientation, rel_orientation, out_orientation);
199 last_orientation = out_orientation;
200 }
201 }
202
203 // Get each joint's tracking-relative position by rotating its parent-relative translation by the
204 // tracking-relative orientation of its parent, then adding that to its parent's tracking-relative position.
205 struct xrt_vec3 zero = XRT_VEC3_ZERO;
206 for (size_t finger = 0; finger < HAND_SIM_NUM_FINGERS; finger++) {
207 const struct xrt_vec3 *last_translation = &zero;
208 const struct xrt_quat *last_orientation = &orientation_root;
209 for (size_t bone = 0; bone < HAND_SIM_NUM_JOINTS_IN_FINGER; bone++) {
210 struct xrt_vec3 *out_translation = &translations_absolute->t[finger][bone];
211 struct xrt_vec3 *rel_translation = &rel_translations.t[finger][bone];
212
213 // rotate and scale
214 math_quat_rotate_vec3(last_orientation, rel_translation, out_translation);
215 math_vec3_scalar_mul(opt->hand_size, out_translation);
216
217 // If this is a right hand, mirror it.
218 if (is_right) {
219 out_translation->x *= -1;
220 }
221
222 out_translation->x += last_translation->x;
223 out_translation->y += last_translation->y;
224 out_translation->z += last_translation->z;
225
226 // Next iteration, the orientation to rotate by should be the tracking-relative orientation of
227 // this joint.
228
229 // If bone < 4 so we don't go over the end of orientations_absolute. I hope this gets optimized
230 // out anyway.
231 if (bone < 4) {
232 last_orientation = &orientations_absolute->q[finger][bone];
233 // Ditto for translation
234 last_translation = out_translation;
235 }
236 }
237 }
238}
239
240static inline void
241zldtt_ori_right(const struct xrt_quat *orientation, struct xrt_quat *out)
242{
243 struct xrt_quat tmp;
244 tmp.w = orientation->w;
245 tmp.x = orientation->x;
246 tmp.y = orientation->y;
247 tmp.z = orientation->z;
248
249 struct xrt_vec3 x = XRT_VEC3_UNIT_X;
250 struct xrt_vec3 z = XRT_VEC3_UNIT_Z;
251
252 math_quat_rotate_vec3(&tmp, &x, &x);
253 math_quat_rotate_vec3(&tmp, &z, &z);
254
255 // This is a very squashed change-of-basis from left-handed coordinate systems to right-handed coordinate
256 // systems: you multiply everything by (-1 0 0) then negate the X axis.
257
258 x.y *= -1;
259 x.z *= -1;
260
261 z.x *= -1;
262
263 math_quat_from_plus_x_z(&x, &z, out);
264}
265
266
267static inline void
268zldtt(const struct xrt_vec3 *trans, const struct xrt_quat *orientation, bool is_right, struct xrt_space_relation *out)
269{
270
271 out->relation_flags = (enum xrt_space_relation_flags)(
272 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
273 XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
274 out->pose.position.x = trans->x;
275 out->pose.position.y = trans->y;
276 out->pose.position.z = trans->z;
277
278 if (is_right) {
279 zldtt_ori_right(orientation, &out->pose.orientation);
280 } else {
281 out->pose.orientation = *orientation;
282 }
283}
284
285
286static void
287our_eval_to_viz_hand(struct u_hand_sim_hand *opt,
288 struct translations55 *translations_absolute,
289 struct orientations54 *orientations_absolute,
290 bool is_right,
291 struct xrt_hand_joint_set *out_viz_hand)
292{
293 XRT_TRACE_MARKER();
294
295 eval_hand_with_orientation(opt, is_right, translations_absolute, orientations_absolute);
296
297 struct xrt_quat final_wrist_orientation = XRT_QUAT_IDENTITY;
298
299 int joint_acc_idx = 0;
300
301 // Palm.
302 struct xrt_vec3 palm_position;
303 palm_position.x = (translations_absolute->t[2][0].x + translations_absolute->t[2][1].x) / 2;
304 palm_position.y = (translations_absolute->t[2][0].y + translations_absolute->t[2][1].y) / 2;
305 palm_position.z = (translations_absolute->t[2][0].z + translations_absolute->t[2][1].z) / 2;
306
307 struct xrt_quat *palm_orientation = &orientations_absolute->q[2][0];
308
309 zldtt(&palm_position, palm_orientation, is_right,
310 &out_viz_hand->values.hand_joint_set_default[joint_acc_idx++].relation);
311
312 // Wrist.
313 zldtt(&opt->wrist_pose.pose.position, &final_wrist_orientation, is_right,
314 &out_viz_hand->values.hand_joint_set_default[joint_acc_idx++].relation);
315
316 for (int finger = 0; finger < 5; finger++) {
317 for (int joint = 0; joint < 5; joint++) {
318 // This one is necessary
319 if (finger == 0 && joint == 0) {
320 continue;
321 }
322 struct xrt_quat *orientation;
323 if (joint != 4) {
324 orientation = &orientations_absolute->q[finger][joint];
325 } else {
326 orientation = &orientations_absolute->q[finger][joint - 1];
327 }
328 zldtt(&translations_absolute->t[finger][joint], orientation, is_right,
329 &out_viz_hand->values.hand_joint_set_default[joint_acc_idx++].relation);
330 }
331 }
332 out_viz_hand->is_active = true;
333}
334
335static void
336hand_sim_hand_init(struct u_hand_sim_hand *out_opt, enum xrt_hand xhand, const struct xrt_space_relation *root_pose)
337{
338 out_opt->hand_size = 0.095f;
339
340 out_opt->is_right = xhand == XRT_HAND_RIGHT;
341 out_opt->hand_pose = *root_pose;
342
343 for (int i = 0; i < 4; i++) {
344 //!@todo needed?
345 out_opt->finger[i].metacarpal.swing.x = 0.f;
346 out_opt->finger[i].metacarpal.twist = 0.f;
347
348 out_opt->finger[i].proximal_swing.x = DEG_TO_RAD(15);
349 out_opt->finger[i].rotations[0] = DEG_TO_RAD(-5);
350 out_opt->finger[i].rotations[1] = DEG_TO_RAD(-5);
351 }
352
353 out_opt->thumb.metacarpal.swing.x = 0.f;
354 out_opt->thumb.metacarpal.swing.y = 0.f;
355 out_opt->thumb.metacarpal.twist = 0.f;
356
357 out_opt->thumb.rotations[0] = DEG_TO_RAD(10);
358 out_opt->thumb.rotations[1] = DEG_TO_RAD(10);
359
360 out_opt->finger[0].metacarpal.swing.y = -0.19f;
361 out_opt->finger[1].metacarpal.swing.y = 0.f;
362 out_opt->finger[2].metacarpal.swing.y = 0.19f;
363 out_opt->finger[3].metacarpal.swing.y = 0.38f;
364
365 out_opt->finger[0].proximal_swing.y = -0.01f;
366 out_opt->finger[1].proximal_swing.y = 0.f;
367 out_opt->finger[2].proximal_swing.y = 0.01f;
368 out_opt->finger[3].proximal_swing.y = 0.02f;
369}
370
371
372void
373u_hand_sim_simulate(struct u_hand_sim_hand *hand_ptr, struct xrt_hand_joint_set *out_set)
374{
375 struct translations55 translations;
376 struct orientations54 orientations;
377
378 eval_hand_with_orientation(hand_ptr, hand_ptr->is_right, &translations, &orientations);
379
380 our_eval_to_viz_hand(hand_ptr, &translations, &orientations, hand_ptr->is_right, out_set);
381
382 u_hand_joints_apply_joint_width(out_set);
383
384 out_set->hand_pose = hand_ptr->hand_pose;
385
386 out_set->hand_pose.relation_flags = (enum xrt_space_relation_flags)(
387 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
388 XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
389
390 out_set->is_active = true;
391}
392
393void
394u_hand_sim_simulate_for_valve_index_knuckles(const struct u_hand_tracking_curl_values *values,
395 enum xrt_hand xhand,
396 const struct xrt_space_relation *root_pose,
397 struct xrt_hand_joint_set *out_set)
398{
399 struct u_hand_sim_hand hand;
400
401 hand_sim_hand_init(&hand, xhand, root_pose);
402 hand.wrist_pose.pose.position.x = 0.f;
403 hand.wrist_pose.pose.position.y = 0.f;
404 hand.wrist_pose.pose.position.z = 0.f;
405
406 hand.hand_size = 0.095;
407
408 // Thumb
409 hand.thumb.metacarpal.swing.x += values->thumb * 0.08f;
410 hand.thumb.metacarpal.swing.y += -0.35f;
411 hand.thumb.metacarpal.twist = 0;
412 hand.thumb.rotations[0] += values->thumb * -1.57f;
413 hand.thumb.rotations[1] += values->thumb * -1.4f;
414
415 // Index finger - this is treated differently on Valve Knuckles controllers so the pinch gesture feels good
416 float finger_values[4] = {values->index, values->middle, values->ring, values->little};
417
418 {
419 int finger = 0;
420 float val_turn = finger_values[finger] * -1.1f;
421 hand.finger[finger].proximal_swing.x = val_turn * 1.3f;
422 hand.finger[finger].rotations[0] = val_turn;
423 hand.finger[finger].rotations[1] = val_turn;
424 }
425
426 for (int finger = 1; finger < 4; finger++) {
427 float val_turn = finger_values[finger] * -1.1f * 1.3f;
428 hand.finger[finger].proximal_swing.x = val_turn * 1.3f;
429 hand.finger[finger].rotations[0] = val_turn * 1.0f;
430 hand.finger[finger].rotations[1] = val_turn * 0.4f;
431 }
432
433 u_hand_sim_simulate(&hand, out_set);
434}
435
436static void
437u_hand_sim_apply_generic_finger_transform(const struct u_hand_tracking_finger_value *finger_value,
438 struct u_hand_sim_finger *out_finger)
439{
440 out_finger->metacarpal.swing.x = finger_value->joint_curls[0] * -1.f;
441
442 out_finger->proximal_swing.x = finger_value->joint_curls[1] * -1.f;
443 out_finger->proximal_swing.y = finger_value->splay;
444
445 out_finger->rotations[0] = finger_value->joint_curls[2] * -1.f;
446 out_finger->rotations[1] = finger_value->joint_curls[3] * -1.f;
447}
448
449void
450u_hand_sim_simulate_generic(const struct u_hand_tracking_values *values,
451 enum xrt_hand xhand,
452 const struct xrt_space_relation *root_pose,
453 struct xrt_hand_joint_set *out_set)
454{
455 struct u_hand_sim_hand hand;
456
457 hand_sim_hand_init(&hand, xhand, root_pose);
458 hand.wrist_pose.pose.position.x = 0.f;
459 hand.wrist_pose.pose.position.y = 0.f;
460 hand.wrist_pose.pose.position.z = 0.f;
461
462 hand.hand_size = 0.095;
463
464 // Thumb
465 hand.thumb.metacarpal.swing.x += values->thumb.joint_curls[0] * 0.08f; // curl
466
467 hand.thumb.metacarpal.swing.y += values->thumb.splay; // splay
468 hand.thumb.metacarpal.twist = 0;
469 hand.thumb.rotations[0] += values->thumb.joint_curls[1] * -1.f;
470 hand.thumb.rotations[1] += values->thumb.joint_curls[2] * -1.f;
471
472 u_hand_sim_apply_generic_finger_transform(&values->little, &hand.finger[3]);
473 u_hand_sim_apply_generic_finger_transform(&values->ring, &hand.finger[2]);
474 u_hand_sim_apply_generic_finger_transform(&values->middle, &hand.finger[1]);
475 u_hand_sim_apply_generic_finger_transform(&values->index, &hand.finger[0]);
476
477 u_hand_sim_simulate(&hand, out_set);
478}