diff --git a/gpx-rs/src/algorithms/mod.rs b/gpx-rs/src/algorithms/mod.rs new file mode 100644 index 000000000..ea1c6c971 --- /dev/null +++ b/gpx-rs/src/algorithms/mod.rs @@ -0,0 +1,5 @@ +mod simplify; +mod smooth; + +pub use simplify::*; +pub use smooth::*; diff --git a/gpx-rs/src/algorithms/simplify.rs b/gpx-rs/src/algorithms/simplify.rs new file mode 100644 index 000000000..1232dfc40 --- /dev/null +++ b/gpx-rs/src/algorithms/simplify.rs @@ -0,0 +1,42 @@ +use crate::gpx::TrackPoint; + +pub fn ramer_douglas_peucker(n: usize, distance: &F, epsilon: f64) -> Vec +where + F: Fn(usize, usize, usize) -> f64, +{ + if n <= 2 { + (0..n).collect() + } else { + let mut indices = vec![0]; + ramer_douglas_peucker_helper(0, n - 1, distance, epsilon, &mut indices); + indices.push(n - 1); + indices + } +} + +fn ramer_douglas_peucker_helper( + start: usize, + end: usize, + distance: &F, + epsilon: f64, + indices: &mut Vec, +) where + F: Fn(usize, usize, usize) -> f64, +{ + let mut idx = 0; + let mut max_dist = 0.0; + + for i in (start + 1)..end { + let dist = distance(start, end, i); + if dist > max_dist { + idx = i; + max_dist = dist; + } + } + + if max_dist > epsilon && idx != 0 { + ramer_douglas_peucker_helper(start, idx, distance, epsilon, indices); + indices.push(idx); + ramer_douglas_peucker_helper(idx, end, distance, epsilon, indices); + } +} diff --git a/gpx-rs/src/algorithms/smooth.rs b/gpx-rs/src/algorithms/smooth.rs new file mode 100644 index 000000000..40d824692 --- /dev/null +++ b/gpx-rs/src/algorithms/smooth.rs @@ -0,0 +1,39 @@ +#[macro_export] +macro_rules! for_each_window { + ( + $left:expr, + $right:expr, + $window:expr, + |$a:ident, $b:ident| $distance:expr, + |$i:ident, $l:ident, $r:ident| $body:block, + ) => {{ + let mut start = $left; + + for $i in $left..$right { + while start + 1 < $i && { + let $a = start; + let $b = $i; + $distance + } > $window + { + start += 1; + } + + let mut end = $right.min($i + 2); + + while end < $right && { + let $a = $i; + let $b = end; + $distance + } <= $window + { + end += 1; + } + + let $l = start; + let $r = end - 1; + + $body + } + }}; +} diff --git a/gpx-rs/src/gpx/common.rs b/gpx-rs/src/gpx/common.rs index f025fe792..ff2593021 100644 --- a/gpx-rs/src/gpx/common.rs +++ b/gpx-rs/src/gpx/common.rs @@ -4,7 +4,7 @@ pub struct Link { pub text: Option, } -#[derive(Debug, Default)] +#[derive(Debug, Default, Clone, Copy)] pub struct LngLat { pub lng: f64, pub lat: f64, diff --git a/gpx-rs/src/gpx/file.rs b/gpx-rs/src/gpx/file.rs index 6107fbcad..092cccf55 100644 --- a/gpx-rs/src/gpx/file.rs +++ b/gpx-rs/src/gpx/file.rs @@ -7,6 +7,7 @@ pub struct GPXFile { pub info: GPXFileInfo, pub trk: Vec, pub wpt: Vec>, + // TODO routes } #[derive(Debug, Default)] diff --git a/gpx-rs/src/gpx/segment.rs b/gpx-rs/src/gpx/segment.rs index 770ee4c99..2e9272bec 100644 --- a/gpx-rs/src/gpx/segment.rs +++ b/gpx-rs/src/gpx/segment.rs @@ -7,6 +7,45 @@ pub struct TrackSegment { pub chunks: Vec>, } +impl TrackSegment { + pub fn iter(&self) -> TrackSegmentIterator { + TrackSegmentIterator::new(self) + } +} + +pub struct TrackSegmentIterator<'a> { + trkseg: &'a TrackSegment, + chunk_idx: usize, + trkpt_idx: usize, +} + +impl<'a> TrackSegmentIterator<'a> { + pub fn new(trkseg: &'a TrackSegment) -> Self { + Self { + trkseg, + chunk_idx: 0, + trkpt_idx: 0, + } + } +} + +impl<'a> Iterator for TrackSegmentIterator<'a> { + type Item = &'a TrackPoint; + + fn next(&mut self) -> Option { + if self.chunk_idx >= self.trkseg.chunks.len() { + None + } else if self.trkpt_idx >= self.trkseg.chunks[self.chunk_idx].trkpt.len() { + self.chunk_idx += 1; + self.trkpt_idx = 0; + self.next() + } else { + self.trkpt_idx += 1; + Some(&self.trkseg.chunks[self.chunk_idx].trkpt[self.trkpt_idx - 1]) + } + } +} + static MAX_CHUNK_SIZE: usize = 4096; #[derive(Debug, Default)] diff --git a/gpx-rs/src/lib.rs b/gpx-rs/src/lib.rs index d8c1f9054..1f901dc6f 100644 --- a/gpx-rs/src/lib.rs +++ b/gpx-rs/src/lib.rs @@ -4,3 +4,4 @@ mod gpx; mod stack; mod statistics; mod utils; +mod algorithms; diff --git a/gpx-rs/src/statistics.rs b/gpx-rs/src/statistics.rs index 4545c2598..9d3f72f0e 100644 --- a/gpx-rs/src/statistics.rs +++ b/gpx-rs/src/statistics.rs @@ -1,13 +1,15 @@ use crate::{ + algorithms::ramer_douglas_peucker, + for_each_window, gpx::{LngLat, LngLatBounds, TrackPoint, TrackPointChunk}, - utils::{distance, speed}, + utils::{distance, slope, speed}, }; -#[derive(Default)] +#[derive(Default, Debug)] pub struct GPXStatistics { pub total_distance: f64, - pub moving_distance: f64, - pub moving_time: i64, + pub moving_distance: Option, + pub moving_time: Option, pub elevation_gain: f64, pub elevation_loss: f64, pub start_time: Option, @@ -17,6 +19,39 @@ pub struct GPXStatistics { } impl GPXStatistics { + pub fn total_time(&self) -> Option { + self.start_time.zip(self.end_time).map(|(t1, t2)| t2 - t1) + } + + pub fn total_speed(&self) -> Option { + self.total_time().map(|t| speed(self.total_distance, t)) + } + + pub fn moving_speed(&self) -> Option { + self.moving_distance + .zip(self.moving_time) + .map(|(d, t)| speed(d, t)) + } + + pub fn compute(chunk: &TrackPointChunk) -> Self { + let mut stats = Self::default(); + if chunk.trkpt.is_empty() { + return stats; + } + + let mut prev = &chunk.trkpt[0]; + for i in 0..chunk.trkpt.len() { + let cur = &chunk.trkpt[i]; + stats.accumulate(prev, cur); + prev = cur; + } + + stats.compute_smoothed_speed(chunk); + stats.compute_smoothed_elevation_gain(chunk); + + stats + } + fn accumulate(&mut self, prev: &TrackPoint, cur: &TrackPoint) { self.accumulate_distance_and_time(prev, cur); self.update_time_bounds(cur.time); @@ -26,7 +61,7 @@ impl GPXStatistics { } fn accumulate_distance_and_time(&mut self, prev: &TrackPoint, cur: &TrackPoint) { - let dist = distance(&prev.coordinates, &cur.coordinates); + let dist = distance(prev.coordinates, cur.coordinates); let time = prev.time.zip(cur.time).map(|(t1, t2)| t2 - t1); self.total_distance += dist; @@ -34,8 +69,8 @@ impl GPXStatistics { if let Some(time) = time { let speed = speed(dist, time); if speed >= 0.5 && speed <= 1500.0 { - self.moving_distance += dist; - self.moving_time += time; + self.moving_distance = self.moving_distance.map_or(Some(dist), |d| Some(d + dist)); + self.moving_time = self.moving_time.map_or(Some(time), |t| Some(t + time)); } } } @@ -55,21 +90,139 @@ impl GPXStatistics { self.bounds.ne.lng = self.bounds.ne.lng.min(coordinates.lng); self.bounds.ne.lat = self.bounds.ne.lat.min(coordinates.lat); } + + fn compute_smoothed_speed(&mut self, chunk: &TrackPointChunk) { + for_each_window!( + 0, + chunk.trkpt.len(), + Some(10000), + |i, j| { + chunk.trkpt[i] + .time + .zip(chunk.trkpt[j].time) + .map(|(t1, t2)| t2 - t1) + }, + |i, left, right| { + self.local[i].speed = + chunk.trkpt[left] + .time + .zip(chunk.trkpt[right].time) + .map(|(t1, t2)| { + speed( + self.local[right].total_distance - self.local[left].total_distance, + t2 - t1, + ) + }); + }, + ); + } + + fn compute_smoothed_elevation_gain(&mut self, chunk: &TrackPointChunk) { + let simplified = ramer_douglas_peucker( + chunk.trkpt.len(), + &|i, j, k| { + let x1 = self.local[i].total_distance * 1000.0; + let x2 = self.local[j].total_distance * 1000.0; + let x3 = self.local[k].total_distance * 1000.0; + let y1 = chunk.trkpt[i].ele; + let y2 = chunk.trkpt[j].ele; + let y3 = chunk.trkpt[k].ele; + + let dist = ((y2 - y1).powi(2) + (x2 - x1).powi(2)).sqrt(); + if dist == 0.0 { + ((x3 - x1).powi(2) + (y3 - y1).powi(2)).sqrt() + } else { + ((y2 - y1) * x3 - (x2 - x1) * y3 + x2 * y1 - y2 * x1).abs() / dist + } + }, + 20.0, + ); + + for i in 0..(simplified.len() - 1) { + let start = simplified[i]; + let end = simplified[i + 1]; + let last = i + 1 == simplified.len() - 1; + + let mut cumul_ele = 0.0; + let mut current_left = start; + let mut current_right = start; + let mut prev_smoothed_ele = chunk.trkpt[start].ele; + + for_each_window!( + start, + end, + 0.1, + |i, j| self.local[j].total_distance - self.local[i].total_distance, + |i, left, right| { + for i in current_left..left { + cumul_ele -= chunk.trkpt[i].ele; + } + for i in current_right..=right { + cumul_ele += chunk.trkpt[i].ele; + } + current_left = left; + current_right = right + 1; + + let smoothed_ele: f64 = if i == start || i == end { + chunk.trkpt[i].ele + } else { + cumul_ele / (right - left + 1) as f64 + }; + + let delta = smoothed_ele - prev_smoothed_ele; + if delta > 0.0 { + self.elevation_gain += delta; + } else if delta < 0.0 { + self.elevation_loss -= delta; + } + + if i < end || last { + self.local[i].elevation_gain = self.elevation_gain; + self.local[i].elevation_loss = self.elevation_loss; + } + + prev_smoothed_ele = smoothed_ele; + }, + ); + + let segment_dist = self.local[end].total_distance - self.local[start].total_distance; + let segment_ele = chunk.trkpt[end].ele - chunk.trkpt[start].ele; + let segment_slope = slope(segment_ele, segment_dist); + for k in start..(end + last as usize) { + self.local[k].slope_segment = SlopeSegment { + slope: segment_slope, + distance: segment_dist, + }; + } + } + + for_each_window!( + 0, + chunk.trkpt.len(), + 0.05, + |i, j| self.local[j].total_distance - self.local[i].total_distance, + |i, left, right| { + let dist = self.local[right].total_distance - self.local[left].total_distance; + let ele = chunk.trkpt[right].ele - chunk.trkpt[left].ele; + self.local[i].slope = slope(ele, dist); + }, + ); + } } -#[derive(Default)] +#[derive(Default, Debug)] pub struct SlopeSegment { pub slope: f64, pub distance: f64, } -#[derive(Default)] +#[derive(Default, Debug)] pub struct TrackpointStatistics { pub total_distance: f64, - pub moving_distance: f64, - pub total_time: i64, - pub moving_time: i64, - pub speed: f64, + pub moving_distance: Option, + pub total_time: Option, + pub moving_time: Option, + pub speed: Option, pub elevation_gain: f64, pub elevation_loss: f64, pub slope: f64, @@ -81,12 +234,9 @@ impl TrackpointStatistics { Self { total_distance: stats.total_distance, moving_distance: stats.moving_distance, - total_time: stats - .start_time - .zip(stats.end_time) - .map_or(0, |(t1, t2)| t2 - t1), + total_time: stats.start_time.zip(stats.end_time).map(|(t1, t2)| t2 - t1), moving_time: stats.moving_time, - speed: todo!(), + speed: None, elevation_gain: stats.elevation_gain, elevation_loss: stats.elevation_loss, slope: Default::default(), @@ -95,19 +245,24 @@ impl TrackpointStatistics { } } -impl GPXStatistics { - pub fn compute(chunk: &TrackPointChunk) -> Self { - let mut stats = Self::default(); - if chunk.trkpt.is_empty() { - return stats; - } +#[cfg(test)] +mod tests { + use std::{fs::File, io::Read}; - let mut prev = &chunk.trkpt[0]; - for i in 0..chunk.trkpt.len() { - let cur = &chunk.trkpt[i]; - stats.accumulate(prev, cur); - prev = cur; - } - stats + use crate::actions::parse; + + use super::*; + + #[test] + fn test_parse_simple() { + let mut f = File::open("data/with_time.gpx").unwrap(); + let mut data = String::new(); + let _ = f.read_to_string(&mut data); + let gpx = parse(data.as_bytes()).unwrap(); + + println!( + "{:?}", + GPXStatistics::compute(&gpx.trk[0].trkseg[0].chunks[0]) + ); } } diff --git a/gpx-rs/src/utils.rs b/gpx-rs/src/utils.rs index d453fe1c5..7ad73f2a6 100644 --- a/gpx-rs/src/utils.rs +++ b/gpx-rs/src/utils.rs @@ -6,11 +6,11 @@ static TO_RADIANS: f64 = PI / 180.0; static EARTH_RADIUS: f64 = 6371.0088; /// Computes the distance in kilometers between two coordinates using the Haversine formula -pub fn distance(coord1: &LngLat, coord2: &LngLat) -> f64 { - let lat1 = coord1.lat * TO_RADIANS; - let lat2 = coord2.lat * TO_RADIANS; +pub fn distance(p1: LngLat, p2: LngLat) -> f64 { + let lat1 = p1.lat * TO_RADIANS; + let lat2 = p2.lat * TO_RADIANS; let delta_lat = lat2 - lat1; - let delta_lng = (coord2.lng - coord1.lng) * TO_RADIANS; + let delta_lng = (p2.lng - p1.lng) * TO_RADIANS; let a = (delta_lat / 2.0).sin().powi(2) + lat1.cos() * lat2.cos() * (delta_lng / 2.0).sin().powi(2); @@ -22,3 +22,90 @@ pub fn distance(coord1: &LngLat, coord2: &LngLat) -> f64 { pub fn speed(distance: f64, time: i64) -> f64 { distance / (time as f64 / 3600_000.0) } + +pub fn slope(ele: f64, distance: f64) -> f64 { + if distance == 0.0 { + 100.0 + } else { + 0.1 * ele / distance + } +} + +static METERS_PER_LATITUDE_DEGREE: f64 = 111320.0; + +fn get_meters_per_longitude_degree(latitude: f64) -> f64 { + ((latitude * PI) / 180.0).cos() * METERS_PER_LATITUDE_DEGREE +} + +// Calculates the point on the line segment defined by p1 and p2 +// that is closest to the third point, p3. +// Uses simple planar geometry (ignores earth curvature). +fn projected(p1: LngLat, p2: LngLat, coord3: LngLat) -> LngLat { + // Convert to meters using approximate scaling + let meters_per_longitude_degree = get_meters_per_longitude_degree(p1.lat); + + let x1 = p1.lng * meters_per_longitude_degree; + let y1 = p1.lat * METERS_PER_LATITUDE_DEGREE; + let x2 = p2.lng * meters_per_longitude_degree; + let y2 = p2.lat * METERS_PER_LATITUDE_DEGREE; + let x3 = coord3.lng * meters_per_longitude_degree; + let y3 = coord3.lat * METERS_PER_LATITUDE_DEGREE; + + let dx = x2 - x1; + let dy = y2 - y1; + let segment_length_squared = dx * dx + dy * dy; + + if segment_length_squared == 0.0 { + // p1 and p2 are the same point + p1 + } else { + // Project p3 onto the line defined by p1-p2 + let t = + 0.0_f64.max(1.0_f64.min(((x3 - x1) * dx + (y3 - y1) * dy) / segment_length_squared)); + + // Find the closest point on the segment + let proj_x = x1 + t * dx; + let proj_y = y1 + t * dy; + + // Convert back to degrees + LngLat { + lng: proj_x / meters_per_longitude_degree, + lat: proj_y / METERS_PER_LATITUDE_DEGREE, + } + } +} + +/// Calculates the perpendicular distance in meters +/// between a line segment (defined by p1 and p2) and a third point, p3. +/// Uses simple planar geometry (ignores earth curvature). +fn crossarc(p1: LngLat, p2: LngLat, p3: LngLat) -> f64 { + // Convert to meters using approximate scaling + let meters_per_longitude_degree = get_meters_per_longitude_degree(p1.lat); + + let x1 = p1.lng * meters_per_longitude_degree; + let y1 = p1.lat * METERS_PER_LATITUDE_DEGREE; + let x2 = p2.lng * meters_per_longitude_degree; + let y2 = p2.lat * METERS_PER_LATITUDE_DEGREE; + let x3 = p3.lng * meters_per_longitude_degree; + let y3 = p3.lat * METERS_PER_LATITUDE_DEGREE; + + let dx = x2 - x1; + let dy = y2 - y1; + let segment_length_squared = dx * dx + dy * dy; + + if segment_length_squared == 0.0 { + // p1 and p2 are the same point + ((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1)).sqrt() + } else { + // Project p3 onto the line defined by p1 - p2 + let t = + 0.0_f64.max(1.0_f64.min(((x3 - x1) * dx + (y3 - y1) * dy) / segment_length_squared)); + + // Find the closest point on the segment + let proj_x = x1 + t * dx; + let proj_y = y1 + t * dy; + + // Return distance from p3 to the projected point + ((x3 - proj_x) * (x3 - proj_x) + (y3 - proj_y) * (y3 - proj_y)).sqrt() + } +}