Skip to content

Commit

Permalink
Cleanup from initial review
Browse files Browse the repository at this point in the history
  • Loading branch information
@ianthetechie
ianthetechie committed Sep 11, 2024
1 parent 90d6402 commit 50832c2
Show file tree
Hide file tree
Showing 4 changed files with 140 additions and 59 deletions.
1 change: 1 addition & 0 deletions common/ferrostar/proptest-regressions/algorithms.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,3 +12,4 @@ cc 08a3370b3e481e130a11a6a81deed2d5a2d30341a7edeadb7e18517cf638897f # shrinks to
cc 9343327e3112d7e3337bda8d95349a10f7882025f43fa76201929eba8deaa87d # shrinks to x1 = 0.0003739224474886329, y1 = 2.15972600670539e-309, x2 = 0.0, y2 = 3.234338053641099e-10
cc b87a314ecd2957021a836398aa55a516f91d5fd526c2a4f9e5a57a42e8f597ae # shrinks to x1 = 1.7321418387536385, y1 = 1.3433541773527303e-308, x2 = 0.0, y2 = 17.833686113757857
cc 9fa5f4eafafbffedde0d3fdaf6ac83f92c4aae3c9382d58855c07b562e1c59f1 # shrinks to x1 = 8.250574670883433e29, y1 = 0.0, x2 = 0.0, y2 = 7.980493463728753e-93, x3 = 0.0, y3 = 0.0, has_next_step = true, distance = 0, minimum_horizontal_accuracy = 0, excess_inaccuracy = 0.0, automatic_advance_distance = None
cc 561ce71db95074e5505c8db3e95cb5d0c5b14a2e38e137dc7e72954dadc1608b # shrinks to bearing = 359.65552398004445
160 changes: 121 additions & 39 deletions common/ferrostar/src/algorithms.rs
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,22 @@
//! Common spatial algorithms which are useful for navigation.
use crate::{models::CourseOverGround, navigation_controller::models::{
StepAdvanceMode, StepAdvanceStatus::{self, Advanced, EndOfRoute},
}};
use crate::{
models::CourseOverGround,
navigation_controller::models::{
StepAdvanceMode,
StepAdvanceStatus::{self, Advanced, EndOfRoute},
},
};
use crate::{
models::{GeographicCoordinate, RouteStep, UserLocation},
navigation_controller::models::TripProgress,
};
use geo::{
Closest, ClosestPoint, Coord, EuclideanDistance, GeodesicBearing, HaversineDistance, HaversineLength, LineLocatePoint, LineString, Point
};
use geo::{Closest, ClosestPoint, Coord, EuclideanDistance, GeodesicBearing, HaversineDistance, HaversineLength, LineLocatePoint, LineString, Point};

#[cfg(test)]
use {
crate::navigation_controller::test_helpers::gen_dummy_route_step,
geo::{coord, point},
geo::{coord, point, CoordsIter},
proptest::{collection::vec, prelude::*},
};

Expand All @@ -23,6 +25,23 @@ 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),
Expand Down Expand Up @@ -52,25 +71,19 @@ pub fn index_of_closest_segment_origin(location: UserLocation, line: &LineString
}

/// Get the bearing to the next point on the LineString.
///
/// Returns [`None`] if the index is the last point on the LineString.
pub fn get_bearing_to_next_point(index_along_line: u64, line: &LineString) -> Option<CourseOverGround> {
let points = line.clone()
.into_inner()
.into_iter()
.map(|coord| Point::from(coord)).collect::<Vec<Point>>();

if (index_along_line as usize) == points.len() - 1 {
return None;
}
///
/// Returns [`None`] if the index points at or past the last point in the LineString.
fn get_bearing_to_next_point(
index_along_line: usize,
line: &LineString,
) -> Option<CourseOverGround> {
let mut points = line.points().skip(index_along_line);

let current = points[index_along_line as usize];
let next = points[index_along_line as usize + 1];
let current = points.next()?;
let next = points.next()?;

let mut degrees = current.geodesic_bearing(next);
if degrees < 0.0 {
degrees += 360.0;
}
// 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: degrees as u16,
Expand All @@ -79,12 +92,20 @@ pub fn get_bearing_to_next_point(index_along_line: u64, line: &LineString) -> Op
}

/// Apply a snapped course to a user location.
///
/// This function sets the location's course over ground to the snapped course (calculated from the route),
/// and falls back on the raw location engine course or `None` if no course is available.
pub fn apply_snapped_course(location: UserLocation, index_along_line: Option<u64>, line: &LineString) -> UserLocation {
let snapped_course = index_along_line
.and_then(|index| get_bearing_to_next_point(index, line));
///
/// This function snaps the course to travel along the provided line,
/// starting from the given coordinate index along the line.
///
/// If the given index is None or out of bounds, the original location will be returned unmodified.
/// `index_along_line` is optional to improve ergonomics elsewhere in the codebase,
/// despite the API looking a little funny.
pub fn apply_snapped_course(
location: UserLocation,
index_along_line: Option<u64>,
line: &LineString,
) -> UserLocation {
let snapped_course =
index_along_line.and_then(|index| get_bearing_to_next_point(index as usize, line));

let course_over_ground = snapped_course.or(location.course_over_ground);

Expand Down Expand Up @@ -705,6 +726,31 @@ proptest! {
// We should never be able to go past the origin of the final pair
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);
let result = get_bearing_to_next_point(index, &line);
if index < line.coords_count() - 1 {
prop_assert!(result.is_some());
} else {
prop_assert!(result.is_none());
}
}

#[test]
fn test_bearing_end_of_line(coords in vec(arb_coord(), 2..500)) {
let line = LineString::new(coords);
prop_assert!(get_bearing_to_next_point(line.coords_count(), &line).is_none());
prop_assert!(get_bearing_to_next_point(line.coords_count() - 1, &line).is_none());
prop_assert!(get_bearing_to_next_point(line.coords_count() - 2, &line).is_some());
}
}

#[cfg(test)]
Expand Down Expand Up @@ -756,7 +802,6 @@ mod linestring_based_tests {
let index = index_of_closest_segment_origin(make_user_location(10.0, 10.0), &line);
assert_eq!(index, Some(3));
}

}

#[cfg(test)]
Expand All @@ -778,19 +823,49 @@ mod bearing_snapping_tests {
let line = LineString::new(COORDS.to_vec());

let bearing = get_bearing_to_next_point(0, &line);
assert_eq!(bearing, Some(CourseOverGround { degrees: 45, accuracy: None }));
assert_eq!(
bearing,
Some(CourseOverGround {
degrees: 45,
accuracy: None
})
);

let bearing = get_bearing_to_next_point(1, &line);
assert_eq!(bearing, Some(CourseOverGround { degrees: 89, accuracy: None }));
assert_eq!(
bearing,
Some(CourseOverGround {
degrees: 90,
accuracy: None
})
);

let bearing = get_bearing_to_next_point(2, &line);
assert_eq!(bearing, Some(CourseOverGround { degrees: 0, accuracy: None }));
assert_eq!(
bearing,
Some(CourseOverGround {
degrees: 0,
accuracy: None
})
);

let bearing = get_bearing_to_next_point(3, &line);
assert_eq!(bearing, Some(CourseOverGround { degrees: 180, accuracy: None }));
assert_eq!(
bearing,
Some(CourseOverGround {
degrees: 180,
accuracy: None
})
);

let bearing = get_bearing_to_next_point(4, &line);
assert_eq!(bearing, Some(CourseOverGround { degrees: 270, accuracy: None }));
assert_eq!(
bearing,
Some(CourseOverGround {
degrees: 270,
accuracy: None
})
);

// At the end
let bearing = get_bearing_to_next_point(5, &line);
Expand All @@ -801,14 +876,21 @@ mod bearing_snapping_tests {
fn test_apply_snapped_course() {
let line = LineString::new(COORDS.to_vec());

let user_location = make_user_location(1.0, 1.0);
// The value of the coordinates does not actually matter;
// we are testing the course snapping
let user_location = make_user_location(5.0, 1.0);

// Apply a course to a user location
let updated_location = apply_snapped_course(user_location, Some(1), &line);

assert_eq!(updated_location.course_over_ground, Some(CourseOverGround { degrees: 89, accuracy: None }));
assert_eq!(
updated_location.course_over_ground,
Some(CourseOverGround {
degrees: 90,
accuracy: None
})
);
}

}

// TODO: Other unit tests
Expand Down
29 changes: 15 additions & 14 deletions common/ferrostar/src/navigation_controller/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@ pub(crate) mod test_helpers;

use crate::{
algorithms::{
advance_step, apply_snapped_course, calculate_trip_progress, index_of_closest_segment_origin, should_advance_to_next_step, snap_user_location_to_line
advance_step, apply_snapped_course, calculate_trip_progress,
index_of_closest_segment_origin, should_advance_to_next_step, snap_user_location_to_line,
},
models::{Route, UserLocation},
};
Expand Down Expand Up @@ -50,11 +51,11 @@ impl NavigationController {
let current_step_linestring = current_route_step.get_linestring();
let snapped_user_location = snap_user_location_to_line(location, &current_step_linestring);
let current_step_geometry_index =
index_of_closest_segment_origin(snapped_user_location, &current_step_linestring);
index_of_closest_segment_origin(snapped_user_location, &current_step_linestring);
let snapped_user_location_with_course = apply_snapped_course(
snapped_user_location,
current_step_geometry_index,
&current_step_linestring
snapped_user_location,
current_step_geometry_index,
&current_step_linestring,
);

let progress = calculate_trip_progress(
Expand Down Expand Up @@ -145,9 +146,9 @@ impl NavigationController {
&self.route.get_linestring(),
);
let snapped_user_location_with_course = apply_snapped_course(
*snapped_user_location,
current_step_geometry_index,
&linestring
*snapped_user_location,
current_step_geometry_index,
&linestring,
);

let visual_instruction = current_step
Expand Down Expand Up @@ -210,9 +211,9 @@ impl NavigationController {
let snapped_user_location =
snap_user_location_to_line(location, &current_step_linestring);
let snapped_user_location_with_course = apply_snapped_course(
snapped_user_location,
*current_step_geometry_index,
&current_step_linestring
snapped_user_location,
*current_step_geometry_index,
&current_step_linestring,
);

let progress = calculate_trip_progress(
Expand Down Expand Up @@ -278,9 +279,9 @@ impl NavigationController {
&current_step_linestring,
);
let snapped_user_location_with_course = apply_snapped_course(
snapped_user_location,
current_step_geometry_index,
&current_step_linestring
snapped_user_location,
current_step_geometry_index,
&current_step_linestring,
);

TripState::Navigating {
Expand Down
9 changes: 3 additions & 6 deletions common/ferrostar/src/simulation.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
//! # }
//! ```
use crate::algorithms::trunc_float;
use crate::algorithms::{normalize_bearing, trunc_float};
use crate::models::{CourseOverGround, GeographicCoordinate, Route, UserLocation};
use geo::{coord, DensifyHaversine, GeodesicBearing, LineString, Point};
use polyline::decode_polyline;
Expand Down Expand Up @@ -193,16 +193,13 @@ 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 mut bearing = current_point.geodesic_bearing(next_point);
if bearing < 0.0 {
bearing += 360.0;
}
let bearing = normalize_bearing(current_point.geodesic_bearing(next_point));

let next_location = UserLocation {
coordinates: *next_coordinate,
horizontal_accuracy: 0.0,
course_over_ground: Some(CourseOverGround {
degrees: bearing.round() as u16,
degrees: bearing,
accuracy: None,
}),
timestamp: SystemTime::now(),
Expand Down

0 comments on commit 50832c2

Please sign in to comment.