added pathfinding

Map.pde

@@ -1,23 +1,6 @@
 import java.util.ArrayDeque;
 import pathfinder.*;
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
 PImage img_houses;
 PImage img_streets;
 PImage img_spawnPoints;

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;
+}