matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019, Collabora, Ltd.
2// Copyright 2016, Sensics, Inc.
3// SPDX-License-Identifier: Apache-2.0
4/*!
5 * @file
6 * @brief Base implementations for math library.
7 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
8 * @ingroup aux_math
9 *
10 * Based in part on inc/osvr/Util/EigenQuatExponentialMap.h in OSVR-Core
11 */
12// IWYU pragma: no_include "src/Core/MatrixBase.h"
13
14#include "math/m_api.h"
15#include "math/m_eigen_interop.hpp"
16
17#include <Eigen/Core>
18#include <Eigen/Geometry>
19
20#include <assert.h>
21
22
23// anonymous namespace for internal types
24namespace {
25template <typename Scalar> struct FourthRootMachineEps;
26
27template <> struct FourthRootMachineEps<double>
28{
29 /// machine epsilon is 1e-53, so fourth root is roughly 1e-13
30 static double
31 get()
32 {
33 return 1.e-13;
34 }
35};
36template <> struct FourthRootMachineEps<float>
37{
38 /// machine epsilon is 1e-24, so fourth root is 1e-6
39 static float
40 get()
41 {
42 return 1.e-6f;
43 }
44};
45/// Computes the "historical" (un-normalized) sinc(Theta)
46/// (sine(theta)/theta for theta != 0, defined as the limit value of 0
47/// at theta = 0)
48template <typename Scalar>
49inline Scalar
50sinc(Scalar theta)
51{
52 /// fourth root of machine epsilon is recommended cutoff for taylor
53 /// series expansion vs. direct computation per
54 /// Grassia, F. S. (1998). Practical Parameterization of Rotations
55 /// Using the Exponential Map. Journal of Graphics Tools, 3(3),
56 /// 29-48. http://doi.org/10.1080/10867651.1998.10487493
57 Scalar ret;
58 if (theta < FourthRootMachineEps<Scalar>::get()) {
59 // taylor series expansion.
60 ret = Scalar(1.f) - theta * theta / Scalar(6.f);
61 return ret;
62 }
63 // direct computation.
64 ret = std::sin(theta) / theta;
65 return ret;
66}
67
68/// fully-templated free function for quaternion expontiation
69template <typename Derived>
70inline Eigen::Quaternion<typename Derived::Scalar>
71quat_exp(Eigen::MatrixBase<Derived> const &vec)
72{
73 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
74 using Scalar = typename Derived::Scalar;
75 /// Implementation inspired by
76 /// Grassia, F. S. (1998). Practical Parameterization of Rotations
77 /// Using the Exponential Map. Journal of Graphics Tools, 3(3),
78 /// 29–48. http://doi.org/10.1080/10867651.1998.10487493
79 ///
80 /// However, that work introduced a factor of 1/2 which I could not
81 /// derive from the definition of quaternion exponentiation and
82 /// whose absence thus distinguishes this implementation. Without
83 /// that factor of 1/2, the exp and ln functions successfully
84 /// round-trip and match other implementations.
85 ///
86 /// @todo That 1/2 term is important, fix it and enable disabled test on
87 /// tests_quatexpmap.cpp
88 Scalar theta = vec.norm();
89 Scalar vecscale = sinc(theta);
90 Eigen::Quaternion<Scalar> ret;
91 ret.vec() = vecscale * vec;
92 ret.w() = std::cos(theta);
93 return ret.normalized();
94}
95
96/// Taylor series expansion of theta over sin(theta), also known as cosecant, for
97/// use near 0 when you want continuity and validity at 0.
98template <typename Scalar>
99inline Scalar
100cscTaylorExpansion(Scalar theta)
101{
102 return Scalar(1) +
103 // theta ^ 2 / 6
104 (theta * theta) / Scalar(6) +
105 // 7 theta^4 / 360
106 (Scalar(7) * theta * theta * theta * theta) / Scalar(360) +
107 // 31 theta^6/15120
108 (Scalar(31) * theta * theta * theta * theta * theta * theta) / Scalar(15120);
109}
110
111/// fully-templated free function for quaternion log map.
112///
113/// Assumes a unit quaternion.
114template <typename Scalar>
115inline Eigen::Matrix<Scalar, 3, 1>
116quat_ln(Eigen::Quaternion<Scalar> const &quat)
117{
118 // ln q = ( (phi)/(norm of vec) vec, ln(norm of quat))
119 // When we assume a unit quaternion, ln(norm of quat) = 0
120 // so then we scale the vector part by phi/sin(phi) to get the
121 // result (i.e., ln(qv, qw) = (phi/sin(phi)) * qv )
122 Scalar vecnorm = quat.vec().norm();
123
124 // "best for numerical stability" vs asin or acos
125 Scalar phi = std::atan2(vecnorm, quat.w());
126
127 // Here is where we compute the coefficient to scale the vector part
128 // by, which is nominally phi / std::sin(phi).
129 // When the angle approaches zero, we compute the coefficient
130 // differently, since it gets a bit like sinc in that we want it
131 // continuous but 0 is undefined.
132 Scalar phiOverSin = vecnorm < 1e-4 ? cscTaylorExpansion<Scalar>(phi) : (phi / std::sin(phi));
133 return quat.vec() * phiOverSin;
134}
135
136} // namespace
137
138using namespace xrt::auxiliary::math;
139
140extern "C" void
141math_quat_integrate_velocity(const struct xrt_quat *quat,
142 const struct xrt_vec3 *ang_vel,
143 const float dt,
144 struct xrt_quat *result)
145{
146 assert(quat != NULL);
147 assert(ang_vel != NULL);
148 assert(result != NULL);
149 assert(dt != 0);
150
151
152 Eigen::Quaternionf q = map_quat(*quat);
153 Eigen::Quaternionf incremental_rotation = quat_exp(map_vec3(*ang_vel) * dt * 0.5f).normalized();
154 map_quat(*result) = q * incremental_rotation;
155}
156
157extern "C" void
158math_quat_finite_difference(const struct xrt_quat *quat0,
159 const struct xrt_quat *quat1,
160 const float dt,
161 struct xrt_vec3 *out_ang_vel)
162{
163 assert(quat0 != NULL);
164 assert(quat1 != NULL);
165 assert(out_ang_vel != NULL);
166 assert(dt != 0);
167
168
169 Eigen::Quaternionf inc_quat = map_quat(*quat1) * map_quat(*quat0).conjugate();
170 map_vec3(*out_ang_vel) = 2.f * quat_ln(inc_quat) / dt;
171}
172
173extern "C" void
174math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat)
175{
176 map_quat(*out_quat) = quat_exp(map_vec3(*axis_angle));
177}
178
179extern "C" void
180math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle)
181{
182 Eigen::Quaternionf eigen_quat = map_quat(*quat);
183 map_vec3(*out_axis_angle) = quat_ln(eigen_quat);
184}