/// <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; }