public void SparseDetermination(Coordinates coordinates, out bool sparseDownstream, out bool sparseNow, out double sparseDistance)
{
ArbiterLanePartition alp = this.GetClosestPartition(coordinates);
if (alp.Type == PartitionType.Sparse)
{
sparseDownstream = true;
sparseDistance = 0.0;
sparseNow = true;
return;
}
while (alp != null)
{
if (alp.Type == PartitionType.Sparse)
{
sparseDownstream = true;
sparseDistance = this.DistanceBetween(coordinates, alp.Initial.Position);
sparseNow = false;
return;
}
alp = alp.Final.NextPartition;
}
sparseDownstream = false;
sparseDistance = double.MaxValue;
sparseNow = false;
}
/// <summary>
/// Generates adjacency of a partition to another lane
/// </summary>
/// <param name="alp"></param>
/// <param name="al"></param>
private void GenerateSinglePartitionAdjacency(ArbiterLanePartition alp, ArbiterLane al)
{
// fake path
List <IPathSegment> fakePathSegments = new List <IPathSegment>();
fakePathSegments.Add(new LinePathSegment(alp.Initial.Position, alp.Final.Position));
Path fakePath = new Path(fakePathSegments);
// partitions adjacent
List <ArbiterLanePartition> adjacent = new List <ArbiterLanePartition>();
// tracks
PointOnPath current = fakePath.StartPoint;
double increment = 0.5;
double tmpInc = 0;
// increment along
while (tmpInc == 0)
{
// loop over partitions
foreach (ArbiterLanePartition alpar in al.Partitions)
{
// get fake path for partition
List <IPathSegment> ips = new List <IPathSegment>();
ips.Add(new LinePathSegment(alpar.Initial.Position, alpar.Final.Position));
Path alpPath = new Path(ips);
// get closest point on tmp partition to current
PointOnPath closest = alpPath.GetClosest(current.pt);
// check general distance
if (closest.pt.DistanceTo(current.pt) < 20)
{
// check not start or end
if (!closest.Equals(alpPath.StartPoint) && !closest.Equals(alpPath.EndPoint))
{
// check not already added
if (!adjacent.Contains(alpar))
{
// add
adjacent.Add(alpar);
}
}
}
}
// set inc
tmpInc = increment;
// increment point
current = fakePath.AdvancePoint(current, ref tmpInc);
}
// add adjacent
alp.NonLaneAdjacentPartitions.AddRange(adjacent);
}
public void SetDefaultSparsePolygon()
{
ArbiterLanePartition alp = this;
List <Coordinates> polyCoords = new List <Coordinates>();
polyCoords.Add(alp.Initial.Position);
polyCoords.AddRange(alp.NotInitialPathCoords());
LinePath lpr = (new LinePath(polyCoords)).ShiftLateral(-TahoeParams.VL * 3.0);
LinePath lpl = (new LinePath(polyCoords)).ShiftLateral(TahoeParams.VL * 3.0);
List <Coordinates> finalCoords = new List <Coordinates>(polyCoords.ToArray());
finalCoords.AddRange(lpr);
finalCoords.AddRange(lpl);
Polygon p = Polygon.GrahamScan(finalCoords);
this.SparsePolygon = p;
}
public bool IsInsideClose(Coordinates c)
{
if (this.UserPartitionPath == null)
{
this.UserPartitionPath = new LinePath(new Coordinates[] { this.InitialGeneric.Position, this.FinalGeneric.Position });
}
if (this.Partition is ArbiterLanePartition)
{
ArbiterLanePartition alp = (ArbiterLanePartition)this.Partition;
LinePath.PointOnPath closest = this.UserPartitionPath.GetClosestPoint(c);
if (closest.Location.DistanceTo(c) < alp.Lane.Width)
{
if (!closest.Equals(this.UserPartitionPath.StartPoint) && !closest.Equals(this.UserPartitionPath.EndPoint))
{
return(true);
}
}
}
return(false);
}
/// <summary>
/// Gets the closest partition to the input position
/// </summary>
/// <param name="position"></param>
/// <returns></returns>
public ArbiterLanePartition GetClosestPartition(Coordinates position)
{
// closest partition
ArbiterLanePartition closest = null;
double minDistance = Double.MaxValue;
// loop partitions
foreach (ArbiterLanePartition alp in this.Partitions)
{
// get distance of pt to partition
double tmpDistance = alp.DistanceTo(position);
// if less than cur min
if (tmpDistance < minDistance)
{
// set as cur min
closest = alp;
minDistance = tmpDistance;
}
}
// return
return(closest);
}
/// <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);
}
}
private bool IsPartitionSameDirection(ArbiterLanePartition partition, Coordinates heading)
{
return (partition.Vector().Normalize().Dot(heading) > 0);
}
private LinePath BuildPath(ArbiterLanePartition parition, bool forward, double distBackTarget, double distForwardTarget)
{
if (forward) {
// iterate all the way backward until we have our distance
ArbiterLanePartition backPartition = parition;
double distBack = 0;
while (backPartition.Initial.PreviousPartition != null && distBack < distBackTarget) {
backPartition = backPartition.Initial.PreviousPartition;
distBack += backPartition.Length;
}
LinePath path = new LinePath();
// insert the backmost point
while (backPartition != parition) {
path.Add(backPartition.Initial.Position);
backPartition = backPartition.Final.NextPartition;
}
// add our initial position
path.Add(parition.Initial.Position);
// add our final position
path.Add(parition.Final.Position);
double distForward = 0;
while (parition.Final.NextPartition != null && distForward < distForwardTarget) {
parition = parition.Final.NextPartition;
distForward += parition.Length;
path.Add(parition.Final.Position);
}
return path;
}
else {
// iterate all the way backward until we have our distance
ArbiterLanePartition backPartition = parition;
double distBack = 0;
while (backPartition.Final.NextPartition != null && distBack < distBackTarget) {
backPartition = backPartition.Final.NextPartition;
distBack += backPartition.Length;
}
LinePath path = new LinePath();
// insert the backmost point
while (backPartition != parition) {
path.Add(backPartition.Final.Position);
backPartition = backPartition.Initial.PreviousPartition;
}
// add our initial position
path.Add(parition.Final.Position);
// add our final position
path.Add(parition.Initial.Position);
double distForward = 0;
while (parition.Initial.PreviousPartition != null && distForward < distForwardTarget) {
parition = parition.Initial.PreviousPartition;
distForward += parition.Length;
path.Add(parition.Initial.Position);
}
return path;
}
}
/// <summary>
/// Add a costly blockage across the road
/// </summary>
/// <param name="partition"></param>
/// <param name="c"></param>
public void AddHarshBlockageAcrossSegment(ArbiterLanePartition partition, Coordinates c)
{
partition.Blockage.BlockageExists = true;
partition.Blockage.BlockageCoordinates = c;
partition.Blockage.SecondsSinceObserved = 0.0;
partition.Blockage.BlockageTimeCost = 3600;
partition.Blockage.BlockageLifetime = partition.Blockage.BlockageLifetime * 4.0;
foreach (ArbiterLanePartition alp in ((ArbiterLanePartition)partition).NonLaneAdjacentPartitions)
{
if (alp.IsInside(c))
{
alp.Blockage.BlockageExists = true;
alp.Blockage.BlockageCoordinates = c;
alp.Blockage.SecondsSinceObserved = 0.0;
alp.Blockage.BlockageTimeCost = 3600;
alp.Blockage.BlockageLifetime = alp.Blockage.BlockageLifetime * 4.0;
}
}
// reset navigation costs
this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();
}
public void InsertUserWaypoint(Coordinates c)
{
if (this.Partition is ArbiterLanePartition)
{
ArbiterLanePartition alp = (ArbiterLanePartition)this.Partition;
ArbiterUserWaypoint final = null;
if (this.InitialGeneric is ArbiterUserWaypoint)
{
ArbiterUserWaypoint auw = (ArbiterUserWaypoint)this.InitialGeneric;
ArbiterUserWaypointId auwi = new ArbiterUserWaypointId(auw.WaypointId.Number + 1, this.Partition.ConnectionId);
final = new ArbiterUserWaypoint(c, auwi, this.Partition);
}
else
{
ArbiterUserWaypointId auwi = new ArbiterUserWaypointId(1, this.Partition.ConnectionId);
final = new ArbiterUserWaypoint(c, auwi, this.Partition);
}
foreach (ArbiterUserWaypoint aup in alp.UserWaypoints)
{
if (aup.WaypointId.Number >= final.WaypointId.Number)
{
aup.WaypointId.Number++;
}
}
ArbiterUserPartition aup1 = new ArbiterUserPartition(
new ArbiterUserPartitionId(this.Partition.ConnectionId, this.InitialGeneric.GenericId, final.WaypointId),
this.Partition, this.InitialGeneric, final);
aup1.FinalGeneric = final;
aup1.InitialGeneric = this.InitialGeneric;
final.Previous = aup1;
if (aup1.InitialGeneric is ArbiterUserWaypoint)
{
((ArbiterUserWaypoint)aup1.InitialGeneric).Next = aup1;
}
ArbiterUserPartition aup2 = new ArbiterUserPartition(
new ArbiterUserPartitionId(this.Partition.ConnectionId, final.WaypointId, this.FinalGeneric.GenericId),
this.Partition, final, this.FinalGeneric);
aup2.InitialGeneric = final;
aup2.FinalGeneric = this.FinalGeneric;
final.Next = aup2;
if (aup2.FinalGeneric is ArbiterUserWaypoint)
{
((ArbiterUserWaypoint)aup2.FinalGeneric).Previous = aup2;
}
alp.UserPartitions.Remove(this);
alp.Lane.Way.Segment.RoadNetwork.DisplayObjects.Remove(this);
alp.Lane.Way.Segment.RoadNetwork.DisplayObjects.Add(final);
alp.UserWaypoints.Add(final);
alp.Lane.Way.Segment.RoadNetwork.DisplayObjects.Add(aup1);
alp.Lane.Way.Segment.RoadNetwork.DisplayObjects.Add(aup2);
alp.UserPartitions.Add(aup1);
alp.UserPartitions.Add(aup2);
alp.UserPartitions.Sort();
alp.Lane.ReformPath();
}
}
/// <summary>
/// Get a road plan while setting partition costs very high
/// </summary>
/// <param name="partition"></param>
/// <param name="goal"></param>
/// <param name="blockAdjacent"></param>
/// <param name="c"></param>
/// <returns></returns>
public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
{
KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();
RoadPlan rp = null;
if (!blockAdjacent)
{
NavigationBlockage nb = partition.Blockage;
NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
partition.Blockage = tmp;
rp = this.PlanNavigableArea(partition.Lane, c, goal, new List<ArbiterWaypoint>());
partition.Blockage = nb;
}
else
{
// save
List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>> savedBlockages = new List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>>();
// set
savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(partition, partition.Blockage));
// create new
NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
anewerBlockage.BlockageExists = true;
partition.Blockage = anewerBlockage;
foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
{
if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
{
savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(alp, alp.Blockage));
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
}
// plan
rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List<ArbiterWaypoint>());
// restore
foreach (KeyValuePair<ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
{
saved.Key.Blockage = saved.Value;
}
}
// restore
this.currentTimes = tmpCurrentTimes;
// return
return rp;
}
/// <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>
/// 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>
/// Gets exits downstream, or the goal we are currently reaching
/// </summary>
/// <param name="currentPosition"></param>
/// <param name="ignorable"></param>
/// <param name="goal"></param>
/// <returns></returns>
public List <DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List <ArbiterWaypoint> ignorable, INavigableNode goal)
{
// downstream final
List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>();
foreach (ArbiterLane al in Way.Lanes.Values)
{
if (al.Equals(this) || (al.GetClosestPartition(currentPosition).Type != PartitionType.Startup &&
((this.LaneOnLeft != null && this.LaneOnLeft.Equals(al) && this.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(this.LaneOnRight != null && this.LaneOnRight.Equals(al) && this.BoundaryRight != ArbiterLaneBoundary.SolidWhite))))
{
// get starting waypoint
ArbiterWaypoint waypoint = null;
// get closest partition
ArbiterLanePartition alp = al.GetClosestPartition(currentPosition);
if (alp.Initial.Position.DistanceTo(currentPosition) < TahoeParams.VL - 2)
{
waypoint = alp.Initial;
}
else if (alp.IsInside(currentPosition) || alp.Final.Position.DistanceTo(currentPosition) < TahoeParams.VL)
{
waypoint = alp.Final;
}
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition))
{
waypoint = alp.Initial;
}
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition) && alp.Final.NextPartition == null)
{
waypoint = null;
}
else
{
waypoint = null;
}
// check waypoint exists
if (waypoint != null)
{
// save start
ArbiterWaypoint initial = waypoint;
// initial cost
double initialCost = 0.0;
if (al.Equals(this))
{
initialCost = currentPosition.DistanceTo(waypoint.Position) / waypoint.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
}
else if (waypoint.WaypointId.Number != 1)
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
}
else
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
ArbiterWaypoint tmpF = this.GetClosestWaypoint(waypoint.Position, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
initialCost += this.TimeCostInLane(tmpI, tmpF, new List <ArbiterWaypoint>());
}
// loop while waypoint not null
while (waypoint != null)
{
if (waypoint.IsCheckpoint && (goal is ArbiterWaypoint) && ((ArbiterWaypoint)goal).WaypointId.Equals(waypoint.WaypointId))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = true;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.IsExit && !ignorable.Contains(waypoint))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = true;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.NextPartition == null && !ignorable.Contains(waypoint))
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = Double.MaxValue;
waypoints.Add(dpoi);
}
waypoint = waypoint.NextPartition != null ? waypoint.NextPartition.Final : null;
}
}
}
}
return(waypoints);
}
/// <summary>
/// Generates adjacency of a partition to another lane
/// </summary>
/// <param name="alp"></param>
/// <param name="al"></param>
private void GenerateSinglePartitionAdjacency(ArbiterLanePartition alp, ArbiterLane al)
{
// fake path
List<IPathSegment> fakePathSegments = new List<IPathSegment>();
fakePathSegments.Add(new LinePathSegment(alp.Initial.Position, alp.Final.Position));
Path fakePath = new Path(fakePathSegments);
// partitions adjacent
List<ArbiterLanePartition> adjacent = new List<ArbiterLanePartition>();
// tracks
PointOnPath current = fakePath.StartPoint;
double increment = 0.5;
double tmpInc = 0;
// increment along
while (tmpInc == 0)
{
// loop over partitions
foreach (ArbiterLanePartition alpar in al.Partitions)
{
// get fake path for partition
List<IPathSegment> ips = new List<IPathSegment>();
ips.Add(new LinePathSegment(alpar.Initial.Position, alpar.Final.Position));
Path alpPath = new Path(ips);
// get closest point on tmp partition to current
PointOnPath closest = alpPath.GetClosest(current.pt);
// check general distance
if (closest.pt.DistanceTo(current.pt) < 20)
{
// check not start or end
if (!closest.Equals(alpPath.StartPoint) && !closest.Equals(alpPath.EndPoint))
{
// check not already added
if (!adjacent.Contains(alpar))
{
// add
adjacent.Add(alpar);
}
}
}
}
// set inc
tmpInc = increment;
// increment point
current = fakePath.AdvancePoint(current, ref tmpInc);
}
// add adjacent
alp.NonLaneAdjacentPartitions.AddRange(adjacent);
}
/// <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>
/// Constructor
/// </summary>
/// <param name="lane"></param>
public StartupOffChuteState(ArbiterLanePartition final)
{
this.Final = final;
}
/// <summary>
/// Gets adjacent lane partitions
/// </summary>
/// <param name="alp"></param>
/// <returns></returns>
private List<NavigableEdge> AdjacentPartitions(ArbiterLanePartition alp)
{
List<NavigableEdge> edges = new List<NavigableEdge>();
edges.Add(alp);
List<ArbiterLanePartition> pars = alp.NonLaneAdjacentPartitions;
foreach (ArbiterLanePartition alps in pars)
edges.Add(alps);
return edges;
}