/// <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>
/// 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;
}
/// <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>
/// 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>
/// 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>
/// Generates the xySegments into segments and inputs them into the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
{
foreach (SimpleSegment ss in segments)
{
// seg
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
ArbiterSegment asg = new ArbiterSegment(asi);
arn.ArbiterSegments.Add(asi, asg);
asg.RoadNetwork = arn;
asg.SpeedLimits = new ArbiterSpeedLimit();
asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed
// way1
ArbiterWayId awi1 = new ArbiterWayId(1, asi);
ArbiterWay aw1 = new ArbiterWay(awi1);
aw1.Segment = asg;
asg.Ways.Add(awi1, aw1);
asg.Way1 = aw1;
// way2
ArbiterWayId awi2 = new ArbiterWayId(2, asi);
ArbiterWay aw2 = new ArbiterWay(awi2);
aw2.Segment = asg;
asg.Ways.Add(awi2, aw2);
asg.Way2 = aw2;
// make lanes
foreach (SimpleLane sl in ss.Lanes)
{
// lane
ArbiterLaneId ali;
ArbiterLane al;
// get way of lane id
if (ss.Way1Lanes.Contains(sl))
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
al = new ArbiterLane(ali);
aw1.Lanes.Add(ali, al);
al.Way = aw1;
}
else
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
al = new ArbiterLane(ali);
aw2.Lanes.Add(ali, al);
al.Way = aw2;
}
// add to display
arn.DisplayObjects.Add(al);
// width
al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;
if (sl.LaneWidth == 0)
{
Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
}
// lane boundaries
al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound);
al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);
// add lane to seg
asg.Lanes.Add(ali, al);
// waypoints
List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>();
// generate waypoints
foreach (SimpleWaypoint sw in sl.Waypoints)
{
// waypoint
ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi);
aw.Lane = al;
// stop
if (sl.Stops.Contains(sw.ID))
{
aw.IsStop = true;
}
// checkpoint
foreach (SimpleCheckpoint sc in sl.Checkpoints)
{
if (sw.ID == sc.WaypointId)
{
aw.IsCheckpoint = true;
aw.CheckpointId = int.Parse(sc.CheckpointId);
arn.Checkpoints.Add(aw.CheckpointId, aw);
}
}
// add
asg.Waypoints.Add(awi, aw);
arn.ArbiterWaypoints.Add(awi, aw);
al.Waypoints.Add(awi, aw);
waypointList.Add(aw);
arn.DisplayObjects.Add(aw);
arn.LegacyWaypointLookup.Add(sw.ID, aw);
}
al.WaypointList = waypointList;
// lane partitions
List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>();
al.Partitions = alps;
// generate lane partitions
for (int i = 0; i < waypointList.Count - 1; i++)
{
// create lane partition
ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg);
alp.Lane = al;
waypointList[i].NextPartition = alp;
waypointList[i + 1].PreviousPartition = alp;
alps.Add(alp);
arn.DisplayObjects.Add(alp);
// crete initial user partition
ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>();
aups.Add(aup);
alp.UserPartitions = aups;
alp.SetDefaultSparsePolygon();
arn.DisplayObjects.Add(aup);
}
// path segments of lane
List <IPathSegment> ips = new List <IPathSegment>();
List <Coordinates> pathSegments = new List <Coordinates>();
pathSegments.Add(alps[0].Initial.Position);
// loop
foreach (ArbiterLanePartition alPar in alps)
{
ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
// make new segment
pathSegments.Add(alPar.Final.Position);
}
// generate lane partition path
LinePath partitionPath = new LinePath(pathSegments);
al.SetLanePath(partitionPath);
al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);
// safeto zones
foreach (ArbiterWaypoint aw in al.Waypoints.Values)
{
if (aw.IsStop)
{
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
arn.DisplayObjects.Add(asz);
arn.ArbiterSafetyZones.Add(asz);
}
}
}
}
return(arn);
}
/// <summary>
/// Generates the xySegments into segments and inputs them into the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
{
foreach (SimpleSegment ss in segments)
{
// seg
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
ArbiterSegment asg = new ArbiterSegment(asi);
arn.ArbiterSegments.Add(asi, asg);
asg.RoadNetwork = arn;
asg.SpeedLimits = new ArbiterSpeedLimit();
asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed
// way1
ArbiterWayId awi1 = new ArbiterWayId(1, asi);
ArbiterWay aw1 = new ArbiterWay(awi1);
aw1.Segment = asg;
asg.Ways.Add(awi1, aw1);
asg.Way1 = aw1;
// way2
ArbiterWayId awi2 = new ArbiterWayId(2, asi);
ArbiterWay aw2 = new ArbiterWay(awi2);
aw2.Segment = asg;
asg.Ways.Add(awi2, aw2);
asg.Way2 = aw2;
// make lanes
foreach (SimpleLane sl in ss.Lanes)
{
// lane
ArbiterLaneId ali;
ArbiterLane al;
// get way of lane id
if (ss.Way1Lanes.Contains(sl))
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
al = new ArbiterLane(ali);
aw1.Lanes.Add(ali, al);
al.Way = aw1;
}
else
{
ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
al = new ArbiterLane(ali);
aw2.Lanes.Add(ali, al);
al.Way = aw2;
}
// add to display
arn.DisplayObjects.Add(al);
// width
al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;
if(sl.LaneWidth == 0)
Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
// lane boundaries
al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound);
al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);
// add lane to seg
asg.Lanes.Add(ali, al);
// waypoints
List<ArbiterWaypoint> waypointList = new List<ArbiterWaypoint>();
// generate waypoints
foreach (SimpleWaypoint sw in sl.Waypoints)
{
// waypoint
ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi);
aw.Lane = al;
// stop
if (sl.Stops.Contains(sw.ID))
{
aw.IsStop = true;
}
// checkpoint
foreach (SimpleCheckpoint sc in sl.Checkpoints)
{
if (sw.ID == sc.WaypointId)
{
aw.IsCheckpoint = true;
aw.CheckpointId = int.Parse(sc.CheckpointId);
arn.Checkpoints.Add(aw.CheckpointId, aw);
}
}
// add
asg.Waypoints.Add(awi, aw);
arn.ArbiterWaypoints.Add(awi, aw);
al.Waypoints.Add(awi, aw);
waypointList.Add(aw);
arn.DisplayObjects.Add(aw);
arn.LegacyWaypointLookup.Add(sw.ID, aw);
}
al.WaypointList = waypointList;
// lane partitions
List<ArbiterLanePartition> alps = new List<ArbiterLanePartition>();
al.Partitions = alps;
// generate lane partitions
for (int i = 0; i < waypointList.Count-1; i++)
{
// create lane partition
ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i+1], asg);
alp.Lane = al;
waypointList[i].NextPartition = alp;
waypointList[i + 1].PreviousPartition = alp;
alps.Add(alp);
arn.DisplayObjects.Add(alp);
// crete initial user partition
ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
List<ArbiterUserPartition> aups = new List<ArbiterUserPartition>();
aups.Add(aup);
alp.UserPartitions = aups;
alp.SetDefaultSparsePolygon();
arn.DisplayObjects.Add(aup);
}
// path segments of lane
List<IPathSegment> ips = new List<IPathSegment>();
List<Coordinates> pathSegments = new List<Coordinates>();
pathSegments.Add(alps[0].Initial.Position);
// loop
foreach(ArbiterLanePartition alPar in alps)
{
ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
// make new segment
pathSegments.Add(alPar.Final.Position);
}
// generate lane partition path
LinePath partitionPath = new LinePath(pathSegments);
al.SetLanePath(partitionPath);
al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);
// safeto zones
foreach (ArbiterWaypoint aw in al.Waypoints.Values)
{
if(aw.IsStop)
{
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
arn.DisplayObjects.Add(asz);
arn.ArbiterSafetyZones.Add(asz);
}
}
}
}
return arn;
}
