From bdf34b97e22ee6846eb347aa4881c07cebc4174f Mon Sep 17 00:00:00 2001 From: Yuto Horikawa Date: Wed, 7 Dec 2022 09:22:19 +0900 Subject: [PATCH] Remove rotation-related functions (#111) * remove rotation-realted functions * remove exports for rotations * fix tests for rotations * add an example page for rotation with quaternion * replace rotationmatrix with rotmatrix_from_quat * Apply suggestion (fix english and LaTeX) Co-authored-by: Seth Axen * Apply suggestion (fix english) Co-authored-by: Seth Axen * apply english suggestion from code review * update the documentation for basic rotation with unit quaternion * add U(1,\mathbb{H}) * update the symbols for rotating a vector * update around SO(3) and SU(2) * fix typo * fix LaTeX command * fix typo * add more argument type specification in rotations.md Co-authored-by: Seth Axen --- docs/make.jl | 5 +- docs/src/examples/dual_quaternions.md | 12 +- docs/src/examples/rotations.md | 140 +++++++++++++++++++++++ src/Quaternion.jl | 89 --------------- src/Quaternions.jl | 5 - test/Quaternion.jl | 158 +++++++------------------- 6 files changed, 195 insertions(+), 214 deletions(-) create mode 100644 docs/src/examples/rotations.md diff --git a/docs/make.jl b/docs/make.jl index b32dfa71..31e20f24 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -15,7 +15,10 @@ makedocs(; pages=[ "Home" => "index.md", "APIs" => "api.md", - "Examples" => ["examples/dual_quaternions.md"], + "Examples" => [ + "examples/rotations.md", + "examples/dual_quaternions.md" + ], ], ) diff --git a/docs/src/examples/dual_quaternions.md b/docs/src/examples/dual_quaternions.md index 1759d7dc..48e7ed58 100644 --- a/docs/src/examples/dual_quaternions.md +++ b/docs/src/examples/dual_quaternions.md @@ -88,8 +88,18 @@ function transform(d::DualQuaternion, p::AbstractVector) return pnew end +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 + function transformationmatrix(d::DualQuaternion) - R = rotationmatrix(rotation_part(d)) + R = rotmatrix_from_quat(rotation_part(d)) t = translation(d; first=false) T = similar(R, 4, 4) T[1:3, 1:3] .= R diff --git a/docs/src/examples/rotations.md b/docs/src/examples/rotations.md new file mode 100644 index 00000000..3c377c5a --- /dev/null +++ b/docs/src/examples/rotations.md @@ -0,0 +1,140 @@ +# Rotations with quaternions + +One of the most useful application of quaternions is representation of 3D-rotations. +See also [Rotations.jl documentation](https://juliageometry.github.io/Rotations.jl/stable/3d_quaternion/) + +```@example rotation +using Quaternions +using LinearAlgebra +``` + +## Basics +A 3D rotation can be represented by a [unit quaternion (versor)](https://en.wikipedia.org/wiki/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 +```math +q = \cos(\theta/2) + \sin(\theta/2)(iv_x + jv_y + kv_z). +``` + +```@example rotation +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 +nothing # hide +``` + +```@repl rotation +q1 = quat_from_axisangle([0,1,0], deg2rad(90)) # 90° rotation around y-axis +q2 = quat_from_axisangle([1,1,1], deg2rad(120)) +q3 = -q2 # additive inverse quaternion represents the same rotation +``` + +## 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 +```math +\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} +``` + +```@example rotation +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 +nothing # hide +``` + +```@repl rotation +rotate_vector(q1, [1,2,3]) +rotate_vector(q2, [1,2,3]) +rotate_vector(q3, [1,2,3]) # Same as q2 +``` + +## Convert a quaternion to a rotation matrix +A unit quaternion can be converted to a rotation matrix. + +```@example rotation +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 +nothing # hide +``` + +```@repl rotation +m1 = rotmatrix_from_quat(q1) +m2 = rotmatrix_from_quat(q2) +m3 = rotmatrix_from_quat(q3) # Same as q2 +``` + +This function does not return [`StaticMatrix`](https://juliaarrays.github.io/StaticArrays.jl/dev/pages/api/#StaticArraysCore.StaticArray), so the implementation is not much effective. +If you need more performance, please consider using [Rotations.jl](https://github.com/JuliaGeometry/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](https://arxiv.org/pdf/math/0701759.pdf). +Note that the following mapping ``SO(3) \to SU(2)`` is not surjective. + +```@example rotation +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 +nothing # hide +``` + +```@repl rotation +quat_from_rotmatrix(m1) +quat_from_rotmatrix(m2) +quat_from_rotmatrix(m3) +quat_from_rotmatrix(m1) ≈ q1 +quat_from_rotmatrix(m2) ≈ q2 +quat_from_rotmatrix(m3) ≈ q3 # q2 == -q3 +``` + +## Interpolate two rotations (slerp) +Slerp (spherical linear interpolation) is a method to interpolate between two unit quaternions. +This function [`slerp`](@ref) 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`.) + +```@repl rotation +slerp(q1, q2, 0) ≈ q1 +slerp(q1, q2, 1) ≈ q2 +slerp(q1, q3, 1) ≈ q3 +slerp(q1, q3, 1) ≈ -q3 +r = slerp(q1, q2, 1/2) +abs(q1-r) ≈ abs(q2-r) # Same distance +abs(r) # Interpolates on the unit sphere S³ +``` diff --git a/src/Quaternion.jl b/src/Quaternion.jl index a0c77993..9ec132e8 100644 --- a/src/Quaternion.jl +++ b/src/Quaternion.jl @@ -187,20 +187,6 @@ Base.:(==)(q::Quaternion, w::Quaternion) = (q.s == w.s) & (q.v1 == w.v1) & (q.v2 angleaxis(q::Quaternion) = angle(q), axis(q) -function Base.angle(q::Quaternion) - Base.depwarn("`Base.angle(::Quaternion)` is deprecated. Please consider using Rotations package instead.", :angle) - 2 * atan(abs_imag(q), real(q)) -end - -function axis(q::Quaternion) - Base.depwarn("`axis(::Quaternion)` is deprecated. Please consider using Rotations package instead.", :axis) - q = sign(q) - s = sin(angle(q) / 2) - abs(s) > 0 ? - [q.v1, q.v2, q.v3] / s : - [1.0, 0.0, 0.0] -end - """ extend_analytic(f, q::Quaternion) @@ -299,81 +285,6 @@ function Base.randn(rng::AbstractRNG, ::Type{Quaternion{T}}) where {T<:AbstractF ) end -## Rotations - -function qrotation(axis::AbstractVector{T}, theta) where {T <: Real} - Base.depwarn("`qrotation(::AbstractVector)` is deprecated. Please consider using Rotations package instead.", :qrotation) - if length(axis) != 3 - error("Must be a 3-vector") - end - normaxis = norm(axis) - if iszero(normaxis) - normaxis = oneunit(normaxis) - theta = zero(theta) - end - s,c = sincos(theta / 2) - scaleby = s / normaxis - Quaternion(c, scaleby * axis[1], scaleby * axis[2], scaleby * axis[3]) -end - -# Variant of the above where norm(rotvec) encodes theta -function qrotation(rotvec::AbstractVector{T}) where {T <: Real} - Base.depwarn("`qrotation(::AbstractVector)` is deprecated. Please consider using Rotations package instead.", :qrotation) - if length(rotvec) != 3 - error("Must be a 3-vector") - end - theta = norm(rotvec) - s,c = sincos(theta / 2) - scaleby = s / (iszero(theta) ? one(theta) : theta) - Quaternion(c, scaleby * rotvec[1], scaleby * rotvec[2], scaleby * rotvec[3]) -end - -function qrotation(dcm::AbstractMatrix{T}) where {T<:Real} - Base.depwarn("`qrotation(::AbstractMatrix)` is deprecated. Please consider using Rotations package instead.", :qrotation) - # See https://arxiv.org/pdf/math/0701759.pdf - a2 = 1 + dcm[1,1] + dcm[2,2] + dcm[3,3] - b2 = 1 + dcm[1,1] - dcm[2,2] - dcm[3,3] - c2 = 1 - dcm[1,1] + dcm[2,2] - dcm[3,3] - d2 = 1 - dcm[1,1] - dcm[2,2] + dcm[3,3] - - if a2 ≥ max(b2, c2, d2) - 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 - elseif b2 ≥ max(c2, d2) - b = sqrt(b2)/2 - a,c,d = (dcm[3,2]-dcm[2,3])/4b, (dcm[2,1]+dcm[1,2])/4b, (dcm[1,3]+dcm[3,1])/4b - elseif c2 ≥ d2 - c = sqrt(c2)/2 - a,b,d = (dcm[1,3]-dcm[3,1])/4c, (dcm[2,1]+dcm[1,2])/4c, (dcm[3,2]+dcm[2,3])/4c - else - d = sqrt(d2)/2 - a,b,c = (dcm[2,1]-dcm[1,2])/4d, (dcm[1,3]+dcm[3,1])/4d, (dcm[3,2]+dcm[2,3])/4d - end - if a > 0 - return Quaternion(a,b,c,d) - else - return Quaternion(-a,-b,-c,-d) - end -end - -function qrotation(dcm::AbstractMatrix{T}, qa::Quaternion) where {T<:Real} - Base.depwarn("`qrotation(::AbstractMatrix, ::Quaternion)` is deprecated. Please consider using Rotations package instead.", :qrotation) - q = qrotation(dcm) - abs(q-qa) < abs(q+qa) ? q : -q -end - -rotationmatrix(q::Quaternion) = rotationmatrix_normalized(sign(q)) - -function rotationmatrix_normalized(q::Quaternion) - Base.depwarn("`rotationmatrix_normalized(::Quaternion)` is deprecated. Please consider using Rotations package instead.", :rotationmatrix_normalized) - 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 - [1 - (yy + zz) xy - sz xz + sy; - xy + sz 1 - (xx + zz) yz - sx; - xz - sy yz + sx 1 - (xx + yy)] -end - """ slerp(qa::Quaternion, qb::Quaternion, t::Real) diff --git a/src/Quaternions.jl b/src/Quaternions.jl index ae0a4cb3..8de5bec2 100644 --- a/src/Quaternions.jl +++ b/src/Quaternions.jl @@ -13,12 +13,7 @@ module Quaternions QuaternionF64 export quat export imag_part - export angleaxis - export angle - export axis export quatrand export nquatrand - export qrotation - export rotationmatrix export slerp end diff --git a/test/Quaternion.jl b/test/Quaternion.jl index d516bc49..51134e21 100644 --- a/test/Quaternion.jl +++ b/test/Quaternion.jl @@ -441,101 +441,23 @@ end @inferred(sign(Quaternion(1, 2, 3, 4))) end - @testset "rotations" begin - @testset "qrotation" begin - @test qrotation([0, 0, 0], 1.0) == Quaternion(1.0) # a zero axis should act like zero rotation - @test qrotation([1, 0, 0], 0.0) == Quaternion(1.0) - @test qrotation([0, 0, 0]) == Quaternion(1.0) - qx = qrotation(view([1, 0, 0], :), pi / 4) - @test qx * qx ≈ qrotation([1, 0, 0], pi / 2) - @test qx^2 ≈ qrotation([1, 0, 0], pi / 2) - - # Regression test for - # https://github.com/JuliaGeometry/Quaternions.jl/issues/8#issuecomment-610640094 - # this used to throw an error - @testset "qrotation can handle arbitrary reals" begin - @test qrotation([1, 0, 0], MyReal(1.5)) == qrotation([1, 0, 0], 1.5) - end - end - - @testset "rotationmatrix" begin - theta = pi / 8 - qx = qrotation([1, 0, 0], theta) - c = cos(theta) - s = sin(theta) - Rx = [1 0 0; 0 c -s; 0 s c] - @test rotationmatrix(qx) ≈ Rx - theta = pi / 6 - qy = qrotation([0, 1, 0], theta) - c = cos(theta) - s = sin(theta) - Ry = [c 0 s; 0 1 0; -s 0 c] - @test rotationmatrix(qy) ≈ Ry - theta = 4pi / 3 - qz = qrotation([0, 0, 1], theta) - c = cos(theta) - s = sin(theta) - Rz = [c -s 0; s c 0; 0 0 1] - @test rotationmatrix(qz) ≈ Rz - - @test rotationmatrix(qx * qy * qz) ≈ Rx * Ry * Rz - @test rotationmatrix(qy * qx * qz) ≈ Ry * Rx * Rz - @test rotationmatrix(qz * qx * qy) ≈ Rz * Rx * Ry - - for _ in 1:100 - q1 = nquatrand() - q2 = qrotation(rotationmatrix(q1), q1) - q3 = qrotation(rotationmatrix(q1)) - @test q1 ≈ q2 - @test q2 === q3 || q2 === -q3 - @test real(q3) ≥ 0 - @test abs(q2) ≈ 1 - @test abs(q3) ≈ 1 - end + @testset "slerp" begin + function qrotation(axis, theta) + s, c = sincos(theta / 2) + axis = normalize(axis) + return Quaternion(c, s*axis[1], s*axis[2], s*axis[3]) end - - @testset "angle/axis/angleaxis" begin - @test_throws ErrorException qrotation([0, 1], 0.1) - @test_throws ErrorException qrotation([0, 1, 0, 0], 0.1) - @test_throws ErrorException qrotation([0, 1]) - @test_throws ErrorException qrotation([0, 1, 0, 0]) - @test angle(qrotation([1, 0, 0], 0)) ≈ 0 - @test angle(qrotation([0, 1, 0], pi / 4)) ≈ pi / 4 - @test angle(qrotation([0, 0, 1], pi / 2)) ≈ pi / 2 - - @testset "numerical stability of angle" begin - ax = randn(3) - for θ in [1e-9, π - 1e-9] - q = qrotation(ax, θ) - @test angle(q) ≈ θ - end - end - - @testset "qrotation and angleaxis inverse" begin + @testset "q1=1" begin + a = quat(1, 0, 0, 0.0) + b = quat(0, 0, 0, 1.0) + @test slerp(a, b, 0.0) ≈ a + @test slerp(a, b, 1.0) ≈ b + @test slerp(a, b, 0.5) ≈ qrotation([0, 0, 1], deg2rad(90)) + @test abs(slerp(a, b, 0.0)) ≈ 1 + @test abs(slerp(a, b, 1.0)) ≈ 1 + @test abs(slerp(a, b, 0.5)) ≈ 1 + @testset "scale $scale" for scale in (1, 1e-5, 1e-10) for _ in 1:100 - ax = randn(3) - ax = ax / norm(ax) - Θ = π * rand() - q = qrotation(ax, Θ) - @test angle(q) ≈ Θ - @test axis(q) ≈ ax - @test angleaxis(q)[1] ≈ Θ - @test angleaxis(q)[2] ≈ ax - end - end - end - - @testset "slerp" begin - @testset "q1=1" begin - a = quat(1, 0, 0, 0.0) - b = quat(0, 0, 0, 1.0) - @test slerp(a, b, 0.0) ≈ a - @test slerp(a, b, 1.0) ≈ b - @test slerp(a, b, 0.5) ≈ qrotation([0, 0, 1], deg2rad(90)) - @test abs(slerp(a, b, 0.0)) ≈ 1 - @test abs(slerp(a, b, 1.0)) ≈ 1 - @test abs(slerp(a, b, 0.5)) ≈ 1 - for _ in 1:100, scale in (1, 1e-5, 1e-10) q1 = quat(1, 0, 0, 0.0) θ = rand() * π * scale ax = randn(3) @@ -552,36 +474,36 @@ end @test slerp(q1, qsmall, 0.5) ≈ sign((q1 + qsmall) / 2) end end + end - @testset "conjugate invariance" begin - for _ in 1:100 - q, q1, q2 = randn(QuaternionF64, 3) - ⊗(s, t) = s * t * inv(s) - t = rand() - @test q ⊗ slerp(q1, q2, t) ≈ slerp(q ⊗ q1, q ⊗ q2, t) - end + @testset "conjugate invariance" begin + for _ in 1:100 + q, q1, q2 = randn(QuaternionF64, 3) + ⊗(s, t) = s * t * inv(s) + t = rand() + @test q ⊗ slerp(q1, q2, t) ≈ slerp(q ⊗ q1, q ⊗ q2, t) end + end - @testset "type promotion" begin - @test slerp(quat(1),quat(1),1) isa Quaternion{Float64} - @test slerp(quat(1),quat(1),big(1)) isa Quaternion{BigFloat} - @test slerp(quat(1),quat(1),Float32(1)) isa Quaternion{Float32} - @test slerp(quat(1),quat(Float32(1)),Float32(1)) isa Quaternion{Float32} - @test slerp(quat(Float64(1)),quat(Float32(1)),Float32(1)) isa Quaternion{Float64} - end + @testset "type promotion" begin + @test slerp(quat(1),quat(1),1) isa Quaternion{Float64} + @test slerp(quat(1),quat(1),big(1)) isa Quaternion{BigFloat} + @test slerp(quat(1),quat(1),Float32(1)) isa Quaternion{Float32} + @test slerp(quat(1),quat(Float32(1)),Float32(1)) isa Quaternion{Float32} + @test slerp(quat(Float64(1)),quat(Float32(1)),Float32(1)) isa Quaternion{Float64} + end - @testset "DomainError" begin - @test_throws DomainError slerp(quat(1),quat(0),1) - @test_throws DomainError slerp(quat(0),quat(1),0) - end + @testset "DomainError" begin + @test_throws DomainError slerp(quat(1),quat(0),1) + @test_throws DomainError slerp(quat(0),quat(1),0) + end - @testset "Normalizing input quaternions" begin - for _ in 1:100 - q1 = randn(QuaternionF64) - q2 = randn(QuaternionF64) - t = rand() - @test slerp(sign(q1),sign(q2),t) ≈ slerp(q1,q2,t) - end + @testset "Normalizing input quaternions" begin + for _ in 1:100 + q1 = randn(QuaternionF64) + q2 = randn(QuaternionF64) + t = rand() + @test slerp(sign(q1),sign(q2),t) ≈ slerp(q1,q2,t) end end end