/// <summary>
/// Sets the speed limits of the areas of the network
/// </summary>
/// <param name="speedLimits"></param>
public void SetSpeedLimits(List <ArbiterSpeedLimit> speedLimits)
{
// loop over speed
foreach (ArbiterSpeedLimit asl in speedLimits)
{
// check if segment
if (asl.Area is ArbiterSegmentId)
{
// get seg
ArbiterSegment asg = this.ArbiterSegments[(ArbiterSegmentId)asl.Area];
// set speed
asg.SpeedLimits = asl;
asg.SpeedLimits.MaximumSpeed = Math.Min(asg.SpeedLimits.MaximumSpeed, 13.4112);
}
// check if zone
else if (asl.Area is ArbiterZoneId)
{
// get zone
ArbiterZone az = this.ArbiterZones[(ArbiterZoneId)asl.Area];
// set speed
az.SpeedLimits = asl;
az.SpeedLimits.MaximumSpeed = Math.Min(az.SpeedLimits.MaximumSpeed, 2.24);
az.SpeedLimits.MinimumSpeed = Math.Min(az.SpeedLimits.MinimumSpeed, 2.24);
}
// unknown
else
{
// notify
Console.WriteLine("Unknown speed limit area type: " + asl.Area.ToString());
}
}
}
/// <summary>
/// Directed edge between two INavigableNodes
/// </summary>
/// <remarks>Can be made up of multiple partitions or zones</remarks>
public NavigableEdge(bool isZone, ArbiterZone zone, bool isSegment, ArbiterSegment segment, List <IConnectAreaWaypoints> contained,
INavigableNode start, INavigableNode end)
{
this.Start = start;
this.End = end;
this.Segment = segment;
this.IsSegment = isSegment;
this.Zone = zone;
this.IsZone = isZone;
this.Contained = contained == null ? new List <IConnectAreaWaypoints>() : contained;
this.standardEdgeDistance = this.Start.Position.DistanceTo(this.End.Position);
}
public void SelectZone(Coordinates c)
{
this.current = null;
if (this.parentTool.arn != null)
{
foreach(ArbiterZone az in this.parentTool.arn.ArbiterZones.Values)
{
if (az.Perimeter.PerimeterPolygon.IsInside(c))
{
this.current = az;
this.ZoneToolboxSelectedZoneTextBox.Text = az.ToString();
this.Invalidate();
}
}
}
if (this.current == null)
{
this.ZoneToolboxSelectedZoneTextBox.Text = "None";
}
}
private void ZoneToolbox_Load(object sender, EventArgs e)
{
this.current = null;
}
private ArbiterZoneNavigableNode[][] NodeMatrix(ArbiterZone az)
{
// get bL and tR
Coordinates bl;
Coordinates tr;
this.GetBoundingPoints(az, out bl, out tr);
int n = (int)tr.Y;
int s = (int)bl.Y;
int e = (int)tr.X;
int w = (int)bl.X;
// create matrix
ArbiterZoneNavigableNode[][] nodeMatrix = new ArbiterZoneNavigableNode[e - w + 1][];
// loop through coordinates
for (int i = w; i <= e; i++)
{
for (int j = s; j <= n; j++)
{
// position
Coordinates c = new Coordinates((double)i, (double)j);
// check inside perimeter
if (az.Perimeter.PerimeterPolygon.IsInside(c))
{
// check interacts
bool clear = true;
foreach (Polygon o in az.StayOutAreas)
{
if (o.IsInside(c))
clear = false;
}
// not inside out of polys
if (clear)
{
nodeMatrix[i - w][j - s] = new ArbiterZoneNavigableNode(c);
}
}
}
}
// return
return nodeMatrix;
}
private void GetBoundingPoints(ArbiterZone az, out Coordinates bl, out Coordinates tr)
{
// get bL and tR
double n = Double.MinValue;
double s = Double.MaxValue;
double e = Double.MinValue;
double w = Double.MaxValue;
foreach (ArbiterPerimeterWaypoint apw in this.current.Perimeter.PerimeterPoints.Values)
{
if (apw.Position.X > e)
e = apw.Position.X;
if (apw.Position.X < w)
w = apw.Position.X;
if (apw.Position.Y > n)
n = apw.Position.Y;
if (apw.Position.Y < s)
s = apw.Position.Y;
}
bl = new Coordinates(w, s);
tr = new Coordinates(n, e);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneStartupState(ArbiterZone zone, bool unreachable)
: base(zone)
{
this.unreachablePathExists = unreachable;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneStartupState(ArbiterZone zone)
: base(zone)
{
}
/// <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>
/// 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>
/// Constructor
/// </summary>
/// <param name="zone"></param>
/// <param name="parkingSpot"></param>
public ParkingState(ArbiterZone zone, ArbiterParkingSpot parkingSpot)
: base(zone)
{
this.ParkingSpot = parkingSpot;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneOrientationState(ArbiterZone zone, NavigableEdge final)
: base(zone)
{
this.final = final;
}
public SceneZonePartition(ArbiterZone az)
{
this.Zone = az;
apws = new ArbiterPerimeterWaypoint[Zone.Perimeter.PerimeterPoints.Count];
Zone.Perimeter.PerimeterPoints.Values.CopyTo(apws, 0);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneTravelingState(ArbiterZone zone, INavigableNode start)
: base(zone)
{
this.Start = start;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneState(ArbiterZone zone)
{
this.Zone = zone;
}
/// <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;
}
