/// <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);
}
public bool HitGoal(VehicleState state, Coordinates goal, IAreaSubtypeWaypointId id)
{
// check if forced
if (this.EntryParameters.HasValue && this.EntryParameters.Value.ForcedOpposing)
{
// get other way
ArbiterWay other = this.OpposingLane.Way.WayId.Number == 1 ? this.OpposingLane.Way.Segment.Way2 : this.OpposingLane.Way.Segment.Way1;
// check goal in other way
if (id is ArbiterWaypointId && ((ArbiterWaypointId)id).LaneId.WayId.Equals(other.WayId))
{
// center
Coordinates vehicleCenter = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);
// check all lanes
foreach (ArbiterLane al in other.Lanes.Values)
{
// get closest point to the center of this vehicle
bool b = al.LanePath().GetClosestPoint(vehicleCenter).Location.DistanceTo(
al.LanePath().GetClosestPoint(goal).Location) < TahoeParams.VL / 2.0;
if (b)
{
return(true);
}
}
}
}
return(false);
}
/// <summary>
/// Waypoints to ignore
/// </summary>
/// <param name="way"></param>
/// <param name="position"></param>
/// <returns></returns>
public static List <ArbiterWaypoint> WaypointsClose(ArbiterWay way, Coordinates position, ArbiterWaypoint ignoreSpecifically)
{
List <ArbiterWaypoint> waypoints = new List <ArbiterWaypoint>();
foreach (ArbiterLane al in way.Lanes.Values)
{
ArbiterWaypoint aw = al.GetClosestWaypoint(position, TahoeParams.VL * 2.0);
if (aw != null)
{
waypoints.Add(aw);
}
}
if (ignoreSpecifically != null && !waypoints.Contains(ignoreSpecifically))
{
waypoints.Add(ignoreSpecifically);
}
return(waypoints);
}
/// <summary>
/// Route ploan for downstream exits
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
private void DetermineDownstreamPointRouteTimes(List <DownstreamPointOfInterest> downstreamPoints, INavigableNode goal, ArbiterWay aw)
{
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
foreach (DownstreamPointOfInterest dpoi in downstreamPoints)
{
// check if exit
if (dpoi.IsExit)
{
// current best time
double bestCurrent = Double.MaxValue;
List <INavigableNode> bestRoute = null;
ArbiterInterconnect bestInterconnect = null;
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List <INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
bestCurrent = timeCost;
bestRoute = routeNodes;
bestInterconnect = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// plan from each interconnect to find the best time from exit
/*foreach (ArbiterInterconnect ai in dpoi.PointOfInterest.Exits)
* {
* // init fields
* double timeCost;
* List<INavigableNode> routeNodes;
*
* // plan
* this.Plan(ai.End, goal, out routeNodes, out timeCost);
*
* // check
* if (timeCost < bestCurrent)
* {
* bestCurrent = timeCost;
* bestRoute = routeNodes;
* bestInterconnect = ai;
* }
* }*/
// set best
dpoi.RouteTime = bestCurrent;
dpoi.BestRoute = bestRoute;
dpoi.BestExit = bestInterconnect;
}
}
}
/// <summary>
/// Gets the road plan
/// </summary>
/// <param name="pointsOfInterest"></param>
/// <returns></returns>
private RoadPlan GetRoadPlan(List <DownstreamPointOfInterest> pointsOfInterest, ArbiterWay way)
{
// road plan
RoadPlan rp = new RoadPlan(new Dictionary <ArbiterLaneId, LanePlan>());
// find the best in each lane
foreach (ArbiterLane al in way.Lanes.Values)
{
// points in lane
List <DownstreamPointOfInterest> lanePoints = new List <DownstreamPointOfInterest>();
// loop over all points
foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
{
if (dpoi.PointOfInterest.Lane.Equals(al))
{
lanePoints.Add(dpoi);
}
}
// if exist any
if (lanePoints.Count > 0)
{
// sort
lanePoints.Sort();
// choose best and add
rp.LanePlans.Add(al.LaneId, new LanePlan(lanePoints[0]));
}
}
// return
return(rp);
}
