diff --git a/.github/workflows/ci-macos.yml b/.github/workflows/ci-macos.yml index 08d7893..a23b79c 100644 --- a/.github/workflows/ci-macos.yml +++ b/.github/workflows/ci-macos.yml @@ -33,5 +33,10 @@ jobs: - name: Run Tests (Release) run: swift test -c release --parallel -Xswiftc -warnings-as-errors + # Without the simd trait, FirebladeMath uses its portable implementations while Apple's + # simd is still importable, so PortableVsSIMDTests compares the two. + - name: Run Tests (Release, portable path vs Apple simd) + run: swift test -c release --disable-default-traits --parallel -Xswiftc -warnings-as-errors + - name: Check @inlinable Coverage run: Scripts/check-inlinable.sh diff --git a/Sources/FirebladeMath/Functions/adjugate.swift b/Sources/FirebladeMath/Functions/adjugate.swift index 9ce93e0..1da56d2 100644 --- a/Sources/FirebladeMath/Functions/adjugate.swift +++ b/Sources/FirebladeMath/Functions/adjugate.swift @@ -47,64 +47,7 @@ public func adjugate(_ inMat: Mat3x3d) -> Mat3x3d { /// - Returns: The adjugate of the input matrix. @inlinable public func adjugate(_ matrix: Mat4x4f) -> Mat4x4f { - // Adjugate is the transpose of the cofactor matrix. - // cofactor(i, j) = (-1)^(i+j) * minor(i, j) - // Here we use matrix[col, row] indexing. - var res = Mat4x4f.identity - - res[0, 0] = matrix[1, 1] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 1] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - + matrix[3, 1] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2]) - res[1, 0] = -(matrix[1, 0] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 0] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - + matrix[3, 0] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2])) - res[2, 0] = matrix[1, 0] * (matrix[2, 1] * matrix[3, 3] - matrix[2, 3] * matrix[3, 1]) - - matrix[2, 0] * (matrix[1, 1] * matrix[3, 3] - matrix[1, 3] * matrix[3, 1]) - + matrix[3, 0] * (matrix[1, 1] * matrix[2, 3] - matrix[1, 3] * matrix[2, 1]) - res[3, 0] = -(matrix[1, 0] * (matrix[2, 1] * matrix[3, 2] - matrix[2, 2] * matrix[3, 1]) - - matrix[2, 0] * (matrix[1, 1] * matrix[3, 2] - matrix[1, 2] * matrix[3, 1]) - + matrix[3, 0] * (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1])) - - res[0, 1] = -(matrix[0, 1] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 1] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 1] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2])) - res[1, 1] = matrix[0, 0] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 0] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 0] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2]) - res[2, 1] = -(matrix[0, 0] * (matrix[2, 1] * matrix[3, 3] - matrix[2, 3] * matrix[3, 1]) - - matrix[2, 0] * (matrix[0, 1] * matrix[3, 3] - matrix[0, 3] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[2, 3] - matrix[0, 3] * matrix[2, 1])) - res[3, 1] = matrix[0, 0] * (matrix[2, 1] * matrix[3, 2] - matrix[2, 2] * matrix[3, 1]) - - matrix[2, 0] * (matrix[0, 1] * matrix[3, 2] - matrix[0, 2] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[2, 2] - matrix[0, 2] * matrix[2, 1]) - - res[0, 2] = matrix[0, 1] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - - matrix[1, 1] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 1] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2]) - res[1, 2] = -(matrix[0, 0] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - - matrix[1, 0] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 0] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2])) - res[2, 2] = matrix[0, 0] * (matrix[1, 1] * matrix[3, 3] - matrix[1, 3] * matrix[3, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[3, 3] - matrix[0, 3] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[1, 3] - matrix[0, 3] * matrix[1, 1]) - res[3, 2] = -(matrix[0, 0] * (matrix[1, 1] * matrix[3, 2] - matrix[1, 2] * matrix[3, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[3, 2] - matrix[0, 2] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[1, 2] - matrix[0, 2] * matrix[1, 1])) - - res[0, 3] = -(matrix[0, 1] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2]) - - matrix[1, 1] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2]) - + matrix[2, 1] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2])) - res[1, 3] = matrix[0, 0] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2]) - - matrix[1, 0] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2]) - + matrix[2, 0] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2]) - res[2, 3] = -(matrix[0, 0] * (matrix[1, 1] * matrix[2, 3] - matrix[1, 3] * matrix[2, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[2, 3] - matrix[0, 3] * matrix[2, 1]) - + matrix[2, 0] * (matrix[0, 1] * matrix[1, 3] - matrix[0, 3] * matrix[1, 1])) - res[3, 3] = matrix[0, 0] * (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[2, 2] - matrix[0, 2] * matrix[2, 1]) - + matrix[2, 0] * (matrix[0, 1] * matrix[1, 2] - matrix[0, 2] * matrix[1, 1]) - - return res + adjugateAndDeterminant(matrix).adjugate } /// Computes the adjugate of a 4x4 matrix. @@ -112,59 +55,5 @@ public func adjugate(_ matrix: Mat4x4f) -> Mat4x4f { /// - Returns: The adjugate of the input matrix. @inlinable public func adjugate(_ matrix: Mat4x4d) -> Mat4x4d { - var res = Mat4x4d.identity - - res[0, 0] = matrix[1, 1] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 1] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - + matrix[3, 1] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2]) - res[1, 0] = -(matrix[1, 0] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 0] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - + matrix[3, 0] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2])) - res[2, 0] = matrix[1, 0] * (matrix[2, 1] * matrix[3, 3] - matrix[2, 3] * matrix[3, 1]) - - matrix[2, 0] * (matrix[1, 1] * matrix[3, 3] - matrix[1, 3] * matrix[3, 1]) - + matrix[3, 0] * (matrix[1, 1] * matrix[2, 3] - matrix[1, 3] * matrix[2, 1]) - res[3, 0] = -(matrix[1, 0] * (matrix[2, 1] * matrix[3, 2] - matrix[2, 2] * matrix[3, 1]) - - matrix[2, 0] * (matrix[1, 1] * matrix[3, 2] - matrix[1, 2] * matrix[3, 1]) - + matrix[3, 0] * (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1])) - - res[0, 1] = -(matrix[0, 1] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 1] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 1] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2])) - res[1, 1] = matrix[0, 0] * (matrix[2, 2] * matrix[3, 3] - matrix[2, 3] * matrix[3, 2]) - - matrix[2, 0] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 0] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2]) - res[2, 1] = -(matrix[0, 0] * (matrix[2, 1] * matrix[3, 3] - matrix[2, 3] * matrix[3, 1]) - - matrix[2, 0] * (matrix[0, 1] * matrix[3, 3] - matrix[0, 3] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[2, 3] - matrix[0, 3] * matrix[2, 1])) - res[3, 1] = matrix[0, 0] * (matrix[2, 1] * matrix[3, 2] - matrix[2, 2] * matrix[3, 1]) - - matrix[2, 0] * (matrix[0, 1] * matrix[3, 2] - matrix[0, 2] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[2, 2] - matrix[0, 2] * matrix[2, 1]) - - res[0, 2] = matrix[0, 1] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - - matrix[1, 1] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 1] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2]) - res[1, 2] = -(matrix[0, 0] * (matrix[1, 2] * matrix[3, 3] - matrix[1, 3] * matrix[3, 2]) - - matrix[1, 0] * (matrix[0, 2] * matrix[3, 3] - matrix[0, 3] * matrix[3, 2]) - + matrix[3, 0] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2])) - res[2, 2] = matrix[0, 0] * (matrix[1, 1] * matrix[3, 3] - matrix[1, 3] * matrix[3, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[3, 3] - matrix[0, 3] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[1, 3] - matrix[0, 3] * matrix[1, 1]) - res[3, 2] = -(matrix[0, 0] * (matrix[1, 1] * matrix[3, 2] - matrix[1, 2] * matrix[3, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[3, 2] - matrix[0, 2] * matrix[3, 1]) - + matrix[3, 0] * (matrix[0, 1] * matrix[1, 2] - matrix[0, 2] * matrix[1, 1])) - - res[0, 3] = -(matrix[0, 1] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2]) - - matrix[1, 1] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2]) - + matrix[2, 1] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2])) - res[1, 3] = matrix[0, 0] * (matrix[1, 2] * matrix[2, 3] - matrix[1, 3] * matrix[2, 2]) - - matrix[1, 0] * (matrix[0, 2] * matrix[2, 3] - matrix[0, 3] * matrix[2, 2]) - + matrix[2, 0] * (matrix[0, 2] * matrix[1, 3] - matrix[0, 3] * matrix[1, 2]) - res[2, 3] = -(matrix[0, 0] * (matrix[1, 1] * matrix[2, 3] - matrix[1, 3] * matrix[2, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[2, 3] - matrix[0, 3] * matrix[2, 1]) - + matrix[2, 0] * (matrix[0, 1] * matrix[1, 3] - matrix[0, 3] * matrix[1, 1])) - res[3, 3] = matrix[0, 0] * (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1]) - - matrix[1, 0] * (matrix[0, 1] * matrix[2, 2] - matrix[0, 2] * matrix[2, 1]) - + matrix[2, 0] * (matrix[0, 1] * matrix[1, 2] - matrix[0, 2] * matrix[1, 1]) - - return res + adjugateAndDeterminant(matrix).adjugate } diff --git a/Sources/FirebladeMath/Functions/determinant.swift b/Sources/FirebladeMath/Functions/determinant.swift index de7354c..f338481 100644 --- a/Sources/FirebladeMath/Functions/determinant.swift +++ b/Sources/FirebladeMath/Functions/determinant.swift @@ -10,27 +10,7 @@ public func determinant(_ mat: Mat4x4f) -> Float { #if FRB_MATH_USE_SIMD return simd.simd_determinant(mat.storage) #else - let subFactor00 = mat[1, 1] * (mat[2, 2] * mat[3, 3] - mat[2, 3] * mat[3, 2]) - let subFactor01 = mat[1, 2] * (mat[2, 1] * mat[3, 3] - mat[2, 3] * mat[3, 1]) - let subFactor02 = mat[1, 3] * (mat[2, 1] * mat[3, 2] - mat[2, 2] * mat[3, 1]) - let det0 = mat[0, 0] * (subFactor00 - subFactor01 + subFactor02) - - let subFactor10 = mat[1, 0] * (mat[2, 2] * mat[3, 3] - mat[2, 3] * mat[3, 2]) - let subFactor11 = mat[1, 2] * (mat[2, 0] * mat[3, 3] - mat[2, 3] * mat[3, 0]) - let subFactor12 = mat[1, 3] * (mat[2, 0] * mat[3, 2] - mat[2, 2] * mat[3, 0]) - let det1 = mat[0, 1] * (subFactor10 - subFactor11 + subFactor12) - - let subFactor20 = mat[1, 0] * (mat[2, 1] * mat[3, 3] - mat[2, 3] * mat[3, 1]) - let subFactor21 = mat[1, 1] * (mat[2, 0] * mat[3, 3] - mat[2, 3] * mat[3, 0]) - let subFactor22 = mat[1, 3] * (mat[2, 0] * mat[3, 1] - mat[2, 1] * mat[3, 0]) - let det2 = mat[0, 2] * (subFactor20 - subFactor21 + subFactor22) - - let subFactor30 = mat[1, 0] * (mat[2, 1] * mat[3, 2] - mat[2, 2] * mat[3, 1]) - let subFactor31 = mat[1, 1] * (mat[2, 0] * mat[3, 2] - mat[2, 2] * mat[3, 0]) - let subFactor32 = mat[1, 2] * (mat[2, 0] * mat[3, 1] - mat[2, 1] * mat[3, 0]) - let det3 = mat[0, 3] * (subFactor30 - subFactor31 + subFactor32) - - return det0 - det1 + det2 - det3 + return adjugateAndDeterminant(mat).determinant #endif } @@ -57,27 +37,7 @@ public func determinant(_ mat: Mat4x4d) -> Double { #if FRB_MATH_USE_SIMD return simd.simd_determinant(mat.storage) #else - let subFactor00 = mat[1, 1] * (mat[2, 2] * mat[3, 3] - mat[2, 3] * mat[3, 2]) - let subFactor01 = mat[1, 2] * (mat[2, 1] * mat[3, 3] - mat[2, 3] * mat[3, 1]) - let subFactor02 = mat[1, 3] * (mat[2, 1] * mat[3, 2] - mat[2, 2] * mat[3, 1]) - let det0 = mat[0, 0] * (subFactor00 - subFactor01 + subFactor02) - - let subFactor10 = mat[1, 0] * (mat[2, 2] * mat[3, 3] - mat[2, 3] * mat[3, 2]) - let subFactor11 = mat[1, 2] * (mat[2, 0] * mat[3, 3] - mat[2, 3] * mat[3, 0]) - let subFactor12 = mat[1, 3] * (mat[2, 0] * mat[3, 2] - mat[2, 2] * mat[3, 0]) - let det1 = mat[0, 1] * (subFactor10 - subFactor11 + subFactor12) - - let subFactor20 = mat[1, 0] * (mat[2, 1] * mat[3, 3] - mat[2, 3] * mat[3, 1]) - let subFactor21 = mat[1, 1] * (mat[2, 0] * mat[3, 3] - mat[2, 3] * mat[3, 0]) - let subFactor22 = mat[1, 3] * (mat[2, 0] * mat[3, 1] - mat[2, 1] * mat[3, 0]) - let det2 = mat[0, 2] * (subFactor20 - subFactor21 + subFactor22) - - let subFactor30 = mat[1, 0] * (mat[2, 1] * mat[3, 2] - mat[2, 2] * mat[3, 1]) - let subFactor31 = mat[1, 1] * (mat[2, 0] * mat[3, 2] - mat[2, 2] * mat[3, 0]) - let subFactor32 = mat[1, 2] * (mat[2, 0] * mat[3, 1] - mat[2, 1] * mat[3, 0]) - let det3 = mat[0, 3] * (subFactor30 - subFactor31 + subFactor32) - - return det0 - det1 + det2 - det3 + return adjugateAndDeterminant(mat).determinant #endif } diff --git a/Sources/FirebladeMath/Functions/inverse.swift b/Sources/FirebladeMath/Functions/inverse.swift index 5e19251..4b91fc8 100644 --- a/Sources/FirebladeMath/Functions/inverse.swift +++ b/Sources/FirebladeMath/Functions/inverse.swift @@ -38,8 +38,7 @@ public func inverse(_ matrix: Mat4x4f) -> Mat4x4f { #if FRB_MATH_USE_SIMD return Mat4x4f(storage: simd.simd_inverse(matrix.storage)) #else - let adj: Mat4x4f = adjugate(matrix) - let det: Float = determinant(matrix) + let (adj, det) = adjugateAndDeterminant(matrix) return multiply(1.0 / det, adj) #endif } @@ -52,8 +51,7 @@ public func inverse(_ matrix: Mat4x4d) -> Mat4x4d { #if FRB_MATH_USE_SIMD return Mat4x4d(storage: simd.simd_inverse(matrix.storage)) #else - let adj: Mat4x4d = adjugate(matrix) - let det: Double = determinant(matrix) + let (adj, det) = adjugateAndDeterminant(matrix) return multiply(1.0 / det, adj) #endif } diff --git a/Sources/FirebladeMath/Matrix/Matrix4x4+Cofactors.swift b/Sources/FirebladeMath/Matrix/Matrix4x4+Cofactors.swift new file mode 100644 index 0000000..8e62f82 --- /dev/null +++ b/Sources/FirebladeMath/Matrix/Matrix4x4+Cofactors.swift @@ -0,0 +1,71 @@ +/// Computes the adjugate and the determinant of a 4x4 matrix in one pass. +/// +/// Uses the Laplace expansion by 2x2 minors: the six 2x2 sub-determinants of the first two rows +/// (`s0...s5`) and of the last two rows (`c0...c5`) are computed once and shared by all sixteen +/// cofactors and the determinant, instead of re-deriving each 3x3 minor from scratch. +/// +/// - Parameter matrix: The matrix. +/// - Returns: The adjugate (transposed cofactor matrix) and the determinant of `matrix`. +@inlinable +func adjugateAndDeterminant(_ matrix: Matrix4x4) -> (adjugate: Matrix4x4, determinant: Storage.Value) { // swiftlint:disable:this explicit_top_level_acl + typealias Value = Storage.Value + typealias Vector = Matrix4x4.Vector + + // Every intermediate is explicitly typed and kept short: older compilers (Swift 6.1/6.2) + // otherwise fail to type-check these generic expressions in reasonable time. + + // Row-major names: aRC is the element in row R, column C. + let (col0, col1, col2, col3) = matrix.columns + let a00: Value = col0.x, a10: Value = col0.y, a20: Value = col0.z, a30: Value = col0.w + let a01: Value = col1.x, a11: Value = col1.y, a21: Value = col1.z, a31: Value = col1.w + let a02: Value = col2.x, a12: Value = col2.y, a22: Value = col2.z, a32: Value = col2.w + let a03: Value = col3.x, a13: Value = col3.y, a23: Value = col3.z, a33: Value = col3.w + + let s0: Value = a00 * a11 - a10 * a01 + let s1: Value = a00 * a12 - a10 * a02 + let s2: Value = a00 * a13 - a10 * a03 + let s3: Value = a01 * a12 - a11 * a02 + let s4: Value = a01 * a13 - a11 * a03 + let s5: Value = a02 * a13 - a12 * a03 + + let c5: Value = a22 * a33 - a32 * a23 + let c4: Value = a21 * a33 - a31 * a23 + let c3: Value = a21 * a32 - a31 * a22 + let c2: Value = a20 * a33 - a30 * a23 + let c1: Value = a20 * a32 - a30 * a22 + let c0: Value = a20 * a31 - a30 * a21 + + let determinant01: Value = s0 * c5 - s1 * c4 + let determinant23: Value = s2 * c3 + s3 * c2 + let determinant45: Value = s5 * c0 - s4 * c1 + let determinant: Value = determinant01 + determinant23 + determinant45 + + // bRC is the adjugate element in row R, column C; the matrix is built from its columns. + let b00: Value = a11 * c5 - a12 * c4 + a13 * c3 + let b10: Value = -a10 * c5 + a12 * c2 - a13 * c1 + let b20: Value = a10 * c4 - a11 * c2 + a13 * c0 + let b30: Value = -a10 * c3 + a11 * c1 - a12 * c0 + + let b01: Value = -a01 * c5 + a02 * c4 - a03 * c3 + let b11: Value = a00 * c5 - a02 * c2 + a03 * c1 + let b21: Value = -a00 * c4 + a01 * c2 - a03 * c0 + let b31: Value = a00 * c3 - a01 * c1 + a02 * c0 + + let b02: Value = a31 * s5 - a32 * s4 + a33 * s3 + let b12: Value = -a30 * s5 + a32 * s2 - a33 * s1 + let b22: Value = a30 * s4 - a31 * s2 + a33 * s0 + let b32: Value = -a30 * s3 + a31 * s1 - a32 * s0 + + let b03: Value = -a21 * s5 + a22 * s4 - a23 * s3 + let b13: Value = a20 * s5 - a22 * s2 + a23 * s1 + let b23: Value = -a20 * s4 + a21 * s2 - a23 * s0 + let b33: Value = a20 * s3 - a21 * s1 + a22 * s0 + + let adjugate = Matrix4x4( + Vector(b00, b10, b20, b30), + Vector(b01, b11, b21, b31), + Vector(b02, b12, b22, b32), + Vector(b03, b13, b23, b33) + ) + return (adjugate, determinant) +} diff --git a/Sources/FirebladeMath/Quat/Quaternion+Multiplication.swift b/Sources/FirebladeMath/Quat/Quaternion+Multiplication.swift index f2cb143..6901fcf 100644 --- a/Sources/FirebladeMath/Quat/Quaternion+Multiplication.swift +++ b/Sources/FirebladeMath/Quat/Quaternion+Multiplication.swift @@ -33,11 +33,16 @@ public func multiply(_ lhs: Quat4f, _ rhs: Quat4f) -> Quat4f { #if FRB_MATH_USE_SIMD return Quat4f(storage: simd.simd_mul(lhs.storage, rhs.storage)) #else - let n0 = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z - let n1 = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y - let n2 = lhs.w * rhs.y - lhs.x * rhs.z + lhs.y * rhs.w + lhs.z * rhs.x - let n3 = lhs.w * rhs.z + lhs.x * rhs.y - lhs.y * rhs.x + lhs.z * rhs.w - return Quat4f(n1, n2, n3, n0) + // Hamilton product as four 4-wide multiply-adds over the [x, y, z, w] lanes. + // Terms are explicitly typed so older compilers type-check them in reasonable time. + let lv: Vec4f = lhs.storage.storage + let rv: Vec4f = rhs.storage.storage + let term0: Vec4f = Vec4f(repeating: lv.w) * rv + let term1: Vec4f = Vec4f(lv.x, lv.y, lv.z, lv.x) * Vec4f(rv.w, rv.w, rv.w, -rv.x) + let term2: Vec4f = Vec4f(lv.y, lv.z, lv.x, lv.y) * Vec4f(rv.z, rv.x, rv.y, -rv.y) + let term3: Vec4f = Vec4f(lv.z, lv.x, lv.y, lv.z) * Vec4f(rv.y, rv.z, rv.x, rv.z) + let product: Vec4f = (term0 + term1) + (term2 - term3) + return Quat4f(product) #endif } @@ -135,11 +140,16 @@ public func multiply(_ lhs: Quat4d, _ rhs: Quat4d) -> Quat4d { #if FRB_MATH_USE_SIMD return Quat4d(storage: simd.simd_mul(lhs.storage, rhs.storage)) #else - let n0 = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z - let n1 = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y - let n2 = lhs.w * rhs.y - lhs.x * rhs.z + lhs.y * rhs.w + lhs.z * rhs.x - let n3 = lhs.w * rhs.z + lhs.x * rhs.y - lhs.y * rhs.x + lhs.z * rhs.w - return Quat4d(n1, n2, n3, n0) + // Hamilton product as four 4-wide multiply-adds over the [x, y, z, w] lanes. + // Terms are explicitly typed so older compilers type-check them in reasonable time. + let lv: Vec4d = lhs.storage.storage + let rv: Vec4d = rhs.storage.storage + let term0: Vec4d = Vec4d(repeating: lv.w) * rv + let term1: Vec4d = Vec4d(lv.x, lv.y, lv.z, lv.x) * Vec4d(rv.w, rv.w, rv.w, -rv.x) + let term2: Vec4d = Vec4d(lv.y, lv.z, lv.x, lv.y) * Vec4d(rv.z, rv.x, rv.y, -rv.y) + let term3: Vec4d = Vec4d(lv.z, lv.x, lv.y, lv.z) * Vec4d(rv.y, rv.z, rv.x, rv.z) + let product: Vec4d = (term0 + term1) + (term2 - term3) + return Quat4d(product) #endif } diff --git a/Tests/FirebladeMathTests/Matrix4x4CofactorTests.swift b/Tests/FirebladeMathTests/Matrix4x4CofactorTests.swift new file mode 100644 index 0000000..29b5f62 --- /dev/null +++ b/Tests/FirebladeMathTests/Matrix4x4CofactorTests.swift @@ -0,0 +1,97 @@ +// +// Matrix4x4CofactorTests.swift +// FirebladeMathTests +// + +// Checks the 4x4 adjugate, determinant and inverse against a straightforward +// cofactor expansion that is written independently of the library code. + +import FirebladeMath +import Testing + +struct Matrix4x4CofactorTests { + /// Deterministic, well-conditioned test matrices (column-major element lists). + static let matrices: [[Double]] = { + var state: UInt64 = 0x9E37_79B9_7F4A_7C15 + func next() -> Double { + state = state &* 6_364_136_223_846_793_005 &+ 1_442_695_040_888_963_407 + return Double(state >> 11) / Double(1 << 53) * 4 - 2 + } + var result: [[Double]] = [] + for _ in 0..<32 { + var values = (0..<16).map { _ in next() } + // Strengthen the diagonal so every matrix is safely invertible. + for index in stride(from: 0, to: 16, by: 5) { + values[index] += 6 + } + result.append(values) + } + return result + }() + + @Test(arguments: matrices) + func adjugateDeterminantInverseDouble(_ values: [Double]) { + let matrix = Mat4x4d(values) + let (expectedAdjugate, expectedDeterminant) = referenceAdjugateAndDeterminant(values) + + #expect(isClose(determinant(matrix), expectedDeterminant, tolerance: 1e-9)) + #expect(isClose(adjugate(matrix).elements, expectedAdjugate, tolerance: 1e-9)) + + let product = (matrix * inverse(matrix)).elements + #expect(isClose(product, Mat4x4d.identity.elements, tolerance: 1e-12)) + } + + @Test(arguments: matrices) + func adjugateDeterminantInverseFloat(_ values: [Double]) { + let matrix = Mat4x4f(values.map(Float.init)) + let (expectedAdjugate, expectedDeterminant) = referenceAdjugateAndDeterminant(values) + + #expect(isClose(Double(determinant(matrix)), expectedDeterminant, tolerance: 1e-4)) + #expect(isClose(adjugate(matrix).elements.map(Double.init), expectedAdjugate, tolerance: 1e-4)) + + let product = (matrix * inverse(matrix)).elements + #expect(isClose(product.map(Double.init), Mat4x4d.identity.elements, tolerance: 1e-5)) + } + + @Test func singularMatrixHasZeroDeterminant() { + let singular = Mat4x4d([1, 2, 3, 4, 2, 4, 6, 8, 0, 1, 0, 1, 5, 0, 5, 0]) + #expect(determinant(singular) == 0) + #expect(determinant(Mat4x4f(singular.elements.map(Float.init))) == 0) + } +} + +/// Cofactor expansion on a column-major element list; element (row, column) is `values[column * 4 + row]`. +private func referenceAdjugateAndDeterminant(_ values: [Double]) -> (adjugate: [Double], determinant: Double) { + func element(_ row: Int, _ column: Int) -> Double { values[column * 4 + row] } + + func minor(_ skipRow: Int, _ skipColumn: Int) -> Double { + let rows = (0..<4).filter { $0 != skipRow } + let columns = (0..<4).filter { $0 != skipColumn } + func m(_ row: Int, _ column: Int) -> Double { element(rows[row], columns[column]) } + return m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) + - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)) + } + + func cofactor(_ row: Int, _ column: Int) -> Double { + ((row + column).isMultiple(of: 2) ? 1 : -1) * minor(row, column) + } + + // adjugate(row, column) = cofactor(column, row); stored column-major. + var adjugate = [Double](repeating: 0, count: 16) + for column in 0..<4 { + for row in 0..<4 { + adjugate[column * 4 + row] = cofactor(column, row) + } + } + let determinant = (0..<4).reduce(0) { $0 + element(0, $1) * cofactor(0, $1) } + return (adjugate, determinant) +} + +private func isClose(_ lhs: Double, _ rhs: Double, tolerance: Double) -> Bool { + Swift.abs(lhs - rhs) <= tolerance * Swift.max(1, Swift.abs(rhs)) +} + +private func isClose(_ lhs: [Double], _ rhs: [Double], tolerance: Double) -> Bool { + lhs.count == rhs.count && zip(lhs, rhs).allSatisfy { isClose($0, $1, tolerance: tolerance) } +} diff --git a/Tests/FirebladeMathTests/PortableVsSIMDTests.swift b/Tests/FirebladeMathTests/PortableVsSIMDTests.swift new file mode 100644 index 0000000..eaeb83f --- /dev/null +++ b/Tests/FirebladeMathTests/PortableVsSIMDTests.swift @@ -0,0 +1,203 @@ +// +// PortableVsSIMDTests.swift +// FirebladeMathTests +// + +// Differential tests: FirebladeMath's portable (non-simd) implementations against Apple's +// simd library, which is exactly what FirebladeMath forwards to when the `simd` trait is on. +// +// Only compiled when Apple's simd is available but the `simd` trait is disabled, i.e. +// `swift test --disable-default-traits` on Apple platforms (see the macOS CI workflow). +// +// Covers the functions with portable implementations changed in #76: +// 4x4 inverse, determinant and adjugate, and quaternion multiplication. +// +// Tolerances are about 10x the worst deviation measured over all inputs; both sides differ +// only by floating-point rounding. + +#if canImport(simd) && !FRB_MATH_USE_SIMD +import FirebladeMath +import simd +import Testing + +struct PortableVsSIMDTests { + // MARK: - Inputs + + /// Column-major 4x4 element lists: pseudo-random matrices plus structured cases. + static let matrices: [[Double]] = { + var generator = SplitMix(seed: 0x5EED_CAFE_F00D_BEEF) + var result: [[Double]] = (0..<256).map { _ in (0..<16).map { _ in generator.next(in: -2...2) } } + + let identity: [Double] = [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1] + result.append(identity) + result.append([2, 0, 0, 0, 0, -3, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 7]) // diagonal + result.append([0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0]) // permutation + result.append(rotationAndTranslation(angle: 0.7, axis: (1, 2, 3), translation: (4, -5, 6))) + result.append(identity.map { $0 * 1e-3 }) // small scale + result.append(result[0].map { $0 * 1e3 }) // large scale + return result + }() + + /// Singular matrices: only the determinant is compared for these. + static let singularMatrices: [[Double]] = [ + [Double](repeating: 0, count: 16), + [1, 2, 3, 4, 2, 4, 6, 8, 0, 1, 0, 1, 5, 0, 5, 0], // column 1 = 2 * column 0 + [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16] // rank 2 + ] + + /// Quaternion pairs as [x, y, z, w]: pseudo-random plus basis elements. + static let quaternionPairs: [([Double], [Double])] = { + var generator = SplitMix(seed: 0x0DDB_A11C_0FFE_E123) + var result: [([Double], [Double])] = (0..<256).map { _ in + ((0..<4).map { _ in generator.next(in: -2...2) }, (0..<4).map { _ in generator.next(in: -2...2) }) + } + let basis: [[Double]] = [[0, 0, 0, 1], [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]] + for lhs in basis { + for rhs in basis { + result.append((lhs, rhs)) + } + } + return result + }() + + // MARK: - 4x4 determinant + + @Test(arguments: matrices + singularMatrices) + func determinantDouble(_ values: [Double]) { + let portable = FirebladeMath.determinant(Mat4x4d(values)) + let reference = simd_determinant(simd_double4x4(values)) + #expect(isClose(portable, reference, scale: magnitude(values) ^^ 4, tolerance: 1e-14)) + } + + @Test(arguments: matrices + singularMatrices) + func determinantFloat(_ values: [Double]) { + let floats = values.map(Float.init) + let portable = FirebladeMath.determinant(Mat4x4f(floats)) + let reference = simd_determinant(simd_float4x4(floats)) + #expect(isClose(Double(portable), Double(reference), scale: magnitude(values) ^^ 4, tolerance: 3e-6)) + } + + // MARK: - 4x4 inverse + + @Test(arguments: matrices) + func inverseDouble(_ values: [Double]) { + let reference = simd_inverse(simd_double4x4(values)) + let portable = FirebladeMath.inverse(Mat4x4d(values)) + #expect(isClose(portable.elements, reference.elements, tolerance: 5e-13)) + } + + @Test(arguments: matrices) + func inverseFloat(_ values: [Double]) { + let floats = values.map(Float.init) + let reference = simd_inverse(simd_float4x4(floats)) + let portable = FirebladeMath.inverse(Mat4x4f(floats)) + #expect(isClose(portable.elements.map(Double.init), reference.elements.map(Double.init), tolerance: 2e-4)) + } + + // MARK: - 4x4 adjugate (simd has none; adjugate(m) = inverse(m) * determinant(m)) + + @Test(arguments: matrices) + func adjugateDouble(_ values: [Double]) { + let matrix = simd_double4x4(values) + let reference = simd_inverse(matrix) * simd_determinant(matrix) + let portable = FirebladeMath.adjugate(Mat4x4d(values)) + #expect(isClose(portable.elements, reference.elements, tolerance: 5e-13)) + } + + @Test(arguments: matrices) + func adjugateFloat(_ values: [Double]) { + let floats = values.map(Float.init) + let matrix = simd_float4x4(floats) + let reference = simd_inverse(matrix) * simd_determinant(matrix) + let portable = FirebladeMath.adjugate(Mat4x4f(floats)) + #expect(isClose(portable.elements.map(Double.init), reference.elements.map(Double.init), tolerance: 2e-4)) + } + + // MARK: - Quaternion multiplication + + @Test(arguments: quaternionPairs) + func quaternionMultiplyDouble(_ pair: ([Double], [Double])) { + let (lhs, rhs) = pair + let reference = simd_mul(simd_quatd(vector: SIMD4(lhs)), simd_quatd(vector: SIMD4(rhs))).vector + let portable = FirebladeMath.multiply(Quat4d(SIMD4(lhs)), Quat4d(SIMD4(rhs))) + #expect(isClose(portable.elements, Array(reference), tolerance: 1e-14)) + } + + @Test(arguments: quaternionPairs) + func quaternionMultiplyFloat(_ pair: ([Double], [Double])) { + let (lhs, rhs) = pair + let left = SIMD4(lhs.map(Float.init)) + let right = SIMD4(rhs.map(Float.init)) + let reference = simd_mul(simd_quatf(vector: left), simd_quatf(vector: right)).vector + let portable = FirebladeMath.multiply(Quat4f(left), Quat4f(right)) + #expect(isClose(portable.elements.map(Double.init), Array(reference).map(Double.init), tolerance: 2e-6)) + } +} + +// MARK: - Helpers + +private struct SplitMix { + var state: UInt64 + + init(seed: UInt64) { state = seed } + + mutating func next(in range: ClosedRange) -> Double { + state &+= 0x9E37_79B9_7F4A_7C15 + var z = state + z = (z ^ (z >> 30)) &* 0xBF58_476D_1CE4_E5B9 + z = (z ^ (z >> 27)) &* 0x94D0_49BB_1331_11EB + z ^= z >> 31 + let unit = Double(z >> 11) / Double(1 << 53) + return range.lowerBound + unit * (range.upperBound - range.lowerBound) + } +} + +private func rotationAndTranslation(angle: Double, axis: (Double, Double, Double), translation: (Double, Double, Double)) -> [Double] { + let rotation = simd_matrix4x4(simd_quatd(angle: angle, axis: simd_normalize(SIMD3(axis.0, axis.1, axis.2)))) + var values = rotation.elements + values[12] = translation.0 + values[13] = translation.1 + values[14] = translation.2 + return values +} + +private extension simd_double4x4 { + init(_ values: [Double]) { + self.init(SIMD4(values[0..<4]), SIMD4(values[4..<8]), SIMD4(values[8..<12]), SIMD4(values[12..<16])) + } + + var elements: [Double] { + [columns.0, columns.1, columns.2, columns.3].flatMap { [$0.x, $0.y, $0.z, $0.w] } + } +} + +private extension simd_float4x4 { + init(_ values: [Float]) { + self.init(SIMD4(values[0..<4]), SIMD4(values[4..<8]), SIMD4(values[8..<12]), SIMD4(values[12..<16])) + } + + var elements: [Float] { + [columns.0, columns.1, columns.2, columns.3].flatMap { [$0.x, $0.y, $0.z, $0.w] } + } +} + +private func magnitude(_ values: [Double]) -> Double { + Swift.max(1, values.map { Swift.abs($0) }.max() ?? 1) +} + +infix operator ^^: MultiplicationPrecedence +private func ^^ (base: Double, exponent: Int) -> Double { + (0.. Bool { + Swift.abs(lhs - rhs) <= tolerance * Swift.max(1, scale) +} + +/// Compares element-wise relative to the largest reference element. +private func isClose(_ lhs: [Double], _ rhs: [Double], tolerance: Double) -> Bool { + let scale = Swift.max(1, rhs.map { Swift.abs($0) }.max() ?? 1) + return lhs.count == rhs.count && zip(lhs, rhs).allSatisfy { Swift.abs($0 - $1) <= tolerance * scale } +} +#endif diff --git a/Tests/FirebladeMathTests/QuaternionMultiplicationTests.swift b/Tests/FirebladeMathTests/QuaternionMultiplicationTests.swift new file mode 100644 index 0000000..e66cc8f --- /dev/null +++ b/Tests/FirebladeMathTests/QuaternionMultiplicationTests.swift @@ -0,0 +1,101 @@ +// +// QuaternionMultiplicationTests.swift +// FirebladeMathTests +// + +// Checks the quaternion (Hamilton) product for Float and Double against a scalar +// reference written independently of the library, plus the defining algebraic rules. + +import FirebladeMath +import Testing + +struct QuaternionMultiplicationTests { + /// Deterministic pseudo-random quaternion pairs as [x, y, z, w] component lists. + static let pairs: [([Double], [Double])] = { + var state: UInt64 = 0xD1B5_4A32_D192_ED03 + func next() -> Double { + state = state &* 6_364_136_223_846_793_005 &+ 1_442_695_040_888_963_407 + return Double(state >> 11) / Double(1 << 53) * 4 - 2 + } + return (0..<64).map { _ in ((0..<4).map { _ in next() }, (0..<4).map { _ in next() }) } + }() + + @Test(arguments: pairs) + func matchesReferenceDouble(_ pair: ([Double], [Double])) { + let (lhs, rhs) = pair + let expected = referenceHamiltonProduct(lhs, rhs) + let product = Quat4d(lhs[0], lhs[1], lhs[2], lhs[3]) * Quat4d(rhs[0], rhs[1], rhs[2], rhs[3]) + #expect(isClose(product.elements, expected, tolerance: 1e-12)) + #expect(multiply(Quat4d(lhs[0], lhs[1], lhs[2], lhs[3]), Quat4d(rhs[0], rhs[1], rhs[2], rhs[3])) == product) + } + + @Test(arguments: pairs) + func matchesReferenceFloat(_ pair: ([Double], [Double])) { + let (lhs, rhs) = pair + let expected = referenceHamiltonProduct(lhs, rhs) + let left = Quat4f(Float(lhs[0]), Float(lhs[1]), Float(lhs[2]), Float(lhs[3])) + let right = Quat4f(Float(rhs[0]), Float(rhs[1]), Float(rhs[2]), Float(rhs[3])) + let product = left * right + #expect(isClose(product.elements.map(Double.init), expected, tolerance: 1e-5)) + #expect(multiply(left, right) == product) + } + + @Test func basisRulesDouble() { + let one = Quat4d(0, 0, 0, 1) + let i = Quat4d(1, 0, 0, 0) + let j = Quat4d(0, 1, 0, 0) + let k = Quat4d(0, 0, 1, 0) + let minusOne = Quat4d(0, 0, 0, -1) + + #expect(i * j == k) + #expect(j * k == i) + #expect(k * i == j) + #expect(j * i == Quat4d(0, 0, -1, 0)) + #expect(i * i == minusOne) + #expect(j * j == minusOne) + #expect(k * k == minusOne) + #expect(one * i == i) + #expect(i * one == i) + } + + @Test func basisRulesFloat() { + let one = Quat4f(0, 0, 0, 1) + let i = Quat4f(1, 0, 0, 0) + let j = Quat4f(0, 1, 0, 0) + let k = Quat4f(0, 0, 1, 0) + let minusOne = Quat4f(0, 0, 0, -1) + + #expect(i * j == k) + #expect(j * k == i) + #expect(k * i == j) + #expect(j * i == Quat4f(0, 0, -1, 0)) + #expect(i * i == minusOne) + #expect(j * j == minusOne) + #expect(k * k == minusOne) + #expect(one * i == i) + #expect(i * one == i) + } + + @Test(arguments: pairs) + func normIsMultiplicative(_ pair: ([Double], [Double])) { + let (lhs, rhs) = pair + let left = Quat4d(lhs[0], lhs[1], lhs[2], lhs[3]) + let right = Quat4d(rhs[0], rhs[1], rhs[2], rhs[3]) + #expect(isClose([(left * right).length], [left.length * right.length], tolerance: 1e-12)) + } +} + +/// Hamilton product on [x, y, z, w] component lists, written out per component. +private func referenceHamiltonProduct(_ lhs: [Double], _ rhs: [Double]) -> [Double] { + let (ax, ay, az, aw) = (lhs[0], lhs[1], lhs[2], lhs[3]) + let (bx, by, bz, bw) = (rhs[0], rhs[1], rhs[2], rhs[3]) + let x = aw * bx + ax * bw + ay * bz - az * by + let y = aw * by - ax * bz + ay * bw + az * bx + let z = aw * bz + ax * by - ay * bx + az * bw + let w = aw * bw - ax * bx - ay * by - az * bz + return [x, y, z, w] +} + +private func isClose(_ lhs: [Double], _ rhs: [Double], tolerance: Double) -> Bool { + lhs.count == rhs.count && zip(lhs, rhs).allSatisfy { Swift.abs($0 - $1) <= tolerance * Swift.max(1, Swift.abs($1)) } +}