function [A B] = EulerAngsFromRot(M)
% M is a rotation matrix and A and B are angle triples which can produce M.
% There are 12 possible conventions for using three angles of rotation
% around three axis choices to represent a rotation, and all may be
% called ``Euler Angles.''
% These 12 are the possibilities XYX, XYZ, ... where ABC represents ``first
% rotate around C-axis, then rotate around new B-axis and then rotate around
% doubly-new A-axis'' and no two consecutive axis choices are to be identical.
% Interestingly, the robot manipulator and mobile robot communities seem to
% use different conventions: XYZ and ZYX, respectively.
% The words ``roll, pitch and yaw'' are often used for certain of these angles.
% For instance, if you are sitting in an airplane at the origin with the
% airplane nose directed along the positive x-axis, left wing along the
% positive y-axis and positive z-axis ``up'' the word ``roll'' refers to
% rotation around the x-axis, ``pitch'' is rotation around the new y-axis
% and ``yaw'' is rotation around the doubly-new z-axis, the ZYX combination.
% We use this choice here.
% Still, Euler Angles are not unique-there are at least two (we produce them)
% geometrically distinct (and in the case of gimbal lock, infinitely many)
% angle triples that will reproduce a given rotation matrix.
% We use the convention [psi,theta, phi] to represent rotation angles
% around x-axis, new y-axis and doubly-new z-axis, respectively.
% So M should be R_z(phi)*R_y(theta)*R_x(psi) where the R_# matrices are
% the individual rotation matrices around the indicated axes.
% It is customary to require theta to be between +/- pi/2 and the other
% angles to be between +/-pi.
if ~(M(3,1)==1 || M(3,1)==-1)
theta1=-asin(M(3,1));
theta2=pi-theta1;
psi1=atan2(M(3,2)/cos(theta1),M(3,3)/cos(theta1));
psi2=atan2(M(3,2)/cos(theta2),M(3,3)/cos(theta2));
phi1=atan2(M(2,1)/cos(theta1),M(1,1)/cos(theta1));
phi2=atan2(M(2,1)/cos(theta2),M(1,1)/cos(theta2));
elseif M(3,1)==-1 fprintf('warning:gimbal lock (The first and third angles are dependent, but one can be chosen arbitrarily. The difference between first and third angles will be constant.)')
phi1=0;
phi2=pi;
theta1=pi/2;
theta2=pi/2;
psi1=phi1+atan2(M(1,2),M(1,3));
psi2=phi2+atan2(M(1,2),M(1,3));
else fprintf('warning:gimbal lock (The first and third angles are dependent, but one can be chosen arbitrarily. The difference between first and third angles will be constant.)')
phi1=0;
phi2=pi;
theta1=-pi/2;
theta2=-pi/2;
psi1=-phi1+atan2(-M(1,2),-M(1,3));
psi2=-phi2+atan2(-M(1,2),-M(1,3));
end
A=[psi1 theta1 phi1];
B=[psi2 theta2 phi2];
end