Spatial Audio: Roll pitch yaw logging.
Add roll, pitch, yaw angle logging.
Test: atest libheadtracking-test
Test: check dumpsys, use head tracker
Bug: 269620212
Bug: 269683154
Change-Id: Iaa0249b8498a0b5d4e72e777d62036096e643f37
diff --git a/media/libheadtracking/QuaternionUtil-test.cpp b/media/libheadtracking/QuaternionUtil-test.cpp
index ebb4223..cfeca00 100644
--- a/media/libheadtracking/QuaternionUtil-test.cpp
+++ b/media/libheadtracking/QuaternionUtil-test.cpp
@@ -51,6 +51,92 @@
EXPECT_EQ(vec, quaternionToRotationVector(rotationVectorToQuaternion(vec)));
}
+// Float precision necessitates this precision (1e-4f fails)
+constexpr float NEAR = 1e-3f;
+
+TEST(QuaternionUtil, quaternionToAngles_basic) {
+ float pitch, roll, yaw;
+
+ // angles as reported.
+ // choose 11 angles between -M_PI / 2 to M_PI / 2
+ for (int step = -5; step <= 5; ++step) {
+ const float angle = M_PI * step * 0.1f;
+
+ quaternionToAngles(rotationVectorToQuaternion({angle, 0.f, 0.f}), &pitch, &roll, &yaw);
+ EXPECT_NEAR(angle, pitch, NEAR);
+ EXPECT_NEAR(0.f, roll, NEAR);
+ EXPECT_NEAR(0.f, yaw, NEAR);
+
+ quaternionToAngles(rotationVectorToQuaternion({0.f, angle, 0.f}), &pitch, &roll, &yaw);
+ EXPECT_NEAR(0.f, pitch, NEAR);
+ EXPECT_NEAR(angle, roll, NEAR);
+ EXPECT_NEAR(0.f, yaw, NEAR);
+
+ quaternionToAngles(rotationVectorToQuaternion({0.f, 0.f, angle}), &pitch, &roll, &yaw);
+ EXPECT_NEAR(0.f, pitch, NEAR);
+ EXPECT_NEAR(0.f, roll, NEAR);
+ EXPECT_NEAR(angle, yaw, NEAR);
+ }
+
+ // Generates a debug string
+ const std::string s = quaternionToAngles<true /* DEBUG */>(
+ rotationVectorToQuaternion({M_PI, 0.f, 0.f}), &pitch, &roll, &yaw);
+ ASSERT_FALSE(s.empty());
+}
+
+TEST(QuaternionUtil, quaternionToAngles_zaxis) {
+ float pitch, roll, yaw;
+
+ for (int rot_step = -10; rot_step <= 10; ++rot_step) {
+ const float rot_angle = M_PI * rot_step * 0.1f;
+ // pitch independent of world Z rotation
+
+ // We don't test the boundaries of pitch +-M_PI/2 as roll can become
+ // degenerate and atan(0, 0) may report 0, PI, or -PI.
+ for (int step = -4; step <= 4; ++step) {
+ const float angle = M_PI * step * 0.1f;
+ auto q = rotationVectorToQuaternion({angle, 0.f, 0.f});
+ auto world_z = rotationVectorToQuaternion({0.f, 0.f, rot_angle});
+
+ // Sequential active rotations (on world frame) compose as R_2 * R_1.
+ quaternionToAngles(world_z * q, &pitch, &roll, &yaw);
+
+ EXPECT_NEAR(angle, pitch, NEAR);
+ EXPECT_NEAR(0.f, roll, NEAR);
+ }
+
+ // roll independent of world Z rotation
+ for (int step = -5; step <= 5; ++step) {
+ const float angle = M_PI * step * 0.1f;
+ auto q = rotationVectorToQuaternion({0.f, angle, 0.f});
+ auto world_z = rotationVectorToQuaternion({0.f, 0.f, rot_angle});
+
+ // Sequential active rotations (on world frame) compose as R_2 * R_1.
+ quaternionToAngles(world_z * q, &pitch, &roll, &yaw);
+
+ EXPECT_NEAR(0.f, pitch, NEAR);
+ EXPECT_NEAR(angle, roll, NEAR);
+
+ // Convert extrinsic (world-based) active rotations to a sequence of
+ // intrinsic rotations (each rotation based off of previous rotation
+ // frame).
+ //
+ // R_1 * R_intrinsic = R_extrinsic * R_1
+ // implies
+ // R_intrinsic = (R_1)^-1 R_extrinsic R_1
+ //
+ auto world_z_intrinsic = rotationVectorToQuaternion(
+ q.inverse() * Vector3f(0.f, 0.f, rot_angle));
+
+ // Sequential intrinsic rotations compose as R_1 * R_2.
+ quaternionToAngles(q * world_z_intrinsic, &pitch, &roll, &yaw);
+
+ EXPECT_NEAR(0.f, pitch, NEAR);
+ EXPECT_NEAR(angle, roll, NEAR);
+ }
+ }
+}
+
} // namespace
} // namespace media
} // namespace android