/** 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("");
}
