Pathfinder library diagonal path


#1

hi, I’m having a certain problem with my code.
It’s a program with certain points (obstacles) and a robot which should find a path around it.
eventually the follower class will be a physical robot with a LiDAR sensor which sees the obstacles.

somehow the generated path does not always use the diagonals. what I’ve found is that is does when moving right to left, but not the other way around…

if someone could help me I’d appreciate it a lot!

(You can click to set a new endpoint)

code:

import java.util.*;
import pathfinder.*;

follower robot;
Graph graph;
GraphEdge[] edges;
GraphNode[] nodes;
GraphNode[] route;
IGraphSearch pathFinder;
GraphNode startNode, endNode, node;

int amount = 45;  //amount of nodes per axis (preferably uneven amount so 'robot' is in the center)
float size = 20;  //size of the spacing between the nodes (more = accurate but slower)
//total area = amount^2 * size

boolean newEndNode = true;   //boolean to check if new endNode should be determined
int len = 360;               //amount of contourpoint, in endapplication will be 1440. now 1 per degree
float[] x = new float[len];  //x location per contourpoint
float[] y = new float[len];  //y location per contourpoint
float[] angles = new float[len], distances = new float[len];
float minDist = (size*sqrt(2.0))/1.9;  //minimum distance to check for closest node
//based on square with sides length "size", diagonal = size*sqrt(2), half of that. rounding errors caused a small gap, so instead of dividing by 2, divide by 1.9

void setup() {
  size(1000, 1000); //create window of 1000 by 1000 pixels
  robot = new follower(width/2,height/2);
  //createGraph();  //create the graph based on amount and size
  translate(width/2, height/2);  //translate to the center of the window
  strokeWeight(3);               //set the strokeWeight (thickness of the lines) to 3
  for (int i = 0; i < len; i++) {  //for every contourpoint:
    angles[i] = (i*TWO_PI/len);    //determine the angle (degrees per point * current number of point)
    //next are arbitrary angles and distances to create a view of contourpoints
    if ( (angles[i] <= PI/3) || (angles[i] >= (6.5/4.0)*PI) ) {
      distances[i] = 100/cos(angles[i])*2;
    } else if ( (angles[i] > PI/3) && (angles[i] < PI/2) ) {
      distances[i] = 100/sin(angles[i]);
    } else if ( (angles[i] >= PI/2) && (angles[i] < TWO_PI/3) ) {
      float a = map(angles[i]-PI/2, 0, (TWO_PI/3)-(PI/2), 100, 200);
      distances[i] = a/cos(radians(i-90));
    } else if ( (angles[i] >= TWO_PI/3) && (angles[i] < (4*PI)/3) ) {
      distances[i] = -150*(sin(angles[i]/2)/cos(angles[i]));
    } else {
      distances[i] = angles[i] * 10;
    }
    x[i] = distances[i]*cos(angles[i]); //store contourpoints in x and y point
    y[i] = distances[i]*sin(angles[i]);
    //stroke(255,0,0);
    //point(x[i], y[i]);                  //draw points

    //if (graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist) != null) {  //if there is a graphNode at current location:
    //  graph.removeNode(graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist).id());  //remove the node (creating a wall for pathfinder), also removes connecting edges
    //}
  }
  // nodes = graph.getNodeArray();    //create node-array with all nodes left in the graph
  // edges = graph.getAllEdgeArray(); //create edge-array with all edges left in the graph
  // graph.compact();                 //remove floating edges
}
 
void mousePressed() {
  newEndNode = true;               //allow new endnode on mouseclick
}

void draw() {
  background(255);    //white background
  pathPlanner();      //custom path planning function (line 90)
  drawPath();         //custom path drawing function
  drawGraph();        //custom graph drawing function
  if (endNode != null) {  //if the endNode is not empty
    node = graph.getNodeAt(endNode.xf(), endNode.yf(), 0, minDist); //closest node on the graph to the endNode location 
    if (node != null) {      //if the node is not empty (there is a closest node)
      //stroke(255, 0, 0);     //color red
      //ellipse(node.xf(), node.yf(), 10, 10);  //draw a circle at node x and y with size 10
    }
    //stroke(0, 255, 0);   //color green 
    //ellipse(endNode.xf(), endNode.yf(), 10, 10);    //draw a circel at endNode x and y with size 10
    //stroke(0);  //color black
  }
  //float mx = mouseX-width/2;  //translated mouse x
  //float my = mouseY-height/2; //translated mouse y
  robot.show();
  //robot.update(route);

  translate(width/2, height/2); //translate to the center of the window
  strokeWeight(3);  //set line thickness to 3
  stroke(255,0,0);
  for (int i = 0; i < len; i++) {  //for every contourpoint:
    point(x[i], y[i]);             //draw contourpoint
    //ellipse(mx, my, 10, 10);       //draw circle at mouse position
  }
}

void pathPlanner() {           //custom path planner function
  createGraph();               //create the graph with size and amount
  for (int i = 0; i < len; i++) {    //for every 'obstacle' point
    if (graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist) != null) { //if there is a node
      int id = graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist).id(); //get the id of the closest node

      LinkedList<GraphEdge> edgeList1 = graph.getEdgeList(id+1); //node to the right
      LinkedList<GraphEdge> edgeList2 = graph.getEdgeList(id-1); //node to the left
      LinkedList<GraphEdge> edgeList3 = graph.getEdgeList(id+amount); //node underneath (1 row lower)
      LinkedList<GraphEdge> edgeList4 = graph.getEdgeList(id-amount); //node above      (1 row higher)
      for (int k = edgeList1.size()-1; k >= 0; k--) {      //for every edge connected to node to the right:
        graph.addEdge(edgeList1.get(k).from().id(), edgeList1.get(k).to().id(), 400);  //add edge with higher cost (400 instead of 100) 
        graph.removeEdge(edgeList1.get(k).from().id(), edgeList1.get(k).to().id());    //remove the older edge
      }
      for (int k = edgeList2.size()-1; k >= 0; k--) {      //for every edge connected to node to the left
        graph.addEdge(edgeList2.get(k).from().id(), edgeList2.get(k).to().id(), 400);  //add edge with higher cost
        graph.removeEdge(edgeList2.get(k).from().id(), edgeList2.get(k).to().id());    //remove old edge
      }
      for (int k = edgeList3.size()-1; k >= 0; k--) {      //for every edge connected to node below
        graph.addEdge(edgeList3.get(k).from().id(), edgeList3.get(k).to().id(), 400);  //add edge with higher cost
        graph.removeEdge(edgeList3.get(k).from().id(), edgeList3.get(k).to().id());    //remove old edge
      }
      for (int k = edgeList4.size()-1; k >= 0; k--) {      //for every edge connected to node above
        graph.addEdge(edgeList4.get(k).from().id(), edgeList4.get(k).to().id(), 400);  //add edge with higher cost
        graph.removeEdge(edgeList4.get(k).from().id(), edgeList4.get(k).to().id());    //remove old edge
      }
      graph.removeNode(id);//graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist).id());    //remove node
    }
  }
  startNode = graph.getNodeAt(robot.x, robot.y, 0, minDist);   //startNode is at mouse position

  graph.compact();                 //remove floating edges
  nodes = graph.getNodeArray();    //nodes array with all nodes from the graph
  edges = graph.getAllEdgeArray(); //edges array with all edges from the graph
  pathFinder = new GraphSearch_Astar(graph, new AshCrowFlight(1.0)); //set the pathfinder A* algoritm

  if (newEndNode == true) {    //if new endNode should be determined
    int randomNode = floor(random(nodes.length-1));  //find random node-ID from the nodes array
    node = nodes[randomNode];    //node is the new node
    while(node.xf() > width || node.xf() < 0 || node.yf() > height || node.yf() < 0){
      println("finding new node");
      randomNode = floor(random(nodes.length-1));
      node = nodes[randomNode];
    }
    //node = graph.getNodeAt(mouseX-width/2,mouseY-height/2,0,minDist);
    endNode = node;              //endNode is node
    //println(endNode.xf(), endNode.yf());
    newEndNode = false;          //new endNode is determined, no new one neccessary
  }
  if (startNode != null && endNode != null && node != null) { //if all neccessary nodes are determined  
    pathFinder.search(startNode.id(), node.id());  //find the path between the startNode and endNode
    route = pathFinder.getRoute();                 //node-array route stores all nodes in the path
  }
}

void createGraph() {      //function to create the graph
  graph = new Graph();    //create new graph
  // Create and add node
  GraphNode node;         //create Graphnode
  int node_id = 0;        //start at node-id 0
  int cost = 100;            //cost = 100 (standard)
  int deltaX = amount + 1;
  for (int y = 0; y < amount; y++) {    //for the amount on the y-axis
    node_id = deltaX * y + deltaX;
    for (int x = 0; x < amount; x++) {  //for the amount on the x-axis
      node = new GraphNode(node_id, (x-amount/2) * size + robot.x, (y-amount/2) * size + robot.y); //create node at location
      graph.addNode(node); //add the node to the graph
      if(x > 0){
          graph.addEdge(node_id, node_id - 1, cost);
          graph.addEdge(node_id, node_id - deltaX - 1, cost);
          graph.addEdge(node_id, node_id + deltaX - 1, cost);
      }
      if(x < amount - 1){
        graph.addEdge(node_id, node_id + 1, cost);
        graph.addEdge(node_id, node_id - deltaX - 1, cost);
        graph.addEdge(node_id, node_id + deltaX - 1, cost);
      }
      if(y > 0)
        graph.addEdge(node_id, node_id - deltaX, cost);
      if(y < amount - 1){
          graph.addEdge(node_id, node_id + deltaX, cost);
      }
      node_id++;
    }
  }
}

void drawGraph() {    //custom function to draw the graph
  /* Edges first */
  stroke(180, 180, 200);    //lighter color
  for (int i = 0; i < edges.length; i++) {  //for every edge in the edges-array
    GraphNode from = edges[i].from();       //get the startnode
    GraphNode to = edges[i].to();           //get the endnode
    int cost = int((float)(graph.getEdgeCost(from.id(), to.id()))); //get the cost from the edge between these nodes
    strokeWeight(float(cost)/100.0);        //thickness of the line depends on cost
    line(from.xf(), from.yf(), to.xf(), to.yf());  //draw the edge
  }
  /* Nodes next */
  stroke(0);        //color black
  strokeWeight(2);  //set thickness to 2
  for (int i = 0; i < nodes.length; i++) {  //for every node in the nodes-array
    GraphNode node = nodes[i];              //get the current node
    //ellipse(node.xf(), node.yf(), 2, 2);    //draw the node
    //if (i%1000 == 0)                        //every 1000 nodes:
    //text(node.id(), node.xf() + 4, node.yf() - 2); //draw text with node-id
  }
}

void drawPath() {      //custom function to draw the path
  strokeWeight(3);
  stroke(0,150,0);
  noFill();
  if (route != null && route.length != 0) {       //if the route is not empty
    //beginShape();                                 //start new shape
    //curveVertex(route[0].xf(), route[0].yf());    //start curvevertex
    for (int i = 1; i < route.length; i++) {      //for every node in the route-array

      //if (i % 2 == 0)  //every 2 points:
        //curveVertex(route[i].xf(), route[i].yf());    //add new points to smooth vertex
      line(route[i-1].xf(), route[i-1].yf(), route[i].xf(), route[i].yf());  //draw route
      
    }
    //curveVertex(route[route.length-1].xf(), route[route.length-1].yf());  //add the last vertex
    //curveVertex(route[route.length-1].xf(), route[route.length-1].yf());  //end the shape
    //endShape();    //end the shape and draw the complete line
  }
}

class follower {  //visualize the 'robot'
float x,y;        //position coordinates for robot
  follower(float startX, float startY) {  //given start position when initialised (see void setup())
    x = startX;   //position is start position
    y = startY;
  }


  void update(GraphNode[] path) {             //update function of the path following class
    int current = 0;                          //current index of the path-array
    if(path != null && path.length != 0 && path.length > 1){  //if the path is valid and the robot is not yet at the end
      float dx = path[current+1].xf()-path[current].xf();     //calculate the difference between next x-position in path and current
      float dy = path[current+1].yf()-path[current].yf();     //calculate the difference between next y-position in path and current
      println(dx,dy);   //print the diffences to console
      x += dx*0.5;      //add half of the diffence to x (0.5 determines speed, at full, it will oscillate at destination)
      y += dy*0.5;      //same as x
      current++;        //go to the next index of the path-array
      if(path.length == 1){ //if the path only has 1 point (current position is the same as end position)
        current = 0;        //current index = 0, start new because path is finished
        println("path finished"); //print to console
      }
      println("following path", dx, dy);  //print to console, it's still following with current differences
    }else println("path finished");       //print end of the path has been reached
  }  

  void show() {          //show function of the follower class
    stroke(0,0,255);     //color blue
    strokeWeight(3);     //thickness 3
    fill(0,0,255);       //fill color blue
    ellipse(x,y,2,2);    //draw circle with diameter 2 at position
  }
}

also the vertexes in drawPath are used to smoothen the line drawn for a more pleasent look, but right now that’s commented out to show the actual path


#2

okay I seem to have fixed by adding

        graph.addEdge(node_id - deltaX - 1, node_id, cost);
        graph.addEdge(node_id + deltaX - 1, node_id, cost);

twice in this part:


 if(x > 0){
        graph.addEdge(node_id, node_id - 1, cost);
        graph.addEdge(node_id, node_id - deltaX - 1, cost);
        graph.addEdge(node_id, node_id + deltaX - 1, cost);
        //added here
      }
      if(x < amount - 1){
        graph.addEdge(node_id, node_id + 1, cost);
        graph.addEdge(node_id, node_id - deltaX - 1, cost);
        graph.addEdge(node_id, node_id + deltaX - 1, cost);
        //and added here
      }

so is has kind of a “reversed” direction of the diagonals?
It just kind of seems like i’m doing stuff twice which shouldn’t be necessary.

If @quark could confirm this is necessary or if I’m actually missing something simple that would be great!
I really need quite a bit of optimization so little things like this could matter a lot…


#3

if x > 0 and x < ammount - 1 then you are duplicating edges.

graph.addEdge(node_id, node_id - 1, cost);

Will create a one-way road from node_id to node_id - 1 costing cost but if we do

graph.addEdge(node_id, node_id - 1, cost, cost);

we create a two way road.

Now consider your node grid -

  • For all nodes where x > 0 && y > 0 i.e. ignoring top row and left column create a two-way road with the 3 nodes, left, up, left-up and left-up. This last is a diagonal so its cost should be increased, but not too high that diagonals are avoided. Also if not the last row add two-way street left-down
  • For all nodes on the left edge (x == 0) but not the top one create a two-way road with the one above
  • For all nodes on the top edge (y == 0) but not the leftmost one create a two-way road with the one to the left and left-down

So I have modified your createGraph method (see below) to do this and it works.

Note I have added a diagonal cost because it is further to move. if dcost > 2 * cost then the path will avoid diagonals.

void createGraph() {      //function to create the graph
  graph = new Graph();    //create new graph
  // Create and add node
  GraphNode node;         //  create Graphnode
  int node_id = 0;        //  start at node-id 0
  int cost = 100;         //  cost = 100 (standard)
  int dcost = 140;        // diagonal cost = 140 (standard)
  int deltaX = amount + 1;
  for (int y = 0; y < amount; y++) {    //for the amount on the y-axis
    node_id = deltaX * y + deltaX;
    for (int x = 0; x < amount; x++) {  //for the amount on the x-axis
      node = new GraphNode(node_id, (x-amount/2) * size + robot.x, (y-amount/2) * size + robot.y); //create node at location
      graph.addNode(node); //add the node to the graph
      if (x > 0 && y > 0) {
        graph.addEdge(node_id, node_id - 1, cost, cost);              // Node to left
        graph.addEdge(node_id, node_id - deltaX, cost, cost);         // Node above
        graph.addEdge(node_id, node_id - deltaX - 1, dcost, dcost);   // Node above left
        if (y < amount - 1) {
          graph.addEdge(node_id, node_id + deltaX - 1, dcost, dcost); // Node below left
        }
      } else if (x == 0 && y > 0) {
        graph.addEdge(node_id, node_id - deltaX, cost, cost);         // Node above
      } else if (y == 0 && x > 0) {
        graph.addEdge(node_id, node_id - 1, cost, cost);              // Node to left
        graph.addEdge(node_id, node_id + deltaX - 1, dcost, dcost);   // Node below left
      }
      node_id++;
    }
  }
}