Skip to content

Commit 922247e

Browse files
c42fc42f
authored
Merge pull request #260 from TsurHerman/pull-request/a0d168f3
replace inv and det for 4x4 matrices,
2 parents 7630a92 + 801b43c commit 922247e

File tree

3 files changed

+72
-66
lines changed

3 files changed

+72
-66
lines changed

‎src/det.jl

Lines changed: 22 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,26 +1,41 @@
11
@inline function det(A::StaticMatrix)
22
T = eltype(A)
33
S = typeof((one(T)*zero(T) + zero(T))/one(T))
4-
_det(Size(A),A,S)
4+
A_S = convert(similar_type(A,S),A)
5+
_det(Size(A_S),A_S)
56
end
67

78
@inline logdet(A::StaticMatrix) = log(det(A))
89

9-
@inline _det(::Size{(1,1)}, A::StaticMatrix,S::Type) = @inbounds return convert(S,A[1])
10+
@inline _det(::Size{(1,1)}, A::StaticMatrix) = @inbounds return A[1]
1011

11-
@inline function _det(::Size{(2,2)}, A::StaticMatrix, S::Type)
12-
A = similar_type(A,S)(A)
12+
@inline function _det(::Size{(2,2)}, A::StaticMatrix)
1313
@inbounds return A[1]*A[4] - A[3]*A[2]
1414
end
1515

16-
@inline function _det(::Size{(3,3)}, A::StaticMatrix, S::Type)
17-
A = similar_type(A,S)(A)
16+
@inline function _det(::Size{(3,3)}, A::StaticMatrix)
1817
@inbounds x0 = SVector(A[1], A[2], A[3])
1918
@inbounds x1 = SVector(A[4], A[5], A[6])
2019
@inbounds x2 = SVector(A[7], A[8], A[9])
2120
return vecdot(x0, cross(x1, x2))
2221
end
2322

24-
@inline function _det(::Size, A::StaticMatrix,::Type)
23+
@inline function _det(::Size{(4,4)}, A::StaticMatrix)
24+
@inbounds return (
25+
A[13] * A[10] * A[7] * A[4] - A[9] * A[14] * A[7] * A[4] -
26+
A[13] * A[6] * A[11] * A[4] + A[5] * A[14] * A[11] * A[4] +
27+
A[9] * A[6] * A[15] * A[4] - A[5] * A[10] * A[15] * A[4] -
28+
A[13] * A[10] * A[3] * A[8] + A[9] * A[14] * A[3] * A[8] +
29+
A[13] * A[2] * A[11] * A[8] - A[1] * A[14] * A[11] * A[8] -
30+
A[9] * A[2] * A[15] * A[8] + A[1] * A[10] * A[15] * A[8] +
31+
A[13] * A[6] * A[3] * A[12] - A[5] * A[14] * A[3] * A[12] -
32+
A[13] * A[2] * A[7] * A[12] + A[1] * A[14] * A[7] * A[12] +
33+
A[5] * A[2] * A[15] * A[12] - A[1] * A[6] * A[15] * A[12] -
34+
A[9] * A[6] * A[3] * A[16] + A[5] * A[10] * A[3] * A[16] +
35+
A[9] * A[2] * A[7] * A[16] - A[1] * A[10] * A[7] * A[16] -
36+
A[5] * A[2] * A[11] * A[16] + A[1] * A[6] * A[11] * A[16])
37+
end
38+
39+
@inline function _det(::Size, A::StaticMatrix)
2540
return det(Matrix(A))
2641
end

‎src/inv.jl

Lines changed: 38 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -1,24 +1,24 @@
1-
@inline inv(A::StaticMatrix) = _inv(Size(A), A)
2-
3-
@inline _inv(::Size{(1,1)}, A) = similar_type(typeof(A), typeof(inv(one(eltype(A)))))(inv(A[1]))
4-
5-
@inline function _inv(::Size{(2,2)}, A)
1+
@inline function inv(A::StaticMatrix)
62
T = eltype(A)
73
S = typeof((one(T)*zero(T) + zero(T))/one(T))
8-
newtype = similar_type(A, S)
4+
A_S = convert(similar_type(A,S),A)
5+
_inv(Size(A_S),A_S)
6+
end
7+
8+
@inline _inv(::Size{(1,1)}, A) = similar_type(A)(inv(A[1]))
99

10-
d = det(A)
11-
@inbounds return newtype((A[4]/d, -(A[2]/d), -(A[3]/d), A[1]/d))
10+
@inline function _inv(::Size{(2,2)}, A)
11+
newtype = similar_type(A)
12+
idet = 1/det(A)
13+
@inbounds return newtype((A[4]*idet, -(A[2]*idet), -(A[3]*idet), A[1]*idet))
1214
end
1315

1416
@inline function _inv(::Size{(3,3)}, A)
15-
T = eltype(A)
16-
S = typeof((one(T)*zero(T) + zero(T))/one(T))
17-
newtype = similar_type(A, S)
17+
newtype = similar_type(A)
1818

19-
@inbounds x0 = SVector{3,S}(A[1], A[2], A[3])
20-
@inbounds x1 = SVector{3,S}(A[4], A[5], A[6])
21-
@inbounds x2 = SVector{3,S}(A[7], A[8], A[9])
19+
@inbounds x0 = SVector{3}(A[1], A[2], A[3])
20+
@inbounds x1 = SVector{3}(A[4], A[5], A[6])
21+
@inbounds x2 = SVector{3}(A[7], A[8], A[9])
2222

2323
y0 = cross(x1,x2)
2424
d = vecdot(x0, y0)
@@ -30,48 +30,31 @@ end
3030
@inbounds return newtype((y0[1], y1[1], y2[1], y0[2], y1[2], y2[2], y0[3], y1[3], y2[3]))
3131
end
3232

33-
Base.@pure function splitrange(r::SUnitRange)
34-
mid = (first(r) + last(r)) ÷ 2
35-
(SUnitRange(first(r), mid), SUnitRange(mid+1, last(r)))
36-
end
37-
38-
@noinline function _inv(::Size{(4,4)}, A)
39-
# Partition matrix into four 2x2 blocks. For 4x4 matrices this seems to be
40-
# more stable than directly using the adjugate expansion.
41-
# See http://www.freevec.org/function/inverse_matrix_4x4_using_partitioning
42-
#
43-
# TODO: This decomposition works in higher dimensions, but numerical
44-
# stability doesn't seem good for badly conditioned matrices. Can be
45-
# fixed?
46-
(i1,i2) = splitrange(SUnitRange(1,4))
47-
@inbounds P = A[i1,i1]
48-
@inbounds Q = A[i1,i2]
49-
@inbounds R = A[i2,i1]
50-
@inbounds S = A[i2,i2]
51-
invP = inv(P)
52-
invP_Q = invP*Q
53-
S2 = inv(S - R*invP_Q)
54-
R2 = -S2*(R*invP)
55-
Q2 = -invP_Q*S2
56-
P2 = invP - invP_Q*R2
57-
[[P2 Q2];
58-
[R2 S2]]
33+
@inline function _inv(::Size{(4,4)}, A)
34+
idet = 1/det(A)
35+
B = @SMatrix [
36+
(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
37+
(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
38+
(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
39+
(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
40+
41+
(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
42+
(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
43+
(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
44+
(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
45+
46+
(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
47+
(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
48+
(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
49+
(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
50+
51+
(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
52+
(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
53+
(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
54+
(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]
55+
return similar_type(A)(B)
5956
end
6057

6158
@inline function _inv(::Size, A)
62-
T = eltype(A)
63-
S = typeof((one(T)*zero(T) + zero(T))/one(T))
64-
AA = convert(Array{S}, A) # lufact() doesn't work with StaticArrays at the moment... and the branches below must be type-stable
65-
if istriu(A)
66-
Ai_ut = inv(UpperTriangular(AA))
67-
# TODO double check these routines leave the parent in a clean (upper triangular) state
68-
return Size(A)(parent(Ai_ut)) # Return a `SizedArray`
69-
elseif istril(A)
70-
Ai_lt = inv(LowerTriangular(AA))
71-
# TODO double check these routines leave the parent in a clean (lower triangular) state
72-
return Size(A)(parent(Ai_lt)) # Return a `SizedArray`
73-
else
74-
Ai_lu = inv(lufact(AA))
75-
return Size(A)(Ai_lu) # Return a `SizedArray`
76-
end
59+
similar_type(A)(inv(Matrix(A)))
7760
end

‎test/inv.jl

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,14 @@ end
4747
sm = SMatrix{4,4}(m)
4848
@test isapprox(inv(sm)::StaticMatrix, inv(m), rtol=2e-15)
4949

50+
# A permutation matrix which can cause problems for inversion methods
51+
# without pivoting, eg 2x2 block decomposition (see #250)
52+
mperm = [1 0 0 0;
53+
0 0 1 0;
54+
0 1 0 0;
55+
0 0 0 1]
56+
@test inv(SMatrix{4,4}(mperm))::StaticMatrix ≈ inv(mperm) rtol=2e-16
57+
5058
# Poorly conditioned matrix; almost_singular_matrix(4, 3, 1e-7)
5159
m = [
5260
2.83056817904263402e-01 1.26822318692296848e-01 2.59665505365002547e-01 1.24524798964590747e-01
@@ -55,8 +63,8 @@ end
5563
1.63327582123091175e-01 1.70552365412100865e-01 4.55720450934200327e-01 1.23299968650232419e-01
5664
]
5765
sm = SMatrix{4,4}(m)
58-
@test norm(Matrix(sm*inv(sm) - eye(4))) < 10*norm(m*inv(m) - eye(4))
59-
@test norm(Matrix(inv(sm)*sm - eye(4))) < 10*norm(inv(m)*m - eye(4))
66+
@test norm(Matrix(sm*inv(sm) - eye(4))) < 12*norm(m*inv(m) - eye(4))
67+
@test norm(Matrix(inv(sm)*sm - eye(4))) < 12*norm(inv(m)*m - eye(4))
6068

6169
# Poorly conditioned matrix, generated by fuzz testing with
6270
# almost_singular_matrix(N, 3, 1e-7)
@@ -66,8 +74,8 @@ end
6674
3.42945636322714076e-01 3.29329508494837497e-01 2.25635541033857107e-01 6.03912636987153917e-02
6775
4.77828437366344727e-01 4.86406974015710591e-01 1.95415684569693188e-01 6.74775080892497797e-02]
6876
sm = SMatrix{4,4}(m)
69-
@test_broken norm(Matrix(inv(sm)*sm - eye(4))) < 10*norm(inv(m)*m - eye(4))
70-
@test_broken norm(Matrix(sm*inv(sm) - eye(4))) < 10*norm(m*inv(m) - eye(4))
77+
@test norm(Matrix(inv(sm)*sm - eye(4))) < 12*norm(inv(m)*m - eye(4))
78+
@test norm(Matrix(sm*inv(sm) - eye(4))) < 12*norm(m*inv(m) - eye(4))
7179
end
7280

7381
@testset "Matrix inverse 5x5" begin

0 commit comments

Comments
 (0)