matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2021, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Simple function to predict a new pose from a given pose.
6 * @author Jakob Bornecrantz <jakob@collabora.com>
7 * @ingroup aux_math
8 */
9
10#include "m_api.h"
11#include "m_vec3.h"
12#include "m_predict.h"
13#include "util/u_trace_marker.h"
14
15
16static void
17do_orientation(const struct xrt_space_relation *rel,
18 enum xrt_space_relation_flags flags,
19 double delta_s,
20 struct xrt_space_relation *out_rel)
21{
22 if (delta_s == 0) {
23 out_rel->pose.orientation = rel->pose.orientation;
24 out_rel->angular_velocity = rel->angular_velocity;
25 return;
26 }
27
28 struct xrt_vec3 accum = {0};
29 bool valid_orientation = (flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0;
30 bool valid_angular_velocity = (flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0;
31
32 if (valid_angular_velocity) {
33 // angular velocity needs to be in body space for prediction
34 struct xrt_vec3 ang_vel_body_space;
35
36 struct xrt_quat orientation_inv;
37 math_quat_invert(&rel->pose.orientation, &orientation_inv);
38
39 math_quat_rotate_derivative(&orientation_inv, &rel->angular_velocity, &ang_vel_body_space);
40
41 accum.x += ang_vel_body_space.x;
42 accum.y += ang_vel_body_space.y;
43 accum.z += ang_vel_body_space.z;
44 }
45
46 // We don't want the angular acceleration, it's way too noisy.
47#if 0
48 if (valid_angular_acceleration) {
49 accum.x += delta_s / 2 * rel->angular_acceleration.x;
50 accum.y += delta_s / 2 * rel->angular_acceleration.y;
51 accum.z += delta_s / 2 * rel->angular_acceleration.z;
52 }
53#endif
54
55 if (valid_orientation) {
56 math_quat_integrate_velocity(&rel->pose.orientation, // Old orientation
57 &accum, // Angular velocity
58 (float)delta_s, // Delta in seconds
59 &out_rel->pose.orientation); // Result
60 }
61
62 // We use everything we integrated in as the new angular_velocity.
63 if (valid_angular_velocity) {
64 // angular velocity is returned in base space.
65 // use the predicted orientation for this calculation.
66 struct xrt_vec3 predicted_ang_vel_base_space;
67 math_quat_rotate_derivative(&out_rel->pose.orientation, &accum, &predicted_ang_vel_base_space);
68
69 out_rel->angular_velocity = predicted_ang_vel_base_space;
70 }
71}
72
73static void
74do_position(const struct xrt_space_relation *rel,
75 enum xrt_space_relation_flags flags,
76 double delta_s,
77 struct xrt_space_relation *out_rel)
78{
79 if (delta_s == 0) {
80 out_rel->pose.position = rel->pose.position;
81 out_rel->linear_velocity = rel->linear_velocity;
82 return;
83 }
84
85 struct xrt_vec3 accum = {0};
86 bool valid_position = (flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0;
87 bool valid_linear_velocity = (flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0;
88
89 if (valid_linear_velocity) {
90 accum.x += rel->linear_velocity.x;
91 accum.y += rel->linear_velocity.y;
92 accum.z += rel->linear_velocity.z;
93 }
94
95 if (valid_position) {
96 out_rel->pose.position = m_vec3_add(rel->pose.position, m_vec3_mul_scalar(accum, (float)delta_s));
97 }
98
99 // We use the new linear velocity with the acceleration integrated.
100 if (valid_linear_velocity) {
101 out_rel->linear_velocity = accum;
102 }
103}
104
105void
106m_predict_relation(const struct xrt_space_relation *rel, double delta_s, struct xrt_space_relation *out_rel)
107{
108 XRT_TRACE_MARKER();
109 enum xrt_space_relation_flags flags = rel->relation_flags;
110
111 do_orientation(rel, flags, delta_s, out_rel);
112 do_position(rel, flags, delta_s, out_rel);
113
114 out_rel->relation_flags = flags;
115}