/// <summary>
/// finish the wrapping and cretae the intersection
/// </summary>
public void FinalizeIntersection()
{
// finalize the intersection polygon
Polygon sq = this.CreateSquarePolygon(this.WrapInitial.Value, this.WrapFinal.Value).Inflate(1.0);
Polygon interPoly = this.CreateIntersectionPolygon(sq);
// check poly not null
if (interPoly != null)
{
// inflate the inter poly
interPoly = interPoly.Inflate(0.05);
// retreive all exits involved in this intersection
Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> exits = this.IntersectionExits(sq);
// make sure the inter contains an exit
if (exits.Count > 0)
{
// make stopped exits, necessarily these are arbiter waypoints not perimeter points
List<ArbiterStoppedExit> ases = this.CreateStoppedExits(exits.Values);
// construct intersection id
ITraversableWaypoint[] exitArray = new ITraversableWaypoint[exits.Count];
exits.Values.CopyTo(exitArray, 0);
ArbiterIntersectionId aii = new ArbiterIntersectionId(exitArray[0].Exits[0].InterconnectId);
// determine incoming lanes
Dictionary<ArbiterLane, LinePath.PointOnPath> incoming = this.DetermineIncoming(interPoly);
// create the intersection
ArbiterIntersection ai = new ArbiterIntersection(
interPoly,
ases,
this.DetermineInvolved(exits.Values, incoming),
incoming,
exits,
interPoly.Center,
aii,
arn,
this.IntersectionEntries(sq)
);
// create safety zones
this.CreateSafetyImplicitZones(ai);
// update poly
//this.UpdateIntersectionPolygon(ai);
/*List<Polygon> ps = new List<Polygon>();
foreach (ITraversableWaypoint itw in exits.Values)
{
foreach (ArbiterInterconnect ait in itw.Exits)
{
ps.Add(ait.TurnPolygon);
}
}
ai.IntersectionPolygon = this.GetIntersectionPolygon(ps, ai.AllExits, ai.AllEntries);*/
//ai.IntersectionPolygon = UrbanChallenge.Arbiter.Core.Common.Tools.PolygonToolkit.PolygonUnion(ps);
try
{
List<Polygon> ps = new List<Polygon>();
foreach (ITraversableWaypoint itw in exits.Values)
{
foreach (ArbiterInterconnect ait in itw.Exits)
{
ps.Add(ait.TurnPolygon);
}
}
ai.IntersectionPolygon = this.GetIntersectionPolygon(ps, ai.AllExits, ai.AllEntries);
if (ai.IntersectionPolygon.IsComplex)
{
EditorOutput.WriteLine("Intersection polygon complex, defaulting");
throw new Exception("complex polygon exception");
}
}
catch (Exception)
{
EditorOutput.WriteLine("Error in union polygon generation, using better default");
try
{
this.UpdateIntersectionPolygon(ai);
}
catch (Exception)
{
EditorOutput.WriteLine("Error in my simple polygon generation, plain default");
List<Coordinates> cs = new List<Coordinates>();
foreach (ITraversableWaypoint itw in ai.AllEntries.Values)
cs.Add(itw.Position);
foreach (ITraversableWaypoint itw in ai.AllExits.Values)
{
cs.Add(itw.Position);
foreach (ArbiterInterconnect aint in itw.Exits)
{
cs.AddRange(aint.TurnPolygon);
}
}
ai.IntersectionPolygon = Polygon.GrahamScan(cs);
}
}
try
{
// add intersection
arn.ArbiterIntersections.Add(aii, ai);
// add to exit lookup
foreach (IAreaSubtypeWaypointId awi in exits.Keys)
{
if (arn.IntersectionLookup.ContainsKey(awi))
arn.IntersectionLookup[awi] = ai;
else
arn.IntersectionLookup.Add(awi, ai);
}
// add to display objects
arn.DisplayObjects.Add(ai);
rd.AddDisplayObject(ai);
}
catch (Exception e)
{
EditorOutput.WriteLine("Error adding intersection: " + aii.ToString());
EditorOutput.WriteLine("Error adding intersection: " + e.ToString());
}
}
}
// reset the tool
this.it.ResetIcons();
this.WrapFinal = null;
this.WrapInitial = null;
}
private void IntersectionReParseAllButton_Click(object sender, EventArgs e)
{
// tool
IntersectionPulloutTool ipt = new IntersectionPulloutTool(this.arn, rd, ed, false);
// save old
ArbiterIntersection[] ais = new ArbiterIntersection[this.arn.ArbiterIntersections.Count];
this.arn.ArbiterIntersections.Values.CopyTo(ais, 0);
// remove all old
/*for(int i =0; i < ais.Length; i++)
{
ArbiterIntersection ai = ais[i];
this.rd.displayObjects.Remove(ai);
ai.RoadNetwork.DisplayObjects.Remove(ai);
ai.RoadNetwork.ArbiterIntersections.Remove(ai.IntersectionId);
foreach (ITraversableWaypoint aw in ai.AllExits.Values)
ai.RoadNetwork.IntersectionLookup.Remove(aw.AreaSubtypeWaypointId);
}*/
// refinalize
for (int j = 0; j < ais.Length; j++)
{
ArbiterIntersection ai = ais[j];
ipt.FinalizeIntersection(ai.IntersectionPolygon.Inflate(3.0), ai);
}
}
/// <summary>
/// finish the wrapping and cretae the intersection
/// </summary>
public void FinalizeIntersection(Polygon interPoly, ArbiterIntersection ai)
{
// check poly not null
if (interPoly != null)
{
this.WrappingHelpers = new List<Coordinates>();
Rect r = interPoly.CalculateBoundingRectangle();
Coordinates bl = new Coordinates(r.x, r.y);
Coordinates tr = new Coordinates(r.x + r.width, r.y + r.height);
Polygon sqPoly = this.CreateSquarePolygon(bl, tr).Inflate(1.0);
//interPoly = this.CreateIntersectionPolygon(interPoly);
//interPoly = interPoly.Inflate(0.05);
// retreive all exits involved in this intersection
Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> exits = this.IntersectionExits(sqPoly);//interPoly);
// make sure the inter contains an exit
if (exits.Count > 0)
{
// make stopped exits, necessarily these are arbiter waypoints not perimeter points
List<ArbiterStoppedExit> ases = this.CreateStoppedExits(exits.Values);
// construct intersection id
ITraversableWaypoint[] exitArray = new ITraversableWaypoint[exits.Count];
exits.Values.CopyTo(exitArray, 0);
ArbiterIntersectionId aii = new ArbiterIntersectionId(exitArray[0].Exits[0].InterconnectId);
// determine incoming lanes
Dictionary<ArbiterLane, LinePath.PointOnPath> incoming = this.DetermineIncoming(interPoly);
// create the intersection
ai.IntersectionPolygon = interPoly;
ai.Center = interPoly.BoundingCircle.center;
ai.StoppedExits = ases;
ai.IncomingLanePoints = incoming;
ai.AllExits = exits;
ai.AllEntries = this.IntersectionEntries(sqPoly);
ai.PriorityLanes = this.DetermineInvolved(exits.Values, incoming);
// update poly
//this.UpdateIntersectionPolygon(ai);
try
{
List<Polygon> ps = new List<Polygon>();
foreach (ITraversableWaypoint itw in exits.Values)
{
foreach (ArbiterInterconnect ait in itw.Exits)
{
ps.Add(ait.TurnPolygon);
}
}
ai.IntersectionPolygon = this.GetIntersectionPolygon(ps, ai.AllExits, ai.AllEntries);
if (ai.IntersectionPolygon.IsComplex)
{
EditorOutput.WriteLine("Intersection polygon complex, defaulting");
throw new Exception("complex polygon exception");
}
}
catch (Exception)
{
EditorOutput.WriteLine("Error in union polygon generation, using better default");
try
{
this.UpdateIntersectionPolygon(ai);
}
catch (Exception)
{
EditorOutput.WriteLine("Error in my simple polygon generation, plain default");
List<Coordinates> cs = new List<Coordinates>();
foreach (ITraversableWaypoint itw in ai.AllEntries.Values)
cs.Add(itw.Position);
foreach (ITraversableWaypoint itw in ai.AllExits.Values)
{
cs.Add(itw.Position);
foreach (ArbiterInterconnect aint in itw.Exits)
{
cs.AddRange(aint.TurnPolygon);
}
}
ai.IntersectionPolygon = Polygon.GrahamScan(cs);
}
}
// add intersection
/*arn.ArbiterIntersections.Add(aii, ai);
// add to exit lookup
foreach (IAreaSubtypeWaypointId awi in exits.Keys)
arn.IntersectionLookup.Add(awi, ai);
// add to display objects
arn.DisplayObjects.Add(ai);
rd.AddDisplayObject(ai);*/
}
}
}
public void CreateSafetyImplicitZones(ArbiterIntersection ai)
{
if (ai.StoppedExits.Count > 0)
{
foreach (ITraversableWaypoint itw in ai.AllExits.Values)
{
if (itw is ArbiterWaypoint && !itw.IsStop)
{
ArbiterWaypoint aw = (ArbiterWaypoint)itw;
ArbiterLane al = aw.Lane;
LinePath.PointOnPath end = aw.Lane.LanePath().GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
EditorOutput.WriteLine("safety zone too close to start of lane, setting start to start of lane: " + aw.ToString());
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
al.SafetyZones.Add(asz);
al.Way.Segment.RoadNetwork.DisplayObjects.Add(asz);
al.Way.Segment.RoadNetwork.ArbiterSafetyZones.Add(asz);
if (aw != null && aw.IsExit == true)
{
asz.isExit = true;
asz.Exit = aw;
}
// add to display
this.rd.displayObjects.Add(asz);
}
}
}
}
private void UpdateIntersectionPolygon(ArbiterIntersection aInt)
{
// get previous polygon
Polygon interPoly = new Polygon();
// add all turn points
foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
{
interPoly.AddRange(ai.TurnPolygon);
}
// wrap it to get intersection polygon
interPoly = Polygon.GrahamScan(interPoly);
// get outer edges of poly
List<LinePath> polyEdges = new List<LinePath>();
for (int i = 0; i < interPoly.Count; i++)
{
Coordinates init = interPoly[i];
Coordinates fin = i == interPoly.Count - 1 ? interPoly[0] : interPoly[i + 1];
polyEdges.Add(new LinePath(new Coordinates[] { init, fin }));
}
// get all edges of all the turns
List<LinePath> other = new List<LinePath>();
foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
{
for (int i = 0; i < ai.TurnPolygon.Count; i++)
{
Coordinates init = ai.TurnPolygon[i];
Coordinates fin = i == ai.TurnPolygon.Count - 1 ? ai.TurnPolygon[0] : ai.TurnPolygon[i + 1];
other.Add(new LinePath(new Coordinates[] { init, fin }));
}
}
// test points
List<Coordinates> testPoints = new List<Coordinates>();
// path segs of inner turns
List<LinePath> innerTurnPaths = new List<LinePath>();
// check all inner points against all turn edges
foreach (ArbiterInterconnect ai in aInt.PriorityLanes.Keys)
{
// check for inner point
if (ai.InnerCoordinates.Count == 3)
{
// inner point of the turn on the inside
testPoints.Add(ai.InnerCoordinates[1]);
// add to paths
innerTurnPaths.Add(new LinePath(new Coordinates[] { ai.InnerCoordinates[0], ai.InnerCoordinates[1] }));
innerTurnPaths.Add(new LinePath(new Coordinates[] { ai.InnerCoordinates[1], ai.InnerCoordinates[2] }));
}
}
// list of used segments
List<LinePath> closed = new List<LinePath>();
// check all other intersections of turn polygon edges
foreach (LinePath seg in innerTurnPaths)
{
foreach (LinePath otherEdge in other)
{
if (!seg.Equals(otherEdge) && !closed.Contains(otherEdge))
{
double x1 = seg[0].X;
double y1 = seg[0].Y;
double x2 = seg[1].X;
double y2 = seg[1].Y;
double x3 = otherEdge[0].X;
double y3 = otherEdge[0].Y;
double x4 = otherEdge[1].X;
double y4 = otherEdge[1].Y;
// get if inside both
double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
if (0.05 < ua && ua < 0.95 && 0.5 < ub && ub < 0.95)
{
double x = x1 + ua * (x2 - x1);
double y = y1 + ua * (y2 - y1);
testPoints.Add(new Coordinates(x, y));
}
}
}
closed.Add(seg);
}
// loop through test points
foreach(Coordinates inner in testPoints)
{
// list of replacements
List<LinePath> toReplace = new List<LinePath>();
// loop through outer
foreach (LinePath edge in polyEdges)
{
// flag to replace
bool replace = false;
// make sure this goes to a valid edge section
LinePath.PointOnPath closest = edge.GetClosestPoint(inner);
if (!closest.Equals(edge.StartPoint) && !closest.Equals(edge.EndPoint) &&
!(closest.Location.DistanceTo(edge.StartPoint.Location) < 0.5) &&
!(closest.Location.DistanceTo(edge.EndPoint.Location) < 0.5))
{
// create seg (extend a bit)
Coordinates expansion = closest.Location - inner;
LinePath seg = new LinePath(new Coordinates[] { inner, closest.Location + expansion.Normalize(1.0) });
// set flag
replace = true;
// loop through other edges
foreach (LinePath otherEdge in other)
{
double x1 = seg[0].X;
double y1 = seg[0].Y;
double x2 = seg[1].X;
double y2 = seg[1].Y;
double x3 = otherEdge[0].X;
double y3 = otherEdge[0].Y;
double x4 = otherEdge[1].X;
double y4 = otherEdge[1].Y;
// get if inside both
double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
if (0.01 < ua && ua < 0.99 && 0 < ub && ub < 1)
{
replace = false;
}
}
}
// check if should replace
if (replace)
{
// add analyzed to adjacent
toReplace.Add(edge);
}
}
// loop through replacements
foreach (LinePath ll in toReplace)
{
LinePath[] tmpArrayPoly = new LinePath[polyEdges.Count];
polyEdges.CopyTo(tmpArrayPoly);
List<LinePath> tmpPoly = new List<LinePath>(tmpArrayPoly);
// get index of edge
int index = tmpPoly.IndexOf(ll);
// remove
tmpPoly.RemoveAt(index);
// add correctly to outer
tmpPoly.Insert(index, new LinePath(new Coordinates[] { ll[0], inner }));
tmpPoly.Insert(index + 1, new LinePath(new Coordinates[] { inner, ll[1] }));
// poly
Polygon temp = new Polygon();
foreach(LinePath lpTemp in tmpPoly)
temp.Add(lpTemp[1]);
temp.Inflate(0.5);
// make sure none of original outside
bool ok = true;
foreach (LinePath lp in other)
{
if (!temp.IsInside(lp[1]) && !temp.Contains(lp[1]))
ok = false;
}
// set if created ok
if (ok)
polyEdges = tmpPoly;
}
}
// create final
List<Coordinates> finalPoly = new List<Coordinates>();
foreach (LinePath outerEdge in polyEdges)
finalPoly.Add(outerEdge[1]);
interPoly = new Polygon(finalPoly);
aInt.IntersectionPolygon = interPoly;
aInt.Center = interPoly.GetCentroid();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="ourExit"></param>
public IntersectionMonitor(ITraversableWaypoint ourExit, ArbiterIntersection ai, VehicleState ourState, IConnectAreaWaypoints desired)
{
// set intersection
this.Intersection = ai;
// create monitors
this.AllMonitors = new List<IIntersectionQueueable>();
this.PriorityMonitors = new List<IIntersectionQueueable>();
this.IntersectionPriorityQueue = new List<IIntersectionQueueable>();
this.InsideIntersectionVehicles = new List<IntersectionVehicleMonitor>();
this.NonStopPriorityAreas = new List<IDominantMonitor>();
this.PreviouslyWaiting = new List<IDominantMonitor>();
this.AllMonitors = new List<IIntersectionQueueable>();
this.cooldownTimer = new Stopwatch();
#region Stopped Exits
// get ours
IIntersectionQueueable ourMonitor = null;
// get stopped exits
foreach(ArbiterStoppedExit ase in ai.StoppedExits)
{
// add a monitor
SubmissiveEntryMonitor sem = new SubmissiveEntryMonitor(this, ase.Waypoint, ase.Waypoint.Equals(ourExit));
// add to monitors
PriorityMonitors.Add(sem);
// check not our
if(!sem.IsOurs)
{
// initial update
sem.Update(ourState);
// check if add first
if(!sem.Clear(ourState))
IntersectionPriorityQueue.Add(sem);
}
else
{
// set ours
ourMonitor = sem;
this.OurMonitor = ourMonitor;
}
}
// check if our monitor exists
if (ourMonitor != null)
{
// add ours
this.IntersectionPriorityQueue.Add(ourMonitor);
}
#endregion
#region Priority Areas Over Interconnect
// check contains priority lane for this
if (ai.PriorityLanes.ContainsKey(desired.ToInterconnect))
{
// loop through all other priority monitors over this interconnect
foreach (IntersectionInvolved ii in ai.PriorityLanes[desired.ToInterconnect])
{
// check area type if lane
if (ii.Area is ArbiterLane)
{
// add lane to non stop priority areas
this.NonStopPriorityAreas.Add(new DominantLaneEntryMonitor(this, ii));
}
// otherwise is zone
else if (ii.Area is ArbiterZone)
{
// add lane to non stop priority areas
this.NonStopPriorityAreas.Add(
new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)ii.Exit, ((ArbiterInterconnect)desired), false, this, ii));
}
else
{
throw new ArgumentException("unknown intersection involved area", "ii");
}
}
}
// otherwise be like, wtf?
else
{
ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + desired.ToString() + " returning can go");
//ArbiterOutput.Output("Error in intersection monitor!!!!!!!!!@Q!!, desired interconnect: " + desired.ToInterconnect.ToString() + " not found in intersection: " + ai.ToString() + ", not setting any dominant monitors");
}
#endregion
#region Entry Area
if (desired.FinalGeneric is ArbiterWaypoint)
{
this.EntryAreaMonitor = new LaneEntryAreaMonitor((ArbiterWaypoint)desired.FinalGeneric);
}
else if (desired.FinalGeneric is ArbiterPerimeterWaypoint)
{
this.EntryAreaMonitor = new ZoneAreaEntryMonitor((ArbiterPerimeterWaypoint)desired.FinalGeneric, (ArbiterInterconnect)desired,
false, this, new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
else
{
throw new Exception("unhandled desired interconnect final type");
}
#endregion
#region Our Monitor
// check ours
if (ourExit is ArbiterWaypoint)
{
this.OurMonitor = new DominantLaneEntryMonitor(this,
new IntersectionInvolved(ourExit, ((ArbiterWaypoint)ourExit).Lane,
desired is ArbiterInterconnect ? ((ArbiterInterconnect)desired).TurnDirection : ArbiterTurnDirection.Straight));
}
else if (ourExit is ArbiterPerimeterWaypoint)
{
// add lane to non stop priority areas
this.OurMonitor =
new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)desired.InitialGeneric, ((ArbiterInterconnect)desired), true, this,
new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.InitialGeneric).Perimeter.Zone,
((ArbiterInterconnect)desired).TurnDirection));
}
#endregion
// add to all
this.AllMonitors.AddRange(this.PriorityMonitors);
foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
this.AllMonitors.Add(iiq);
this.AllMonitors.Add(this.EntryAreaMonitor);
// update all
this.Update(ourState);
// check if we need to populate previously waiting
if(this.OurMonitor is IDominantMonitor)
{
// cast our
IDominantMonitor ours = (IDominantMonitor)this.OurMonitor;
// loop through to determine those previously waiting
foreach (IDominantMonitor idm in this.NonStopPriorityAreas)
{
if(idm.Waypoint != null && !idm.Waypoint.IsStop && idm.WaitingTimerRunning)
this.PreviouslyWaiting.Add(idm);
}
}
}
public IntersectionStartupState(ArbiterIntersection ai)
{
this.Intersection = ai;
}
