diff --git a/common/Cargo.lock b/common/Cargo.lock index b615bc6b..7a3e9817 100644 --- a/common/Cargo.lock +++ b/common/Cargo.lock @@ -325,6 +325,31 @@ dependencies = [ "libc", ] +[[package]] +name = "crossbeam-deque" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "613f8cc01fe9cf1a3eb3d7f488fd2fa8388403e97039e2f73692932e291a770d" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22ec99545bb0ed0ea7bb9b8e1e9122ea386ff8a48c0922e43f36d45ab09e0e80" + [[package]] name = "crypto-common" version = "0.1.6" @@ -519,14 +544,15 @@ dependencies = [ [[package]] name = "geo" -version = "0.28.0" +version = "0.29.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f811f663912a69249fa620dcd2a005db7254529da2d8a0b23942e81f47084501" +checksum = "81d088357a9cc60cec8253b3578f6834b4a3aa20edb55f5d1c030c36d8143f11" dependencies = [ "earcutr", "float_next_after", "geo-types", "geographiclib-rs", + "i_overlay", "log", "num-traits", "robust", @@ -626,6 +652,50 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" +[[package]] +name = "i_float" +version = "1.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f5fe043aae28ce70bd2f78b2f5f82a3654d63607c82594da4dabb8b6cb81f2b2" +dependencies = [ + "serde", +] + +[[package]] +name = "i_key_sort" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "347c253b4748a1a28baf94c9ce133b6b166f08573157e05afe718812bc599fcd" + +[[package]] +name = "i_overlay" +version = "1.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a469f68cb8a7cef375b2b0f581faf5859b4b50600438c00d46b71acc25ebbd0c" +dependencies = [ + "i_float", + "i_key_sort", + "i_shape", + "i_tree", + "rayon", +] + +[[package]] +name = "i_shape" +version = "1.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b44852d57a991c7dedaf76c55bc44f677f547ff899a430d29e13efd6133d7d8" +dependencies = [ + "i_float", + "serde", +] + +[[package]] +name = "i_tree" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "155181bc97d770181cf9477da51218a19ee92a8e5be642e796661aee2b601139" + [[package]] name = "indexmap" version = "2.6.0" @@ -953,6 +1023,26 @@ dependencies = [ "rand_core", ] +[[package]] +name = "rayon" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b418a60154510ca1a002a752ca9714984e21e4241e804d32555251faf8b78ffa" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + [[package]] name = "regex" version = "1.11.0" diff --git a/common/ferrostar/Cargo.toml b/common/ferrostar/Cargo.toml index 5a620391..60e59d6e 100644 --- a/common/ferrostar/Cargo.toml +++ b/common/ferrostar/Cargo.toml @@ -28,7 +28,7 @@ wasm_js = [ ] [dependencies] -geo = "0.28.0" +geo = "0.29.0" polyline = "0.11.0" serde = { version = "1.0.210", features = ["derive"] } serde_json = { version = "1.0.128", default-features = false } diff --git a/common/ferrostar/src/algorithms.rs b/common/ferrostar/src/algorithms.rs index 83852350..7f9f7b15 100644 --- a/common/ferrostar/src/algorithms.rs +++ b/common/ferrostar/src/algorithms.rs @@ -12,8 +12,8 @@ use crate::{ navigation_controller::models::TripProgress, }; use geo::{ - Closest, ClosestPoint, Coord, EuclideanDistance, GeodesicBearing, HaversineDistance, - HaversineLength, LineLocatePoint, LineString, Point, + Bearing, Closest, Coord, Distance, Euclidean, Geodesic, Haversine, HaversineClosestPoint, + Length, LineLocatePoint, LineString, Point, }; #[cfg(test)] @@ -28,23 +28,6 @@ use std::time::SystemTime; #[cfg(all(test, feature = "web-time"))] use web_time::SystemTime; -/// Normalizes a bearing returned from several `geo` crate functions, -/// which may be negative, into a positive unsigned integer. -/// -/// NOTE: This function assumes that the input values are in the range -360 to +360, -/// and does not check the inputs for validity. -pub(crate) fn normalize_bearing(degrees: f64) -> u16 { - let rounded = degrees.round(); - let normalized = if rounded < 0.0 { - rounded + 360.0 - } else if rounded >= 360.0 { - rounded - 360.0 - } else { - rounded - }; - normalized.round() as u16 -} - /// Get the index of the closest *segment* to the user's location within a [`LineString`]. /// /// A [`LineString`] is a set of points (ex: representing the geometry of a maneuver), @@ -64,10 +47,13 @@ pub fn index_of_closest_segment_origin(location: UserLocation, line: &LineString // Iterate through all segments of the line .enumerate() // Find the line segment closest to the user's location - .min_by(|(_, line1), (_, line2)| { + .min_by(|(_, line_segment_1), (_, line_segment_2)| { // Note: lines don't implement haversine distances - let dist1 = line1.euclidean_distance(&point); - let dist2 = line2.euclidean_distance(&point); + // In case you're tempted to say that this looks like cross track distance, + // note that the Line type here is actually a line *segment*, + // and we actually want to find the closest segment, not the closest mathematical line. + let dist1 = Euclidean::distance(line_segment_1, &point); + let dist2 = Euclidean::distance(line_segment_2, &point); dist1.total_cmp(&dist2) }) .map(|(index, _)| index as u64) @@ -85,13 +71,8 @@ fn get_bearing_to_next_point( let current = points.next()?; let next = points.next()?; - // This function may return negative bearing values, but we want to always normalize to [0, 360) - let degrees = normalize_bearing(current.geodesic_bearing(next)); - - Some(CourseOverGround { - degrees, - accuracy: None, - }) + let degrees = Geodesic::bearing(current, next); + Some(CourseOverGround::new(degrees, None)) } /// Apply a snapped course to a user location. @@ -161,7 +142,7 @@ fn snap_point_to_line(point: &Point, line: &LineString) -> Option { // Bail early when we have two essentially identical points. // This can cause some issues with edge cases (captured in proptest regressions) // with the underlying libraries. - if line.euclidean_distance(point) < 0.000_001 { + if Euclidean::distance(line, point) < 0.000_001 { return Some(*point); } @@ -170,8 +151,7 @@ fn snap_point_to_line(point: &Point, line: &LineString) -> Option { return None; } - // TODO: Use haversine_closest_point once a new release is cut which doesn't panic on intersections - match line.closest_point(point) { + match line.haversine_closest_point(point) { Closest::Intersection(snapped) | Closest::SinglePoint(snapped) => { let (x, y) = (snapped.x(), snapped.y()); if is_valid_float(x) && is_valid_float(y) { @@ -210,7 +190,7 @@ fn snap_point_to_line(point: &Point, line: &LineString) -> Option { /// }; /// let off_line = point! { /// x: 1.0, -/// y: 0.5, +/// y: 0.5, /// }; /// /// // The origin is directly on the line @@ -220,13 +200,14 @@ fn snap_point_to_line(point: &Point, line: &LineString) -> Option { /// assert_eq!(deviation_from_line(&midpoint, &linestring), Some(0.0)); /// /// // This point, however is off the line. -/// // That's a huge number, because we're dealing with degrees ;) +/// // That's a huge number, because we're dealing with points jumping by degrees ;) +/// println!("{:?}", deviation_from_line(&off_line, &linestring)); /// assert!(deviation_from_line(&off_line, &linestring) -/// .map_or(false, |deviation| deviation - 39312.21257675703 < f64::EPSILON)); +/// .map_or(false, |deviation| deviation - 39316.14208341989 < f64::EPSILON)); /// ``` pub fn deviation_from_line(point: &Point, line: &LineString) -> Option { snap_point_to_line(point, line).and_then(|snapped| { - let distance = snapped.haversine_distance(point); + let distance = Haversine::distance(snapped, *point); if distance.is_nan() || distance.is_infinite() { None @@ -243,7 +224,7 @@ fn is_close_enough_to_end_of_linestring( ) -> bool { if let Some(end_coord) = current_step_linestring.coords().last() { let end_point = Point::from(*end_coord); - let distance_to_end = end_point.haversine_distance(current_position); + let distance_to_end = Haversine::distance(end_point, *current_position); distance_to_end <= threshold } else { @@ -310,8 +291,8 @@ pub fn should_advance_to_next_step( // If the user's distance to the snapped location on the *next* step is <= // the user's distance to the snapped location on the *current* step, // advance to the next step - current_position.haversine_distance(&next_step_closest_point) - <= current_position.haversine_distance(¤t_step_closest_point) + Haversine::distance(current_position, next_step_closest_point) + <= Haversine::distance(current_position, current_step_closest_point) } else { // The user's location couldn't be mapped to a single point on both the current and next step. // Fall back to the distance to end of step mode, which has some graceful fallbacks. @@ -366,7 +347,7 @@ pub(crate) fn advance_step(remaining_steps: &[RouteStep]) -> StepAdvanceStatus { /// The result is given in meters. /// The result may be [`None`] in case of invalid input such as infinite floats. fn distance_along(point: &Point, linestring: &LineString) -> Option { - let total_length = linestring.haversine_length(); + let total_length = linestring.length::(); if total_length == 0.0 { return Some(0.0); } @@ -379,9 +360,9 @@ fn distance_along(point: &Point, linestring: &LineString) -> Option { // Compute distance to the line (sadly Euclidean only; no haversine_distance in GeoRust // but this is probably OK for now) - let segment_distance_to_point = segment.euclidean_distance(point); + let segment_distance_to_point = Euclidean::distance(&segment, point); // Compute total segment length in meters - let segment_length = segment_linestring.haversine_length(); + let segment_length = segment_linestring.length::(); if segment_distance_to_point < closest_dist_to_point { let segment_fraction = segment.line_locate_point(point)?; @@ -410,7 +391,7 @@ fn distance_to_end_of_step( snapped_location: &Point, current_step_linestring: &LineString, ) -> Option { - let step_length = current_step_linestring.haversine_length(); + let step_length = current_step_linestring.length::(); distance_along(snapped_location, current_step_linestring) .map(|traversed| step_length - traversed) } @@ -536,7 +517,7 @@ proptest! { prop_assert!(is_valid_float(x) || (!is_valid_float(x1) && x == x1)); prop_assert!(is_valid_float(y) || (!is_valid_float(y1) && y == y1)); - prop_assert!(line.euclidean_distance(&snapped) < 0.000001); + prop_assert!(Euclidean::distance(&line, &snapped) < 0.000001); } else { // Edge case 1: extremely small differences in values let is_miniscule_difference = (x1 - x2).abs() < 0.00000001 || (y1 - y2).abs() < 0.00000001; @@ -635,7 +616,7 @@ proptest! { speed: None }; let user_location_point = Point::from(user_location); - let distance_from_end_of_current_step = user_location_point.haversine_distance(&end_of_step.into()); + let distance_from_end_of_current_step = Haversine::distance(user_location_point, end_of_step.into()); // Never advance to the next step when StepAdvanceMode is Manual prop_assert!(!should_advance_to_next_step(¤t_route_step.get_linestring(), next_route_step.as_ref(), &user_location, StepAdvanceMode::Manual)); @@ -730,12 +711,6 @@ proptest! { prop_assert!(index < (coord_len - 1) as u64); } - #[test] - fn test_bearing_correction_valid_range(bearing in -360f64..360f64) { - let result = normalize_bearing(bearing); - prop_assert!(result < 360); - } - #[test] fn test_bearing_fuzz(coords in vec(arb_coord(), 2..500), index in 0usize..1_000usize) { let line = LineString::new(coords); diff --git a/common/ferrostar/src/models.rs b/common/ferrostar/src/models.rs index 694f6ff2..176c2890 100644 --- a/common/ferrostar/src/models.rs +++ b/common/ferrostar/src/models.rs @@ -171,8 +171,18 @@ pub struct CourseOverGround { } impl CourseOverGround { - pub fn new(degrees: u16, accuracy: Option) -> Self { - Self { degrees, accuracy } + /// # Arguments + /// + /// - degrees: The direction in which the user's device is traveling, measured in clockwise degrees from + /// true north (N = 0, E = 90, S = 180, W = 270). + /// NOTE: Input values must lie in the range [0, 360). + /// - accuracy: the accuracy of the course value, measured in degrees. + pub fn new(degrees: f64, accuracy: Option) -> Self { + debug_assert!(degrees >= 0.0 && degrees < 360.0); + Self { + degrees: degrees.round() as u16, + accuracy, + } } } diff --git a/common/ferrostar/src/navigation_controller/mod.rs b/common/ferrostar/src/navigation_controller/mod.rs index 141d1659..cb1a8bec 100644 --- a/common/ferrostar/src/navigation_controller/mod.rs +++ b/common/ferrostar/src/navigation_controller/mod.rs @@ -12,7 +12,10 @@ use crate::{ }, models::{Route, UserLocation}, }; -use geo::{HaversineDistance, LineString, Point}; +use geo::{ + algorithm::{Distance, Haversine}, + geometry::{LineString, Point}, +}; use models::{NavigationControllerConfig, StepAdvanceStatus, TripState}; use std::clone::Clone; @@ -125,7 +128,7 @@ impl NavigationController { let next_waypoint: Point = waypoint.coordinate.into(); // TODO: This is just a hard-coded threshold for the time being. // More sophisticated behavior will take some time and use cases, so punting on this for now. - current_location.haversine_distance(&next_waypoint) < 100.0 + Haversine::distance(current_location, next_waypoint) < 100.0 } else { false }; diff --git a/common/ferrostar/src/navigation_controller/test_helpers.rs b/common/ferrostar/src/navigation_controller/test_helpers.rs index fd89e4d1..ca39f075 100644 --- a/common/ferrostar/src/navigation_controller/test_helpers.rs +++ b/common/ferrostar/src/navigation_controller/test_helpers.rs @@ -1,7 +1,7 @@ use crate::models::{BoundingBox, GeographicCoordinate, Route, RouteStep, Waypoint, WaypointKind}; #[cfg(feature = "alloc")] use alloc::string::ToString; -use geo::{line_string, BoundingRect, HaversineLength, LineString, Point}; +use geo::{line_string, BoundingRect, Haversine, Length, LineString, Point}; pub fn gen_dummy_route_step( start_lng: f64, @@ -24,7 +24,7 @@ pub fn gen_dummy_route_step( (x: start_lng, y: start_lat), (x: end_lng, y: end_lat) ] - .haversine_length(), + .length::(), duration: 0.0, road_name: None, instruction: "".to_string(), diff --git a/common/ferrostar/src/simulation.rs b/common/ferrostar/src/simulation.rs index 78f009a1..dc2f42b0 100644 --- a/common/ferrostar/src/simulation.rs +++ b/common/ferrostar/src/simulation.rs @@ -38,9 +38,9 @@ //! # } //! ``` -use crate::algorithms::{normalize_bearing, trunc_float}; +use crate::algorithms::trunc_float; use crate::models::{CourseOverGround, GeographicCoordinate, Route, UserLocation}; -use geo::{coord, DensifyHaversine, GeodesicBearing, LineString, Point}; +use geo::{coord, Bearing, Densify, Geodesic, Haversine, LineString, Point}; use polyline::decode_polyline; #[cfg(any(test, feature = "wasm-bindgen"))] @@ -101,14 +101,11 @@ pub fn location_simulation_from_coordinates( if let Some(next) = rest.first() { let current_point = Point::from(*current); let next_point = Point::from(*next); - let bearing = current_point.geodesic_bearing(next_point); + let bearing = Geodesic::bearing(current_point, next_point); let current_location = UserLocation { coordinates: *current, horizontal_accuracy: 0.0, - course_over_ground: Some(CourseOverGround { - degrees: bearing.round() as u16, - accuracy: None, - }), + course_over_ground: Some(CourseOverGround::new(bearing, None)), timestamp: SystemTime::now(), speed: None, }; @@ -125,7 +122,7 @@ pub fn location_simulation_from_coordinates( }) .collect(); let linestring: LineString = coords.into(); - let densified_linestring = linestring.densify_haversine(distance); + let densified_linestring = linestring.densify::(distance); densified_linestring .points() .map(|point| GeographicCoordinate { @@ -199,15 +196,11 @@ pub fn advance_location_simulation(state: &LocationSimulationState) -> LocationS if let Some((next_coordinate, rest)) = state.remaining_locations.split_first() { let current_point = Point::from(state.current_location.coordinates); let next_point = Point::from(*next_coordinate); - let bearing = normalize_bearing(current_point.geodesic_bearing(next_point)); - + let bearing = Geodesic::bearing(current_point, next_point); let next_location = UserLocation { coordinates: *next_coordinate, horizontal_accuracy: 0.0, - course_over_ground: Some(CourseOverGround { - degrees: bearing, - accuracy: None, - }), + course_over_ground: Some(CourseOverGround::new(bearing, None)), timestamp: SystemTime::now(), speed: None, }; @@ -277,7 +270,7 @@ pub fn js_advance_location_simulation(state: JsValue) -> JsValue { mod tests { use super::*; use crate::algorithms::snap_user_location_to_line; - use geo::HaversineDistance; + use geo::{Distance, Haversine}; use rstest::rstest; #[rstest] @@ -348,7 +341,7 @@ mod tests { // The distance between each point in the simulation should be <= max_distance let current_point: Point = state.current_location.into(); let next_point: Point = new_state.current_location.into(); - let distance = current_point.haversine_distance(&next_point); + let distance = Haversine::distance(current_point, next_point); // I'm actually not 100% sure why this extra fudge is needed, but it's not a concern for today. assert!( distance <= max_distance + 7.0, @@ -358,7 +351,7 @@ mod tests { let snapped = snap_user_location_to_line(new_state.current_location, &original_linestring); let snapped_point: Point = snapped.coordinates.into(); - let distance = next_point.haversine_distance(&snapped_point); + let distance = Haversine::distance(next_point, snapped_point); assert!( distance <= max_distance, "Expected snapped point to be on the line; was {distance}m away" diff --git a/common/ferrostar/src/snapshots/ferrostar__simulation__tests__state_from_polyline.snap b/common/ferrostar/src/snapshots/ferrostar__simulation__tests__state_from_polyline.snap index 41bd94f3..adcf7afe 100644 --- a/common/ferrostar/src/snapshots/ferrostar__simulation__tests__state_from_polyline.snap +++ b/common/ferrostar/src/snapshots/ferrostar__simulation__tests__state_from_polyline.snap @@ -1,5 +1,6 @@ --- source: ferrostar/src/simulation.rs +assertion_line: 322 expression: state --- current_location: @@ -8,7 +9,7 @@ current_location: lng: -149.543469 horizontal_accuracy: 0 course_over_ground: - degrees: 0 + degrees: 288 accuracy: ~ speed: ~ remaining_locations: