Align two 3D vectors using Euler Angles ($\alpha_x,\alpha_y,\alpha_z$)

Assume there are two 3D vectors $p$ and $q$ and $p$ needs to be aligned (point in the same direction as) to $q$.

First, let’s extract the axis-angle representation $k$ and $\theta$:

$
\begin{eqnarray}
k &=& p \times q \\
k &=& \frac{k}{||k||} \\
p &=& \frac{p}{||p||} \\
q &=& \frac{q}{||q||} \\
\theta &=& cos^{-1}(p \cdot q)
\end{eqnarray} $

Convert axis-angle ($k$ and $\theta$) to a matrix $R$

$\begin{eqnarray}
K &=&
\left[ \begin{array}{ccc}
0 & k(3) & k(2) \\
k(3) & 0 & -k(1) \\
-k(2) & k(1) & 0
\end{array} \right] \\
R &=& e^{\theta K} \\
R &=& I +sin(\theta)K + (1-cos(\theta))K^2
\end{eqnarray} $

Convert matrix $R$ to Euler angles ($\alpha_x,\alpha_y,\alpha_z$)

$\begin{eqnarray}
\alpha_x &=& atan2(R(3,2), R(3,3)) \\
\alpha_y &=& atan2(-R(3,1), \sqrt{R(3,2)^2 + R(3,3)^2}) \\
\alpha_z &=& atan2(R(2,1), R(1,1))
\end{eqnarray} $

Verify Results

$\begin{eqnarray}
M_x &=&
\left[ \begin{array}{ccc}
1 & 0 & 0 \\
0 & cos(\alpha_x) & -sin(\alpha_x) \\
0 & sin(\alpha_x) & cos(\alpha_x)
\end{array} \right] \\
M_y &=&
\left[ \begin{array}{ccc}
cos(\alpha_y) & 0 & sin(\alpha_y) \\
0 & 1 & 0 \\
-sin(\alpha_y) & 0 & cos(\alpha_y)
\end{array} \right] \\
M_z &=&
\left[ \begin{array}{ccc}
cos(\alpha_z) & -sin(\alpha_z) & 0 \\
sin(\alpha_z) & cos(\alpha_z) & 0 \\
0 & 0 & 1
\end{array} \right] \\
p_{rot} &=& M_zM_yM_xp \\
p_{rot} &=& \frac{p_{rot}}{||p_{rot}||} \\
q &=& \frac{q}{||q||} \\
metric &=& atan2(||p_{rot} \times q||, p_{rot} \cdot q)
\end{eqnarray} $

Metric should be zero.

Leave a Reply

Your email address will not be published.