citizen/PathFinding.pde

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 initPathFinding() {
  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();
  //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 usePathFinder(IGraphSearch pf) {
  pf.search(start, end, true);
  rNodes = pf.getRoute();
  exploredEdges = pf.getExaminedEdges();
}

void displayPathFinding() {
  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 mousePressed() {
  startNode = gs.getNodeAt(mouseX, mouseY, 0, 10.0f);
  if (startNode != null)
    selectMode = true;
}

void mouseDragged() {
  if (selectMode)
    endNode = gs.getNodeAt(mouseX, mouseY, 0, 10.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) +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(){
  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();
 }