# Pathfinder library diagonal path

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);
} 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.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.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.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.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
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
if(x > 0){
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 - deltaX - 1, cost);
graph.addEdge(node_id, node_id + deltaX - 1, cost);
}
if(y > 0)
if(y < amount - 1){
}
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

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 - deltaX - 1, cost);
graph.addEdge(node_id, node_id + deltaX - 1, cost);
}
if(x < amount - 1){
graph.addEdge(node_id, node_id - deltaX - 1, cost);
graph.addEdge(node_id, node_id + deltaX - 1, cost);
}

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…

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

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