Fix slerp and deprecate linpol

Project.toml

@@ -1,6 +1,6 @@
 name = "Quaternions"
 uuid = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0"
-version = "0.5.5"
+version = "0.5.6"
 
 [deps]
 DualNumbers = "fa6b7ba4-c1ee-5f82-b5fc-ecf0adba8f74"

README.md

@@ -60,7 +60,7 @@ Implemented functions are:
     cospi
     sincos
     sincospi
-    linpol  (interpolate between 2 normalized quaternions)
+    slerp
     rand
     randn
 

src/Quaternion.jl

@@ -230,29 +230,6 @@ end
 
 (^)(q::Quaternion, w::Quaternion) = exp(w * log(q))
 
-function linpol(p::Quaternion, q::Quaternion, t::Real)
-    p = normalize(p)
-    q = normalize(q)
-    qm = -q
-    if abs(p - q) > abs(p - qm)
-        q = qm
-    end
-    c = p.s * q.s + p.v1 * q.v1 + p.v2 * q.v2 + p.v3 * q.v3
-    if c < 1.0
-        o = acos(c)
-        s = sin(o)
-        sp = sin((1 - t) * o) / s
-        sq = sin(t * o) / s
-    else
-        sp = 1 - t
-        sq = t
-    end
-    Quaternion(sp * p.s  + sq * q.s,
-                sp * p.v1 + sq * q.v1,
-                sp * p.v2 + sq * q.v2,
-                sp * p.v3 + sq * q.v3, true)
-end
-
 quatrand(rng = Random.GLOBAL_RNG)  = quat(randn(rng), randn(rng), randn(rng), randn(rng))
 nquatrand(rng = Random.GLOBAL_RNG) = normalize(quatrand(rng))
 
@@ -340,42 +317,46 @@ function rotationmatrix_normalized(q::Quaternion)
         xz - sy      yz + sx  1 - (xx + yy)]
 end
 
-
-function slerp(qa::Quaternion{T}, qb::Quaternion{T}, t::T) where {T}
+@inline function slerp(qa0::Quaternion{T}, qb0::Quaternion{T}, t::T) where T<:Real
     # http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
-    coshalftheta = qa.s * qb.s + qa.v1 * qb.v1 + qa.v2 * qb.v2 + qa.v3 * qb.v3;
+    iszero(qa0) && throw(DomainError(qa0, "The input quaternion must be non-zero."))
+    iszero(qb0) && throw(DomainError(qb0, "The input quaternion must be non-zero."))
+    qa = qa0 / abs(qa0)
+    qb = qb0 / abs(qb0)
+    coshalftheta = qa.s * qb.s + qa.v1 * qb.v1 + qa.v2 * qb.v2 + qa.v3 * qb.v3
 
     if coshalftheta < 0
-        qm = -qb
+        qb = -qb
         coshalftheta = -coshalftheta
-    else
-        qm = qb
-    end
-    abs(coshalftheta) >= 1.0 && return qa
-
-    halftheta    = acos(coshalftheta)
-    sinhalftheta = sqrt(one(T) - coshalftheta * coshalftheta)
-
-    if abs(sinhalftheta) < 0.001
-        return Quaternion(
-            T(0.5) * (qa.s  + qb.s),
-            T(0.5) * (qa.v1 + qb.v1),
-            T(0.5) * (qa.v2 + qb.v2),
-            T(0.5) * (qa.v3 + qb.v3),
-        )
     end
 
-    ratio_a = sin((one(T) - t) * halftheta) / sinhalftheta
-    ratio_b = sin(t * halftheta) / sinhalftheta
+    if coshalftheta < 1
+        halftheta    = acos(coshalftheta)
+        sinhalftheta = sqrt(1 - coshalftheta^2)
+
+        ratio_a = sin((1 - t) * halftheta) / sinhalftheta
+        ratio_b = sin(t * halftheta) / sinhalftheta
+    else
+        ratio_a = float(1 - t)
+        ratio_b = float(t)
+    end
 
-    Quaternion(
-        qa.s  * ratio_a + qm.s  * ratio_b,
-        qa.v1 * ratio_a + qm.v1 * ratio_b,
-        qa.v2 * ratio_a + qm.v2 * ratio_b,
-        qa.v3 * ratio_a + qm.v3 * ratio_b,
+    return Quaternion(
+        qa.s  * ratio_a + qb.s  * ratio_b,
+        qa.v1 * ratio_a + qb.v1 * ratio_b,
+        qa.v2 * ratio_a + qb.v2 * ratio_b,
+        qa.v3 * ratio_a + qb.v3 * ratio_b,
+        true
     )
 end
 
+function slerp(qa::Quaternion{Ta}, qb::Quaternion{Tb}, t::T) where {Ta, Tb, T}
+    S = promote_type(Ta,Tb,T)
+    return slerp(Quaternion{S}(qa),Quaternion{S}(qb),S(t))
+end
+
+Base.@deprecate linpol(p::Quaternion, q::Quaternion, t::Real) slerp(p, q, t)
+
 function sylvester(a::Quaternion{T}, b::Quaternion{T}, c::Quaternion{T}) where {T<:Real}
     isreal(a) && return sylvester(real(a), b, c)
     isreal(b) && return sylvester(a, real(b), c)

src/Quaternions.jl

@@ -35,7 +35,6 @@ module Quaternions
   export angleaxis
   export angle
   export axis
-  export linpol
   export normalize
   export normalizea
   export dconj

test/Quaternion.jl

@@ -599,6 +599,9 @@ Base.:(/)(a::MyReal, b::Real) = a.val / b
                 @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 slerp(a, b, 0.0).norm
+                @test slerp(a, b, 1.0).norm
+                @test slerp(a, b, 0.5).norm
                 for _ in 1:100
                     q1 = quat(1, 0, 0, 0.0)
                     # there are numerical stability issues with slerp atm
@@ -630,6 +633,32 @@ Base.:(/)(a::MyReal, b::Real) = a.val / b
                     @test q ⊗ linpol(q1, q2, t) ≈ linpol(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 "DomainError" begin
+                @test_throws DomainError slerp(quat(1),quat(0),1)
+                @test_throws DomainError slerp(quat(0),quat(1),0)
+            end
+
+            @testset "Deprecated warning" begin
+                @test_deprecated linpol(quat(1),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
+            end
         end
     end