159 lines
3.9 KiB
Plaintext
159 lines
3.9 KiB
Plaintext
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();
|
|
}
|