public int CompareTo(object obj)
{
if (obj is RouteInformation)
{
RouteInformation other = (RouteInformation)obj;
if (this.RouteTimeCost < other.RouteTimeCost)
{
return(-1);
}
else if (this.RouteTimeCost == other.RouteTimeCost)
{
return(0);
}
else
{
return(1);
}
}
else
{
throw new Exception("obj is not route information");
}
}
/// <summary>
/// Plan in a zone
/// </summary>
/// <param name="az"></param>
/// <param name="goal"></param>
/// <returns></returns>
public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal)
{
// zone plan
ZonePlan zp = new ZonePlan();
zp.Zone = az;
zp.Start = start;
// given start and goal, get plan
double time;
List<INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
zp.Time = time;
// final path
LinePath recommendedPath = new LinePath();
List<INavigableNode> pathNodes = new List<INavigableNode>();
// start and end counts
int startCount = 0;
int endCount = 0;
// check start type
if(start is ArbiterParkingSpotWaypoint)
startCount = 2;
// check end type
if(goal is ArbiterParkingSpotWaypoint &&
((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az))
endCount = -2;
// loop through nodes
for(int i = startCount; i < nodes.Count + endCount; i++)
{
// go to parking spot or endpoint
if(nodes[i] is ArbiterParkingSpotWaypoint)
{
// set zone goal
zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint;
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return zp;
}
// go to perimeter waypoint if this is one
else if(nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit)
{
// add
recommendedPath.Add(nodes[i].Position);
// set zone goal
zp.ZoneGoal = nodes[i];
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return zp;
}
// otherwise just add
else
{
// add
recommendedPath.Add(nodes[i].Position);
pathNodes.Add(nodes[i]);
}
}
// set zone goal
zp.ZoneGoal = goal;
// set path, return
zp.RecommendedPath = recommendedPath;
// set route info
CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString());
// return the plan
return zp;
}
/// <summary>
/// Gets route information for sending to remote listeners
/// </summary>
public List<RouteInformation> RouteInformation(Coordinates currentPosition)
{
List<RouteInformation> routes = new List<RouteInformation>();
foreach (KeyValuePair<ArbiterLaneId, LanePlan> lp in this.LanePlans)
{
List<Coordinates> route = new List<Coordinates>();
double time = lp.Value.laneWaypointOfInterest.TotalTime;
// get lane coords
ArbiterLanePartition current = lp.Value.laneWaypointOfInterest.PointOfInterest.Lane.GetClosestPartition(currentPosition);
if (CoreCommon.CorePlanningState is StayInSupraLaneState)
{
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
if(sisls.Lane.ClosestComponent(currentPosition) == SLComponentType.Initial)
{
LinePath p = sisls.Lane.LanePath(currentPosition, sisls.Lane.Interconnect.InitialGeneric.Position);
route.AddRange(p);
route.Add(sisls.Lane.Interconnect.FinalGeneric.Position);
current = ((ArbiterWaypoint)sisls.Lane.Interconnect.FinalGeneric).NextPartition;
}
}
while (current != null && current.Initial != lp.Value.laneWaypointOfInterest.PointOfInterest)
{
route.Add(current.Final.Position);
current = current.Final.NextPartition;
}
// get route coords
if (lp.Value.laneWaypointOfInterest.BestRoute != null)
{
foreach (INavigableNode inn in lp.Value.laneWaypointOfInterest.BestRoute)
{
route.Add(inn.Position);
}
}
RouteInformation ri = new RouteInformation(route, time, lp.Value.laneWaypointOfInterest.PointOfInterest.WaypointId.ToString());
routes.Add(ri);
}
routes.Sort();
return routes;
}