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-axis
QuaternionF64(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 rotation
QuaternionF64(-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 q2
3-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 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
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) ≈ q1
true
julia> quat_from_rotmatrix(m2) ≈ q2
true
julia> quat_from_rotmatrix(m3) ≈ q3 # q2 == -q3
false
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) ≈ q1
true
julia> slerp(q1, q2, 1) ≈ q2
true
julia> slerp(q1, q3, 1) ≈ q3
false
julia> slerp(q1, q3, 1) ≈ -q3
true
julia> r = slerp(q1, q2, 1/2)
QuaternionF64(0.6532814824381884, 0.2705980500730985, 0.6532814824381883, 0.2705980500730985)
julia> abs(q1-r) ≈ abs(q2-r) # Same distance
true
julia> abs(r) # Interpolates on the unit sphere S³
1.0