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    // Opposing direction: car is moving away from the (possibly retargeted)
67    // destination. Must brake at `deceleration` before accelerating back —
68    // not at `acceleration`, which is the wrong physics when accel ≠ decel.
69    let opposing = velocity * sign < 0.0;
70
71    let new_velocity = if opposing || stopping_distance >= distance_remaining - EPSILON {
72        // Decelerate
73        let v = crate::fp::fma(-safe_decel * dt, velocity.signum(), velocity);
74        // Clamp to zero if sign would flip.
75        if velocity > 0.0 && v < 0.0 || velocity < 0.0 && v > 0.0 {
76            0.0
77        } else {
78            v
79        }
80    } else if speed < max_speed {
81        // Accelerate toward target
82        let v = crate::fp::fma(acceleration * dt, sign, velocity);
83        // Clamp magnitude to max_speed
84        if v.abs() > max_speed {
85            sign * max_speed
86        } else {
87            v
88        }
89    } else {
90        // Cruise
91        sign * max_speed
92    };
93
94    let new_pos = crate::fp::fma(new_velocity, dt, position);
95
96    // Overshoot check: did we cross the target?
97    let new_displacement = target_position - new_pos;
98    if new_displacement.abs() < EPSILON || (new_displacement.signum() - sign).abs() > EPSILON {
99        return MovementResult {
100            position: target_position,
101            velocity: 0.0,
102            arrived: true,
103        };
104    }
105
106    MovementResult {
107        position: new_pos,
108        velocity: new_velocity,
109        arrived: false,
110    }
111}
elevator_core/movement.rs

elevator_core/
movement.rs
