From d9392369aa82b73bc93395d883057c0a7a4b498b Mon Sep 17 00:00:00 2001 From: Kenta Sato Date: Tue, 28 Feb 2023 03:48:52 +0100 Subject: [PATCH 1/5] add Slerp for 3D rotations --- src/Rotations.jl | 2 +- src/unitquaternion.jl | 8 ++++++++ test/rotation_tests.jl | 14 ++++++++++++++ 3 files changed, 23 insertions(+), 1 deletion(-) diff --git a/src/Rotations.jl b/src/Rotations.jl index 8ec7ae75..07229abd 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -53,7 +53,7 @@ export RotationVecGenerator, # Quaternion math ops - logm, expm, ⊖, ⊕, + logm, expm, ⊖, ⊕, slerp, # Quaternion maps ExponentialMap, QuatVecMap, CayleyMap, MRPMap, IdentityMap, diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index ec11ca64..80bcb389 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -328,6 +328,14 @@ function rotation_between(from::SVector{3}, to::SVector{3}) @inbounds return QuatRotation(w, v[1], v[2], v[3]) # relies on normalization in constructor end +""" + slerp(R1::Rotaion{3}, R2::Rotaion{3}, t::Real) + +Perform spherical linear interpolation (Slerp) between rotations `R1` and `R2`. +""" +Quaternions.slerp(q1::QuatRotation, q2::QuatRotation, t::Real) = QuatRotation(slerp(q1.q, q2.q, t)) +Quaternions.slerp(r1::Rotation{3}, r2::Rotation{3}, t::Real) = typeof(r1)(slerp(QuatRotation(r1), QuatRotation(r2), t)) + # ~~~~~~~~~~~~~~~ Kinematics ~~~~~~~~~~~~~~~ $ """ kinematics(R::Rotation{3}, ω::AbstractVector) diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index 2bd08407..2e5e2eef 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -246,6 +246,20 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec, end end + @testset "Slerp rotations" begin + repeats = 100 + @testset "slerp($(R1), $(R2), rand())" for R1 in rot_types, R2 in rot_types + Random.seed!(0) + for i in 1:repeats + r1 = rand(R1) + q1 = QuatRotation(r1) + r2 = rand(R2) + q2 = QuatRotation(r2) + t = rand() + @test slerp(r1, r2, t) ≈ slerp(q1, q2, t) + end + end + end ######################################################################### # Test conversions between rotation types From 7aa3736781f026bc2b93732ae16b565eb969624d Mon Sep 17 00:00:00 2001 From: Kenta Sato Date: Thu, 2 Mar 2023 01:18:35 +0100 Subject: [PATCH 2/5] Update src/unitquaternion.jl Co-authored-by: Yuto Horikawa --- src/unitquaternion.jl | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 80bcb389..c4268d4b 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -335,6 +335,9 @@ Perform spherical linear interpolation (Slerp) between rotations `R1` and `R2`. """ Quaternions.slerp(q1::QuatRotation, q2::QuatRotation, t::Real) = QuatRotation(slerp(q1.q, q2.q, t)) Quaternions.slerp(r1::Rotation{3}, r2::Rotation{3}, t::Real) = typeof(r1)(slerp(QuatRotation(r1), QuatRotation(r2), t)) +Quaternions.slerp(r1::RotX, r2::RotX, t::Real) = RotX(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) +Quaternions.slerp(r1::RotY, r2::RotY, t::Real) = RotY(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) +Quaternions.slerp(r1::RotZ, r2::RotZ, t::Real) = RotZ(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) # ~~~~~~~~~~~~~~~ Kinematics ~~~~~~~~~~~~~~~ $ """ From d1bdeed61efd01235790c024cc94d1bf0214f082 Mon Sep 17 00:00:00 2001 From: Kenta Sato Date: Thu, 2 Mar 2023 01:18:46 +0100 Subject: [PATCH 3/5] Update src/unitquaternion.jl Co-authored-by: Yuto Horikawa --- src/unitquaternion.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index c4268d4b..bc18e02a 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -334,7 +334,7 @@ end Perform spherical linear interpolation (Slerp) between rotations `R1` and `R2`. """ Quaternions.slerp(q1::QuatRotation, q2::QuatRotation, t::Real) = QuatRotation(slerp(q1.q, q2.q, t)) -Quaternions.slerp(r1::Rotation{3}, r2::Rotation{3}, t::Real) = typeof(r1)(slerp(QuatRotation(r1), QuatRotation(r2), t)) +Quaternions.slerp(r1::Rotation{3}, r2::Rotation{3}, t::Real) = slerp(QuatRotation(r1), QuatRotation(r2), t) Quaternions.slerp(r1::RotX, r2::RotX, t::Real) = RotX(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) Quaternions.slerp(r1::RotY, r2::RotY, t::Real) = RotY(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) Quaternions.slerp(r1::RotZ, r2::RotZ, t::Real) = RotZ(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) From 3f721c40110d5f62de305309ab195b959a7d6cd8 Mon Sep 17 00:00:00 2001 From: Kenta Sato Date: Thu, 2 Mar 2023 01:26:57 +0100 Subject: [PATCH 4/5] move specialized slerp methods --- src/euler_types.jl | 3 +++ src/unitquaternion.jl | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/euler_types.jl b/src/euler_types.jl index 403f47ac..b48445f4 100644 --- a/src/euler_types.jl +++ b/src/euler_types.jl @@ -30,6 +30,9 @@ for axis in [:X, :Y, :Z] @inline Base.inv(r::$RotType) = $RotType(-r.theta) + # specialized slerp for single axis rotations + Quaternions.slerp(r1::$RotType, r2::$RotType, t::Real) = $RotType(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) + # define identity rotations for convenience @inline Base.one(::Type{$RotType}) = $RotType(0.0) @inline Base.one(::Type{$RotType{T}}) where {T} = $RotType{T}(zero(T)) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index bc18e02a..a14f1ccc 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -335,9 +335,6 @@ Perform spherical linear interpolation (Slerp) between rotations `R1` and `R2`. """ Quaternions.slerp(q1::QuatRotation, q2::QuatRotation, t::Real) = QuatRotation(slerp(q1.q, q2.q, t)) Quaternions.slerp(r1::Rotation{3}, r2::Rotation{3}, t::Real) = slerp(QuatRotation(r1), QuatRotation(r2), t) -Quaternions.slerp(r1::RotX, r2::RotX, t::Real) = RotX(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) -Quaternions.slerp(r1::RotY, r2::RotY, t::Real) = RotY(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) -Quaternions.slerp(r1::RotZ, r2::RotZ, t::Real) = RotZ(r1.theta + (mod2pi(r2.theta - r1.theta + π) - π) * t) # ~~~~~~~~~~~~~~~ Kinematics ~~~~~~~~~~~~~~~ $ """ From 6b49efb8be6e89c9c50920e07df89a07c92850f0 Mon Sep 17 00:00:00 2001 From: Kenta Sato Date: Thu, 2 Mar 2023 01:27:13 +0100 Subject: [PATCH 5/5] test all rotation types --- test/rotation_tests.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index 2e5e2eef..ca317930 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -248,7 +248,7 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec, @testset "Slerp rotations" begin repeats = 100 - @testset "slerp($(R1), $(R2), rand())" for R1 in rot_types, R2 in rot_types + @testset "slerp($(R1), $(R2), rand())" for R1 in all_types, R2 in all_types Random.seed!(0) for i in 1:repeats r1 = rand(R1)