public static GLOSAResult ProjectedSignalTimingsForLane(MapDataIntersectionsIntersectionGeometryGenericLane lane, SPAT spat, ulong maneuver, int crocsTime)
{
GLOSAResult crocsResult = new GLOSAResult();
SPATIntersectionsIntersectionStateMovementState state = LocateSignalMovementStateForLane(spat, lane, maneuver);
if (state != null)
{
List <StateTimeMovementEvent> stateTimeMovementEvents = ProjectedMovementEventStates(state, crocsTime);
crocsResult.StateTimeMovementEvents = stateTimeMovementEvents;
if (IsTimeWithinCurrentMovementSequence(crocsTime, stateTimeMovementEvents) == true)
{
crocsResult.CurrentStateTimeMovement = FindNextMovementEvent(crocsTime, stateTimeMovementEvents);
}
else
{
crocsResult.Errors = GLOSAErrors.UnableToProjectMovementStates;
}
}
else
{
crocsResult.Errors = GLOSAErrors.UnableToFindProjectedStateForLane;
}
return(crocsResult);
}
public static GLOSAResult ProjectedLaneForManeuver(MapData map, List <GPSLocation> gpsHistory, double?deviceHeading, ulong maneuver)
{
List <TrafficNode> trafficNodePositions = ExtractTrafficNodesFromMAP(map, maneuver);
List <GPSLocation> gpsLocations = gpsHistory;
GPSLocation location = gpsLocations.First();
var sortedList = trafficNodePositions.
OrderBy(m => Distance.CalculateDistanceBetween2PointsKMs(
m.GPSLocation.Latitude,
m.GPSLocation.Longitude,
location.Latitude, location.Longitude)).ToList();
TrafficNode nearestNode = sortedList[0];
MapDataIntersectionsIntersectionGeometryGenericLane lane = null;
MapDataIntersectionsIntersectionGeometryGenericLane[] lanes = map.intersections.IntersectionGeometry.laneSet;
var possibleLanes = lanes.Where(genricLane => genricLane.laneID.ToString() == nearestNode.ID);
if (possibleLanes.Count() > 0)
{
lane = possibleLanes.First();
}
// Verify the lane is in the same direction as the vehicle
var nodes = 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();
GLOSAResult result = IsDirectionOfVehicleInSameDirectionAsLane(map.intersections.IntersectionGeometry.id.id, sortedNodes, deviceHeading, gpsHistory, 50, maneuver);
result.Description = $"LaneId: {lane.laneID}, {result.Description}";
result.Object = lane;
return(result);
}
public static List <TrafficNode> ExtractTrafficNodesFromLane(MapDataIntersectionsIntersectionGeometryGenericLane lane)
{
List <TrafficNode> trafficNodePositions = new List <TrafficNode>();
foreach (var node in lane.nodeList.nodes)
{
TrafficNode trafficNode = new TrafficNode()
{
GPSLocation = new GPSLocation()
{
Latitude = node.delta.nodeLatLon.lat / Constants.MAPCoordinateIntConverterUnit,
Longitude = node.delta.nodeLatLon.lon / Constants.MAPCoordinateIntConverterUnit,
},
ID = lane.laneID.ToString(),
};
trafficNodePositions.Add(trafficNode);
}
return(trafficNodePositions);
}
public static SPATIntersectionsIntersectionStateMovementState LocateSignalMovementStateForLane(SPAT spat, MapDataIntersectionsIntersectionGeometryGenericLane lane, ulong maneuver)
{
SPATIntersectionsIntersectionStateMovementState movementState = null;
if (lane.connectsTo != null)
{
var laneConnection = lane.connectsTo.Where(connection => connection.connectingLane.maneuver == maneuver);
if (laneConnection.Count() > 0)
{
byte signalGroup = laneConnection.First().signalGroup;
movementState = spat.intersections.IntersectionState.states.Where(state => state.signalGroup == signalGroup).FirstOrDefault();
}
}
return(movementState);
}