staticarrays_eigen_fix.jl

using BenchmarkTools
using StaticArrays

function compute_eigenvector1(A, val)
    R = A - val * one(Mat3{Bool})
    rxr = (R[1,:] × R[2,:],
           R[1,:] × R[3,:],
           R[2,:] × R[3,:])

    d = map(x->x⋅x, rxr)
    dmax, imax = findmax(d)
    rxr[imax] / sqrt(dmax)
end

function orthonormal_complement(W)
    U = if abs(W[1]) > abs(W[2])
        invLength = 1 / sqrt(W[1]^2 + W[3]^2)
        Vec3(-W[3] * invLength, 0, W[1] * invLength)
    else
        invLength = 1 / sqrt(W[2]^2 + W[3]^2)
        Vec3(0, W[3] * invLength, -W[2] * invLength)
    end
    U, (W × U)
end

function compute_eigenvector2(A, evec1, eval1, eval2)
    (U, V), W = orthonormal_complement(evec1), evec1

    AU = A * U
    AV = A * V

    m00 = U ⋅ AU - eval1
    m01 = U ⋅ AV
    m11 = V ⋅ AV - eval1

    am00, am01, am11 = abs.((m00, m01, m11))

    if am00 >= am11
        maxAbsComp = max(am00, am01)
        if maxAbsComp > 0
            if am00 > am01
                m01 /= m00
                m00 = 1 / sqrt(1 + m01^2)
                m01 *= m00
            else
                m00 /= m01
                m01 = 1 / sqrt(1 + m00^2)
                m00 *= m01
            end
            m01 * U - m00 * V
        else
            U
        end
    else
        maxAbsComp = max(am11, am01)
        if maxAbsComp > 0
            if am11 >= am01
                m01 /= m11
                m11 = 1 / sqrt(1 + m01^2)
                m01 *= m11
            else
                m11 /= m01
                m01 = 1 / sqrt(1 + m11^2)
                m11 *= m01
            end
            m11 * U - m01 * V
        else
            U
        end
    end
end

println("Overriding StaticArrays._eig(::Size{(3,3)},...)")
@inline function StaticArrays._eig(::Size{(3,3)}, A::LinearAlgebra.HermOrSym{T}, permute, scale) where {T <: Real}
    maxAbsElement = maximum(abs, A)
    if (maxAbsElement == 0)
        return Eigen(SVector{3,T}(0,0,0), one(SMatrix{3,3,T}))
    end

    invMaxAbsElement = 1 / maxAbsElement
    A = A * invMaxAbsElement

    offdiag_norm = A[1,2]^2 + A[1,3]^2 + A[2,3]^2

    evecs, evals = if offdiag_norm > 0
        q = (A[1,1] + A[2,2] + A[3,3]) / 3
        b = diag(A) .- q
        p = sqrt((b ⋅ b + offdiag_norm * 2) / 6)

        c00 = b[2] * b[3] - A[2,3] * A[2,3]
        c01 = A[1,2] * b[3] - A[2,3] * A[1,3]
        c02 = A[1,2] * A[2,3] - b[2] * A[1,3]
        det = (b[1] * c00 - A[1,2] * c01 + A[1,3] * c02) / p^3

        halfdet = det / 2
        halfdet = clamp(halfdet, -1, 1)

        angle = acos(halfdet) / 3

        beta2 = 2cos(angle)
        beta0 = 2cos(angle + T(2π/3))
        beta1 = -(beta0 + beta2)

        evals = p .* SVector(beta0, beta1, beta2) .+ q

        evecs = if halfdet >= 0
            evec3 = compute_eigenvector1(A, evals[3])
            evec2 = compute_eigenvector2(A, evec3, evals[3], evals[2])
            evec1 = evec2 × evec3
            hcat(evec1, evec2, evec3)
        else
            evec1 = compute_eigenvector1(A, evals[1])
            evec2 = compute_eigenvector2(A, evec1, evals[1], evals[2])
            evec3 = evec1 × evec2
            hcat(evec1, evec2, evec3)
        end
        evecs, evals
    else
        SMatrix{3,3,T}(I), diag(A)
    end

    evals = evals .* maxAbsElement

    Eigen(evals, evecs)
end