Need help going around "wall" with pathfinder

I need help with the library for path planning from @quark, although it may not actually be specific for the library.

I have the program from this thread: Path finder java pathfinder library processing, but tweaked it a little.
when you run it, you will see the specific locations too close to the wall shown in red.
What I want, is that the path goes around the wall, so that none of the locations are red if that makes sense.

I would like to keep the option to put an endpoint within that red area if needed, and the path will still find a path to there, while going through the red area as little as possible.

the idea behind it is like a robot, with a width. when it drives past the wall it should avoid it enough to not bump into it, but when the destination is again the wall, it will drive towards it, with the last part (inside the red zone) perpendicular to the wall.

the code:

import pathfinder.*;

int amount = 100;
int size;
//float[][] invalidPos;
ArrayList<PVector> invalidPos;
// PathFinding_01
Graph graph;
// These next 2 are only needed to display 
// the nodes and edges.
GraphEdge[] edges;
GraphNode[] nodes;
GraphNode[] route;
// Pathfinder algorithm
IGraphSearch pathFinder;

// Used to indicate the start and end nodes as selected by the user.
GraphNode startNode, endNode;

void setup() {
  size(600, 600);
  size = width/amount;
  textSize(8);
  // Create graph
  createGraph();
  for (int i = amount/4; i < amount-amount/4; i++) {
    graph.removeNode(i + amount/2 * amount);
  }
  // Get nodes and edges
  nodes = graph.getNodeArray();
  edges = graph.getAllEdgeArray();
  // Now get a path finder object
  pathFinder = new GraphSearch_Astar(graph);
  // Now get a route between 2 nodes
  // You can change the parameter values but they must be valid IDs
  int xStart = 0;  //start at top left
  int yStart = 0;  //start at top left
  int xEnd = amount-1; //end at bottom right
  int yEnd = amount-1; //end at bottom right
  pathFinder.search(xStart + yStart * amount, xEnd + yEnd * amount); //search for path between start and end
  route = pathFinder.getRoute();    //get the path
  //invalidPos = new float[route.length][2];
  invalidPos = new ArrayList<PVector>();
  int counter = 0;
  boolean oldValue = false;
  for (int i = 0; i < route.length; i++) {    //check every point in the path
    float x = route[i].xf(); 
    float y = route[i].yf();
    for (int j = width/4; j < width-width/4; j+= size) {  //with every point of the "wall"
      float d = dist(x, y, j, height/2); //calc the distance
      if (d < 5*size) { //if within range of 5 nodes
        for (int k = 0; k < invalidPos.size(); k++) {    //check for if the value is aleady existent.
          oldValue = (x == invalidPos.get(k).x && y == invalidPos.get(k).y);
          if (oldValue) break;
        }
        if (!oldValue) {  //if it's a new value, save it
          invalidPos.add(new PVector(x, y));     //probably more efficient with Arraylist of PVectors (done),
          //invalidPos[counter][1] = y;     //would create dynamic length instead of fixed length of route
          oldValue = true;
          counter++;
        }
      }
    }
  }
}

void draw() {
  background(0);
  drawGraph();
  drawPath();
  println(invalidPos.size());
  for (int i = 0; i < invalidPos.size(); i++) {
    stroke(255, 0, 0);
    ellipse(invalidPos.get(i).x, invalidPos.get(i).y, 5, 5);
  }
  noLoop();
}

void drawGraph() {
  // Edges first
  strokeWeight(2);
  stroke(180, 180, 200);
  //for (int i = 0; i < edges.length; i++) {
  //  GraphNode from = edges[i].from();
  //  GraphNode to = edges[i].to();
  //  line(from.xf(), from.yf(), to.xf(), to.yf());
  //}
  // Nodes next
  noStroke();
  fill(240, 255, 240);
  for (int i = 0; i < nodes.length; i++) {
    GraphNode node = nodes[i];
    ellipse(node.xf(), node.yf(), 2, 2);
  }
}

void drawPath() {
  strokeWeight(5);
  stroke(200, 255, 200, 160);
  for (int i = 1; i < route.length; i++) {
    GraphNode from = route[i-1];
    GraphNode to = route[i];
    line(from.xf(), from.yf(), to.xf(), to.yf());
  }
}

void createGraph() {
  graph = new Graph();
  // Create and add node
  GraphNode node;
  int node_id = 0;
  for (int v = 0; v < amount; v++) {
    for (int h = 0; h < amount; h++) {
      node = new GraphNode(node_id, size/2 + h * size, size/2 + v * size);
      graph.addNode(node);
      node_id++;
    }
  }
  // Create horizontal edges
  for (int v = 0; v < amount; v++) {
    for (int h = 1; h < amount; h++) {
      graph.addEdge(v * amount + h-1, v * amount + h, 0, 0);
    }
  }
  // Create vertical edges
  for (int h = 0; h < amount; h++) {
    for (int v = 1; v < amount; v++) {
      graph.addEdge(v * amount - amount + h, v * amount + h, 0, 0);
    }
  }
}

if anyone needs any clarification, ask away :smiley:

1 Like

One solution would be to modify the edge weights.

In your code you have

  // Create horizontal edges
  for (int v = 0; v < amount; v++) {
    for (int h = 1; h < amount; h++) {
      graph.addEdge(v * amount + h-1, v * amount + h, 0, 0);
    }
  }
  // Create vertical edges
  for (int h = 0; h < amount; h++) {
    for (int v = 1; v < amount; v++) {
      graph.addEdge(v * amount - amount + h, v * amount + h, 0, 0);
    }
  }

In the two calls to addEdge the last two parameters are the edge weights from-to node and to-from node. At the moment they are both zero so the library will use the distance between the nodes for the weightings.

Instead set them to some nominal amount, say 100. Now we want to keep away from the wall when passing it but still be able to approach a destination near the wall. In this picture we would want to avoid the nodes in the blue rectangle unless it was a destination. So the way to do it is to increase the edge weights for all edges in this area say about 200.

37

Working out where the walls are and where ‘near-wall’ zones are is quite difficult so here is an easier way to do it.

Using a paint program first create an image the same size as the sketch area, in this case 600x600pixels. Now make the background white add black rectangles for the walls and red areas for the near wall positions and save the image off as a png (don’t use jpg).

Now create all the nodes in the grid even if they correspond to a wall position, you will remove these nodes later just like you before.

Now when you create the edges first calculate the [x,y] coordinates for both nodes and get the colours in the image at those coordinates. Now adjust the edge weight according to the colours found, low if white high if red you can even have a different weight if one is red and the other white!

Finally remove all nodes whose position corresponds to a black pixel, this will automatically delete any edges connected to it.

You can extend on this idea by using extra colours to represent different types of area e.g. terrain type giving each colour different weights.

1 Like

Yess thank you so much!
I tried it without image in the background, which was probable a little harder (though with just this straight line not too bad) and as you can see it even keeps going around it and only goes inside the red zone when it’s perpendicular to the wall.
the thicker lines have the “cost” of 200, while the rest has 100, and I adjusted to strokeWeight() accordingly with getEdgeCost().

Really useful library @quark! :smiley: