The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

a/math: Add faster (and correct!) math_quat_from_swing_twist

+62 -9
+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 /*