Simulink 2 fully implemented #3

Quaternions.jl

@@ -1,15 +1,14 @@
-
-
-export Quaternion
-
 struct Quaternion
     i::Float64
     j::Float64
     k::Float64
     r::Float64
 
+    # TODO: Determine an acceptable tolerance. ≈ default is too strict.
     Quaternion(i, j, k, r) =
-        norm([i, j, k, r]) ≈ 1 ? new(i, j, k, r) : error("Magnitude not equal to 1.")
+        abs(norm([i, j, k, r]) - 1) < 0.1 ? new(i, j, k, r) :
+        error("Magnitude not equal to 1: " * string(norm([i, j, k, r])))
+
     Quaternion(q) = Quaternion(q[1], q[2], q[3], q[4])
     Quaternion() = new(0, 0, 0, 1)
 
@@ -52,3 +51,42 @@ Base.isapprox(a::Quaternion, b::Quaternion) = isapprox(collect(a), collect(b))
 
 LinearAlgebra.norm(q::Quaternion) = norm(collect(q))
 LinearAlgebra.normalize(q::Quaternion) = collect(q) / norm(q)
+
+# I dont think we need an actual type for this it might just be easier to keep it as a Vector or something.
+# struct EulerAngles
+#     roll::Float64
+#     pitch::Float64
+#     yaw::Float64
+
+#     EulerAngles(r, p, y) = new(r, p, y)
+#     EulerAngles() = new(0.0, 0.0, 0.0)
+# end
+
+
+function q_to_Euler(q::Quaternion)
+    # https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2
+
+    # roll (x-axis rotation)
+    sinr_cosp = 2 * (q.r * q.i + q.j * q.k)
+    cosr_cosp = 1 - 2 * (q.i * q.i + q.j * q.j)
+    roll = atan(sinr_cosp, cosr_cosp)
+
+    # pitch (y-axis rotation)
+    sinp = 2 * (q.r * q.j - q.k * q.i)
+    if (abs(sinp) >= 1)
+        pitch = copysign(π / 2, sinp) # use 90 degrees if out of range
+    else
+        pitch = asin(sinp)
+    end
+
+    # yaw (z-axis rotation)
+    siny_cosp = 2 * (q.r * q.k + q.i * q.j)
+    cosy_cosp = 1 - 2 * (q.j * q.j + q.k * q.k)
+    yaw = atan(siny_cosp, cosy_cosp)
+
+
+    # return EulerAngles(roll, pitch, yaw)
+    return (roll = rad2deg(roll), pitch = rad2deg(pitch), yaw = rad2deg(yaw))
+    # return (roll = roll, pitch = pitch, yaw = yaw)
+end
+