matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Functions for manipulating @ref xrt_pose, @ref xrt_space_relation and
6 * @ref xrt_relation_chain structs.
7 * @author Jakob Bornecrantz <jakob@collabora.com>
8 * @ingroup aux_math
9 */
10
11#pragma once
12
13#include "xrt/xrt_defines.h"
14
15#include "math/m_api.h"
16
17
18#ifdef __cplusplus
19extern "C" {
20#endif
21
22
23/*!
24 * @addtogroup aux_math
25 * @{
26 */
27
28/*
29 *
30 * Pose functions.
31 *
32 */
33
34static inline bool
35m_pose_is_identity(const struct xrt_pose *pose)
36{
37 struct xrt_pose p = *pose;
38
39 return ((p.position.x == 0.0f || p.position.x == -0.0f) && // x
40 (p.position.y == 0.0f || p.position.y == -0.0f) && // y
41 (p.position.z == 0.0f || p.position.z == -0.0f) && // z
42 (p.orientation.x == 0.0f || p.orientation.x == -0.0f) && // x
43 (p.orientation.y == 0.0f || p.orientation.y == -0.0f) && // y
44 (p.orientation.z == 0.0f || p.orientation.z == -0.0f) && // z
45 (p.orientation.w == 1.0f || p.orientation.w == -1.0f) // w
46 );
47}
48
49
50/*
51 *
52 * Space relation functions.
53 *
54 */
55
56/*!
57 * Create an xrt_space_relation from a pose. If @p set_tracked is false, only orientation and position valid flags are
58 * set.
59 */
60static inline void
61m_space_relation_from_pose(const struct xrt_pose *pose, bool set_tracked, struct xrt_space_relation *out_relation)
62{
63 enum xrt_space_relation_flags flags =
64 set_tracked ? XRT_SPACE_RELATION_BITMASK_ALL
65 : (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
66 XRT_SPACE_RELATION_POSITION_VALID_BIT);
67
68 struct xrt_space_relation relation = {
69 flags,
70 *pose,
71 XRT_VEC3_ZERO,
72 XRT_VEC3_ZERO,
73 };
74
75 *out_relation = relation;
76}
77
78/*!
79 * Create an xrt_space_relation with only orientation and position valid flags.
80 */
81static inline void
82m_space_relation_ident(struct xrt_space_relation *out_relation)
83{
84 struct xrt_pose identity = XRT_POSE_IDENTITY;
85
86 m_space_relation_from_pose(&identity, false, out_relation);
87}
88
89void
90m_space_relation_invert(struct xrt_space_relation *relation, struct xrt_space_relation *out_relation);
91
92/*!
93 * Linearly interpolate between two relations @p a and @p b. Uses slerp for
94 * their orientations. Sets @p flags in @p out_relation.
95 */
96void
97m_space_relation_interpolate(struct xrt_space_relation *a,
98 struct xrt_space_relation *b,
99 float t,
100 enum xrt_space_relation_flags flags,
101 struct xrt_space_relation *out_relation);
102
103/*
104 *
105 * Relation chain functions.
106 *
107 */
108
109/*!
110 * Reserve a step in the chain and return a pointer to the relation.
111 *
112 * @note The data pointed to by the returned pointer is not initialized:
113 * you must populate it before using @ref m_relation_chain_resolve
114 *
115 * @public @memberof xrt_relation_chain
116 */
117static inline struct xrt_space_relation *
118m_relation_chain_reserve(struct xrt_relation_chain *xrc)
119{
120 if (xrc->step_count < XRT_RELATION_CHAIN_CAPACITY) {
121 return &xrc->steps[xrc->step_count++];
122 }
123 return NULL;
124}
125
126/*!
127 * Append a new relation
128 *
129 * @public @memberof xrt_relation_chain
130 */
131static inline void
132m_relation_chain_push_relation(struct xrt_relation_chain *xrc, const struct xrt_space_relation *relation)
133{
134 if (xrc->step_count >= XRT_RELATION_CHAIN_CAPACITY) {
135 return;
136 }
137
138 xrc->steps[xrc->step_count++] = *relation;
139}
140
141/*!
142 * Append the inverse of the provided relation.
143 *
144 * Validity flags stay the same, only the pose and velocities are inverted.
145 *
146 * @public @memberof xrt_relation_chain
147 */
148static inline void
149m_relation_chain_push_inverted_relation(struct xrt_relation_chain *xrc, const struct xrt_space_relation *relation)
150{
151 struct xrt_space_relation r = *relation;
152
153 struct xrt_space_relation invert;
154 m_space_relation_invert(&r, &invert);
155 m_relation_chain_push_relation(xrc, &invert);
156}
157
158/*!
159 * Append a new pose as a fully tracked relation.
160 * This follows OpenXR conventions where a space that is purely an offset to another space is fully tracked in that
161 * space.
162 * Conceptually, a pose is only considered not fully tracked when it is related to an xrt_space_relation that is not
163 * fully tracked.
164 *
165 * @public @memberof xrt_relation_chain
166 */
167static inline void
168m_relation_chain_push_pose(struct xrt_relation_chain *xrc, const struct xrt_pose *pose)
169{
170 struct xrt_space_relation relation;
171 m_space_relation_from_pose(pose, true, &relation);
172 m_relation_chain_push_relation(xrc, &relation);
173}
174
175/*!
176 * Append a new pose as a relation without velocity, if it is not the identity pose.
177 *
178 * @public @memberof xrt_relation_chain
179 */
180static inline void
181m_relation_chain_push_pose_if_not_identity(struct xrt_relation_chain *xrc, const struct xrt_pose *pose)
182{
183 struct xrt_pose p = *pose;
184
185 if (m_pose_is_identity(&p)) {
186 return;
187 }
188
189 m_relation_chain_push_pose(xrc, &p);
190}
191
192/*!
193 * Append the inverse of a pose as a relation without velocity, if it is not the identity pose.
194 *
195 * Validity flags stay the same, only the pose is inverted.
196 *
197 * @public @memberof xrt_relation_chain
198 */
199static inline void
200m_relation_chain_push_inverted_pose_if_not_identity(struct xrt_relation_chain *xrc, const struct xrt_pose *pose)
201{
202 struct xrt_pose p = *pose;
203
204 if (m_pose_is_identity(&p)) {
205 return;
206 }
207
208 struct xrt_pose invert;
209 math_pose_invert(&p, &invert);
210 m_relation_chain_push_pose(xrc, &invert);
211}
212
213/*!
214 * Compute the equivalent single relation from flattening a relation chain.
215 *
216 * The input chain is not modified.
217 *
218 * @public @memberof xrt_relation_chain
219 */
220void
221m_relation_chain_resolve(const struct xrt_relation_chain *xrc, struct xrt_space_relation *out_relation);
222
223/*!
224 * @}
225 */
226
227
228#ifdef __cplusplus
229}
230#endif