elevator_core/
movement.rs

1//! Trapezoidal velocity-profile movement physics.
2
3/// Distance required to brake to a stop from a given velocity at a fixed
4/// deceleration rate.
5///
6/// Uses the standard kinematic formula `v² / (2·a)`. Returns `0.0` for a
7/// stationary object or a non-positive deceleration (defensive: avoids
8/// division-by-zero / negative-distance footguns in consumer code).
9#[must_use]
10pub fn braking_distance(velocity: f64, deceleration: f64) -> f64 {
11    if deceleration <= 0.0 {
12        return 0.0;
13    }
14    let speed = velocity.abs();
15    speed * speed / (2.0 * deceleration)
16}
17
18/// Result of one tick of movement physics.
19#[derive(Debug, Clone, Copy)]
20pub struct MovementResult {
21    /// Current position after this tick.
22    pub position: f64,
23    /// Current velocity after this tick.
24    pub velocity: f64,
25    /// Whether the elevator has arrived at the target.
26    pub arrived: bool,
27}
28
29/// Advance position/velocity toward a target using a trapezoidal velocity profile.
30///
31/// - `position`: current position
32/// - `velocity`: current velocity (signed)
33/// - `target_position`: where we want to be
34/// - `max_speed`: maximum speed magnitude
35/// - `acceleration`: acceleration rate (positive)
36/// - `deceleration`: deceleration rate (positive)
37/// - `dt`: time step
38#[must_use]
39pub fn tick_movement(
40    position: f64,
41    velocity: f64,
42    target_position: f64,
43    max_speed: f64,
44    acceleration: f64,
45    deceleration: f64,
46    dt: f64,
47) -> MovementResult {
48    const EPSILON: f64 = 1e-9;
49
50    let displacement = target_position - position;
51
52    // Already at target and stationary.
53    if displacement.abs() < EPSILON && velocity.abs() < EPSILON {
54        return MovementResult {
55            position: target_position,
56            velocity: 0.0,
57            arrived: true,
58        };
59    }
60
61    let sign = displacement.signum();
62    let distance_remaining = displacement.abs();
63    let speed = velocity.abs();
64    let stopping_distance = speed * speed / (2.0 * deceleration);
65
66    let new_velocity = if stopping_distance >= distance_remaining - EPSILON {
67        // Decelerate
68        let v = (-deceleration * dt).mul_add(velocity.signum(), velocity);
69        // Clamp to zero if sign would flip.
70        if velocity > 0.0 && v < 0.0 || velocity < 0.0 && v > 0.0 {
71            0.0
72        } else {
73            v
74        }
75    } else if speed < max_speed {
76        // Accelerate toward target
77        let v = (acceleration * dt).mul_add(sign, velocity);
78        // Clamp magnitude to max_speed
79        if v.abs() > max_speed {
80            sign * max_speed
81        } else {
82            v
83        }
84    } else {
85        // Cruise
86        sign * max_speed
87    };
88
89    let new_pos = new_velocity.mul_add(dt, position);
90
91    // Overshoot check: did we cross the target?
92    let new_displacement = target_position - new_pos;
93    if new_displacement.abs() < EPSILON || (new_displacement.signum() - sign).abs() > EPSILON {
94        return MovementResult {
95            position: target_position,
96            velocity: 0.0,
97            arrived: true,
98        };
99    }
100
101    MovementResult {
102        position: new_pos,
103        velocity: new_velocity,
104        arrived: false,
105    }
106}
elevator_core/movement.rs

elevator_core/
movement.rs
