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/Twist.cpp b/media/libheadtracking/Twist.cpp
new file mode 100644
index 0000000..664c4d5
--- /dev/null
+++ b/media/libheadtracking/Twist.cpp
@@ -0,0 +1,43 @@
+/*
+ * 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 "media/Twist.h"
+
+#include "QuaternionUtil.h"
+
+namespace android {
+namespace media {
+
+Pose3f integrate(const Twist3f& twist, float dt) {
+ Eigen::Vector3f translation = twist.translationalVelocity() * dt;
+ Eigen::Vector3f rotationVector = twist.rotationalVelocity() * dt;
+ return Pose3f(translation, rotationVectorToQuaternion(rotationVector));
+}
+
+Twist3f differentiate(const Pose3f& pose, float dt) {
+ Eigen::Vector3f translationalVelocity = pose.translation() / dt;
+ Eigen::Vector3f rotationalVelocity = quaternionToRotationVector(pose.rotation()) / dt;
+ return Twist3f(translationalVelocity, rotationalVelocity);
+}
+
+std::ostream& operator<<(std::ostream& os, const Twist3f& twist) {
+ os << "translation: " << twist.translationalVelocity().transpose()
+ << " rotation vector: " << twist.rotationalVelocity().transpose();
+ return os;
+}
+
+} // namespace media
+} // namespace android