From 049e46f51aeabefd8b066ad647d19c4b1152cd61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20=C3=96stlin?= Date: Tue, 1 Mar 2022 09:48:00 +0100 Subject: [PATCH 1/2] Fixes compatibility with CMDeviceMotion rotation matrix. Flips column/row of the generated CMRotationMatrix. Removes normalization --- .../Models/Attitude.swift | 31 +++++++------------ 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/Sources/ComposableCoreMotion/Models/Attitude.swift b/Sources/ComposableCoreMotion/Models/Attitude.swift index dc960d8..34f632e 100644 --- a/Sources/ComposableCoreMotion/Models/Attitude.swift +++ b/Sources/ComposableCoreMotion/Models/Attitude.swift @@ -24,26 +24,19 @@ public var rotationMatrix: CMRotationMatrix { let q = self.quaternion - let s = - 1 - / (self.quaternion.w * self.quaternion.w - + self.quaternion.x * self.quaternion.x - + self.quaternion.y * self.quaternion.y - + self.quaternion.z * self.quaternion.z) - var matrix = CMRotationMatrix() - - matrix.m11 = 1 - 2 * s * (q.y * q.y + q.z * q.z) - matrix.m12 = 2 * s * (q.x * q.y - q.z * q.w) - matrix.m13 = 2 * s * (q.x * q.z + q.y * q.w) - - matrix.m21 = 2 * s * (q.x * q.y + q.z * q.w) - matrix.m22 = 1 - 2 * s * (q.x * q.x + q.z * q.z) - matrix.m23 = 2 * s * (q.y * q.z - q.x * q.w) - - matrix.m31 = 2 * s * (q.x * q.z - q.y * q.w) - matrix.m32 = 2 * s * (q.y * q.z + q.x * q.w) - matrix.m33 = 1 - 2 * s * (q.x * q.x + q.y * q.y) + + matrix.m11 = 1 - 2 * (q.y * q.y + q.z * q.z) + matrix.m12 = 2 * (q.x * q.y + q.z * q.w) + matrix.m13 = 2 * (q.x * q.z - q.y * q.w) + + matrix.m21 = 2 * (q.x * q.y - q.z * q.w) + matrix.m22 = 1 - 2 * (q.x * q.x + q.z * q.z) + matrix.m23 = 2 * (q.y * q.z + q.x * q.w) + + matrix.m31 = 2 * (q.x * q.z + q.y * q.w) + matrix.m32 = 2 * (q.y * q.z - q.x * q.w) + matrix.m33 = 1 - 2 * (q.x * q.x + q.y * q.y) return matrix } From 2e7043c624976b2193958556084c68acf9ac51cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20=C3=96stlin?= Date: Wed, 2 Mar 2022 17:22:03 +0100 Subject: [PATCH 2/2] Flips pitch/roll for compatibility Adds some tests --- .../Models/Attitude.swift | 32 ++-- .../ComposableCoreMotionTests.swift | 8 +- .../RotationTests.swift | 177 ++++++++++++++++++ 3 files changed, 200 insertions(+), 17 deletions(-) create mode 100644 Tests/ComposableCoreMotionTests/RotationTests.swift diff --git a/Sources/ComposableCoreMotion/Models/Attitude.swift b/Sources/ComposableCoreMotion/Models/Attitude.swift index 34f632e..c737453 100644 --- a/Sources/ComposableCoreMotion/Models/Attitude.swift +++ b/Sources/ComposableCoreMotion/Models/Attitude.swift @@ -23,26 +23,32 @@ @inlinable public var rotationMatrix: CMRotationMatrix { let q = self.quaternion + + let s = 1 + / (self.quaternion.w * self.quaternion.w + + self.quaternion.x * self.quaternion.x + + self.quaternion.y * self.quaternion.y + + self.quaternion.z * self.quaternion.z) var matrix = CMRotationMatrix() - matrix.m11 = 1 - 2 * (q.y * q.y + q.z * q.z) - matrix.m12 = 2 * (q.x * q.y + q.z * q.w) - matrix.m13 = 2 * (q.x * q.z - q.y * q.w) + matrix.m11 = 1 - 2 * s * (q.y * q.y + q.z * q.z) + matrix.m12 = 2 * s * (q.x * q.y + q.z * q.w) + matrix.m13 = 2 * s * (q.x * q.z - q.y * q.w) - matrix.m21 = 2 * (q.x * q.y - q.z * q.w) - matrix.m22 = 1 - 2 * (q.x * q.x + q.z * q.z) - matrix.m23 = 2 * (q.y * q.z + q.x * q.w) + matrix.m21 = 2 * s * (q.x * q.y - q.z * q.w) + matrix.m22 = 1 - 2 * s * (q.x * q.x + q.z * q.z) + matrix.m23 = 2 * s * (q.y * q.z + q.x * q.w) - matrix.m31 = 2 * (q.x * q.z + q.y * q.w) - matrix.m32 = 2 * (q.y * q.z - q.x * q.w) - matrix.m33 = 1 - 2 * (q.x * q.x + q.y * q.y) + matrix.m31 = 2 * s * (q.x * q.z + q.y * q.w) + matrix.m32 = 2 * s * (q.y * q.z - q.x * q.w) + matrix.m33 = 1 - 2 * s * (q.x * q.x + q.y * q.y) return matrix } @inlinable - public var roll: Double { + public var pitch: Double { let q = self.quaternion return atan2( 2 * (q.w * q.x + q.y * q.z), @@ -50,12 +56,12 @@ ) } + @inlinable - public var pitch: Double { + public var roll: Double { let q = self.quaternion let p = 2 * (q.w * q.y - q.z * q.x) - return p > 1 - ? Double.pi / 2 + return p > 1 ? Double.pi / 2 : p < -1 ? -Double.pi / 2 : asin(p) diff --git a/Tests/ComposableCoreMotionTests/ComposableCoreMotionTests.swift b/Tests/ComposableCoreMotionTests/ComposableCoreMotionTests.swift index 7efdf35..eb78148 100644 --- a/Tests/ComposableCoreMotionTests/ComposableCoreMotionTests.swift +++ b/Tests/ComposableCoreMotionTests/ComposableCoreMotionTests.swift @@ -31,12 +31,12 @@ class ComposableCoreMotionTests: XCTestCase { let q3 = Attitude(quaternion: .init(x: 0, y: 0, z: 1, w: 0)) let q4 = Attitude(quaternion: .init(x: 0, y: 0, z: 0, w: 1)) - XCTAssertEqual(q1.roll, Double.pi) - XCTAssertEqual(q1.pitch, 0) + XCTAssertEqual(q1.roll, 0) + XCTAssertEqual(q1.pitch, Double.pi) XCTAssertEqual(q1.yaw, 0) - XCTAssertEqual(q2.roll, Double.pi) - XCTAssertEqual(q2.pitch, 0) + XCTAssertEqual(q2.roll, 0) + XCTAssertEqual(q2.pitch, Double.pi) XCTAssertEqual(q2.yaw, Double.pi) XCTAssertEqual(q3.roll, 0) diff --git a/Tests/ComposableCoreMotionTests/RotationTests.swift b/Tests/ComposableCoreMotionTests/RotationTests.swift new file mode 100644 index 0000000..4352358 --- /dev/null +++ b/Tests/ComposableCoreMotionTests/RotationTests.swift @@ -0,0 +1,177 @@ +import ComposableArchitecture +import CoreMotion +import XCTest + +@testable import ComposableCoreMotion + +class RotationTests: XCTestCase { + func makeQuaternion(_ pitch: Double, _ roll: Double, _ yaw: Double) -> CMQuaternion { + let qx = sin(pitch/2) * cos(roll/2) * cos(yaw/2) + let qy = cos(pitch/2) * sin(roll/2) * cos(yaw/2) + let qz = cos(pitch/2) * cos(roll/2) * sin(yaw/2) + let qw = cos(pitch/2) * cos(roll/2) * cos(yaw/2) + + let q = CMQuaternion(x: qx, y: qy, z: qz, w: qw) + return q + } + + func testMakeQuaternion() { + let q1 = makeQuaternion(.pi, 0, 0) + XCTAssertEqual(q1.x, 1, accuracy: 1e-15) + XCTAssertEqual(q1.y, 0, accuracy: 1e-15) + XCTAssertEqual(q1.z, 0, accuracy: 1e-15) + XCTAssertEqual(q1.w, 0, accuracy: 1e-15) + + let q2 = makeQuaternion(0,.pi, 0) + XCTAssertEqual(q2.x, 0, accuracy: 1e-15) + XCTAssertEqual(q2.y, 1, accuracy: 1e-15) + XCTAssertEqual(q2.z, 0, accuracy: 1e-15) + XCTAssertEqual(q2.w, 0, accuracy: 1e-15) + + let q3 = makeQuaternion(0, 0, .pi) + XCTAssertEqual(q3.x, 0, accuracy: 1e-15) + XCTAssertEqual(q3.y, 0, accuracy: 1e-15) + XCTAssertEqual(q3.z, 1, accuracy: 1e-15) + XCTAssertEqual(q3.w, 0, accuracy: 1e-15) + } + + func testPitch() { + let q = makeQuaternion(.pi/2, 0, 0) + let a = Attitude(quaternion: q) + + XCTAssertEqual(a.pitch, .pi/2, accuracy: 1e-15) + XCTAssertEqual(a.roll, 0) + XCTAssertEqual(a.yaw, 0) + + let r = a.rotationMatrix + + XCTAssertEqual(r.m11, 1, accuracy: 1e-15) + XCTAssertEqual(r.m21, 0, accuracy: 1e-15) + XCTAssertEqual(r.m31, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m12, 0, accuracy: 1e-15) + XCTAssertEqual(r.m22, 0, accuracy: 1e-15) + XCTAssertEqual(r.m32, -1, accuracy: 1e-15) + + XCTAssertEqual(r.m13, 0, accuracy: 1e-15) + XCTAssertEqual(r.m23, 1, accuracy: 1e-15) + XCTAssertEqual(r.m33, 0, accuracy: 1e-15) + + } + + func testPitch2() { + let q = makeQuaternion(.pi/4, 0, 0) + let a = Attitude(quaternion: q) + + XCTAssertEqual(a.pitch, .pi/4, accuracy: 1e-15) + XCTAssertEqual(a.roll, 0) + XCTAssertEqual(a.yaw, 0) + + let r = a.rotationMatrix + + XCTAssertEqual(r.m11, 1, accuracy: 1e-15) + XCTAssertEqual(r.m21, 0, accuracy: 1e-15) + XCTAssertEqual(r.m31, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m12, 0, accuracy: 1e-15) + XCTAssertEqual(r.m22, sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m32, -sqrt(2)/2, accuracy: 1e-15) + + XCTAssertEqual(r.m13, 0, accuracy: 1e-15) + XCTAssertEqual(r.m23, sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m33, sqrt(2)/2, accuracy: 1e-15) + + } + + func testRoll() { + let q = makeQuaternion(0, .pi/2, 0) + let a = Attitude(quaternion: q) + + XCTAssertEqual(a.pitch, 0) + XCTAssertEqual(a.roll, .pi/2, accuracy: 1e-15) + XCTAssertEqual(a.yaw, 0) + + let r = a.rotationMatrix + + XCTAssertEqual(r.m11, 0, accuracy: 1e-15) + XCTAssertEqual(r.m21, 0, accuracy: 1e-15) + XCTAssertEqual(r.m31, 1, accuracy: 1e-15) + + XCTAssertEqual(r.m12, 0, accuracy: 1e-15) + XCTAssertEqual(r.m22, 1, accuracy: 1e-15) + XCTAssertEqual(r.m32, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m13, -1, accuracy: 1e-15) + XCTAssertEqual(r.m23, 0, accuracy: 1e-15) + XCTAssertEqual(r.m33, 0, accuracy: 1e-15) + } + + func testRoll2() { + let q = makeQuaternion(0, .pi/4, 0) + let a = Attitude(quaternion: q) + + XCTAssertEqual(a.pitch, 0) + XCTAssertEqual(a.roll, .pi/4, accuracy: 1e-15) + XCTAssertEqual(a.yaw, 0) + + let r = a.rotationMatrix + + XCTAssertEqual(r.m11, sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m21, 0, accuracy: 1e-15) + XCTAssertEqual(r.m31, sqrt(2)/2, accuracy: 1e-15) + + XCTAssertEqual(r.m12, 0, accuracy: 1e-15) + XCTAssertEqual(r.m22, 1, accuracy: 1e-15) + XCTAssertEqual(r.m32, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m13, -sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m23, 0, accuracy: 1e-15) + XCTAssertEqual(r.m33, sqrt(2)/2, accuracy: 1e-15) + } + + func testYaw() { + let q = makeQuaternion(0, 0, .pi/2) + let a = Attitude(quaternion: q) + + XCTAssertEqual(a.pitch, 0) + XCTAssertEqual(a.roll, 0) + XCTAssertEqual(a.yaw, .pi/2, accuracy: 1e-15) + + let r = a.rotationMatrix + + XCTAssertEqual(r.m11, 0, accuracy: 1e-15) + XCTAssertEqual(r.m21, -1, accuracy: 1e-15) + XCTAssertEqual(r.m31, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m12, 1, accuracy: 1e-15) + XCTAssertEqual(r.m22, 0, accuracy: 1e-15) + XCTAssertEqual(r.m32, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m13, 0, accuracy: 1e-15) + XCTAssertEqual(r.m23, 0, accuracy: 1e-15) + XCTAssertEqual(r.m33, 1, accuracy: 1e-15) + } + + func testYaw2() { + let q = makeQuaternion(0, 0, .pi/4) + let a = Attitude(quaternion: q) + + XCTAssertEqual(a.pitch, 0) + XCTAssertEqual(a.roll, 0) + XCTAssertEqual(a.yaw, .pi/4, accuracy: 1e-15) + + let r = a.rotationMatrix + + XCTAssertEqual(r.m11, sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m21, -sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m31, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m12, sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m22, sqrt(2)/2, accuracy: 1e-15) + XCTAssertEqual(r.m32, 0, accuracy: 1e-15) + + XCTAssertEqual(r.m13, 0, accuracy: 1e-15) + XCTAssertEqual(r.m23, 0, accuracy: 1e-15) + XCTAssertEqual(r.m33, 1, accuracy: 1e-15) + } +}