citizen/PathFinding.pde

import pathfinder.*;

Graph gs;
GraphNode[] gNodes;
GraphEdge[] gEdges;
float nodeSize;

void initPathFinding() {
  //generate a public path finding graph and get nodemap 
  img_nodes = createImage(img_houses.width, img_houses.height, ARGB);
  nodeSize = 5.0f;
  gs = new Graph();
  makeGraphFromImage(gs, img_houses, int(img_houses.width * f_nodeResolution), int(img_houses.height * f_nodeResolution), true);
  //gNodes =  gs.getNodeArray();
  gs.compact();
  gNodes =  gs.getNodeArray();
  gEdges = gs.getAllEdgeArray();
}



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) +1;
  int dy = (img_nodes.height / tilesY) +1;
  
  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;
  }
}



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

void drawNodes(){
  pg_map.pushStyle();
  pg_map.noStroke();
  pg_map.fill(255,0,255,72);
  for(GraphNode node : gNodes)
    pg_map.ellipse(node.xf(), node.yf(), nodeSize, nodeSize);
  pg_map.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;

  pg_map.noFill();
  pg_map.strokeWeight(4.0f);
  pg_map.stroke(160, 128);
  pg_map.line(sx,sy,ax,ay);

  pg_map.noStroke();
  pg_map.fill(48, 128);
  pg_map.beginShape(TRIANGLES);
  pg_map.vertex(ex, ey);
  pg_map.vertex(ax - awidthx, ay - awidthy);
  pg_map.vertex(ax + awidthx, ay + awidthy);
  pg_map.endShape();
 }