The open source OpenXR runtime
a/math: Fix matrix identities and use isometry
Isometry3f is a 4x4 matrix transform that performs only rotation and translation
(an SE(3) matrix). Its inverse can be computed trivially by Eigen compared to a
regular 4x4 transform.
+10
-21
+10
-21
src/xrt/auxiliary/math/m_base.cpp
+10
-21
src/xrt/auxiliary/math/m_base.cpp
···
409
409
extern "C" void
410
410
math_matrix_3x3_identity(struct xrt_matrix_3x3 *mat)
411
411
{
412
-
mat->v[0] = mat->v[4] = mat->v[8] = 1.0;
412
+
map_matrix_3x3(*mat) = Eigen::Matrix3f::Identity();
413
413
}
414
414
415
415
extern "C" void
416
416
math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat)
417
417
{
418
-
mat->v[0] = mat->v[4] = mat->v[8] = 1.0;
418
+
map_matrix_3x3_f64(*mat) = Eigen::Matrix3d::Identity();
419
419
}
420
420
421
421
extern "C" void
···
514
514
Eigen::Quaternionf orientation = copy(&pose->orientation);
515
515
516
516
Eigen::Translation3f translation(position);
517
-
Eigen::Affine3f transformation = translation * orientation;
517
+
Eigen::Isometry3f transformation = translation * orientation;
518
518
519
-
map_matrix_4x4(*result) = transformation.matrix().inverse();
519
+
map_matrix_4x4(*result) = transformation.inverse().matrix();
520
520
}
521
521
522
522
extern "C" void
···
609
609
Eigen::Quaterniond orientation = copyd(pose->orientation);
610
610
611
611
Eigen::Translation3d translation(position);
612
-
Eigen::Affine3d transformation = translation * orientation;
612
+
Eigen::Isometry3d transformation = translation * orientation;
613
613
614
-
map_matrix_4x4_f64(*result) = transformation.matrix().inverse();
614
+
map_matrix_4x4_f64(*result) = transformation.inverse().matrix();
615
615
}
616
616
617
617
···
632
632
extern "C" void
633
633
math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
634
634
{
635
-
assert(pose != NULL);
636
-
assert(outPose != NULL);
637
-
638
-
// Store results to temporary locals so we can do this "in-place"
639
-
// (pose == outPose) if desired. Pure copies here.
640
-
Eigen::Vector3f newPosition = position(*pose);
641
-
Eigen::Quaternionf newOrientation = orientation(*pose);
642
-
643
-
// Conjugate legal here since pose must be normalized/unit length.
644
-
newOrientation = newOrientation.conjugate();
645
-
// Use the newly inverted rotation, to rotate position.
646
-
newPosition = -(newOrientation * newPosition);
647
-
648
-
position(*outPose) = newPosition;
649
-
orientation(*outPose) = newOrientation;
635
+
Eigen::Isometry3f transform{orientation(*pose) * Eigen::Translation3f{position(*pose)}};
636
+
Eigen::Isometry3f inverse = transform.inverse();
637
+
position(*outPose) = inverse.translation();
638
+
orientation(*outPose) = inverse.rotation();
650
639
}
651
640
652
641
extern "C" void