四元数坐标 — RibleQCF

RibleQCF 是一个扩展包，在默认的自然坐标格式（NCF）之外提供基于四元数的坐标类型 QC。单个刚体由 7 个广义坐标描述——3 个平移分量和 4 个单位四元数分量——在单位模长约束被处理之后得到 6 个自由度。其惯量表示及数值影响遵循 [2] 和 [3] 所建立的框架。

Tip

QCF 会得到非常数质量矩阵，需要在状态更新时重新计算，并涉及到复杂的雅可比计算。

坐标类型 `QC`

核心类型 QCF.QC 存储质量、对角化的转动惯量张量以及由它们推导出的若干预计算标量：

julia

struct QC{T,JT} <: AbstractCoordinates{3,T}
    m      # 质量
    m⁻¹    # 1/质量
    γ      # 惯量对角元素最大值（正则化参数）
    Jγ     # J - γ·I
    γ⁻¹    # 1/γ
    J⁻¹γ   # J⁻¹ - (1/γ)·I
end

通过传入质量和 3×3 惯量矩阵来构造 QC 实例：

julia

m = 0.5
J = Diagonal(SA[0.001, 0.001, 0.002])

qc = QCF.QC(m, J)

惯量矩阵仅有对角元素被使用，非对角项会在内部被丢弃。可选关键字参数 γ 默认为 maximum(diag(J))，作为正则化参数保持旋转质量矩阵的良好条件数。

查询坐标特性

可以在任意层级查询刚体是否使用恒定质量坐标：

julia

has_constant_mass_matrix(body.coords)   # QC 返回 Val(false)
has_constant_mass_matrix(body)          # 从 coords 向上传播
has_constant_mass_matrix(structure)     # 从 bodies 向上传播

非恒定质量矩阵

与 NCF（has_constant_mass_matrix 返回 Val(true)）不同，QCF 的广义质量矩阵依赖于当前四元数状态：

julia

has_constant_mass_matrix(::QCF.QC) = Val(false)

7×7 质量矩阵为块对角形式：

其中是 4×4 四元数左乘矩阵。由于每步都变化，求解器必须在每次迭代中重新组装和分解它。Rible 通过求解器分派自动处理这一点：当检测到 Val(false) 时，会选择 Zhong06_Nonconstant_Mass 求解器族而非恒定质量族。

实际影响

每步计算代价高于 NCF，因为需要逐步组装并分解质量矩阵。对于单体仿真差异可忽略；对于包含大量 QCF 刚体的多体系统，需权衡 NCF 是否足够。
紧容差有助于稳定性。四元数约束在求解器容差较松时会漂移。推荐默认使用 ftol = 1e-14、maxiters = 50。

内禀约束

QC 携带一个内禀代数约束——四元数单位模长：

此约束由求解器与关节约束一起自动执行。框架在 constraints.jl 中提供了约束函数、其雅可比矩阵以及力雅可比矩阵。

完整示例：基于 QCF 的旋转陀螺

内置的旋转陀螺模型通过 cT 参数同时支持 NCF 和 QCF。下面我们构造一个 QCF 陀螺，验证其非恒定质量特性，并执行完整的接触仿真。

构造

julia

include(joinpath(pathof(Rible), "../../examples/robots/spinningtop.jl"))

origin_position = [0.0, 0.0, 0.5]
R = RotX(0.0)
origin_velocity = [1.0, 0.0, 0.0]
Ω = [0.0, 0.0, 200.0]

top = make_top(origin_position, R, origin_velocity, Ω, :QCF; μ = 0.95, e = 0.5, loadmesh = true)

Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}}, 1}, Vector{Int64}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{Vector{Int64}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, NaiveOperator, Vector{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}(Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}}, 1}, Vector{Int64}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}(3, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}}, 1}((RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}[RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}(RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 0.5838707, [0.00022129 0.0 0.0; 0.0 0.00022129 0.0; 0.0 0.0 0.00030207], Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.0, 0.0, -0.03795882], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.46204118], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}(0.5838707, 1.7127079677058636, 0.00030207, Diagonal([-8.078e-5, -8.078e-5, 0.0]), 3310.4909458072634, Diagonal([1208.4660789114314, 1208.4660789114314, 0.0])), RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0], StaticArraysCore.MMatrix{3, 7, Float64, 21}[[1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … -0.07591764 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0], [1.0 0.0 … 0.0 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [0.5838707, 0.5838707, 0.5838707, 0.00120828, 0.00088516, 0.00088516, 0.00120828], 7, 7), sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [1.7127079677058636, 1.7127079677058636, 1.7127079677058636, 827.6227364518159, 1129.7392561796737, 1129.7392561796737, 827.6227364518159], 7, 7), sparse([4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7], [4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7], [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0], 7, 7), sparse([4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7], [4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7], [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0], 7, 7), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0], true), nothing), Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}(...))],), ([1],)), Int64[], Connectivity{Vector{Vector{Int64}}}(1, 0, 0, 0, 7, 1, 0, 1, 6, 6, 0, [1, 2, 3, 4, 5, 6, 7], [[1, 2, 3, 4, 5, 6, 7]], [[1]], [[1, 2, 3, 4, 5, 6]], Vector{Int64}[], Vector{Int64}[], [[1, 2, 3, 4, 5, 6]], Signifier{Int64}[Signifier{Int64}(1, 1), Signifier{Int64}(1, 2), Signifier{Int64}(1, 3), Signifier{Int64}(1, 4), Signifier{Int64}(1, 5), Signifier{Int64}(1, 6)], [[1, 2, 3], [4, 5, 6], [7, 8, 9], [10, 11, 12], [13, 14, 15], [16, 17, 18]], [[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18]], 7, 18, 18, 0, 18, Vector{Int64}[], Vector{Int64}[]), Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}(CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, -0.0, 0.0, 0.0, 100.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0], Float64[], [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0], [0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.0, 0.0, -0.03795882, 0.0, 0.0, 0.0]), CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}[CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}(0.0, [0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, -0.0, 0.0, 0.0, 100.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.0, 0.0, -0.03795882, 0.0, 0.0, 0.0])]), (activated_bits = Bool[0, 0, 0, 0, 0, 0], persistent_bits = Bool[0, 0, 0, 0, 0, 0], friction_coefficients = [0.95, 0.95, 0.95, 0.95, 0.95, 0.95], restitution_coefficients = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5], gaps = [Inf, Inf, Inf, Inf, Inf, Inf]), Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}(InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}(sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 7, 7), sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 7, 7), sparse(Int64[], Int64[], Float64[], 7, 7), sparse(Int64[], Int64[], Float64[], 7, 7), sparsevec(Int64[], Float64[], 7), sparsevec(Int64[], Float64[], 7), true))), ControlHub{Vector{Int64}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, NaiveOperator, Vector{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}(Int64[], TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, PositionCaptum, Vector{Float64}}}}, 1}((ErrorGauge{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, PositionCaptum, Vector{Float64}}[ErrorGauge{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, PositionCaptum, Vector{Float64}}(1, Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}(RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}(RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 0.5838707, [0.00022129 0.0 0.0; 0.0 0.00022129 0.0; 0.0 0.0 0.00030207], Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.0, 0.0, -0.03795882], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.46204118], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}(0.5838707, 1.7127079677058636, 0.00030207, Diagonal([-8.078e-5, -8.078e-5, 0.0]), 3310.4909458072634, Diagonal([1208.4660789114314, 1208.4660789114314, 0.0])), RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0], StaticArraysCore.MMatrix{3, 7, Float64, 21}[[1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … -0.07591764 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0], [1.0 0.0 … 0.0 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [0.5838707, 0.5838707, 0.5838707, 0.00120828, 0.00088516, 0.00088516, 0.00120828], 7, 7), sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [1.7127079677058636, 1.7127079677058636, 1.7127079677058636, 827.6227364518159, 1129.7392561796737, 1129.7392561796737, 827.6227364518159], 7, 7), sparse([4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7], [4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7], [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0], 7, 7), sparse([4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7], [4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7], [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0], 7, 7), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0], true), nothing), Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}(...)), 6), PositionCaptum(), [0.5, 0.0, 0.2])],), ([1],)), TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, NaiveOperator, Vector{Float64}, Float64}}}, 1}((ExternalForceActuator{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, NaiveOperator, Vector{Float64}, Float64}[ExternalForceActuator{Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}, NaiveOperator, Vector{Float64}, Float64}(1, Signifier{RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}}(RigidBody{3, 3, Float64, 9, RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}, RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}, GeometryBasics.Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, (:position, :normal, :color), Tuple{Vector{GeometryBasics.Point{3, Float64}}, Vector{GeometryBasics.Vec{3, Float32}}, Vector{ColorTypes.RGB{Float32}}}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}(RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 0.5838707, [0.00022129 0.0 0.0; 0.0 0.00022129 0.0; 0.0 0.0 0.00030207], Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.0, 0.0, -0.03795882], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5), Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.95, 0.5)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.03111269837220809, 0.03111269837220809, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.46204118], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.5], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), RibleQCF.QC{Float64, LinearAlgebra.Diagonal{Float64, StaticArraysCore.SVector{3, Float64}}}(0.5838707, 1.7127079677058636, 0.00030207, Diagonal([-8.078e-5, -8.078e-5, 0.0]), 3310.4909458072634, Diagonal([1208.4660789114314, 1208.4660789114314, 0.0])), RigidBodyCache{StaticArraysCore.MMatrix{3, 7, Float64, 21}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}, Nothing}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0], StaticArraysCore.MMatrix{3, 7, Float64, 21}[[1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … 0.0 -0.06222539674441618; 0.0 1.0 … 0.0 0.06222539674441618; 0.0 0.0 … -0.06222539674441618 0.0], [1.0 0.0 … -0.07591764 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0], [1.0 0.0 … 0.0 -0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … -0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{7, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [0.5838707, 0.5838707, 0.5838707, 0.00120828, 0.00088516, 0.00088516, 0.00120828], 7, 7), sparse([1, 2, 3, 4, 5, 6, 7], [1, 2, 3, 4, 5, 6, 7], [1.7127079677058636, 1.7127079677058636, 1.7127079677058636, 827.6227364518159, 1129.7392561796737, 1129.7392561796737, 827.6227364518159], 7, 7), sparse([4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7], [4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7], [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0], 7, 7), sparse([4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7], [4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7], [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0], 7, 7), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0], true), nothing), Mesh{3, Float64, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}(...)), 5), NaiveOperator(1), [0.0, 1.0, 0.0], [0.0])],), ([1],)), Coalition(1, 0, 1, 0, 1, Vector{Int64}[], [[1]], 1, [[1]]), (c = Float64[], e = [0.16999999999999998], u = [0.0])), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}[CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, -0.0, 0.0, 0.0, 100.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0], Float64[], [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0], [0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.03111269837220809, 0.03111269837220809, 0.0, 0.0, 0.0, -0.03795882, 0.0, 0.0, 0.0])], Vector{Rible.Contact{Float64}}[[Rible.Contact{Float64}(1, 0.95, 0.5, Rible.ContactState{3, Float64, 9}(false, true, 1.0, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0])), Rible.Contact{Float64}(2, 0.95, 0.5, Rible.ContactState{3, Float64, 9}(false, true, 1.0, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0])), Rible.Contact{Float64}(3, 0.95, 0.5, Rible.ContactState{3, Float64, 9}(false, true, 1.0, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0])), Rible.Contact{Float64}(4, 0.95, 0.5, Rible.ContactState{3, Float64, 9}(false, true, 1.0, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0])), Rible.Contact{Float64}(5, 0.95, 0.5, Rible.ContactState{3, Float64, 9}(false, true, 1.0, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0])), Rible.Contact{Float64}(6, 0.95, 0.5, Rible.ContactState{3, Float64, 9}(false, true, 1.0, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]))]], @NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}[(na = 0, bodyid2act_idx = [[0, 0, 0, 0, 0, 0]], persistent_idx = [], activated_bits = [0, 0, 0, 0, 0, 0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 7), Dper = Matrix{Float64}(undef, 0, 7), Dimp = Matrix{Float64}(undef, 0, 7), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 7), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = [])], [(c = Float64[], e = [0.16999999999999998], u = [0.0])])

验证非恒定质量特性

julia

body1 = get_bodies(top.structure)[1]
has_constant_mass_matrix(body1.coords) === Val(false)

true

接触设置与仿真

求解器对象与 NCF 相同——一个带有内层接触求解器的 Zhong06 积分器。框架在内部将 QCF 刚体路由到非恒定质量代码路径。

julia

planes = StaticContactSurfaces([
    HalfSpace([0.0, 0.0, 1.0], [0.0, 0.0, 0.0]),
])

contact_model = RestitutionFrictionCombined(
    NewtonRestitution(),
    CoulombFriction(),
)

prob = DynamicsProblem(top; env=planes, contact_model)

solver = DynamicsSolver(
    Zhong06(),
    InnerLayerContactSolver(InteriorPointMethod()),
)

sim_result = solve!(
    prob,
    solver;
    tspan = (0.0, 0.2),
    dt = 1e-3,
    ftol = 1e-14,
    maxiters = 50,
    exception = false,
)

tip_pos = get_trajectory!(top, 1, 1).u[end]
tip_vel = get_velocity!(top, 1, 1).u[end]
tip_pos, tip_vel

([0.15600299208285928, -0.0005131221481740021, 0.3038000000000203], [1.1026244296346408, -8.79940158342844, -1.9619999999997975])

可视化

由于陀螺在构造时使用了 loadmesh = true，vis! 会直接渲染 STL 几何体：

julia

key_steps = round.(Int, range(1, length(top.traj.t), length = 4))

fig_traj = plot_traj!(
    top;
    do_slide = false,
    show_info = false,
    show_loci = false,
    show_background = false,
    show_ground = false,
    gridsize = (2, 2),
    at_steps = key_steps,
)

对比 QCF 与 NCF 结果

可以用相同的初始条件和 :NCF 重建模型进行对比：

julia

top_ncf = make_top(origin_position, R, origin_velocity, Ω, :NCF; μ = 0.95, e = 0.5, loadmesh = false)

has_constant_mass_matrix(get_bodies(top_ncf.structure)[1].coords) === Val(true)

true

参考文献

X. Xu and W. Zhong. On the Numerical Influences of Inertia Representation for Rigid Body Dynamics in Terms of Unit Quaternion. Journal of Applied Mechanics-Transactions of the ASME 83, 061006 (2016). Accessed on Oct 30, 2022.
X. Xu, J. Luo and Z. Wu. The Numerical Influence of Additional Parameters of Inertia Representations for Quaternion-Based Rigid Body Dynamics. Multibody System Dynamics 49, 237–270 (2020). Accessed on Oct 13, 2022.

四元数坐标 — RibleQCF ​

坐标类型 QC ​

非恒定质量矩阵 ​

实际影响 ​

内禀约束 ​

完整示例：基于 QCF 的旋转陀螺 ​

构造 ​

验证非恒定质量特性 ​

接触设置与仿真 ​

可视化 ​

对比 QCF 与 NCF 结果 ​

参考文献 ​

四元数坐标 — RibleQCF

坐标类型 `QC`

非恒定质量矩阵

实际影响

内禀约束

完整示例：基于 QCF 的旋转陀螺

构造

验证非恒定质量特性

接触设置与仿真

可视化

对比 QCF 与 NCF 结果

参考文献