public Polygon DefaultPoly()
{
ArbiterInterconnect ai = this;
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
List <Coordinates> polyPoints = new List <Coordinates>();
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
return(Polygon.GrahamScan(polyPoints));
}
/// <summary>
/// Gets edge going to input node from this exit
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public ArbiterInterconnect GetEdge(INavigableNode node)
{
foreach (NavigableEdge ne in exit.OutgoingConnections)
{
if (ne is ArbiterInterconnect)
{
ArbiterInterconnect ai = (ArbiterInterconnect)ne;
if (ai.FinalGeneric.Equals(node))
{
return(ai);
}
}
}
return(null);
}
/// <summary>
/// Get interconnect closest to our position and orientation
/// </summary>
/// <param name="point"></param>
/// <param name="heading"></param>
/// <returns></returns>
public ArbiterInterconnect ClosestInterconnect(Coordinates point, Coordinates heading)
{
ArbiterInterconnect closest = null;
double best = Double.MaxValue;
foreach (ArbiterInterconnect ai in this.ArbiterInterconnects.Values)
{
// get closest
double distance = ai.InterconnectPath.GetPoint(ai.InterconnectPath.GetClosestPoint(point)).DistanceTo(point);
// get heading of interconnect approximately
Coordinates interHeading = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
if (distance < best && Math.Abs(interHeading.ToDegrees() - heading.ToDegrees()) < 45.0)
{
best = distance;
closest = ai;
}
}
return(closest);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial"></param>
/// <param name="next"></param>
/// <param name="final"></param>
public SupraLane(ArbiterLane initial, ArbiterInterconnect next, ArbiterLane final)
{
Components = new SupraLaneComponentList();
Components.Add(initial);
Components.Add(next);
Components.Add(final);
this.Initial = initial;
this.Final = final;
this.Interconnect = next;
if (!this.Initial.Equals(this.Final))
{
this.supraWaypoints = new List <ArbiterWaypoint>();
this.supraWaypoints.AddRange(this.Initial.WaypointsInclusive(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
this.supraWaypoints.AddRange(this.Final.WaypointsInclusive((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
this.SetDefaultLanePath();
}
else
{
this.supraWaypoints = this.Initial.WaypointList;
this.supraPath = this.Initial.LanePath();
this.supraPath.Add(this.Interconnect.FinalGeneric.Position);
}
}
/// <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);
}
public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
{
List <Coordinates> polyPoints = new List <Coordinates>();
try
{
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
ai.TurnDirection == ArbiterTurnDirection.Straight ||
!(ai.InitialGeneric is ArbiterWaypoint) ||
!(ai.FinalGeneric is ArbiterWaypoint))
{
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(polyPoints);
if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
List <Coordinates> updatedPts = new List <Coordinates>();
LinePath interTmp = ai.InterconnectPath.Clone();
Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
lp = interTmp.ShiftLateral(TahoeParams.VL);
rp = interTmp.ShiftLateral(-TahoeParams.VL);
updatedPts.AddRange(lp);
updatedPts.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
}
}
else
{
// polygon points
List <Coordinates> interPoints = new List <Coordinates>();
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
// left and right path
LinePath leftPath = new LinePath();
LinePath rightPath = new LinePath();
// some initial points
LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
LinePath il = initialPath.ShiftLateral(width / 2.0);
LinePath ir = initialPath.ShiftLateral(-width / 2.0);
leftPath.Add(il[1]);
rightPath.Add(ir[1]);
// some final points
LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
LinePath fl = finalPath.ShiftLateral(width / 2.0);
LinePath fr = finalPath.ShiftLateral(-width / 2.0);
leftPath.Add(fl[0]);
rightPath.Add(fr[0]);
// initial and final paths
Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);
// get where the paths intersect and vector to normal path
Coordinates c;
iPath.Intersect(fPath, out c);
Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
Coordinates center = c + vector.Normalize((vector.Length / 2.0));
// get width expansion
Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;
// get inner point (small scale)
Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);
// get outer
Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);
if (ai.TurnDirection == ArbiterTurnDirection.Right)
{
rightPath.Insert(1, innerPoint);
ai.InnerCoordinates = rightPath;
leftPath.Reverse();
leftPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
else
{
leftPath.Insert(1, innerPoint);
ai.InnerCoordinates = leftPath;
rightPath.Reverse();
rightPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
}
}
catch (Exception e)
{
Console.WriteLine("error generating turn polygon: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
public void SetTurnDirection(ArbiterInterconnect ai)
{
#region Turn Direction
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric;
// check not uturn
if (!initWp.Lane.Way.Segment.Equals(finWp.Lane.Way.Segment) || initWp.Lane.Way.Equals(finWp.Lane.Way))
{
Coordinates iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
if (arcTan > 45.0)
{
ai.TurnDirection = ArbiterTurnDirection.Left;
}
else if (arcTan < -45.0)
{
ai.TurnDirection = ArbiterTurnDirection.Right;
}
else
{
ai.TurnDirection = ArbiterTurnDirection.Straight;
}
}
else
{
Coordinates iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
if (arcTan > 45.0 && arcTan < 135.0)
{
ai.TurnDirection = ArbiterTurnDirection.Left;
}
else if (arcTan < -45.0 && arcTan > -135.0)
{
ai.TurnDirection = ArbiterTurnDirection.Right;
}
else if (Math.Abs(arcTan) < 45.0)
{
ai.TurnDirection = ArbiterTurnDirection.Straight;
}
else
{
ai.TurnDirection = ArbiterTurnDirection.UTurn;
}
}
}
else
{
Coordinates iVec = new Coordinates();
double iRot = 0.0;
Coordinates fVec = new Coordinates();
double fDeg = 0.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);
iRot = -iVec.ArcTan;
}
else if (ai.InitialGeneric is ArbiterPerimeterWaypoint)
{
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.InitialGeneric;
Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
iVec = apw.Position - centerPoly;
iVec = iVec.Normalize(1.0);
iRot = -iVec.ArcTan;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric;
fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
fDeg = fVec.ToDegrees();
}
else if (ai.FinalGeneric is ArbiterPerimeterWaypoint)
{
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
fVec = centerPoly - apw.Position;
fVec = fVec.Normalize(1.0);
fVec = fVec.Rotate(iRot);
fDeg = fVec.ToDegrees();
}
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
if (arcTan > 45.0)
{
ai.TurnDirection = ArbiterTurnDirection.Left;
}
else if (arcTan < -45.0)
{
ai.TurnDirection = ArbiterTurnDirection.Right;
}
else
{
ai.TurnDirection = ArbiterTurnDirection.Straight;
}
}
#endregion
}
/// <summary>
/// Generates the default time cost for the edge
/// </summary>
/// <returns></returns>
public double CalculateDefaultTimeCost()
{
// add extra costs of the end of this edge
double cost = End.ExtraTimeCost;
if (IsZone)
{
// zone returns justdistance of edge divided by the zone's minimum speed
cost += this.standardEdgeDistance / Math.Max(NavigationPenalties.ZoneMinSpeedDefault, this.Zone.SpeedLimits.MinimumSpeed);
return(cost);
}
else if (IsSegment)
{
// default cost of segment edge
cost += this.standardEdgeDistance / this.Segment.SpeedLimits.MaximumSpeed;
// time costs of adjacent
foreach (IConnectAreaWaypoints icaw in this.Contained)
{
if (!icaw.Equals(this))
{
// add changing lanes cost
cost += NavigationPenalties.ChangeLanes;
if (icaw is NavigableEdge)
{
cost += ((NavigableEdge)icaw).TimeCost();
}
}
}
// interconnect if stop
if (Contained.Count > 0 && Contained[0] is ArbiterLanePartition)
{
ArbiterLanePartition alp = (ArbiterLanePartition)Contained[0];
if (alp.Type == PartitionType.Sparse)
{
cost += this.standardEdgeDistance * 2.0;
}
if (alp.Initial.IsStop)
{
ArbiterInterconnect ai = alp.ToInterconnect;
cost += ai.ExtraCost - NavigationPenalties.Interconnect;
}
}
// cost of partition by default
return(cost);
}
else
{
// default cost
cost += this.standardEdgeDistance / this.defaultSpeed;
// interconnect
if (Contained.Count > 0 && Contained[0] is ArbiterInterconnect)
{
ArbiterInterconnect ai = (ArbiterInterconnect)Contained[0];
cost = this.standardEdgeDistance / ai.MaximumDefaultSpeed + ai.ExtraCost;
}
// return
return(cost);
}
}
public int ComparePriority(ArbiterInterconnect other)
{
if (this.initialWaypoint is ITraversableWaypoint && ((ITraversableWaypoint)initialWaypoint).IsStop)
{
if (other.InitialGeneric is ITraversableWaypoint && !((ITraversableWaypoint)other.InitialGeneric).IsStop)
{
return(1);
}
}
else
{
if (other.InitialGeneric is ITraversableWaypoint && ((ITraversableWaypoint)other.InitialGeneric).IsStop)
{
return(-1);
}
}
if (this.TurnDirection == ArbiterTurnDirection.Unknown || other.TurnDirection == ArbiterTurnDirection.Unknown)
{
return(0);
}
if (this.TurnDirection == ArbiterTurnDirection.Left)
{
if (other.TurnDirection == ArbiterTurnDirection.UTurn)
{
return(-1);
}
if (other.TurnDirection == ArbiterTurnDirection.Left)
{
return(0);
}
else
{
return(1);
}
}
else if (this.TurnDirection == ArbiterTurnDirection.Right)
{
if (other.TurnDirection == ArbiterTurnDirection.Right)
{
return(0);
}
else if (other.TurnDirection == ArbiterTurnDirection.Straight)
{
return(1);
}
else
{
return(-1);
}
}
else if (this.TurnDirection == ArbiterTurnDirection.Straight)
{
if (other.TurnDirection == ArbiterTurnDirection.Straight)
{
return(0);
}
else
{
return(-1);
}
}
else if (this.TurnDirection == ArbiterTurnDirection.UTurn)
{
if (other.TurnDirection == ArbiterTurnDirection.UTurn)
{
return(0);
}
else
{
return(1);
}
}
else
{
return(1);
}
}
