/// <summary> /// Waypoints to ignore /// </summary> /// <param name="way"></param> /// <param name="position"></param> /// <returns></returns> public static List<ArbiterWaypoint> WaypointsClose(ArbiterWay way, Coordinates position, ArbiterWaypoint ignoreSpecifically) { List<ArbiterWaypoint> waypoints = new List<ArbiterWaypoint>(); foreach (ArbiterLane al in way.Lanes.Values) { ArbiterWaypoint aw = al.GetClosestWaypoint(position, TahoeParams.VL * 2.0); if(aw != null) waypoints.Add(aw); } if (ignoreSpecifically != null && !waypoints.Contains(ignoreSpecifically)) waypoints.Add(ignoreSpecifically); return waypoints; }
/// <summary> /// Gets the road plan /// </summary> /// <param name="pointsOfInterest"></param> /// <returns></returns> private RoadPlan GetRoadPlan(List<DownstreamPointOfInterest> pointsOfInterest, ArbiterWay way) { // road plan RoadPlan rp = new RoadPlan(new Dictionary<ArbiterLaneId, LanePlan>()); // find the best in each lane foreach (ArbiterLane al in way.Lanes.Values) { // points in lane List<DownstreamPointOfInterest> lanePoints = new List<DownstreamPointOfInterest>(); // loop over all points foreach (DownstreamPointOfInterest dpoi in pointsOfInterest) { if (dpoi.PointOfInterest.Lane.Equals(al)) lanePoints.Add(dpoi); } // if exist any if (lanePoints.Count > 0) { // sort lanePoints.Sort(); // choose best and add rp.LanePlans.Add(al.LaneId, new LanePlan(lanePoints[0])); } } // return return rp; }
/// <summary> /// Gets all exits downstream from a point /// </summary> /// <param name="position"></param> /// <param name="way"></param> /// <param name="exits">We are looking for exits</param> /// <returns></returns> private List<DownstreamPointOfInterest> Downstream(Coordinates position, ArbiterWay way, bool exits, List<ArbiterWaypoint> ignorable) { List<DownstreamPointOfInterest> waypoints = new List<DownstreamPointOfInterest>(); foreach (ArbiterLane al in way.Lanes.Values) { LinePath.PointOnPath pop = al.GetClosestPoint(position); if (al.IsInside(position) || position.DistanceTo(al.LanePath().StartPoint.Location) < 1.0) { ArbiterLanePartition currentPartition = al.GetClosestPartition(position); ArbiterWaypoint initial = currentPartition.Final; double initialCost = position.DistanceTo(currentPartition.Final.Position) / way.Segment.SpeedLimits.MaximumSpeed; do { if(((exits && currentPartition.Final.IsExit) || (!exits && currentPartition.Final.IsEntry)) && !ignorable.Contains(currentPartition.Final)) { double timeCost = initialCost + this.TimeCostInLane(initial, currentPartition.Final); DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.LanePath().DistanceBetween(pop, al.GetClosestPoint(currentPartition.Final.Position)); dpoi.IsExit = true; dpoi.IsGoal = false; dpoi.PointOfInterest = currentPartition.Final; dpoi.TimeCostToPoint = timeCost; waypoints.Add(dpoi); } currentPartition = currentPartition.Final.NextPartition; } while(currentPartition != null); } } return waypoints; }
/// <summary> /// Route ploan for downstream exits /// </summary> /// <param name="downstreamPoints"></param> /// <param name="goal"></param> private void DetermineDownstreamPointRouteTimes(List<DownstreamPointOfInterest> downstreamPoints, INavigableNode goal, ArbiterWay aw) { // so, for each exit downstream we need to plan from the end of each interconnect to the goal foreach (DownstreamPointOfInterest dpoi in downstreamPoints) { // check if exit if (dpoi.IsExit) { // current best time double bestCurrent = Double.MaxValue; List<INavigableNode> bestRoute = null; ArbiterInterconnect bestInterconnect = null; // fake node FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest); // init fields double timeCost; List<INavigableNode> routeNodes; // plan this.Plan(fen, goal, out routeNodes, out timeCost); bestCurrent = timeCost; bestRoute = routeNodes; bestInterconnect = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null; // plan from each interconnect to find the best time from exit /*foreach (ArbiterInterconnect ai in dpoi.PointOfInterest.Exits) { // init fields double timeCost; List<INavigableNode> routeNodes; // plan this.Plan(ai.End, goal, out routeNodes, out timeCost); // check if (timeCost < bestCurrent) { bestCurrent = timeCost; bestRoute = routeNodes; bestInterconnect = ai; } }*/ // set best dpoi.RouteTime = bestCurrent; dpoi.BestRoute = bestRoute; dpoi.BestExit = bestInterconnect; } } }
/// <summary> /// Checks if a goal is downstream on the current road /// </summary> /// <param name="way"></param> /// <param name="currentPosition"></param> /// <param name="goal"></param> /// <returns></returns> private DownstreamPointOfInterest IsGoalDownStream(ArbiterWay way, Coordinates currentPosition, INavigableNode goal) { if (goal is ArbiterWaypoint) { ArbiterWaypoint goalWaypoint = (ArbiterWaypoint)goal; if (goalWaypoint.Lane.Way.Equals(way)) { foreach (ArbiterLane lane in way.Lanes.Values) { if (lane.Equals(goalWaypoint.Lane)) { Coordinates current = lane.GetClosest(currentPosition); Coordinates goalPoint = lane.GetClosest(goal.Position); if (lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) { double distToGoal = lane.DistanceBetween(current, goalPoint); if (distToGoal > 0) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = distToGoal; dpoi.IsGoal = true; dpoi.IsExit = false; dpoi.PointOfInterest = goalWaypoint; dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint); dpoi.RouteTime = 0.0; return dpoi; } } } else { Coordinates current = lane.GetClosest(currentPosition); Coordinates goalPoint = lane.GetClosest(goal.Position); if ((lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) && lane.IsInside(goalPoint)) { double distToGoal = lane.DistanceBetween(current, goalPoint); if (distToGoal > 0) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = distToGoal; dpoi.IsGoal = true; dpoi.IsExit = false; dpoi.PointOfInterest = goalWaypoint; dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint); dpoi.RouteTime = 0.0; //return dpoi; } } } } } } return null; }
/// <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; }