|
| 1 | +# Rotations with quaternions |
| 2 | + |
| 3 | +One of the most useful application of quaternions is representation of 3D-rotations. |
| 4 | +See also [Rotations.jl documentation](https://juliageometry.github.io/Rotations.jl/stable/3d_quaternion/) |
| 5 | + |
| 6 | +```@example rotation |
| 7 | +using Quaternions |
| 8 | +using LinearAlgebra |
| 9 | +``` |
| 10 | + |
| 11 | +## Basics |
| 12 | +A 3D rotation can be represented by a [unit quaternion (versor)](https://en.wikipedia.org/wiki/Versor). |
| 13 | +For example, a 90° rotation around the ``y``-axis is ``q = \frac{1}{\sqrt{2}} + 0i + \frac{1}{\sqrt{2}} j + 0k``. |
| 14 | +Rotations with quaternions have the following properties: |
| 15 | + |
| 16 | +* A unit quaternion (4 real numbers) is more efficient for representing a rotation than a rotation matrix (9 real numbers). |
| 17 | + * This results in higher computational performance in terms of time, memory usage, and accuracy. |
| 18 | +* The negative of a unit quaternion represents the same rotation. |
| 19 | +* The conjugate of a unit quaternion represents the inverse rotation. |
| 20 | + * The quaternion has unit length, so conjugate and multiplicative inverse is the same. |
| 21 | +* 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. |
| 22 | + * ``SU(2) = \{R \in \mathcal{M}(2,\mathbb{C}) \ | \ R R^{*} = I\}`` is isomorphic to ``U(1,\mathbb{H})``. |
| 23 | + * ``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. |
| 24 | + |
| 25 | +## Rotation around a vector |
| 26 | +A ``\theta`` rotation around a unit vector ``v = (v_x, v_y, v_z)`` can be obtained as |
| 27 | +```math |
| 28 | +q = \cos(\theta/2) + \sin(\theta/2)(iv_x + jv_y + kv_z). |
| 29 | +``` |
| 30 | + |
| 31 | +```@example rotation |
| 32 | +function quat_from_axisangle(axis::AbstractVector, theta::Real) |
| 33 | + if length(axis) != 3 |
| 34 | + error("Must be a 3-vector") |
| 35 | + end |
| 36 | + s, c = sincos(theta / 2) |
| 37 | + axis = normalize(axis) |
| 38 | + return Quaternion(c, s*axis[1], s*axis[2], s*axis[3]) |
| 39 | +end |
| 40 | +nothing # hide |
| 41 | +``` |
| 42 | + |
| 43 | +```@repl rotation |
| 44 | +q1 = quat_from_axisangle([0,1,0], deg2rad(90)) # 90° rotation around y-axis |
| 45 | +q2 = quat_from_axisangle([1,1,1], deg2rad(120)) |
| 46 | +q3 = -q2 # additive inverse quaternion represents the same rotation |
| 47 | +``` |
| 48 | + |
| 49 | +## Rotate a vector with a quaternion |
| 50 | +A vector ``u = (u_x, u_y, u_z)`` can be rotated by a unit quaternion ``q``. |
| 51 | +The rotated vector ``v = (v_x, v_y, v_z)`` can be obtained as |
| 52 | +```math |
| 53 | +\begin{aligned} |
| 54 | +q_u &= iu_x + ju_y + ku_z \\ |
| 55 | +q_v &= q q_u \bar{q} = 0 + iv_x + jv_y + kv_z \\ |
| 56 | +v &= (v_x, v_y, v_z). |
| 57 | +\end{aligned} |
| 58 | +``` |
| 59 | + |
| 60 | +```@example rotation |
| 61 | +function rotate_vector(q::Quaternion, u::AbstractVector) |
| 62 | + if length(u) != 3 |
| 63 | + error("Must be a 3-vector") |
| 64 | + end |
| 65 | + q_u = Quaternion(0, u[1], u[2], u[3]) |
| 66 | + q_v = q*q_u*conj(q) |
| 67 | + return [imag_part(q_v)...] |
| 68 | +end |
| 69 | +nothing # hide |
| 70 | +``` |
| 71 | + |
| 72 | +```@repl rotation |
| 73 | +rotate_vector(q1, [1,2,3]) |
| 74 | +rotate_vector(q2, [1,2,3]) |
| 75 | +rotate_vector(q3, [1,2,3]) # Same as q2 |
| 76 | +``` |
| 77 | + |
| 78 | +## Convert a quaternion to a rotation matrix |
| 79 | +A unit quaternion can be converted to a rotation matrix. |
| 80 | + |
| 81 | +```@example rotation |
| 82 | +function rotmatrix_from_quat(q::Quaternion) |
| 83 | + sx, sy, sz = 2q.s * q.v1, 2q.s * q.v2, 2q.s * q.v3 |
| 84 | + xx, xy, xz = 2q.v1^2, 2q.v1 * q.v2, 2q.v1 * q.v3 |
| 85 | + yy, yz, zz = 2q.v2^2, 2q.v2 * q.v3, 2q.v3^2 |
| 86 | + r = [1 - (yy + zz) xy - sz xz + sy; |
| 87 | + xy + sz 1 - (xx + zz) yz - sx; |
| 88 | + xz - sy yz + sx 1 - (xx + yy)] |
| 89 | + return r |
| 90 | +end |
| 91 | +nothing # hide |
| 92 | +``` |
| 93 | + |
| 94 | +```@repl rotation |
| 95 | +m1 = rotmatrix_from_quat(q1) |
| 96 | +m2 = rotmatrix_from_quat(q2) |
| 97 | +m3 = rotmatrix_from_quat(q3) # Same as q2 |
| 98 | +``` |
| 99 | + |
| 100 | +This function does not return [`StaticMatrix`](https://juliaarrays.github.io/StaticArrays.jl/dev/pages/api/#StaticArraysCore.StaticArray), so the implementation is not much effective. |
| 101 | +If you need more performance, please consider using [Rotations.jl](https://github.com/JuliaGeometry/Rotations.jl). |
| 102 | + |
| 103 | +## Convert a rotation matrix to a quaternion |
| 104 | +A rotation matrix can be converted to a unit quaternion. |
| 105 | +The following implementation is based on [https://arxiv.org/pdf/math/0701759.pdf](https://arxiv.org/pdf/math/0701759.pdf). |
| 106 | +Note that the following mapping ``SO(3) \to SU(2)`` is not surjective. |
| 107 | + |
| 108 | +```@example rotation |
| 109 | +function quat_from_rotmatrix(dcm::AbstractMatrix{T}) where {T<:Real} |
| 110 | + a2 = 1 + dcm[1,1] + dcm[2,2] + dcm[3,3] |
| 111 | + a = sqrt(a2)/2 |
| 112 | + b,c,d = (dcm[3,2]-dcm[2,3])/4a, (dcm[1,3]-dcm[3,1])/4a, (dcm[2,1]-dcm[1,2])/4a |
| 113 | + return Quaternion(a,b,c,d) |
| 114 | +end |
| 115 | +nothing # hide |
| 116 | +``` |
| 117 | + |
| 118 | +```@repl rotation |
| 119 | +quat_from_rotmatrix(m1) |
| 120 | +quat_from_rotmatrix(m2) |
| 121 | +quat_from_rotmatrix(m3) |
| 122 | +quat_from_rotmatrix(m1) ≈ q1 |
| 123 | +quat_from_rotmatrix(m2) ≈ q2 |
| 124 | +quat_from_rotmatrix(m3) ≈ q3 # q2 == -q3 |
| 125 | +``` |
| 126 | + |
| 127 | +## Interpolate two rotations (slerp) |
| 128 | +Slerp (spherical linear interpolation) is a method to interpolate between two unit quaternions. |
| 129 | +This function [`slerp`](@ref) equates antipodal points, and interpolates the shortest path. |
| 130 | +Therefore, the output `slerp(q1, q2, 1)` may be different from `q2`. (`slerp(q1, q2, 0)` is always equal to `q1`.) |
| 131 | + |
| 132 | +```@repl rotation |
| 133 | +slerp(q1, q2, 0) ≈ q1 |
| 134 | +slerp(q1, q2, 1) ≈ q2 |
| 135 | +slerp(q1, q3, 1) ≈ q3 |
| 136 | +slerp(q1, q3, 1) ≈ -q3 |
| 137 | +r = slerp(q1, q2, 1/2) |
| 138 | +abs(q1-r) ≈ abs(q2-r) # Same distance |
| 139 | +abs(r) # Interpolates on the unit sphere S³ |
| 140 | +``` |
0 commit comments