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 safe_decel = deceleration.max(EPSILON);
65    let stopping_distance = speed * speed / (2.0 * safe_decel);
66
67    let new_velocity = if stopping_distance >= distance_remaining - EPSILON {
68        // Decelerate
69        let v = (-safe_decel * dt).mul_add(velocity.signum(), velocity);
70        // Clamp to zero if sign would flip.
71        if velocity > 0.0 && v < 0.0 || velocity < 0.0 && v > 0.0 {
72            0.0
73        } else {
74            v
75        }
76    } else if speed < max_speed {
77        // Accelerate toward target
78        let v = (acceleration * dt).mul_add(sign, velocity);
79        // Clamp magnitude to max_speed
80        if v.abs() > max_speed {
81            sign * max_speed
82        } else {
83            v
84        }
85    } else {
86        // Cruise
87        sign * max_speed
88    };
89
90    let new_pos = new_velocity.mul_add(dt, position);
91
92    // Overshoot check: did we cross the target?
93    let new_displacement = target_position - new_pos;
94    if new_displacement.abs() < EPSILON || (new_displacement.signum() - sign).abs() > EPSILON {
95        return MovementResult {
96            position: target_position,
97            velocity: 0.0,
98            arrived: true,
99        };
100    }
101
102    MovementResult {
103        position: new_pos,
104        velocity: new_velocity,
105        arrived: false,
106    }
107}
elevator_core/movement.rs

elevator_core/
movement.rs
