The open source OpenXR runtime
a/math: Add pose interpolate function and others
- math_matrix_4x4_isometry_from_pose
- math_pose_from_isometry
- math_pose_interpolate
- m_vec3_equal
+91
-10
+29
-1
src/xrt/auxiliary/math/m_api.h
+29
-1
src/xrt/auxiliary/math/m_api.h
···
207
207
math_quat_from_angle_vector(float angle_rads, const struct xrt_vec3 *vector, struct xrt_quat *result);
208
208
209
209
/*!
210
-
* Create a rotation from a 3x3 rotation matrix.
210
+
* Create a rotation from a 3x3 rotation (row major) matrix.
211
211
*
212
212
* @relates xrt_quat
213
213
* @see xrt_matrix_3x3
···
570
570
struct xrt_matrix_4x4 *result);
571
571
572
572
/*!
573
+
* Get a col-major isometry matrix —in SE(3)— from a pose.
574
+
*
575
+
* @relates xrt_matrix_4x4
576
+
* @ingroup aux_math
577
+
*/
578
+
void
579
+
math_matrix_4x4_isometry_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result);
580
+
581
+
/*!
573
582
* Compute quad layer model matrix from xrt_pose and xrt_vec2 size.
574
583
*
575
584
* @relates xrt_matrix_4x4
···
625
634
*/
626
635
void
627
636
math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose);
637
+
638
+
/*!
639
+
* Converts a (col-major) isometry into a pose.
640
+
*
641
+
* @relates xrt_pose
642
+
* @ingroup aux_math
643
+
*/
644
+
void
645
+
math_pose_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_pose *result);
646
+
647
+
/*!
648
+
* Interpolated pose between poses `a` and `b` by lerping position and slerping
649
+
* orientation by t.
650
+
*
651
+
* @relates xrt_pose
652
+
* @ingroup aux_math
653
+
*/
654
+
void
655
+
math_pose_interpolate(const struct xrt_pose *a, const struct xrt_pose *b, float t, struct xrt_pose *outPose);
628
656
629
657
/*!
630
658
* Apply a rigid-body transformation to a pose.
+23
src/xrt/auxiliary/math/m_base.cpp
+23
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 "math/m_vec3.h"
15
16
16
17
#include <Eigen/Core>
17
18
#include <Eigen/Geometry>
···
566
567
}
567
568
568
569
extern "C" void
570
+
math_matrix_4x4_isometry_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
571
+
{
572
+
Eigen::Isometry3f transform{Eigen::Translation3f{position(*pose)} * orientation(*pose)};
573
+
map_matrix_4x4(*result) = transform.matrix();
574
+
}
575
+
576
+
extern "C" void
569
577
math_matrix_4x4_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4 *result)
570
578
{
571
579
Eigen::Vector3f position = copy(&pose->position);
···
689
697
Eigen::Isometry3f inverse = transform.inverse();
690
698
position(*outPose) = inverse.translation();
691
699
orientation(*outPose) = inverse.rotation();
700
+
}
701
+
702
+
extern "C" void
703
+
math_pose_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_pose *result)
704
+
{
705
+
Eigen::Isometry3f isometry{map_matrix_4x4(*transform)};
706
+
position(*result) = isometry.translation();
707
+
orientation(*result) = isometry.rotation();
708
+
}
709
+
710
+
extern "C" void
711
+
math_pose_interpolate(const struct xrt_pose *a, const struct xrt_pose *b, float t, struct xrt_pose *outPose)
712
+
{
713
+
math_quat_slerp(&a->orientation, &b->orientation, t, &outPose->orientation);
714
+
outPose->position = m_vec3_lerp(a->position, b->position, t);
692
715
}
693
716
694
717
extern "C" void
+6
src/xrt/auxiliary/math/m_vec3.h
+6
src/xrt/auxiliary/math/m_vec3.h
···
136
136
return m_vec3_add(m_vec3_mul_scalar(from, 1.0f - amount), m_vec3_mul_scalar(to, amount));
137
137
}
138
138
139
+
static inline bool
140
+
m_vec3_equal_exact(struct xrt_vec3 l, struct xrt_vec3 r)
141
+
{
142
+
return l.x == r.x && l.y == r.y && l.z == r.z;
143
+
}
144
+
139
145
#ifdef __cplusplus
140
146
}
141
147
+33
-9
tests/tests_pose.cpp
+33
-9
tests/tests_pose.cpp
···
1
1
// Copyright 2022, Campbell Suter
2
+
// Copyright 2022, Collabora, Ltd.
2
3
// SPDX-License-Identifier: BSL-1.0
3
4
/*!
4
5
* @file
5
-
* @brief Maths function tests.
6
+
* @brief Test xrt_pose functions.
6
7
* @author Campbell Suter <znix@znix.xyz>
8
+
* @author Mateo de Mayo <mateo.demayo@collabora.com>
7
9
*/
8
-
9
-
#include <util/u_worker.hpp>
10
-
#include <math/m_space.h>
11
-
#include <math/m_vec3.h>
12
10
13
11
#include "catch/catch.hpp"
12
+
#include "math/m_api.h"
13
+
#include "math/m_vec3.h"
14
14
15
-
#include <thread>
16
-
#include <chrono>
17
-
18
-
TEST_CASE("CorrectPoseInverse")
15
+
TEST_CASE("Pose invert works")
19
16
{
20
17
// Test that inverting a pose works correctly
21
18
// Pick an arbitrary and non-trivial original pose
···
40
37
CHECK(m_vec3_len(out_b.position) < 0.001);
41
38
CHECK(1 - abs(out_b.orientation.w) < 0.001);
42
39
}
40
+
41
+
TEST_CASE("Pose interpolation works")
42
+
{
43
+
// A random pose
44
+
struct xrt_vec3 pos_a = {1, 2, 3};
45
+
struct xrt_quat ori_a = {1, 2, 3, 4};
46
+
math_quat_normalize(&ori_a);
47
+
struct xrt_pose a = {ori_a, pos_a};
48
+
49
+
// The inverse of that pose
50
+
struct xrt_vec3 pos_b = pos_a * -1;
51
+
struct xrt_quat ori_b = {};
52
+
math_quat_invert(&ori_a, &ori_b);
53
+
struct xrt_pose b = {ori_b, pos_b};
54
+
55
+
// The interpolation at 0.5 should be the identity
56
+
struct xrt_pose res = {};
57
+
math_pose_interpolate(&a, &b, 0.5, &res);
58
+
59
+
CHECK(res.position.x == Approx(0));
60
+
CHECK(res.position.y == Approx(0));
61
+
CHECK(res.position.z == Approx(0));
62
+
CHECK(res.orientation.x == Approx(0));
63
+
CHECK(res.orientation.x == Approx(0));
64
+
CHECK(res.orientation.y == Approx(0));
65
+
CHECK(res.orientation.w == Approx(1));
66
+
}