/// <summary>
/// Plan over all exits
/// </summary>
/// <param name="exitWaypoint"></param>
/// <param name="goal"></param>
/// <returns></returns>
public IntersectionPlan GetIntersectionExitPlan(ITraversableWaypoint exitWaypoint, INavigableNode goal)
{
// initialize the intersection plan
IntersectionPlan ip = new IntersectionPlan(exitWaypoint, new List <PlanableInterconnect>(), null);
ip.ExitWaypoint = exitWaypoint;
//check valid exit
if (exitWaypoint.IsExit)
{
// plan over each interconnect
foreach (ArbiterInterconnect ai in exitWaypoint.Exits)
{
// start of plan is the final wp of itner
INavigableNode start = (INavigableNode)ai.FinalGeneric;
// plan
double time;
List <INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
time += ai.TimeCost();
// create planned interconnect
PlanableInterconnect pi = new PlanableInterconnect(ai, time, nodes);
// add planned interconnect to the intersection plan
ip.PossibleEntries.Add(pi);
}
}
// return the plan
return(ip);
}
/// <summary>
/// Plans over an intersection
/// </summary>
/// <param name="exitWaypoint"></param>
/// <param name="goal"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersection(ITraversableWaypoint exitWaypoint, INavigableNode goal)
{
// road plan if the itnersection has a road available to take from it
RoadPlan rp = null;
// check if road waypoint
if (exitWaypoint is ArbiterWaypoint)
{
// get exit
ArbiterWaypoint awp = (ArbiterWaypoint)exitWaypoint;
// check if it has lane partition moving outwards
if (awp.NextPartition != null)
{
// road plan ignoring exit
List <ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(awp.Lane.Way, awp.Position, awp);
rp = this.PlanNavigableArea(awp.Lane, awp.Position, goal, iws);
}
}
// get exit plan
IntersectionPlan ip = this.GetIntersectionExitPlan(exitWaypoint, goal);
// add road plan if exists
ip.SegmentPlan = rp;
// return the plan
return(ip);
}
/// <summary>
/// Try to plan the intersection heavily penalizing the interconnect
/// </summary>
/// <param name="iTraversableWaypoint"></param>
/// <param name="iArbiterWaypoint"></param>
/// <param name="arbiterInterconnect"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect)
{
// save old blockage
NavigationBlockage tmpBlockage = interconnect.Blockage;
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
interconnect.Blockage = newerBlockage;
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
// plan
IntersectionPlan ip = this.PlanIntersection(exit, goal);
this.currentTimes = tmpCurrentTimes;
// reset interconnect blockage
interconnect.Blockage = tmpBlockage;
// return plan
return(ip);
}
/// <summary>
/// Try to plan the intersection heavily penalizing the interconnect
/// </summary>
/// <param name="iTraversableWaypoint"></param>
/// <param name="iArbiterWaypoint"></param>
/// <param name="arbiterInterconnect"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated)
{
ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric;
if (!blockAllRelated)
{
return(this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect));
}
else
{
Dictionary <IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary <IConnectAreaWaypoints, NavigationBlockage>();
if (entry.IsEntry)
{
foreach (ArbiterInterconnect ai in entry.Entries)
{
saved.Add(ai, ai.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
ai.Blockage = newerBlockage;
}
}
if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null)
{
ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition;
saved.Add(alp, alp.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
// plan
IntersectionPlan ip = this.PlanIntersection(exit, goal);
this.currentTimes = tmpCurrentTimes;
// reset the blockages
foreach (KeyValuePair <IConnectAreaWaypoints, NavigationBlockage> savedPair in saved)
{
savedPair.Key.Blockage = savedPair.Value;
}
// return plan
return(ip);
}
}
/// <summary>
/// Plan over all exits
/// </summary>
/// <param name="exitWaypoint"></param>
/// <param name="goal"></param>
/// <returns></returns>
public IntersectionPlan GetIntersectionExitPlan(ITraversableWaypoint exitWaypoint, INavigableNode goal)
{
// initialize the intersection plan
IntersectionPlan ip = new IntersectionPlan(exitWaypoint, new List<PlanableInterconnect>(), null);
ip.ExitWaypoint = exitWaypoint;
//check valid exit
if (exitWaypoint.IsExit)
{
// plan over each interconnect
foreach (ArbiterInterconnect ai in exitWaypoint.Exits)
{
// start of plan is the final wp of itner
INavigableNode start = (INavigableNode)ai.FinalGeneric;
// plan
double time;
List<INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
time += ai.TimeCost();
// create planned interconnect
PlanableInterconnect pi = new PlanableInterconnect(ai, time, nodes);
// add planned interconnect to the intersection plan
ip.PossibleEntries.Add(pi);
}
}
// return the plan
return ip;
}
