Account for underflow in quaternion rotation

Author: markuswntr

Sources/QuaternionModule/Transformation.swift

@@ -352,12 +352,12 @@ extension Quaternion {
   ///
   ///     p' = qpq⁻¹
   ///
-  /// where `p` is a *pure* quaternion (`real == .zero`) with imaginary part equal
-  /// to vector `v`, and where `p'` is another pure quaternion with imaginary
-  /// part equal to the transformed vector `v'`. The implementation uses this
-  /// formular but boils down to a simpler and faster implementation as `p` is
-  /// known to be pure and `q` is assumed to have unit length – which allows
-  /// simplification.
+  /// where `p` is a *pure* quaternion (`real == .zero`) with imaginary part
+  /// equal to vector `v`, and where `p'` is another pure quaternion with
+  /// imaginary part equal to the transformed vector `v'`. The implementation
+  /// uses this formular but boils down to a simpler and faster implementation
+  /// as `p` is known to be pure and `q` is assumed to have unit length – which
+  /// allows for simplification.
   ///
   /// - Note: This method assumes this quaternion is of unit length.
   ///
@@ -369,6 +369,11 @@ extension Quaternion {
   ///   ```
   ///   SIMD3(-.infinity,0,0) * q == SIMD3(.infinity,.infinity,.infinity)
   ///   ```
+  /// - For any quaternion `q`, even `.zero` or `.infinity`, if `vector` is
+  /// `.zero`, the returning vector is also `.zero`.
+  ///   ```
+  ///   SIMD3(0,0,0) * q == .zero
+  ///   ```
   ///
   /// - Parameter vector: A vector to rotate by this quaternion
   /// - Returns: The vector rotated by this quaternion
@@ -377,17 +382,20 @@ extension Quaternion {
   @inlinable
   public func act(on vector: SIMD3<RealType>) -> SIMD3<RealType> {
     guard vector.isFinite else { return SIMD3(repeating: .infinity) }
+    guard vector != .zero else { return .zero }
 
     // The following expression have been split up so the type-checker
     // can resolve them in a reasonable time.
     let p1 = vector * (real*real - imaginary.lengthSquared)
     let p2 = 2 * imaginary * imaginary.dot(vector)
     let p3 = 2 * real * imaginary.cross(vector)
     let rotatedVector = p1 + p2 + p3
-    if rotatedVector.isFinite { return rotatedVector }
 
-    // If the vector is no longer finite after it is rotated, scale it down,
-    // rotate it again and then scale it back-up after the rotation operation
+    // If the rotation computes without over/underflow, everything is fine
+    // and the result is correct. If not, we have to do the computation
+    // carefully and first unscale the vector, rotate it again and then
+    // rescale the vector
+    if rotatedVector.isNormal { return rotatedVector }
     let scale = max(abs(vector.max()), abs(vector.min()))
     return act(on: vector/scale) * scale
   }
@@ -451,6 +459,12 @@ extension SIMD3 where Scalar: FloatingPoint {
     x.isFinite && y.isFinite && z.isFinite
   }
 
+  /// True if all values of this instance are finite
+  @usableFromInline @inline(__always)
+  internal var isNormal: Bool {
+    x.isNormal && y.isNormal && z.isNormal
+  }
+
   /// Returns the squared length of this instance.
   @usableFromInline @inline(__always)
   internal var lengthSquared: Scalar {

Tests/QuaternionTests/TransformationTests.swift

@@ -291,30 +291,35 @@ final class TransformationTests: XCTestCase {
     testActOnVectorRandom(Float64.self)
   }
 
-  func testActOnVectorEdgeCase<T: Real & ExpressibleByFloatLiteral & SIMDScalar>(_ type: T.Type) {
+  func testActOnVectorEdgeCase<T: Real & SIMDScalar>(_ type: T.Type) {
 
     /// Test for zero, infinity
     let q = Quaternion(angle: .pi, axis: SIMD3(1,0,0))
     XCTAssertEqual(q.act(on:  .zero), .zero)
     XCTAssertEqual(q.act(on: -.zero), .zero)
+    XCTAssertEqual(q.act(on:  .nan     ), SIMD3(repeating: .infinity))
     XCTAssertEqual(q.act(on:  .infinity), SIMD3(repeating: .infinity))
     XCTAssertEqual(q.act(on: -.infinity), SIMD3(repeating: .infinity))
+  }
 
-    // Rotate a vector with a value close to greatestFiniteMagnitude
-    // in all lanes.
-    // A vector this close to the bounds should not hit infinity when it
-    // is rotate by a perpendicular axis with an angle that is a multiple of π
+  func testActOnVectorEdgeCase() {
+    testActOnVectorEdgeCase(Float32.self)
+    testActOnVectorEdgeCase(Float64.self)
+  }
 
-    // An axis perpendicular to the vector, so all lanes are changing equally
-    let axis = SIMD3<T>(1,0,-1) / .sqrt(2)
-    // Create a value (somewhat) close to .greatestFiniteMagnitude
+  func testActOnVectorOverflow<T: Real & ExpressibleByFloatLiteral & SIMDScalar>(_ type: T.Type) {
+    // Create a vector (somewhat) close to greatestFiniteMagnitude on all lanes.
+    // We can not use greatestFiniteMagnitude here to test the careful rotation
+    // path, as we lose some precision in the process and it will overflow after
+    // rescaling the vector.
     let scalar = T(
       sign: .plus, exponent: T.greatestFiniteMagnitude.exponent,
       significand: 1.999999
     )
-
     let closeToBounds = SIMD3<T>(repeating: scalar)
 
+    // An axis perpendicular to the vector, so all lanes change equally
+    let axis = SIMD3<T>(1,0,-1) / .sqrt(2)
     // Perform a 180° rotation on all components
     let pi = Quaternion(angle: .pi, axis: axis).act(on: closeToBounds)
     // Must be finite after the rotation
@@ -324,21 +329,32 @@ final class TransformationTests: XCTestCase {
     XCTAssertTrue(closeEnough(pi.x, -scalar, ulps: 4))
     XCTAssertTrue(closeEnough(pi.y, -scalar, ulps: 4))
     XCTAssertTrue(closeEnough(pi.z, -scalar, ulps: 4))
+  }
 
-    // Perform a 360° rotation on all components
-    let twoPi = Quaternion(angle: 2 * .pi, axis: axis).act(on: closeToBounds)
-    // Must still be finite after the process
-    XCTAssertTrue(twoPi.x.isFinite)
-    XCTAssertTrue(twoPi.y.isFinite)
-    XCTAssertTrue(twoPi.z.isFinite)
-    XCTAssertTrue(closeEnough(twoPi.x, scalar, ulps: 8))
-    XCTAssertTrue(closeEnough(twoPi.y, scalar, ulps: 8))
-    XCTAssertTrue(closeEnough(twoPi.z, scalar, ulps: 8))
+  func testActOnVectorOverflow() {
+    testActOnVectorOverflow(Float32.self)
+    testActOnVectorOverflow(Float64.self)
   }
 
-  func testActOnVectorEdgeCase() {
-    testActOnVectorEdgeCase(Float32.self)
-    testActOnVectorEdgeCase(Float64.self)
+  func testActOnVectorUnderflow<T: Real & ExpressibleByFloatLiteral & SIMDScalar>(_ type: T.Type) {
+    let scalar = T.leastNormalMagnitude
+    let closeToZero = SIMD3<T>(repeating: scalar)
+    // An axis perpendicular to the vector, so all lanes change equally
+    let axis = SIMD3<T>(1,0,-1) / .sqrt(2)
+    // Perform a 180° rotation on all components
+    let pi = Quaternion(angle: .pi, axis: axis).act(on: closeToZero)
+    // Must be finite after the rotation
+    XCTAssertTrue(pi.x.isFinite)
+    XCTAssertTrue(pi.y.isFinite)
+    XCTAssertTrue(pi.z.isFinite)
+    XCTAssertTrue(closeEnough(pi.x, -scalar, ulps: 2))
+    XCTAssertTrue(closeEnough(pi.y, -scalar, ulps: 2))
+    XCTAssertTrue(closeEnough(pi.z, -scalar, ulps: 2))
+  }
+
+  func testActOnVectorUnderflow() {
+    testActOnVectorUnderflow(Float32.self)
+    testActOnVectorUnderflow(Float64.self)
   }
 }