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)
|
||||
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.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) =
|
||||
|
7
SADC.jl
7
SADC.jl
@@ -2,7 +2,6 @@ using LinearAlgebra
|
||||
using NumericalIntegration
|
||||
using ProgressBars
|
||||
|
||||
|
||||
include("Quaternions.jl")
|
||||
|
||||
dt = 0.005
|
||||
@@ -18,7 +17,6 @@ q = Quaternion([0; 0; 0; 1])
|
||||
|
||||
ω_last = [0; 0; 0; ω]
|
||||
ω0 = copy(ω_last)
|
||||
ω_area = [0; 0; 0; 0; 0; 0]
|
||||
|
||||
β = [0; 0; 0]
|
||||
|
||||
@@ -26,7 +24,6 @@ integrate_ω(T, I, ω) = Iinv * (T - cross(ω, I * ω))
|
||||
|
||||
function q_step(β)
|
||||
mag = β .^ 2 |> sum |> sqrt
|
||||
|
||||
Quaternion([normalize(β) .* sin(mag / 2); cos(mag / 2)])
|
||||
|
||||
end
|
||||
@@ -42,16 +39,14 @@ end
|
||||
|
||||
qs = []
|
||||
for t in ProgressBar(time)
|
||||
# t = collect(time)[1]
|
||||
ω′ = integrate_ω(T, I, ω)
|
||||
|
||||
|
||||
ω_new = integrate_vector([ω; ω′], ω_last)
|
||||
β = ω_new[1:3] .- β
|
||||
|
||||
ω_last = [ω; ω′]
|
||||
ω = ω_new[4:6] .- ω
|
||||
ω_area = ω_new .+ ω_area
|
||||
ω_last = [ω; ω′]
|
||||
|
||||
q = q * q_step(β)
|
||||
|
||||
|
Reference in New Issue
Block a user