static matrix inverses

inverses.jl

using StaticArrays

import StaticArrays.SUnitRange

@noinline function inv2(a::StaticMatrix{4,4})
    # See http://www.freevec.org/function/inverse_matrix_4x4_using_partitioning
    # Partition
    i1 = SUnitRange(1,2)
    i2 = SUnitRange(3,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]]
end

Base.@pure function splitrange(r::SUnitRange)
    if isodd(length(r))
        mid = last(r)-1
    else
        mid = (first(r) + last(r)) ÷ 2
    end
    #mid = (first(r) + last(r)) ÷ 2
    (SUnitRange(first(r), mid), SUnitRange(mid+1, last(r)))
end

@inline inv3(a::StaticMatrix{1,1}) = inv(a)
@inline inv3(a::StaticMatrix{2,2}) = inv(a)
@inline inv3(a::StaticMatrix{3,3}) = inv(a)

@noinline function inv3(a::StaticMatrix{N,N}) where {N}
    # See http://www.freevec.org/function/inverse_matrix_4x4_using_partitioning
    (i1,i2) = splitrange(SUnitRange(1,N))
    @inbounds P = a[i1,i1]
    @inbounds Q = a[i1,i2]
    @inbounds R = a[i2,i1]
    @inbounds S = a[i2,i2]
    invP = inv3(P)
    invP_Q = invP*Q
    S2 = inv3(S - R*invP_Q)
    R2 = -S2*(R*invP)
    Q2 = -invP_Q*S2
    P2 = invP - invP_Q*R2
    [[P2 Q2];
     [R2 S2]]
end

#= function inv3x3(a) =#
#= #2233-2332 1332-1233 1223-1322 =#
#= #2331-2133 1133-1331 1321-1123 =#
#= #2132-2231 1231-1132 1122-1221 =#
#=     1/det(a) * =#
#=     [a[2,2]a[3,3]-a[2,3]a[3,2] a[1,3]a[3,2]-a[1,2]a[3,3] a[1,2]a[2,3]-a[1,3]a[2,2] =#
#=      a[2,3]a[3,1]-a[2,1]a[3,3] a[1,1]a[3,3]-a[1,3]a[3,1] a[1,3]a[2,1]-a[1,1]a[2,3] =#
#=      a[2,1]a[3,2]-a[2,2]a[3,1] a[1,2]a[3,1]-a[1,1]a[3,2] a[1,1]a[2,2]-a[1,2]a[2,1]] =#
#= end =#

@noinline function benchloop(A, iters)
    A2 = copy(A)
    for i=1:iters
        A2 += inv3(A)
    end
    A2
end

N = 8
x = 1:N
A = x.+x' + 1e-5*rand(N,N)
SA = SMatrix{N,N}(A)

benchloop(SA, 20)
@time benchloop(SA, 10000000)

inv3(SA)*SA - eye(N)