The open source OpenXR runtime
h/mercury: Use post-rotation residual that'll handle big rotations
Fixes a long-standing bug
+52
-11
+31
-7
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
+31
-7
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
···
289
289
return Vec3(0.f, 0.f, 0.f);
290
290
}
291
291
292
+
Scalar
293
+
norm_sqrd() const
294
+
{
295
+
Scalar len_sqrd = (Scalar)(0);
296
+
297
+
len_sqrd += this->x * this->x;
298
+
len_sqrd += this->y * this->y;
299
+
len_sqrd += this->z * this->z;
300
+
return len_sqrd;
301
+
}
302
+
292
303
// Norm, vector length, whatever.
304
+
// WARNING: Can return NaNs in the derivative part of Jets if magnitude is 0, because d/dx(sqrt(x)) at x=0 is
305
+
// undefined.
306
+
// There's no norm_safe because generally you need to add zero-checks somewhere *before* calling
307
+
// this, and it's not possible to produce correct derivatives from here.
293
308
Scalar
294
-
norm()
309
+
norm() const
295
310
{
296
-
Scalar len = (Scalar)(0);
311
+
Scalar len_sqrd = this->norm_sqrd();
297
312
298
-
len += this->x * this->x;
299
-
len += this->y * this->y;
300
-
len += this->z * this->z;
313
+
return sqrt(len_sqrd);
314
+
}
315
+
316
+
// WARNING: Will return NaNs if vector magnitude is zero due to zero division.
317
+
// Do not call this on vectors with zero norm.
318
+
Vec3
319
+
normalized() const
320
+
{
321
+
Scalar norm = this->norm();
301
322
302
-
len = sqrt(len);
303
-
return len;
323
+
Vec3<Scalar> retval;
324
+
retval.x = this->x / norm;
325
+
retval.y = this->y / norm;
326
+
retval.z = this->z / norm;
327
+
return retval;
304
328
}
305
329
};
306
330
+21
-4
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
+21
-4
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
···
328
328
helper.AddValue((hand.wrist_post_location.z) * stab.stabilityRootPosition);
329
329
330
330
331
-
helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(stab.stabilityHandOrientationXY));
332
-
helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(stab.stabilityHandOrientationXY));
333
-
helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(stab.stabilityHandOrientationZ));
331
+
// Needed because d/dx(sqrt(x)) at x=0 is undefined, and the first iteration *always* starts at 0.
332
+
// x-2sin(0.5x) at x=0.001 is 4.16e-11 - this is a reasonable epsilon to pick.
333
+
const float epsilon = 0.001;
334
+
if (hand.wrist_post_orientation_aax.x < epsilon && //
335
+
hand.wrist_post_orientation_aax.y < epsilon && //
336
+
hand.wrist_post_orientation_aax.z < epsilon) {
337
+
helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(stab.stabilityHandOrientationXY));
338
+
helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(stab.stabilityHandOrientationXY));
339
+
helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(stab.stabilityHandOrientationZ));
340
+
} else {
341
+
T rotation_magnitude = hand.wrist_post_orientation_aax.norm();
342
+
T magnitude_sin = T(2) * sin(T(0.5) * rotation_magnitude);
343
+
344
+
Vec3<T> rotation_axis = hand.wrist_post_orientation_aax.normalized();
345
+
346
+
347
+
helper.AddValue((magnitude_sin * rotation_axis.x) * (T)(stab.stabilityHandOrientationXY));
348
+
helper.AddValue((magnitude_sin * rotation_axis.y) * (T)(stab.stabilityHandOrientationXY));
349
+
helper.AddValue((magnitude_sin * rotation_axis.z) * (T)(stab.stabilityHandOrientationZ));
350
+
}
334
351
335
352
336
353
···
1065
1082
state.this_frame_pre_position = state.last_frame.wrist_final_location;
1066
1083
// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're
1067
1084
// using sin/asin.
1068
-
1085
+
1069
1086
state.last_frame.wrist_post_location.x = 0.0f;
1070
1087
state.last_frame.wrist_post_location.y = 0.0f;
1071
1088
state.last_frame.wrist_post_location.z = 0.0f;