diff --git a/src/det.jl b/src/det.jl index 9b0caa58..ccadb81d 100644 --- a/src/det.jl +++ b/src/det.jl @@ -1,26 +1,41 @@ @inline function det(A::StaticMatrix) T = eltype(A) S = typeof((one(T)*zero(T) + zero(T))/one(T)) - _det(Size(A),A,S) + A_S = convert(similar_type(A,S),A) + _det(Size(A_S),A_S) end @inline logdet(A::StaticMatrix) = log(det(A)) -@inline _det(::Size{(1,1)}, A::StaticMatrix,S::Type) = @inbounds return convert(S,A[1]) +@inline _det(::Size{(1,1)}, A::StaticMatrix) = @inbounds return A[1] -@inline function _det(::Size{(2,2)}, A::StaticMatrix, S::Type) - A = similar_type(A,S)(A) +@inline function _det(::Size{(2,2)}, A::StaticMatrix) @inbounds return A[1]*A[4] - A[3]*A[2] end -@inline function _det(::Size{(3,3)}, A::StaticMatrix, S::Type) - A = similar_type(A,S)(A) +@inline function _det(::Size{(3,3)}, A::StaticMatrix) @inbounds x0 = SVector(A[1], A[2], A[3]) @inbounds x1 = SVector(A[4], A[5], A[6]) @inbounds x2 = SVector(A[7], A[8], A[9]) return vecdot(x0, cross(x1, x2)) end -@inline function _det(::Size, A::StaticMatrix,::Type) +@inline function _det(::Size{(4,4)}, A::StaticMatrix) + @inbounds return ( + A[13] * A[10] * A[7] * A[4] - A[9] * A[14] * A[7] * A[4] - + A[13] * A[6] * A[11] * A[4] + A[5] * A[14] * A[11] * A[4] + + A[9] * A[6] * A[15] * A[4] - A[5] * A[10] * A[15] * A[4] - + A[13] * A[10] * A[3] * A[8] + A[9] * A[14] * A[3] * A[8] + + A[13] * A[2] * A[11] * A[8] - A[1] * A[14] * A[11] * A[8] - + A[9] * A[2] * A[15] * A[8] + A[1] * A[10] * A[15] * A[8] + + A[13] * A[6] * A[3] * A[12] - A[5] * A[14] * A[3] * A[12] - + A[13] * A[2] * A[7] * A[12] + A[1] * A[14] * A[7] * A[12] + + A[5] * A[2] * A[15] * A[12] - A[1] * A[6] * A[15] * A[12] - + A[9] * A[6] * A[3] * A[16] + A[5] * A[10] * A[3] * A[16] + + A[9] * A[2] * A[7] * A[16] - A[1] * A[10] * A[7] * A[16] - + A[5] * A[2] * A[11] * A[16] + A[1] * A[6] * A[11] * A[16]) +end + +@inline function _det(::Size, A::StaticMatrix) return det(Matrix(A)) end diff --git a/src/inv.jl b/src/inv.jl index f1c79331..b74ad02a 100644 --- a/src/inv.jl +++ b/src/inv.jl @@ -1,24 +1,24 @@ -@inline inv(A::StaticMatrix) = _inv(Size(A), A) - -@inline _inv(::Size{(1,1)}, A) = similar_type(typeof(A), typeof(inv(one(eltype(A)))))(inv(A[1])) - -@inline function _inv(::Size{(2,2)}, A) +@inline function inv(A::StaticMatrix) T = eltype(A) S = typeof((one(T)*zero(T) + zero(T))/one(T)) - newtype = similar_type(A, S) + A_S = convert(similar_type(A,S),A) + _inv(Size(A_S),A_S) +end + +@inline _inv(::Size{(1,1)}, A) = similar_type(A)(inv(A[1])) - d = det(A) - @inbounds return newtype((A[4]/d, -(A[2]/d), -(A[3]/d), A[1]/d)) +@inline function _inv(::Size{(2,2)}, A) + newtype = similar_type(A) + idet = 1/det(A) + @inbounds return newtype((A[4]*idet, -(A[2]*idet), -(A[3]*idet), A[1]*idet)) end @inline function _inv(::Size{(3,3)}, A) - T = eltype(A) - S = typeof((one(T)*zero(T) + zero(T))/one(T)) - newtype = similar_type(A, S) + newtype = similar_type(A) - @inbounds x0 = SVector{3,S}(A[1], A[2], A[3]) - @inbounds x1 = SVector{3,S}(A[4], A[5], A[6]) - @inbounds x2 = SVector{3,S}(A[7], A[8], A[9]) + @inbounds x0 = SVector{3}(A[1], A[2], A[3]) + @inbounds x1 = SVector{3}(A[4], A[5], A[6]) + @inbounds x2 = SVector{3}(A[7], A[8], A[9]) y0 = cross(x1,x2) d = vecdot(x0, y0) @@ -30,48 +30,31 @@ end @inbounds return newtype((y0[1], y1[1], y2[1], y0[2], y1[2], y2[2], y0[3], y1[3], y2[3])) end -Base.@pure function splitrange(r::SUnitRange) - mid = (first(r) + last(r)) ÷ 2 - (SUnitRange(first(r), mid), SUnitRange(mid+1, last(r))) -end - -@noinline function _inv(::Size{(4,4)}, A) - # Partition matrix into four 2x2 blocks. For 4x4 matrices this seems to be - # more stable than directly using the adjugate expansion. - # See http://www.freevec.org/function/inverse_matrix_4x4_using_partitioning - # - # TODO: This decomposition works in higher dimensions, but numerical - # stability doesn't seem good for badly conditioned matrices. Can be - # fixed? - (i1,i2) = splitrange(SUnitRange(1,4)) - @inbounds P = A[i1,i1] - @inbounds Q = A[i1,i2] - @inbounds R = A[i2,i1] - @inbounds S = A[i2,i2] - invP = inv(P) - invP_Q = invP*Q - S2 = inv(S - R*invP_Q) - R2 = -S2*(R*invP) - Q2 = -invP_Q*S2 - P2 = invP - invP_Q*R2 - [[P2 Q2]; - [R2 S2]] +@inline function _inv(::Size{(4,4)}, A) + idet = 1/det(A) + B = @SMatrix [ + (A[2,3]*A[3,4]*A[4,2] - A[2,4]*A[3,3]*A[4,2] + A[2,4]*A[3,2]*A[4,3] - A[2,2]*A[3,4]*A[4,3] - A[2,3]*A[3,2]*A[4,4] + A[2,2]*A[3,3]*A[4,4]) * idet + (A[2,4]*A[3,3]*A[4,1] - A[2,3]*A[3,4]*A[4,1] - A[2,4]*A[3,1]*A[4,3] + A[2,1]*A[3,4]*A[4,3] + A[2,3]*A[3,1]*A[4,4] - A[2,1]*A[3,3]*A[4,4]) * idet + (A[2,2]*A[3,4]*A[4,1] - A[2,4]*A[3,2]*A[4,1] + A[2,4]*A[3,1]*A[4,2] - A[2,1]*A[3,4]*A[4,2] - A[2,2]*A[3,1]*A[4,4] + A[2,1]*A[3,2]*A[4,4]) * idet + (A[2,3]*A[3,2]*A[4,1] - A[2,2]*A[3,3]*A[4,1] - A[2,3]*A[3,1]*A[4,2] + A[2,1]*A[3,3]*A[4,2] + A[2,2]*A[3,1]*A[4,3] - A[2,1]*A[3,2]*A[4,3]) * idet + + (A[1,4]*A[3,3]*A[4,2] - A[1,3]*A[3,4]*A[4,2] - A[1,4]*A[3,2]*A[4,3] + A[1,2]*A[3,4]*A[4,3] + A[1,3]*A[3,2]*A[4,4] - A[1,2]*A[3,3]*A[4,4]) * idet + (A[1,3]*A[3,4]*A[4,1] - A[1,4]*A[3,3]*A[4,1] + A[1,4]*A[3,1]*A[4,3] - A[1,1]*A[3,4]*A[4,3] - A[1,3]*A[3,1]*A[4,4] + A[1,1]*A[3,3]*A[4,4]) * idet + (A[1,4]*A[3,2]*A[4,1] - A[1,2]*A[3,4]*A[4,1] - A[1,4]*A[3,1]*A[4,2] + A[1,1]*A[3,4]*A[4,2] + A[1,2]*A[3,1]*A[4,4] - A[1,1]*A[3,2]*A[4,4]) * idet + (A[1,2]*A[3,3]*A[4,1] - A[1,3]*A[3,2]*A[4,1] + A[1,3]*A[3,1]*A[4,2] - A[1,1]*A[3,3]*A[4,2] - A[1,2]*A[3,1]*A[4,3] + A[1,1]*A[3,2]*A[4,3]) * idet + + (A[1,3]*A[2,4]*A[4,2] - A[1,4]*A[2,3]*A[4,2] + A[1,4]*A[2,2]*A[4,3] - A[1,2]*A[2,4]*A[4,3] - A[1,3]*A[2,2]*A[4,4] + A[1,2]*A[2,3]*A[4,4]) * idet + (A[1,4]*A[2,3]*A[4,1] - A[1,3]*A[2,4]*A[4,1] - A[1,4]*A[2,1]*A[4,3] + A[1,1]*A[2,4]*A[4,3] + A[1,3]*A[2,1]*A[4,4] - A[1,1]*A[2,3]*A[4,4]) * idet + (A[1,2]*A[2,4]*A[4,1] - A[1,4]*A[2,2]*A[4,1] + A[1,4]*A[2,1]*A[4,2] - A[1,1]*A[2,4]*A[4,2] - A[1,2]*A[2,1]*A[4,4] + A[1,1]*A[2,2]*A[4,4]) * idet + (A[1,3]*A[2,2]*A[4,1] - A[1,2]*A[2,3]*A[4,1] - A[1,3]*A[2,1]*A[4,2] + A[1,1]*A[2,3]*A[4,2] + A[1,2]*A[2,1]*A[4,3] - A[1,1]*A[2,2]*A[4,3]) * idet + + (A[1,4]*A[2,3]*A[3,2] - A[1,3]*A[2,4]*A[3,2] - A[1,4]*A[2,2]*A[3,3] + A[1,2]*A[2,4]*A[3,3] + A[1,3]*A[2,2]*A[3,4] - A[1,2]*A[2,3]*A[3,4]) * idet + (A[1,3]*A[2,4]*A[3,1] - A[1,4]*A[2,3]*A[3,1] + A[1,4]*A[2,1]*A[3,3] - A[1,1]*A[2,4]*A[3,3] - A[1,3]*A[2,1]*A[3,4] + A[1,1]*A[2,3]*A[3,4]) * idet + (A[1,4]*A[2,2]*A[3,1] - A[1,2]*A[2,4]*A[3,1] - A[1,4]*A[2,1]*A[3,2] + A[1,1]*A[2,4]*A[3,2] + A[1,2]*A[2,1]*A[3,4] - A[1,1]*A[2,2]*A[3,4]) * idet + (A[1,2]*A[2,3]*A[3,1] - A[1,3]*A[2,2]*A[3,1] + A[1,3]*A[2,1]*A[3,2] - A[1,1]*A[2,3]*A[3,2] - A[1,2]*A[2,1]*A[3,3] + A[1,1]*A[2,2]*A[3,3]) * idet] + return similar_type(A)(B) end @inline function _inv(::Size, A) - T = eltype(A) - S = typeof((one(T)*zero(T) + zero(T))/one(T)) - AA = convert(Array{S}, A) # lufact() doesn't work with StaticArrays at the moment... and the branches below must be type-stable - if istriu(A) - Ai_ut = inv(UpperTriangular(AA)) - # TODO double check these routines leave the parent in a clean (upper triangular) state - return Size(A)(parent(Ai_ut)) # Return a `SizedArray` - elseif istril(A) - Ai_lt = inv(LowerTriangular(AA)) - # TODO double check these routines leave the parent in a clean (lower triangular) state - return Size(A)(parent(Ai_lt)) # Return a `SizedArray` - else - Ai_lu = inv(lufact(AA)) - return Size(A)(Ai_lu) # Return a `SizedArray` - end + similar_type(A)(inv(Matrix(A))) end diff --git a/test/inv.jl b/test/inv.jl index 41502d3e..104a3f36 100644 --- a/test/inv.jl +++ b/test/inv.jl @@ -47,6 +47,14 @@ end sm = SMatrix{4,4}(m) @test isapprox(inv(sm)::StaticMatrix, inv(m), rtol=2e-15) + # A permutation matrix which can cause problems for inversion methods + # without pivoting, eg 2x2 block decomposition (see #250) + mperm = [1 0 0 0; + 0 0 1 0; + 0 1 0 0; + 0 0 0 1] + @test inv(SMatrix{4,4}(mperm))::StaticMatrix ≈ inv(mperm) rtol=2e-16 + # Poorly conditioned matrix; almost_singular_matrix(4, 3, 1e-7) m = [ 2.83056817904263402e-01 1.26822318692296848e-01 2.59665505365002547e-01 1.24524798964590747e-01 @@ -55,8 +63,8 @@ end 1.63327582123091175e-01 1.70552365412100865e-01 4.55720450934200327e-01 1.23299968650232419e-01 ] sm = SMatrix{4,4}(m) - @test norm(Matrix(sm*inv(sm) - eye(4))) < 10*norm(m*inv(m) - eye(4)) - @test norm(Matrix(inv(sm)*sm - eye(4))) < 10*norm(inv(m)*m - eye(4)) + @test norm(Matrix(sm*inv(sm) - eye(4))) < 12*norm(m*inv(m) - eye(4)) + @test norm(Matrix(inv(sm)*sm - eye(4))) < 12*norm(inv(m)*m - eye(4)) # Poorly conditioned matrix, generated by fuzz testing with # almost_singular_matrix(N, 3, 1e-7) @@ -66,8 +74,8 @@ end 3.42945636322714076e-01 3.29329508494837497e-01 2.25635541033857107e-01 6.03912636987153917e-02 4.77828437366344727e-01 4.86406974015710591e-01 1.95415684569693188e-01 6.74775080892497797e-02] sm = SMatrix{4,4}(m) - @test_broken norm(Matrix(inv(sm)*sm - eye(4))) < 10*norm(inv(m)*m - eye(4)) - @test_broken norm(Matrix(sm*inv(sm) - eye(4))) < 10*norm(m*inv(m) - eye(4)) + @test norm(Matrix(inv(sm)*sm - eye(4))) < 12*norm(inv(m)*m - eye(4)) + @test norm(Matrix(sm*inv(sm) - eye(4))) < 12*norm(m*inv(m) - eye(4)) end @testset "Matrix inverse 5x5" begin