From 316edcb36908d3b2dfe9dea26de94ca48e034be3 Mon Sep 17 00:00:00 2001 From: frederic wagner Date: Sun, 6 Nov 2022 05:04:59 +0100 Subject: [PATCH] getting ready for wasm --- apps/gipy/gpconv/.gitignore | 3 + apps/gipy/gpconv/Cargo.lock | 70 +++ apps/gipy/gpconv/Cargo.toml | 8 +- apps/gipy/gpconv/src/interests.rs | 78 ++++ apps/gipy/gpconv/src/lib.rs | 575 ++++++++++++++++++++++++ apps/gipy/gpconv/src/main.rs | 713 +----------------------------- apps/gipy/gpconv/src/osm.rs | 82 +--- apps/gipy/gpconv/src/svg.rs | 86 ++++ 8 files changed, 835 insertions(+), 780 deletions(-) create mode 100644 apps/gipy/gpconv/src/interests.rs create mode 100644 apps/gipy/gpconv/src/lib.rs create mode 100644 apps/gipy/gpconv/src/svg.rs diff --git a/apps/gipy/gpconv/.gitignore b/apps/gipy/gpconv/.gitignore index ea8c4bf7f..a4f078e1e 100644 --- a/apps/gipy/gpconv/.gitignore +++ b/apps/gipy/gpconv/.gitignore @@ -1 +1,4 @@ /target +*.gpc +*.pbf +*.svg diff --git a/apps/gipy/gpconv/Cargo.lock b/apps/gipy/gpconv/Cargo.lock index c280558cd..6986021d7 100644 --- a/apps/gipy/gpconv/Cargo.lock +++ b/apps/gipy/gpconv/Cargo.lock @@ -67,6 +67,12 @@ version = "1.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" +[[package]] +name = "bumpalo" +version = "3.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "572f695136211188308f16ad2ca5c851a712c464060ae6974944458eb83880ba" + [[package]] name = "byteorder" version = "1.4.3" @@ -272,6 +278,7 @@ dependencies = [ "itertools", "lazy_static", "osmio", + "wasm-bindgen", ] [[package]] @@ -355,6 +362,15 @@ dependencies = [ "vcpkg", ] +[[package]] +name = "log" +version = "0.4.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" +dependencies = [ + "cfg-if", +] + [[package]] name = "memchr" version = "2.5.0" @@ -633,6 +649,60 @@ version = "0.11.0+wasi-snapshot-preview1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" +[[package]] +name = "wasm-bindgen" +version = "0.2.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eaf9f5aceeec8be17c128b2e93e031fb8a4d469bb9c4ae2d7dc1888b26887268" +dependencies = [ + "cfg-if", + "wasm-bindgen-macro", +] + +[[package]] +name = "wasm-bindgen-backend" +version = "0.2.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4c8ffb332579b0557b52d268b91feab8df3615f265d5270fec2a8c95b17c1142" +dependencies = [ + "bumpalo", + "log", + "once_cell", + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "052be0f94026e6cbc75cdefc9bae13fd6052cdcaf532fa6c45e7ae33a1e6c810" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "07bc0c051dc5f23e307b13285f9d75df86bfdf816c5721e573dec1f9b8aa193c" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-backend", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1c38c045535d93ec4f0b4defec448e4291638ee608530863b1e2ba115d4fff7f" + [[package]] name = "winapi" version = "0.3.9" diff --git a/apps/gipy/gpconv/Cargo.toml b/apps/gipy/gpconv/Cargo.toml index 735eaa6ed..afcbec440 100644 --- a/apps/gipy/gpconv/Cargo.toml +++ b/apps/gipy/gpconv/Cargo.toml @@ -4,9 +4,15 @@ version = "0.1.0" edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +[features] +osm = ["dep:osmio"] + +# [lib] +# crate-type = ["cdylib"] [dependencies] gpx="*" itertools="*" lazy_static="*" -osmio="*" +osmio={version="*", optional=true} +wasm-bindgen="*" \ No newline at end of file diff --git a/apps/gipy/gpconv/src/interests.rs b/apps/gipy/gpconv/src/interests.rs new file mode 100644 index 000000000..6d078e75d --- /dev/null +++ b/apps/gipy/gpconv/src/interests.rs @@ -0,0 +1,78 @@ +use super::Point; +use lazy_static::lazy_static; +use std::collections::HashMap; + +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub enum Interest { + Bakery, + DrinkingWater, + Toilets, + // BikeShop, + // ChargingStation, + // Bank, + // Supermarket, + // Table, + // TourismOffice, + Artwork, + // Pharmacy, +} + +impl Into for Interest { + fn into(self) -> u8 { + match self { + Interest::Bakery => 0, + Interest::DrinkingWater => 1, + Interest::Toilets => 2, + // Interest::BikeShop => 8, + // Interest::ChargingStation => 4, + // Interest::Bank => 5, + // Interest::Supermarket => 6, + // Interest::Table => 7, + Interest::Artwork => 3, + // Interest::Pharmacy => 9, + } + } +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub struct InterestPoint { + pub point: Point, + pub interest: Interest, +} + +lazy_static! { + static ref INTERESTS: HashMap<(&'static str, &'static str), Interest> = { + [ + (("shop", "bakery"), Interest::Bakery), + (("amenity", "drinking_water"), Interest::DrinkingWater), + (("amenity", "toilets"), Interest::Toilets), + // (("shop", "bicycle"), Interest::BikeShop), + // (("amenity", "charging_station"), Interest::ChargingStation), + // (("amenity", "bank"), Interest::Bank), + // (("shop", "supermarket"), Interest::Supermarket), + // (("leisure", "picnic_table"), Interest::Table), + // (("tourism", "information"), Interest::TourismOffice), + (("tourism", "artwork"), Interest::Artwork), + // (("amenity", "pharmacy"), Interest::Pharmacy), + ] + .into_iter() + .collect() + }; +} + +impl InterestPoint { + pub fn color(&self) -> &'static str { + match self.interest { + Interest::Bakery => "red", + Interest::DrinkingWater => "blue", + Interest::Toilets => "brown", + // Interest::BikeShop => "purple", + // Interest::ChargingStation => "green", + // Interest::Bank => "black", + // Interest::Supermarket => "red", + // Interest::Table => "pink", + Interest::Artwork => "orange", + // Interest::Pharmacy => "chartreuse", + } + } +} diff --git a/apps/gipy/gpconv/src/lib.rs b/apps/gipy/gpconv/src/lib.rs new file mode 100644 index 000000000..a58922c17 --- /dev/null +++ b/apps/gipy/gpconv/src/lib.rs @@ -0,0 +1,575 @@ +use itertools::Itertools; +use std::collections::{HashMap, HashSet}; +use std::fs::File; +use std::io::{BufReader, BufWriter, Read, Write}; +use std::path::Path; +use wasm_bindgen::prelude::*; + +use gpx::read; +use gpx::Gpx; + +mod interests; +use interests::InterestPoint; + +mod svg; + +#[cfg(feature = "osm")] +mod osm; +#[cfg(feature = "osm")] +use osm::{parse_osm_data, InterestPoint}; + +const LOWER_SHARP_TURN: f64 = 80.0 * std::f64::consts::PI / 180.0; +const UPPER_SHARP_TURN: f64 = std::f64::consts::PI * 2.0 - LOWER_SHARP_TURN; + +const KEY: u16 = 47490; +const FILE_VERSION: u16 = 3; + +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Point { + x: f64, + y: f64, +} + +impl Eq for Point {} +impl std::hash::Hash for Point { + fn hash(&self, state: &mut H) { + unsafe { std::mem::transmute::(self.x) }.hash(state); + unsafe { std::mem::transmute::(self.y) }.hash(state); + } +} + +impl Point { + fn squared_distance_between(&self, other: &Point) -> f64 { + let dx = other.x - self.x; + let dy = other.y - self.y; + dx * dx + dy * dy + } + fn distance_to_segment(&self, v: &Point, w: &Point) -> f64 { + let l2 = v.squared_distance_between(w); + if l2 == 0.0 { + return self.squared_distance_between(v).sqrt(); + } + // Consider the line extending the segment, parameterized as v + t (w - v). + // We find projection of point p onto the line. + // It falls where t = [(p-v) . (w-v)] / |w-v|^2 + // We clamp t from [0,1] to handle points outside the segment vw. + let x0 = self.x - v.x; + let y0 = self.y - v.y; + let x1 = w.x - v.x; + let y1 = w.y - v.y; + let dot = x0 * x1 + y0 * y1; + let t = (dot / l2).min(1.0).max(0.0); + + let proj = Point { + x: v.x + x1 * t, + y: v.y + y1 * t, + }; + + proj.squared_distance_between(self).sqrt() + } +} + +fn points(reader: R) -> (HashSet, Vec) { + // read takes any io::Read and gives a Result. + let mut gpx: Gpx = read(reader).unwrap(); + eprintln!("we have {} tracks", gpx.tracks.len()); + + let mut waypoints = HashSet::new(); + + let points = gpx + .tracks + .pop() + .unwrap() + .segments + .into_iter() + .flat_map(|segment| segment.points.into_iter()) + .map(|p| { + let is_commented = p.comment.is_some(); + let (x, y) = p.point().x_y(); + let p = Point { x, y }; + if is_commented { + waypoints.insert(p); + } + p + }) + .collect::>(); + (waypoints, points) +} + +// // NOTE: this angles idea could maybe be use to get dp from n^3 to n^2 +// fn acceptable_angles(p1: &(f64, f64), p2: &(f64, f64), epsilon: f64) -> (f64, f64) { +// // first, convert p2's coordinates for p1 as origin +// let (x1, y1) = *p1; +// let (x2, y2) = *p2; +// let (x, y) = (x2 - x1, y2 - y1); +// // rotate so that (p1, p2) ends on x axis +// let theta = y.atan2(x); +// let rx = x * theta.cos() - y * theta.sin(); +// let ry = x * theta.sin() + y * theta.cos(); +// assert!(ry.abs() <= std::f64::EPSILON); +// +// // now imagine a line at an angle alpha. +// // we want the distance d from (rx, 0) to our line +// // we have sin(alpha) = d / rx +// // limiting d to epsilon, we solve +// // sin(alpha) = e / rx +// // and get +// // alpha = arcsin(e/rx) +// let alpha = (epsilon / rx).asin(); +// +// // now we just need to rotate back +// let a1 = theta + alpha.abs(); +// let a2 = theta - alpha.abs(); +// assert!(a1 >= a2); +// (a1, a2) +// } +// +// // this is like ramer douglas peucker algorithm +// // except that we advance from the start without knowing the end. +// // each point we meet constrains the chosen segment's angle +// // a bit more. +// // +// fn simplify(mut points: &[(f64, f64)]) -> Vec<(f64, f64)> { +// let mut remaining_points = Vec::new(); +// while !points.is_empty() { +// let (sx, sy) = points.first().unwrap(); +// let i = match points +// .iter() +// .enumerate() +// .map(|(i, (x, y))| todo!("compute angles")) +// .try_fold( +// (0.0f64, std::f64::consts::FRAC_2_PI), +// |(amin, amax), (i, (amin2, amax2))| -> Result<(f64, f64), usize> { +// let new_amax = amax.min(amax2); +// let new_amin = amin.max(amin2); +// if new_amin >= new_amax { +// Err(i) +// } else { +// Ok((new_amin, new_amax)) +// } +// }, +// ) { +// Err(i) => i, +// Ok(_) => points.len(), +// }; +// remaining_points.push(points.first().cloned().unwrap()); +// points = &points[i..]; +// } +// remaining_points +// } + +fn extract_prog_dyn_solution( + points: &[Point], + start: usize, + end: usize, + cache: &HashMap<(usize, usize), (Option, usize)>, +) -> Vec { + if let Some(choice) = cache.get(&(start, end)).unwrap().0 { + let mut v1 = extract_prog_dyn_solution(points, start, choice + 1, cache); + let mut v2 = extract_prog_dyn_solution(points, choice, end, cache); + v1.pop(); + v1.append(&mut v2); + v1 + } else { + vec![points[start], points[end - 1]] + } +} + +fn simplify_prog_dyn( + points: &[Point], + start: usize, + end: usize, + epsilon: f64, + cache: &mut HashMap<(usize, usize), (Option, usize)>, +) -> usize { + if let Some(val) = cache.get(&(start, end)) { + val.1 + } else { + let res = if end - start <= 2 { + assert_eq!(end - start, 2); + (None, end - start) + } else { + let first_point = &points[start]; + let last_point = &points[end - 1]; + + if points[(start + 1)..end] + .iter() + .map(|p| p.distance_to_segment(first_point, last_point)) + .all(|d| d <= epsilon) + { + (None, 2) + } else { + // now we test all possible cutting points + ((start + 1)..(end - 1)) //TODO: take middle min + .map(|i| { + let v1 = simplify_prog_dyn(points, start, i + 1, epsilon, cache); + let v2 = simplify_prog_dyn(points, i, end, epsilon, cache); + (Some(i), v1 + v2 - 1) + }) + .min_by_key(|(_, v)| *v) + .unwrap() + } + }; + cache.insert((start, end), res); + res.1 + } +} + +fn rdp(points: &[Point], epsilon: f64) -> Vec { + if points.len() <= 2 { + points.iter().copied().collect() + } else { + if points.first().unwrap() == points.last().unwrap() { + let first = points.first().unwrap(); + let index_farthest = points + .iter() + .enumerate() + .skip(1) + .max_by(|(_, p1), (_, p2)| { + first + .squared_distance_between(p1) + .partial_cmp(&first.squared_distance_between(p2)) + .unwrap() + }) + .map(|(i, _)| i) + .unwrap(); + + let start = &points[..(index_farthest + 1)]; + let end = &points[index_farthest..]; + let mut res = rdp(start, epsilon); + res.pop(); + res.append(&mut rdp(end, epsilon)); + res + } else { + let (index_farthest, farthest_distance) = points + .iter() + .map(|p| p.distance_to_segment(points.first().unwrap(), points.last().unwrap())) + .enumerate() + .max_by(|(_, d1), (_, d2)| { + if d1.is_nan() { + std::cmp::Ordering::Greater + } else { + if d2.is_nan() { + std::cmp::Ordering::Less + } else { + d1.partial_cmp(d2).unwrap() + } + } + }) + .unwrap(); + if farthest_distance <= epsilon { + vec![ + points.first().copied().unwrap(), + points.last().copied().unwrap(), + ] + } else { + let start = &points[..(index_farthest + 1)]; + let end = &points[index_farthest..]; + let mut res = rdp(start, epsilon); + res.pop(); + res.append(&mut rdp(end, epsilon)); + res + } + } + } +} + +fn simplify_path(points: &[Point], epsilon: f64) -> Vec { + if points.len() <= 600 { + optimal_simplification(points, epsilon) + } else { + hybrid_simplification(points, epsilon) + } +} + +fn save_gpc( + mut writer: W, + points: &[Point], + waypoints: &HashSet, + buckets: &[Bucket], +) -> std::io::Result<()> { + eprintln!("saving {} points", points.len()); + + let mut unique_interest_points = Vec::new(); + let mut correspondance = HashMap::new(); + let interests_on_path = buckets + .iter() + .flat_map(|b| &b.points) + .map(|p| match correspondance.entry(*p) { + std::collections::hash_map::Entry::Occupied(o) => *o.get(), + std::collections::hash_map::Entry::Vacant(v) => { + let index = unique_interest_points.len(); + unique_interest_points.push(*p); + v.insert(index); + index + } + }) + .collect::>(); + + writer.write_all(&KEY.to_le_bytes())?; + writer.write_all(&FILE_VERSION.to_le_bytes())?; + writer.write_all(&(points.len() as u16).to_le_bytes())?; + writer.write_all(&(unique_interest_points.len() as u16).to_le_bytes())?; + writer.write_all(&(interests_on_path.len() as u16).to_le_bytes())?; + points + .iter() + .flat_map(|p| [p.x, p.y]) + .try_for_each(|c| writer.write_all(&c.to_le_bytes()))?; + + let mut waypoints_bits = std::iter::repeat(0u8) + .take(points.len() / 8 + if points.len() % 8 != 0 { 1 } else { 0 }) + .collect::>(); + points.iter().enumerate().for_each(|(i, p)| { + if waypoints.contains(p) { + waypoints_bits[i / 8] |= 1 << (i % 8) + } + }); + waypoints_bits + .iter() + .try_for_each(|byte| writer.write_all(&byte.to_le_bytes()))?; + + unique_interest_points + .iter() + .flat_map(|p| [p.point.x, p.point.y]) + .try_for_each(|c| writer.write_all(&c.to_le_bytes()))?; + + let counts: HashMap<_, usize> = + unique_interest_points + .iter() + .fold(HashMap::new(), |mut h, p| { + *h.entry(p.interest).or_default() += 1; + h + }); + counts.into_iter().for_each(|(interest, count)| { + eprintln!("{:?} appears {} times", interest, count); + }); + + unique_interest_points + .iter() + .map(|p| p.interest.into()) + .try_for_each(|i: u8| writer.write_all(&i.to_le_bytes()))?; + + interests_on_path + .iter() + .map(|i| *i as u16) + .try_for_each(|i| writer.write_all(&i.to_le_bytes()))?; + + buckets + .iter() + .map(|b| b.start as u16) + .try_for_each(|i| writer.write_all(&i.to_le_bytes()))?; + + Ok(()) +} + +fn optimal_simplification(points: &[Point], epsilon: f64) -> Vec { + let mut cache = HashMap::new(); + simplify_prog_dyn(&points, 0, points.len(), epsilon, &mut cache); + extract_prog_dyn_solution(&points, 0, points.len(), &cache) +} + +fn hybrid_simplification(points: &[Point], epsilon: f64) -> Vec { + if points.len() <= 300 { + optimal_simplification(points, epsilon) + } else { + if points.first().unwrap() == points.last().unwrap() { + let first = points.first().unwrap(); + let index_farthest = points + .iter() + .enumerate() + .skip(1) + .max_by(|(_, p1), (_, p2)| { + first + .squared_distance_between(p1) + .partial_cmp(&first.squared_distance_between(p2)) + .unwrap() + }) + .map(|(i, _)| i) + .unwrap(); + + let start = &points[..(index_farthest + 1)]; + let end = &points[index_farthest..]; + let mut res = hybrid_simplification(start, epsilon); + res.pop(); + res.append(&mut hybrid_simplification(end, epsilon)); + res + } else { + let (index_farthest, farthest_distance) = points + .iter() + .map(|p| p.distance_to_segment(points.first().unwrap(), points.last().unwrap())) + .enumerate() + .max_by(|(_, d1), (_, d2)| { + if d1.is_nan() { + std::cmp::Ordering::Greater + } else { + if d2.is_nan() { + std::cmp::Ordering::Less + } else { + d1.partial_cmp(d2).unwrap() + } + } + }) + .unwrap(); + if farthest_distance <= epsilon { + vec![ + points.first().copied().unwrap(), + points.last().copied().unwrap(), + ] + } else { + let start = &points[..(index_farthest + 1)]; + let end = &points[index_farthest..]; + let mut res = hybrid_simplification(start, epsilon); + res.pop(); + res.append(&mut hybrid_simplification(end, epsilon)); + res + } + } + } +} + +pub struct Bucket { + points: Vec, + start: usize, +} + +fn position_interests_along_path( + interests: &mut [InterestPoint], + path: &[Point], + d: f64, + buckets_size: usize, // final points are indexed in buckets + groups_size: usize, // how many segments are compacted together +) -> Vec { + interests.sort_unstable_by(|p1, p2| p1.point.x.partial_cmp(&p2.point.x).unwrap()); + // first compute for each segment a vec containing its nearby points + let mut positions = Vec::new(); + for segment in path.windows(2) { + let mut local_interests = Vec::new(); + let x0 = segment[0].x; + let x1 = segment[1].x; + let (xmin, xmax) = if x0 <= x1 { (x0, x1) } else { (x1, x0) }; + let i = interests.partition_point(|p| p.point.x < xmin - d); + let interests = &interests[i..]; + let i = interests.partition_point(|p| p.point.x <= xmax + d); + let interests = &interests[..i]; + for interest in interests { + if interest.point.distance_to_segment(&segment[0], &segment[1]) <= d { + local_interests.push(*interest); + } + } + positions.push(local_interests); + } + // fuse points on chunks of consecutive segments together + let grouped_positions = positions + .chunks(groups_size) + .map(|c| c.iter().flatten().unique().copied().collect::>()) + .collect::>(); + // now, group the points in buckets + let chunks = grouped_positions + .iter() + .enumerate() + .flat_map(|(i, points)| points.iter().map(move |p| (i, p))) + .chunks(buckets_size); + let mut buckets = Vec::new(); + for bucket_points in &chunks { + let mut bucket_points = bucket_points.peekable(); + let start = bucket_points.peek().unwrap().0; + let points = bucket_points.map(|(_, p)| *p).collect(); + buckets.push(Bucket { points, start }); + } + buckets +} + +fn detect_sharp_turns(path: &[Point], waypoints: &mut HashSet) { + path.iter() + .tuple_windows() + .map(|(a, b, c)| { + let xd1 = b.x - a.x; + let yd1 = b.y - a.y; + let angle1 = yd1.atan2(xd1); + + let xd2 = c.x - b.x; + let yd2 = c.y - b.y; + let angle2 = yd2.atan2(xd2); + let adiff = angle2 - angle1; + let adiff = if adiff < 0.0 { + adiff + std::f64::consts::PI * 2.0 + } else { + adiff + }; + (adiff, b) + }) + .filter_map(|(adiff, b)| { + if adiff > LOWER_SHARP_TURN && adiff < UPPER_SHARP_TURN { + Some(b) + } else { + None + } + }) + .for_each(|b| { + waypoints.insert(*b); + }); +} + +#[wasm_bindgen] +pub fn convert_gpx_strings(input_str: &str) -> Vec { + let mut interests = Vec::new(); + let mut output: Vec = Vec::new(); + convert_gpx(input_str.as_bytes(), &mut output, &mut interests); + output +} + +pub fn convert_gpx_files(input_file: &str, interests: &mut [InterestPoint]) { + let file = File::open(input_file).unwrap(); + let reader = BufReader::new(file); + let output_path = Path::new(&input_file).with_extension("gpc"); + let writer = BufWriter::new(File::create(output_path).unwrap()); + convert_gpx(reader, writer, interests); +} + +fn convert_gpx( + input_reader: R, + output_writer: W, + interests: &mut [InterestPoint], +) { + // load all points composing the trace and mark commented points + // as special waypoints. + let (mut waypoints, p) = points(input_reader); + + // detect sharp turns before path simplification to keep them + detect_sharp_turns(&p, &mut waypoints); + waypoints.insert(p.first().copied().unwrap()); + waypoints.insert(p.last().copied().unwrap()); + println!("we have {} waypoints", waypoints.len()); + + println!("initially we had {} points", p.len()); + + // simplify path + let mut rp = Vec::new(); + let mut segment = Vec::new(); + for point in &p { + segment.push(*point); + if waypoints.contains(point) { + if segment.len() >= 2 { + let mut s = simplify_path(&segment, 0.00015); + rp.append(&mut s); + segment = rp.pop().into_iter().collect(); + } + } + } + rp.append(&mut segment); + println!("we now have {} points", rp.len()); + + // add interest points from open street map if we have any + let buckets = position_interests_along_path(interests, &rp, 0.001, 5, 3); + + // save_svg( + // "test.svg", + // &p, + // &rp, + // buckets.iter().flat_map(|b| &b.points), + // &waypoints, + // ) + // .unwrap(); + + save_gpc(output_writer, &rp, &waypoints, &buckets).unwrap(); +} diff --git a/apps/gipy/gpconv/src/main.rs b/apps/gipy/gpconv/src/main.rs index 9b7a19193..c804d0889 100644 --- a/apps/gipy/gpconv/src/main.rs +++ b/apps/gipy/gpconv/src/main.rs @@ -1,705 +1,22 @@ -use itertools::Itertools; -use osmio::ObjId; -use std::collections::{HashMap, HashSet}; -use std::fs::File; -use std::io::{BufReader, BufWriter, Write}; -use std::path::Path; - -use gpx::read; -use gpx::Gpx; - -mod osm; -use osm::{parse_osm_data, InterestPoint}; - -const LOWER_SHARP_TURN: f64 = 80.0 * std::f64::consts::PI / 180.0; -const UPPER_SHARP_TURN: f64 = std::f64::consts::PI * 2.0 - LOWER_SHARP_TURN; - -const KEY: u16 = 47490; -const FILE_VERSION: u16 = 3; - -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct Point { - x: f64, - y: f64, -} - -impl Eq for Point {} -impl std::hash::Hash for Point { - fn hash(&self, state: &mut H) { - unsafe { std::mem::transmute::(self.x) }.hash(state); - unsafe { std::mem::transmute::(self.y) }.hash(state); - } -} - -impl Point { - fn squared_distance_between(&self, other: &Point) -> f64 { - let dx = other.x - self.x; - let dy = other.y - self.y; - dx * dx + dy * dy - } - fn distance_to_segment(&self, v: &Point, w: &Point) -> f64 { - let l2 = v.squared_distance_between(w); - if l2 == 0.0 { - return self.squared_distance_between(v).sqrt(); - } - // Consider the line extending the segment, parameterized as v + t (w - v). - // We find projection of point p onto the line. - // It falls where t = [(p-v) . (w-v)] / |w-v|^2 - // We clamp t from [0,1] to handle points outside the segment vw. - let x0 = self.x - v.x; - let y0 = self.y - v.y; - let x1 = w.x - v.x; - let y1 = w.y - v.y; - let dot = x0 * x1 + y0 * y1; - let t = (dot / l2).min(1.0).max(0.0); - - let proj = Point { - x: v.x + x1 * t, - y: v.y + y1 * t, - }; - - proj.squared_distance_between(self).sqrt() - } -} - -fn points(filename: &str) -> (HashSet, Vec) { - let file = File::open(filename).unwrap(); - let reader = BufReader::new(file); - - // read takes any io::Read and gives a Result. - let mut gpx: Gpx = read(reader).unwrap(); - eprintln!("we have {} tracks", gpx.tracks.len()); - - let mut waypoints = HashSet::new(); - - let points = gpx - .tracks - .pop() - .unwrap() - .segments - .into_iter() - .flat_map(|segment| segment.points.into_iter()) - .map(|p| { - let is_commented = p.comment.is_some(); - let (x, y) = p.point().x_y(); - let p = Point { x, y }; - if is_commented { - waypoints.insert(p); - } - p - }) - .collect::>(); - (waypoints, points) -} - -// // NOTE: this angles idea could maybe be use to get dp from n^3 to n^2 -// fn acceptable_angles(p1: &(f64, f64), p2: &(f64, f64), epsilon: f64) -> (f64, f64) { -// // first, convert p2's coordinates for p1 as origin -// let (x1, y1) = *p1; -// let (x2, y2) = *p2; -// let (x, y) = (x2 - x1, y2 - y1); -// // rotate so that (p1, p2) ends on x axis -// let theta = y.atan2(x); -// let rx = x * theta.cos() - y * theta.sin(); -// let ry = x * theta.sin() + y * theta.cos(); -// assert!(ry.abs() <= std::f64::EPSILON); -// -// // now imagine a line at an angle alpha. -// // we want the distance d from (rx, 0) to our line -// // we have sin(alpha) = d / rx -// // limiting d to epsilon, we solve -// // sin(alpha) = e / rx -// // and get -// // alpha = arcsin(e/rx) -// let alpha = (epsilon / rx).asin(); -// -// // now we just need to rotate back -// let a1 = theta + alpha.abs(); -// let a2 = theta - alpha.abs(); -// assert!(a1 >= a2); -// (a1, a2) -// } -// -// // this is like ramer douglas peucker algorithm -// // except that we advance from the start without knowing the end. -// // each point we meet constrains the chosen segment's angle -// // a bit more. -// // -// fn simplify(mut points: &[(f64, f64)]) -> Vec<(f64, f64)> { -// let mut remaining_points = Vec::new(); -// while !points.is_empty() { -// let (sx, sy) = points.first().unwrap(); -// let i = match points -// .iter() -// .enumerate() -// .map(|(i, (x, y))| todo!("compute angles")) -// .try_fold( -// (0.0f64, std::f64::consts::FRAC_2_PI), -// |(amin, amax), (i, (amin2, amax2))| -> Result<(f64, f64), usize> { -// let new_amax = amax.min(amax2); -// let new_amin = amin.max(amin2); -// if new_amin >= new_amax { -// Err(i) -// } else { -// Ok((new_amin, new_amax)) -// } -// }, -// ) { -// Err(i) => i, -// Ok(_) => points.len(), -// }; -// remaining_points.push(points.first().cloned().unwrap()); -// points = &points[i..]; -// } -// remaining_points -// } - -fn extract_prog_dyn_solution( - points: &[Point], - start: usize, - end: usize, - cache: &HashMap<(usize, usize), (Option, usize)>, -) -> Vec { - if let Some(choice) = cache.get(&(start, end)).unwrap().0 { - let mut v1 = extract_prog_dyn_solution(points, start, choice + 1, cache); - let mut v2 = extract_prog_dyn_solution(points, choice, end, cache); - v1.pop(); - v1.append(&mut v2); - v1 - } else { - vec![points[start], points[end - 1]] - } -} - -fn simplify_prog_dyn( - points: &[Point], - start: usize, - end: usize, - epsilon: f64, - cache: &mut HashMap<(usize, usize), (Option, usize)>, -) -> usize { - if let Some(val) = cache.get(&(start, end)) { - val.1 - } else { - let res = if end - start <= 2 { - assert_eq!(end - start, 2); - (None, end - start) - } else { - let first_point = &points[start]; - let last_point = &points[end - 1]; - - if points[(start + 1)..end] - .iter() - .map(|p| p.distance_to_segment(first_point, last_point)) - .all(|d| d <= epsilon) - { - (None, 2) - } else { - // now we test all possible cutting points - ((start + 1)..(end - 1)) //TODO: take middle min - .map(|i| { - let v1 = simplify_prog_dyn(points, start, i + 1, epsilon, cache); - let v2 = simplify_prog_dyn(points, i, end, epsilon, cache); - (Some(i), v1 + v2 - 1) - }) - .min_by_key(|(_, v)| *v) - .unwrap() - } - }; - cache.insert((start, end), res); - res.1 - } -} - -fn rdp(points: &[Point], epsilon: f64) -> Vec { - if points.len() <= 2 { - points.iter().copied().collect() - } else { - if points.first().unwrap() == points.last().unwrap() { - let first = points.first().unwrap(); - let index_farthest = points - .iter() - .enumerate() - .skip(1) - .max_by(|(_, p1), (_, p2)| { - first - .squared_distance_between(p1) - .partial_cmp(&first.squared_distance_between(p2)) - .unwrap() - }) - .map(|(i, _)| i) - .unwrap(); - - let start = &points[..(index_farthest + 1)]; - let end = &points[index_farthest..]; - let mut res = rdp(start, epsilon); - res.pop(); - res.append(&mut rdp(end, epsilon)); - res - } else { - let (index_farthest, farthest_distance) = points - .iter() - .map(|p| p.distance_to_segment(points.first().unwrap(), points.last().unwrap())) - .enumerate() - .max_by(|(_, d1), (_, d2)| { - if d1.is_nan() { - std::cmp::Ordering::Greater - } else { - if d2.is_nan() { - std::cmp::Ordering::Less - } else { - d1.partial_cmp(d2).unwrap() - } - } - }) - .unwrap(); - if farthest_distance <= epsilon { - vec![ - points.first().copied().unwrap(), - points.last().copied().unwrap(), - ] - } else { - let start = &points[..(index_farthest + 1)]; - let end = &points[index_farthest..]; - let mut res = rdp(start, epsilon); - res.pop(); - res.append(&mut rdp(end, epsilon)); - res - } - } - } -} - -fn simplify_path(points: &[Point], epsilon: f64) -> Vec { - if points.len() <= 600 { - optimal_simplification(points, epsilon) - } else { - hybrid_simplification(points, epsilon) - } -} - -fn convert_coordinates(points: &[(f64, f64)]) -> (f64, f64, Vec<(i32, i32)>) { - let xmin = points - .iter() - .map(|(x, _)| x) - .min_by(|x1, x2| x1.partial_cmp(x2).unwrap()) - .unwrap(); - - let ymin = points - .iter() - .map(|(_, y)| y) - .min_by(|y1, y2| y1.partial_cmp(y2).unwrap()) - .unwrap(); - - // 0.00001 is 1 meter - // max distance is 1000km - // so we need at most 10^6 - ( - *xmin, - *ymin, - points - .iter() - .map(|(x, y)| { - eprintln!("x {} y {}", x, y); - let r = ( - ((*x - xmin) * 100_000.0) as i32, - ((*y - ymin) * 100_000.0) as i32, - ); - eprintln!( - "again x {} y {}", - xmin + r.0 as f64 / 100_000.0, - ymin + r.1 as f64 / 100_000.0 - ); - r - }) - .collect(), - ) -} - -fn compress_coordinates(points: &[(i32, i32)]) -> Vec<(i16, i16)> { - // we could store the diffs such that - // diffs are either 8bits or 16bits nums - // we store how many nums are 16bits - // then all their indices (compressed with diffs) - // then all nums as either 8 or 16bits - let xdiffs = std::iter::once(0).chain( - points - .iter() - .map(|(x, _)| x) - .tuple_windows() - .map(|(x1, x2)| (x2 - x1) as i16), - ); - - let ydiffs = std::iter::once(0).chain( - points - .iter() - .map(|(_, y)| y) - .tuple_windows() - .map(|(y1, y2)| (y2 - y1) as i16), - ); - - xdiffs.zip(ydiffs).collect() -} - -fn save_gpc>( - path: P, - points: &[Point], - waypoints: &HashSet, - buckets: &[Bucket], -) -> std::io::Result<()> { - let mut writer = BufWriter::new(File::create(path)?); - - eprintln!("saving {} points", points.len()); - - let mut unique_interest_points = Vec::new(); - let mut correspondance = HashMap::new(); - let interests_on_path = buckets - .iter() - .flat_map(|b| &b.points) - .map(|p| match correspondance.entry(*p) { - std::collections::hash_map::Entry::Occupied(o) => *o.get(), - std::collections::hash_map::Entry::Vacant(v) => { - let index = unique_interest_points.len(); - unique_interest_points.push(*p); - v.insert(index); - index - } - }) - .collect::>(); - - writer.write_all(&KEY.to_le_bytes())?; - writer.write_all(&FILE_VERSION.to_le_bytes())?; - writer.write_all(&(points.len() as u16).to_le_bytes())?; - writer.write_all(&(unique_interest_points.len() as u16).to_le_bytes())?; - writer.write_all(&(interests_on_path.len() as u16).to_le_bytes())?; - points - .iter() - .flat_map(|p| [p.x, p.y]) - .try_for_each(|c| writer.write_all(&c.to_le_bytes()))?; - - let mut waypoints_bits = std::iter::repeat(0u8) - .take(points.len() / 8 + if points.len() % 8 != 0 { 1 } else { 0 }) - .collect::>(); - points.iter().enumerate().for_each(|(i, p)| { - if waypoints.contains(p) { - waypoints_bits[i / 8] |= 1 << (i % 8) - } - }); - waypoints_bits - .iter() - .try_for_each(|byte| writer.write_all(&byte.to_le_bytes()))?; - - unique_interest_points - .iter() - .flat_map(|p| [p.point.x, p.point.y]) - .try_for_each(|c| writer.write_all(&c.to_le_bytes()))?; - - let counts: HashMap<_, usize> = - unique_interest_points - .iter() - .fold(HashMap::new(), |mut h, p| { - *h.entry(p.interest).or_default() += 1; - h - }); - counts.into_iter().for_each(|(interest, count)| { - eprintln!("{:?} appears {} times", interest, count); - }); - - unique_interest_points - .iter() - .map(|p| p.interest.into()) - .try_for_each(|i: u8| writer.write_all(&i.to_le_bytes()))?; - - interests_on_path - .iter() - .map(|i| *i as u16) - .try_for_each(|i| writer.write_all(&i.to_le_bytes()))?; - - buckets - .iter() - .map(|b| b.start as u16) - .try_for_each(|i| writer.write_all(&i.to_le_bytes()))?; - - Ok(()) -} - -fn optimal_simplification(points: &[Point], epsilon: f64) -> Vec { - let mut cache = HashMap::new(); - simplify_prog_dyn(&points, 0, points.len(), epsilon, &mut cache); - extract_prog_dyn_solution(&points, 0, points.len(), &cache) -} - -fn hybrid_simplification(points: &[Point], epsilon: f64) -> Vec { - if points.len() <= 300 { - optimal_simplification(points, epsilon) - } else { - if points.first().unwrap() == points.last().unwrap() { - let first = points.first().unwrap(); - let index_farthest = points - .iter() - .enumerate() - .skip(1) - .max_by(|(_, p1), (_, p2)| { - first - .squared_distance_between(p1) - .partial_cmp(&first.squared_distance_between(p2)) - .unwrap() - }) - .map(|(i, _)| i) - .unwrap(); - - let start = &points[..(index_farthest + 1)]; - let end = &points[index_farthest..]; - let mut res = hybrid_simplification(start, epsilon); - res.pop(); - res.append(&mut hybrid_simplification(end, epsilon)); - res - } else { - let (index_farthest, farthest_distance) = points - .iter() - .map(|p| p.distance_to_segment(points.first().unwrap(), points.last().unwrap())) - .enumerate() - .max_by(|(_, d1), (_, d2)| { - if d1.is_nan() { - std::cmp::Ordering::Greater - } else { - if d2.is_nan() { - std::cmp::Ordering::Less - } else { - d1.partial_cmp(d2).unwrap() - } - } - }) - .unwrap(); - if farthest_distance <= epsilon { - vec![ - points.first().copied().unwrap(), - points.last().copied().unwrap(), - ] - } else { - let start = &points[..(index_farthest + 1)]; - let end = &points[index_farthest..]; - let mut res = hybrid_simplification(start, epsilon); - res.pop(); - res.append(&mut hybrid_simplification(end, epsilon)); - res - } - } - } -} - -fn save_path(writer: &mut W, p: &[Point], stroke: &str) -> std::io::Result<()> { - write!( - writer, - "")?; - Ok(()) -} - -fn save_svg<'a, P: AsRef, I: IntoIterator>( - filename: P, - p: &[Point], - rp: &[Point], - interest_points: I, - waypoints: &HashSet, -) -> std::io::Result<()> { - let mut writer = BufWriter::new(std::fs::File::create(filename)?); - let (xmin, xmax) = p - .iter() - .map(|p| p.x) - .minmax_by(|a, b| a.partial_cmp(b).unwrap()) - .into_option() - .unwrap(); - - let (ymin, ymax) = p - .iter() - .map(|p| p.y) - .minmax_by(|a, b| a.partial_cmp(b).unwrap()) - .into_option() - .unwrap(); - - writeln!( - &mut writer, - "", - xmin, - ymin, - xmax - xmin, - ymax - ymin - )?; - write!( - &mut writer, - "", - xmin, - ymin, - xmax - xmin, - ymax - ymin - )?; - - save_path(&mut writer, &p, "red")?; - save_path(&mut writer, &rp, "black")?; - - for point in interest_points { - writeln!( - &mut writer, - "", - point.point.x, - point.point.y, - point.color(), - )?; - } - - waypoints.iter().try_for_each(|p| { - writeln!( - &mut writer, - "", - p.x, p.y, - ) - })?; - - writeln!(&mut writer, "")?; - Ok(()) -} - -pub struct Bucket { - points: Vec, - start: usize, -} - -fn position_interests_along_path( - interests: &mut [InterestPoint], - path: &[Point], - d: f64, - buckets_size: usize, // final points are indexed in buckets - groups_size: usize, // how many segments are compacted together -) -> Vec { - interests.sort_unstable_by(|p1, p2| p1.point.x.partial_cmp(&p2.point.x).unwrap()); - // first compute for each segment a vec containing its nearby points - let mut positions = Vec::new(); - for segment in path.windows(2) { - let mut local_interests = Vec::new(); - let x0 = segment[0].x; - let x1 = segment[1].x; - let (xmin, xmax) = if x0 <= x1 { (x0, x1) } else { (x1, x0) }; - let i = interests.partition_point(|p| p.point.x < xmin - d); - let interests = &interests[i..]; - let i = interests.partition_point(|p| p.point.x <= xmax + d); - let interests = &interests[..i]; - for interest in interests { - if interest.point.distance_to_segment(&segment[0], &segment[1]) <= d { - local_interests.push(*interest); - } - } - positions.push(local_interests); - } - // fuse points on chunks of consecutive segments together - let grouped_positions = positions - .chunks(groups_size) - .map(|c| c.iter().flatten().unique().copied().collect::>()) - .collect::>(); - // now, group the points in buckets - let chunks = grouped_positions - .iter() - .enumerate() - .flat_map(|(i, points)| points.iter().map(move |p| (i, p))) - .chunks(buckets_size); - let mut buckets = Vec::new(); - for bucket_points in &chunks { - let mut bucket_points = bucket_points.peekable(); - let start = bucket_points.peek().unwrap().0; - let points = bucket_points.map(|(_, p)| *p).collect(); - buckets.push(Bucket { points, start }); - } - buckets -} - -fn detect_sharp_turns(path: &[Point], waypoints: &mut HashSet) { - path.iter() - .tuple_windows() - .map(|(a, b, c)| { - let xd1 = b.x - a.x; - let yd1 = b.y - a.y; - let angle1 = yd1.atan2(xd1); - - let xd2 = c.x - b.x; - let yd2 = c.y - b.y; - let angle2 = yd2.atan2(xd2); - let adiff = angle2 - angle1; - let adiff = if adiff < 0.0 { - adiff + std::f64::consts::PI * 2.0 - } else { - adiff - }; - (adiff, b) - }) - .filter_map(|(adiff, b)| { - if adiff > LOWER_SHARP_TURN && adiff < UPPER_SHARP_TURN { - Some(b) - } else { - None - } - }) - .for_each(|b| { - waypoints.insert(*b); - }); -} +use gpconv::convert_gpx_files; fn main() { let input_file = std::env::args().nth(1).unwrap_or("m.gpx".to_string()); - let osm_file = std::env::args().nth(2); + let mut interests; - let mut interests = if let Some(osm) = osm_file { - parse_osm_data(osm) - } else { - Vec::new() - }; - - println!("input is {}", input_file); - let (mut waypoints, p) = points(&input_file); - - detect_sharp_turns(&p, &mut waypoints); - waypoints.insert(p.first().copied().unwrap()); - waypoints.insert(p.last().copied().unwrap()); - println!("we have {} waypoints", waypoints.len()); - - println!("initially we had {} points", p.len()); - - let mut rp = Vec::new(); - let mut segment = Vec::new(); - for point in &p { - segment.push(*point); - if waypoints.contains(point) { - if segment.len() >= 2 { - let mut s = simplify_path(&segment, 0.00015); - rp.append(&mut s); - segment = rp.pop().into_iter().collect(); - } - } + #[cfg(feature = "osm")] + { + let osm_file = std::env::args().nth(2); + let mut interests = if let Some(osm) = osm_file { + interests = parse_osm_data(osm); + } else { + Vec::new() + }; + } + #[cfg(not(feature = "osm"))] + { + interests = Vec::new() } - rp.append(&mut segment); - println!("we now have {} points", rp.len()); - // let mut interests = parse_osm_data("isere.osm.pbf"); - let buckets = position_interests_along_path(&mut interests, &rp, 0.001, 5, 3); - // let i = get_openstreetmap_data(&rp).await; - // let i = HashSet::new(); - save_svg( - "test.svg", - &p, - &rp, - buckets.iter().flat_map(|b| &b.points), - &waypoints, - ) - .unwrap(); - - save_gpc( - Path::new(&input_file).with_extension("gpc"), - &rp, - &waypoints, - &buckets, - ) - .unwrap(); + convert_gpx_files(&input_file, &mut interests); } diff --git a/apps/gipy/gpconv/src/osm.rs b/apps/gipy/gpconv/src/osm.rs index 4877165a5..ebc7071ef 100644 --- a/apps/gipy/gpconv/src/osm.rs +++ b/apps/gipy/gpconv/src/osm.rs @@ -1,3 +1,4 @@ +use super::Interest; use super::Point; use itertools::Itertools; use lazy_static::lazy_static; @@ -6,87 +7,6 @@ use osmio::{prelude::*, ObjId}; use std::collections::{HashMap, HashSet}; use std::path::Path; -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -pub enum Interest { - Bakery, - DrinkingWater, - Toilets, - // BikeShop, - // ChargingStation, - // Bank, - // Supermarket, - // Table, - // TourismOffice, - Artwork, - // Pharmacy, -} - -impl Into for Interest { - fn into(self) -> u8 { - match self { - Interest::Bakery => 0, - Interest::DrinkingWater => 1, - Interest::Toilets => 2, - // Interest::BikeShop => 8, - // Interest::ChargingStation => 4, - // Interest::Bank => 5, - // Interest::Supermarket => 6, - // Interest::Table => 7, - Interest::Artwork => 3, - // Interest::Pharmacy => 9, - } - } -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -pub struct InterestPoint { - pub point: Point, - pub interest: Interest, -} - -lazy_static! { - static ref INTERESTS: HashMap<(&'static str, &'static str), Interest> = { - [ - (("shop", "bakery"), Interest::Bakery), - (("amenity", "drinking_water"), Interest::DrinkingWater), - (("amenity", "toilets"), Interest::Toilets), - // (("shop", "bicycle"), Interest::BikeShop), - // (("amenity", "charging_station"), Interest::ChargingStation), - // (("amenity", "bank"), Interest::Bank), - // (("shop", "supermarket"), Interest::Supermarket), - // (("leisure", "picnic_table"), Interest::Table), - // (("tourism", "information"), Interest::TourismOffice), - (("tourism", "artwork"), Interest::Artwork), - // (("amenity", "pharmacy"), Interest::Pharmacy), - ] - .into_iter() - .collect() - }; -} - -impl Interest { - fn new(key: &str, value: &str) -> Option { - INTERESTS.get(&(key, value)).cloned() - } -} - -impl InterestPoint { - pub fn color(&self) -> &'static str { - match self.interest { - Interest::Bakery => "red", - Interest::DrinkingWater => "blue", - Interest::Toilets => "brown", - // Interest::BikeShop => "purple", - // Interest::ChargingStation => "green", - // Interest::Bank => "black", - // Interest::Supermarket => "red", - // Interest::Table => "pink", - Interest::Artwork => "orange", - // Interest::Pharmacy => "chartreuse", - } - } -} - pub fn parse_osm_data>(path: P) -> Vec { let reader = osmio::read_pbf(path).ok(); reader diff --git a/apps/gipy/gpconv/src/svg.rs b/apps/gipy/gpconv/src/svg.rs new file mode 100644 index 000000000..387a0f7a0 --- /dev/null +++ b/apps/gipy/gpconv/src/svg.rs @@ -0,0 +1,86 @@ +use itertools::Itertools; +use std::{ + collections::HashSet, + io::{BufWriter, Write}, + path::Path, +}; + +use crate::{interests::InterestPoint, Point}; + +fn save_path(writer: &mut W, p: &[Point], stroke: &str) -> std::io::Result<()> { + write!( + writer, + "")?; + Ok(()) +} + +// save svg file from given path and interest points. +// useful for debugging path simplification and previewing traces. +pub fn save_svg<'a, P: AsRef, I: IntoIterator>( + filename: P, + p: &[Point], + rp: &[Point], + interest_points: I, + waypoints: &HashSet, +) -> std::io::Result<()> { + let mut writer = BufWriter::new(std::fs::File::create(filename)?); + let (xmin, xmax) = p + .iter() + .map(|p| p.x) + .minmax_by(|a, b| a.partial_cmp(b).unwrap()) + .into_option() + .unwrap(); + + let (ymin, ymax) = p + .iter() + .map(|p| p.y) + .minmax_by(|a, b| a.partial_cmp(b).unwrap()) + .into_option() + .unwrap(); + + writeln!( + &mut writer, + "", + xmin, + ymin, + xmax - xmin, + ymax - ymin + )?; + write!( + &mut writer, + "", + xmin, + ymin, + xmax - xmin, + ymax - ymin + )?; + + save_path(&mut writer, &p, "red")?; + save_path(&mut writer, &rp, "black")?; + + for point in interest_points { + writeln!( + &mut writer, + "", + point.point.x, + point.point.y, + point.color(), + )?; + } + + waypoints.iter().try_for_each(|p| { + writeln!( + &mut writer, + "", + p.x, p.y, + ) + })?; + + writeln!(&mut writer, "")?; + Ok(()) +}