Add slerp and tests

src/exports.jl

@@ -127,6 +127,7 @@ distance,
 phi,
 theta,
 zenith,
+slerp,
 
 # Real-time
 @ip_str,

src/tools/math.jl

@@ -17,10 +17,19 @@ distance(a::Position, b::Position) = norm(a - b)
 """
 
 Interpolate between two vectors (e.g. quaternions) using the slerp method. `t`
-should be between 0 and 1, 0 will produce `q1` and `1` `q2`.
+should be between 0 and 1. 0 will produce `q₁` and `1` `q₂`.
+
+The input vectors `q₁` and `q₂` will be normalised unless `normalized` is
+`false`. It is not done by default to shave off a few dozens of nanoseconds.
+Make sure to set `normalized=false` if the input vectors are not unit vectors.
 
 """
-function slerp(q₁, q₂, t::Real; dot_threshold=0.9995)
+function slerp(q₁, q₂, t::Real; dot_threshold=0.9995, normalized=true)
+    if !normalized
+        q₁ = normalize(q₁)
+        q₂ = normalize(q₂)
+    end
+
     dot = q₁⋅q₂
 
     if (dot < 0.0)

test/math.jl

+import KM3io: slerp
+using Test
+
+@testset "slerp()" begin
+    q1 = [1.0, 0.0]
+    q2 = [0.0, 1.0]
+    @test q1 ≈ slerp(q1, q2, 0)
+    @test q2 ≈ slerp(q1, q2, 1)
+    @test [0.9510565162951538, 0.30901699437494745] ≈ slerp(q1, q2, 0.2)
+    @test [0.70710678, 0.70710678] ≈ slerp(q1, q2, 0.5)
+    @test [0.45399049973954686, 0.8910065241883678] ≈ slerp(q1, q2, 0.7)
+
+    # should normalise internally
+    q1 = [0.4, 0.0]
+    q2 = [0.0, 0.9]
+    @test [1.0, 0.0] ≈ slerp(q1, q2, 0)
+    @test [0.0, 1.0] ≈ slerp(q1, q2, 1)
+    @test [0.9510565162951538, 0.30901699437494745] ≈ slerp(q1, q2, 0.2)
+    @test [0.70710678, 0.70710678] ≈ slerp(q1, q2, 0.5)
+    @test [0.45399049973954686, 0.8910065241883678] ≈ slerp(q1, q2, 0.7)
+
+    q1 = [1.0, 0.0, 0.0]
+    q2 = [0.0, 0.0, 1.0]
+    @test q1 ≈ slerp(q1, q2, 0)
+    @test q2 ≈ slerp(q1, q2, 1)
+    @test [0.9510565162951538, 0.0, 0.30901699437494745] ≈ slerp(q1, q2, 0.2)
+    @test [0.70710678, 0.0, 0.70710678] ≈ slerp(q1, q2, 0.5)
+    @test [0.45399049973954686, 0.0, 0.8910065241883678] ≈ slerp(q1, q2, 0.7)
+end

test/runtests.jl

@@ -8,4 +8,5 @@ include("hardware.jl")
 include("acoustics.jl")
 include("calibration.jl")
 include("physics.jl")
+include("math.jl")
 include("controlhost.jl")