DeviceMotion.swift 3.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115
  1. /*****************************************************************************
  2. * DeviceMotion.swift
  3. * VLC for iOS
  4. *****************************************************************************
  5. * Copyright (c) 2018 VideoLAN. All rights reserved.
  6. * $Id$
  7. *
  8. * Authors: Carola Nitz <caro # videolan.org>
  9. *
  10. *
  11. * Refer to the COPYING file of the official project for license.
  12. *****************************************************************************/
  13. import Foundation
  14. import CoreMotion
  15. @objc(VLCDeviceMotionDelegate)
  16. protocol DeviceMotionDelegate:NSObjectProtocol {
  17. func deviceMotionHasAttitude(deviceMotion:DeviceMotion, pitch:Double, yaw:Double, roll:Double)
  18. }
  19. struct EulerAngles {
  20. var yaw: Double = 0
  21. var pitch: Double = 0
  22. var roll: Double = 0
  23. }
  24. @objc(VLCDeviceMotion)
  25. class DeviceMotion:NSObject {
  26. let motion = CMMotionManager()
  27. let sqrt2 = 0.5.squareRoot()
  28. @objc weak var delegate: DeviceMotionDelegate? = nil
  29. private func multQuaternion(q1: CMQuaternion, q2: CMQuaternion) -> CMQuaternion {
  30. var ret = CMQuaternion()
  31. ret.x = q1.x * q2.x - q1.y * q2.y - q1.z * q2.z - q1.w * q2.w
  32. ret.y = q1.x * q2.y + q1.y * q2.x + q1.z * q2.w - q1.w * q2.z
  33. ret.z = q1.x * q2.z + q1.z * q2.x - q1.y * q2.w + q1.w * q2.y
  34. ret.w = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y
  35. return ret
  36. }
  37. private func quaternionToEuler(qIn: CMQuaternion) -> EulerAngles {
  38. // Change the axes
  39. var q = CMQuaternion(x:qIn.y, y:qIn.z, z:qIn.x, w:qIn.w)
  40. // Rotation of 90°
  41. let qRot = CMQuaternion(x: 0, y: 0, z: -sqrt2 / 2, w: sqrt2 / 2)
  42. // Perform the rotation
  43. q = multQuaternion(q1:qRot, q2:q)
  44. // Now, we can perform the conversion and manage ourself the singularities
  45. let sqx = q.x * q.x
  46. let sqy = q.y * q.y
  47. let sqz = q.z * q.z
  48. let sqw = q.w * q.w
  49. let unit = sqx + sqy + sqz + sqw // if normalised is one, otherwise is correction factor
  50. let test = q.x * q.y + q.z * q.w
  51. var vp = EulerAngles()
  52. if (test > 0.499 * unit) {
  53. // singularity at north pole
  54. vp.yaw = 2 * atan2(q.x, q.w)
  55. vp.pitch = Double.pi / 2
  56. vp.roll = 0
  57. } else if (test < -0.499 * unit) {
  58. // singularity at south pole
  59. vp.yaw = -2 * atan2(q.x, q.w)
  60. vp.pitch = -Double.pi / 2
  61. vp.roll = 0
  62. } else {
  63. vp.yaw = atan2(2 * q.y * q.w - 2 * q.x * q.z, sqx - sqy - sqz + sqw)
  64. vp.pitch = asin(2 * test / unit)
  65. vp.roll = atan2(2 * q.x * q.w - 2 * q.y * q.z, -sqx + sqy - sqz + sqw)
  66. }
  67. vp.yaw = -vp.yaw * 180 / Double.pi
  68. vp.pitch = vp.pitch * 180 / Double.pi
  69. vp.roll = vp.roll * 180 / Double.pi
  70. return vp
  71. }
  72. @objc func startDeviceMotion() {
  73. if motion.isDeviceMotionAvailable {
  74. motion.gyroUpdateInterval = 1.0 / 60.0 // 60 Hz
  75. motion.startDeviceMotionUpdates(using: .xArbitraryZVertical, to: .main) {
  76. [weak self] (data, error) in
  77. guard let strongSelf = self, let data = data else {
  78. return
  79. }
  80. let euler = strongSelf.quaternionToEuler(qIn: data.attitude.quaternion)
  81. strongSelf.delegate?.deviceMotionHasAttitude(deviceMotion:strongSelf, pitch:euler.pitch, yaw:euler.yaw, roll:euler.roll)
  82. }
  83. }
  84. }
  85. @objc func stopDeviceMotion() {
  86. if motion.isDeviceMotionActive {
  87. motion.stopDeviceMotionUpdates()
  88. }
  89. }
  90. }