function [angl] = compute_angles_from_Q(Q)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [angl] = compute_angles_from_Q(Q)
%
% Given a 3x3 orthogonal matrix Q, find the angles that
% define plane rotations that reduce to zero
%    the (2,1)  element to (angle phi)
%    the (3,1)  element to (angle theta)
%    the (3,2)  element to (angle psi)
% and return the angles as the three components of the
% column vector angl.
% The angles phi and psi are in the range (-pi,pi]
% while theta is in the range (-pi/2,pi/2).
%
%  compute_angles_from_Q.m Dianne P. O'Leary   06/2004
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Qn = Q;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the Euler angles phi, theta, psi.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

phimat = planerot(Qn(1,1:2)');
phi = atan(phimat(1,2)/phimat(1,1));
if (phimat(1,1) < 0)
   phi = pi + phi;
end
Qyaw  = eye(3);
Qyaw (1:2,1:2) = phimat;
Qn = Q*Qyaw';

thetamat = planerot(Qn(1,[1,3])');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Make sure that the cosine is positive, to keep theta in 
% theta in range.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (thetamat(1,1) < 0)
  thetamat = -thetamat;
end
theta = atan(thetamat(2,1)/thetamat(1,1));
Qpitch = eye(3);
Qpitch([1,3],[1,3]) = thetamat;
Qn = Qn*Qpitch';

psimat = planerot(Qn(2,2:3)');
psi = atan(psimat(1,2)/psimat(1,1));
if (psimat(1,1) < 0)
   psi = pi + psi;
end
Qroll = eye(3);
Qroll(2:3,2:3) = psimat;
Qn = Qn*Qroll';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Make sure that phi and psi are in range.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (phi > pi)
   phi = phi - 2*pi;
end

if (psi > pi)
   psi = psi - 2*pi;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Return the vector angl.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

angl = [phi;theta;psi];