Linear Algebra and Geometry – How to Compute Rotation Degrees from Transformation Matrix in 3D Space

3dgeometrylinear algebralinear-transformationsvector-spaces

Assume I have 3 original points in a 3D object (in 3D space) as A1=<xa,ya,za>, A2=<xa,ya,za>, and A3=<xa,ya,za>. The 3D object is moved and rotated in the 3D space, and the destination points in that object become B1=<xb,yb,zb>, B2=<xb,yb,zb>, and B3=<xb,yb,zb>.

How can I compute the transformation matrix and following that, the degree of rotation in each axis? Basically, I need a matrix that if applied to all points, I get the displaced object.

Best Answer

Let us call the transformation matrix $\mathbf{M}$, $$\mathbf{M} = \left [ \begin{matrix} m_{11} & m_{12} & m_{13} \\ m_{21} & m_{22} & m_{23} \\ m_{32} & m_{32} & m_{33} \end{matrix} \right ]$$ your three original vectors $$\begin{array}{l} \vec{p}_1 = ( x_1 , y_1 , z_1 ) \\ \vec{p}_2 = ( x_2 , y_2 , z_2 ) \\ \vec{p}_3 = ( x_3 , y_3 , z_3 ) \end{array}$$ and their corresponding transformed vectors $$\begin{array}{l} \vec{p}_4 = ( x_4 , y_4 , z_4 ) \\ \vec{p}_5 = ( x_5 , y_5 , z_5 ) \\ \vec{p}_6 = ( x_6 , y_6 , z_6 ) \end{array}$$ so that $$\begin{cases} \mathbf{M} \vec{p}_1 = \vec{p}_4 \\ \mathbf{M} \vec{p}_2 = \vec{p}_5 \\ \mathbf{M} \vec{p}_3 = \vec{p}_6 \end{cases}$$ Let $d$ be the triple product of the first three vectors, $$d = \vec{p}_1 \cdot \left ( \vec{p}_2 \times \vec{p}_3 \right ) = x_1 ( y_2 z_3 - y_3 z_2 ) + x_2 ( y_3 z_1 - y_1 z_3 ) + x_3 ( y_1 z_2 - y_2 z_1 )$$ If $d \ne 0$, there exists a solution: $$\begin{cases} m_{11} = \left ( x_4 ( y_2 z_3 - z_2 y_3 ) + x_5 ( z_1 y_3 - y_1 z_3 ) + x_6 ( y_1 z_2 - z_1 y_2 ) \right ) / d \\ m_{12} = \left ( x_4 ( z_2 x_3 - x_2 z_3 ) + x_5 ( x_1 z_3 - z_1 x_3 ) + x_6 ( z_1 x_2 - x_1 z_2 ) \right ) / d \\ m_{13} = \left ( x_4 ( x_2 y_3 - y_2 x_3 ) + x_5 ( y_1 x_3 - x_1 y_3 ) + x_6 ( x_1 y_2 - y_1 x_2 ) \right ) / d \\ m_{21} = \left ( y_4 ( y_2 z_3 - z_2 y_3 ) + y_5 ( z_1 y_3 - y_1 z_3 ) + y_6 ( y_1 z_2 - z_1 y_2 ) \right ) / d \\ m_{22} = \left ( y_4 ( z_2 x_3 - x_2 z_3 ) + y_5 ( x_1 z_3 - z_1 x_3 ) + y_6 ( z_1 x_2 - x_1 z_2 ) \right ) / d \\ m_{23} = \left ( y_4 ( x_2 y_3 - y_2 x_3 ) + y_5 ( y_1 x_3 - x_1 y_3 ) + y_6 ( x_1 y_2 - y_1 x_2 ) \right ) / d \\ m_{31} = \left ( z_4 ( y_2 z_3 - z_2 y_3 ) + z_5 ( z_1 y_3 - y_1 z_3 ) + z_6 ( y_1 z_2 - z_1 y_2 ) \right ) / d \\ m_{32} = \left ( z_4 ( z_2 x_3 - x_2 z_3 ) + z_5 ( x_1 z_3 - z_1 x_3 ) + z_6 ( z_1 x_2 - x_1 z_2 ) \right ) / d \\ m_{33} = \left ( z_4 ( x_2 y_3 - y_2 x_3 ) + z_5 ( y_1 x_3 - x_1 y_3 ) + z_6 ( x_1 y_2 - y_1 x_2 ) \right ) / d \end{cases}$$

If $\mathbf{M}$ is a pure rotation matrix, it will be orthogonal. If we use $\hat{r}_i = ( m_{i 1} ,\, m_{i 2} ,\, m_{i 3} )$ and $\hat{c}_j = ( m_{1 j} ,\, m_{2 j} ,\, m_{3 j} )$, and $\mathbf{M}$ is orthogonal, then $$\begin{array}{l|l|l|l} \hat{r}_1 \cdot \hat{r}_1 = 1 & \hat{c}_1 \cdot \hat{c}_1 = 1 & \hat{r}_1 \times \hat{r}_1 = 0 & \hat{c}_1 \times \hat{c}_1 = 0 \\ \hat{r}_1 \cdot \hat{r}_2 = 0 & \hat{c}_1 \cdot \hat{c}_2 = 0 & \hat{r}_1 \times \hat{r}_2 = \hat{r}_3 & \hat{c}_1 \times \hat{c}_2 = \hat{c}_3 \\ \hat{r}_1 \cdot \hat{r}_3 = 0 & \hat{c}_1 \cdot \hat{c}_3 = 0 & \hat{r}_1 \times \hat{r}_3 = -\hat{r}_2 & \hat{c}_1 \times \hat{c}_3 = -\hat{c}_2 \\ \hline \hat{r}_2 \cdot \hat{r}_1 = 0 & \hat{c}_2 \cdot \hat{c}_1 = 0 & \hat{r}_2 \times \hat{r}_1 = -\hat{r}_3 & \hat{c}_2 \times \hat{c}_1 = -\hat{c}_3 \\ \hat{r}_2 \cdot \hat{r}_2 = 1 & \hat{c}_2 \cdot \hat{c}_2 = 1 & \hat{r}_2 \times \hat{r}_2 = 0 & \hat{c}_2 \times \hat{c}_2 = 0 \\ \hat{r}_2 \cdot \hat{r}_3 = 0 & \hat{c}_2 \cdot \hat{c}_3 = 0 & \hat{r}_2 \times \hat{r}_3 = \hat{r}_1 & \hat{c}_2 \times \hat{c}_3 = \hat{c}_1 \\ \hline \hat{r}_3 \cdot \hat{r}_1 = 0 & \hat{c}_3 \cdot \hat{c}_1 = 0 & \hat{r}_3 \times \hat{r}_1 = \hat{r}_2 & \hat{c}_3 \times \hat{c}_1 = \hat{c}_2 \\ \hat{r}_3 \cdot \hat{r}_2 = 0 & \hat{c}_3 \cdot \hat{c}_2 = 0 & \hat{r}_3 \times \hat{r}_2 = -\hat{r}_1 & \hat{c}_3 \times \hat{c}_2 = -\hat{c}_1 \\ \hat{r}_3 \cdot \hat{r}_3 = 1 & \hat{c}_3 \cdot \hat{c}_3 = 1 & \hat{r}_3 \times \hat{r}_3 = 0 & \hat{c}_3 \times \hat{c}_3 = 0 \end{array}$$

When you are satisfied you have an orthogonal matrix, pick the Euler or Tait-Bryan angles you wish to use, you can extract the angles by inspecting the matrix $\mathbf{M}$.

I personally do not use Euler angles or Tait-Bryan angles, because they are confusing (no single definition; you need to specify the rotation axes and their order, as Euler angles or Tait-Bryan angles refer to a set of definitions, not one definition) and problematic (gimbal lock in particular). I personally use versors and rotation matrices, and occasionally the axis-angle representation and Rodrigues' rotation formula.


One practical way to split a transformation back into a translation and rotation is to define the rotation as rotation around one of the vectors. This yields translation before a rotation, a rotation, and a translation after the rotation.

Let's assume we know three vectors before the transformation, $\vec{a}_1$, $\vec{a}_2$, and $\vec{a}_3$, and the three vectors after the transformation, $\vec{b}_1$, $\vec{b}_2$, $\vec{b}_3$, but not the transformation itself -- not the rotation $\mathbf{R}$ nor the translation after rotation $\vec{t}$: $$\begin{array}{c} \mathbf{R}\vec{a}_1 + \vec{t} = \vec{b}_1 \\ \mathbf{R}\vec{a}_2 + \vec{t} = \vec{b}_2 \\ \mathbf{R}\vec{a}_3 + \vec{t} = \vec{b}_3 \end{array}$$ Since $\mathbf{R}$ describes a pure rotation, it is an orthogonal matrix.

We can split the above into pre- and post-translation thus: $$\begin{array}{c} \mathbf{R}\left(\vec{a}_1 - \vec{t}_1 \right ) + \vec{t}_2 = \vec{b}_1 \\ \mathbf{R}\left(\vec{a}_2 - \vec{t}_1 \right ) + \vec{t}_2 = \vec{b}_2 \\ \mathbf{R}\left(\vec{a}_3 - \vec{t}_1 \right ) + \vec{t}_2 = \vec{b}_3 \end{array}$$ with $\vec{t} = \vec{t}_2 - \mathbf{R} \vec{t}_1$.

Now, if we pick $\vec{t}_1 = \vec{a}_i$ and $\vec{t}_2 = \vec{b}_i$, with $i$ being any of $1$, $2$, or $3$, and solve $\mathbf{R}$ using the other two vector pairs relative to $\vec{a}_i$ and $\vec{b}_i$, we will find a rotation that combined with the specified translation fulfills the requirements. Depending on the vectors and the rotation -- and in practice this is quite sensitive to rounding errors and such --, there may be multiple solutions. However, this should find a valid solution, unless the original vectors are parallel or degenerate.

For simplicity, let's pick $\vec{t}_1 = \vec{a}_1$ and $\vec{t}_2 = \vec{b}_1$. We now have a pure rotation $\mathbf{R}$, such that $$\begin{array}{c} \mathbf{R}\left(\vec{a}_2 - \vec{a}_1\right) = \vec{b}_2 - \vec{b}_1 \\ \mathbf{R}\left(\vec{a}_3 - \vec{a}_1\right) = \vec{b}_3 - \vec{b}_1 \end{array}$$ The simplest way to construct $\mathbf{R}$ is to find the matrix $\mathbf{R}_1$ that rotates the coordinate system to match that used by the $a$ vectors, and $\mathbf{R}_2$ that rotates the coordinate system to match that used by the $b$ vectors. Then, we can express $\mathbf{R}$ as the inverse rotation to $\mathbf{R}_1$ followed by rotation $\mathbf{R}_2$.

Since rotation matrices are orthogonal, their inverse is their transpose, so $$\mathbf{R} = \mathbf{R}_2 \mathbf{R}_1^T$$

We can construct $\mathbf{R}_1$ and $\mathbf{R}_2$ by orthonormalizing the two vectors we have; the third basis vector is then the cross product of the first two.

Let $$\mathbf{R}_1 = \left [ \begin{matrix} \hat{e}_1 & \hat{e}_2 & \hat{e}_1\times\hat{e}_2 \end{matrix} \right ]$$ and $$\mathbf{R}_2 = \left [ \begin{matrix} \hat{e}_4 & \hat{e}_5 & \hat{e}_4\times\hat{e}_5 \end{matrix} \right ]$$ ie. $\hat{e}_i$ are the column vectors (basis unit vectors) in the rotation matrices.

We can use the first leftover vector to define the $x$ axis unit vector, $$\hat{e}_1 = \frac{\vec{a}_2 - \vec{a}_1}{\lVert \vec{a}_2 - \vec{a}_1 \rVert} $$ $$\hat{e}_4 = \frac{\vec{b}_2 - \vec{b}_1}{\lVert \vec{b}_2 - \vec{b}_1 \rVert} $$ and orthonormalize the second vector against the first one, $$\hat{e}_2 = \frac{\vec{a}_3 - \vec{a}_1 - \hat{e}_1 \left ( \hat{e}_1 \cdot (\vec{a}_3 - \vec{a}_1) \right )}{\lVert \vec{a}_3 - \vec{a}_1 - \hat{e}_1 \left ( \hat{e}_1 \cdot (\vec{a}_3 - \vec{a}_1) \right ) \rVert} $$ $$\hat{e}_5 = \frac{\vec{b}_3 - \vec{b}_1 - \hat{e}_4 \left ( \hat{e}_4 \cdot (\vec{b}_3 - \vec{b}_1) \right )}{\lVert \vec{b}_3 - \vec{b}_1 - \hat{e}_4 \left ( \hat{e}_4 \cdot (\vec{b}_3 - \vec{b}_1) \right ) \rVert} $$ As shown in the matrices above, the third basis unit vector is just the cross product of the two others, $$\hat{e}_3 = \hat{e}_1 \times \hat{e}_2$$ $$\hat{e}_6 = \hat{e}_4 \times \hat{e}_5$$

In practice, you'd construct the unit vectors $\hat{e}_i$ first, copying them to the rotation matrices $\mathbf{R}_1$ and $\mathbf{R}_2$, then compute the rotation via matrix multiplication, $\mathbf{R} = \mathbf{R}_2 \mathbf{R}_1^T$, and finally calculate the translation after rotation, $\vec{t} = \vec{b}_1 - \mathbf{R} \vec{a}_1$.

Related Question