|
| 1 | +// |
| 2 | +// Quat4d.swift |
| 3 | +// |
| 4 | +// |
| 5 | +// Created by Christian Treffs on 01.11.20. |
| 6 | +// |
| 7 | + |
| 8 | +extension Quat4d { |
| 9 | + @inlinable public init(_ quat: Quat4f) { |
| 10 | + self.init(Double(quat.x), Double(quat.y), Double(quat.z), Double(quat.w)) |
| 11 | + } |
| 12 | + |
| 13 | + @inlinable public var normalized: Quat4d { |
| 14 | + normalize(self) |
| 15 | + } |
| 16 | + |
| 17 | + /// The length of the quaternion `q`. |
| 18 | + @inlinable public var length: Double { |
| 19 | + FirebladeMath.length(self) |
| 20 | + } |
| 21 | + |
| 22 | + /// Returns the axis about which a quaternion rotates. |
| 23 | + @inlinable public var axis: Vec3d { |
| 24 | + FirebladeMath.axis(self) |
| 25 | + } |
| 26 | + |
| 27 | + @inlinable public var angle: Double { |
| 28 | + FirebladeMath.angle(self) |
| 29 | + } |
| 30 | + |
| 31 | + @inlinable public var inverse: Quat4d { |
| 32 | + FirebladeMath.inverse(self) |
| 33 | + } |
| 34 | + |
| 35 | + @inlinable public var conjugate: Quat4d { |
| 36 | + FirebladeMath.conjugate(self) |
| 37 | + } |
| 38 | + |
| 39 | + @inlinable public var isNaN: Bool { |
| 40 | + x.isNaN || y.isNaN || z.isNaN || w.isNaN |
| 41 | + } |
| 42 | + |
| 43 | + public init(angle angleRadians: Double, axis: SIMD3<Double>) { |
| 44 | + self = quaternion(angle: angleRadians, axis: axis) |
| 45 | + } |
| 46 | + |
| 47 | + public init(rotation matrix: Mat3x3d) { |
| 48 | + self = quaternion(matrix: matrix) |
| 49 | + } |
| 50 | + |
| 51 | + public init(rotation matrix: Mat4x4d) { |
| 52 | + self = quaternion(matrix: matrix) |
| 53 | + } |
| 54 | + |
| 55 | + public init(from: SIMD3<Double>, to: SIMD3<Double>) { |
| 56 | + self = quaternion(from: from, to: to) |
| 57 | + } |
| 58 | + |
| 59 | + public init(yaw: Double, pitch: Double, roll: Double) { |
| 60 | + /// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles |
| 61 | + // Abbreviations for the various angular functions |
| 62 | + let cy: Double = cos(roll * 0.5) |
| 63 | + let sy: Double = sin(roll * 0.5) |
| 64 | + let cp: Double = cos(yaw * 0.5) |
| 65 | + let sp: Double = sin(yaw * 0.5) |
| 66 | + let cr: Double = cos(pitch * 0.5) |
| 67 | + let sr: Double = sin(pitch * 0.5) |
| 68 | + |
| 69 | + let w: Double = cy * cp * cr + sy * sp * sr |
| 70 | + let x: Double = cy * cp * sr - sy * sp * cr |
| 71 | + let y: Double = sy * cp * sr + cy * sp * cr |
| 72 | + let z: Double = sy * cp * cr - cy * sp * sr |
| 73 | + self.init(x, y, z, w) |
| 74 | + } |
| 75 | + |
| 76 | + /// Calculate the local yaw element of this quaternion in radians. |
| 77 | + /// |
| 78 | + /// Returns the 'intuitive' result that is, if you projected the local Z of the quaternion onto the ZX plane, |
| 79 | + /// the angle between it and global Z is returned. |
| 80 | + /// The co-domain of the returned value is from -180 to 180 degrees. |
| 81 | + /// |
| 82 | + /// Yaw would be left/right rotation around the Y axis (vertical) on the XZ plane. |
| 83 | + /// Yaw is used when driving a car. |
| 84 | + @inlinable public var yaw: Double { |
| 85 | + /// https://github.com/OGRECave/ogre/blob/master/OgreMain/src/OgreQuaternion.cpp#L508 |
| 86 | + asin(-2 * (x * z - w * y)) |
| 87 | + } |
| 88 | + |
| 89 | + /// Calculate the local pitch element of this quaternion in radians. |
| 90 | + /// |
| 91 | + /// Returns the 'intuitive' result that is, if you projected the local Y of the quaternion onto the YZ plane, |
| 92 | + /// the angle between it and global Y is returned. |
| 93 | + /// The co-domain of the returned value is from -180 to 180 degrees. |
| 94 | + /// |
| 95 | + /// Pitch is up/down rotation around the X axis (horizontal, pointing right) on the YZ plane. |
| 96 | + /// Pitch is used when flying a jet down or up, or when driving up hill or down. |
| 97 | + @inlinable public var pitch: Double { |
| 98 | + /// https://github.com/OGRECave/ogre/blob/master/OgreMain/src/OgreQuaternion.cpp#L484 |
| 99 | + atan2(2.0 * (y * z + w * x), w * w - x * x - y * y + z * z) |
| 100 | + } |
| 101 | + |
| 102 | + /// Calculate the local roll element of this quaternion in radians. |
| 103 | + /// |
| 104 | + /// Returns the 'intuitive' result that is, if you projected the local X of the quaternion onto the XY plane, |
| 105 | + /// the angle between it and global X is returned. |
| 106 | + /// The co-domain of the returned value is from -180 to 180 degrees. |
| 107 | + /// |
| 108 | + /// Roll is tilt rotation around the Z axis (pointing towards you) on the XY plane. |
| 109 | + /// Roll is literally what happens to your car when you take a curve too fast! |
| 110 | + @inlinable public var roll: Double { |
| 111 | + /// https://github.com/OGRECave/ogre/blob/master/OgreMain/src/OgreQuaternion.cpp#L459 |
| 112 | + atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z) |
| 113 | + } |
| 114 | + |
| 115 | + /// x: yaw, y: pitch, z: roll |
| 116 | + @inlinable public var eulerAngles: Vec3d { |
| 117 | + /// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles |
| 118 | + |
| 119 | + let sinrcosp: Double = +2.0 * (w * x + y * z) |
| 120 | + let cosrcosp: Double = +1.0 - 2.0 * (x * x + y * y) |
| 121 | + let pitch: Double = atan2(sinrcosp, cosrcosp) |
| 122 | + |
| 123 | + // y-axis rotation |
| 124 | + let sinp: Double = +2.0 * (w * y - z * x) |
| 125 | + let yaw: Double |
| 126 | + if abs(sinp) >= 1 { |
| 127 | + yaw = copysign(.pi / 2.0, sinp) // use 90 degrees if out of range |
| 128 | + } else { |
| 129 | + yaw = asin(sinp) |
| 130 | + } |
| 131 | + |
| 132 | + // z-axis rotation |
| 133 | + let sinycosp: Double = +2.0 * (w * z + x * y) |
| 134 | + let cosycosp: Double = +1.0 - 2.0 * (y * y + z * z) |
| 135 | + let roll = atan2(sinycosp, cosycosp) |
| 136 | + |
| 137 | + return Vec3d(yaw, pitch, roll) |
| 138 | + } |
| 139 | + |
| 140 | + /// Returns the rotation angle of the quaternion in radians. |
| 141 | + /// |
| 142 | + /// NOTE: DO NOT USE simd_angle() or .angle on the quaternion since it will always produce `3.1415927` |
| 143 | + @inlinable public var rotationAngle: Double { |
| 144 | + /// https://github.com/OGRECave/ogre/blob/master/OgreMain/src/OgreQuaternion.cpp#L126 |
| 145 | + |
| 146 | + // The quaternion representing the rotation is |
| 147 | + // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) |
| 148 | + let angle: Double |
| 149 | + let fSqrLength: Double = x * x + y * y + z * z |
| 150 | + if fSqrLength > 0.0 { |
| 151 | + angle = 2.0 * acos(w) |
| 152 | + } else { |
| 153 | + // angle is 0 (mod 2*pi), so any axis will do |
| 154 | + angle = radians(0.0) |
| 155 | + } |
| 156 | + |
| 157 | + return angle |
| 158 | + } |
| 159 | + |
| 160 | + /// The (multiplicative) inverse of the quaternion `q`. |
| 161 | + /*@inlinable public var inverse: Quat4d { |
| 162 | + return simd_inverse(self) |
| 163 | + }*/ |
| 164 | +} |
0 commit comments