public static IList <IntersectionNode> SortIntersectionNodes(IList <IntersectionNode> intersectionNodes, GPSLocation refPoint, double?currentBearing, double viewArc, double distance) { foreach (var node in intersectionNodes) { double?bearingLocationToWaypoint = LocationHelper.HeadingBetweenTwoGPSLocations(refPoint, node.GPSLocation); double delta = LocationHelper.DeltaOfVehicleToLaneDirection(currentBearing, bearingLocationToWaypoint); node.angleDiff = Math.Abs(delta);//Make positive node.distance = Distance.CalculateDistanceBetween2PointsKMs(refPoint.Latitude, refPoint.Longitude, node.GPSLocation.Latitude, node.GPSLocation.Longitude); } var ordered = intersectionNodes.OrderBy(wp => wp.distance) .ThenBy(wp => wp.angleDiff) .Where(wp => wp.angleDiff >= -viewArc && wp.angleDiff <= viewArc && wp.distance <= distance); return(ordered.ToList()); }
public kmlDocumentPlacemark LocateWaypointOnRoute(WaypointDetectionMethod method, List <kmlDocumentPlacemark> route, List <GPSLocation> history, double heading, int waypointIndex = 0) { kmlDocumentPlacemark nearestMAPPlacemark = null; var location = history.Last(); var sortedMAPPoints = GLOSAHelper.SortMAPKMLDataByDistanceFromCurrentLocation(route, location.Latitude, location.Longitude).ToList(); if (sortedMAPPoints != null && sortedMAPPoints.Count > 0) { if (method == WaypointDetectionMethod.ShortestDistance) { nearestMAPPlacemark = sortedMAPPoints.First(); } else if (method == WaypointDetectionMethod.DeviceHeading) { // No valid method chosen throw new Exception("Waypoint Detection Method not implemented."); } else if (method == WaypointDetectionMethod.GPSHistoryDirection) { List <TrafficNode> listOfWaypoints = new List <TrafficNode>(); GPSLocation from = history[history.Count - 2]; GPSLocation to = history[history.Count - 1]; double?vehicleBearing = LocationHelper.HeadingBetweenTwoGPSLocations(from, to); foreach (var mapPoint in sortedMAPPoints) { listOfWaypoints.Add(new TrafficNode() { GPSLocation = KMLHelper.XMLCoordinateStringToGPSLocation(mapPoint.Point.coordinates), ID = mapPoint.name, angleDiff = 0, }); } foreach (var waypoint in listOfWaypoints) { double?bearingLocationToWaypoint = LocationHelper.HeadingBetweenTwoGPSLocations(to, waypoint.GPSLocation); double delta = LocationHelper.DeltaOfVehicleToLaneDirection(vehicleBearing, bearingLocationToWaypoint); waypoint.angleDiff = Math.Abs(delta);//Make positive waypoint.distance = Distance.CalculateDistanceBetween2PointsKMs(to.Latitude, to.Longitude, waypoint.GPSLocation.Latitude, waypoint.GPSLocation.Longitude); } var sortedByBearingAndDistance = listOfWaypoints.OrderBy(wp => wp.distance) .ThenBy(wp => wp.angleDiff) .Where(wp => wp.angleDiff >= -50 && wp.angleDiff <= 50 && wp.distance <= 1.0); if (sortedByBearingAndDistance != null && sortedByBearingAndDistance.Count() > waypointIndex) { var tn = sortedByBearingAndDistance.ToList()[waypointIndex]; var pm = sortedMAPPoints.Find(mp => mp.name == tn.ID); nearestMAPPlacemark = pm; } } else { // No valid method chosen throw new Exception("Waypoint Detection Method not properly set."); } } return(nearestMAPPlacemark); }