Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

#2 Fixes bases of rotation matrix and pitch and roll axes #10

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

#2 Fixes bases of rotation matrix and pitch and roll axes #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
049e46f
Fixes compatibility with CMDeviceMotion rotation matrix.
barksten Mar 1, 2022
2e7043c
Flips pitch/roll for compatibility
barksten Mar 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 17 additions & 18 deletions Sources/ComposableCoreMotion/Models/Attitude.swift
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,46 +23,45 @@
@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)

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.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.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.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.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),
1 - 2 * (q.x * q.x + q.y * q.y)
)
}


@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)
Expand Down
8 changes: 4 additions & 4 deletions Tests/ComposableCoreMotionTests/ComposableCoreMotionTests.swift
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
177 changes: 177 additions & 0 deletions Tests/ComposableCoreMotionTests/RotationTests.swift
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)
}
}