mirror of
https://gitlab.com/MisterBiggs/SADC.jl.git
synced 2025-07-27 16:41:32 +00:00
cleanup
This commit is contained in:
@@ -29,14 +29,13 @@ end
|
|||||||
|
|
||||||
function QuaternionMultiplication(l::Quaternion, r::Quaternion)
|
function QuaternionMultiplication(l::Quaternion, r::Quaternion)
|
||||||
R = [
|
R = [
|
||||||
r.r r.k -r.j r.i
|
r.r r.k r.j r.i
|
||||||
-r.k r.r r.i r.j
|
-r.k r.r r.i r.j
|
||||||
r.j -r.i r.r r.k
|
r.j -r.i r.r r.k
|
||||||
-r.i -r.j -r.k r.r
|
-r.i -r.j -r.k r.r
|
||||||
]
|
]
|
||||||
L = [l.i; l.j; l.k; l.r]
|
L = [l.i; l.j; l.k; l.r]
|
||||||
return Quaternion(R * L)
|
return Quaternion(R * L)
|
||||||
|
|
||||||
end
|
end
|
||||||
|
|
||||||
Base.:*(l::Quaternion, r::Quaternion) =
|
Base.:*(l::Quaternion, r::Quaternion) =
|
||||||
|
7
SADC.jl
7
SADC.jl
@@ -2,7 +2,6 @@ using LinearAlgebra
|
|||||||
using NumericalIntegration
|
using NumericalIntegration
|
||||||
using ProgressBars
|
using ProgressBars
|
||||||
|
|
||||||
|
|
||||||
include("Quaternions.jl")
|
include("Quaternions.jl")
|
||||||
|
|
||||||
dt = 0.005
|
dt = 0.005
|
||||||
@@ -18,7 +17,6 @@ q = Quaternion([0; 0; 0; 1])
|
|||||||
|
|
||||||
ω_last = [0; 0; 0; ω]
|
ω_last = [0; 0; 0; ω]
|
||||||
ω0 = copy(ω_last)
|
ω0 = copy(ω_last)
|
||||||
ω_area = [0; 0; 0; 0; 0; 0]
|
|
||||||
|
|
||||||
β = [0; 0; 0]
|
β = [0; 0; 0]
|
||||||
|
|
||||||
@@ -26,7 +24,6 @@ integrate_ω(T, I, ω) = Iinv * (T - cross(ω, I * ω))
|
|||||||
|
|
||||||
function q_step(β)
|
function q_step(β)
|
||||||
mag = β .^ 2 |> sum |> sqrt
|
mag = β .^ 2 |> sum |> sqrt
|
||||||
|
|
||||||
Quaternion([normalize(β) .* sin(mag / 2); cos(mag / 2)])
|
Quaternion([normalize(β) .* sin(mag / 2); cos(mag / 2)])
|
||||||
|
|
||||||
end
|
end
|
||||||
@@ -42,16 +39,14 @@ end
|
|||||||
|
|
||||||
qs = []
|
qs = []
|
||||||
for t in ProgressBar(time)
|
for t in ProgressBar(time)
|
||||||
# t = collect(time)[1]
|
|
||||||
ω′ = integrate_ω(T, I, ω)
|
ω′ = integrate_ω(T, I, ω)
|
||||||
|
|
||||||
|
|
||||||
ω_new = integrate_vector([ω; ω′], ω_last)
|
ω_new = integrate_vector([ω; ω′], ω_last)
|
||||||
β = ω_new[1:3] .- β
|
β = ω_new[1:3] .- β
|
||||||
|
|
||||||
ω_last = [ω; ω′]
|
|
||||||
ω = ω_new[4:6] .- ω
|
ω = ω_new[4:6] .- ω
|
||||||
ω_area = ω_new .+ ω_area
|
ω_last = [ω; ω′]
|
||||||
|
|
||||||
q = q * q_step(β)
|
q = q * q_step(β)
|
||||||
|
|
||||||
|
Reference in New Issue
Block a user