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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
//! Trapezoidal velocity-profile movement physics.
/// Distance required to brake to a stop from a given velocity at a fixed
/// deceleration rate.
///
/// Uses the standard kinematic formula `v² / (2·a)`. Returns `0.0` for a
/// stationary object or a non-positive deceleration (defensive: avoids
/// division-by-zero / negative-distance footguns in consumer code).
#[must_use]
pub fn braking_distance(velocity: f64, deceleration: f64) -> f64 {
if deceleration <= 0.0 {
return 0.0;
}
let speed = velocity.abs();
speed * speed / (2.0 * deceleration)
}
/// 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 safe_decel = deceleration.max(EPSILON);
let stopping_distance = speed * speed / (2.0 * safe_decel);
// Opposing direction: car is moving away from the (possibly retargeted)
// destination. Must brake at `deceleration` before accelerating back —
// not at `acceleration`, which is the wrong physics when accel ≠ decel.
let opposing = velocity * sign < 0.0;
let new_velocity = if opposing || stopping_distance >= distance_remaining - EPSILON {
// Decelerate
let v = crate::fp::fma(-safe_decel * dt, 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 = crate::fp::fma(acceleration * dt, 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 = crate::fp::fma(new_velocity, 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,
}
}