matrixfurry.com / monado
The open source OpenXR runtime
0
fork

Configure Feed

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

a/math: Add multiple isometry-related matrix functions

An isometry is a rigid body transform. In this context I'm using the term to
refer to 4x4 homogeneous matrices in SE(3). I.e., matrices comprised of a
3x3 rotation, a 3x1 translation, and a [0,0,0,1] last row.
This matrix represent both rigid body transforms.

Mateo de Mayo 05b8e320 8b2fa955

+186
+84
src/xrt/auxiliary/math/m_api.h
··· 164 164 math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r, struct xrt_vec3_f64 *result); 165 165 166 166 /*! 167 + * Get translation vector from isometry matrix (col-major). 168 + * 169 + * @relates xrt_vec3 170 + * @ingroup aux_math 171 + */ 172 + void 173 + math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_vec3 *result); 174 + 175 + /*! 167 176 * Normalize a vec3 in place. 168 177 * 169 178 * @relates xrt_vec3 ··· 418 427 math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat); 419 428 420 429 /*! 430 + * Print a 3x3 (col-major) matrix to stdout 431 + * 432 + * @see xrt_matrix_3x3 433 + * @ingroup aux_math 434 + */ 435 + void 436 + math_matrix_3x3_print(const struct xrt_matrix_3x3 *mat); 437 + 438 + /*! 421 439 * Transform a vec3 by a 3x3 matrix 422 440 * 423 441 * @see xrt_matrix_3x3 ··· 460 478 math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result); 461 479 462 480 /*! 481 + * Transpose Matrix3x3 482 + * 483 + * @relates xrt_matrix_3x3 484 + * @ingroup aux_math 485 + */ 486 + void 487 + math_matrix_3x3_transpose(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result); 488 + 489 + /*! 463 490 * Create a rotation from two vectors plus x and z, by 464 491 * creating a rotation matrix by crossing z and x to 465 492 * get the y axis. ··· 475 502 struct xrt_matrix_3x3_f64 *result); 476 503 477 504 /*! 505 + * Get the rotation matrix from an isomertry matrix (col-major). 506 + * 507 + * @relates xrt_matrix_4x4 508 + * @ingroup aux_math 509 + */ 510 + void 511 + math_matrix_3x3_rotation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_matrix_3x3 *result); 512 + 513 + /*! 478 514 * Initialize Matrix4x4 with identity. 479 515 * 480 516 * @relates xrt_matrix_4x4 ··· 484 520 math_matrix_4x4_identity(struct xrt_matrix_4x4 *result); 485 521 486 522 /*! 523 + * Prints a Matrix4x4 (col-major). 524 + * 525 + * @relates xrt_matrix_4x4 526 + * @ingroup aux_math 527 + */ 528 + void 529 + math_matrix_4x4_print(const struct xrt_matrix_4x4 *mat); 530 + 531 + /*! 487 532 * Multiply Matrix4x4. 488 533 * 489 534 * @relates xrt_matrix_4x4 ··· 495 540 struct xrt_matrix_4x4 *result); 496 541 497 542 /*! 543 + * Invert Matrix4x4. 544 + * 545 + * @relates xrt_matrix_4x4 546 + * @ingroup aux_math 547 + */ 548 + void 549 + math_matrix_4x4_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result); 550 + 551 + /*! 552 + * Invert a homogeneous isometry 4x4 (col-major) matrix in SE(3). 553 + * 554 + * @relates xrt_matrix_4x4 555 + * @ingroup aux_math 556 + */ 557 + void 558 + math_matrix_4x4_isometry_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result); 559 + 560 + /*! 561 + * Transpose Matrix4x4 562 + * 563 + * @relates xrt_matrix_4x4 564 + * @ingroup aux_math 565 + */ 566 + void 567 + math_matrix_4x4_transpose(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result); 568 + 569 + /*! 498 570 * Compute view matrix from xrt_pose. 499 571 * 500 572 * @relates xrt_matrix_4x4 ··· 502 574 */ 503 575 void 504 576 math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result); 577 + 578 + /*! 579 + * Get an isometry matrix —in SE(3)— from a rotation matrix —SO(3)— and a 580 + * translation vector. All col-major matrices. 581 + * 582 + * @relates xrt_matrix_4x4 583 + * @ingroup aux_math 584 + */ 585 + void 586 + math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation, 587 + const struct xrt_vec3 *translation, 588 + struct xrt_matrix_4x4 *result); 505 589 506 590 /*! 507 591 * Compute quad layer model matrix from xrt_pose and xrt_vec2 size.
+77
src/xrt/auxiliary/math/m_base.cpp
··· 12 12 13 13 #include "math/m_api.h" 14 14 #include "math/m_eigen_interop.hpp" 15 + #include "util/u_logging.h" 15 16 16 17 #include <Eigen/Core> 17 18 #include <Eigen/Geometry> ··· 419 420 } 420 421 421 422 extern "C" void 423 + math_matrix_3x3_print(const struct xrt_matrix_3x3 *mat) 424 + { 425 + const auto &m = mat->v; 426 + U_LOG_RAW("[\n"); 427 + U_LOG_RAW("\t%f, %f, %f,\n", m[0], m[3], m[6]); 428 + U_LOG_RAW("\t%f, %f, %f,\n", m[1], m[4], m[7]); 429 + U_LOG_RAW("\t%f, %f, %f \n", m[2], m[5], m[8]); 430 + U_LOG_RAW("]\n"); 431 + } 432 + 433 + extern "C" void 422 434 math_matrix_3x3_f64_transform_vec3_f64(const struct xrt_matrix_3x3_f64 *left, 423 435 const struct xrt_vec3_f64 *right, 424 436 struct xrt_vec3_f64 *result_out) ··· 448 460 result->v[6] = plus_x->z; 449 461 result->v[7] = plus_y.z; 450 462 result->v[8] = plus_z->z; 463 + } 464 + 465 + extern "C" void 466 + math_matrix_3x3_rotation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_matrix_3x3 *result) 467 + { 468 + Eigen::Isometry3f transform{map_matrix_4x4(*isometry)}; 469 + map_matrix_3x3(*result) = transform.linear(); 451 470 } 452 471 453 472 extern "C" void ··· 494 513 } 495 514 496 515 extern "C" void 516 + math_matrix_3x3_transpose(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result) 517 + { 518 + Eigen::Matrix3f m = copy(in); 519 + map_matrix_3x3(*result) = m.transpose(); 520 + } 521 + 522 + extern "C" void 497 523 math_matrix_4x4_identity(struct xrt_matrix_4x4 *result) 498 524 { 499 525 map_matrix_4x4(*result) = Eigen::Matrix4f::Identity(); 500 526 } 501 527 502 528 extern "C" void 529 + math_matrix_4x4_print(const struct xrt_matrix_4x4 *mat) 530 + { 531 + const auto &m = mat->v; 532 + U_LOG_RAW("[\n"); 533 + U_LOG_RAW("\t%f, %f, %f, %f,\n", m[0], m[4], m[8], m[12]); 534 + U_LOG_RAW("\t%f, %f, %f, %f,\n", m[1], m[5], m[9], m[13]); 535 + U_LOG_RAW("\t%f, %f, %f, %f,\n", m[2], m[6], m[10], m[14]); 536 + U_LOG_RAW("\t%f, %f, %f, %f \n", m[3], m[7], m[11], m[15]); 537 + U_LOG_RAW("]\n"); 538 + } 539 + 540 + extern "C" void 503 541 math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left, 504 542 const struct xrt_matrix_4x4 *right, 505 543 struct xrt_matrix_4x4 *result) ··· 508 546 } 509 547 510 548 extern "C" void 549 + math_matrix_4x4_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result) 550 + { 551 + Eigen::Matrix4f m = copy(in); 552 + map_matrix_4x4(*result) = m.inverse(); 553 + } 554 + 555 + extern "C" void 556 + math_matrix_4x4_transpose(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result) 557 + { 558 + Eigen::Matrix4f m = copy(in); 559 + map_matrix_4x4(*result) = m.transpose(); 560 + } 561 + 562 + extern "C" void 563 + math_matrix_4x4_isometry_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result) 564 + { 565 + Eigen::Isometry3f m{copy(in)}; 566 + map_matrix_4x4(*result) = m.inverse().matrix(); 567 + } 568 + 569 + extern "C" void 511 570 math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result) 512 571 { 513 572 Eigen::Vector3f position = copy(&pose->position); ··· 517 576 Eigen::Isometry3f transformation = translation * orientation; 518 577 519 578 map_matrix_4x4(*result) = transformation.inverse().matrix(); 579 + } 580 + 581 + extern "C" void 582 + math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation, 583 + const struct xrt_vec3 *translation, 584 + struct xrt_matrix_4x4 *result) 585 + { 586 + Eigen::Isometry3f transformation = Eigen::Isometry3f::Identity(); 587 + transformation.linear() = map_matrix_3x3(*rotation); 588 + transformation.translation() = map_vec3(*translation); 589 + map_matrix_4x4(*result) = transformation.matrix(); 520 590 } 521 591 522 592 extern "C" void ··· 580 650 math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r, struct xrt_vec3_f64 *result) 581 651 { 582 652 map_vec3_f64(*result) = map_vec3_f64(*l).cross(map_vec3_f64(*r)); 653 + } 654 + 655 + extern "C" void 656 + math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_vec3 *result) 657 + { 658 + Eigen::Isometry3f isometry{map_matrix_4x4(*transform)}; 659 + map_vec3(*result) = isometry.translation(); 583 660 } 584 661 585 662 extern "C" void
+25
src/xrt/auxiliary/math/m_eigen_interop.hpp
··· 99 99 } 100 100 101 101 /*! 102 + * @brief Wrap an internal 3x3 matrix struct in an Eigen type, const overload. 103 + * 104 + * Permits zero-overhead manipulation of `xrt_matrix_3x3&` by Eigen routines as 105 + * if it were a `Eigen::Matrix3f&`. 106 + */ 107 + static inline Eigen::Map<const Eigen::Matrix3f> 108 + map_matrix_3x3(const struct xrt_matrix_3x3 &m) 109 + { 110 + return Eigen::Map<const Eigen::Matrix3f>(m.v); 111 + } 112 + 113 + /*! 102 114 * @brief Wrap an internal 3x3 matrix struct in an Eigen type, non-const 103 115 * overload. 104 116 * ··· 122 134 map_matrix_3x3_f64(struct xrt_matrix_3x3_f64 &m) 123 135 { 124 136 return Eigen::Map<Eigen::Matrix3d>(m.v); 137 + } 138 + 139 + /*! 140 + * @brief Wrap an internal 4x4 matrix struct in an Eigen type, const 141 + * overload. 142 + * 143 + * Permits zero-overhead manipulation of `xrt_matrix_4x4&` by Eigen routines as 144 + * if it were a `Eigen::Matrix4f&`. 145 + */ 146 + static inline Eigen::Map<const Eigen::Matrix4f> 147 + map_matrix_4x4(const struct xrt_matrix_4x4 &m) 148 + { 149 + return Eigen::Map<const Eigen::Matrix4f>(m.v); 125 150 } 126 151 127 152 /*!