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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
//! Trapezoidal velocity-profile movement physics.
/// Result of one tick of movement physics.
#[derive(Debug, Clone, Copy)]
pub struct MovementResult {
/// Current position after this tick.
pub position: f64,
/// Current velocity after this tick.
pub velocity: f64,
/// Whether the elevator has arrived at the target.
pub arrived: bool,
}
/// Advance position/velocity toward a target using a trapezoidal velocity profile.
///
/// - `position`: current position
/// - `velocity`: current velocity (signed)
/// - `target_position`: where we want to be
/// - `max_speed`: maximum speed magnitude
/// - `acceleration`: acceleration rate (positive)
/// - `deceleration`: deceleration rate (positive)
/// - `dt`: time step
#[must_use]
pub fn tick_movement(
position: f64,
velocity: f64,
target_position: f64,
max_speed: f64,
acceleration: f64,
deceleration: f64,
dt: f64,
) -> MovementResult {
const EPSILON: f64 = 1e-9;
let displacement = target_position - position;
// Already at target and stationary.
if displacement.abs() < EPSILON && velocity.abs() < EPSILON {
return MovementResult {
position: target_position,
velocity: 0.0,
arrived: true,
};
}
let sign = displacement.signum();
let distance_remaining = displacement.abs();
let speed = velocity.abs();
let stopping_distance = speed * speed / (2.0 * deceleration);
let new_velocity = if stopping_distance >= distance_remaining - EPSILON {
// Decelerate
let v = (-deceleration * dt).mul_add(velocity.signum(), velocity);
// Clamp to zero if sign would flip.
if velocity > 0.0 && v < 0.0 || velocity < 0.0 && v > 0.0 {
0.0
} else {
v
}
} else if speed < max_speed {
// Accelerate toward target
let v = (acceleration * dt).mul_add(sign, velocity);
// Clamp magnitude to max_speed
if v.abs() > max_speed {
sign * max_speed
} else {
v
}
} else {
// Cruise
sign * max_speed
};
let new_pos = new_velocity.mul_add(dt, position);
// Overshoot check: did we cross the target?
let new_displacement = target_position - new_pos;
if new_displacement.abs() < EPSILON || (new_displacement.signum() - sign).abs() > EPSILON {
return MovementResult {
position: target_position,
velocity: 0.0,
arrived: true,
};
}
MovementResult {
position: new_pos,
velocity: new_velocity,
arrived: false,
}
}