Skip to content

replace inv and det for 4x4 matrices, #260

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 9 commits into from
Aug 18, 2017
Merged

replace inv and det for 4x4 matrices, #260

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
a0d168f
replace inv and det for 4x4 matrices,
TsurHerman Jul 29, 2017
02b4890
Relax test for poorly conditioned 4x4 matrices
TsurHerman Jul 30, 2017
6301885
Merge branch 'master' into pull-request/a0d168f3
TsurHerman Aug 5, 2017
0cd325a
Make inv similar to det and clear noise
TsurHerman Aug 5, 2017
3fbb854
speed improvement for 2x2 inv
TsurHerman Aug 5, 2017
cc34e5e
similar_type convert on all inv methods including >4 and 1
TsurHerman Aug 5, 2017
e578b9b
Avoid variable name changing type, moved promotion to dispatching fun…
TsurHerman Aug 7, 2017
045c1cf
explicitly call convert instead of constructor
TsurHerman Aug 8, 2017
801b43c
Add a 4x4 permutation matrix as an inv() test
c42f Aug 18, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 22 additions & 7 deletions src/det.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
93 changes: 38 additions & 55 deletions src/inv.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)
Expand All @@ -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
16 changes: 12 additions & 4 deletions test/inv.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This change is ok I think. There's nothing special about the factor of 10 which you've replaced with 12 here. I'd say some difference is to be expected with a switch in algorithms.


# Poorly conditioned matrix, generated by fuzz testing with
# almost_singular_matrix(N, 3, 1e-7)
Expand All @@ -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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These tests were particularly chosen as examples where the previous implementation had trouble. So they're probably not that relevant to the new implementation.

Presumably we'd be able to find some equivalent problems by rerunning the code at the bottom of this file.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is still a difficult case (as evidenced by the bounds being quite tight). But running the problem generator would be a good idea.

end

@testset "Matrix inverse 5x5" begin
Expand Down