matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2022, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Functions for manipulating a @ref xrt_relation_chain struct.
6 * @author Jakob Bornecrantz <jakob@collabora.com>
7 * @ingroup aux_math
8 */
9
10#include "util/u_misc.h"
11
12#include "math/m_api.h"
13#include "math/m_vec2.h"
14#include "math/m_vec3.h"
15#include "math/m_space.h"
16
17#include <stdio.h>
18#include <assert.h>
19
20
21/*
22 *
23 * Dump functions.
24 *
25 */
26
27static void
28dump_relation(const struct xrt_space_relation *r)
29{
30 fprintf(stderr, "%04x", r->relation_flags);
31
32 if (r->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
33 fprintf(stderr, " P{%f %f %f}", r->pose.position.x, r->pose.position.y, r->pose.position.z);
34 }
35
36 if (r->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
37 fprintf(stderr, " O{%f %f %f %f}", r->pose.orientation.x, r->pose.orientation.y, r->pose.orientation.z,
38 r->pose.orientation.w);
39 }
40
41 if (r->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) {
42 fprintf(stderr, " LV{%f %f %f}", r->linear_velocity.x, r->linear_velocity.y, r->linear_velocity.z);
43 }
44
45 if (r->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) {
46 fprintf(stderr, " AV{%f %f %f}", r->angular_velocity.x, r->angular_velocity.y, r->angular_velocity.z);
47 }
48
49 fprintf(stderr, "\n");
50}
51
52static void
53dump_chain(const struct xrt_relation_chain *xrc)
54{
55 fprintf(stderr, "%s %u\n", __func__, xrc->step_count);
56 for (uint32_t i = 0; i < xrc->step_count; i++) {
57 const struct xrt_space_relation *r = &xrc->steps[i];
58 fprintf(stderr, "\t%2u: ", i);
59 dump_relation(r);
60 }
61}
62
63
64/*
65 *
66 * Helper functions.
67 *
68 */
69
70static bool
71has_step_with_no_pose(const struct xrt_relation_chain *xrc)
72{
73 const enum xrt_space_relation_flags pose_flags = (enum xrt_space_relation_flags)(
74 XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
75
76 for (uint32_t i = 0; i < xrc->step_count; i++) {
77 const struct xrt_space_relation *r = &xrc->steps[i];
78 if ((r->relation_flags & pose_flags) == 0) {
79 return true;
80 }
81 }
82
83 return false;
84}
85
86struct flags
87{
88 unsigned int has_orientation : 1;
89 unsigned int has_position : 1;
90 unsigned int has_linear_velocity : 1;
91 unsigned int has_angular_velocity : 1;
92 unsigned int has_tracked_orientation : 1;
93 unsigned int has_tracked_position : 1;
94};
95
96flags
97get_flags(const struct xrt_space_relation *r)
98{
99 // clang-format off
100 flags flags = {};
101 flags.has_orientation = (r->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0;
102 flags.has_position = (r->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0;
103 flags.has_linear_velocity = (r->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0;
104 flags.has_angular_velocity = (r->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0;
105 flags.has_tracked_orientation = (r->relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) != 0;
106 flags.has_tracked_position = (r->relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0;
107 // clang-format on
108
109 return flags;
110}
111
112static void
113make_valid_pose(flags flags, const struct xrt_pose *in_pose, struct xrt_pose *out_pose)
114{
115 if (flags.has_orientation) {
116 out_pose->orientation = in_pose->orientation;
117 } else {
118 out_pose->orientation = XRT_QUAT_IDENTITY;
119 }
120
121 if (flags.has_position) {
122 out_pose->position = in_pose->position;
123 } else {
124 out_pose->position = XRT_VEC3_ZERO;
125 }
126}
127
128static void
129apply_relation(const struct xrt_space_relation *a,
130 const struct xrt_space_relation *b,
131 struct xrt_space_relation *out_relation)
132{
133 flags af = get_flags(a);
134 flags bf = get_flags(b);
135
136 struct xrt_pose pose = XRT_POSE_IDENTITY;
137 struct xrt_vec3 linear_velocity = XRT_VEC3_ZERO;
138 struct xrt_vec3 angular_velocity = XRT_VEC3_ZERO;
139
140
141 /*
142 * Pose.
143 */
144
145 struct xrt_pose body_pose = XRT_POSE_IDENTITY; // aka valid_a_pose
146 struct xrt_pose base_pose = XRT_POSE_IDENTITY; // aka valid_b_pose
147
148 // If either orientation or position component is not valid, make that component identity so that transforms
149 // work. The flags of the result are determined in nf and not taken from the result of the transform.
150 make_valid_pose(af, &a->pose, &body_pose);
151 make_valid_pose(bf, &b->pose, &base_pose);
152
153
154 // This is a band aid to make 3dof devices work until we have a real solution.
155 // A 3dof device may return a relation with only orientation valid/tracked and no position.
156 //
157 // Monado wants to apply a predefined offset to 3dof devices, giving them a position.
158 //
159 // But per the comment below "If either of the relations does not have a valid or tracked flag, the entire chain
160 // loses that flag".
161 //
162 // For now we upgrade every relation that only has an orientation, to also have a position. Note that
163 // make_valid_pose zeroed the position if has_position was not set originally, ensuring there are no garbage
164 // values propagated.
165 if (af.has_orientation && !af.has_position) {
166 af.has_position = true;
167 }
168 if (bf.has_orientation && !bf.has_position) {
169 bf.has_position = true;
170 }
171
172
173 // If either of the relations does not have a valid or tracked flag, the entire chain loses that flag.
174 flags nf = {};
175 nf.has_orientation = af.has_orientation && bf.has_orientation;
176 nf.has_position = af.has_position && bf.has_position;
177 nf.has_tracked_orientation = af.has_tracked_orientation && bf.has_tracked_orientation;
178 nf.has_tracked_position = af.has_tracked_position && bf.has_tracked_position;
179 nf.has_linear_velocity = af.has_linear_velocity && bf.has_linear_velocity;
180 nf.has_angular_velocity = af.has_angular_velocity && bf.has_angular_velocity;
181
182
183 // Not already valid poses needed to be made valid because the transoformed pose would be undefined otherwise
184 // and we still want e.g. valid positions.
185 math_pose_transform(&base_pose, &body_pose, &pose);
186
187
188 /*
189 * Linear velocity.
190 */
191
192 // We only need to bother with velocities if we know that we will pass them on.
193 if (nf.has_linear_velocity) {
194 struct xrt_vec3 tmp = XRT_VEC3_ZERO;
195
196 math_quat_rotate_vec3(&base_pose.orientation, // Base rotation
197 &a->linear_velocity, // In base space
198 &tmp); // Output
199
200 linear_velocity += tmp;
201 linear_velocity += b->linear_velocity;
202 }
203
204
205 /*
206 * Angular velocity.
207 */
208
209 if (nf.has_angular_velocity) {
210 struct xrt_vec3 tmp = XRT_VEC3_ZERO;
211
212 math_quat_rotate_derivative(&base_pose.orientation, // Base rotation
213 &a->angular_velocity, // In base space
214 &tmp); // Output
215
216 angular_velocity += tmp;
217 angular_velocity += b->angular_velocity;
218
219 // handle tangential velocity AKA "lever arm" effect on velocity:
220 // an angular velocity at the origin produces a linear velocity everywhere else
221 struct xrt_vec3 rotated_position = XRT_VEC3_ZERO;
222 struct xrt_vec3 position = XRT_VEC3_ZERO;
223 struct xrt_quat orientation = XRT_QUAT_IDENTITY;
224 struct xrt_vec3 tangental_velocity = XRT_VEC3_ZERO;
225
226 position = body_pose.position; // In the base space
227 orientation = base_pose.orientation; // Base space
228
229 math_quat_rotate_vec3(&orientation, // Rotation
230 &position, // Vector
231 &rotated_position); // Result
232
233 math_vec3_cross(&b->angular_velocity, // A
234 &rotated_position, // B
235 &tangental_velocity); // Result
236
237 linear_velocity += tangental_velocity;
238 }
239
240
241 /*
242 * Flags.
243 */
244
245 int new_flags = 0;
246
247 if (nf.has_orientation) {
248 new_flags |= XRT_SPACE_RELATION_ORIENTATION_VALID_BIT;
249 }
250 if (nf.has_position) {
251 new_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT;
252 }
253 if (nf.has_tracked_position) {
254 new_flags |= XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
255 }
256 if (nf.has_tracked_orientation) {
257 new_flags |= XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
258 }
259 if (nf.has_linear_velocity) {
260 new_flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
261 }
262 if (nf.has_angular_velocity) {
263 new_flags |= XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT;
264 }
265
266
267 /*
268 * Write everything out.
269 */
270
271 struct xrt_space_relation tmp = {};
272 tmp.relation_flags = (enum xrt_space_relation_flags)new_flags;
273 tmp.pose = pose;
274 tmp.linear_velocity = linear_velocity;
275 tmp.angular_velocity = angular_velocity;
276
277 *out_relation = tmp;
278}
279
280
281/*
282 *
283 * Exported functions.
284 *
285 */
286
287extern "C" void
288m_relation_chain_resolve(const struct xrt_relation_chain *xrc, struct xrt_space_relation *out_relation)
289{
290 if (xrc->step_count == 0 || has_step_with_no_pose(xrc)) {
291 *out_relation = XRT_SPACE_RELATION_ZERO;
292 return;
293 }
294
295 struct xrt_space_relation r = xrc->steps[0];
296 for (uint32_t i = 1; i < xrc->step_count; i++) {
297 apply_relation(&r, &xrc->steps[i], &r);
298 }
299
300#if 0
301 dump_chain(xrc);
302 fprintf(stderr, "\tRR: ");
303 dump_relation(&r);
304#else
305 (void)&dump_chain;
306#endif
307
308 // Ensure no errors have crept in.
309 math_quat_normalize(&r.pose.orientation);
310
311 *out_relation = r;
312}
313
314extern "C" void
315m_space_relation_invert(struct xrt_space_relation *relation, struct xrt_space_relation *out_relation)
316{
317 assert(relation != NULL);
318 assert(out_relation != NULL);
319
320 out_relation->relation_flags = relation->relation_flags;
321 math_pose_invert(&relation->pose, &out_relation->pose);
322 out_relation->linear_velocity = m_vec3_mul_scalar(relation->linear_velocity, -1);
323 out_relation->angular_velocity = m_vec3_mul_scalar(relation->angular_velocity, -1);
324}
325
326extern "C" void
327m_space_relation_interpolate(struct xrt_space_relation *a,
328 struct xrt_space_relation *b,
329 float t,
330 enum xrt_space_relation_flags flags,
331 struct xrt_space_relation *out_relation)
332{
333 out_relation->relation_flags = flags;
334
335 if (flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
336 math_quat_slerp(&a->pose.orientation, &b->pose.orientation, t, &out_relation->pose.orientation);
337 }
338 if (flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
339 out_relation->pose.position = m_vec3_lerp(a->pose.position, b->pose.position, t);
340 }
341 if (flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) {
342 out_relation->linear_velocity = m_vec3_lerp(a->linear_velocity, b->linear_velocity, t);
343 }
344 if (flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) {
345 out_relation->angular_velocity = m_vec3_lerp(a->angular_velocity, b->angular_velocity, t);
346 }
347}