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