1use crate::polyline::{polyline_encode, FieldEncodeOptions};
2use crate::surface::{RoadClassId, SurfaceMapping, SurfaceTypeId};
3use crate::{Column, Section};
4use itertools::Itertools;
5use std::collections::{BTreeMap, HashSet};
6
7fn haversine_distance(prev: &Point, x: f64, y: f64) -> f64 {
8 const MEAN_EARTH_RADIUS: f64 = 6_371_000.0;
10
11 let theta1 = prev.y.to_radians();
12 let theta2 = y.to_radians();
13 let delta_theta = (y - prev.y).to_radians();
14 let delta_lambda = (x - prev.x).to_radians();
15 let a = (delta_theta / 2.0).sin().powi(2) + theta1.cos() * theta2.cos() * (delta_lambda / 2.0).sin().powi(2);
16 let c = 2.0 * a.sqrt().asin();
17 MEAN_EARTH_RADIUS * c
18}
19
20trait FarthestPoint {
21 fn farthest_point(&self) -> (usize, f64);
22}
23
24impl FarthestPoint for &[Point] {
25 fn farthest_point(&self) -> (usize, f64) {
26 let line = Line::new(self.first().unwrap(), self.last().unwrap());
27
28 self.iter()
29 .enumerate()
30 .take(self.len() - 1) .skip(1) .map(|(index, point)| (index, line.distance_2d(&point)))
33 .fold(
34 (0, 0.0),
35 |(farthest_index, farthest_dist), (index, distance)| {
36 if distance > farthest_dist {
37 (index, distance)
38 } else {
39 (farthest_index, farthest_dist)
40 }
41 },
42 )
43 }
44}
45
46#[derive(Clone, Debug, PartialEq)]
47pub(crate) struct Point {
48 pub(crate) index: usize,
49 pub(crate) x: f64,
50 pub(crate) y: f64,
51 pub(crate) d: f64,
52 pub(crate) e: f64,
53 pub(crate) s: Option<SurfaceTypeId>,
54 pub(crate) r: Option<RoadClassId>,
55}
56
57#[cfg(test)]
58impl Default for Point {
59 fn default() -> Self {
60 Self {
61 index: 0,
62 x: 0.0,
63 y: 0.0,
64 d: 0.0,
65 e: 0.0,
66 s: None,
67 r: None,
68 }
69 }
70}
71
72struct Line<'a> {
73 start: &'a Point,
74 end: &'a Point,
75}
76
77impl<'a> Line<'a> {
78 fn new(start: &'a Point, end: &'a Point) -> Self {
79 Self { start, end }
80 }
81
82 fn distance_3d(&self, point: &Point) -> f64 {
83 let mut x = self.start.x;
84 let mut y = self.start.y;
85 let mut e = self.start.e;
86
87 let mut dx = self.end.x - x;
88 let mut dy = self.end.y - y;
89 let mut de = self.end.e - e;
90
91 if dx != 0.0 || dy != 0.0 || de != 0.0 {
92 let t = ((point.x - x) * dx + (point.y - y) * dy + (point.e - e) * de)
93 / (dx * dx + dy * dy + de * de);
94
95 if t > 1.0 {
96 x = self.end.x;
97 y = self.end.y;
98 e = self.end.e;
99 } else if t > 0.0 {
100 x += dx * t;
101 y += dy * t;
102 e += de * t;
103 }
104 }
105
106 dx = point.x - x;
107 dy = point.y - y;
108 de = point.e - e;
109
110 return dx * dx + dy * dy + de * de;
111 }
112
113 fn distance_2d(&self, point: &Point) -> f64 {
114 let mut x = self.start.x;
115 let mut y = self.start.y;
116
117 let mut dx = self.end.x - x;
118 let mut dy = self.end.y - y;
119
120 if dx != 0.0 || dy != 0.0 {
121 let t = ((point.x - x) * dx + (point.y - y) * dy) / (dx * dx + dy * dy);
122
123 if t > 1.0 {
124 x = self.end.x;
125 y = self.end.y;
126 } else if t > 0.0 {
127 x += dx * t;
128 y += dy * t;
129 }
130 }
131
132 dx = point.x - x;
133 dy = point.y - y;
134
135 return dx * dx + dy * dy;
136 }
137}
138
139struct SurfaceGroupIter<'a, 'b> {
140 points: &'a [Point],
141 mapping: &'b SurfaceMapping,
142 group: Option<&'b String>,
143}
144
145impl<'a, 'b> SurfaceGroupIter<'a, 'b> {
146 fn new(points: &'a [Point], mapping: &'b SurfaceMapping) -> Self {
147 Self {
148 points,
149 mapping,
150 group: points
151 .first()
152 .and_then(|point| mapping.get_surface_group(point)),
153 }
154 }
155}
156
157impl<'a, 'b> Iterator for SurfaceGroupIter<'a, 'b> {
158 type Item = &'a [Point];
159
160 fn next(&mut self) -> Option<Self::Item> {
161 let mut partition_len = 0;
162 for point in self.points.iter() {
163 let group = self.mapping.get_surface_group(point);
164
165 if self.group == group {
166 partition_len += 1;
167 } else {
168 self.group = group;
169 break;
170 }
171 }
172
173 if partition_len > 0 {
174 let (partition, new_points) = self.points.split_at(partition_len);
175 self.points = new_points;
176 Some(partition)
177 } else {
178 None
179 }
180 }
181}
182
183fn simplify_points(points: &[Point], mapping: &SurfaceMapping, tolerance: f64) -> HashSet<usize> {
184 fn stack_rdp(points: &[Point], tolerance_sq: f64) -> HashSet<usize> {
185 let mut anchors = HashSet::new();
186 let mut stack = Vec::new();
187 stack.push(points);
188
189 while let Some(slice) = stack.pop() {
190 let (farthest_index, farthest_dist) = slice.farthest_point();
191
192 if farthest_dist > tolerance_sq {
193 stack.push(&slice[..=farthest_index]);
194 stack.push(&slice[farthest_index..]);
195 } else {
196 anchors.insert(slice.first().unwrap().index);
197 anchors.insert(slice.last().unwrap().index);
198 }
199 }
200
201 anchors
202 }
203
204 let tolerance_sq = tolerance * tolerance;
205 SurfaceGroupIter::new(points, mapping)
206 .map(|points| stack_rdp(points, tolerance_sq))
207 .flatten()
208 .collect()
209}
210
211fn section_to_points(section: &Section) -> Vec<Point> {
212 let empty_longfloat_btree = BTreeMap::new();
213 let empty_numbers_btree = BTreeMap::new();
214 let empty_base64_btree = BTreeMap::new();
215
216 let columns = section.columns();
217 let x_map = if let Some(x_column) = columns.get("x") {
218 match x_column {
219 Column::LongFloat(x) => x,
220 _ => panic!("unexpected x column type"),
221 }
222 } else {
223 &empty_longfloat_btree
224 };
225
226 let y_map = if let Some(y_column) = columns.get("y") {
227 match y_column {
228 Column::LongFloat(y) => y,
229 _ => panic!("unexpected y column type"),
230 }
231 } else {
232 &empty_longfloat_btree
233 };
234
235 let e_map = if let Some(e_column) = columns.get("e") {
236 match e_column {
237 Column::LongFloat(e) => e,
238 _ => panic!("unexpected e column type"),
239 }
240 } else {
241 &empty_longfloat_btree
242 };
243
244 let s_map = if let Some(s_column) = columns.get("S") {
245 match s_column {
246 Column::Numbers(s) => s,
247 _ => panic!("unexpected S column type"),
248 }
249 } else {
250 &empty_numbers_btree
251 };
252
253 let r_map = if let Some(r_column) = columns.get("R") {
254 match r_column {
255 Column::Numbers(r) => r,
256 _ => panic!("unexpected R column type"),
257 }
258 } else {
259 &empty_numbers_btree
260 };
261
262 let ep_map = if let Some(ep_column) = columns.get("ep") {
263 match ep_column {
264 Column::Base64(ep) => ep,
265 _ => panic!("unexpected ep column type"),
266 }
267 } else {
268 &empty_base64_btree
269 };
270
271 let all_keys = x_map.keys().chain(y_map.keys());
272
273 let mut points: Vec<Point> = Vec::with_capacity(x_map.len());
274
275 let mut point_index = 0;
276 for index in all_keys.sorted().dedup() {
277 let x = x_map.get(index);
278 let y = y_map.get(index);
279 let ep = ep_map.get(index);
280 let e = e_map.get(index);
281 let s = s_map.get(index);
282 let r = r_map.get(index);
283
284 if let (Some(x), Some(y), Some(e), None) = (x, y, e, ep) {
285 let d = if let Some(prev) = points.last() {
286 prev.d + haversine_distance(prev, *x, *y)
287 } else {
288 0.0
289 };
290
291 points.push(Point {
292 index: point_index,
293 x: *x,
294 y: *y,
295 d,
296 e: *e,
297 s: s.cloned(),
298 r: r.cloned(),
299 });
300 point_index += 1;
301 }
302 }
303
304 points
305}
306
307pub(crate) fn simplify_and_encode(
308 section: &Section,
309 mapping: &SurfaceMapping,
310 tolerance: f64,
311 fields: &[FieldEncodeOptions],
312) -> String {
313 let points = section_to_points(section);
314 let simplified_indexes = simplify_points(&points, mapping, tolerance);
315 let simplified_points = simplified_indexes
316 .into_iter()
317 .sorted()
318 .map(|index| points[index].clone())
319 .collect::<Vec<_>>();
320
321 polyline_encode(&simplified_points, fields)
322}
323
324#[cfg(test)]
325mod tests {
326 use assert_matches::assert_matches;
327 use std::iter::FromIterator;
328 use crate::{Section, SectionType};
329
330 use super::*;
331
332 #[test]
333 fn test_surface_group_iterator_all_points_missing_surface_info() {
334 let mut mapping = SurfaceMapping::new(99);
335 mapping.add_surface(0, "0".to_string());
336 mapping.add_surface(1, "1".to_string());
337 mapping.add_surface(2, "2".to_string());
338
339 let points = vec![
340 Point {
341 s: None,
342 r: None,
343 ..Default::default()
344 },
345 Point {
346 s: None,
347 r: None,
348 ..Default::default()
349 },
350 Point {
351 s: None,
352 r: None,
353 ..Default::default()
354 },
355 ];
356
357 let groups = SurfaceGroupIter::new(&points, &mapping).collect::<Vec<_>>();
358
359 assert_eq!(groups, vec![points.as_slice()]);
360 }
361
362 #[test]
363 fn test_surface_group_iterator_all_points_different_surface() {
364 let mut mapping = SurfaceMapping::new(99);
365 mapping.add_surface(0, "0".to_string());
366 mapping.add_surface(1, "1".to_string());
367 mapping.add_surface(2, "2".to_string());
368
369 let points = vec![
370 Point {
371 s: Some(1),
372 r: None,
373 ..Default::default()
374 },
375 Point {
376 s: Some(2),
377 r: None,
378 ..Default::default()
379 },
380 Point {
381 s: Some(3),
382 r: None,
383 ..Default::default()
384 },
385 ];
386
387 let groups = SurfaceGroupIter::new(&points, &mapping).collect::<Vec<_>>();
388
389 assert_eq!(
390 groups,
391 vec![
392 &[points[0].clone()][..],
393 &[points[1].clone()][..],
394 &[points[2].clone()][..]
395 ]
396 );
397 }
398
399 #[test]
400 fn test_surface_group_iterator_normal_track() {
401 let mut mapping = SurfaceMapping::new(99);
402 mapping.add_surface(0, "0".to_string());
403 mapping.add_surface(1, "1".to_string());
404 mapping.add_surface(2, "2".to_string());
405
406 let points = vec![
407 Point {
408 s: None,
409 r: None,
410 ..Default::default()
411 },
412 Point {
413 s: Some(1),
414 r: None,
415 ..Default::default()
416 },
417 Point {
418 s: Some(1),
419 r: None,
420 ..Default::default()
421 },
422 Point {
423 s: Some(1),
424 r: None,
425 ..Default::default()
426 },
427 Point {
428 s: Some(2),
429 r: None,
430 ..Default::default()
431 },
432 Point {
433 s: Some(2),
434 r: None,
435 ..Default::default()
436 },
437 Point {
438 s: None,
439 r: None,
440 ..Default::default()
441 },
442 ];
443
444 let groups = SurfaceGroupIter::new(&points, &mapping).collect::<Vec<_>>();
445
446 assert_eq!(
447 groups,
448 vec![
449 &[points[0].clone()][..],
450 &[points[1].clone(), points[2].clone(), points[3].clone()][..],
451 &[points[4].clone(), points[5].clone()][..],
452 &[points[6].clone()][..]
453 ]
454 );
455 }
456
457 #[test]
458 fn test_simplifying_zero_points() {
459 let mapping = SurfaceMapping::new(0);
460 assert_eq!(simplify_points(&[], &mapping, 0.0), HashSet::new());
461 }
462
463 #[test]
464 fn test_simplifying_one_point() {
465 let mapping = SurfaceMapping::new(0);
466 assert_eq!(
467 simplify_points(&[Point::default()], &mapping, 0.0),
468 HashSet::from_iter([0])
469 );
470 }
471
472 #[test]
473 fn test_simplifying_two_points() {
474 let mapping = SurfaceMapping::new(0);
475 assert_eq!(
476 simplify_points(
477 &[
478 Point::default(),
479 Point {
480 index: 1,
481 x: 1.0,
482 ..Default::default()
483 }
484 ],
485 &mapping,
486 0.0
487 ),
488 HashSet::from_iter([0, 1])
489 );
490 }
491
492 #[test]
493 fn test_simplifying_three_points() {
494 let mapping = SurfaceMapping::new(0);
495 assert_eq!(
496 simplify_points(
497 &[
498 Point::default(),
499 Point {
500 index: 1,
501 x: 1.0,
502 ..Default::default()
503 },
504 Point {
505 index: 2,
506 x: 2.0,
507 y: 2.0,
508 ..Default::default()
509 }
510 ],
511 &mapping,
512 0.0
513 ),
514 HashSet::from_iter([0, 1, 2])
515 );
516 }
517
518 #[test]
519 fn test_section_to_points_compute_distance() {
520 let mut s = Section::new(SectionType::TrackPoints);
521 assert!(s.add_long_float(0, "x", -122.63074546051908).is_ok());
522 assert!(s.add_long_float(0, "y", 45.503551007119015).is_ok());
523 assert!(s.add_long_float(0, "e", 1.0).is_ok());
524
525 assert!(s.add_long_float(1, "x", -122.62891022329185).is_ok());
526 assert!(s.add_long_float(1, "y", 45.50346836958737).is_ok());
527 assert!(s.add_long_float(1, "e", 2.0).is_ok());
528
529 assert!(s.add_long_float(2, "x", -122.62740666028297).is_ok());
530 assert!(s.add_long_float(2, "y", 45.503370210451294).is_ok());
531 assert!(s.add_long_float(2, "e", 3.0).is_ok());
532 assert!(s.add_short_float(2, "d", 3.0).is_ok());
533
534 assert!(s.add_long_float(3, "x", -122.62586624166765).is_ok());
535 assert!(s.add_long_float(3, "y", 45.503370178115716).is_ok());
536 assert!(s.add_long_float(3, "e", 4.0).is_ok());
537
538 let points = section_to_points(&s);
539
540 assert_eq!(points.len(), 4);
541 assert_matches!(points[0], Point{x, y, d, ..} => {
542 assert_eq!(x, -122.63074546051908);
543 assert_eq!(y, 45.503551007119015);
544 assert_eq!(d, 0.0);
545 });
546 assert_matches!(points[1], Point{x, y, d, ..} => {
547 assert_eq!(x, -122.62891022329185);
548 assert_eq!(y, 45.50346836958737);
549 assert!((143.0 - d).abs() < 1.0);
550 });
551 assert_matches!(points[2], Point{x, y, d, ..} => {
552 assert_eq!(x, -122.62740666028297);
553 assert_eq!(y, 45.503370210451294);
554 assert!((261.0 - d).abs() < 1.0);
555 });
556 assert_matches!(points[3], Point{x, y, d, ..} => {
557 assert_eq!(x, -122.62586624166765);
558 assert_eq!(y, 45.503370178115716);
559 assert!((381.0 - d).abs() < 1.0);
560 });
561 }
562}