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#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
11pub struct Position {
12 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 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 dist += object_length;
93 dist = dist.max(Distance::ZERO).min(other_lane.length());
94 }
95 return Position::new(other_lane.id, dist);
96 }
97 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#[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 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 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 bike_speed_on_incline(max_speed_on_flat_ground.unwrap(), percent_incline)
215 } else if constraints == PathConstraints::Pedestrian {
216 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 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 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 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
253pub const MAX_BIKE_SPEED: Speed = Speed::const_meters_per_second(4.4704);
255pub 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 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 0.3 * max_speed
299}
300
301fn walking_speed_on_incline(max_speed: Speed, percent_incline: f64) -> Speed {
302 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 let result = (tobler / Speed::km_per_hour(5.0)) * max_speed;
308 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 panic!(
361 "{} != {}",
362 2.23694 * s1.inner_meters_per_second(),
363 2.23694 * s2.inner_meters_per_second()
364 );
365 }
366 }
367}