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