map_model/
traversable.rs

1use std::fmt;
2
3use serde::{Deserialize, Serialize};
4
5use geom::{Angle, Distance, PolyLine, Pt2D, Speed};
6
7use crate::{DirectedRoadID, Direction, LaneID, Map, MovementID, PathConstraints, TurnID};
8
9/// Represents a specific point some distance along a lane.
10#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
11pub struct Position {
12    // Don't let callers construct a Position directly, so it's easy to find callers of new().
13    lane: LaneID,
14    dist_along: Distance,
15}
16
17impl fmt::Display for Position {
18    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
19        write!(f, "Position({}, {})", self.lane, self.dist_along)
20    }
21}
22
23impl Position {
24    pub fn new(lane: LaneID, dist_along: Distance) -> Position {
25        Position { lane, dist_along }
26    }
27
28    pub fn start(lane: LaneID) -> Position {
29        Position {
30            lane,
31            dist_along: Distance::ZERO,
32        }
33    }
34
35    pub fn end(lane: LaneID, map: &Map) -> Position {
36        Position {
37            lane,
38            dist_along: map.get_l(lane).length(),
39        }
40    }
41
42    pub fn lane(&self) -> LaneID {
43        self.lane
44    }
45
46    pub fn dist_along(&self) -> Distance {
47        self.dist_along
48    }
49
50    pub fn pt(&self, map: &Map) -> Pt2D {
51        match map
52            .get_l(self.lane)
53            .lane_center_pts
54            .dist_along(self.dist_along)
55        {
56            Ok((pt, _)) => pt,
57            Err(err) => panic!("{} invalid: {}", self, err),
58        }
59    }
60
61    pub fn pt_and_angle(&self, map: &Map) -> (Pt2D, Angle) {
62        match map
63            .get_l(self.lane)
64            .lane_center_pts
65            .dist_along(self.dist_along)
66        {
67            Ok(pair) => pair,
68            Err(err) => panic!("{} invalid: {}", self, err),
69        }
70    }
71
72    /// Given a position along a lane, find the equivalent position along another lane on the same
73    /// road.
74    pub fn equiv_pos(&self, lane: LaneID, map: &Map) -> Position {
75        self.equiv_pos_for_long_object(lane, Distance::ZERO, map)
76    }
77    pub fn equiv_pos_for_long_object(
78        &self,
79        other_lane: LaneID,
80        object_length: Distance,
81        map: &Map,
82    ) -> Position {
83        let our_lane = map.get_l(self.lane);
84        let other_lane = map.get_l(other_lane);
85        assert_eq!(our_lane.id.road, other_lane.id.road);
86
87        let pl = &other_lane.lane_center_pts;
88        let pt = self.pt(map);
89        if let Some((mut dist, _)) = pl.dist_along_of_point(pl.project_pt(pt)) {
90            if other_lane.dir != our_lane.dir {
91                // Account for the object_length if needed
92                dist += object_length;
93                dist = dist.max(Distance::ZERO).min(other_lane.length());
94            }
95            return Position::new(other_lane.id, dist);
96        }
97        // TODO So far I haven't observed this happening, but fallback if so.
98        warn!("equiv_pos of {} for {} messes up", self, other_lane.id);
99
100        let len = other_lane.length();
101        if other_lane.dir == our_lane.dir {
102            Position::new(other_lane.id, self.dist_along.min(len))
103        } else {
104            Position::new(
105                other_lane.id,
106                (len - self.dist_along + object_length)
107                    .max(Distance::ZERO)
108                    .min(len),
109            )
110        }
111    }
112    pub fn min_dist(mut self, dist_along: Distance, map: &Map) -> Option<Position> {
113        if self.dist_along >= dist_along {
114            return Some(self);
115        }
116        if map.get_l(self.lane).length() < dist_along {
117            return None;
118        }
119        self.dist_along = dist_along;
120        Some(self)
121    }
122    pub fn buffer_dist(mut self, buffer: Distance, map: &Map) -> Option<Position> {
123        let len = map.get_l(self.lane).length();
124        if len <= buffer * 2.0 {
125            return None;
126        }
127        self.dist_along = self.dist_along.max(buffer).min(len - buffer);
128        Some(self)
129    }
130}
131
132/// Either a lane or a turn, where most movement happens.
133// TODO Consider adding building and parking lot driveways here.
134#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
135pub enum Traversable {
136    Lane(LaneID),
137    Turn(TurnID),
138}
139
140impl fmt::Display for Traversable {
141    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
142        match self {
143            Traversable::Lane(id) => write!(f, "Traversable::Lane({})", id.encode_u32()),
144            Traversable::Turn(id) => write!(
145                f,
146                "Traversable::Turn({}, {}, {})",
147                id.src, id.dst, id.parent
148            ),
149        }
150    }
151}
152
153impl Traversable {
154    pub fn as_lane(&self) -> LaneID {
155        match *self {
156            Traversable::Lane(id) => id,
157            Traversable::Turn(_) => panic!("not a lane"),
158        }
159    }
160
161    pub fn as_turn(&self) -> TurnID {
162        match *self {
163            Traversable::Turn(id) => id,
164            Traversable::Lane(_) => panic!("not a turn"),
165        }
166    }
167
168    pub fn maybe_turn(&self) -> Option<TurnID> {
169        match *self {
170            Traversable::Turn(id) => Some(id),
171            Traversable::Lane(_) => None,
172        }
173    }
174
175    pub fn maybe_lane(&self) -> Option<LaneID> {
176        match *self {
177            Traversable::Turn(_) => None,
178            Traversable::Lane(id) => Some(id),
179        }
180    }
181
182    /// Return the center-line geometry of this lane or turn.
183    pub fn get_polyline(self, map: &Map) -> &PolyLine {
184        match self {
185            Traversable::Lane(id) => &map.get_l(id).lane_center_pts,
186            Traversable::Turn(id) => &map.get_t(id).geom,
187        }
188    }
189
190    pub fn get_zorder(&self, map: &Map) -> isize {
191        match *self {
192            Traversable::Lane(id) => map.get_parent(id).zorder,
193            Traversable::Turn(id) => map.get_i(id.parent).get_zorder(map),
194        }
195    }
196
197    /// The single definitive place to determine how fast somebody could go along a single road.
198    /// This should be used for pathfinding and simulation. Returns (speed, percent incline).
199    pub(crate) fn max_speed_along_road(
200        dr: DirectedRoadID,
201        max_speed_on_flat_ground: Option<Speed>,
202        constraints: PathConstraints,
203        map: &Map,
204    ) -> (Speed, f64) {
205        let road = map.get_r(dr.road);
206        let percent_incline = if dr.dir == Direction::Fwd {
207            road.percent_incline
208        } else {
209            -road.percent_incline
210        };
211
212        let base = if constraints == PathConstraints::Bike {
213            // We assume every bike has a max_speed defined.
214            bike_speed_on_incline(max_speed_on_flat_ground.unwrap(), percent_incline)
215        } else if constraints == PathConstraints::Pedestrian {
216            // We assume every pedestrian has a max_speed defined.
217            walking_speed_on_incline(max_speed_on_flat_ground.unwrap(), percent_incline)
218        } else {
219            debug_assert!(max_speed_on_flat_ground.is_none());
220            // Incline doesn't affect cars, buses, or trains
221            road.speed_limit
222        };
223
224        let speed = if let Some(s) = max_speed_on_flat_ground {
225            base.min(s)
226        } else {
227            base
228        };
229        (speed, percent_incline)
230    }
231
232    /// The single definitive place to determine how fast somebody could go along a single
233    /// movement. This should be used for pathfinding and simulation. Ignores elevation.
234    pub(crate) fn max_speed_along_movement(
235        mvmnt: MovementID,
236        max_speed_on_flat_ground: Option<Speed>,
237        _: PathConstraints,
238        map: &Map,
239    ) -> Speed {
240        // TODO Ignore elevation on turns?
241        let base = map
242            .get_r(mvmnt.from.road)
243            .speed_limit
244            .min(map.get_r(mvmnt.to.road).speed_limit);
245        if let Some(s) = max_speed_on_flat_ground {
246            base.min(s)
247        } else {
248            base
249        }
250    }
251}
252
253// 10 mph
254pub const MAX_BIKE_SPEED: Speed = Speed::const_meters_per_second(4.4704);
255// 3 mph
256pub const MAX_WALKING_SPEED: Speed = Speed::const_meters_per_second(1.34112);
257
258fn bike_speed_on_incline(max_speed: Speed, percent_incline: f64) -> Speed {
259    // There doesn't seem to be a straightforward way of calculating how an "average" cyclist's
260    // speed is affected by hills. http://www.kreuzotter.de/english/espeed.htm has lots of detail,
261    // but we'd need to guess values like body size, type of bike, etc.
262    // https://github.com/ibi-group/OpenTripPlanner/blob/65dcf0a4142e31028cf9d1b2c15ad32dd1084252/src/main/java/org/opentripplanner/routing/edgetype/StreetEdge.java#L934-L1082
263    // is built from this, but seems to be more appropriate for motorized micromobility devices
264    // like e-scooters.
265
266    // So, we'll adapt the table from Valhalla --
267    // https://valhalla.readthedocs.io/en/latest/sif/elevation_costing/ describes how this works.
268    // Their "weighted grade" should be roughly equivalent to how the elevation_lookups package we
269    // use calculates things.  This table comes from
270    // https://github.com/valhalla/valhalla/blob/f899a940ccbd0bc986769197dec5bb9383014afb/src/sif/bicyclecost.cc#L139.
271    // Valhalla is MIT licensed: https://github.com/valhalla/valhalla/blob/master/COPYING.
272
273    // TODO Could binary search or do something a bit faster here, but doesn't matter much
274    let pct = percent_incline * 100.0;
275    for (grade, factor) in vec![
276        (-10.0, 2.2),
277        (-8.0, 2.0),
278        (-6.5, 1.9),
279        (-5.0, 1.7),
280        (-3.0, 1.4),
281        (-1.5, 1.2),
282        (0.0, 1.0),
283        (1.5, 0.95),
284        (3.0, 0.85),
285        (5.0, 0.75),
286        (6.5, 0.65),
287        (8.0, 0.55),
288        (10.0, 0.5),
289        (11.5, 0.45),
290        (13.0, 0.4),
291    ] {
292        if pct <= grade {
293            return factor * max_speed;
294        }
295    }
296    // The last pair is a factor of 0.3 for grades of 15%, but we'll use it for anything steeper
297    // than 15%
298    0.3 * max_speed
299}
300
301fn walking_speed_on_incline(max_speed: Speed, percent_incline: f64) -> Speed {
302    // https://en.wikipedia.org/wiki/Tobler%27s_hiking_function
303    let exp = -3.5 * (percent_incline + 0.05).abs();
304    let tobler = Speed::km_per_hour(6.0 * std::f64::consts::E.powf(exp));
305    // Tobler's hiking function assumes a flat speed of 5km/hr, but that's different than ours.
306    // Just scale our max_speed proportionally.
307    let result = (tobler / Speed::km_per_hour(5.0)) * max_speed;
308    // Data quality issues, y'know...
309    if result == Speed::ZERO {
310        error!(
311            "walking_speed_on_incline saw an incline of {}. Not going to reduce the speed to 0!",
312            percent_incline
313        );
314        return 0.1 * max_speed;
315    }
316    result
317}
318
319#[cfg(test)]
320mod tests {
321    use super::*;
322
323    #[test]
324    fn test_bike_speed_on_incline() {
325        let base_speed = MAX_BIKE_SPEED;
326        assert_approx_eq(
327            Speed::miles_per_hour(10.0),
328            bike_speed_on_incline(base_speed, 0.0),
329        );
330        assert_approx_eq(
331            Speed::miles_per_hour(22.0),
332            bike_speed_on_incline(base_speed, -0.15),
333        );
334        assert_approx_eq(
335            Speed::miles_per_hour(3.0),
336            bike_speed_on_incline(base_speed, 0.15),
337        );
338    }
339
340    #[test]
341    fn test_walking_speed_on_incline() {
342        let base_speed = MAX_WALKING_SPEED;
343        assert_approx_eq(
344            Speed::miles_per_hour(3.0),
345            walking_speed_on_incline(base_speed, 0.0),
346        );
347        assert_approx_eq(
348            Speed::miles_per_hour(2.54),
349            walking_speed_on_incline(base_speed, -0.15),
350        );
351        assert_approx_eq(
352            Speed::miles_per_hour(1.79),
353            walking_speed_on_incline(base_speed, 0.15),
354        );
355    }
356
357    fn assert_approx_eq(s1: Speed, s2: Speed) {
358        if (s1.inner_meters_per_second() - s2.inner_meters_per_second()).abs() > 0.1 {
359            // Print in mph without rounding
360            panic!(
361                "{} != {}",
362                2.23694 * s1.inner_meters_per_second(),
363                2.23694 * s2.inner_meters_per_second()
364            );
365        }
366    }
367}