1
0
mirror of https://gitlab.com/MisterBiggs/SADC.jl.git synced 2025-07-27 16:41:32 +00:00
This commit is contained in:
2021-08-13 03:31:18 -07:00
parent eb737cefba
commit 4b817cbb26
2 changed files with 2 additions and 8 deletions

View File

@@ -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) =

View File

@@ -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(β)