GitHub

Rotations with quaternions

One of the most useful application of quaternions is representation of 3D-rotations. See also Rotations.jl documentation

using Quaternions
using LinearAlgebra

Basics

A 3D rotation can be represented by a unit quaternion (versor). For example, a 90° rotation around the $y$-axis is $q = \frac{1}{\sqrt{2}} + 0i + \frac{1}{\sqrt{2}} j + 0k$. Rotations with quaternions have the following properties:

  • A unit quaternion (4 real numbers) is more efficient for representing a rotation than a rotation matrix (9 real numbers).
    • This results in higher computational performance in terms of time, memory usage, and accuracy.
  • The negative of a unit quaternion represents the same rotation.
  • The conjugate of a unit quaternion represents the inverse rotation.
    • The quaternion has unit length, so conjugate and multiplicative inverse is the same.
  • The set of unit quaternion $\left\{w + ix + jy + kz \in \mathbb{H} \ | \ x, y, z \in \mathbb{R} \right\} = U(1,\mathbb{H}) \simeq S^3$ forms a group, and the group is homomorphic to the following groups.
    • $SU(2) = \{R \in \mathcal{M}(2,\mathbb{C}) \ | \ R R^{*} = I\}$ is isomorphic to $U(1,\mathbb{H})$.
    • $SO(3) = \{R \in \mathcal{M}(3,\mathbb{R}) \ | \ R R^\top = I\}$ is homomorphic to $U(1,\mathbb{H})$, and the mapping $U(1,\mathbb{H}) \to SO(3)$ is double covering.

Rotation around a vector

A $\theta$ rotation around a unit vector $v = (v_x, v_y, v_z)$ can be obtained as

\[q = \cos(\theta/2) + \sin(\theta/2)(iv_x + jv_y + kv_z).\]

function quat_from_axisangle(axis::AbstractVector, theta::Real)
    if length(axis) != 3
        error("Must be a 3-vector")
    end
    s, c = sincos(theta / 2)
    axis = normalize(axis)
    return Quaternion(c, s*axis[1], s*axis[2], s*axis[3])
end
julia> q1 = quat_from_axisangle([0,1,0], deg2rad(90))  # 90° rotation around y-axisQuaternionF64(0.7071067811865476, 0.0, 0.7071067811865475, 0.0)
julia> q2 = quat_from_axisangle([1,1,1], deg2rad(120))QuaternionF64(0.5000000000000001, 0.5, 0.5, 0.5)
julia> q3 = -q2  # additive inverse quaternion represents the same rotationQuaternionF64(-0.5000000000000001, -0.5, -0.5, -0.5)

Rotate a vector with a quaternion

A vector $u = (u_x, u_y, u_z)$ can be rotated by a unit quaternion $q$. The rotated vector $v = (v_x, v_y, v_z)$ can be obtained as

\[\begin{aligned} q_u &= iu_x + ju_y + ku_z \\ q_v &= q q_u \bar{q} = 0 + iv_x + jv_y + kv_z \\ v &= (v_x, v_y, v_z). \end{aligned}\]

function rotate_vector(q::Quaternion, u::AbstractVector)
    if length(u) != 3
        error("Must be a 3-vector")
    end
    q_u = Quaternion(0, u[1], u[2], u[3])
    q_v = q*q_u*conj(q)
    return [imag_part(q_v)...]
end
julia> rotate_vector(q1, [1,2,3])3-element Vector{Float64}:
  3.0
  2.0
 -0.9999999999999993
julia> rotate_vector(q2, [1,2,3])3-element Vector{Float64}:
 3.0
 1.0
 2.0000000000000004
julia> rotate_vector(q3, [1,2,3])  # Same as q23-element Vector{Float64}:
 3.0
 1.0
 2.0000000000000004

Convert a quaternion to a rotation matrix

A unit quaternion can be converted to a rotation matrix.

function rotmatrix_from_quat(q::Quaternion)
    sx, sy, sz = 2q.s * q.v1, 2q.s * q.v2, 2q.s * q.v3
    xx, xy, xz = 2q.v1^2, 2q.v1 * q.v2, 2q.v1 * q.v3
    yy, yz, zz = 2q.v2^2, 2q.v2 * q.v3, 2q.v3^2
    r = [1 - (yy + zz)     xy - sz     xz + sy;
            xy + sz   1 - (xx + zz)    yz - sx;
            xz - sy      yz + sx  1 - (xx + yy)]
    return r
end
julia> m1 = rotmatrix_from_quat(q1)3×3 Matrix{Float64}:
  2.22045e-16  0.0  1.0
  0.0          1.0  0.0
 -1.0          0.0  2.22045e-16
julia> m2 = rotmatrix_from_quat(q2)3×3 Matrix{Float64}:
  0.0          -1.11022e-16   1.0
  1.0           0.0          -1.11022e-16
 -1.11022e-16   1.0           0.0
julia> m3 = rotmatrix_from_quat(q3)  # Same as q23×3 Matrix{Float64}:
  0.0          -1.11022e-16   1.0
  1.0           0.0          -1.11022e-16
 -1.11022e-16   1.0           0.0

This function does not return StaticMatrix, so the implementation is not much effective. If you need more performance, please consider using Rotations.jl.

Convert a rotation matrix to a quaternion

A rotation matrix can be converted to a unit quaternion. The following implementation is based on https://arxiv.org/pdf/math/0701759.pdf. Note that the following mapping $SO(3) \to U(1,\mathbb{H})$ is not surjective.

function quat_from_rotmatrix(dcm::AbstractMatrix{T}) where {T<:Real}
    a2 = 1 + dcm[1,1] + dcm[2,2] + dcm[3,3]
    a = sqrt(a2)/2
    b,c,d = (dcm[3,2]-dcm[2,3])/4a, (dcm[1,3]-dcm[3,1])/4a, (dcm[2,1]-dcm[1,2])/4a
    return Quaternion(a,b,c,d)
end
julia> quat_from_rotmatrix(m1)QuaternionF64(0.7071067811865476, 0.0, 0.7071067811865475, 0.0)
julia> quat_from_rotmatrix(m2)QuaternionF64(0.5, 0.5, 0.5, 0.5)
julia> quat_from_rotmatrix(m3)QuaternionF64(0.5, 0.5, 0.5, 0.5)
julia> quat_from_rotmatrix(m1) ≈ q1true
julia> quat_from_rotmatrix(m2) ≈ q2true
julia> quat_from_rotmatrix(m3) ≈ q3  # q2 == -q3false

Interpolate two rotations (slerp)

Slerp (spherical linear interpolation) is a method to interpolate between two unit quaternions. This function slerp equates antipodal points, and interpolates the shortest path. Therefore, the output slerp(q1, q2, 1) may be different from q2. (slerp(q1, q2, 0) is always equal to q1.)

julia> slerp(q1, q2, 0) ≈ q1true
julia> slerp(q1, q2, 1) ≈ q2true
julia> slerp(q1, q3, 1) ≈ q3false
julia> slerp(q1, q3, 1) ≈ -q3true
julia> r = slerp(q1, q2, 1/2)QuaternionF64(0.6532814824381884, 0.2705980500730985, 0.6532814824381883, 0.2705980500730985)
julia> abs(q1-r) ≈ abs(q2-r)  # Same distancetrue
julia> abs(r)  # Interpolates on the unit sphere S³1.0