1 /*
2 * Copyright (C) 2021 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "media/QuaternionUtil.h"
18
19 #include <cassert>
20
21 namespace android {
22 namespace media {
23
24 using Eigen::NumTraits;
25 using Eigen::Quaternionf;
26 using Eigen::Vector3f;
27
28 namespace {
29
LogSU2(const Quaternionf & q)30 Vector3f LogSU2(const Quaternionf& q) {
31 // Implementation of the logarithmic map of SU(2) using atan.
32 // This follows Hertzberg et al. "Integrating Generic Sensor Fusion Algorithms
33 // with Sound State Representations through Encapsulation of Manifolds", Eq.
34 // (31)
35 // We use asin and acos instead of atan to enable the use of Eigen Autodiff
36 // with SU2.
37 const float sign_of_w = q.w() < 0.f ? -1.f : 1.f;
38 const float abs_w = sign_of_w * q.w();
39 const Vector3f v = sign_of_w * q.vec();
40 const float squared_norm_of_v = v.squaredNorm();
41
42 assert(abs(1.f - abs_w * abs_w - squared_norm_of_v) < NumTraits<float>::dummy_precision());
43
44 if (squared_norm_of_v > NumTraits<float>::dummy_precision()) {
45 const float norm_of_v = sqrt(squared_norm_of_v);
46 if (abs_w > NumTraits<float>::dummy_precision()) {
47 // asin(x) = acos(x) at x = 1/sqrt(2).
48 if (norm_of_v <= float(M_SQRT1_2)) {
49 return (asin(norm_of_v) / norm_of_v) * v;
50 }
51 return (acos(abs_w) / norm_of_v) * v;
52 }
53 return (M_PI_2 / norm_of_v) * v;
54 }
55
56 // Taylor expansion at squared_norm_of_v == 0
57 return (1.f / abs_w - squared_norm_of_v / (3.f * pow(abs_w, 3))) * v;
58 }
59
ExpSU2(const Vector3f & delta)60 Quaternionf ExpSU2(const Vector3f& delta) {
61 Quaternionf q_delta;
62 const float theta_squared = delta.squaredNorm();
63 if (theta_squared > NumTraits<float>::dummy_precision()) {
64 const float theta = sqrt(theta_squared);
65 q_delta.w() = cos(theta);
66 q_delta.vec() = (sin(theta) / theta) * delta;
67 } else {
68 // taylor expansions around theta == 0
69 q_delta.w() = 1.f - 0.5f * theta_squared;
70 q_delta.vec() = (1.f - 1.f / 6.f * theta_squared) * delta;
71 }
72 return q_delta;
73 }
74
75 } // namespace
76
rotationVectorToQuaternion(const Vector3f & rotationVector)77 Quaternionf rotationVectorToQuaternion(const Vector3f& rotationVector) {
78 // SU(2) is a double cover of SO(3), thus we have to half the tangent vector
79 // delta
80 const Vector3f half_delta = 0.5f * rotationVector;
81 return ExpSU2(half_delta);
82 }
83
quaternionToRotationVector(const Quaternionf & quaternion)84 Vector3f quaternionToRotationVector(const Quaternionf& quaternion) {
85 // SU(2) is a double cover of SO(3), thus we have to multiply the tangent
86 // vector delta by two
87 return 2.f * LogSU2(quaternion);
88 }
89
rotateX(float angle)90 Quaternionf rotateX(float angle) {
91 return rotationVectorToQuaternion(Vector3f(1, 0, 0) * angle);
92 }
93
rotateY(float angle)94 Quaternionf rotateY(float angle) {
95 return rotationVectorToQuaternion(Vector3f(0, 1, 0) * angle);
96 }
97
rotateZ(float angle)98 Quaternionf rotateZ(float angle) {
99 return rotationVectorToQuaternion(Vector3f(0, 0, 1) * angle);
100 }
101
102 } // namespace media
103 } // namespace android
104