% CLHEP/ZOOM Vector Package Euler Angle Computation

\documentclass[twoside,12pt]{article}
\flushbottom
\pagestyle{headings}

\setlength{\topmargin}{0.0in}
\setlength{\textwidth}{5.5in}
\setlength{\oddsidemargin}{.5in}
\setlength{\evensidemargin}{.5in}
\setlength{\textheight}{8.5in}

\addtolength{\parskip}{2pt}

\newcommand{\fpcl}{{\sc fpcl}}

\def \Point {{\tt Point3D}}
\def \Line {{\tt Line3D}}
\def \Direction {{\tt Direction}}
\def \UnitVector {{\tt UnitVector}}
\def \Plane {{\tt Plane3D}}
\def \SpaceVector {{\tt SpaceVector}}
\def \SV {{\tt Hep3Vector}}
\def \SVz {{\tt SpaceVector}}
\def \UV {{\tt UnitVector}}
\def \TangentVector {{\tt TangentVector}}
\def \TV {{\tt TangentVector}}
\def \Ro {{\tt HepRotation}}
\def \Ros {{\tt Rotation}}
\def \Rotation {{\tt Rotation}}
\def \RotationZ {{\tt HepRotationZ}}
\def \Transformation {{\tt Transformation}}
\def \Euclidean {{\tt EuclideanTransformation}}
\def \Angle {{\tt Angle}}
\def \LorentzVector {{\tt LorentzVector}}
\def \LorentzTransformation {{\tt LorentzTransformation}}
\def \LV {{\tt HepLorentzVector}}
\def \LVz {{\tt LorentzVector}}
\def \LT {{\tt HepLorentzRotation}}
\def \LTs {{\tt LorentzTransformation}}
\def \LB {{\tt HepBoost}}
\def \LBs {{\tt LorentzBoost}}
\def \PolarAngle {{\tt PolarAngle}}
\def \PAngle {{\tt PAngle}}
\def \AzimuthalAngle {{\tt AzimuthalAngle}}
\def \AAngle {{\tt AAngle}}
\def \EB {{\tt EBvector}}
\def \AV {{\tt Adjoint3Vector}}
\def \ALV {{\tt Adjoint4Vector}}
\def \Scalar {{\tt Scalar}}
\def \Ax {{\tt HepAxisAngle}}
\def \Es {{\tt HepEulerAngles}}

\newcommand {\see}[1] {\hfill$\triangleright$ see eqn.~#1}

\newenvironment{shortlist}{%
\begin{itemize}
\setlength{\itemsep}{0pt}
\setlength{\parskip}{0pt}
}{%
\end{itemize}
}

\setcounter{secnumdepth}{2}	% Number sub- but not subsubsections
\setcounter{tocdepth}{2}	% Only numbered section headings go in contents

\begin{document}

\title{Euler Angle Computation in the CLHEP {\bf Vector} Package}

\author{Mark Fischler}
\date{Version 1.0, April 24, 2003}
\maketitle

The naive (though algebraically sound) technique for extracting \Es\ 
from a \Ro\ is flawed when the \Ro\ represents a rotation very nearly 
around the Z axis, in the presence of small round-off induced errors 
in the values of the rotation matrix components.
By flawed, we mean that when the resulting Euler angles are used to 
reconstruct the \Ro, that reconstructed \Ro\ may be quite different 
from the original.

This document will illustrate the mechanism by which this flaw comes up,
and will derive and justify a superior algorithm for calculationg the proper
euler angles.

\tableofcontents

\section{The Naive Algorithm}

The relation between the Eueler Angles and a \Ro\ $R$ is given by:

\[  \vec{v}.\mbox{rotate}(\phi, \theta, \psi) \Longrightarrow  \]
\begin{equation}
\label{eq:eulerrot}
\left(
\begin{array}{ccc}
\cos \psi \cos \phi - \sin \psi \cos \theta \sin \phi &
\cos \psi \sin \phi + \sin \psi \cos \theta \cos \phi &
\sin \psi \sin \theta \\
- \sin \psi \cos \phi - \cos \psi \cos \theta \sin \phi &
- \sin \psi \sin \phi + \cos \psi \cos \theta \cos \phi &
\cos \psi \sin \theta \\
\sin \theta \sin \phi &
- \sin \theta \cos \phi &
\cos \theta
\end{array}
\right)
\left(
\begin{array}{c}
v_x\\
v_y\\
v_z
\end{array}
\right)
\end{equation}


As of CLHEP 1.8, the algorithm for comupting Euler angles 
$(\psi, \theta, \phi)$,
glossing over special cases, was as follows:

\begin{enumerate}
\item 
Find $\theta$ by taking $\cos^{-1} R_{zz}$ and  
$\sin \theta = \sqrt{1-R_{zz}^2}$.  By definition, 
$\theta$ is in quadrant I or II, and $\sin \theta \geq 0$.
\item 
Knowing the form of $R_{zy} = - \sin \theta \cos \phi$, extract 
$\cos | \phi | = - R_{zy} / \sin \theta$.  
\item
Similarly, 
$\cos | \psi | = + R_{yz} / \sin \theta$.
\item
Taking the arc cosine of the expressions just computed gives 
$|\phi|$ and $|\psi|$.  The signs of $\psi$ and $\phi$ 
are assigned so as to yield 
proper signs for $R_{zx}$ and $R_{xz}$ respectively; 
if one or both of those is zero then demand instead the proper sign for
$R_{zy}$ and/or $R_{yz}$. 
\item 
The case where $R_{zz} = \pm 1$ is treated specially.  In that case, there
is a freedom to tradeoff  values of $\psi$ and $\phi$, and an arbitrary rule is 
imposed:  $\psi = R_{zz} \phi$.
\end{enumerate}

\section{The Potential Flaw}

The above algorithm is analytically correct, but if one perturbs 
$R_{ij} \rightarrow R_{ij}+\epsilon_{ij}$ for some set of  
$\epsilon_{ij} << 1$, then $\psi \rightarrow \psi + \delta \psi$ and
$\phi \rightarrow \phi + \delta \phi$ where, if $\sin \theta << 1$
then $\delta \psi$ and $\delta \phi$ need not be small.

To illustrate:  Say $R$ represents a rotation by angle $\mu$ about the Z axis,
but $R_{zz}$ is perturbed to be $1 - \epsilon$ 
while none of the other components
in the Z row or column--the components with value 0 for a Z rotation--are 
perturbed.   Now a rotation by angle $\mu$ about the Z axis falls into the 
ambiguous case, where any answer with $\theta << 1$ and $\psi + \phi \sim \mu$
would be acceptable.  

However, the algorithm described above will assign
a tiny {\em but non-zero} value to $\sin \theta$.  So it will decide that the
divisions in $\cos | \psi | = - R_{zy} / \sin \theta$ and 
$\cos | \phi | = + R_{yz} / \sin \theta$ are valid operations.  
Of course, since the numerators are exactly zero, it does not matter that
the denominators are small; these will say that $ |\psi| = \pi /2$
and $ |\phi| = \pi /2$.  

Then the algorithm attempts to determine the signs of $\psi$ and $\phi$; 
and the actual code gets into futher trouble since it knows that $\sin \theta$
is non-zero, thus it is ``impossible'' (yet true) to have all four of 
$R_{xz}, R_{yz}, R_{zx}, R_{zy} = 0$.  
But no matter how clever the algorithm at that point, once it is decided that
$ |\psi| = |\phi| = \pi /2$ there is no way to recover the proper relationship
$\psi + \phi \sim \mu$.  And in fact, if you form a \Ro\ from the computed
Euler angles, you will either get something close to the identity matrix, or 
something close to a 180 degree 
rotation about the Z axis--neither of which resembles
the original matrix in the least!

In general, if the \Ro\ represents any rotation about some axis making an angle 
with the Z axis which is smaller than or comparable to the uncertainty in the
matrix elements, then the algorithm used in CLHEP 1.8 and earlier is flawed
in this way.

\section{Deriving The New Algorithm}

It is true that for $1 - |R_{zz}|$ small, the exact values of Euler angles 
depend  sensitively on the values of all the matrix components.  Thus
the problem of extracting Euler angles for such cases is numerically unstable.
But this is
not fatal; what we want is a set of Euler angles that comes very close to
properly representing the rotation matrix, 
and if other sets of Euler angles would
also come close, we don't mind not finding them.
 
Fortunately, the problem of extracting Euler angles in the presence of 
perturbations of the matrix elements, such that the reconstructed matrix will 
match the original up to differences of the same order as those perturbations,
is tractable and numerically stable, as we now show.  

We start with the observations that 
\begin{eqnarray}
R_{xx} + R_{yy} = + \cos ( \psi + \phi ) (1 + \cos \theta) \\
R_{xx} - R_{yy} = + \cos ( \psi - \phi ) (1 - \cos \theta) 
\end{eqnarray}
If $R_{zz}$ is non-negative, $(1 + \cos \theta) \geq 1$, while 
if $R_{zz}$ is negative, $(1 - \cos \theta) > 1$; so one of the
above equations can always be used to determine $|\psi \pm \phi|$ with no
risk of division by a small quantity.  And if $|R_{zz}|$ is not close to 1,
both equations are numerically stable so both combinations can be found. 

Still working in the X-Y sector, 
\begin{eqnarray}
R_{xy} - R_{yx} = + \sin ( \psi + \phi ) (1 + \cos \theta) \\
R_{xy} + R_{yx} = - \sin ( \psi - \phi ) (1 - \cos \theta) 
\end{eqnarray}

We also observe, in the Z sector, that
\begin{eqnarray}
R_{xz} R_{zx} - R_{yz} R_{zy} = + \cos ( \psi - \phi ) (\sin^2 \theta) \\
R_{xz} R_{zx} + R_{yz} R_{zy} = - \cos ( \psi + \phi ) (\sin^2 \theta)
\end{eqnarray}

And 
\begin{eqnarray}
R_{xz} R_{zy} + R_{yz} R_{zx} = - \sin ( \psi - \phi ) (\sin^2 \theta) \\
R_{xz} R_{zy} - R_{yz} R_{zx} = - \sin ( \psi + \phi ) (\sin^2 \theta)
\end{eqnarray}

Finally, we note that if we can determine cosine and sine of the sum and the 
difference of $\psi$ and $\phi$ in a numerically stable manner, then that
determines $\psi$ and $\phi$ in a stable manner. 

All the above relations are all stable except when $1- |R_{zz}| \sim 0$.
So except when $1- |R_{zz}| \sim 0$ we can use either the Z sector, 
or the X-Y sector, or for
that matter the simple $R_{xz}/\sin \theta$ and $R_{yz}/\sin \theta$ forms
used by the naive algortihm.

When $R_{zz} \sim 1$, the entire Z sector becomes numerically unstable, 
as do the relations determining $\cos ( \psi - \phi )$ and 
$\sin ( \psi - \phi )$ in the X-Y sector.  
That is not so terrible, since at $R_{zz} = 1$ only the value of 
$\psi + \phi$ is relevant; $\psi - \phi$ is moot.

Similarly, when $R_{zz} \sim -1$, the  Z sector is numerically unstable, 
and the relations determining $\cos ( \psi + \phi )$ and 
$\sin ( \psi + \phi )$ in the X-Y sector are useless, but only the value of 
$\psi - \phi$ is relevant; $\psi + \phi$ is moot.
 
So what is needed is some method which always gives the correct values
for meaningful quantities, and sensible values for the
moot quantities.
The way to accomplish this is to combine the relations determining $\sin$ and
$\cos$ of the ``moot'' quantities, so as to find (for the $R_{zz} \sim 1$ case)
$\psi - \phi = \tan^{-1} \frac{ -(R_{xy} + R_{yx}) }{ R_{xx} - R_{yy} }$. 
In this fraction, the factors of $1 - \cos \theta$ cancel one another. 

Computing $\psi - \phi $ as an $\arctan$ would force it to lie between $-\pi/2$
and $\pi/2$.  How can we detect when the correct value is outside that range?
We could look at the sign of $\sin (\psi - \phi)$, 
that is to say, the sign of $-(R_{xy} + R_{yx})$.  This would tell us
whether to use the result of $\arctan$ directly or to add or subtract $\pi$.
On the other hand, it is much easier to use the function {\em atan2(s/c)},
which does this work for you.

One would expect from the above reasoning that when $R$ is slightly perturbed
in a pseudo-random manner,
the formulae given would reproduce the original Euler angles more closely than 
the ``naive'' formulae described earlier.  A bit of mathematical experimentation
verifies that this is so--the new formulae are about an order of magnitude
less sensitive to fluctuations which violate the orthogonality of $R$.

Two improvements are tempting, but turn out to be harmful.  Both address the
issue of attempting to morph smoothly to the action taken when $|R_{zz}| = 1$
precisely (in which case we by fiat choose $\psi = R_{zz} \phi$).   Sometimes
in computing the potentially moot combination of $\psi$ and $\phi$, 
the magnitude of the value computed for 
$\cos (\psi \pm \phi) (1 \mp \cos \theta)$ exceeds $(1 \mp \cos \theta)$.  
It may then be thought that the arithmetic is telling you to consider that 
denominator as illogically large, and just treat the arctan of that 
combination as zero (that is, force $\psi = \pm \phi$).  This turns out to
introduce a flaw in the result (in the sense discussed above) most times that
this ``improvement'' is applied.  Similarly, one can check that the sign of the 
X-Y sector expression for $\sin(\psi \pm \phi)$ matches the sign of the
expression for the same $\sin(\psi \pm \phi)$ computed from components in the Z
sector, and if they do not match, treat this as an indication that the
combination is moot and set $\psi = \pm \phi$.  Again, most times this
opportunity presents itself, setting $\psi = \pm \phi$ introduces a flaw 
into the answer.  

However, one correction of significance must be applied:
The algorithm described will always impute values for $\psi \pm \phi$ 
which are in the range $(-\pi,\pi]$.  But clearly the actual values of 
$\psi$ and $\phi$ can can have a sum or difference anywhere in $(-2\pi,2\pi]$.
Getting this wrong will manifest itself as an error by $\pi$ in the
computed values of $\psi$ and $\phi$.

This can be dealt with by bringing in information in the Z-sector:
If $\psi$ (or $\phi$) has the wrong sign relative to that which can be read off
the last column or row, then one should add or subtract $\pi$ from {\em both} 
$\psi$ and $\phi$.
Here, when $|R_{zz}| \sim 1$, there is the potential for numerical instability;
fortunately, in such cases only the value of one combination $\psi \pm \phi$
is relevant when re-expressing the rotation matrix.  So as long as the
correction is applied to both $\psi$ and $\phi$ or neither, the result (in
such a case) is not truly flawed.

There are, of course, four possible terms in the Z-sector to use to ask whether
we have the wrong value for $\psi$ and $\phi$:
\begin{enumerate}
\item
$R_{xz}$: If $\sin \psi > 0$ then $\psi > 0$.
\item
$R_{yz}$: If $\cos \psi > 0$ then $|\psi| < \pi/2$.
\item
$R_{zx}$: If $\sin \phi > 0$ then $\phi > 0$.
\item
$R_{yz}$: If $-\cos \phi < 0$ then $|\psi| < \pi/2$.
\end{enumerate}

The most stable algorithm will use that quantity among those four which is
largest in absolute value.
 

\section{The New Algorithm}

The algorithm uses only the X-Y sector and $R_{zz}$.
$\cos \theta = R_{zz}$ and this determines $\theta$.
 
\noindent
If $ R_{zz} \geq 0$ the primary stable quantity is $\psi + \phi$:
\begin{enumerate}
\item
Compute $\psi + \phi = \tan^{-1} \frac{R_{xy} - R_{yx}}{R_{xx} + R_{yy}}$.
With the numerator and denominator in this form we can get the 
proper value of $\psi + \phi$ (including the proper quadrant) by using the
{\em atan2(s/c)} function.
\item
Compute $\psi - \phi = \tan^{-1} \frac{-(R_{xy} + R_{yx})}{R_{xx} - R_{yy}}$.
\end{enumerate}

\noindent
If $ R_{zz} < 0$ the primary stable quantity is $\psi - \phi$:
\begin{enumerate}
\item
Compute $\psi - \phi = \tan^{-1} \frac{-(R_{xy} + R_{yx})}{R_{xx} - R_{yy}}$.
We put the numerator and denominator in this form so that we can get the 
proper value of $\psi - \phi$ (including the proper quadrant) by using the
{\em atan2(s/c)} function.
\item
Compute $\psi + \phi = \tan^{-1} \frac{R_{xy} - R_{yx}}{R_{xx} + R_{yy}}$.
\end{enumerate}

\noindent
Then, in either case, compute
$\psi = \frac{1}{2} \left[ ( (\psi + \phi) + (\psi - \phi) \right]$ and
$\phi = \frac{1}{2} \left[ ( (\psi + \phi) - (\psi - \phi) \right]$.

\noindent
Finally, apply the potential $\pm \pi$ corrections, based on the largest of 
$|R_{xz}|,|R_{yz}|,|R_{zx}|,|R_{zy}|$: 
\begin{enumerate}
\item
$If R_{xz} > 0$ and $\psi < 0$ then correct $\psi$ and $\phi$.
\item
$If R_{yz} > 0$ and $|\psi| > \pi/2$ then correct $\psi$ and $\phi$.
\item
$If R_{zx} > 0$ and $\phi < 0$ then correct $\psi$ and $\phi$.
\item
$If R_{zy} < 0$ and $|\psi| > \pi/2$ then correct $\psi$ and $\phi$.
\end{enumerate}
\noindent
In each case, to ``correct'' a quantity means to add $\pi$ if negative, or to
subtract $\pi$ if positive.

\section{Accuracy of a Reconstricted R}

Ultimately,if one perturbs 
$R_{ij} \rightarrow R_{ij}+\epsilon_{ij}$ for some set of  
$\epsilon_{ij}$ each of which is of order $\epsilon << 1$,
then one has a non-orthogonal matrix.
There is no reason to believe that one can perfectly extract Euler angles
from such a matrix, and in fact, any manner of approximating the Euler angles
will have the following property:
A matrix reconstructed from those Euler angles will be perfectly orthogonal
(to within achine roundoff errors) and therefore {\em cannot precisely 
match the original matrix}.

The naive size of the resulting unavoidable ``flaw'' is of order 
$\epsilon$, but in fact (except in special cases such as $\theta \sim \pi/2$)
in general the flaw goes as $\sqrt{\epsilon}$.  This can easily be seen
from the fact that when $R_{zz} = \cos \theta $ 
is perturbed by $\epsilon$, unless
$R_{zz}$ is very small, $\sin \theta$ is perturbed by $O(\sqrt{\epsilon})$.
And $\sin \theta$  does appear multiplying terms in the Z row and column, 
so at least these terms are vulnerable to errors of order $O(\sqrt{\epsilon})$.
 
\end{document}