Add Quaternion -> Compass (and tests)

src/root/calibration.jl

@@ -68,10 +68,10 @@ end
 """
 A compass with yaw, pitch and roll.
 """
-struct Compass
-    yaw::Float64
-    pitch::Float64
-    roll::Float64
+struct Compass{T} <: FieldVector{3, T}
+    yaw::T
+    pitch::T
+    roll::T
 end
 
 """
@@ -95,3 +95,20 @@ function Compass(q::Quaternion)
 
     Compass(yaw, pitch, roll)
 end
+
+Quaternion(c::Compass) = Quaternion(c.yaw, c.pitch, c.roll)
+function Quaternion(yaw, pitch, roll)
+    cr = cos(-roll*0.5)
+    sr = sin(-roll*0.5)
+    cp = cos(pitch*0.5)
+    sp = sin(pitch*0.5)
+    cy = cos(-yaw*0.5)
+    sy = sin(-yaw*0.5)
+
+    Quaternion(
+        cr * cp * cy + sr * sp * sy,
+        sr * cp * cy - cr * sp * sy,
+        cr * sp * cy + sr * cp * sy,
+        cr * cp * sy - sr * sp * cy
+    )
+end

test/math.jl

-import KM3io: slerp
+using KM3io
 using Test
 
 @testset "slerp()" begin
@@ -27,3 +27,14 @@ using Test
     @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
+
+
+@testset "quaternions and compasses" begin
+    c = Compass(1.0, 1.0, 1.0)
+    q = Quaternion(c)
+    @test 0.7860666291368439 == q.q0
+    @test -0.16751879124639693 == q.qx
+    @test 0.5709414713577319 == q.qy
+    @test -0.16751879124639693 == q.qz
+    @test c ≈ Compass(q)
+end