mirror of
https://gitlab.com/MisterBiggs/astro-helper.git
synced 2025-06-16 15:17:20 +00:00
45 lines
1.3 KiB
Julia
45 lines
1.3 KiB
Julia
struct Quaternion
|
|
i::Float64
|
|
j::Float64
|
|
k::Float64
|
|
r::Float64
|
|
|
|
Quaternion(i, j, k, r) = norm([i, j, k, r]) ≈ 1 ? new(i, j, k, r) : error("Magnitude not equal to 1.")
|
|
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)
|
|
|
|
export Quaternion
|