matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Base implementations for math library.
6 * @author Jakob Bornecrantz <jakob@collabora.com>
7 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
8 * @author Moshi Turner <moshiturner@protonmail.com>
9 * @author Nis Madsen <nima_zero_one@protonmail.com>
10 * @ingroup aux_math
11 */
12// IWYU pragma: no_include "src/Core/DenseBase.h"
13// IWYU pragma: no_include "src/Core/MatrixBase.h"
14
15#include "math/m_api.h" // IWYU pragma: associated
16
17#include "math/m_eigen_interop.hpp"
18#include "math/m_vec3.h"
19
20#include <Eigen/Core>
21#include <Eigen/Geometry>
22
23#include <assert.h>
24
25using namespace xrt::auxiliary::math;
26
27/*
28 *
29 * Copy helpers.
30 *
31 */
32
33static inline Eigen::Quaternionf
34copy(const struct xrt_quat &q)
35{
36 // Eigen constructor order is different from XRT, OpenHMD and OpenXR!
37 // Eigen: `float w, x, y, z`.
38 // OpenXR: `float x, y, z, w`.
39 return Eigen::Quaternionf(q.w, q.x, q.y, q.z);
40}
41
42static inline Eigen::Quaternionf
43copy(const struct xrt_quat *q)
44{
45 return copy(*q);
46}
47
48XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
49copyd(const struct xrt_quat &q)
50{
51 // Eigen constructor order is different from XRT, OpenHMD and OpenXR!
52 // Eigen: `float w, x, y, z`.
53 // OpenXR: `float x, y, z, w`.
54 return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
55}
56
57XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
58copyd(const struct xrt_quat *q)
59{
60 return copyd(*q);
61}
62
63static inline Eigen::Vector3f
64copy(const struct xrt_vec3 &v)
65{
66 return Eigen::Vector3f(v.x, v.y, v.z);
67}
68
69static inline Eigen::Vector3f
70copy(const struct xrt_vec3 *v)
71{
72 return copy(*v);
73}
74
75
76static inline Eigen::Vector3d
77copy(const struct xrt_vec3_f64 &v)
78{
79 return Eigen::Vector3d(v.x, v.y, v.z);
80}
81
82static inline Eigen::Vector3d
83copy(const struct xrt_vec3_f64 *v)
84{
85 return copy(*v);
86}
87
88XRT_MAYBE_UNUSED static inline Eigen::Vector3d
89copyd(const struct xrt_vec3 &v)
90{
91 return Eigen::Vector3d(v.x, v.y, v.z);
92}
93
94XRT_MAYBE_UNUSED static inline Eigen::Vector3d
95copyd(const struct xrt_vec3 *v)
96{
97 return copyd(*v);
98}
99
100static inline Eigen::Matrix3f
101copy(const struct xrt_matrix_3x3 *m)
102{
103 Eigen::Matrix3f res;
104 // clang-format off
105 res << m->v[0], m->v[3], m->v[6],
106 m->v[1], m->v[4], m->v[7],
107 m->v[2], m->v[5], m->v[8];
108 // clang-format on
109 return res;
110}
111
112static inline Eigen::Matrix4f
113copy(const struct xrt_matrix_4x4 *m)
114{
115 Eigen::Matrix4f res;
116 // clang-format off
117 res << m->v[0], m->v[4], m->v[8], m->v[12],
118 m->v[1], m->v[5], m->v[9], m->v[13],
119 m->v[2], m->v[6], m->v[10], m->v[14],
120 m->v[3], m->v[7], m->v[11], m->v[15];
121 // clang-format on
122 return res;
123}
124
125
126/*
127 *
128 * Exported vector functions.
129 *
130 */
131
132extern "C" bool
133math_vec3_validate(const struct xrt_vec3 *vec3)
134{
135 assert(vec3 != NULL);
136
137 return map_vec3(*vec3).allFinite();
138}
139
140extern "C" void
141math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut)
142{
143 assert(additional != NULL);
144 assert(inAndOut != NULL);
145
146 map_vec3(*inAndOut) += map_vec3(*additional);
147}
148
149extern "C" void
150math_vec3_subtract(const struct xrt_vec3 *subtrahend, struct xrt_vec3 *inAndOut)
151{
152 assert(subtrahend != NULL);
153 assert(inAndOut != NULL);
154
155 map_vec3(*inAndOut) -= map_vec3(*subtrahend);
156}
157
158extern "C" void
159math_vec3_scalar_mul(float scalar, struct xrt_vec3 *inAndOut)
160{
161 assert(inAndOut != NULL);
162
163 map_vec3(*inAndOut) *= scalar;
164}
165
166extern "C" void
167math_vec3_cross(const struct xrt_vec3 *l, const struct xrt_vec3 *r, struct xrt_vec3 *result)
168{
169 map_vec3(*result) = map_vec3(*l).cross(map_vec3(*r));
170}
171
172extern "C" void
173math_vec3_normalize(struct xrt_vec3 *in)
174{
175 map_vec3(*in) = map_vec3(*in).normalized();
176}
177
178extern "C" void
179math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_vec3 *result)
180{
181 Eigen::Isometry3f isometry{map_matrix_4x4(*transform)};
182 map_vec3(*result) = isometry.translation();
183}
184
185
186/*
187 *
188 * Exported 64 bit vector functions.
189 *
190 */
191
192extern "C" void
193math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r, struct xrt_vec3_f64 *result)
194{
195 map_vec3_f64(*result) = map_vec3_f64(*l).cross(map_vec3_f64(*r));
196}
197
198extern "C" void
199math_vec3_f64_normalize(struct xrt_vec3_f64 *in)
200{
201 map_vec3_f64(*in) = map_vec3_f64(*in).normalized();
202}
203
204
205/*
206 *
207 * Exported quaternion functions.
208 *
209 */
210
211extern "C" void
212math_quat_from_angle_vector(float angle_rads, const struct xrt_vec3 *vector, struct xrt_quat *result)
213{
214 map_quat(*result) = Eigen::AngleAxisf(angle_rads, copy(vector));
215}
216
217extern "C" void
218math_quat_from_euler_angles(const struct xrt_vec3 *angles, struct xrt_quat *result)
219{
220 map_quat(*result) = Eigen::AngleAxisf(angles->z, Eigen::Vector3f::UnitZ()) *
221 Eigen::AngleAxisf(angles->y, Eigen::Vector3f::UnitY()) *
222 Eigen::AngleAxisf(angles->x, Eigen::Vector3f::UnitX());
223}
224
225extern "C" void
226math_quat_to_euler_angles(const struct xrt_quat *quat, struct xrt_vec3 *euler_angles)
227{
228 Eigen::Quaternionf eigen_quat(quat->w, quat->x, quat->y, quat->z);
229 Eigen::Vector3f eigen_euler = eigen_quat.toRotationMatrix().eulerAngles(2, 1, 0);
230
231 euler_angles->x = eigen_euler.x();
232 euler_angles->y = eigen_euler.y();
233 euler_angles->z = eigen_euler.z();
234}
235
236extern "C" void
237math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat, struct xrt_quat *result)
238{
239 Eigen::Matrix3f m;
240 m << mat->v[0], mat->v[1], mat->v[2], mat->v[3], mat->v[4], mat->v[5], mat->v[6], mat->v[7], mat->v[8];
241
242 Eigen::Quaternionf q(m);
243 map_quat(*result) = q;
244}
245
246extern "C" void
247math_quat_from_plus_x_z(const struct xrt_vec3 *plus_x, const struct xrt_vec3 *plus_z, struct xrt_quat *result)
248{
249 xrt_vec3 plus_y;
250 math_vec3_cross(plus_z, plus_x, &plus_y);
251
252 xrt_matrix_3x3 m = {{
253 plus_x->x,
254 plus_y.x,
255 plus_z->x,
256 plus_x->y,
257 plus_y.y,
258 plus_z->y,
259 plus_x->z,
260 plus_y.z,
261 plus_z->z,
262 }};
263
264 math_quat_from_matrix_3x3(&m, result);
265}
266
267extern "C" void
268math_quat_from_vec_a_to_vec_b(const struct xrt_vec3 *vec_a, const struct xrt_vec3 *vec_b, struct xrt_quat *result)
269{
270 map_quat(*result) = Eigen::Quaternionf::FromTwoVectors(copy(vec_a), copy(vec_b));
271}
272
273static bool
274quat_validate(const float precision, const struct xrt_quat *quat)
275{
276 assert(quat != NULL);
277 auto rot = copy(*quat);
278
279
280 /*
281 * This was originally squaredNorm, but that could result in a norm
282 * value that was further from 1.0f then FLOAT_EPSILON (two).
283 *
284 * Our tracking system would produce such orientations and looping those
285 * back into say a quad layer would cause this to fail. And even
286 * normalizing the quat would not fix this as normalizations uses
287 * non-squared "length" which does fall into the range and doesn't
288 * change the elements of the quat.
289 */
290 auto norm = rot.norm();
291 if (norm > 1.0f + precision || norm < 1.0f - precision) {
292 return false;
293 }
294
295 // Technically not yet a required check, but easier to stop problems
296 // now than once denormalized numbers pollute the rest of our state.
297 // see https://gitlab.khronos.org/openxr/openxr/issues/922
298 if (!rot.coeffs().allFinite()) {
299 return false;
300 }
301
302 return true;
303}
304
305extern "C" bool
306math_quat_validate(const struct xrt_quat *quat)
307{
308 const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
309 return quat_validate(FLOAT_EPSILON, quat);
310}
311
312extern "C" bool
313math_quat_validate_within_1_percent(const struct xrt_quat *quat)
314{
315 return quat_validate(0.01f, quat);
316}
317
318extern "C" void
319math_quat_invert(const struct xrt_quat *quat, struct xrt_quat *out_quat)
320{
321 map_quat(*out_quat) = map_quat(*quat).conjugate();
322}
323
324extern "C" float
325math_quat_len(const struct xrt_quat *quat)
326{
327 return map_quat(*quat).norm();
328}
329
330extern "C" void
331math_quat_normalize(struct xrt_quat *inout)
332{
333 assert(inout != NULL);
334 map_quat(*inout).normalize();
335}
336
337extern "C" bool
338math_quat_ensure_normalized(struct xrt_quat *inout)
339{
340 assert(inout != NULL);
341
342 if (math_quat_validate(inout))
343 return true;
344
345 const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
346 const float TOLERANCE = FLOAT_EPSILON * 5;
347
348 auto rot = copy(*inout);
349 auto norm = rot.norm();
350 if (norm > 1.0f + TOLERANCE || norm < 1.0f - TOLERANCE) {
351 return false;
352 }
353
354 map_quat(*inout).normalize();
355 return true;
356}
357
358
359extern "C" void
360math_quat_rotate(const struct xrt_quat *left, const struct xrt_quat *right, struct xrt_quat *result)
361{
362 assert(left != NULL);
363 assert(right != NULL);
364 assert(result != NULL);
365
366 auto l = copy(left);
367 auto r = copy(right);
368
369 auto q = l * r;
370
371 map_quat(*result) = q;
372}
373
374extern "C" void
375math_quat_unrotate(const struct xrt_quat *left, const struct xrt_quat *right, struct xrt_quat *result)
376{
377 assert(left != NULL);
378 assert(right != NULL);
379 assert(result != NULL);
380
381 auto l = copy(left);
382 auto r = copy(right);
383
384 auto q = l.inverse() * r;
385
386 map_quat(*result) = q;
387}
388
389extern "C" void
390math_quat_rotate_vec3(const struct xrt_quat *left, const struct xrt_vec3 *right, struct xrt_vec3 *result)
391{
392 assert(left != NULL);
393 assert(right != NULL);
394 assert(result != NULL);
395
396 auto l = copy(left);
397 auto r = copy(right);
398
399 auto v = l * r;
400
401 map_vec3(*result) = v;
402}
403
404extern "C" void
405math_quat_rotate_derivative(const struct xrt_quat *quat, const struct xrt_vec3 *deriv, struct xrt_vec3 *result)
406{
407 assert(quat != NULL);
408 assert(deriv != NULL);
409 assert(result != NULL);
410
411 auto l = copy(quat);
412 auto m = Eigen::Quaternionf(0.0f, deriv->x, deriv->y, deriv->z);
413 auto r = l.conjugate();
414
415 auto v = l * m * r;
416
417 struct xrt_vec3 ret = {v.x(), v.y(), v.z()};
418 *result = ret;
419}
420
421extern "C" void
422math_quat_slerp(const struct xrt_quat *left, const struct xrt_quat *right, float t, struct xrt_quat *result)
423{
424 assert(left != NULL);
425 assert(right != NULL);
426 assert(result != NULL);
427
428 auto l = copy(left);
429 auto r = copy(right);
430
431 map_quat(*result) = l.slerp(t, r);
432}
433
434extern "C" void
435math_quat_from_swing(const struct xrt_vec2 *swing, struct xrt_quat *result)
436{
437 assert(swing != NULL);
438 assert(result != NULL);
439 const float *a0 = &swing->x;
440 const float *a1 = &swing->y;
441 const float theta_squared = *a0 * *a0 + *a1 * *a1;
442
443 if (theta_squared > 0.f) {
444 const float theta = sqrt(theta_squared);
445 const float half_theta = theta * 0.5f;
446 const float k = sin(half_theta) / theta;
447 result->w = cos(half_theta);
448 result->x = *a0 * k;
449 result->y = *a1 * k;
450 result->z = 0.f;
451 } else {
452 // lim(x->0) (sin(x/2)/x) = 0.5, but sin(0)/0 is undefined, so we need to catch this with a conditional.
453 const float k = 0.5f;
454 result->w = 1.0f;
455 result->x = *a0 * k;
456 result->y = *a1 * k;
457 result->z = 0.f;
458 }
459}
460
461// See https://gitlab.freedesktop.org/slitcch/rotation_visualizer/-/blob/main/lm_rotations_story.inl for the derivation
462extern "C" void
463math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, struct xrt_quat *result)
464{
465 assert(swing != NULL);
466 assert(result != NULL);
467
468 float swing_x = swing->x;
469 float swing_y = swing->y;
470
471 float theta_squared_swing = swing_x * swing_x + swing_y * swing_y;
472
473 if (theta_squared_swing > float(0.0)) {
474 // theta_squared_swing is nonzero, so we the regular derived conversion.
475
476 float theta = sqrt(theta_squared_swing);
477
478 float half_theta = theta * float(0.5);
479
480 // the "other" theta
481 float half_twist = twist * float(0.5);
482
483 float cos_half_theta = cos(half_theta);
484 float cos_half_twist = cos(half_twist);
485
486 float sin_half_theta = sin(half_theta);
487 float sin_half_twist = sin(half_twist);
488
489 float sin_half_theta_over_theta = sin_half_theta / theta;
490
491 result->w = cos_half_theta * cos_half_twist;
492
493 float x_part_1 = (swing_x * cos_half_twist * sin_half_theta_over_theta);
494 float x_part_2 = (swing_y * sin_half_twist * sin_half_theta_over_theta);
495
496 result->x = x_part_1 + x_part_2;
497
498 float y_part_1 = (swing_y * cos_half_twist * sin_half_theta_over_theta);
499 float y_part_2 = (swing_x * sin_half_twist * sin_half_theta_over_theta);
500
501 result->y = y_part_1 - y_part_2;
502
503 result->z = cos_half_theta * sin_half_twist;
504
505 } else {
506 // sin_half_theta/theta would be undefined, but
507 // the limit approaches 0.5, so we do this.
508 // Note the differences w/ lm_rotations.inl - we can skip some things as we're not using this to compute
509 // a jacobian.
510
511 float half_twist = twist * float(0.5);
512
513 float cos_half_twist = cos(half_twist);
514
515 float sin_half_twist = sin(half_twist);
516
517 float sin_half_theta_over_theta = float(0.5);
518
519 // cos(0) is 1 so no cos_half_theta necessary
520 result->w = cos_half_twist;
521
522 float x_part_1 = (swing_x * cos_half_twist * sin_half_theta_over_theta);
523 float x_part_2 = (swing_y * sin_half_twist * sin_half_theta_over_theta);
524
525 result->x = x_part_1 + x_part_2;
526
527 float y_part_1 = (swing_y * cos_half_twist * sin_half_theta_over_theta);
528 float y_part_2 = (swing_x * sin_half_twist * sin_half_theta_over_theta);
529
530 result->y = y_part_1 - y_part_2;
531
532 result->z = sin_half_twist;
533 }
534}
535
536/*!
537 * Converts a quaternion to XY-swing and Z-twist
538 *
539 * @relates xrt_quat
540 * @ingroup aux_math
541 */
542extern "C" void
543math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, float *out_twist)
544{
545 Eigen::Quaternionf rot = map_quat(*in);
546
547 Eigen::Vector3f our_z = rot * (Eigen::Vector3f::UnitZ());
548
549 Eigen::Quaternionf swing = Eigen::Quaternionf().setFromTwoVectors(Eigen::Vector3f::UnitZ(), our_z);
550
551 Eigen::Quaternionf twist = swing.inverse() * rot;
552
553 Eigen::AngleAxisf twist_aax = Eigen::AngleAxisf(twist);
554
555 Eigen::AngleAxisf swing_aax = Eigen::AngleAxisf(swing);
556
557 out_swing->x = swing_aax.axis().x() * swing_aax.angle();
558 out_swing->y = swing_aax.axis().y() * swing_aax.angle();
559 assert(swing_aax.axis().z() < 0.001);
560
561 *out_twist = twist_aax.axis().z() * twist_aax.angle();
562}
563
564void
565math_quat_decompose_swing_twist(const struct xrt_quat *in,
566 const struct xrt_vec3 *twist_axis,
567 struct xrt_quat *swing,
568 struct xrt_quat *twist)
569{
570 struct xrt_quat twist_inv;
571 struct xrt_vec3 orig_axis;
572 float dot;
573
574 orig_axis.x = in->x;
575 orig_axis.y = in->y;
576 orig_axis.z = in->z;
577
578 /* Calculate projection onto the twist axis */
579 dot = m_vec3_dot(orig_axis, *twist_axis);
580 struct xrt_vec3 projection = *twist_axis;
581 math_vec3_scalar_mul(dot, &projection);
582
583 twist->x = projection.x;
584 twist->y = projection.y;
585 twist->z = projection.z;
586 twist->w = in->w;
587
588 if (math_quat_dot(twist, twist) < FLT_EPSILON) {
589 /* Singularity - 180 degree rotation and perpendicular
590 * decomp axis, so twist is the identity quat */
591 twist->x = twist->y = twist->z = 0.0;
592 twist->w = 1.0;
593 } else {
594 math_quat_normalize(twist);
595 }
596
597 math_quat_invert(twist, &twist_inv);
598 math_quat_rotate(in, &twist_inv, swing);
599 math_quat_normalize(swing);
600}
601
602/*
603 *
604 * Exported matrix functions.
605 *
606 */
607
608
609
610extern "C" void
611math_matrix_3x3_identity(struct xrt_matrix_3x3 *mat)
612{
613 map_matrix_3x3(*mat) = Eigen::Matrix3f::Identity();
614}
615
616extern "C" void
617math_matrix_3x3_from_quat(const struct xrt_quat *q, struct xrt_matrix_3x3 *result_out)
618{
619 struct xrt_matrix_3x3 result = {{
620 1 - 2 * q->y * q->y - 2 * q->z * q->z,
621 2 * q->x * q->y - 2 * q->w * q->z,
622 2 * q->x * q->z + 2 * q->w * q->y,
623
624 2 * q->x * q->y + 2 * q->w * q->z,
625 1 - 2 * q->x * q->x - 2 * q->z * q->z,
626 2 * q->y * q->z - 2 * q->w * q->x,
627
628 2 * q->x * q->z - 2 * q->w * q->y,
629 2 * q->y * q->z + 2 * q->w * q->x,
630 1 - 2 * q->x * q->x - 2 * q->y * q->y,
631 }};
632
633 *result_out = result;
634}
635
636extern "C" void
637math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat)
638{
639 map_matrix_3x3_f64(*mat) = Eigen::Matrix3d::Identity();
640}
641
642extern "C" void
643math_matrix_3x3_f64_transform_vec3_f64(const struct xrt_matrix_3x3_f64 *left,
644 const struct xrt_vec3_f64 *right,
645 struct xrt_vec3_f64 *result_out)
646{
647 Eigen::Matrix3d m;
648 m << left->v[0], left->v[1], left->v[2], // 1
649 left->v[3], left->v[4], left->v[5], // 2
650 left->v[6], left->v[7], left->v[8]; // 3
651
652 map_vec3_f64(*result_out) = m * copy(right);
653}
654
655extern "C" void
656math_matrix_3x3_f64_from_plus_x_z(const struct xrt_vec3_f64 *plus_x,
657 const struct xrt_vec3_f64 *plus_z,
658 struct xrt_matrix_3x3_f64 *result)
659{
660 xrt_vec3_f64 plus_y;
661 math_vec3_f64_cross(plus_z, plus_x, &plus_y);
662
663 result->v[0] = plus_x->x;
664 result->v[1] = plus_y.x;
665 result->v[2] = plus_z->x;
666 result->v[3] = plus_x->y;
667 result->v[4] = plus_y.y;
668 result->v[5] = plus_z->y;
669 result->v[6] = plus_x->z;
670 result->v[7] = plus_y.z;
671 result->v[8] = plus_z->z;
672}
673
674extern "C" void
675math_matrix_3x3_rotation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_matrix_3x3 *result)
676{
677 Eigen::Isometry3f transform{map_matrix_4x4(*isometry)};
678 map_matrix_3x3(*result) = transform.linear();
679}
680
681extern "C" void
682math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left,
683 const struct xrt_vec3 *right,
684 struct xrt_vec3 *result_out)
685{
686 Eigen::Matrix3f m;
687 m << left->v[0], left->v[1], left->v[2], // 1
688 left->v[3], left->v[4], left->v[5], // 2
689 left->v[6], left->v[7], left->v[8]; // 3
690
691 map_vec3(*result_out) = m * copy(right);
692}
693
694extern "C" void
695math_matrix_4x4_transform_vec3(const struct xrt_matrix_4x4 *left,
696 const struct xrt_vec3 *right,
697 struct xrt_vec3 *result_out)
698{
699 Eigen::Matrix4f m = copy(left);
700
701 Eigen::Vector4f v;
702 v << right->x, right->y, right->z, 1.0;
703
704 Eigen::Vector4f res;
705 res = m * v;
706
707 result_out->x = res.x();
708 result_out->y = res.y();
709 result_out->z = res.z();
710}
711
712extern "C" void
713math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left,
714 const struct xrt_matrix_3x3 *right,
715 struct xrt_matrix_3x3 *result_out)
716{
717 const struct xrt_matrix_3x3 l = *left;
718 const struct xrt_matrix_3x3 r = *right;
719
720 struct xrt_matrix_3x3 result = {{
721 l.v[0] * r.v[0] + l.v[1] * r.v[3] + l.v[2] * r.v[6],
722 l.v[0] * r.v[1] + l.v[1] * r.v[4] + l.v[2] * r.v[7],
723 l.v[0] * r.v[2] + l.v[1] * r.v[5] + l.v[2] * r.v[8],
724 l.v[3] * r.v[0] + l.v[4] * r.v[3] + l.v[5] * r.v[6],
725 l.v[3] * r.v[1] + l.v[4] * r.v[4] + l.v[5] * r.v[7],
726 l.v[3] * r.v[2] + l.v[4] * r.v[5] + l.v[5] * r.v[8],
727 l.v[6] * r.v[0] + l.v[7] * r.v[3] + l.v[8] * r.v[6],
728 l.v[6] * r.v[1] + l.v[7] * r.v[4] + l.v[8] * r.v[7],
729 l.v[6] * r.v[2] + l.v[7] * r.v[5] + l.v[8] * r.v[8],
730 }};
731
732 *result_out = result;
733}
734
735extern "C" void
736math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result)
737{
738 Eigen::Matrix3f m = copy(in);
739 map_matrix_3x3(*result) = m.inverse();
740}
741
742extern "C" void
743math_matrix_3x3_transpose(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result)
744{
745 Eigen::Matrix3f m = copy(in);
746 map_matrix_3x3(*result) = m.transpose();
747}
748
749extern "C" void
750math_matrix_4x4_identity(struct xrt_matrix_4x4 *result)
751{
752 map_matrix_4x4(*result) = Eigen::Matrix4f::Identity();
753}
754
755extern "C" void
756math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
757 const struct xrt_matrix_4x4 *right,
758 struct xrt_matrix_4x4 *result)
759{
760 map_matrix_4x4(*result) = copy(left) * copy(right);
761}
762
763extern "C" void
764math_matrix_4x4_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
765{
766 Eigen::Matrix4f m = copy(in);
767 map_matrix_4x4(*result) = m.inverse();
768}
769
770extern "C" void
771math_matrix_4x4_transpose(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
772{
773 Eigen::Matrix4f m = copy(in);
774 map_matrix_4x4(*result) = m.transpose();
775}
776
777extern "C" void
778math_matrix_4x4_isometry_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
779{
780 Eigen::Isometry3f m{copy(in)};
781 map_matrix_4x4(*result) = m.inverse().matrix();
782}
783
784extern "C" void
785math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
786{
787 Eigen::Vector3f position = copy(&pose->position);
788 Eigen::Quaternionf orientation = copy(&pose->orientation);
789
790 Eigen::Translation3f translation(position);
791 Eigen::Isometry3f transformation = translation * orientation;
792
793 map_matrix_4x4(*result) = transformation.inverse().matrix();
794}
795
796extern "C" void
797math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation,
798 const struct xrt_vec3 *translation,
799 struct xrt_matrix_4x4 *result)
800{
801 Eigen::Isometry3f transformation = Eigen::Isometry3f::Identity();
802 transformation.linear() = map_matrix_3x3(*rotation);
803 transformation.translation() = map_vec3(*translation);
804 map_matrix_4x4(*result) = transformation.matrix();
805}
806
807extern "C" void
808math_matrix_4x4_isometry_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
809{
810 Eigen::Isometry3f transform{Eigen::Translation3f{position(*pose)} * orientation(*pose)};
811 map_matrix_4x4(*result) = transform.matrix();
812}
813
814extern "C" void
815math_matrix_4x4_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4 *result)
816{
817 Eigen::Vector3f position = copy(&pose->position);
818 Eigen::Quaternionf orientation = copy(&pose->orientation);
819
820 auto scale = Eigen::Scaling(size->x, size->y, size->z);
821
822 Eigen::Translation3f translation(position);
823 Eigen::Affine3f transformation = translation * orientation * scale;
824
825 map_matrix_4x4(*result) = transformation.matrix();
826}
827
828extern "C" void
829math_matrix_4x4_inverse_view_projection(const struct xrt_matrix_4x4 *view,
830 const struct xrt_matrix_4x4 *projection,
831 struct xrt_matrix_4x4 *result)
832{
833 Eigen::Matrix4f v = copy(view);
834 Eigen::Matrix4f v3 = Eigen::Matrix4f::Identity();
835 v3.block<3, 3>(0, 0) = v.block<3, 3>(0, 0);
836 Eigen::Matrix4f vp = copy(projection) * v3;
837 map_matrix_4x4(*result) = vp.inverse();
838}
839
840
841/*
842 *
843 * Exported Matrix 4x4 functions.
844 *
845 */
846
847extern "C" void
848m_mat4_f64_identity(struct xrt_matrix_4x4_f64 *result)
849{
850 map_matrix_4x4_f64(*result) = Eigen::Matrix4d::Identity();
851}
852
853extern "C" void
854m_mat4_f64_invert(const struct xrt_matrix_4x4_f64 *matrix, struct xrt_matrix_4x4_f64 *result)
855{
856 Eigen::Matrix4d m = map_matrix_4x4_f64(*matrix);
857 map_matrix_4x4_f64(*result) = m.inverse();
858}
859
860extern "C" void
861m_mat4_f64_multiply(const struct xrt_matrix_4x4_f64 *left,
862 const struct xrt_matrix_4x4_f64 *right,
863 struct xrt_matrix_4x4_f64 *result)
864{
865 Eigen::Matrix4d l = map_matrix_4x4_f64(*left);
866 Eigen::Matrix4d r = map_matrix_4x4_f64(*right);
867
868 map_matrix_4x4_f64(*result) = l * r;
869}
870
871extern "C" void
872m_mat4_f64_orientation(const struct xrt_quat *quat, struct xrt_matrix_4x4_f64 *result)
873{
874 map_matrix_4x4_f64(*result) = Eigen::Affine3d(copyd(*quat)).matrix();
875}
876
877extern "C" void
878m_mat4_f64_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4_f64 *result)
879{
880 Eigen::Vector3d position = copyd(pose->position);
881 Eigen::Quaterniond orientation = copyd(pose->orientation);
882
883 auto scale = Eigen::Scaling(copyd(size));
884
885 Eigen::Translation3d translation(position);
886 Eigen::Affine3d transformation = translation * orientation * scale;
887
888 map_matrix_4x4_f64(*result) = transformation.matrix();
889}
890
891extern "C" void
892m_mat4_f64_view(const struct xrt_pose *pose, struct xrt_matrix_4x4_f64 *result)
893{
894 Eigen::Vector3d position = copyd(pose->position);
895 Eigen::Quaterniond orientation = copyd(pose->orientation);
896
897 Eigen::Translation3d translation(position);
898 Eigen::Isometry3d transformation = translation * orientation;
899
900 map_matrix_4x4_f64(*result) = transformation.inverse().matrix();
901}
902
903
904/*
905 *
906 * Exported pose functions.
907 *
908 */
909
910extern "C" bool
911math_pose_validate(const struct xrt_pose *pose)
912{
913 assert(pose != NULL);
914
915 return math_vec3_validate(&pose->position) && math_quat_validate(&pose->orientation);
916}
917
918extern "C" void
919math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
920{
921 Eigen::Isometry3f transform{Eigen::Translation3f{position(*pose)} * orientation(*pose)};
922 Eigen::Isometry3f inverse = transform.inverse();
923 position(*outPose) = inverse.translation();
924 orientation(*outPose) = inverse.rotation();
925}
926
927extern "C" void
928math_pose_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_pose *result)
929{
930 Eigen::Isometry3f isometry{map_matrix_4x4(*transform)};
931 position(*result) = isometry.translation();
932 orientation(*result) = isometry.rotation();
933}
934
935extern "C" void
936math_pose_interpolate(const struct xrt_pose *a, const struct xrt_pose *b, float t, struct xrt_pose *outPose)
937{
938 math_quat_slerp(&a->orientation, &b->orientation, t, &outPose->orientation);
939 outPose->position = m_vec3_lerp(a->position, b->position, t);
940}
941
942extern "C" void
943math_pose_identity(struct xrt_pose *pose)
944{
945 pose->position.x = 0.0;
946 pose->position.y = 0.0;
947 pose->position.z = 0.0;
948 pose->orientation.x = 0.0;
949 pose->orientation.y = 0.0;
950 pose->orientation.z = 0.0;
951 pose->orientation.w = 1.0;
952}
953
954/*!
955 * Return the result of transforming a point by a pose/transform.
956 */
957static inline Eigen::Vector3f
958transform_point(const xrt_pose &transform, const xrt_vec3 &point)
959{
960 return orientation(transform) * map_vec3(point) + position(transform);
961}
962
963/*!
964 * Return the result of transforming a pose by a pose/transform.
965 */
966static inline xrt_pose
967transform_pose(const xrt_pose &transform, const xrt_pose &pose)
968{
969 xrt_pose ret;
970 position(ret) = transform_point(transform, pose.position);
971 orientation(ret) = orientation(transform) * orientation(pose);
972 return ret;
973}
974
975extern "C" void
976math_pose_transform(const struct xrt_pose *transform, const struct xrt_pose *pose, struct xrt_pose *outPose)
977{
978 assert(pose != NULL);
979 assert(transform != NULL);
980 assert(outPose != NULL);
981
982 xrt_pose newPose = transform_pose(*transform, *pose);
983 memcpy(outPose, &newPose, sizeof(xrt_pose));
984}
985
986extern "C" void
987math_pose_transform_point(const struct xrt_pose *transform, const struct xrt_vec3 *point, struct xrt_vec3 *out_point)
988{
989 assert(transform != NULL);
990 assert(point != NULL);
991 assert(out_point != NULL);
992
993 map_vec3(*out_point) = transform_point(*transform, *point);
994}