public ZoneNavigationEdgeSort(double distance, NavigableEdge edge, LinePath lp) { this.distance = distance; this.edge = edge; this.lp = lp; this.zp = null; }
/// <summary> /// Plan in a zone /// </summary> /// <param name="az"></param> /// <param name="goal"></param> /// <returns></returns> public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal) { // zone plan ZonePlan zp = new ZonePlan(); zp.Zone = az; zp.Start = start; // given start and goal, get plan double time; List <INavigableNode> nodes; this.Plan(start, goal, out nodes, out time); zp.Time = time; // final path LinePath recommendedPath = new LinePath(); List <INavigableNode> pathNodes = new List <INavigableNode>(); // start and end counts int startCount = 0; int endCount = 0; // check start type if (start is ArbiterParkingSpotWaypoint) { startCount = 2; } // check end type if (goal is ArbiterParkingSpotWaypoint && ((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az)) { endCount = -2; } // loop through nodes for (int i = startCount; i < nodes.Count + endCount; i++) { // go to parking spot or endpoint if (nodes[i] is ArbiterParkingSpotWaypoint) { // set zone goal zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return(zp); } // go to perimeter waypoint if this is one else if (nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit) { // add recommendedPath.Add(nodes[i].Position); // set zone goal zp.ZoneGoal = nodes[i]; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return(zp); } // otherwise just add else { // add recommendedPath.Add(nodes[i].Position); pathNodes.Add(nodes[i]); } } // set zone goal zp.ZoneGoal = goal; // set path, return zp.RecommendedPath = recommendedPath; // set route info CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString()); // return the plan return(zp); }
/// <summary> /// Plan in a zone /// </summary> /// <param name="az"></param> /// <param name="goal"></param> /// <returns></returns> public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal) { // zone plan ZonePlan zp = new ZonePlan(); zp.Zone = az; zp.Start = start; // given start and goal, get plan double time; List<INavigableNode> nodes; this.Plan(start, goal, out nodes, out time); zp.Time = time; // final path LinePath recommendedPath = new LinePath(); List<INavigableNode> pathNodes = new List<INavigableNode>(); // start and end counts int startCount = 0; int endCount = 0; // check start type if(start is ArbiterParkingSpotWaypoint) startCount = 2; // check end type if(goal is ArbiterParkingSpotWaypoint && ((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az)) endCount = -2; // loop through nodes for(int i = startCount; i < nodes.Count + endCount; i++) { // go to parking spot or endpoint if(nodes[i] is ArbiterParkingSpotWaypoint) { // set zone goal zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return zp; } // go to perimeter waypoint if this is one else if(nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit) { // add recommendedPath.Add(nodes[i].Position); // set zone goal zp.ZoneGoal = nodes[i]; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return zp; } // otherwise just add else { // add recommendedPath.Add(nodes[i].Position); pathNodes.Add(nodes[i]); } } // set zone goal zp.ZoneGoal = goal; // set path, return zp.RecommendedPath = recommendedPath; // set route info CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString()); // return the plan return zp; }