The open source OpenXR runtime
a/math: Add faster (and correct!) math_quat_from_swing_twist
+62
-9
+62
-9
src/xrt/auxiliary/math/m_base.cpp
+62
-9
src/xrt/auxiliary/math/m_base.cpp
···
409
409
}
410
410
}
411
411
412
+
// See https://gitlab.freedesktop.org/slitcch/rotation_visualizer/-/blob/main/lm_rotations_story.inl for the derivation
412
413
extern "C" void
413
414
math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, struct xrt_quat *result)
414
415
{
415
416
assert(swing != NULL);
416
417
assert(result != NULL);
417
418
418
-
struct xrt_quat swing_quat;
419
-
struct xrt_quat twist_quat;
419
+
float swing_x = swing->x;
420
+
float swing_y = swing->y;
421
+
422
+
float theta_squared_swing = swing_x * swing_x + swing_y * swing_y;
423
+
424
+
if (theta_squared_swing > float(0.0)) {
425
+
// theta_squared_swing is nonzero, so we the regular derived conversion.
426
+
427
+
float theta = sqrt(theta_squared_swing);
428
+
429
+
float half_theta = theta * float(0.5);
430
+
431
+
// the "other" theta
432
+
float half_twist = twist * float(0.5);
433
+
434
+
float cos_half_theta = cos(half_theta);
435
+
float cos_half_twist = cos(half_twist);
436
+
437
+
float sin_half_theta = sin(half_theta);
438
+
float sin_half_twist = sin(half_twist);
439
+
440
+
float sin_half_theta_over_theta = sin_half_theta / theta;
441
+
442
+
result->w = cos_half_theta * cos_half_twist;
443
+
444
+
float x_part_1 = (swing_x * cos_half_twist * sin_half_theta_over_theta);
445
+
float x_part_2 = (swing_y * sin_half_twist * sin_half_theta_over_theta);
446
+
447
+
result->x = x_part_1 + x_part_2;
448
+
449
+
float y_part_1 = (swing_y * cos_half_twist * sin_half_theta_over_theta);
450
+
float y_part_2 = (swing_x * sin_half_twist * sin_half_theta_over_theta);
451
+
452
+
result->y = y_part_1 - y_part_2;
453
+
454
+
result->z = cos_half_theta * sin_half_twist;
455
+
456
+
} else {
457
+
// sin_half_theta/theta would be undefined, but
458
+
// the limit approaches 0.5, so we do this.
459
+
// Note the differences w/ lm_rotations.inl - we can skip some things as we're not using this to compute
460
+
// a jacobian.
461
+
462
+
float half_twist = twist * float(0.5);
463
+
464
+
float cos_half_twist = cos(half_twist);
465
+
466
+
float sin_half_twist = sin(half_twist);
467
+
468
+
float sin_half_theta_over_theta = float(0.5);
469
+
470
+
// cos(0) is 1 so no cos_half_theta necessary
471
+
result->w = cos_half_twist;
420
472
421
-
struct xrt_vec3 aax_twist;
473
+
float x_part_1 = (swing_x * cos_half_twist * sin_half_theta_over_theta);
474
+
float x_part_2 = (swing_y * sin_half_twist * sin_half_theta_over_theta);
422
475
423
-
aax_twist.x = 0.f;
424
-
aax_twist.y = 0.f;
425
-
aax_twist.z = twist;
476
+
result->x = x_part_1 + x_part_2;
426
477
427
-
math_quat_from_swing(swing, &swing_quat);
478
+
float y_part_1 = (swing_y * cos_half_twist * sin_half_theta_over_theta);
479
+
float y_part_2 = (swing_x * sin_half_twist * sin_half_theta_over_theta);
428
480
429
-
math_quat_exp(&aax_twist, &twist_quat);
481
+
result->y = y_part_1 - y_part_2;
430
482
431
-
math_quat_rotate(&swing_quat, &twist_quat, result);
483
+
result->z = sin_half_twist;
484
+
}
432
485
}
433
486
434
487
/*