add Slerp for 3D rotations (#252)

* add Slerp for 3D rotations

* Update src/unitquaternion.jl

Co-authored-by: Yuto Horikawa <[email protected]>

* Update src/unitquaternion.jl

Co-authored-by: Yuto Horikawa <[email protected]>

* move specialized slerp methods

* test all rotation types

---------

Co-authored-by: Yuto Horikawa <[email protected]>

Author: bicycle1885

src/Rotations.jl

@@ -53,7 +53,7 @@ export
     RotationVecGenerator,
 
     # Quaternion math ops
-    logm, expm, ⊖, ⊕,
+    logm, expm, ⊖, ⊕, slerp,
 
     # Quaternion maps
     ExponentialMap, QuatVecMap, CayleyMap, MRPMap, IdentityMap,

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))

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) = slerp(QuatRotation(r1), QuatRotation(r2), t)
+
 # ~~~~~~~~~~~~~~~ Kinematics ~~~~~~~~~~~~~~~ $
 """
     kinematics(R::Rotation{3}, ω::AbstractVector)

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 all_types, R2 in all_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