static lu decomp

lu.jl

using StaticArrays

import Base: \
import LinearAlgebra: lu
using Base: OneTo

struct CompletelyPivotedLU{T,N,TA<:SMatrix{N,N,T},TP}
    A::TA
    p::TP
    q::TP
end

function lu_fullpivot(A::SMatrix{N,N,T}) where {N,T}
    A = MMatrix(A)
    p = @MVector fill(N, N)
    q = @MVector fill(N, N)

    @inbounds for k = OneTo(N - 1)
        # Find max value in sub-part.
        m, n, maxval = 1, 1, zero(T)
        for j = k:N, i = k:N
            if abs(A[i,j]) > maxval
                m, n, maxval = i, j, abs(A[i,j])
            end
        end

        # Store the p and q
        p[k] = m
        q[k] = n

        # Swap row and col
        for j = k:N
            A[k, j], A[m, j] = A[m, j], A[k, j]
        end

        for j = k:N
            A[j, k], A[j, n] = A[j, n], A[j, k]
        end

        Akk = A[k,k]

        for i = k+1:N
            A[i, k] /= Akk
        end

        for j = k+1:N
            Akj = A[k,j]

            for i = k+1:N
                A[i,j] -= A[i,k] * Akj
            end
        end
    end

    return CompletelyPivotedLU(SMatrix(A), SVector(p), SVector(q))
end

function (\)(LU::CompletelyPivotedLU{T,N}, rhs::SVector{N}) where {T,N}
    x = MVector(rhs)

    @inbounds for i = OneTo(N)
        x[i], x[LU.p[i]] = x[LU.p[i]], x[i]
        for j = i+1:N
            x[j] -= LU.A[j, i] * x[i]
        end
    end

    @inbounds for i = N:-1:1
        for j = N:-1:i+1
            x[i] -= LU.A[i, j] * x[j]
        end
        x[i] /= LU.A[i,i]
        x[i], x[LU.q[i]] = x[LU.q[i]], x[i]
    end

    SVector(x)
end

function sylv(A::SMatrix{1,1,T}, B::SMatrix{2,2,T}, C::SMatrix{1,2,T}) where {T}
    D = @SMatrix [A[1,1]+B[1,1]' B[2,1]'       ;
                  B[1,2]'        A[1,1]+B[2,2]']
    SMatrix{1,2}(lu_fullpivot(D) \ SVector{2}(C))
end

function sylv(A::SMatrix{2,2,T}, B::SMatrix{1,1,T}, C::SMatrix{2,1,T}) where {T}
    D = @SMatrix [A[1,1]+B[1,1]' A[1,2]        ;
                  A[2,1]         A[2,2]+B[1,1]']
    SMatrix{2,1}(lu_fullpivot(D) \ SVector{2}(C))
end

function sylv(A::SMatrix{2,2,T}, B::SMatrix{2,2,T}, C::SMatrix{2,2,T}) where {T}
    D = @SMatrix [A[1,1]+B[1,1]' A[1,2]         B[2,1]'        T(0)          ;
                  A[2,1]         A[2,2]+B[1,1]' T(0)           B[2,1]'       ;
                  B[1,2]'        T(0)           A[1,1]+B[2,2]' A[1,2]        ;
                  T(0)           B[1,2]'        A[2,1]         A[2,2]+B[2,2]']
    SMatrix{2,2}(lu_fullpivot(D) \ SVector{4}(C))
end