public void IsVehicleHeadingInSameDirectionAsLaneDirection_WithLaneInSameDirection_ShouldReturnTrue()
{
// arrange
//SETTINGS_ROUTE_DIRECTION_NS = 1: - Not Supported Yet
//SETTINGS_ROUTE_DIRECTION_SN = 2: - Not Supported Yet
//SETTINGS_ROUTE_DIRECTION_EW = 3;
//SETTINGS_ROUTE_DIRECTION_WE = 4;
var intersectionId = "2111";
var gpsHistory = KMLHelper.GLOSATestRouteIntersectionHistory(intersectionId, 2);
// Load Map from file
string file = $"MAP-{Settings.IntersectionId}.xml";
MapData map = XMLHelper.LoadMAPDataFromFile(file);
GLOSAResult result = GLOSAHelper.ProjectedLaneForManeuver(map, gpsHistory, 0, Constants.MANEUVER_DIRECTION_AHEAD);
var lane = (MapDataIntersectionsIntersectionGeometryGenericLane)result.Object;
var nodes = GLOSAHelper.ExtractTrafficNodesFromLane(lane);
// Let's sort all lane nodes from MAP Ref Point
var refPoint = map.intersections.IntersectionGeometry.refPoint;
var mapLocation = new GPSLocation()
{
Latitude = refPoint.lat / Constants.MAPCoordinateIntConverterUnit,
Longitude = refPoint.@long / Constants.MAPCoordinateIntConverterUnit,
};
// Sort the nodes by distance ascending
var sortedNodes = nodes.OrderBy(node => Distance.CalculateDistanceBetween2PointsKMs(node.GPSLocation.Latitude, node.GPSLocation.Longitude, mapLocation.Latitude, mapLocation.Longitude)).ToList();
bool expectedResult = true;
//act
result = GLOSAHelper.IsDirectionOfVehicleInSameDirectionAsLane(map.intersections.IntersectionGeometry.id.id, sortedNodes, 0, gpsHistory, 50, Constants.MANEUVER_DIRECTION_AHEAD);
bool actualResult = result.Errors == GLOSAErrors.NoErrors;
//assert
Assert.Equal(expectedResult, actualResult);
}
public static IList <IntersectionNode> Filter(IList <IntersectionNode> intersectionNodes, double?heading, List <GPSLocation> gpsTrail)
{
return(intersectionNodes.Where(n => GLOSAHelper.ProjectedLaneForManeuver(n.MapData, gpsTrail, heading, Constants.MANEUVER_DIRECTION_AHEAD).Errors == GLOSAErrors.NoErrors).ToList());
}