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 bool valid_angular_acceleration = (flags & XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT) != 0;
32
33 if (valid_angular_velocity) {
34 // angular velocity needs to be in body space for prediction
35 struct xrt_vec3 ang_vel_body_space;
36
37 struct xrt_quat orientation_inv;
38 math_quat_invert(&rel->pose.orientation, &orientation_inv);
39
40 math_quat_rotate_derivative(&orientation_inv, &rel->angular_velocity, &ang_vel_body_space);
41
42 accum.x += ang_vel_body_space.x;
43 accum.y += ang_vel_body_space.y;
44 accum.z += ang_vel_body_space.z;
45 }
46
47 if (valid_angular_acceleration) {
48 accum.x += delta_s / 2 * rel->angular_acceleration.x;
49 accum.y += delta_s / 2 * rel->angular_acceleration.y;
50 accum.z += delta_s / 2 * rel->angular_acceleration.z;
51 }
52
53 if (valid_orientation) {
54 math_quat_integrate_velocity(&rel->pose.orientation, // Old orientation
55 &accum, // Angular velocity
56 (float)delta_s, // Delta in seconds
57 &out_rel->pose.orientation); // Result
58 }
59
60 // We use everything we integrated in as the new angular_velocity.
61 if (valid_angular_velocity) {
62 // angular velocity is returned in base space.
63 // use the predicted orientation for this calculation.
64 struct xrt_vec3 predicted_ang_vel_base_space;
65 math_quat_rotate_derivative(&out_rel->pose.orientation, &accum, &predicted_ang_vel_base_space);
66
67 out_rel->angular_velocity = predicted_ang_vel_base_space;
68 }
69}
70
71static void
72do_position(const struct xrt_space_relation *rel,
73 enum xrt_space_relation_flags flags,
74 double delta_s,
75 struct xrt_space_relation *out_rel)
76{
77 if (delta_s == 0) {
78 out_rel->pose.position = rel->pose.position;
79 out_rel->linear_velocity = rel->linear_velocity;
80 return;
81 }
82
83 struct xrt_vec3 accum = {0};
84 bool valid_position = (flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0;
85 bool valid_linear_velocity = (flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0;
86
87 if (valid_linear_velocity) {
88 accum.x += rel->linear_velocity.x;
89 accum.y += rel->linear_velocity.y;
90 accum.z += rel->linear_velocity.z;
91 }
92
93 if (valid_position) {
94 out_rel->pose.position = m_vec3_add(rel->pose.position, m_vec3_mul_scalar(accum, (float)delta_s));
95 }
96
97 // We use the new linear velocity with the acceleration integrated.
98 if (valid_linear_velocity) {
99 out_rel->linear_velocity = accum;
100 }
101}
102
103void
104m_predict_relation(const struct xrt_space_relation *rel, double delta_s, struct xrt_space_relation *out_rel)
105{
106 XRT_TRACE_MARKER();
107 enum xrt_space_relation_flags flags = rel->relation_flags;
108
109 do_orientation(rel, flags, delta_s, out_rel);
110 do_position(rel, flags, delta_s, out_rel);
111
112 out_rel->relation_flags = flags;
113}