Hamilton's quaternions and 3D rotation with Python

Post Views: 39,641

With the Euler angles the foundations for the calculation of the rotation of bodies in three-dimensional spaces were founded. However, it was later discovered that Hamilton’s quaternions are a more efficient tool for studying the rotation mode of bodies. In this article we will see what quaternions are, how they are calculated and how they apply to the rotation of a body, also helping us in this case with some Python code.

Hamilton’s Quaternions

Quaternions were devised by the mathematician Hamilton (1805-1865) to extend the functionality of complex numbers into a four-dimensional system.

Complex numbers are generally denoted by notation

$z = a + bi$

where a is the real part, b is the imaginary component while i is the imaginary unit defined as the square root of -1

$i = \sqrt{-1}$

Quaternions extend the concept of complex numbers, and then two more -1 symbols are added:

$q=w+xi+yj+zk$

where w is the real part, and i, j, k denote the three imaginary units, and x, y, z denote the three imaginary components. To make things easier you can simply write:

$q=w+v$

where w is the scalar component and v = (x, y, z) is the vector of the imaginary components.

The algebra of quaternions

The following basic rules are based on the algebra of quaternions:

$i^2=j^2=k^2=ijk= -1$

$ij = k = -ji$

$jk = i = -kj$

$ki = j = -ik$

Furthermore, given two quaternions q₁ and q₂:

$q_1=w_1+x_1i+y_1j+z_1k$

$q_2=w_2+x_2i+y_2j+z_2k$

the sum of the two quaternions is defined as follows:

$q_1+q_2=(w_1+w_2)+(x_1+x_2)i+(y_1+y_2)j+(z_1+z_2)k$

while the product of the two quaternions takes place in this way:

$q_1q_2=(w_1w_2–x_1x_2−y_1y_2–z_1z_2)+(w_1x_2+x_1w_2+y_1z_2–z_1y_2)i+(w_1y_2–x_1z_2+y_1w_2+z_1x_2)j+(w_1z_2+x_1y_2–y_1x_2+z_1w_2)k$

In the notation with the vector v we have a more synthetic expression

$q_1q_2=w_1w_2 - v_1\cdot v_2 + w_1v_2+w_2v_1 + v_1 \times v_2$

where v1 ∙ v2 is the scalar product and v1 × v2 is the vector product.

Other important definitions for quaternion algebra are the conjugate, the norm and the inverse of a quaternion.

The conjugate of a quaternion is denoted by q * and is defined as:

$q^*=w-xi-yj-zk$

If we multiply a quaternion with its conjugate we have:

$q q^* = w^2 + x^2 + y^2 + z^2$

The length or norm of a quaternion is instead defined as:

$N(q) = \mid q \mid = \sqrt{ q q^* } = \sqrt{w^2 + x^2 + y^2 + z^2}$

Finally for every quaternion, except q = 0, there is an inverse defined as:

$q^{-1} = \frac{q^*}{\mid q \mid ^2}$

The last algebraic operation is the division between two quaternions, which can be obtained by multiplying the first quaternion by the inverse of the second

$q_1 q_2^{-1}$

A final consideration in this regard is that while complex numbers constitute an algebraic field, quaternions instead constitute an algebraic body. This is because for quaternions the commutative property of the product does not hold and therefore q₁ x q₂ is different from q₂ x q₁.

Quaternions as a tool for calculating the rotations of a body around an arbitrary axis

According to Euler’s theorem, any rotation or sequence of rotations of a rigid body around a fixed point is equivalent to a single rotation of an angle θ around a certain axis (called the Euler axis) passing through this fixed point.

Angoli di Eulero - due sistemi di riferimento

The Euler axis is normally represented by a vector unit (unit vector) $\hat{u}$ . Therefore any rotation in 3 dimensions can be represented as a combination of a unit vector $\hat{u}$ and a scalar value θ.

A rotation of an angle θ around an axis defined by the unit vector

$\hat{u} = (u_x, u_y, u_z) = u_x \hat{i} + u_y \hat{j} + u_z \hat{k}$

Similarly to how we did with Euler angles and the rotation matrix R, which can be applied to an ordinary vector p:

$\vec{p} = (p_x, p_y, p_z)$

We can therefore indicate with R(θ,p) the rotation of a vector p by an angle θ around the axis indicated by the unit vector u, as we have already seen in the previous article on Euler angles.

$p_1 = R p$

The same rotation can be represented with a Hamilton quaternion:

$q = q_w + q_x \hat{i} + q_y \hat{j} + q_z \hat{k}$

also representable in form

$q = \begin{bmatrix} q_w & q_x & q_y & q_z \end{bmatrix}^T$

with

$\mid q \mid ^2 = q_w^2 + q_x^2 + q_y^2 + q_z^2$

associating the various terms to the angles of rotation

$q_w = cos(\theta /2)$

$q_x = sin(\theta /2) cos(\beta_x)$

$q_y = sin(\theta /2) cos(\beta_y)$

$q_z = sin(\theta /2) cos(\beta_z)$

where theta is the rotation angle and $cos(\beta_x), cos(\beta_y), cos(\beta_z)$ are the director cosines of the rotation axis indicated by the unit vector u..

Furthermore, it is possible to obtain a result of the Rotation matrix used with Euler angles, using the quaternion q, calculating the conjugation of the vector p₀ with q. We thus obtain the new vector p₁ which is the result of the rotation.

$p_1 = q p_0 q^*$

with

$q^* = e^{-\frac{1}{2} \theta ( u_x \hat{i} + u_y \hat{j} + u_z \hat{k} ) }$

$= cos \frac{\theta}{2} - ( u_x \hat{i} + u_y \hat{j} + u_z \hat{k} ) sin\frac{\theta}{2}$

Applying several theorems and defining a pure quaternion:

$P = (0, v)$

and a unitary quaternion

$\vert \vert q \vert \vert = 1$

$q = (cos\theta, u sin\theta)$

you find that

$R(\theta,v) = q P q^*$

Further expanding the second term a matrix is obtained

$R = \begin{pmatrix} 1 - 2y^2 -2z^2 & 2xy - 2wz & 2xz +2wy \\ 2xy + 2wz & 1 - 2x^2 - 2z^2 & 2yz - 2wx \\ 2xz - 2wy & 2yz + 2wx & 1 - 2x^2 - 2y^2 \end{pmatrix}$

Given the rotation matrix described above, it is possible to go back to the corresponding quaternion with a few steps. The trace of the matrix R (sum of the diagonal elements) is calculated:

$Tr(R) = 3 - 4x^2 - 4y^2 - 4z^2 = 4w^2 - 1$

this is because the quaternion is unitary

$x^2 + y^2 + z^2 +w^2 = 1$

therefore

$w = \sqrt{\frac{Tr(R)+1}{4}}$

The other components x, y, z are calculated in a similar way.

Rotation with quaternions in Python

Now that we have seen that it is possible to perform rotation calculations with quaternions and which are the mathematical expressions to use, let’s start implementing everything in Python.

First we need to calculate the quaternion conjugate.

def q_conjugate(q):
    w, x, y, z = q
    return (w, -x, -y, -z)

We now have all the elements to carry out the multiplication

$P_1 = q P_0 q^*$

In fact P is none other than the pure quaternion obtained, using the vector v to rotate for the three imaginary terms and the real part w equal to zero.

$P = (0, v)$

So in Python we can implement like this:

def qv_mult(q1, v1):
    q2 = (0.0,) + v1
    return q_mult(q_mult(q1, q2), q_conjugate(q1))[1:]

As we can see in the function, it is necessary to use the multiplication between quaternions, and therefore we must implement it in Python using the equations previously performed mathematically.

def q_mult(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return w, x, y, z

Moreover, if we have a sequence of multiple rotations, as it was possible with Euler to multiply among them the rotation matrices R, in the same way the quaternions corresponding to the single rotations are multiplied among them. The resulting quaternion will be equivalent to the total rotation.

Conversion from Euler angles to quaternions and vice versa

Another very useful and common operation is that of passing from Euler’s formulation to that of quaternions. It is therefore possible, knowing the Euler angles ( $\phi, \theta, \psi$ ), find the corresponding quaternion q.

For example, for the Euler angles in an XYZ convention, we obtain the quaternion q corresponding to this rotation with the following expression:

$q = \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix} = \begin{bmatrix} cos(\phi /2)cos(\theta /2)cos(\psi /2) + sin(\phi /2)sin(\theta /2)sin(\psi /2) \\ sin(\phi /2)cos(\theta /2)cos(\psi /2) - cos(\phi /2)sin(\theta /2)sin(\psi /2) \\ cos(\phi /2)sin(\theta /2)cos(\psi /2) + sin(\phi /2)cos(\theta /2)sin(\psi /2) \\ cos(\phi /2)cos(\theta /2)sin(\psi /2) + sin(\phi /2)sin(\theta /2)cos(\psi /2) \end{bmatrix}$

Which in Python can be expressed in the following function:

def euler_to_quaternion(phi, theta, psi):

        qw = m.cos(phi/2) * m.cos(theta/2) * m.cos(psi/2) + m.sin(phi/2) * m.sin(theta/2) * m.sin(psi/2)
        qx = m.sin(phi/2) * m.cos(theta/2) * m.cos(psi/2) - m.cos(phi/2) * m.sin(theta/2) * m.sin(psi/2)
        qy = m.cos(phi/2) * m.sin(theta/2) * m.cos(psi/2) + m.sin(phi/2) * m.cos(theta/2) * m.sin(psi/2)
        qz = m.cos(phi/2) * m.cos(theta/2) * m.sin(psi/2) - m.sin(phi/2) * m.sin(theta/2) * m.cos(psi/2)

        return [qw, qx, qy, qz]

Same thing we can do in reverse. Knowing the terms of the quaternion, obtain the Euler angles in an XYZ convention.

$\begin{bmatrix} \phi \\ \theta \\ \psi \end{bmatrix} = \begin{bmatrix} arctan \frac{2(q_0 q_1 + q_2 q_3)}{1-2(q_1^2 q_2^2)} \\ arcsin (2(q_0 q_2 - q_3 q_1)) \\ arctan \frac{2(q_0 q_3 + q_1 q_2)}{1-2(q_2^2 q_3^2)} \end{bmatrix}$

Which can be implemented in Python in the following function:

def quaternion_to_euler(w, x, y, z):

        t0 = 2 * (w * x + y * z)
        t1 = 1 - 2 * (x * x + y * y)
        X = m.atan2(t0, t1)

        t2 = 2 * (w * y - z * x)
        t2 = 1 if t2 > 1 else t2
        t2 = -1 if t2 < -1 else t2
        Y = m.asin(t2)
        
        t3 = 2 * (w * z + x * y)
        t4 = 1 - 2 * (y * y + z * z)
        Z = m.atan2(t3, t4)

        return X, Y, Z

Example – the rotation of a point in space

We will now take the same example we did with Euler’s transformations, but this time using quaternions instead of the rotation matrix R.

The simplest example of application of what we have already seen in the article is the rotation of a point located in a coordinate space (X, Y, Z). A point in space can be represented by a 3-element vector that characterizes its values on the three coordinate axes.

$v = \begin{bmatrix} x \\ y \\ z \end{bmatrix}$

So for our example, we will start from a simple point on the X axis described by the following vector. Which is the same as the example with Euler’s angles.

$v = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}$

Let’s express the vector as a tuple this time:

v1 = (1,0,0)

Now to obtain the quaternion that is equivalent to the same rotation of the example with the Euler angles, we will use the latter and convert them into the corresponding quaternion using the function we have just defined. So let’s first define the three Euler angles and then convert them into the quaternion whose terms we are going to visualize.

phi = m.pi/2
theta = m.pi/4
psi = m.pi/2
q = euler_to_quaternion(phi, theta, psi)
print("w =", q[0])
print("x =", q[1])
print("y =", q[2])
print("z =", q[3])

By executing the following quaternion is obtained

w = 0.6532814824381883
x = 0.27059805007309845
y = 0.6532814824381882
z = 0.2705980500730985

Now let’s do the multiplication between vector and quaternion

v2 = qv_mult(q,v1)
print(np.round(v2, decimals=2))

And we will get the same vector we obtained in the example of Euler’s angles.

[ 0. 0.71 -0.71]

Let’s see the result of our rotation by plotting the position of the vector (which describes the point) before and after the rotation on a graph with Cartesian axes.

import matplotlib.pyplot as plt
 
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Cartesian axes
ax.quiver(-1, 0, 0, 3, 0, 0, color='#aaaaaa',linestyle='dashed')
ax.quiver(0, -1, 0, 0,3, 0, color='#aaaaaa',linestyle='dashed')
ax.quiver(0, 0, -1, 0, 0, 3, color='#aaaaaa',linestyle='dashed')
# Vector before rotation
ax.quiver(0, 0, 0, 1, 0, 0, color='b')
# Vector after rotation
ax.quiver(0, 0, 0, 0, 0.71, -0.71, color='r')
ax.set_xlim([-1.5, 1.5])
ax.set_ylim([-1.5, 1.5])
ax.set_zlim([-1.5, 1.5])
plt.show()

By running the code you will get the graphical representation.

Angoli di Eulero - verso delle rotazioni elementari — Vector display before (blue) and after rotation (red)

From here you can then move on to the rotation of lines, geometric figures and three-dimensional objects. All at the basis of the 3D engines with which many video games are developed.

Advantages of using quaternions with respect to Euler angles

The use of quaternions instead of Euler angles when it comes to calculating the rotation in a three-dimensional space leads to some advantages. Let’s see in detail what they are.

For the calculation of a rotation with Euler angles, 9 parameters are required, while for quaternions only 4 parameters are needed.

The rotation with Euler angles consists of a sequence of elementary rotations around one of the three Cartesian axes with many possible combinations. This requires different equations and resolutions for each case. While the rotation through quaternions is unique.

Quaternion rotation is not subject to gimbal lock.

2 thoughts on “Hamilton’s quaternions and 3D rotation with Python”

Anastasiia 24 June 2021 Log in to Reply

Hi! Seems there is a minor typo in the formula for the rotation matrix made of quaternion elements R. For element R[1][2] it should be 2yz-2wx instead of 2yz-2wz. Anyway thanks a lot! It is a very useful article!
- webmaster 24 June 2021 Log in to Reply
  
  Thanks. Yes, I correct the formula.