97 lines
2.8 KiB
V
97 lines
2.8 KiB
V
module sim
|
|
|
|
import math
|
|
|
|
pub const (
|
|
default_rope_length = 0.25
|
|
default_bearing_mass = 0.03
|
|
default_magnet_spacing = 0.05
|
|
default_magnet_height = 0.03
|
|
default_magnet_strength = 10.0
|
|
default_gravity = 4.9
|
|
)
|
|
|
|
[params]
|
|
pub struct SimParams {
|
|
rope_length f64 = sim.default_rope_length
|
|
bearing_mass f64 = sim.default_bearing_mass
|
|
magnet_spacing f64 = sim.default_magnet_spacing
|
|
magnet_height f64 = sim.default_magnet_height
|
|
magnet_strength f64 = sim.default_magnet_strength
|
|
gravity f64 = sim.default_gravity
|
|
}
|
|
|
|
pub fn sim_params(params SimParams) SimParams {
|
|
return SimParams{
|
|
...params
|
|
}
|
|
}
|
|
|
|
pub fn (params SimParams) get_rope_vector(state SimState) Vector3D {
|
|
rope_origin := vector(z: params.rope_length)
|
|
|
|
return state.position + rope_origin.scale(-1)
|
|
}
|
|
|
|
pub fn (params SimParams) get_forces_sum(state SimState) Vector3D {
|
|
// force due to gravity
|
|
f_gravity := params.get_grav_force(state)
|
|
|
|
// force due to magnets
|
|
f_magnet1 := params.get_magnet1_force(state)
|
|
f_magnet2 := params.get_magnet2_force(state)
|
|
f_magnet3 := params.get_magnet3_force(state)
|
|
|
|
mut f_passive := vector(x: 0.0, y: 0.0, z: 0.0)
|
|
for force in [f_gravity, f_magnet1, f_magnet2, f_magnet3] {
|
|
f_passive = f_passive + force
|
|
}
|
|
|
|
// force due to tension of the rope
|
|
f_tension := params.get_tension_force(state, f_passive)
|
|
|
|
return f_passive + f_tension
|
|
}
|
|
|
|
pub fn (params SimParams) get_grav_force(state SimState) Vector3D {
|
|
return vector(z: -params.bearing_mass * params.gravity)
|
|
}
|
|
|
|
pub fn (params SimParams) get_magnet_position(theta f64) Vector3D {
|
|
return vector(
|
|
x: math.cos(theta) * params.magnet_spacing
|
|
y: math.sin(theta) * params.magnet_spacing
|
|
z: -params.magnet_height
|
|
)
|
|
}
|
|
|
|
pub fn (params SimParams) get_magnet_force(theta f64, state SimState) Vector3D {
|
|
magnet_position := params.get_magnet_position(theta)
|
|
mut diff := magnet_position + state.position.scale(-1)
|
|
distance_squared := diff.norm_squared()
|
|
diff = diff.scale(1.0 / math.sqrt(distance_squared))
|
|
return diff.scale(params.magnet_strength / distance_squared)
|
|
}
|
|
|
|
pub fn (params SimParams) get_magnet_dist(theta f64, state SimState) f64 {
|
|
return (params.get_magnet_position(theta) + state.position.scale(-1)).norm()
|
|
}
|
|
|
|
pub fn (params SimParams) get_magnet1_force(state SimState) Vector3D {
|
|
return params.get_magnet_force(0.0 * math.pi / 3.0, state)
|
|
}
|
|
|
|
pub fn (params SimParams) get_magnet2_force(state SimState) Vector3D {
|
|
return params.get_magnet_force(2.0 * math.pi / 3.0, state)
|
|
}
|
|
|
|
pub fn (params SimParams) get_magnet3_force(state SimState) Vector3D {
|
|
return params.get_magnet_force(4.0 * math.pi / 3.0, state)
|
|
}
|
|
|
|
pub fn (params SimParams) get_tension_force(state SimState, f_passive Vector3D) Vector3D {
|
|
rope_vector := params.get_rope_vector(state)
|
|
rope_vector_norm := rope_vector.scale(1.0 / rope_vector.norm())
|
|
return rope_vector_norm.scale(-1.0 * (rope_vector_norm * f_passive))
|
|
}
|