diff --git a/Map.pde b/Map.pde index c9983e3..73a2e97 100644 --- a/Map.pde +++ b/Map.pde @@ -1,23 +1,6 @@ import java.util.ArrayDeque; import pathfinder.*; - - - - - - - - - - - - - - - - - PImage img_houses; PImage img_streets; PImage img_spawnPoints; diff --git a/PathFinding.pde b/PathFinding.pde new file mode 100644 index 0000000..79173c5 --- /dev/null +++ b/PathFinding.pde @@ -0,0 +1,249 @@ +import pathfinder.*; + + +Graph gs; +GraphNode[] gNodes, rNodes; +GraphEdge[] gEdges, exploredEdges; +int start, end; +int pathAlgorithm = 3; +float nodeSize; +IGraphSearch pathFinder; +GraphNode startNode, endNode; //put inside citizen-class +boolean selectMode = false; //put inside citizen-class, only for manual interference + +void nodeDisplay() { + drawNodes(); + drawRoute(rNodes, color(200, 0, 0), 5.0f); + + if (selectMode) { + stroke(0); + strokeWeight(1.5f); + if (endNode != null) + line(startNode.xf(), startNode.yf(), endNode.xf(), endNode.yf()); + else + line(startNode.xf(), startNode.yf(), mouseX, mouseY); + } +} + +void drawNodes(){ + pushStyle(); + noStroke(); + fill(255,0,255,72); + for(GraphNode node : gNodes) + ellipse(node.xf(), node.yf(), nodeSize, nodeSize); + popStyle(); +} + +void drawEdges(GraphEdge[] edges, int lineCol, float sWeight, boolean arrow){ + if(edges != null){ + pushStyle(); + noFill(); + stroke(lineCol); + strokeWeight(sWeight); + for(GraphEdge ge : edges){ + if(arrow) { + //drawArrow(ge.from(), ge.to(), nodeSize / 2.0f, 6); + } else { + line(ge.from().xf(), ge.from().yf(), ge.to().xf(), ge.to().yf()); + } + } + popStyle(); + } +} + +void drawRoute(GraphNode[] r, int lineCol, float sWeight){ + if(r.length >= 2){ + pushStyle(); + stroke(lineCol); + strokeWeight(sWeight); + noFill(); + for(int i = 1; i < r.length; i++) + line(r[i-1].xf(), r[i-1].yf(), r[i].xf(), r[i].yf()); + // Route start node + strokeWeight(2.0f); + stroke(0,0,160); + fill(0,0,255); + ellipse(r[0].xf(), r[0].yf(), nodeSize, nodeSize); + // Route end node + stroke(160,0,0); + fill(255,0,0); + ellipse(r[r.length-1].xf(), r[r.length-1].yf(), nodeSize, nodeSize); + popStyle(); + } +} + +void drawArrow(GraphNode fromNode, GraphNode toNode, float nodeRad, float arrowSize){ + float fx, fy, tx, ty; + float ax, ay, sx, sy, ex, ey; + float awidthx, awidthy; + + fx = fromNode.xf(); + fy = fromNode.yf(); + tx = toNode.xf(); + ty = toNode.yf(); + + float deltaX = tx - fx; + float deltaY = (ty - fy); + float d = sqrt(deltaX * deltaX + deltaY * deltaY); + + sx = fx + (nodeRad * deltaX / d); + sy = fy + (nodeRad * deltaY / d); + ex = tx - (nodeRad * deltaX / d); + ey = ty - (nodeRad * deltaY / d); + ax = tx - (nodeRad + arrowSize) * deltaX / d; + ay = ty - (nodeRad + arrowSize) * deltaY / d; + + awidthx = - (ey - ay); + awidthy = ex - ax; + + noFill(); + strokeWeight(4.0f); + stroke(160, 128); + line(sx,sy,ax,ay); + + noStroke(); + fill(48, 128); + beginShape(TRIANGLES); + vertex(ex, ey); + vertex(ax - awidthx, ay - awidthy); + vertex(ax + awidthx, ay + awidthy); + endShape(); + } + + +void initPathFinding() { + img_nodes = createImage(img_houses.width, img_houses.height, ARGB); + nodeSize = 10.0f; + gs = new Graph(); + makeGraphFromImage(gs, img_houses, 20, 20, true); + gNodes = gs.getNodeArray(); + end = gNodes[(int) random(0, gNodes.length / 4)].id(); + do + start = gNodes[(int) random((3 * gNodes.length) / 4, gNodes.length/4)].id(); + while (start == end); + gs.compact(); + + // Get arrays of both the nodes and edges used by the + // selected graph. + gNodes = gs.getNodeArray(); + gEdges = gs.getAllEdgeArray(); + // Create a path finder object based on the algorithm + pathFinder = makePathFinder(gs, pathAlgorithm); + usePathFinder(pathFinder); +} + + +void mousePressed() { + startNode = gs.getNodeAt(mouseX, mouseY, 0, 30.0f); + if (startNode != null) + selectMode = true; +} + +void mouseDragged() { + if (selectMode) + endNode = gs.getNodeAt(mouseX, mouseY, 0, 30.0f); +} + +void mouseReleased() { + if (selectMode && endNode!= null && startNode != null && startNode != endNode) { + start = startNode.id(); + end = endNode.id(); + usePathFinder(pathFinder); + } + selectMode = false; + startNode = endNode = null; +} + + + +PImage img_nodes; +void makeGraphFromImage(Graph g, PImage map, int tilesX, int tilesY, boolean allowDiagonals) { + img_nodes.loadPixels(); + for (int i = 0; i < img_nodes.pixels.length; i++) { + if (map.pixels[i] != -1) { + img_nodes.pixels[i] = int(color(0)); + } else { + img_nodes.pixels[i] = int(color(255)); + } + } + img_nodes.updatePixels(); + + int dx = img_nodes.width / tilesX; + int dy = img_nodes.height / tilesY; + int sx = dx / 2, sy = dy / 2; + // use deltaX to avoid horizontal wrap around edges + int deltaX = tilesX + 1; // must be > tilesX + + float hCost = dx, vCost = dy, dCost = sqrt(dx*dx + dy*dy); + float cost = 0; + int px, py, nodeID, col; + GraphNode aNode; + + py = sy; + for (int y = 0; y < tilesY; y++) { + nodeID = deltaX * y + deltaX; + px = sx; + for (int x = 0; x < tilesX; x++) { + // Calculate the cost + col = img_nodes.get(px, py) & 0xFF; + cost = 1; + // If col is not black then create the node and edges + if (col != 0) { + aNode = new GraphNode(nodeID, px, py); + g.addNode(aNode); + if (x > 0) { + g.addEdge(nodeID, nodeID - 1, hCost * cost); + if (allowDiagonals) { + g.addEdge(nodeID, nodeID - deltaX - 1, dCost * cost); + g.addEdge(nodeID, nodeID + deltaX - 1, dCost * cost); + } + } + if (x < tilesX -1) { + g.addEdge(nodeID, nodeID + 1, hCost * cost); + if (allowDiagonals) { + g.addEdge(nodeID, nodeID - deltaX + 1, dCost * cost); + g.addEdge(nodeID, nodeID + deltaX + 1, dCost * cost); + } + } + if (y > 0) + g.addEdge(nodeID, nodeID - deltaX, vCost * cost); + if (y < tilesY - 1) + g.addEdge(nodeID, nodeID + deltaX, vCost * cost); + } + px += dx; + nodeID++; + } + py += dy; + } +} + + + +void usePathFinder(IGraphSearch pf) { + pf.search(start, end, true); + rNodes = pf.getRoute(); + exploredEdges = pf.getExaminedEdges(); +} + +IGraphSearch makePathFinder(Graph graph, int pathFinder) { + IGraphSearch pf = null; + float f = 1.0f; + switch(pathFinder) { + case 0: + pf = new GraphSearch_DFS(gs); + break; + case 1: + pf = new GraphSearch_BFS(gs); + break; + case 2: + pf = new GraphSearch_Dijkstra(gs); + break; + case 3: + pf = new GraphSearch_Astar(gs, new AshCrowFlight(f)); + break; + case 4: + pf = new GraphSearch_Astar(gs, new AshManhattan(f)); + break; + } + return pf; +}