/// <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> /// Generates the bounding waypoints of the acceptable U-Turn area given Rndf Hazards and specified exit and entry waypoints /// </summary> /// <returns></returns> public static Polygon uTurnBounds(Coordinates exit, ArbiterSegment segment) { // initialize the bounding box List<Coordinates> boundingBox = new List<Coordinates>(); // put in coords for every available lane foreach (ArbiterLane al in segment.Lanes.Values) { PointOnPath? pop = null; if (!al.IsInside(exit)) { ArbiterWaypoint aw = al.GetClosestWaypoint(exit, 10.0); if (aw != null) pop = al.PartitionPath.GetClosest(aw.Position); } else pop = al.PartitionPath.GetClosest(exit); if(pop != null) { ArbiterLanePartition alp = al.GetClosestPartition(exit); Coordinates vector = alp.Vector().Normalize(15); Coordinates back = pop.Value.pt - vector; vector = vector.Normalize(30); boundingBox.AddRange(InflatePartition(back, vector, alp.Lane.Width)); } } // return the box return GeneralToolkit.JarvisMarch(boundingBox); }
/// <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); }
/// <summary> /// Constructor /// </summary> /// <param name="partitionId"></param> public ArbiterLanePartition(ArbiterLanePartitionId partitionId, ArbiterWaypoint initial, ArbiterWaypoint final, ArbiterSegment segment) : base(false, null, true, segment, new List <IConnectAreaWaypoints>(), initial, final) { this.PartitionId = partitionId; this.Initial = initial; this.Final = final; this.Length = this.Initial.Position.DistanceTo(this.Final.Position); this.UserWaypoints = new List <ArbiterUserWaypoint>(); this.NonLaneAdjacentPartitions = new List <ArbiterLanePartition>(); // create a path of the partition and get the closest List <Coordinates> coords = new List <Coordinates>(); coords.Add(initial.Position); coords.Add(final.Position); PartitionPath = new LinePath(coords); // nav edge stuff this.Contained.Add(this); this.blockage = new NavigationBlockage(0.0); }
/// <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> /// Constructor /// </summary> /// <param name="partitionId"></param> public ArbiterLanePartition(ArbiterLanePartitionId partitionId, ArbiterWaypoint initial, ArbiterWaypoint final, ArbiterSegment segment) : base(false, null, true, segment, new List<IConnectAreaWaypoints>(), initial, final) { this.PartitionId = partitionId; this.Initial = initial; this.Final = final; this.Length = this.Initial.Position.DistanceTo(this.Final.Position); this.UserWaypoints = new List<ArbiterUserWaypoint>(); this.NonLaneAdjacentPartitions = new List<ArbiterLanePartition>(); // create a path of the partition and get the closest List<Coordinates> coords = new List<Coordinates>(); coords.Add(initial.Position); coords.Add(final.Position); PartitionPath = new LinePath(coords); // nav edge stuff this.Contained.Add(this); this.blockage = new NavigationBlockage(0.0); }
/// <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; }