Date: May 4, 2013 8:44 AM
Author: Bruno Luong
Subject: Re: Using Solve/FSolve for Multiple Trig Equations

Here is the code using Euler's angles:

% Fake data
y = rand*2*pi;
r = rand*2*pi;
p = rand*2*pi;
I = sin(y) * sin(r) + cos(y) * sin(p) * cos(r);
J = -sin(y) * cos(r) + cos(y) * sin(p) * sin(r);
K = cos(y) * cos(p);
clear y r p;

% Solve
u = [0 0 1]';
v = [-J,I,K]';
v = v/norm(v);
% (Random) Rotation solution of (R*u) = v
w = 0.5*(u + v);
q = cross(u,v);
f = randn(); % free parameter, any number will work
q = q + f*w;
q = q / norm(q);
d = dot(u, q);
c = q*d;
u1 = u-c;
u1 = u1/norm(u1);
v1 = v-c;
v1 = v1/norm(v1);
% Rodrigues's formula:
k = cross(u1, v1);
costheta = dot(u1,v1);
R =[ 0 -k(3) k(2);
k(3) 0 -k(1);
-k(2) k(1) 0];
R = costheta*eye(3) + R + k*k'*(1-costheta)/sum(k.^2);
r = -atan2(R(1,2),R(2,2));
p = -asin(R(3,2));
y = -atan2(R(3,1),R(3,3));

% Check solution
II = (sin(y) * sin(r) + cos(y) * sin(p) * cos(r));
JJ = (-sin(y) * cos(r) + cos(y) * sin(p) * sin(r));
KK = (cos(y) * cos(p));
norm([I J K]-[II JJ KK]) % should be small

% Bruno