public PointAnalysisTool(PlanarProjection projection, bool snap, ArbiterRoadNetwork arn, WorldTransform wt)
{
this.projection = projection;
this.snapToWaypoints = snap;
this.roadNetwork = arn;
this.wt = wt;
}
/// <summary>
/// World service
/// </summary>
/// <param name="arn"></param>
public WorldService(ArbiterRoadNetwork arn)
{
this.RoadNetwork = arn;
this.worldState = new WorldState();
this.lidar = new SimSensor(Math.PI/3, -Math.PI/3, 0.5*Math.PI/180.0, SensorType.Scan, 50);
this.lidar2 = new SimSensor(4 * Math.PI / 3, 2 * Math.PI / 3, 0.5 * Math.PI / 180.0, SensorType.Scan, 30);
}
public ShiftNetwork(ArbiterRoadNetwork roads, RoadDisplay roadDisplay)
{
InitializeComponent();
this.roads = roads;
this.display = roadDisplay;
}
public IntersectionToolbox(ArbiterRoadNetwork arn, RoadDisplay rd, Editor ed)
{
this.arn = arn;
this.rd = rd;
this.ed = ed;
InitializeComponent();
}
/// <summary>
/// Generates lane partition adjacency
/// </summary>
/// <param name="arn"></param>
private void GenerateLanePartitionAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// loop over lanes
foreach (ArbiterLane al in asg.Lanes.Values)
{
// loop over lane partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
// left lane
if (al.LaneOnLeft != null)
{
this.GenerateSinglePartitionAdjacency(alp, al.LaneOnLeft);
}
// right lane
if (al.LaneOnRight != null)
{
this.GenerateSinglePartitionAdjacency(alp, al.LaneOnRight);
}
}
}
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zoneId"></param>
/// <param name="perimter"></param>
/// <param name="parkingSpots"></param>
public ArbiterZone(ArbiterZoneId zoneId, ArbiterPerimeter perimter,
List<ArbiterParkingSpot> parkingSpots, ArbiterRoadNetwork roadNetwork)
{
this.ZoneId = zoneId;
this.Perimeter = perimter;
this.ParkingSpots = parkingSpots;
this.ParkingSpotWaypoints = new Dictionary<ArbiterParkingSpotWaypointId, ArbiterParkingSpotWaypoint>();
this.ZoneExits = new List<ArbiterPerimeterWaypoint>();
this.StayOutAreas = new List<Polygon>();
this.NavigationNodes = new List<INavigableNode>();
this.NavigableEdges = new List<NavigableEdge>();
this.RoadNetwork = roadNetwork;
// set zone
this.Perimeter.Zone = this;
// create waypoint lookup
foreach (ArbiterParkingSpot aps in parkingSpots)
{
// set zone
aps.Zone = this;
foreach (ArbiterParkingSpotWaypoint apsw in aps.Waypoints.Values)
{
ParkingSpotWaypoints.Add(apsw.WaypointId, apsw);
}
}
// create exits
foreach (ArbiterPerimeterWaypoint apw in perimter.PerimeterPoints.Values)
{
ZoneExits.Add(apw);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zoneId"></param>
/// <param name="perimter"></param>
/// <param name="parkingSpots"></param>
public ArbiterZone(ArbiterZoneId zoneId, ArbiterPerimeter perimter,
List <ArbiterParkingSpot> parkingSpots, ArbiterRoadNetwork roadNetwork)
{
this.ZoneId = zoneId;
this.Perimeter = perimter;
this.ParkingSpots = parkingSpots;
this.ParkingSpotWaypoints = new Dictionary <ArbiterParkingSpotWaypointId, ArbiterParkingSpotWaypoint>();
this.ZoneExits = new List <ArbiterPerimeterWaypoint>();
this.StayOutAreas = new List <Polygon>();
this.NavigationNodes = new List <INavigableNode>();
this.NavigableEdges = new List <NavigableEdge>();
this.RoadNetwork = roadNetwork;
// set zone
this.Perimeter.Zone = this;
// create waypoint lookup
foreach (ArbiterParkingSpot aps in parkingSpots)
{
// set zone
aps.Zone = this;
foreach (ArbiterParkingSpotWaypoint apsw in aps.Waypoints.Values)
{
ParkingSpotWaypoints.Add(apsw.WaypointId, apsw);
}
}
// create exits
foreach (ArbiterPerimeterWaypoint apw in perimter.PerimeterPoints.Values)
{
ZoneExits.Add(apw);
}
}
/// <summary>
/// Generates all adjacency mappings for the road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateAdjacencies(ArbiterRoadNetwork arn)
{
// generate lane adjacency
this.GenerateLaneAdjacency(arn);
// generate partition adjacency
this.GenerateLanePartitionAdjacency(arn);
// generate entry adjacency
this.GenerateNavigationalAdjacency(arn);
// return
return arn;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="p"></param>
/// <param name="exits"></param>
/// <param name="involved"></param>
/// <param name="incoming"></param>
public ArbiterIntersection(Polygon p, List<ArbiterStoppedExit> exits, Dictionary<ArbiterInterconnect, List<IntersectionInvolved>> priority,
Dictionary<ArbiterLane, LinePath.PointOnPath> incoming, Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> allExits, Coordinates center,
ArbiterIntersectionId id, ArbiterRoadNetwork network, Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> entries)
{
this.IntersectionPolygon = p;
this.StoppedExits = exits;
this.PriorityLanes = priority;
this.IncomingLanePoints = incoming;
this.AllExits = allExits;
this.Center = center;
this.IntersectionId = id;
this.RoadNetwork = network;
this.AllEntries = entries;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="p"></param>
/// <param name="exits"></param>
/// <param name="involved"></param>
/// <param name="incoming"></param>
public ArbiterIntersection(Polygon p, List <ArbiterStoppedExit> exits, Dictionary <ArbiterInterconnect, List <IntersectionInvolved> > priority,
Dictionary <ArbiterLane, LinePath.PointOnPath> incoming, Dictionary <IAreaSubtypeWaypointId, ITraversableWaypoint> allExits, Coordinates center,
ArbiterIntersectionId id, ArbiterRoadNetwork network, Dictionary <IAreaSubtypeWaypointId, ITraversableWaypoint> entries)
{
this.IntersectionPolygon = p;
this.StoppedExits = exits;
this.PriorityLanes = priority;
this.IncomingLanePoints = incoming;
this.AllExits = allExits;
this.Center = center;
this.IntersectionId = id;
this.RoadNetwork = network;
this.AllEntries = entries;
}
/// <summary>
/// Generates all adjacency mappings for the road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateAdjacencies(ArbiterRoadNetwork arn)
{
// generate lane adjacency
this.GenerateLaneAdjacency(arn);
// generate partition adjacency
this.GenerateLanePartitionAdjacency(arn);
// generate entry adjacency
this.GenerateNavigationalAdjacency(arn);
// return
return(arn);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="arn"></param>
/// <param name="rd"></param>
/// <param name="ed"></param>
public ZoneTool(ArbiterRoadNetwork arn, RoadDisplay rd, Editor ed)
{
// set helpers we can access
this.arn = arn;
this.rd = rd;
this.ed = ed;
// helpers to wrap intersections for polygons
this.WrappingHelpers = new List<Coordinates>();
// create toolbox
zt = new ZoneToolbox(this);
zt.Show();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="arn"></param>
/// <param name="rd"></param>
/// <param name="ed"></param>
public IntersectionPulloutTool(ArbiterRoadNetwork arn, RoadDisplay rd, Editor ed, bool show)
{
// set helpers we can access
this.arn = arn;
this.rd = rd;
this.ed = ed;
// helpers to wrap intersections for polygons
this.WrappingHelpers = new List<Coordinates>();
// create toolbox
if (show)
{
it = new IntersectionToolbox(arn, rd, ed);
it.Show();
}
}
/// <summary>
/// Generate the arbiter road network from the internal xyRndf
/// </summary>
/// <returns></returns>
public ArbiterRoadNetwork GenerateRoadNetwork()
{
// the road network we're generating
ArbiterRoadNetwork arn = new ArbiterRoadNetwork();
arn.Name = xyRndf.Name;
arn.CreationDate = xyRndf.CreationDate;
// if zoens exist
if (xyRndf.Zones != null)
{
// construct zone generator
ZoneGeneration zg = new ZoneGeneration(xyRndf.Zones);
// generate zones
arn = zg.GenerateZones(arn);
}
// if segments exist
if (xyRndf.Segments != null)
{
// generate segments
SegmentGeneration sg = new SegmentGeneration(xyRndf.Segments);
// generate segments
arn = sg.GenerateSegments(arn);
}
// interconnects
InterconnectGeneration ig = new InterconnectGeneration(xyRndf);
arn = ig.GenerateInterconnects(arn);
// adjacency (lane, partition, entry)
AdjacencyGeneration ag = new AdjacencyGeneration();
arn = ag.GenerateAdjacencies(arn);
// other stuff (vehicle areas)
arn.GenerateVehicleAreas();
// return
return(arn);
}
/// <summary>
/// Generate the arbiter road network from the internal xyRndf
/// </summary>
/// <returns></returns>
public ArbiterRoadNetwork GenerateRoadNetwork()
{
// the road network we're generating
ArbiterRoadNetwork arn = new ArbiterRoadNetwork();
arn.Name = xyRndf.Name;
arn.CreationDate = xyRndf.CreationDate;
// if zoens exist
if (xyRndf.Zones != null)
{
// construct zone generator
ZoneGeneration zg = new ZoneGeneration(xyRndf.Zones);
// generate zones
arn = zg.GenerateZones(arn);
}
// if segments exist
if (xyRndf.Segments != null)
{
// generate segments
SegmentGeneration sg = new SegmentGeneration(xyRndf.Segments);
// generate segments
arn = sg.GenerateSegments(arn);
}
// interconnects
InterconnectGeneration ig = new InterconnectGeneration(xyRndf);
arn = ig.GenerateInterconnects(arn);
// adjacency (lane, partition, entry)
AdjacencyGeneration ag = new AdjacencyGeneration();
arn = ag.GenerateAdjacencies(arn);
// other stuff (vehicle areas)
arn.GenerateVehicleAreas();
// return
return arn;
}
/// <summary>
/// Generates mission
/// </summary>
/// <param name="mdf"></param>
/// <returns></returns>
public ArbiterMissionDescription GenerateMission(IMdf mdf, ArbiterRoadNetwork arn)
{
Queue<ArbiterCheckpoint> checks = new Queue<ArbiterCheckpoint>();
List<ArbiterSpeedLimit> speeds = new List<ArbiterSpeedLimit>();
// checkpoints
foreach (string s in mdf.CheckpointOrder)
{
int num = int.Parse(s);
checks.Enqueue(new ArbiterCheckpoint(num, arn.Checkpoints[num].AreaSubtypeWaypointId));
}
// speeds
foreach (SpeedLimit sl in mdf.SpeedLimits)
{
ArbiterSpeedLimit asl = new ArbiterSpeedLimit();
asl.MaximumSpeed = sl.MaximumVelocity * 0.44704;
asl.MinimumSpeed = sl.MinimumVelocity * 0.44704;
asl.Traveled = false;
ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(sl.SegmentID));
ArbiterZoneId azi = new ArbiterZoneId(int.Parse(sl.SegmentID));
if (arn.ArbiterZones.ContainsKey(azi))
asl.Area = azi;
else if (arn.ArbiterSegments.ContainsKey(asi))
asl.Area = asi;
else
throw new Exception("Unknown area id: " + sl.SegmentID);
speeds.Add(asl);
}
// return
return new ArbiterMissionDescription(checks, speeds);
}
/// <summary>
/// Generates interconnects into the road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn)
{
// list of all exit entries in the xy rndf
List <SimpleExitEntry> sees = new List <SimpleExitEntry>();
// zones
if (xyRndf.Zones != null)
{
// loop over zones
foreach (SimpleZone sz in xyRndf.Zones)
{
// add all ee's
sees.AddRange(sz.Perimeter.ExitEntries);
}
}
// segments
if (xyRndf.Segments != null)
{
// loop over segments
foreach (SimpleSegment ss in xyRndf.Segments)
{
// lanes
foreach (SimpleLane sl in ss.Lanes)
{
// add all ee's
sees.AddRange(sl.ExitEntries);
}
}
}
// loop over ee's and create interconnects
foreach (SimpleExitEntry see in sees)
{
IArbiterWaypoint initial = arn.LegacyWaypointLookup[see.ExitId];
IArbiterWaypoint final = arn.LegacyWaypointLookup[see.EntryId];
ArbiterInterconnect ai = new ArbiterInterconnect(initial, final);
arn.ArbiterInterconnects.Add(ai.InterconnectId, ai);
arn.DisplayObjects.Add(ai);
if (initial is ITraversableWaypoint)
{
ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial;
initialWaypoint.IsExit = true;
if (initialWaypoint.Exits == null)
{
initialWaypoint.Exits = new List <ArbiterInterconnect>();
}
initialWaypoint.Exits.Add(ai);
}
else
{
throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint");
}
if (final is ITraversableWaypoint)
{
ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final;
finalWaypoint.IsEntry = true;
if (finalWaypoint.Entries == null)
{
finalWaypoint.Entries = new List <ArbiterInterconnect>();
}
finalWaypoint.Entries.Add(ai);
}
else
{
throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint");
}
// set the turn direction
this.SetTurnDirection(ai);
// interconnectp olygon stuff
this.GenerateInterconnectPolygon(ai);
if (ai.TurnPolygon.IsComplex)
{
Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
return(arn);
}
/// <summary> /// Sets the road network and mission for the car /// </summary> /// <param name="roadNetwork"></param> /// <param name="mission"></param> public abstract void SetRoadNetworkAndMission(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission);
/// <summary>
/// Starts the vehicle
/// </summary>
/// <param name="arbiterRoadNetwork"></param>
/// <param name="arbiterMissionDescription"></param>
/// <returns></returns>
public bool StartupVehicle(ArbiterRoadNetwork arbiterRoadNetwork, ArbiterMissionDescription arbiterMissionDescription)
{
try
{
// get arbiter
this.client.ClientArbiter = (ArbiterAdvancedRemote)this.objectDirectory.Resolve("ArbiterAdvancedRemote_" + SimulatorClient.MachineName);
// set road and mission and spool up ai to wait for initial data stream
this.client.ClientArbiter.JumpstartArbiter(arbiterRoadNetwork, arbiterMissionDescription);
// success!
return true;
}
catch (Exception e)
{
Console.WriteLine("Error spooling up ai: \n" + e.ToString());
return false;
}
}
public abstract void JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission);
/// <summary>
/// Generates the arbiter zones form the internal xy zones for the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
/// <remarks>TODO: add zone cost map, adjacency of parking spots, figure out width</remarks>
public ArbiterRoadNetwork GenerateZones(ArbiterRoadNetwork arn)
{
Dictionary<ArbiterZoneId, ArbiterZone> zones = new Dictionary<ArbiterZoneId,ArbiterZone>();
List<IArbiterWaypoint> waypoints = new List<IArbiterWaypoint>();
foreach (SimpleZone sz in xyZones)
{
ArbiterZoneId azi = new ArbiterZoneId(int.Parse(sz.ZoneID));
#region Generate Perimeter
// old perim
ZonePerimeter zp = sz.Perimeter;
// perim id
ArbiterPerimeterId api = new ArbiterPerimeterId(GenerationTools.GetId(zp.PerimeterID)[1], azi);
#region Perimeter Waypoints
List<ArbiterPerimeterWaypoint> perimeterWaypoints = new List<ArbiterPerimeterWaypoint>();
foreach (PerimeterPoint pp in zp.PerimeterPoints)
{
// id
ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId(
GenerationTools.GetId(pp.ID)[2], api);
// point
ArbiterPerimeterWaypoint apw = new ArbiterPerimeterWaypoint(apwi, pp.position);
// add
perimeterWaypoints.Add(apw);
waypoints.Add(apw);
arn.DisplayObjects.Add(apw);
arn.LegacyWaypointLookup.Add(pp.ID, apw);
}
#endregion
// generate perimeter
ArbiterPerimeter ap = new ArbiterPerimeter(api, perimeterWaypoints);
arn.DisplayObjects.Add(ap);
// set per in points
foreach (ArbiterPerimeterWaypoint apw in perimeterWaypoints)
{
apw.Perimeter = ap;
}
#region Set Defined Links
// set links among perimeter nodes
for (int i = 1; i <= ap.PerimeterPoints.Count; i++)
{
ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId(i, ap.PerimeterId);
ArbiterPerimeterWaypoint apw = ap.PerimeterPoints[apwi];
if (i < ap.PerimeterPoints.Count)
{
ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(i+1, ap.PerimeterId);
apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext];
}
else
{
ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(1, ap.PerimeterId);
apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext];
}
}
#endregion
#endregion
#region Generate Parking Spots
List<ArbiterParkingSpot> parkingSpots = new List<ArbiterParkingSpot>();
#region Parking Spots
foreach (ParkingSpot ps in sz.ParkingSpots)
{
// spot id
int apsiNum = GenerationTools.GetId(ps.SpotID)[1];
ArbiterParkingSpotId apsi = new ArbiterParkingSpotId(apsiNum, azi);
// spot width
double spotWidth;
// check if spot width not set
if(ps.SpotWidth == null || ps.SpotWidth == "" || ps.SpotWidth == "0")
{
spotWidth = 3.0;
}
else
{
// convert feet to meters
spotWidth = double.Parse(ps.SpotWidth) * 0.3048;
}
// spot
ArbiterParkingSpot aps = new ArbiterParkingSpot(spotWidth, apsi);
arn.DisplayObjects.Add(aps);
// waypoints
List<ArbiterParkingSpotWaypoint> parkingSpotWaypoints = new List<ArbiterParkingSpotWaypoint>();
#region Parking Spot Waypoints
#region Waypoint 1
// id
int apwi1Number = GenerationTools.GetId(ps.Waypoint1.ID)[2];
ArbiterParkingSpotWaypointId apwi1 = new ArbiterParkingSpotWaypointId(apwi1Number, apsi);
// generate waypoint 1
ArbiterParkingSpotWaypoint apw1 = new ArbiterParkingSpotWaypoint(
ps.Waypoint1.Position, apwi1, aps);
// set
parkingSpotWaypoints.Add(apw1);
waypoints.Add(apw1);
arn.DisplayObjects.Add(apw1);
arn.LegacyWaypointLookup.Add(ps.Waypoint1.ID, apw1);
apw1.ParkingSpot = aps;
// checkpoint or not?
if (ps.CheckpointWaypointID == ps.Waypoint1.ID)
{
apw1.IsCheckpoint = true;
apw1.CheckpointId = int.Parse(ps.CheckpointID);
aps.Checkpoint = apw1;
arn.Checkpoints.Add(apw1.CheckpointId, apw1);
}
else
{
aps.NormalWaypoint = apw1;
}
#endregion
#region Waypoint 2
// id
int apwi2Number = GenerationTools.GetId(ps.Waypoint2.ID)[2];
ArbiterParkingSpotWaypointId apwi2 = new ArbiterParkingSpotWaypointId(apwi2Number, apsi);
// generate waypoint 2
ArbiterParkingSpotWaypoint apw2 = new ArbiterParkingSpotWaypoint(
ps.Waypoint2.Position, apwi2, aps);
// set
parkingSpotWaypoints.Add(apw2);
waypoints.Add(apw2);
arn.DisplayObjects.Add(apw2);
arn.LegacyWaypointLookup.Add(ps.Waypoint2.ID, apw2);
apw2.ParkingSpot = aps;
// checkpoint or not?
if (ps.CheckpointWaypointID == ps.Waypoint2.ID)
{
apw2.IsCheckpoint = true;
apw2.CheckpointId = int.Parse(ps.CheckpointID);
aps.Checkpoint = apw2;
arn.Checkpoints.Add(apw2.CheckpointId, apw2);
}
else
{
aps.NormalWaypoint = apw2;
}
#endregion
#endregion
// set waypoints
aps.SetWaypoints(parkingSpotWaypoints);
// add
parkingSpots.Add(aps);
}
#endregion
#endregion
#region Create Zone
// create zone
ArbiterZone az = new ArbiterZone(azi, ap, parkingSpots, arn);
// zone
az.SpeedLimits = new ArbiterSpeedLimit();
az.SpeedLimits.MaximumSpeed = 2.24;
az.SpeedLimits.MinimumSpeed = 2.24;
// add to final dictionary
zones.Add(az.ZoneId, az);
arn.DisplayObjects.Add(az);
#endregion
}
// set zones
arn.ArbiterZones = zones;
// add waypoints
foreach (IArbiterWaypoint iaw in waypoints)
{
// set waypoint
arn.ArbiterWaypoints.Add(iaw.AreaSubtypeWaypointId, iaw);
}
// return
return arn;
}
/// <summary>
/// Generates the arbiter zones form the internal xy zones for the input road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
/// <remarks>TODO: add zone cost map, adjacency of parking spots, figure out width</remarks>
public ArbiterRoadNetwork GenerateZones(ArbiterRoadNetwork arn)
{
Dictionary <ArbiterZoneId, ArbiterZone> zones = new Dictionary <ArbiterZoneId, ArbiterZone>();
List <IArbiterWaypoint> waypoints = new List <IArbiterWaypoint>();
foreach (SimpleZone sz in xyZones)
{
ArbiterZoneId azi = new ArbiterZoneId(int.Parse(sz.ZoneID));
#region Generate Perimeter
// old perim
ZonePerimeter zp = sz.Perimeter;
// perim id
ArbiterPerimeterId api = new ArbiterPerimeterId(GenerationTools.GetId(zp.PerimeterID)[1], azi);
#region Perimeter Waypoints
List <ArbiterPerimeterWaypoint> perimeterWaypoints = new List <ArbiterPerimeterWaypoint>();
foreach (PerimeterPoint pp in zp.PerimeterPoints)
{
// id
ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId(
GenerationTools.GetId(pp.ID)[2], api);
// point
ArbiterPerimeterWaypoint apw = new ArbiterPerimeterWaypoint(apwi, pp.position);
// add
perimeterWaypoints.Add(apw);
waypoints.Add(apw);
arn.DisplayObjects.Add(apw);
arn.LegacyWaypointLookup.Add(pp.ID, apw);
}
#endregion
// generate perimeter
ArbiterPerimeter ap = new ArbiterPerimeter(api, perimeterWaypoints);
arn.DisplayObjects.Add(ap);
// set per in points
foreach (ArbiterPerimeterWaypoint apw in perimeterWaypoints)
{
apw.Perimeter = ap;
}
#region Set Defined Links
// set links among perimeter nodes
for (int i = 1; i <= ap.PerimeterPoints.Count; i++)
{
ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId(i, ap.PerimeterId);
ArbiterPerimeterWaypoint apw = ap.PerimeterPoints[apwi];
if (i < ap.PerimeterPoints.Count)
{
ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(i + 1, ap.PerimeterId);
apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext];
}
else
{
ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(1, ap.PerimeterId);
apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext];
}
}
#endregion
#endregion
#region Generate Parking Spots
List <ArbiterParkingSpot> parkingSpots = new List <ArbiterParkingSpot>();
#region Parking Spots
foreach (ParkingSpot ps in sz.ParkingSpots)
{
// spot id
int apsiNum = GenerationTools.GetId(ps.SpotID)[1];
ArbiterParkingSpotId apsi = new ArbiterParkingSpotId(apsiNum, azi);
// spot width
double spotWidth;
// check if spot width not set
if (ps.SpotWidth == null || ps.SpotWidth == "" || ps.SpotWidth == "0")
{
spotWidth = 3.0;
}
else
{
// convert feet to meters
spotWidth = double.Parse(ps.SpotWidth) * 0.3048;
}
// spot
ArbiterParkingSpot aps = new ArbiterParkingSpot(spotWidth, apsi);
arn.DisplayObjects.Add(aps);
// waypoints
List <ArbiterParkingSpotWaypoint> parkingSpotWaypoints = new List <ArbiterParkingSpotWaypoint>();
#region Parking Spot Waypoints
#region Waypoint 1
// id
int apwi1Number = GenerationTools.GetId(ps.Waypoint1.ID)[2];
ArbiterParkingSpotWaypointId apwi1 = new ArbiterParkingSpotWaypointId(apwi1Number, apsi);
// generate waypoint 1
ArbiterParkingSpotWaypoint apw1 = new ArbiterParkingSpotWaypoint(
ps.Waypoint1.Position, apwi1, aps);
// set
parkingSpotWaypoints.Add(apw1);
waypoints.Add(apw1);
arn.DisplayObjects.Add(apw1);
arn.LegacyWaypointLookup.Add(ps.Waypoint1.ID, apw1);
apw1.ParkingSpot = aps;
// checkpoint or not?
if (ps.CheckpointWaypointID == ps.Waypoint1.ID)
{
apw1.IsCheckpoint = true;
apw1.CheckpointId = int.Parse(ps.CheckpointID);
aps.Checkpoint = apw1;
arn.Checkpoints.Add(apw1.CheckpointId, apw1);
}
else
{
aps.NormalWaypoint = apw1;
}
#endregion
#region Waypoint 2
// id
int apwi2Number = GenerationTools.GetId(ps.Waypoint2.ID)[2];
ArbiterParkingSpotWaypointId apwi2 = new ArbiterParkingSpotWaypointId(apwi2Number, apsi);
// generate waypoint 2
ArbiterParkingSpotWaypoint apw2 = new ArbiterParkingSpotWaypoint(
ps.Waypoint2.Position, apwi2, aps);
// set
parkingSpotWaypoints.Add(apw2);
waypoints.Add(apw2);
arn.DisplayObjects.Add(apw2);
arn.LegacyWaypointLookup.Add(ps.Waypoint2.ID, apw2);
apw2.ParkingSpot = aps;
// checkpoint or not?
if (ps.CheckpointWaypointID == ps.Waypoint2.ID)
{
apw2.IsCheckpoint = true;
apw2.CheckpointId = int.Parse(ps.CheckpointID);
aps.Checkpoint = apw2;
arn.Checkpoints.Add(apw2.CheckpointId, apw2);
}
else
{
aps.NormalWaypoint = apw2;
}
#endregion
#endregion
// set waypoints
aps.SetWaypoints(parkingSpotWaypoints);
// add
parkingSpots.Add(aps);
}
#endregion
#endregion
#region Create Zone
// create zone
ArbiterZone az = new ArbiterZone(azi, ap, parkingSpots, arn);
// zone
az.SpeedLimits = new ArbiterSpeedLimit();
az.SpeedLimits.MaximumSpeed = 2.24;
az.SpeedLimits.MinimumSpeed = 2.24;
// add to final dictionary
zones.Add(az.ZoneId, az);
arn.DisplayObjects.Add(az);
#endregion
}
// set zones
arn.ArbiterZones = zones;
// add waypoints
foreach (IArbiterWaypoint iaw in waypoints)
{
// set waypoint
arn.ArbiterWaypoints.Add(iaw.AreaSubtypeWaypointId, iaw);
}
// return
return(arn);
}
public abstract void SetRoadNetwork(ArbiterRoadNetwork roadNetwork);
/// <summary>
/// Generates lane adjacency in the network
/// </summary>
/// <param name="arn"></param>
private void GenerateLaneAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// check if both ways exist for first algorithm
if (asg.Way1.IsValid && asg.Way2.IsValid)
{
// lanes of the segment
Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// get a sample lane from way 1
Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator();
way1Enum.MoveNext();
ArbiterLane way1Sample = way1Enum.Current.Value;
// get a sample lane from way 2
Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator();
way2Enum.MoveNext();
ArbiterLane way2Sample = way2Enum.Current.Value;
// direction, 1 means way1 has lower # lanes
int modifier = 1;
// check if way 2 has lower lane numbers
if (way1Sample.LaneId.Number > way2Sample.LaneId.Number)
{
// set modifier to -1 so count other way
modifier = -1;
}
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// if not lane 1
if (al.LaneId.Number != 1)
{
// get lower # lane in way 1
ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId);
// check if the segment contains this lane
if (segLanes.ContainsKey(lowerNumWay1Id))
{
// get lane
ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id];
// check if current lane is also in way 1
if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to right
if (modifier == 1)
{
al.LaneOnRight = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
// otherwise -1 => lane is to left
else
{
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnRight = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
}
// otherwise the lowe lane is in way 2
else
{
// set lane
ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)];
// check if current lane is also in way 2
if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to left
if (modifier == 1)
{
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnRight = al;
}
// otherwise -1 => lane is to right
else
{
al.LaneOnRight = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
}
} // loop over lanes
} // both ways valid
// otherwise only a single way is valid
else
{
// lanes of the segment
Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// make sure more than one lane exists
if (segLanes.Count > 1)
{
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// get theoretical id of lane one number up
ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId);
// check if lane one number up exists
if (segLanes.ContainsKey(ali))
{
// get lane one number up
ArbiterLane alu = segLanes[ali];
// check # waypoints
if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1)
{
// get closest points on this lane and other lane
PointOnPath p1;
PointOnPath p2;
double distance;
CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance);
// get partition points of closest point on this lane
Coordinates partitionStart = p1.segment.Start;
Coordinates partitionEnd = p1.segment.End;
// get area of partition triangle
double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd);
// determine if closest point on other lane is to the left or right of partition
bool onLeft = true;
if (triangeArea >= 0)
{
onLeft = false;
}
// set adjacency accordingly for both lanes
if (onLeft)
{
al.LaneOnLeft = alu;
alu.LaneOnRight = al;
}
// otherwise on right
else
{
al.LaneOnRight = alu;
alu.LaneOnLeft = al;
}
}
}
}
}
} // end single way only valid
// loop over lanes to print info on adjacency
/*foreach (ArbiterLane al in asg.Lanes.Values)
* {
* Console.Write(al.LaneId.ToString() + ": ");
*
* if (al.LaneOnLeft != null)
* {
* Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString());
* }
*
* if (al.LaneOnRight != null)
* {
* Console.Write("Right-" + al.LaneOnRight.LaneId.ToString());
* }
*
* Console.Write("\n");
* }
*/
} // segment loop
}
/// <summary>
/// Generates lane partition adjacency
/// </summary>
/// <param name="arn"></param>
private void GenerateLanePartitionAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// loop over lanes
foreach (ArbiterLane al in asg.Lanes.Values)
{
// loop over lane partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
// left lane
if (al.LaneOnLeft != null)
{
this.GenerateSinglePartitionAdjacency(alp, al.LaneOnLeft);
}
// right lane
if (al.LaneOnRight != null)
{
this.GenerateSinglePartitionAdjacency(alp, al.LaneOnRight);
}
}
}
}
}
/// <summary>
/// Generates entry adjacency
/// </summary>
/// <param name="arn"></param>
/// <remarks>Determines for every entry in a segment, the closest, reachable
/// waypoints on lanes in the same way if it exists</remarks>
private void GenerateNavigationalAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach(ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// loop over segment waypoints
foreach(ArbiterWaypoint aw in asg.Waypoints.Values)
{
#region Next Waypoint
// check if has next
if (aw.NextPartition != null)
{
// add next waypoint
aw.OutgoingConnections.Add(aw.NextPartition);
}
#endregion
#region Exits
// check if exit
if (aw.IsExit)
{
// loop over interconnect
foreach (ArbiterInterconnect ai in aw.Exits)
{
// add entries
aw.OutgoingConnections.Add(ai);
}
}
#endregion
#region Adjacent Lanes
// check if entry
if (aw.IsEntry)
{
// foreach lane test in same way as aw
foreach (ArbiterLane al in aw.Lane.Way.Lanes.Values)
{
// check if not same lane
if (!aw.Lane.Equals(al) && al.RelativelyInside(aw.Position))
{
// check ok
if ((aw.Lane.LaneOnLeft != null && aw.Lane.LaneOnLeft.Equals(al) && aw.Lane.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(aw.Lane.LaneOnRight != null && aw.Lane.LaneOnRight.Equals(al) && aw.Lane.BoundaryRight != ArbiterLaneBoundary.SolidWhite))
{
// get closest partition to aw's point
ArbiterLanePartition alp = al.GetClosestPartition(aw.Position);
// check downstream from this lane?
if (aw.Lane.DistanceBetween(aw.Position, alp.Final.Position) >= -0.01 ||
(alp.Final.NextPartition != null && aw.Lane.DistanceBetween(aw.Position, alp.Final.NextPartition.Final.Position) >= -0.01))
{
// new list of contained partitions
List<IConnectAreaWaypoints> containedPartitions = new List<IConnectAreaWaypoints>();
containedPartitions.Add(alp);
containedPartitions.Add(aw.NextPartition);
// set aw as linking to that partition's final waypoint
aw.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, aw, alp.Final));
}
}
}
}
}
#endregion
}
#region Adjacent starting in middle
// loop over segment lanes
foreach (ArbiterLane al in asg.Lanes.Values)
{
// get init wp
ArbiterWaypoint initial = al.WaypointList[0];
// get adjacent lanes
foreach (ArbiterLane adj in al.Way.Lanes.Values)
{
if (!adj.Equals(al))
{
// check if initial is inside other
if (adj.RelativelyInside(initial.Position))
{
if ((adj.LaneOnLeft != null && adj.LaneOnLeft.Equals(al) && adj.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(adj.LaneOnRight != null && adj.LaneOnRight.Equals(al) && adj.BoundaryRight != ArbiterLaneBoundary.SolidWhite))
{
// closest partition
ArbiterLanePartition alp = adj.GetClosestPartition(initial.Position);
// new list of contained partitions
List<IConnectAreaWaypoints> containedPartitions = new List<IConnectAreaWaypoints>();
containedPartitions.Add(alp);
containedPartitions.Add(initial.NextPartition);
// set aw as linking to that partition's final waypoint
alp.Initial.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, alp.Initial, initial));
}
}
}
}
}
#endregion
}
// loop over zones
foreach (ArbiterZone az in arn.ArbiterZones.Values)
{
#region Perimeter Point adjacency
// loop over zone perimeter points
foreach (ArbiterPerimeterWaypoint apw in az.Perimeter.PerimeterPoints.Values)
{
#region Old Connectivity
// check if this is an entry
/*if (apw.IsEntry)
{
#region Link Perimeter Points
// loop over perimeter points
foreach(ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values)
{
// check not this and is exit
if (!apw.Equals(apwTest) && apwTest.IsExit)
{
// add to connections
apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apwTest));
}
}
#endregion
#region Link Checkpoints
// loop over parking spot waypoints
foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
{
// check if checkpoint
if (apsw.IsCheckpoint)
{
// add to connections
apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apsw));
}
}
#endregion
}*/
#endregion
// check if the point is an exit
if (apw.IsExit)
{
foreach (ArbiterInterconnect ai in apw.Exits)
{
// add to connections
apw.OutgoingConnections.Add(ai);
}
}
}
#endregion
#region Checkpoint adjacency
// loop over parking spot waypoints
foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
{
if (apsw.ParkingSpot.NormalWaypoint.Equals(apsw))
{
apsw.OutgoingConnections.Add(
new NavigableEdge(true, az, false, null, new List<IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.Checkpoint));
}
else
{
apsw.OutgoingConnections.Add(
new NavigableEdge(true, az, false, null, new List<IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.NormalWaypoint));
}
}
#region Old
/*
// loop over parking spot waypoints
foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
{
// check if checkpoint
if (apsw.IsCheckpoint)
{
#region Link Perimeter Points
// loop over perimeter points
foreach (ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values)
{
// check not this and is exit
if (apwTest.IsExit)
{
// add to connections
apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apwTest));
}
}
#endregion
#region Link Checkpoints
// loop over parking spot waypoints
foreach (ArbiterParkingSpotWaypoint apswTest in az.ParkingSpotWaypoints.Values)
{
// check if checkpoint
if (!apsw.Equals(apswTest) && apswTest.IsCheckpoint)
{
// add to connections
apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apswTest));
}
}
#endregion
}
}
*/
#endregion
#endregion
}
}
/// <summary>
/// Jumpstarts the ai with a new road network and mission if we can
/// </summary>
/// <param name="roadNetwork"></param>
/// <param name="mission"></param>
public override void JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission)
{
try
{
ArbiterOutput.Output("Jumpstarting Arbiter");
// warn if carmode not correct
CarMode carMode = CoreCommon.Communications.GetCarMode();
if (carMode != CarMode.Pause && carMode != CarMode.Human)
ArbiterOutput.Output("Warning: Vehicle is in CarMode: " + carMode.ToString());
if (roadNetwork != null && mission != null)
{
// destroy intelligence if exists
this.intelligenceCore.DestroyIntelligence();
// create roads and mission
roadNetwork.SetSpeedLimits(mission.SpeedLimits);
roadNetwork.GenerateVehicleAreas();
this.arbiterRoadNetwork = roadNetwork;
this.arbiterMissionDescription = mission;
CoreCommon.RoadNetwork = roadNetwork;
CoreCommon.Mission = mission;
// startup ai
this.intelligenceCore.Jumpstart();
}
else
{
ArbiterOutput.Output("RoadNetwork and Mission must both have value");
}
}
catch (Exception ex)
{
ArbiterOutput.Output("JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission) Failed", ex);
}
}
/// <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>
/// Restore from a saved editor save
/// </summary>
/// <param name="es"></param>
private void Restore(SimulatorSave es)
{
// restore the editor
this.gridSizeToolStripComboBox.SelectedIndex = es.GridSizeIndex;
this.arbiterRoads = es.ArbiterRoads;
this.simEngine = es.SimEngine;
this.simEngine.simulationMain = this;
this.simEngine.SetPropertyGrid(this.SimulatorPropertyGrid);
this.defaultMissionDescription = es.Mission;
// restore the display
this.roadDisplay1.LoadSave(es.displaySave);
// attempt to rehash the clients
this.clientHandler.ReBindAll(this.simEngine.Vehicles);
}
/// <summary>
/// Sets the road network
/// </summary>
/// <param name="roads"></param>
private void SetRoadNetwork(ArbiterRoadNetwork roads)
{
// set network
RemoraCommon.RoadNetwork = roads;
// clean out display
this.roadDisplay1.RemoveDisplayObjectType(this.roadDisplay1.RoadNetworkFilter);
// add to display
this.roadDisplay1.AddDisplayObjectRange(roads.DisplayObjects);
// notify
RemoraOutput.WriteLine("Set Road Network", OutputType.Remora);
// test mission
if (RemoraCommon.Mission != null)
{
RemoraOutput.WriteLine("Testing Previously Loaded Mission aginst new Road Network, Removing and Reloading", OutputType.Remora);
ArbiterMissionDescription tmp = RemoraCommon.Mission;
RemoraCommon.Mission = null;
this.SetMission(tmp);
}
// redraw
this.roadDisplay1.Invalidate();
}
/// <summary>
/// Open a road network from a file
/// </summary>
/// <param name="p"></param>
private void OpenRoadNetworkFromFile(string p)
{
// create file
FileStream fs = new FileStream(p, FileMode.Open);
// serializer
BinaryFormatter bf = new BinaryFormatter();
// serialize
ArbiterRoadNetwork es = (ArbiterRoadNetwork)bf.Deserialize(fs);
// remove previous road network
this.roadDisplay1.RemoveDisplayObjectType(this.roadDisplay1.RoadNetworkFilter);
// set netwrok
this.arbiterRoads = es;
// display
this.roadDisplay1.AddDisplayObjectRange(es.DisplayObjects);
// release holds
fs.Dispose();
}
/// <summary>
/// Generates lane adjacency in the network
/// </summary>
/// <param name="arn"></param>
private void GenerateLaneAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// check if both ways exist for first algorithm
if (asg.Way1.IsValid && asg.Way2.IsValid)
{
// lanes of the segment
Dictionary<ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// get a sample lane from way 1
Dictionary<ArbiterLaneId, ArbiterLane>.Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator();
way1Enum.MoveNext();
ArbiterLane way1Sample = way1Enum.Current.Value;
// get a sample lane from way 2
Dictionary<ArbiterLaneId, ArbiterLane>.Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator();
way2Enum.MoveNext();
ArbiterLane way2Sample = way2Enum.Current.Value;
// direction, 1 means way1 has lower # lanes
int modifier = 1;
// check if way 2 has lower lane numbers
if (way1Sample.LaneId.Number > way2Sample.LaneId.Number)
{
// set modifier to -1 so count other way
modifier = -1;
}
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// if not lane 1
if (al.LaneId.Number != 1)
{
// get lower # lane in way 1
ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId);
// check if the segment contains this lane
if (segLanes.ContainsKey(lowerNumWay1Id))
{
// get lane
ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id];
// check if current lane is also in way 1
if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to right
if (modifier == 1)
{
al.LaneOnRight = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
// otherwise -1 => lane is to left
else
{
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnRight = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay1;
lowerNumWay1.LaneOnLeft = al;
}
}
// otherwise the lowe lane is in way 2
else
{
// set lane
ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)];
// check if current lane is also in way 2
if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId))
{
// check modifier for 1 => lower is to left
if (modifier == 1)
{
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnRight = al;
}
// otherwise -1 => lane is to right
else
{
al.LaneOnRight = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
// otherwise the current lane is in a different way
else
{
// the lane is to the left by default
al.LaneOnLeft = lowerNumWay2;
lowerNumWay2.LaneOnLeft = al;
}
}
}
} // loop over lanes
} // both ways valid
// otherwise only a single way is valid
else
{
// lanes of the segment
Dictionary<ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;
// make sure more than one lane exists
if(segLanes.Count > 1)
{
// loop over lanes
foreach (ArbiterLane al in segLanes.Values)
{
// get theoretical id of lane one number up
ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId);
// check if lane one number up exists
if(segLanes.ContainsKey(ali))
{
// get lane one number up
ArbiterLane alu = segLanes[ali];
// check # waypoints
if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1)
{
// get closest points on this lane and other lane
PointOnPath p1;
PointOnPath p2;
double distance;
CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance);
// get partition points of closest point on this lane
Coordinates partitionStart = p1.segment.Start;
Coordinates partitionEnd = p1.segment.End;
// get area of partition triangle
double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd);
// determine if closest point on other lane is to the left or right of partition
bool onLeft = true;
if (triangeArea >= 0)
{
onLeft = false;
}
// set adjacency accordingly for both lanes
if (onLeft)
{
al.LaneOnLeft = alu;
alu.LaneOnRight = al;
}
// otherwise on right
else
{
al.LaneOnRight = alu;
alu.LaneOnLeft = al;
}
}
}
}
}
} // end single way only valid
// loop over lanes to print info on adjacency
/*foreach (ArbiterLane al in asg.Lanes.Values)
{
Console.Write(al.LaneId.ToString() + ": ");
if (al.LaneOnLeft != null)
{
Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString());
}
if (al.LaneOnRight != null)
{
Console.Write("Right-" + al.LaneOnRight.LaneId.ToString());
}
Console.Write("\n");
}
*/
} // segment loop
}
/// <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>
/// Begin simulation
/// </summary>
public void BeginSimulation(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission)
{
try
{
// startup clients
foreach (KeyValuePair<SimVehicleId, SimVehicle> vhcs in this.Vehicles)
{
// set road and mission
SimulatorClientFacade scf = this.simulationMain.clientHandler.AvailableClients[this.simulationMain.clientHandler.VehicleToClientMap[vhcs.Key]];
// check if we need to randomize mission
if (vhcs.Value.RandomMission)
{
// create random mission
Queue<ArbiterCheckpoint> checks = new Queue<ArbiterCheckpoint>(60);
int num = mission.MissionCheckpoints.Count - 1;
ArbiterCheckpoint[] checkPointArray = mission.MissionCheckpoints.ToArray();
Random r = new Random();
for (int i = 0; i < 60; i++)
{
checks.Enqueue(checkPointArray[r.Next(num)]);
}
ArbiterMissionDescription amd = new ArbiterMissionDescription(checks, mission.SpeedLimits);
// set road , mission
scf.SetRoadNetworkAndMission(roadNetwork, amd);
}
// otherwise no random mission
else
{
// set road , mission
scf.SetRoadNetworkAndMission(roadNetwork, mission);
}
// startup ai
bool b = scf.StartupVehicle();
// check for false
if (!b)
{
Console.WriteLine("Error starting simulation for vehicle id: " + vhcs.Key.ToString());
return;
}
}
}
catch (Exception e)
{
Console.WriteLine("Error starting simulation: \n" + e.ToString());
return;
}
// run sim
this.RunSimulation();
}
/// <summary>
/// Sets the road network and mission for the car
/// </summary>
/// <param name="roadNetwork"></param>
/// <param name="mission"></param>
public override void SetRoadNetworkAndMission(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission)
{
this.roadNetwork = roadNetwork;
this.mission = mission;
this.roadNetwork.SetSpeedLimits(mission.SpeedLimits);
this.world = new WorldService(this.roadNetwork);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="roadNetwork"></param>
public SceneRoadGraphGeneration(ArbiterRoadNetwork roadNetwork)
{
this.roadNetwork = roadNetwork;
}
/// <summary>
/// Generates entry adjacency
/// </summary>
/// <param name="arn"></param>
/// <remarks>Determines for every entry in a segment, the closest, reachable
/// waypoints on lanes in the same way if it exists</remarks>
private void GenerateNavigationalAdjacency(ArbiterRoadNetwork arn)
{
// loop over segments
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
// loop over segment waypoints
foreach (ArbiterWaypoint aw in asg.Waypoints.Values)
{
#region Next Waypoint
// check if has next
if (aw.NextPartition != null)
{
// add next waypoint
aw.OutgoingConnections.Add(aw.NextPartition);
}
#endregion
#region Exits
// check if exit
if (aw.IsExit)
{
// loop over interconnect
foreach (ArbiterInterconnect ai in aw.Exits)
{
// add entries
aw.OutgoingConnections.Add(ai);
}
}
#endregion
#region Adjacent Lanes
// check if entry
if (aw.IsEntry)
{
// foreach lane test in same way as aw
foreach (ArbiterLane al in aw.Lane.Way.Lanes.Values)
{
// check if not same lane
if (!aw.Lane.Equals(al) && al.RelativelyInside(aw.Position))
{
// check ok
if ((aw.Lane.LaneOnLeft != null && aw.Lane.LaneOnLeft.Equals(al) && aw.Lane.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(aw.Lane.LaneOnRight != null && aw.Lane.LaneOnRight.Equals(al) && aw.Lane.BoundaryRight != ArbiterLaneBoundary.SolidWhite))
{
// get closest partition to aw's point
ArbiterLanePartition alp = al.GetClosestPartition(aw.Position);
// check downstream from this lane?
if (aw.Lane.DistanceBetween(aw.Position, alp.Final.Position) >= -0.01 ||
(alp.Final.NextPartition != null && aw.Lane.DistanceBetween(aw.Position, alp.Final.NextPartition.Final.Position) >= -0.01))
{
// new list of contained partitions
List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>();
containedPartitions.Add(alp);
containedPartitions.Add(aw.NextPartition);
// set aw as linking to that partition's final waypoint
aw.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, aw, alp.Final));
}
}
}
}
}
#endregion
}
#region Adjacent starting in middle
// loop over segment lanes
foreach (ArbiterLane al in asg.Lanes.Values)
{
// get init wp
ArbiterWaypoint initial = al.WaypointList[0];
// get adjacent lanes
foreach (ArbiterLane adj in al.Way.Lanes.Values)
{
if (!adj.Equals(al))
{
// check if initial is inside other
if (adj.RelativelyInside(initial.Position))
{
if ((adj.LaneOnLeft != null && adj.LaneOnLeft.Equals(al) && adj.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(adj.LaneOnRight != null && adj.LaneOnRight.Equals(al) && adj.BoundaryRight != ArbiterLaneBoundary.SolidWhite))
{
// closest partition
ArbiterLanePartition alp = adj.GetClosestPartition(initial.Position);
// new list of contained partitions
List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>();
containedPartitions.Add(alp);
containedPartitions.Add(initial.NextPartition);
// set aw as linking to that partition's final waypoint
alp.Initial.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, alp.Initial, initial));
}
}
}
}
}
#endregion
}
// loop over zones
foreach (ArbiterZone az in arn.ArbiterZones.Values)
{
#region Perimeter Point adjacency
// loop over zone perimeter points
foreach (ArbiterPerimeterWaypoint apw in az.Perimeter.PerimeterPoints.Values)
{
#region Old Connectivity
// check if this is an entry
/*if (apw.IsEntry)
* {
#region Link Perimeter Points
*
* // loop over perimeter points
* foreach(ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values)
* {
* // check not this and is exit
* if (!apw.Equals(apwTest) && apwTest.IsExit)
* {
* // add to connections
* apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apwTest));
* }
* }
*
#endregion
*
#region Link Checkpoints
*
* // loop over parking spot waypoints
* foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
* {
* // check if checkpoint
* if (apsw.IsCheckpoint)
* {
* // add to connections
* apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apsw));
* }
* }
*
#endregion
* }*/
#endregion
// check if the point is an exit
if (apw.IsExit)
{
foreach (ArbiterInterconnect ai in apw.Exits)
{
// add to connections
apw.OutgoingConnections.Add(ai);
}
}
}
#endregion
#region Checkpoint adjacency
// loop over parking spot waypoints
foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
{
if (apsw.ParkingSpot.NormalWaypoint.Equals(apsw))
{
apsw.OutgoingConnections.Add(
new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.Checkpoint));
}
else
{
apsw.OutgoingConnections.Add(
new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.NormalWaypoint));
}
}
#region Old
/*
* // loop over parking spot waypoints
* foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values)
* {
* // check if checkpoint
* if (apsw.IsCheckpoint)
* {
#region Link Perimeter Points
*
* // loop over perimeter points
* foreach (ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values)
* {
* // check not this and is exit
* if (apwTest.IsExit)
* {
* // add to connections
* apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apwTest));
* }
* }
*
#endregion
*
#region Link Checkpoints
*
* // loop over parking spot waypoints
* foreach (ArbiterParkingSpotWaypoint apswTest in az.ParkingSpotWaypoints.Values)
* {
* // check if checkpoint
* if (!apsw.Equals(apswTest) && apswTest.IsCheckpoint)
* {
* // add to connections
* apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apswTest));
* }
* }
*
#endregion
* }
* }
*/
#endregion
#endregion
}
}
public IntersectionPulloutTool(ArbiterRoadNetwork arn)
{
this.arn = arn;
}
public RulerTool(bool snap, ArbiterRoadNetwork arn, WorldTransform wt)
{
this.snapToWaypoints = snap;
this.roadNetwork = arn;
this.wt = wt;
}
/// <summary>
/// Generates interconnects into the road network
/// </summary>
/// <param name="arn"></param>
/// <returns></returns>
public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn)
{
// list of all exit entries in the xy rndf
List<SimpleExitEntry> sees = new List<SimpleExitEntry>();
// zones
if(xyRndf.Zones != null)
{
// loop over zones
foreach (SimpleZone sz in xyRndf.Zones)
{
// add all ee's
sees.AddRange(sz.Perimeter.ExitEntries);
}
}
// segments
if (xyRndf.Segments != null)
{
// loop over segments
foreach (SimpleSegment ss in xyRndf.Segments)
{
// lanes
foreach (SimpleLane sl in ss.Lanes)
{
// add all ee's
sees.AddRange(sl.ExitEntries);
}
}
}
// loop over ee's and create interconnects
foreach (SimpleExitEntry see in sees)
{
IArbiterWaypoint initial = arn.LegacyWaypointLookup[see.ExitId];
IArbiterWaypoint final = arn.LegacyWaypointLookup[see.EntryId];
ArbiterInterconnect ai = new ArbiterInterconnect(initial, final);
arn.ArbiterInterconnects.Add(ai.InterconnectId, ai);
arn.DisplayObjects.Add(ai);
if (initial is ITraversableWaypoint)
{
ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial;
initialWaypoint.IsExit = true;
if (initialWaypoint.Exits == null)
initialWaypoint.Exits = new List<ArbiterInterconnect>();
initialWaypoint.Exits.Add(ai);
}
else
{
throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint");
}
if (final is ITraversableWaypoint)
{
ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final;
finalWaypoint.IsEntry = true;
if (finalWaypoint.Entries == null)
finalWaypoint.Entries = new List<ArbiterInterconnect>();
finalWaypoint.Entries.Add(ai);
}
else
{
throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint");
}
// set the turn direction
this.SetTurnDirection(ai);
// interconnectp olygon stuff
this.GenerateInterconnectPolygon(ai);
if (ai.TurnPolygon.IsComplex)
{
Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
return arn;
}