Fix location marker orientation issue

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/snap/SnapToRoute.java

@@ -1,24 +1,17 @@
 package com.mapbox.services.android.navigation.v5.snap;
 
 import android.location.Location;
-import android.support.annotation.NonNull;
-import android.support.annotation.Nullable;
 
 import com.mapbox.geojson.Feature;
 import com.mapbox.geojson.LineString;
 import com.mapbox.geojson.Point;
-import com.mapbox.services.android.navigation.v5.routeprogress.RouteLegProgress;
 import com.mapbox.services.android.navigation.v5.routeprogress.RouteProgress;
-import com.mapbox.services.android.navigation.v5.routeprogress.RouteStepProgress;
 import com.mapbox.services.android.navigation.v5.utils.MathUtils;
-import com.mapbox.turf.TurfConstants;
 import com.mapbox.turf.TurfMeasurement;
 import com.mapbox.turf.TurfMisc;
 
 import java.util.List;
 
-import static com.mapbox.core.constants.Constants.PRECISION_6;
-
 /**
  * This attempts to snap the user to the closest position along the route. Prior to snapping the
  * user, their location's checked to ensure that the user didn't veer off-route. If your application
@@ -31,95 +24,38 @@ public class SnapToRoute extends Snap {
 
   @Override
   public Location getSnappedLocation(Location location, RouteProgress routeProgress) {
-    Location snappedLocation = snapLocationLatLng(location, routeProgress.currentStepPoints());
-    snappedLocation.setBearing(snapLocationBearing(routeProgress));
-    return snappedLocation;
-  }
-
-  /**
-   * Logic used to snap the users location coordinates to the closest position along the current
-   * step.
-   *
-   * @param location        the raw location
-   * @param stepCoordinates the list of step geometry coordinates
-   * @return the altered user location
-   * @since 0.4.0
-   */
-  private static Location snapLocationLatLng(Location location, List<Point> stepCoordinates) {
     Location snappedLocation = new Location(location);
     Point locationToPoint = Point.fromLngLat(location.getLongitude(), location.getLatitude());
 
     // Uses Turf's pointOnLine, which takes a Point and a LineString to calculate the closest
     // Point on the LineString.
+    List<Point> stepCoordinates = routeProgress.currentStepPoints();
     if (stepCoordinates.size() > 1) {
       Feature feature = TurfMisc.nearestPointOnLine(locationToPoint, stepCoordinates);
       Point point = ((Point) feature.geometry());
       snappedLocation.setLongitude(point.longitude());
       snappedLocation.setLatitude(point.latitude());
-    }
-    return snappedLocation;
-  }
-
-  /**
-   * Creates a snapped bearing for the snapped {@link Location}.
-   * <p>
-   * This is done by measuring 1 meter ahead of the current step distance traveled and
-   * creating a {@link Point} with this distance using {@link TurfMeasurement#along(LineString, double, String)}.
-   * <p>
-   * If the step distance remaining is zero, the distance ahead is 1 meter into the upcoming step.
-   * This way, an accurate bearing is upheld transitioning between steps.
-   *
-   * @param routeProgress for all current progress values
-   * @return float bearing snapped to route
-   */
-  private static float snapLocationBearing(RouteProgress routeProgress) {
 
-    RouteLegProgress legProgress = routeProgress.currentLegProgress();
-    RouteStepProgress stepProgress = legProgress.currentStepProgress();
-    double distanceTraveled = stepProgress.distanceTraveled();
-    double distanceRemaining = stepProgress.distanceRemaining();
-    boolean distanceRemainingZero = distanceRemaining == 0;
-
-    // Either want to measure our current step distance traveled + 1 or 1 meter into the upcoming step
-    double distanceAhead = distanceRemainingZero ? 1 : distanceTraveled + 1;
-    // Create the step linestring from the geometry
-    LineString upcomingLineString = createUpcomingLineString(legProgress, distanceRemainingZero);
-    LineString currentLineString = createCurrentLineString(legProgress);
-
-    // Measure 1 meter ahead of the users current location, only if the distance remaining isn't zero
-    Point futurePoint = createFuturePoint(distanceAhead, upcomingLineString, currentLineString);
-    Point currentPoint = TurfMeasurement.along(currentLineString, distanceTraveled, TurfConstants.UNIT_METERS);
-
-    // Get bearing and convert azimuth to degrees
-    double azimuth = TurfMeasurement.bearing(currentPoint, futurePoint);
-    return (float) MathUtils.wrap(azimuth, 0, 360);
-  }
-
-  @NonNull
-  private static LineString createCurrentLineString(RouteLegProgress legProgress) {
-    String currentGeometry = legProgress.currentStep().geometry();
-    return LineString.fromPolyline(currentGeometry, PRECISION_6);
-  }
-
-  @Nullable
-  private static LineString createUpcomingLineString(RouteLegProgress legProgress, boolean distanceRemainingZero) {
-    LineString upcomingLineString = null;
-    if (distanceRemainingZero && legProgress.upComingStep() != null) {
-      String upcomingGeometry = legProgress.upComingStep().geometry();
-      upcomingLineString = LineString.fromPolyline(upcomingGeometry, PRECISION_6);
+      float bearing = calculateBearing(location, point, stepCoordinates);
+      snappedLocation.setBearing(bearing);
     }
-    return upcomingLineString;
+    return snappedLocation;
   }
 
-  @NonNull
-  private static Point createFuturePoint(double distanceAhead, LineString upcomingLineString,
-                                         LineString currentLineString) {
-    Point futurePoint;
-    if (upcomingLineString != null) {
-      futurePoint = TurfMeasurement.along(upcomingLineString, distanceAhead, TurfConstants.UNIT_METERS);
+  private float calculateBearing(Location location, Point snappedLocation, List<Point> stepCoordinates) {
+    LineString stepLineString = LineString.fromLngLats(stepCoordinates);
+    Point maneuverPoint = stepCoordinates.get(stepCoordinates.size() - 1);
+    // get slice line string between current point to maneuver point
+    LineString remainingStepLineString = TurfMisc.lineSlice(snappedLocation, maneuverPoint, stepLineString);
+    List<Point> coordinates = remainingStepLineString.coordinates();
+    // has current point and next point
+    if (coordinates.size() > 1) {
+      // get second point to have upcoming point
+      Point upcomingPoint = coordinates.get(1);
+      double azimuth = TurfMeasurement.bearing(snappedLocation, upcomingPoint);
+      return (float) MathUtils.wrap(azimuth, 0, 360);
     } else {
-      futurePoint = TurfMeasurement.along(currentLineString, distanceAhead, TurfConstants.UNIT_METERS);
+      return location.getBearing();
     }
-    return futurePoint;
   }
 }