/// <summary> /// Gets all exits downstream from a point /// </summary> /// <param name="position"></param> /// <param name="way"></param> /// <param name="exits">We are looking for exits</param> /// <returns></returns> private List <DownstreamPointOfInterest> Downstream(Coordinates position, ArbiterWay way, bool exits, List <ArbiterWaypoint> ignorable) { List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>(); foreach (ArbiterLane al in way.Lanes.Values) { LinePath.PointOnPath pop = al.GetClosestPoint(position); if (al.IsInside(position) || position.DistanceTo(al.LanePath().StartPoint.Location) < 1.0) { ArbiterLanePartition currentPartition = al.GetClosestPartition(position); ArbiterWaypoint initial = currentPartition.Final; double initialCost = position.DistanceTo(currentPartition.Final.Position) / way.Segment.SpeedLimits.MaximumSpeed; do { if (((exits && currentPartition.Final.IsExit) || (!exits && currentPartition.Final.IsEntry)) && !ignorable.Contains(currentPartition.Final)) { double timeCost = initialCost + this.TimeCostInLane(initial, currentPartition.Final); DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.LanePath().DistanceBetween(pop, al.GetClosestPoint(currentPartition.Final.Position)); dpoi.IsExit = true; dpoi.IsGoal = false; dpoi.PointOfInterest = currentPartition.Final; dpoi.TimeCostToPoint = timeCost; waypoints.Add(dpoi); } currentPartition = currentPartition.Final.NextPartition; }while(currentPartition != null); } } return(waypoints); }
/// <summary> /// Checks if a goal is downstream on the current road /// </summary> /// <param name="way"></param> /// <param name="currentPosition"></param> /// <param name="goal"></param> /// <returns></returns> private DownstreamPointOfInterest IsGoalDownStream(ArbiterWay way, Coordinates currentPosition, INavigableNode goal) { if (goal is ArbiterWaypoint) { ArbiterWaypoint goalWaypoint = (ArbiterWaypoint)goal; if (goalWaypoint.Lane.Way.Equals(way)) { foreach (ArbiterLane lane in way.Lanes.Values) { if (lane.Equals(goalWaypoint.Lane)) { Coordinates current = lane.GetClosest(currentPosition); Coordinates goalPoint = lane.GetClosest(goal.Position); if (lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) { double distToGoal = lane.DistanceBetween(current, goalPoint); if (distToGoal > 0) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = distToGoal; dpoi.IsGoal = true; dpoi.IsExit = false; dpoi.PointOfInterest = goalWaypoint; dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint); dpoi.RouteTime = 0.0; return(dpoi); } } } else { Coordinates current = lane.GetClosest(currentPosition); Coordinates goalPoint = lane.GetClosest(goal.Position); if ((lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) && lane.IsInside(goalPoint)) { double distToGoal = lane.DistanceBetween(current, goalPoint); if (distToGoal > 0) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = distToGoal; dpoi.IsGoal = true; dpoi.IsExit = false; dpoi.PointOfInterest = goalWaypoint; dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint); dpoi.RouteTime = 0.0; //return dpoi; } } } } } } return(null); }
/// <summary> /// Plan given that we are starting on a road /// </summary> /// <param name="currentLane"></param> /// <param name="currentPosition"></param> /// <param name="goal"></param> /// <returns></returns> public RoadPlan PlanRoads(ArbiterLane currentLane, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable) { // get all downstream points of interest List <DownstreamPointOfInterest> downstreamPoints = new List <DownstreamPointOfInterest>(); // get exits downstream from this current position in the way downstreamPoints.AddRange(this.Downstream(currentPosition, currentLane.Way, true, ignorable)); // determine if goal is downstream in a specific lane, add to possible route times to consider DownstreamPointOfInterest goalDownstream = this.IsGoalDownStream(currentLane.Way, currentPosition, goal); // add goal to points downstream if it exists if (goalDownstream != null) { downstreamPoints.Add(goalDownstream); } // so, for each exit downstream we need to plan from the end of each interconnect to the goal this.DetermineDownstreamPointRouteTimes(downstreamPoints, goal, currentLane.Way); // get road plan RoadPlan rp = this.GetRoadPlan(downstreamPoints, currentLane.Way); // update arbiter information List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition); // make sure we're in a road state if (CoreCommon.CorePlanningState == null || CoreCommon.CorePlanningState is TravelState || CoreCommon.CorePlanningState is TurnState) { // check route 1 if (routeInfo.Count > 0) { RouteInformation ri = routeInfo[0]; CoreCommon.CurrentInformation.Route1 = ri; CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6"); CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint; } // check route 2 if (routeInfo.Count > 1) { RouteInformation ri = routeInfo[1]; CoreCommon.CurrentInformation.Route2 = ri; CoreCommon.CurrentInformation.Route2Time = ri.RouteTimeCost.ToString("F6"); CoreCommon.CurrentInformation.Route2Wp = ri.Waypoint; } } // return road plan return(rp); }
public DownstreamPointOfInterest Clone() { DownstreamPointOfInterest tmp = new DownstreamPointOfInterest(); tmp.BestExit = this.BestExit; tmp.BestRoute = this.BestRoute; tmp.DistanceToPoint = this.DistanceToPoint; tmp.IsExit = this.IsExit; tmp.IsGoal = this.IsGoal; tmp.PointOfInterest = this.PointOfInterest; tmp.RouteTime = this.RouteTime; tmp.TimeCostToPoint = this.TimeCostToPoint; return tmp; }
public DownstreamPointOfInterest Clone() { DownstreamPointOfInterest tmp = new DownstreamPointOfInterest(); tmp.BestExit = this.BestExit; tmp.BestRoute = this.BestRoute; tmp.DistanceToPoint = this.DistanceToPoint; tmp.IsExit = this.IsExit; tmp.IsGoal = this.IsGoal; tmp.PointOfInterest = this.PointOfInterest; tmp.RouteTime = this.RouteTime; tmp.TimeCostToPoint = this.TimeCostToPoint; return(tmp); }
/// <summary> /// Comparer /// </summary> /// <param name="obj"></param> /// <returns></returns> public int CompareTo(object obj) { if (obj is DownstreamPointOfInterest) { DownstreamPointOfInterest other = (DownstreamPointOfInterest)obj; if (this.TotalTime < other.TotalTime) { return(-1); } else if (this.TotalTime > other.TotalTime) { return(1); } else { return(0); } } return(-1); }
/// <summary> /// Route ploan for downstream exits /// </summary> /// <param name="downstreamPoints"></param> /// <param name="goal"></param> private void RouteTimes(List <DownstreamPointOfInterest> downstreamPoints, INavigableNode goal) { // check if we are planning over the correct goal if (this.currentTimes.Key != CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber || this.currentTimes.Value == null) { // create new lookup this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >( CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber, new Dictionary <ArbiterWaypointId, DownstreamPointOfInterest>()); } // so, for each exit downstream we need to plan from the end of each interconnect to the goal foreach (DownstreamPointOfInterest dpoi in downstreamPoints) { // container flag bool contains = this.currentTimes.Value.ContainsKey(dpoi.PointOfInterest.WaypointId); // check if exit if (dpoi.IsExit && !contains) { // fake node FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest); // init fields double timeCost; List <INavigableNode> routeNodes; // plan this.Plan(fen, goal, out routeNodes, out timeCost); // set best dpoi.RouteTime = timeCost; dpoi.BestRoute = routeNodes; dpoi.BestExit = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null; // add to keepers this.currentTimes.Value.Add(dpoi.PointOfInterest.WaypointId, dpoi.Clone()); } else if (dpoi.IsExit && contains) { DownstreamPointOfInterest tmp = this.currentTimes.Value[dpoi.PointOfInterest.WaypointId]; if (tmp.BestExit == null) { ArbiterOutput.Output("NAV RouteTimes: Removing exit with no valid route: " + dpoi.PointOfInterest.WaypointId.ToString()); // remove this.currentTimes.Value.Remove(dpoi.PointOfInterest.WaypointId); dpoi.PointOfInterest = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[dpoi.PointOfInterest.WaypointId]; // fake node FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest); // init fields double timeCost; List <INavigableNode> routeNodes; // plan this.Plan(fen, goal, out routeNodes, out timeCost); // set best dpoi.RouteTime = timeCost; dpoi.BestRoute = routeNodes; dpoi.BestExit = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null; // add to keepers this.currentTimes.Value.Add(dpoi.PointOfInterest.WaypointId, dpoi); } else { dpoi.RouteTime = tmp.RouteTime; dpoi.BestRoute = tmp.BestRoute; dpoi.BestExit = tmp.BestExit; } } } }
/// <summary> /// Gets exits downstream, or the goal we are currently reaching /// </summary> /// <param name="currentPosition"></param> /// <param name="ignorable"></param> /// <param name="goal"></param> /// <returns></returns> public List<DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List<ArbiterWaypoint> ignorable, INavigableNode goal) { // downstream final List<DownstreamPointOfInterest> waypoints = new List<DownstreamPointOfInterest>(); foreach (ArbiterLane al in Way.Lanes.Values) { if (al.Equals(this) || (al.GetClosestPartition(currentPosition).Type != PartitionType.Startup && ((this.LaneOnLeft != null && this.LaneOnLeft.Equals(al) && this.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (this.LaneOnRight != null && this.LaneOnRight.Equals(al) && this.BoundaryRight != ArbiterLaneBoundary.SolidWhite)))) { // get starting waypoint ArbiterWaypoint waypoint = null; // get closest partition ArbiterLanePartition alp = al.GetClosestPartition(currentPosition); if (alp.Initial.Position.DistanceTo(currentPosition) < TahoeParams.VL - 2) waypoint = alp.Initial; else if (alp.IsInside(currentPosition) || alp.Final.Position.DistanceTo(currentPosition) < TahoeParams.VL) waypoint = alp.Final; else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition)) waypoint = alp.Initial; else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition) && alp.Final.NextPartition == null) waypoint = null; else waypoint = null; // check waypoint exists if (waypoint != null) { // save start ArbiterWaypoint initial = waypoint; // initial cost double initialCost = 0.0; if (al.Equals(this)) initialCost = currentPosition.DistanceTo(waypoint.Position) / waypoint.Lane.Way.Segment.SpeedLimits.MaximumSpeed; else if (waypoint.WaypointId.Number != 1) { // get closest partition ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue); initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number); initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed; } else { // get closest partition ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue); ArbiterWaypoint tmpF = this.GetClosestWaypoint(waypoint.Position, Double.MaxValue); initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number); initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed; initialCost += this.TimeCostInLane(tmpI, tmpF, new List<ArbiterWaypoint>()); } // loop while waypoint not null while (waypoint != null) { if (waypoint.IsCheckpoint && (goal is ArbiterWaypoint) && ((ArbiterWaypoint)goal).WaypointId.Equals(waypoint.WaypointId)) { double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable); DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position); dpoi.IsExit = false; dpoi.IsGoal = true; dpoi.PointOfInterest = waypoint; dpoi.TimeCostToPoint = timeCost; waypoints.Add(dpoi); } else if (waypoint.IsExit && !ignorable.Contains(waypoint)) { double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable); DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position); dpoi.IsExit = true; dpoi.IsGoal = false; dpoi.PointOfInterest = waypoint; dpoi.TimeCostToPoint = timeCost; waypoints.Add(dpoi); } else if (waypoint.NextPartition == null && !ignorable.Contains(waypoint)) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position); dpoi.IsExit = false; dpoi.IsGoal = false; dpoi.PointOfInterest = waypoint; dpoi.TimeCostToPoint = Double.MaxValue; waypoints.Add(dpoi); } waypoint = waypoint.NextPartition != null ? waypoint.NextPartition.Final : null; } } } } return waypoints; }
/// <summary> /// Gets all exits downstream from a point /// </summary> /// <param name="position"></param> /// <param name="way"></param> /// <param name="exits">We are looking for exits</param> /// <returns></returns> private List<DownstreamPointOfInterest> Downstream(Coordinates position, ArbiterWay way, bool exits, List<ArbiterWaypoint> ignorable) { List<DownstreamPointOfInterest> waypoints = new List<DownstreamPointOfInterest>(); foreach (ArbiterLane al in way.Lanes.Values) { LinePath.PointOnPath pop = al.GetClosestPoint(position); if (al.IsInside(position) || position.DistanceTo(al.LanePath().StartPoint.Location) < 1.0) { ArbiterLanePartition currentPartition = al.GetClosestPartition(position); ArbiterWaypoint initial = currentPartition.Final; double initialCost = position.DistanceTo(currentPartition.Final.Position) / way.Segment.SpeedLimits.MaximumSpeed; do { if(((exits && currentPartition.Final.IsExit) || (!exits && currentPartition.Final.IsEntry)) && !ignorable.Contains(currentPartition.Final)) { double timeCost = initialCost + this.TimeCostInLane(initial, currentPartition.Final); DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.LanePath().DistanceBetween(pop, al.GetClosestPoint(currentPartition.Final.Position)); dpoi.IsExit = true; dpoi.IsGoal = false; dpoi.PointOfInterest = currentPartition.Final; dpoi.TimeCostToPoint = timeCost; waypoints.Add(dpoi); } currentPartition = currentPartition.Final.NextPartition; } while(currentPartition != null); } } return waypoints; }
/// <summary> /// Checks if a goal is downstream on the current road /// </summary> /// <param name="way"></param> /// <param name="currentPosition"></param> /// <param name="goal"></param> /// <returns></returns> private DownstreamPointOfInterest IsGoalDownStream(ArbiterWay way, Coordinates currentPosition, INavigableNode goal) { if (goal is ArbiterWaypoint) { ArbiterWaypoint goalWaypoint = (ArbiterWaypoint)goal; if (goalWaypoint.Lane.Way.Equals(way)) { foreach (ArbiterLane lane in way.Lanes.Values) { if (lane.Equals(goalWaypoint.Lane)) { Coordinates current = lane.GetClosest(currentPosition); Coordinates goalPoint = lane.GetClosest(goal.Position); if (lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) { double distToGoal = lane.DistanceBetween(current, goalPoint); if (distToGoal > 0) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = distToGoal; dpoi.IsGoal = true; dpoi.IsExit = false; dpoi.PointOfInterest = goalWaypoint; dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint); dpoi.RouteTime = 0.0; return dpoi; } } } else { Coordinates current = lane.GetClosest(currentPosition); Coordinates goalPoint = lane.GetClosest(goal.Position); if ((lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) && lane.IsInside(goalPoint)) { double distToGoal = lane.DistanceBetween(current, goalPoint); if (distToGoal > 0) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = distToGoal; dpoi.IsGoal = true; dpoi.IsExit = false; dpoi.PointOfInterest = goalWaypoint; dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint); dpoi.RouteTime = 0.0; //return dpoi; } } } } } } return null; }
/// <summary> /// constructor /// </summary> /// <param name="point"></param> public LanePlan(DownstreamPointOfInterest point) { this.laneWaypointOfInterest = point; }
/// <summary> /// constructor /// </summary> /// <param name="point"></param> public LanePlan(DownstreamPointOfInterest point) { this.laneWaypointOfInterest = point; }