-
Notifications
You must be signed in to change notification settings - Fork 25
/
constructor.jl
50 lines (40 loc) · 1.37 KB
/
constructor.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
"""
Body{T} <: Node{T}
A rigid body object
id: unique identifying number
name: unique identifying name
mass: inertial property (kilograms)
inertia: inertia matrix (kilograms meter^2)
state: State; representation of the system's: position, linear velocity, orientation, and angular velocity
shape: Shape; geometry information about the Body
"""
mutable struct Body{T} <: Node{T}
id::Int64
name::Symbol
mass::T
inertia::SMatrix{3,3,T,9}
state::State{T}
shape::Shape{T}
function Body(mass::Real, inertia::AbstractMatrix;
name::Symbol=Symbol("body_" * randstring(4)),
shape::Shape=EmptyShape())
T = promote_type(eltype.((mass, inertia))...)
new{T}(getGlobalID(), name, mass, inertia, State{T}(), shape)
end
end
function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, body::Body)
summary(io, body)
println(io, "")
println(io, " id: "*string(body.id))
println(io, " name: "*string(body.name))
println(io, " mass: "*string(body.mass))
println(io, " inertia: "*string(body.inertia))
end
Base.length(::Body) = 6
# warn that body has poor inertial properties
function check_body(body::Body)
if norm(body.mass) == 0 || norm(body.inertia) == 0
@info "Bad inertial properties detected"
end
#TODO check condition number and potentially adapt
end