/** Plots a path between the Agent's home Node and its work Node */ private void findNewAStarPath(Gridlock_NorfolkTEST geoTest) { // get the home and work Nodes with which this Agent is associated Node currentJunction = geoTest.network.findNode(location.geometry.getCoordinate()); Node destinationJunction = workNode; if (currentJunction == null) { return; // just a check } // find the appropriate A* path between them AStar pathfinder = new AStar(); ArrayList<GeomPlanarGraphDirectedEdge> path = pathfinder.astarPath(currentJunction, destinationJunction); // if the path works, lay it in if (path != null && path.size() > 0) { // save it pathFromHomeToWork = path; // set up how to traverse this first link GeomPlanarGraphEdge edge = (GeomPlanarGraphEdge) path.get(0).getEdge(); setupEdge(edge); // update the current position for this link updatePosition(segment.extractPoint(currentIndex)); } }
public void testSmall() { State node5 = new State(5, 0, 0, true); State node6 = new State(6, 0, 0, true); State node2 = new State(2, 1, 6, false); node2.next[0] = node5; node2.cost[0] = 6; State node3 = new State(3, 2, 2, false); node3.next[0] = node5; node3.cost[0] = 4; node3.next[1] = node6; node3.cost[1] = 2; State node4 = new State(4, 1, 6, false); node4.next[0] = node6; node4.cost[0] = 6; State node0 = new State(0, 2, 4, false); node0.next[0] = node2; node0.cost[0] = 2; node0.next[1] = node3; node0.cost[1] = 2; State node1 = new State(1, 2, 3, false); node1.next[0] = node3; node1.cost[0] = 1; node1.next[1] = node4; node1.cost[1] = 1; State[][] paths = new State[6][]; double[] costs = new double[6]; paths[0] = new State[] {node6, node3, node1}; costs[0] = 3; paths[1] = new State[] {node6, node3, node0}; costs[1] = 4; paths[2] = new State[] {node5, node3, node1}; costs[2] = 5; paths[3] = new State[] {node5, node3, node0}; costs[3] = 6; paths[4] = new State[] {node6, node4, node1}; costs[4] = 7; paths[5] = new State[] {node5, node2, node0}; costs[5] = 8; AStar s = new AStar(new State[] {node0, node1}, 7); int i = 0; while (s.hasNext()) { assertTrue("number of answers > " + i, i < 6); SearchNode n = s.nextAnswer(); assertEquals("costs[" + i + "] != " + n.getPriority(), costs[i], n.getPriority(), 1e-5); int j = 0; while (n != null) { assertTrue("path length > " + j, j < 3); assertTrue("path[" + i + "][" + j + "] != " + n, paths[i][j] == n.getState()); j++; n = (SearchNode) n.getParent(); } assertTrue("path length != " + j, j == 3); i++; } assertTrue("number of answers != " + i, i == 6); }
/* * Each maze is stored as a (connected) graph: all nodes have neighbours, stored in an array of length 4. The * index of the array associates the direction the neighbour is located at: '[up,right,down,left]'. * For instance, if node '9' has neighbours '[-1,12,-1,6]', you can reach node '12' by going right, and node * 6 by going left. The directions returned by the controllers should thus be in {0,1,2,3} and can be used * directly to determine the next node to go to. */ public Maze(int index) { loadNodes(nodeNames[index]); loadDistances(distNames[index]); // create A* graph for shortest paths for the ghosts astar = new AStar(); astar.createGraph(graph); }
public static void main(String[] args) { Quadrado[][] grade = new Quadrado[10][10]; for (int i = 0; i < grade.length; i++) { for (int j = 0; j < grade[i].length; j++) { grade[i][j] = new Quadrado(i, j); } } // configura grade Quadrado origem = grade[1][5]; Quadrado destino = grade[9][5]; AStar aStar = new AStar(grade, origem, destino); aStar.addBloqueio(grade[4][3]); aStar.addBloqueio(grade[4][4]); aStar.addBloqueio(grade[4][5]); aStar.addBloqueio(grade[4][6]); aStar.addBloqueio(grade[4][7]); aStar.addBloqueio(grade[8][4]); aStar.addBloqueio(grade[9][4]); long tempo1 = System.nanoTime(); boolean pesquisaOk = aStar.iniciarPesquisa(); long tempo2 = System.nanoTime(); System.out.println("Tempo de pesquisa: " + (tempo2 - tempo1) + "ns"); if (pesquisaOk) { System.out.println("Caminho:"); System.out.println("X\tY"); // lista caminho encontrado for (int i = 0; i < aStar.getListaCaminho().size(); i++) { System.out.println( aStar.getListaCaminho().get(i).getX() + "\t" + aStar.getListaCaminho().get(i).getY()); } } else { System.out.println("Caminho nao foi encontrado."); } // imprime em forma de matriz for (int i = 0; i < grade.length; i++) { System.out.println(""); for (int j = 0; j < grade[i].length; j++) { if (origem.getX() == j && origem.getY() == i) { System.out.print("[O]"); } else if (destino.getX() == j && destino.getY() == i) { System.out.print("[D]"); } else { boolean isCaminho = false; for (int k = 0; k < aStar.getListaCaminho().size(); k++) { if (aStar.getListaCaminho().get(k).getX() == j && aStar.getListaCaminho().get(k).getY() == i) { System.out.print("[*]"); isCaminho = true; break; } } boolean isBloqueio = false; for (int k = 0; k < aStar.getListaBloqueios().size(); k++) { if (aStar.getListaBloqueios().get(k).getX() == j && aStar.getListaBloqueios().get(k).getY() == i) { System.out.print("[X]"); isBloqueio = true; break; } } if (!isCaminho && !isBloqueio) { System.out.print("[ ]"); } } } } System.out.println(""); }