feat: starting point for course snapping

android/core/src/main/java/com/stadiamaps/ferrostar/core/NavigationViewModel.kt

@@ -70,11 +70,14 @@ class DefaultNavigationViewModel(
           .map { coreState ->
             lastLocation =
                 when (coreState.tripState) {
-                  is TripState.Navigating -> coreState.tripState.snappedUserLocation
+                  is TripState.Navigating -> {
+                    Log.d("NavigationViewModel", "Course is ${lastLocation?.courseOverGround?.degrees} at idx ${coreState.tripState.currentStepGeometryIndex}")
+                    coreState.tripState.snappedUserLocation
+                  }
                   is TripState.Complete,
                   TripState.Idle -> locationProvider.lastLocation
                 }
-
+            
             uiState(coreState, spokenInstructionObserver?.isMuted, lastLocation)
             // This awkward dance is required because Kotlin doesn't have a way to map over
             // StateFlows

common/ferrostar/src/algorithms.rs

@@ -1,16 +1,14 @@
 //! Common spatial algorithms which are useful for navigation.
 
-use crate::navigation_controller::models::{
-    StepAdvanceMode, StepAdvanceStatus,
-    StepAdvanceStatus::{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, HaversineDistance, HaversineLength,
-    LineLocatePoint, LineString, Point,
+    Closest, ClosestPoint, Coord, EuclideanDistance, GeodesicBearing, HaversineDistance, HaversineLength, LineLocatePoint, LineString, Point
 };
 
 #[cfg(test)]
@@ -53,6 +51,49 @@ pub fn index_of_closest_segment_origin(location: UserLocation, line: &LineString
         .map(|(index, _)| index as u64)
 }
 
+/// 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;
+    }
+
+    let current = points[index_along_line as usize];
+    let next = points[index_along_line as usize + 1];
+
+    let mut degrees = current.geodesic_bearing(next);
+    if degrees < 0.0 {
+        degrees += 360.0;
+    }
+
+    Some(CourseOverGround {
+        degrees: degrees as u16,
+        accuracy: None,
+    })
+}
+
+/// 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));
+
+    let course_over_ground = snapped_course.or(location.course_over_ground);
+
+    UserLocation {
+        course_over_ground,
+        ..location
+    }
+}
+
 /// Snaps a user location to the closest point on a route line.
 ///
 /// If the location cannot be snapped (should only be possible with an invalid coordinate or geometry),
@@ -667,7 +708,7 @@ proptest! {
 }
 
 #[cfg(test)]
-mod geom_index_tests {
+mod linestring_based_tests {
 
     use super::*;
 
@@ -715,6 +756,59 @@ mod geom_index_tests {
         let index = index_of_closest_segment_origin(make_user_location(10.0, 10.0), &line);
         assert_eq!(index, Some(3));
     }
+
+}
+
+#[cfg(test)]
+mod bearing_snapping_tests {
+
+    use super::*;
+
+    static COORDS: [Coord; 6] = [
+        coord!(x: 0.0, y: 0.0),
+        coord!(x: 1.0, y: 1.0),
+        coord!(x: 2.0, y: 1.0),
+        coord!(x: 2.0, y: 2.0),
+        coord!(x: 2.0, y: 1.0),
+        coord!(x: 1.0, y: 1.0),
+    ];
+
+    #[test]
+    fn test_bearing_to_next_point() {
+        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 }));
+
+        let bearing = get_bearing_to_next_point(1, &line);
+        assert_eq!(bearing, Some(CourseOverGround { degrees: 89, accuracy: None }));
+
+        let bearing = get_bearing_to_next_point(2, &line);
+        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 }));
+
+        let bearing = get_bearing_to_next_point(4, &line);
+        assert_eq!(bearing, Some(CourseOverGround { degrees: 270, accuracy: None }));
+
+        // At the end
+        let bearing = get_bearing_to_next_point(5, &line);
+        assert_eq!(bearing, None);
+    }
+
+    #[test]
+    fn test_apply_snapped_course() {
+        let line = LineString::new(COORDS.to_vec());
+
+        let user_location = make_user_location(1.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 }));
+    }
+
 }
 
 // TODO: Other unit tests

common/ferrostar/src/navigation_controller/mod.rs

@@ -7,8 +7,7 @@ pub(crate) mod test_helpers;
 
 use crate::{
     algorithms::{
-        advance_step, 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},
 };
@@ -51,9 +50,15 @@ 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
+        );
+
         let progress = calculate_trip_progress(
-            &snapped_user_location.into(),
+            &snapped_user_location_with_course.into(),
             &current_step_linestring,
             &remaining_steps,
         );
@@ -71,7 +76,7 @@ impl NavigationController {
 
         TripState::Navigating {
             current_step_geometry_index,
-            snapped_user_location,
+            snapped_user_location: snapped_user_location_with_course,
             remaining_steps: remaining_steps.clone(),
             // Skip the first waypoint, as it is the current one
             remaining_waypoints: self.route.waypoints.iter().skip(1).copied().collect(),
@@ -139,6 +144,11 @@ impl NavigationController {
                             *snapped_user_location,
                             &self.route.get_linestring(),
                         );
+                        let snapped_user_location_with_course = apply_snapped_course(
+                            *snapped_user_location, 
+                            current_step_geometry_index, 
+                            &linestring
+                        );
 
                         let visual_instruction = current_step
                             .get_active_visual_instruction(progress.distance_to_next_maneuver)
@@ -199,15 +209,20 @@ impl NavigationController {
                 let current_step_linestring = current_step.get_linestring();
                 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
+                );
 
                 let progress = calculate_trip_progress(
-                    &snapped_user_location.into(),
+                    &snapped_user_location_with_course.into(),
                     &current_step_linestring,
                     remaining_steps,
                 );
                 let intermediate_state = TripState::Navigating {
                     current_step_geometry_index: *current_step_geometry_index,
-                    snapped_user_location,
+                    snapped_user_location: snapped_user_location_with_course,
                     remaining_steps: remaining_steps.clone(),
                     remaining_waypoints: remaining_waypoints.clone(),
                     progress,
@@ -262,10 +277,15 @@ impl NavigationController {
                             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
+                        );
 
                         TripState::Navigating {
                             current_step_geometry_index,
-                            snapped_user_location,
+                            snapped_user_location: snapped_user_location_with_course,
                             remaining_steps,
                             remaining_waypoints,
                             progress,