Rotation matrix computation

using LinearAlgebra

"""
    quadraticSolver(A, b, x)

Solves an equation system of the form:

    forall k in 1..n sum_{i=1}^n sum_{j=1}^n a_{kij} x_i x_j = b_k

Uses Newton's method; the optimization starts from `x`.
"""
function quadraticSolver(A, b, x)
    tolerance = 1.0e-8
    maxiter = 100
    n = length(A)
    for _ in 1:maxiter
        D = zeros(n, n)
        f = -b
        for k in 1:n, i in 1:n, j in 1:n
            f[k] += A[k][i,j] * x[i] * x[j]
        end
        for k in 1:n, l in 1:n
            D[k,l] = 2 * A[k][l,l] * x[l]
            for i in 1:n
                D[k,l] += (A[k][i,l] + A[k][l,i]) * x[i]
            end
        end
        delta = inv(D) * f
        norm(delta) < tolerance && break
        x -= delta
    end
    x
end

"""
    findMatrix(p, n)

`e` and `n` are vectors of three 3D vectors.
Returns a rotation matrix s.t. n[i]'*R*e[i] == 0.
"""
function findMatrix(e, n)
    # Set up the quaternion equations
    A = [Matrix{Float64}(I, 4, 4)]
    for i in 1:3
        E = [   0    -e[i][1] -e[i][2] -e[i][3]
             e[i][1]     0     e[i][3] -e[i][2]
             e[i][2] -e[i][3]     0     e[i][1]
             e[i][3]  e[i][2] -e[i][1]     0   ]
        N = [   0    -n[i][1] -n[i][2] -n[i][3]
             n[i][1]     0    -n[i][3]  n[i][2]
             n[i][2]  n[i][3]     0    -n[i][1]
             n[i][3] -n[i][2]  n[i][1]     0   ]
        push!(A, E' * N)
    end
    b = [1., 0, 0, 0]
    # Compute q such that q' * A[i] * q = b[i], i = 1..4
    q = quadraticSolver(A, b, [1., 0, 0, 0])
    # Convert q to rotation matrix form
    [q[1]^2+q[2]^2-q[3]^2-q[4]^2 2(q[2]*q[3]-q[1]*q[4])      2(q[2]*q[4]+q[1]*q[3])
     2(q[2]*q[3]+q[1]*q[4])      q[1]^2+q[3]^2-q[2]^2-q[4]^2 2(q[3]*q[4]-q[1]*q[2])
     2(q[2]*q[4]-q[1]*q[3])      2(q[3]*q[4]+q[1]*q[2])      q[1]^2+q[4]^2-q[2]^2-q[3]^2]
end

function test()
    e = [normalize(rand(3)) for _ in 1:3]
    n = [normalize(rand(3)) for _ in 1:3]
    R = findMatrix(e, n)
    println("Error: $(sum(i -> abs(dot(n[i], R * e[i])), 1:3))")
end