diff --git a/apps/gipy/TODO b/apps/gipy/TODO new file mode 100644 index 000000000..44ef08e23 --- /dev/null +++ b/apps/gipy/TODO @@ -0,0 +1,20 @@ +- plugins +- do not redraw if no coordinates changed +- display distance to target + +- detect reached waypoints +- beep when reaching waypoint +- display distance to next waypoint +- display average speed +- turn off gps when moving to next waypoint +- beep when moving away from path +- dynamic map rescale +- display scale (100m) + +- store several tracks + +- map rotation to match direction +- water points + +- compress path +- avoid display of all segments diff --git a/apps/gipy/app.js b/apps/gipy/app.js index 92ec0ccfc..3b71c7f6a 100644 --- a/apps/gipy/app.js +++ b/apps/gipy/app.js @@ -14,17 +14,35 @@ var lon = null; class Path { constructor(filename) { let buffer = require("Storage").readArrayBuffer(filename); - this.points_number = (buffer.byteLength - 2*8)/4; - this.view = DataView(buffer); - this.min_lon = this.view.getFloat64(0); - this.min_lat = this.view.getFloat64(8); - this.current_start = 0; // index of first point to be displayed - this.current_x = 0; - this.current_y = 0; + this.points = Float64Array(buffer); + } + + point(index) { + let lon = this.points[2*index]; + let lat = this.points[2*index+1]; + return new Point(lon, lat); + } + + // return index of segment which is nearest from point + nearest_segment(point, start, end) { + let previous_point = null; + let min_index = 0; + let min_distance = Number.MAX_VALUE; + for(let i = Math.max(0, start) ; i < Math.min(this.len, end) ; i++) { + let current_point = this.point(i); + if (previous_point !== null) { + let distance = point.distance_to_segment(previous_point, current_point); + if (distance <= min_distance) { + min_distance = distance; + min_index = i-1; + } + } + previous_point = current_point; + } + return min_index; } - get len() { - return this.points_number; + return this.points.length /2; } } @@ -34,52 +52,134 @@ class Point { this.lat = lat; } screen_x() { - return 192/2 + Math.round((this.lon - lon) * 100000.0); + return 172/2 + Math.round((this.lon - lon) * 100000.0); } screen_y() { - return 192/2 + Math.round((this.lat - lat) * 100000.0); + return 172/2 + Math.round((this.lat - lat) * 100000.0); + } + minus(other_point) { + let xdiff = this.lon - other_point.lon; + let ydiff = this.lat - other_point.lat; + return new Point(xdiff, ydiff); + } + plus(other_point) { + return new Point(this.lon + other_point.lon, this.lat + other_point.lat); + } + length_squared(other_point) { + let d = this.minus(other_point); + return (d.lon*d.lon + d.lat*d.lat); + } + times(scalar) { + return new Point(this.lon * scalar, this.lat * scalar); + } + dot(other_point) { + return this.lon * other_point.lon + this.lat * other_point.lat; + } + distance(other_point) { + return Math.sqrt(this.length_squared(other_point)); + } + distance_to_segment(v, w) { + // from : https://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment + // Return minimum distance between line segment vw and point p + let l2 = v.length_squared(w); // i.e. |w-v|^2 - avoid a sqrt + if (l2 == 0.0) { + return this.distance(v); // v == w case + } + // 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 t = Math.max(0, Math.min(1, (this.minus(v)).dot(w.minus(v)) / l2)); + let projection = v.plus((w.minus(v)).times(t)); // Projection falls on the segment + return this.distance(projection); } } function display(path) { g.clear(); + g.setColor(g.theme.fg); + let next_segment = path.nearest_segment(new Point(lon, lat), current_segment-2, current_segment+3); + if (next_segment < current_segment) { + console.log("error going from", current_segment, "back to", next_segment, "at", lon, lat); + console.log("we are at", fake_gps_point); + let previous_point = null; + for (let i = 0 ; i < current_segment+2 ; i++) { + let point = path.point(i); + if (previous_point !== null) { + let distance = new Point(lon, lat).distance_to_segment(previous_point, point); + console.log(i, distance); + } + previous_point = point; + } + } + current_segment = next_segment; + //current_segment = path.nearest_segment(new Point(lon, lat), 0, path.len); let previous_point = null; - let current_x = path.current_x; - let current_y = path.current_y; - for (let i = path.current_start ; i < path.len ; i++) { - current_x += path.view.getInt16(2*8+4*i); - current_y += path.view.getInt16(2*8+4*i+2); - let point = new Point(current_x/100000.0 + path.min_lon, current_y/100000.0 + path.min_lat); + let start = Math.max(current_segment - 4, 0); + let end = Math.min(current_segment + 5, path.len); + for (let i=start ; i < end ; i++) { + let point = path.point(i); + let px = point.screen_x(); + let py = point.screen_y(); if (previous_point !== null) { + if (i == current_segment + 1) { + g.setColor(0.0, 1.0, 0.0); + } else { + g.setColor(1.0, 0.0, 0.0); + } g.drawLine( previous_point.screen_x(), previous_point.screen_y(), - point.screen_x(), - point.screen_y() + px, + py ); } + g.setColor(g.theme.fg2); + g.fillCircle(px, py, 4); + g.setColor(g.theme.fg); + g.fillCircle(px, py, 3); previous_point = point; } - g.setColor(1.0, 0.0, 0.0); - g.fillCircle(192/2, 192/2, 5); + g.setColor(g.theme.fgH); + g.fillCircle(172/2, 172/2, 5); } let path = new Path("test.gpc"); lat = path.min_lat; lon = path.min_lon; +console.log("len is", path.len); +var current_segment = path.nearest_segment(new Point(lon, lat), 0, Number.MAX_VALUE); -function set_coordinates(data) { - if (!isNaN(data.lat)) { - lat = data.lat; - } - if (!isNaN(data.lon)) { - lon = data.lon; +// function set_coordinates(data) { +// if (!isNaN(data.lat)) { +// lat = data.lat; +// } +// if (!isNaN(data.lon)) { +// lon = data.lon; +// } +// } +// Bangle.setGPSPower(true, "gipy"); +// Bangle.on('GPS', set_coordinates); + + + +let fake_gps_point = 0.0; +function simulate_gps(path) { + let point_index = Math.floor(fake_gps_point); + if (point_index >= path.len) { + return; } + let p1 = path.point(point_index); + let p2 = path.point(point_index+1); + let alpha = fake_gps_point - point_index; + + lon = (1-alpha)*p1.lon + alpha*p2.lon; + lat = (1-alpha)*p1.lat + alpha*p2.lat; + fake_gps_point += 0.2; + display(path); } -Bangle.setGPSPower(true, "gipy"); -Bangle.on('GPS', set_coordinates); - -setInterval(display, 1000, path); +setInterval(simulate_gps, 500, path); +//// setInterval(display, 1000, path); diff --git a/apps/gipy/gpconv/.gitignore b/apps/gipy/gpconv/.gitignore new file mode 100644 index 000000000..ea8c4bf7f --- /dev/null +++ b/apps/gipy/gpconv/.gitignore @@ -0,0 +1 @@ +/target diff --git a/apps/gipy/gpconv/Cargo.lock b/apps/gipy/gpconv/Cargo.lock new file mode 100644 index 000000000..fbe549955 --- /dev/null +++ b/apps/gipy/gpconv/Cargo.lock @@ -0,0 +1,257 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "addr2line" +version = "0.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9ecd88a8c8378ca913a680cd98f0f13ac67383d35993f86c90a70e3f137816b" +dependencies = [ + "gimli", +] + +[[package]] +name = "adler" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f26201604c87b1e01bd3d98f8d5d9a8fcbb815e8cedb41ffccbeb4bf593a35fe" + +[[package]] +name = "assert_approx_eq" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c07dab4369547dbe5114677b33fbbf724971019f3818172d59a97a61c774ffd" + +[[package]] +name = "autocfg" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" + +[[package]] +name = "backtrace" +version = "0.3.65" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11a17d453482a265fd5f8479f2a3f405566e6ca627837aaddb85af8b1ab8ef61" +dependencies = [ + "addr2line", + "cc", + "cfg-if", + "libc", + "miniz_oxide", + "object", + "rustc-demangle", +] + +[[package]] +name = "cc" +version = "1.0.73" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fff2a6927b3bb87f9595d67196a70493f627687a71d87a0d692242c33f58c11" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "either" +version = "1.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e78d4f1cc4ae33bbfc157ed5d5a5ef3bc29227303d595861deb238fcec4e9457" + +[[package]] +name = "error-chain" +version = "0.12.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2d2f06b9cac1506ece98fe3231e3cc9c4410ec3d5b1f24ae1c8946f0742cdefc" +dependencies = [ + "backtrace", + "version_check", +] + +[[package]] +name = "geo-types" +version = "0.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9805fbfcea97de816e6408e938603241879cc41eea3fba3f84f122f4f6f9c54" +dependencies = [ + "num-traits", +] + +[[package]] +name = "gimli" +version = "0.26.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78cc372d058dcf6d5ecd98510e7fbc9e5aec4d21de70f65fea8fecebcd881bd4" + +[[package]] +name = "gpconv" +version = "0.1.0" +dependencies = [ + "gpx", + "itertools", +] + +[[package]] +name = "gpx" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b03599b85866c88fd0125db7ca7a683be1550724918682c736c7893a399dc5e" +dependencies = [ + "assert_approx_eq", + "error-chain", + "geo-types", + "thiserror", + "time", + "xml-rs", +] + +[[package]] +name = "itertools" +version = "0.10.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a9a9d19fa1e79b6215ff29b9d6880b706147f16e9b1dbb1e4e5947b5b02bc5e3" +dependencies = [ + "either", +] + +[[package]] +name = "itoa" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "112c678d4050afce233f4f2852bb2eb519230b3cf12f33585275537d7e41578d" + +[[package]] +name = "libc" +version = "0.2.126" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "349d5a591cd28b49e1d1037471617a32ddcda5731b99419008085f72d5a53836" + +[[package]] +name = "memchr" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" + +[[package]] +name = "miniz_oxide" +version = "0.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f5c75688da582b8ffc1f1799e9db273f32133c49e048f614d22ec3256773ccc" +dependencies = [ + "adler", +] + +[[package]] +name = "num-traits" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd" +dependencies = [ + "autocfg", +] + +[[package]] +name = "num_threads" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2819ce041d2ee131036f4fc9d6ae7ae125a3a40e97ba64d04fe799ad9dabbb44" +dependencies = [ + "libc", +] + +[[package]] +name = "object" +version = "0.28.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e42c982f2d955fac81dd7e1d0e1426a7d702acd9c98d19ab01083a6a0328c424" +dependencies = [ + "memchr", +] + +[[package]] +name = "proc-macro2" +version = "1.0.40" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd96a1e8ed2596c337f8eae5f24924ec83f5ad5ab21ea8e455d3566c69fbcaf7" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3bcdf212e9776fbcb2d23ab029360416bb1706b1aea2d1a5ba002727cbcab804" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rustc-demangle" +version = "0.1.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ef03e0a2b150c7a90d01faf6254c9c48a41e95fb2a8c2ac1c6f0d2b9aefc342" + +[[package]] +name = "syn" +version = "1.0.98" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c50aef8a904de4c23c788f104b7dddc7d6f79c647c7c8ce4cc8f73eb0ca773dd" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "thiserror" +version = "1.0.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd829fe32373d27f76265620b5309d0340cb8550f523c1dda251d6298069069a" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0396bc89e626244658bef819e22d0cc459e795a5ebe878e6ec336d1674a8d79a" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "time" +version = "0.3.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72c91f41dcb2f096c05f0873d667dceec1087ce5bcf984ec8ffb19acddbb3217" +dependencies = [ + "itoa", + "libc", + "num_threads", +] + +[[package]] +name = "unicode-ident" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5bd2fe26506023ed7b5e1e315add59d6f584c621d037f9368fea9cfb988f368c" + +[[package]] +name = "version_check" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" + +[[package]] +name = "xml-rs" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2d7d3948613f75c98fd9328cfdcc45acc4d360655289d0a7d4ec931392200a3" diff --git a/apps/gipy/gpconv/Cargo.toml b/apps/gipy/gpconv/Cargo.toml new file mode 100644 index 000000000..69891c99c --- /dev/null +++ b/apps/gipy/gpconv/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "gpconv" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +gpx="*" +itertools="*" \ No newline at end of file diff --git a/apps/gipy/gpconv/src/main.rs b/apps/gipy/gpconv/src/main.rs new file mode 100644 index 000000000..a60924a53 --- /dev/null +++ b/apps/gipy/gpconv/src/main.rs @@ -0,0 +1,208 @@ +use itertools::Itertools; +use std::fs::File; +use std::io::{BufReader, Write}; +use std::path::Path; + +use gpx::read; +use gpx::{Gpx, Track, TrackSegment}; + +fn points(filename: &str) -> impl Iterator { + // This XML file actually exists — try it for yourself! + 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()); + + gpx.tracks + .pop() + .unwrap() + .segments + .into_iter() + .flat_map(|segment| segment.linestring().points().collect::>()) + .map(|point| (point.x(), point.y())) +} + +// returns distance from point p to line passing through points p1 and p2 +// see https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line +fn distance_to_line(p: &(f64, f64), p1: &(f64, f64), p2: &(f64, f64)) -> f64 { + let (x0, y0) = *p; + let (x1, y1) = *p1; + let (x2, y2) = *p2; + let dx = x2 - x1; + let dy = y2 - y1; + (dx * (y1 - y0) - dy * (x1 - x0)).abs() / (dx * dx + dy * dy).sqrt() +} + +fn rdp(points: &[(f64, f64)], epsilon: f64) -> Vec<(f64, f64)> { + if points.len() <= 2 { + points.iter().copied().collect() + } else { + let (index_farthest, farthest_distance) = points + .iter() + .map(|p| distance_to_line(p, points.first().unwrap(), points.last().unwrap())) + .enumerate() + .max_by(|(_, d1), (_, d2)| d1.partial_cmp(d2).unwrap()) + .unwrap(); + if farthest_distance <= epsilon { + vec![ + points.first().copied().unwrap(), + points.last().copied().unwrap(), + ] + } else { + let (start, end) = points.split_at(index_farthest); + let mut res = rdp(start, epsilon); + res.append(&mut rdp(end, epsilon)); + res + } + } +} + +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_coordinates>( + path: P, + //xmin: f64, + //ymin: f64, + // points: &[(i32, i32)], + points: &[(f64, f64)], +) -> std::io::Result<()> { + let mut writer = std::io::BufWriter::new(File::create(path)?); + + eprintln!("saving {} points", points.len()); + // writer.write_all(&xmin.to_be_bytes())?; + // writer.write_all(&ymin.to_be_bytes())?; + points + .iter() + .flat_map(|(x, y)| [x, y]) + .try_for_each(|c| writer.write_all(&c.to_le_bytes()))?; + + Ok(()) +} + +fn save_json>(path: P, points: &[(f64, f64)]) -> std::io::Result<()> { + let mut writer = std::io::BufWriter::new(File::create(path)?); + + eprintln!("saving {} points", points.len()); + writeln!(&mut writer, "[")?; + points + .iter() + .map(|(x, y)| format!("{{\"lat\": {}, \"lon\":{}}}", y, x)) + .intersperse_with(|| ",\n".to_string()) + .try_for_each(|s| write!(&mut writer, "{}", s))?; + write!(&mut writer, "]")?; + + Ok(()) +} + +fn main() { + let input_file = std::env::args().nth(2).unwrap_or("m.gpx".to_string()); + let p = points(&input_file).collect::>(); + let rp = rdp(&p, 0.001); + // let rp = rdp(&p, 0.0001); + save_coordinates("test.gpc", &rp).unwrap(); + return; + eprintln!("we go from {} to {}", p.len(), rp.len()); + + //TODO: assert we don't wrap around the globe + let (xmin, ymin, p) = convert_coordinates(&rp); + // let diffs = compress_coordinates(&p); + + // save_coordinates("test.gpc", xmin, ymin, &p).unwrap(); + + // // compress_coordinates(&p); + // let (xmin, xmax) = p + // .iter() + // .map(|&(x, _)| x) + // .minmax_by(|a, b| a.partial_cmp(b).unwrap()) + // .into_option() + // .unwrap(); + + // let (ymin, ymax) = p + // .iter() + // .map(|&(_, y)| y) + // .minmax_by(|a, b| a.partial_cmp(b).unwrap()) + // .into_option() + // .unwrap(); + + // println!( + // "", + // xmin, + // ymin, + // xmax - xmin, + // ymax - ymin + // ); + // print!( + // "", + // xmin, + // ymin, + // xmax - xmin, + // ymax - ymin + // ); + // print!(""); + // println!(""); +}