mirror of
https://gitlab.com/MisterBiggs/SADC.jl.git
synced 2025-07-27 16:41:32 +00:00
92 lines
2.7 KiB
Julia
92 lines
2.7 KiB
Julia
struct Quaternion
|
|
i::Float64
|
|
j::Float64
|
|
k::Float64
|
|
r::Float64
|
|
|
|
# TODO: Determine an acceptable tolerance. ≈ default is too strict.
|
|
Quaternion(i, j, k, r) =
|
|
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)
|
|
|
|
Quaternion(yaw, pitch, roll) =
|
|
Quaternion([0 0 sin(yaw / 2) cos(yaw / 2)]) *
|
|
Quaternion([0 sin(pitch / 2) 0 cos(pitch / 2)]) *
|
|
Quaternion([sin(roll / 2) 0 0 cos(roll / 2)])
|
|
|
|
function Quaternion(ē, ϕ)
|
|
if sum(ē) == 0
|
|
return Quaternion([0 0 0 cos(ϕ / 2)])
|
|
end
|
|
dir = normalize(ē) * sin(ϕ / 2)
|
|
return Quaternion(dir[1], dir[2], dir[3], cos(ϕ / 2))
|
|
end
|
|
|
|
end
|
|
|
|
function QuaternionMultiplication(l::Quaternion, r::Quaternion)
|
|
R = [
|
|
r.r r.k r.j r.i
|
|
-r.k r.r r.i r.j
|
|
r.j -r.i r.r r.k
|
|
-r.i -r.j -r.k r.r
|
|
]
|
|
L = [l.i; l.j; l.k; l.r]
|
|
return Quaternion(R * L)
|
|
end
|
|
|
|
Base.:*(l::Quaternion, r::Quaternion) =
|
|
QuaternionMultiplication(l::Quaternion, r::Quaternion)
|
|
|
|
Base.iterate(q::Quaternion, state = 1) =
|
|
state > 4 ? nothing : (collect(q)[state], state + 1)
|
|
Base.length(q::Quaternion) = 4
|
|
Base.collect(q::Quaternion) = [q.i, q.j, q.k, q.r]
|
|
Base.getindex(q::Quaternion, i) = collect(q)[i]
|
|
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
|
|
|