Head-tracking library for Immersive Audio
See README.md for details.
Bug: 188502620
Test: atest --host libheadtracking-test
Change-Id: I34201c4780e5e581cc96449bd89863bcbc250783
diff --git a/media/libheadtracking/QuaternionUtil.cpp b/media/libheadtracking/QuaternionUtil.cpp
new file mode 100644
index 0000000..5d090de
--- /dev/null
+++ b/media/libheadtracking/QuaternionUtil.cpp
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "QuaternionUtil.h"
+
+#include <cassert>
+
+namespace android {
+namespace media {
+
+using Eigen::NumTraits;
+using Eigen::Quaternionf;
+using Eigen::Vector3f;
+
+namespace {
+
+Vector3f LogSU2(const Quaternionf& q) {
+ // Implementation of the logarithmic map of SU(2) using atan.
+ // This follows Hertzberg et al. "Integrating Generic Sensor Fusion Algorithms
+ // with Sound State Representations through Encapsulation of Manifolds", Eq.
+ // (31)
+ // We use asin and acos instead of atan to enable the use of Eigen Autodiff
+ // with SU2.
+ const float sign_of_w = q.w() < 0.f ? -1.f : 1.f;
+ const float abs_w = sign_of_w * q.w();
+ const Vector3f v = sign_of_w * q.vec();
+ const float squared_norm_of_v = v.squaredNorm();
+
+ assert(abs(1.f - abs_w * abs_w - squared_norm_of_v) < NumTraits<float>::dummy_precision());
+
+ if (squared_norm_of_v > NumTraits<float>::dummy_precision()) {
+ const float norm_of_v = sqrt(squared_norm_of_v);
+ if (abs_w > NumTraits<float>::dummy_precision()) {
+ // asin(x) = acos(x) at x = 1/sqrt(2).
+ if (norm_of_v <= float(M_SQRT1_2)) {
+ return (asin(norm_of_v) / norm_of_v) * v;
+ }
+ return (acos(abs_w) / norm_of_v) * v;
+ }
+ return (M_PI_2 / norm_of_v) * v;
+ }
+
+ // Taylor expansion at squared_norm_of_v == 0
+ return (1.f / abs_w - squared_norm_of_v / (3.f * pow(abs_w, 3))) * v;
+}
+
+Quaternionf ExpSU2(const Vector3f& delta) {
+ Quaternionf q_delta;
+ const float theta_squared = delta.squaredNorm();
+ if (theta_squared > NumTraits<float>::dummy_precision()) {
+ const float theta = sqrt(theta_squared);
+ q_delta.w() = cos(theta);
+ q_delta.vec() = (sin(theta) / theta) * delta;
+ } else {
+ // taylor expansions around theta == 0
+ q_delta.w() = 1.f - 0.5f * theta_squared;
+ q_delta.vec() = (1.f - 1.f / 6.f * theta_squared) * delta;
+ }
+ return q_delta;
+}
+
+} // namespace
+
+Quaternionf rotationVectorToQuaternion(const Vector3f& rotationVector) {
+ // SU(2) is a double cover of SO(3), thus we have to half the tangent vector
+ // delta
+ const Vector3f half_delta = 0.5f * rotationVector;
+ return ExpSU2(half_delta);
+}
+
+Vector3f quaternionToRotationVector(const Quaternionf& quaternion) {
+ // SU(2) is a double cover of SO(3), thus we have to multiply the tangent
+ // vector delta by two
+ return 2.f * LogSU2(quaternion);
+}
+
+Quaternionf rotateX(float angle) {
+ return rotationVectorToQuaternion(Vector3f(1, 0, 0) * angle);
+}
+
+Quaternionf rotateY(float angle) {
+ return rotationVectorToQuaternion(Vector3f(0, 1, 0) * angle);
+}
+
+Quaternionf rotateZ(float angle) {
+ return rotationVectorToQuaternion(Vector3f(0, 0, 1) * angle);
+}
+
+} // namespace media
+} // namespace android