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); }