public double PathSize(RouteMeasures route) { double size = 0; if (route.Status != EPathStatus.Found) { return(-1); } var nodes = route.NodeIDs; Node current = this._graph.GetNodeById(long.Parse(nodes[0])); for (var i = 1; i < nodes.Length; i++) { long nextNodeId = long.Parse(nodes[i]); var nextNode = this._graph.GetNodeById(nextNodeId); PathRoute pathRoute = Node.ShortestPathBetweenNeihbors(current, nextNode); if (pathRoute.Status == EPathStatus.Found) { current = nextNode; size += pathRoute.Distance; } else { return(-1); } } return(size); }
public void CorrectlyBuildPathRouteFromEdges_AradToSibiu() { // to bucharest from arad running 418km (arad > sibiu > rimnicu vilcea > pitesti > bucharest) Graph graph = Import.LoadCityFromText(InfraTests.file_path + "test_graph_3.norvig.txt"); var arad = graph.GetNodeByLabel("Arad"); var sibiu = graph.GetNodeByLabel("Sibiu"); var rimnicuVilcea = graph.GetNodeByLabel("Rimnicu Vilcea"); var pitesti = graph.GetNodeByLabel("Pitesti"); var bucharest = graph.GetNodeByLabel("Bucharest"); var edge_arad_sibiu = graph.GetEdgeByLabel("arad_sibiu"); var edge_sibiu_rimnicu_vilcea = graph.GetEdgeByLabel("sibiu_rimnicu_vilcea"); var edge_rimnicu_vilcea_pitesti = graph.GetEdgeByLabel("rimnicu_vilcea_pitesti"); var edge_pitesti_bucharest = graph.GetEdgeByLabel("pitesti_bucharest"); var edges = new Edge[] { edge_arad_sibiu, edge_sibiu_rimnicu_vilcea, edge_rimnicu_vilcea_pitesti, edge_pitesti_bucharest }; var pathRoute = new PathRoute(EPathStatus.Found, arad, sibiu, edges, 1); // pathRoute.Nodes.ToList().ForEach(Console.WriteLine); Assert.Equal(EPathStatus.Found, pathRoute.Status); Assert.Equal(new Node[] { arad, sibiu, rimnicuVilcea, pitesti, bucharest }, pathRoute.Nodes); Assert.Equal(4, pathRoute.Jumps); }
public static PathRoute FindRoute(PathPosition startPos, PathPosition endPos) { if (startPos == null || endPos == null || startPos._path == null || endPos._path == null) { return(null); } PathRoute bestRoute = null; List <PathNode> linkedNodes = new List <PathNode>(); PathNode nodeA = GetNextNode(startPos, Direction1D.Forwards); if (nodeA != null && nodeA.isActiveAndEnabled) { linkedNodes.Add(nodeA); } PathNode nodeB = GetNextNode(startPos, Direction1D.Backwards); if (nodeB != null && nodeA != nodeB && nodeB.isActiveAndEnabled) { linkedNodes.Add(nodeB); } List <PathNode> traversedNodes = new List <PathNode>(); FindRoute(startPos, endPos, startPos._path, startPos._pathT, linkedNodes, 0.0f, new List <PathRouteWaypoint>(), ref bestRoute, ref traversedNodes); return(bestRoute); }
private void WipeData() { Log = new List <Tuple <string, Color> >(); MapMarkers = new Dictionary <string, GMapMarker>(); MarkersQueue = new Queue <NewMapObject>(); LogQueue = new Queue <Tuple <string, Color> >(); PathRoute.Points.Clear(); PathRoute.RegenerateShape(null); }
public override PathRoute Search(Node source, Node target, double radius) { PathRoute pathRoute = this.SearchParametersEvaluation(source, target, radius); if (pathRoute != null) { return(pathRoute); } var superSource = Collapse.collapse(this.Graph, source, radius, -1, 0); var superTarget = Collapse.collapse(this.Graph, target, radius, -2, 0); try { pathRoute = Graph.ShortestPathHeuristic(superSource, superTarget); if (pathRoute.Status == EPathStatus.Found) { var fakeSource = Node.ShortestPathBetweenNeihbors(pathRoute.Nodes[0], pathRoute.Nodes[1]).Edges[0]; var fakeTarget = Node.ShortestPathBetweenNeihbors(pathRoute.Nodes[pathRoute.Nodes.Count() - 2], pathRoute.Nodes[pathRoute.Nodes.Count() - 1]).Edges[0]; var originalSource = (Edge)fakeSource.GetAttribute("original_edge"); var originalTarget = (Edge)fakeTarget.GetAttribute("original_edge"); //TODO: update the pathroute distance too? pathRoute.Nodes[0] = originalSource.Source; pathRoute.Nodes[pathRoute.Nodes.Count() - 1] = originalTarget.Target; pathRoute.Source = source; pathRoute.Edges[0] = originalSource; pathRoute.Edges[pathRoute.Edges.Count() - 1] = originalTarget; pathRoute.Target = target; } else { pathRoute.Source = source; pathRoute.Target = target; } } catch (Exception e) { pathRoute = new PathRoute(EPathStatus.UnexpectedException, source, target, e); } finally { Collapse.Expand(this.Graph, -1); Collapse.Expand(this.Graph, -2); } return(pathRoute); }
public void ForTheGivenPathReturnTheCorrectNodesForGraph0() { var g = G0(); Edge[] edges = new Edge[3]; edges[0] = g.GetNodeById(1).EdgesWhenSourceOf(g.GetNodeById(2))[0]; edges[1] = g.GetNodeById(2).EdgesWhenSourceOf(g.GetNodeById(3))[0]; edges[2] = g.GetNodeById(3).EdgesWhenSourceOf(g.GetNodeById(4))[0]; Node[] nodes = new Node[] { g.GetNodeById(1), g.GetNodeById(2), g.GetNodeById(3), g.GetNodeById(4) }; PathRoute pr = new PathRoute(EPathStatus.Found, g.GetNodeById(1), g.GetNodeById(3), edges, 4); Assert.Equal(nodes, pr.Nodes); }
public void CorrectlyBuildPathRouteFromEdges_1To3() { Graph g = getGraph(3); var p_1 = g.GetNodeById(1); var p_3 = g.GetNodeById(3); var e_1_3 = g.GetEdgeByLabel("1_3"); var edges = new Edge[] { e_1_3 }; var pathRoute = new PathRoute(EPathStatus.Found, p_1, p_3, edges, 1); Assert.Equal(new Node[] { p_1, p_3 }, pathRoute.Nodes); Assert.Equal(1, pathRoute.Jumps); Assert.Equal(EPathStatus.Found, pathRoute.Status); }
public override PathRoute Search(Node source, Node target, double radius) { PathRoute betterPathRoute = this.SearchParametersEvaluation(source, target, radius); if (betterPathRoute != null) { betterPathRoute.Source = source; betterPathRoute.Target = target; return(betterPathRoute); } var sources = this.Graph.GetNodesByRadius(source, radius); var targets = this.Graph.GetNodesByRadius(target, radius); foreach (var origin in sources) { foreach (var destination in targets) { if (origin != destination) { PathRoute pathRoute = Graph.ShortestPathHeuristic(origin, destination); if (betterPathRoute == null || betterPathRoute.Status != EPathStatus.Found || (betterPathRoute.Status == EPathStatus.Found && pathRoute.Status == EPathStatus.Found && betterPathRoute.Distance > pathRoute.Distance)) { betterPathRoute = pathRoute; } } } } if (betterPathRoute != null)// && betterPathRoute.Status != EPathStatus.Found) { betterPathRoute.Source = source; betterPathRoute.Target = target; } return(betterPathRoute); }
private void WipeData() { try { Log = new List <Tuple <string, Color> >(); MapMarkers = new Dictionary <string, GMapMarker>(); MarkersQueue = new Queue <NewMapObject>(); LogQueue = new Queue <Tuple <string, Color> >(); PathRoute.Points.Clear(); PathRoute.RegenerateShape(null); } catch (Exception ex) { Session.EventDispatcher.Send(new WarnEvent { Message = "Error during wiping bot data!" }); Logger.Write($"[WIPE FAIL] Error: {ex.Message}", LogLevel.Error); } }
public void CorrectlyBuildPathRouteFromEdges_1To3_3To2_2To4() { Graph g = getGraph(3); var p_1 = g.GetNodeById(1); var p_3 = g.GetNodeById(3); var p_2 = g.GetNodeById(2); var p_4 = g.GetNodeById(4); var e_1_3 = g.GetEdgeByLabel("1_3"); var e_3_2 = g.GetEdgeByLabel("3_2"); var e_2_4 = g.GetEdgeByLabel("2_4"); var edges = new Edge[] { e_1_3, e_3_2, e_2_4 }; var pathRoute = new PathRoute(EPathStatus.Found, p_1, p_4, edges, 3); Assert.Equal(new Node[] { p_1, p_3, p_2, p_4 }, pathRoute.Nodes); Assert.Equal(3, pathRoute.Jumps); Assert.Equal(EPathStatus.Found, pathRoute.Status); }
public static PathRoute Search(Map.MapNode[,] MapNode, Vector3 Position, Vector3 objPosition, PathRoute Path, List <Map.MapNode> openList, List <Map.MapNode> closedList) { Map.MapNode start = Map.ConvertPositionToGrid(MapNode, Position); Map.MapNode obj = Map.ConvertPositionToGrid(MapNode, objPosition); return(aStarSearch(start, obj, openList, closedList, MapNode, Path)); }
private static PathRoute aStarSearch(Map.MapNode start, Map.MapNode obj, List <Map.MapNode> openList, List <Map.MapNode> closedList, Map.MapNode[,] grid, PathRoute path) { Reset(grid, openList, closedList); Map.MapNode currentNode = start; // will be changed to whichever has the lowerest cost int x = currentNode.posX; int y = currentNode.posY; currentNode.fcost = Map.HCost(obj, currentNode); // calc f cost currentNode.cameFrom[0] = currentNode; openList.Add(currentNode); while (openList.Count != 0) { if (currentNode.id == obj.id) { closedList.Add(currentNode); path = generatePath(closedList, grid, start, path); return(path); } else { for (int i = 0; i < currentNode.neighbours.Count; i++) //loop all neighbours { x = currentNode.neighbours[i].posX; y = currentNode.neighbours[i].posY; var temp = currentNode.neighbours[i]; temp.gcost = Map.GCost(currentNode, currentNode.neighbours[i]); temp.hcost = Map.HCost(obj, currentNode.neighbours[i]); temp.cameFrom[0] = currentNode; temp.fcost = temp.gcost + temp.hcost; currentNode.neighbours[i] = temp; for (int j = 0; j < openList.Count; j++) { if (openList[j].id == currentNode.neighbours[i].id) //if it's already on the open list then { grid[x, y].state = Map.status.onOpenList; if (temp.fcost >= openList[j].fcost) //we already have a cheaper node { break; } if (temp.fcost < openList[j].fcost) //if this happens our heuristic is broken { openList[j].parent[0] = currentNode; } } } if (grid[x, y].state != Map.status.onOpenList) //if not on the openList we check if it's already on the closed list if it isn't add it to the openlist { for (int k = 0; k < closedList.Count; k++) { if (closedList[k].id == currentNode.neighbours[i].id) //if already on the closed list, check if neighbour is cheaper if it is set parent { grid[x, y].state = Map.status.searched; if (temp.fcost >= closedList[k].fcost) { break; } if (temp.fcost < closedList[k].fcost) { closedList[k].parent[0] = currentNode; } } } if (grid[x, y].state != Map.status.searched) { currentNode.neighbours[i].parent[0] = currentNode; grid[x, y].state = Map.status.onOpenList; openList.Add(currentNode.neighbours[i]); } } } x = currentNode.posX; y = currentNode.posY; grid[x, y].state = Map.status.searched; openList.Remove(currentNode); closedList.Add(currentNode); openList.Sort(delegate(Map.MapNode a, Map.MapNode b) { return(a.fcost.CompareTo(b.fcost)); }); //sorting function so first one on openList is cheapest currentNode = openList[0]; } } return(path); }
public static PathRoute generatePath(List <Map.MapNode> closedList, Map.MapNode[,] grid, Map.MapNode Start, PathRoute Path) { int index = closedList.Count - 1; //need to work backwards to try and get to the start node int PathIndex = 0; Map.MapNode a = closedList[index]; Map.MapNode temp = a; Path.PathList[0] = Map.ConvertGridNodeToVector(a); ++PathIndex; while (temp.id != Start.id) { Path.PathList[PathIndex] = (Map.ConvertGridNodeToVector(a.parent[0])); temp = a.parent[0]; a = temp; ++PathIndex; } Path.PathSize = PathIndex; Path.Index = 0; Array.Reverse(Path.PathList, 0, PathIndex); return(Path); }
private static void FindRoute(PathPosition startPos, PathPosition endPos, Path currentPath, float currentPosT, List <PathNode> nextNodes, float currentDistance, List <PathRouteWaypoint> currentRoute, ref PathRoute bestRoute, ref List <PathNode> traversedNodes) { //Check if the end position is on the current path. bool endPosOnCurrentPath = false; //If end position is a path node if (endPos._pathNode != null) { foreach (Path path in endPos._pathNode.GetPaths()) { if (path == currentPath) { endPos._path = currentPath; endPos._pathT = currentPath.GetPathT(endPos._pathNode); endPosOnCurrentPath = true; break; } } } else if (currentPath == endPos._path) { endPosOnCurrentPath = true; } //If current path is the same as the end pos path, check direct route to end pos if (endPosOnCurrentPath) { float routeDistance = currentDistance + currentPath.GetDistanceBetween(currentPosT, endPos._pathT); if (bestRoute == null || routeDistance < bestRoute._distance) { bestRoute = new PathRoute(); bestRoute._startPosition = startPos; bestRoute._endPosition = endPos; bestRoute._waypoints = currentRoute.ToArray(); bestRoute._distance = routeDistance; } } //loop through all possible routes from this node foreach (PathNode node in nextNodes) { //Need to check if already searched this node (to stop loops) if (node != null && !traversedNodes.Contains(node)) { //First work out the rating to this path node. only consider a route to this node if the rating is still better than the best route rating float nodeT = currentPath.GetPathT(node); float routeDistance = currentDistance + currentPath.GetDistanceBetween(currentPosT, nodeT); if (bestRoute == null || routeDistance < bestRoute._distance) { traversedNodes.Add(node); foreach (Path path in node.GetPaths()) { if (path.IsActive()) { List <PathRouteWaypoint> route = new List <PathRouteWaypoint>(currentRoute); PathRouteWaypoint waypoint = new PathRouteWaypoint(); waypoint._path = path; waypoint._pathNode = node; route.Add(waypoint); FindRoute(startPos, endPos, path, path.GetPathT(node), GetLinkedPathNodes(node, path), routeDistance, route, ref bestRoute, ref traversedNodes); } } } } } }