Finding Axes and Angles of Rotation from 3D Rotation Matrix

1. Any given 3D Rotation Matrix can be converted to A Pair of Axis Vectors and Corresponding Rotation Angles around those Axis Vectors. If one of the Axis Vector is Vector \(\vec{A}\) with Counter Clockwize Rotation Angle \(\phi\), the other Axis Vector is Vector \(-\vec{A}\) with Counter Clockwize Rotation Angle \(2\pi - \phi\) (where \(0 \leq \phi < 2\pi\)).
2. The formula for General 3D Rotation Matrix \(R\) is given as
   
   \(R=\begin{bmatrix} R[0][0] & R[0][1] & R[0][2] \\R[2][0] & R[1][1] & R[1][2]\\R[2][0]& R[2][1]& R[2][2]\end{bmatrix}= \begin{bmatrix} tX^2 + c & tXY - sZ & tXZ + sY\\tXY + sZ & tY^2 + c & tYZ - sX\\tXZ - sY & tYZ + sX & tZ^2 + c\end{bmatrix}\)   ...(1)
   
   where \(c = \cos \phi, s = \sin \phi, t = 1-\cos \phi\),  \(\phi\) is the Angle of Rotation and \(X\), \(Y\) and \(Z\) are the Components of Unit Vector representing the Axis Vector of Rotation.
3. The following gives the steps / pseudocode for Finding Axes and Angles from Rotation Matrix as given in equation (1) above
   1. Find the value of \(\cos \phi\) as follows
      
      \(\cos \phi = {\Large \frac{R[0][0] + R[1][1] + R[2][2] - 1}{2}}\)
      
      \(\Rightarrow \cos \phi = {\Large \frac{tX^2 + c + tY^2 + c + tZ^2 + c - 1}{2}}\)
      
      \(\Rightarrow \cos \phi = {\Large \frac{t(X^2 + Y^2 + Z^2) + 3c - 1}{2}}\)
      
      Since \(X^2 + Y^2 + Z^2=1\) therefore,
      
      \(\cos \phi = {\Large \frac{t + 3c - 1}{2}}\)
      
      Since \(t=1-c\) therefore,
      
      \(\cos \phi = {\Large \frac{1-c + 3c - 1}{2}}\)
      
      \(\Rightarrow \cos \phi = {\Large \frac{2c}{2}}=c\)
   2. If \(\cos \phi=1\) then \(\phi=0\). This makes \(t=0\) and \(\sin\phi=0\). Under such circumstances the Rotation Matrix is an Identity Matrix and Any Arbitraty Vector can be given as the Axis.
   3. If \(\cos \phi=-1\) then \(\phi=\pi=180^\circ\). This makes \(t=2\) and \(\sin\phi=0\). To find the Axis, find \(X^2\), \(Y^2\), \(Z^2\) as follows
      
      \(X^2 = {\Large \frac{R[0][0] + 1}{2}}= {\Large \frac{tX^2 + c +1}{2}}={\Large \frac{2X^2 -1 + 1}{2}}\)
      
      \(Y^2 = {\Large \frac{R[1][1] + 1}{2}}= {\Large \frac{tY^2 + c +1}{2}}={\Large \frac{2Y^2 -1 + 1}{2}}\)
      
      \(Z^2 = {\Large \frac{R[2][2] + 1}{2}}= {\Large \frac{tZ^2 + c +1}{2}}={\Large \frac{2Z^2 -1 + 1}{2}}\)
      
      If \(X^2 \neq 0\) then find the 2 values of \(X\). For each value of \(X\) find the values of \(Y\) and \(Z\) as follows
      
      \(Y={\Large \frac{R[0][1]}{2X}}\)
      
      \(Z={\Large \frac{R[0][2]}{2X}}\)
      
      If \(X^2 = 0\) but \(Y^2 \neq 0\) then find the 2 values of \(Y\). For each value of \(Y\) find the values of \(X\) and \(Z\) as follows
      
      \(X={\Large \frac{R[1][0]}{2Y}}\)
      
      \(Z={\Large \frac{R[1][2]}{2Y}}\)
      
      If \(X^2 = 0\) and \(Y^2 = 0\) but \(Z^2 \neq 0\) then find the 2 values of \(Z\). For each value of \(Z\) find the values of \(X\) and \(Y\) as follows
      
      \(X={\Large \frac{R[2][0]}{2Z}}\)
      
      \(Y={\Large \frac{R[2][1]}{2Z}}\)
      
      The 2 Sets of \(X\), \(Y\) and \(Z\) values thus obtained give the Components of the 2 Unit Vectors for the 2 Axes of Rotation. Both the Unit Vectors are in Opposite Direction to each other.
   4. If \(\cos \phi\neq 1\) and \(\cos \phi\neq -1\) then calculate the value of \(t=1-\cos\phi\). To find the Axes and Angles of Rotation, find \(X^2\), \(Y^2\), \(Z^2\) as follows
      
      \(X^2 = {\Large \frac{R[0][0] - c}{t}}= {\Large \frac{tX^2 + c -c}{t}}={\Large \frac{tX^2}{t}}\)
      
      \(Y^2 = {\Large \frac{R[1][1] - c}{t}}= {\Large \frac{tY^2 + c -c}{t}}={\Large \frac{tY^2}{t}}\)
      
      \(Z^2 = {\Large \frac{R[2][2] - c}{t}}= {\Large \frac{tZ^2 + c -c}{t}}={\Large \frac{tZ^2}{t}}\)
      
      If \(X^2 \neq 0\) then find the 2 values of \(X\). For each value of \(X\) find the values of \(\sin \phi\), \(\phi\), \(Y\) and \(Z\) as follows
      
      \(s=\sin \phi= {\Large \frac{R[2][1] - R[1][2]}{2X}}\)
      
      \(\phi=m\_arctan2(\cos \phi,\sin \phi)\)
      
      \(Y= {\Large \frac{R[0][2] - R[2][0]}{2s}}\)
      
      \(Z= {\Large \frac{R[1][0] - R[0][1]}{2s}}\)
      
      If \(X^2 = 0\) but \(Y^2 \neq 0\) then find the 2 values of \(Y\). For each value of \(Y\) find the values of \(\sin \phi\), \(\phi\), \(X\) and \(Z\) as follows
      
      \(s=\sin \phi= {\Large \frac{R[0][2] - R[2][0]}{2Y}}\)
      
      \(\phi=m\_arctan2(\cos \phi,\sin \phi)\)
      
      \(X= {\Large \frac{R[2][1] - R[1][2]}{2s}}\)
      
      \(Z= {\Large \frac{R[1][0] - R[0][1]}{2s}}\)
      
      If \(X^2 = 0\) and \(Y^2 = 0\) but \(Z^2 \neq 0\) then find the 2 values of \(Z\). For each value of \(Z\) find the values of \(\sin \phi\), \(\phi\), \(X\) and \(Y\) as follows
      
      \(s=\sin \phi= {\Large \frac{R[1][0] - R[0][1]}{2Z}}\)
      
      \(\phi=m\_arctan2(\cos \phi,\sin \phi)\)
      
      \(X= {\Large \frac{R[2][1] - R[1][2]}{2s}}\)
      
      \(Y= {\Large \frac{R[0][2] - R[2][0]}{2s}}\)
      
      The 2 Sets of \(X\), \(Y\) \(Z\) and \(\phi\), values thus obtained give the Components of the 2 Unit Vectors for the 2 Axes of Rotation and the Corresponding Angles of Rotation. Both the Unit Vectors are in Opposite Direction to each other. If one of the Angles Values obtained is \(\theta\) the other is \(2\pi - \theta\).
      
      The \(m\_arctan2\) is the Modified Arctangent Function, which calculates the angle \(\phi\) such that \(0 \leq \phi < 2\pi\).