using LinearAlgebra """ quadraticSolver(A, b, x; maxiter, tolerance) 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; maxiter = 100, tolerance = 1.0e-8) n = length(A) fn(x) = [x' * A[k] * x for k in 1:n] - b dfn(x) = [sum(i -> (A[k][i,l] + A[k][l,i]) * x[i], 1:n) for k in 1:n, l in 1:n] for _ in 1:maxiter delta = inv(dfn(x)) * fn(x) x -= delta norm(delta) < norm(x) * tolerance && break end x end """ findMatrix(e, 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