Instantly share code, notes, and snippets.

# benruijl/Example.java Created Aug 18, 2012

Java pathfinding utility class
 package pathfinding; import pathfinding.Grid2d; public class Example { public Example() { double[][] map = { { 0, 1, 2 }, { 3, 3, 2 }, { 0, -1, 0 } }; Grid2d map2d = new Grid2d(map, false); System.out.println(map2d.findPath(0, 0, 2, 2)); } public static void main(String[] args) { new Example(); } }
 package pathfinding; import java.util.HashSet; import java.util.List; import java.util.Set; /** * Creates nodes and neighbours from a 2d grid. Each point in the map has an * integer value that specifies the cost of crossing that point. If this value * is negative, the point is unreachable. * * If diagonal movement is allowed, the Chebyshev distance is used, else * Manhattan distance is used. * * @author Ben Ruijl * */ public class Grid2d { private final double[][] map; private final boolean allowDiagonal; /** * A node in a 2d map. This is simply the coordinates of the point. * * @author Ben Ruijl * */ public class MapNode implements Node { private final int x, y; public MapNode(int x, int y) { this.x = x; this.y = y; } public double getHeuristic(MapNode goal) { if (allowDiagonal) { return Math.max(Math.abs(x - goal.x), Math.abs(y - goal.y)); } else { return Math.abs(x - goal.x) + Math.abs(y - goal.y); } } public double getTraversalCost(MapNode neighbour) { return 1 + map[neighbour.y][neighbour.x]; } public Set getNeighbours() { Set neighbours = new HashSet(); for (int i = x - 1; i <= x + 1; i++) { for (int j = y - 1; j <= y + 1; j++) { if ((i == x && j == y) || i < 0 || j < 0 || j >= map.length || i >= map[j].length) { continue; } if (!allowDiagonal && ((i < x && j < y) || (i > x && j > y))) { continue; } if (map[j][i] < 0) { continue; } // TODO: create cache instead of recreation neighbours.add(new MapNode(i, j)); } } return neighbours; } @Override public String toString() { return "(" + x + ", " + y + ")"; } @Override public int hashCode() { final int prime = 31; int result = 1; result = prime * result + getOuterType().hashCode(); result = prime * result + x; result = prime * result + y; return result; } @Override public boolean equals(Object obj) { if (this == obj) return true; if (obj == null) return false; if (getClass() != obj.getClass()) return false; MapNode other = (MapNode) obj; if (!getOuterType().equals(other.getOuterType())) return false; if (x != other.x) return false; if (y != other.y) return false; return true; } private Grid2d getOuterType() { return Grid2d.this; } } public Grid2d(double[][] map, boolean allowDiagonal) { this.map = map; this.allowDiagonal = allowDiagonal; } public List findPath(int xStart, int yStart, int xGoal, int yGoal) { return PathFinding.doAStar(new MapNode(xStart, yStart), new MapNode( xGoal, yGoal)); } }
 package pathfinding; import java.util.Set; /** * A node in a graph, useful for pathfinding. * * @author Ben Ruijl * * @param * Actual type of the node */ public interface Node { /** * The heuristic cost to move from the current node to the goal. In most * cases this is the Manhattan distance or Chebyshev distance. * * @param goal * @return */ double getHeuristic(T goal); /** * The cost of moving from the current node to the neighbour. In most cases * this is just the distance between the nodes, but additional terrain cost * can be added as well (for example climbing a mountain is more expensive * than walking on a road). * * @param neighbour * Neighbour of current node * @return Traversal cost */ double getTraversalCost(T neighbour); /** * Gets the set of neighbouring nodes. * * @return Neighbouring nodes */ Set getNeighbours(); }
 package pathfinding; import java.util.Comparator; import java.util.HashMap; import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Map; import java.util.PriorityQueue; import java.util.Set; /** * Helper class containing pathfinding algorithms. * * @author Ben Ruijl * */ public class PathFinding { /** * A Star pathfinding. Note that the heuristic has to be monotonic: * {@code h(x) <= * d(x, y) + h(y)}. * * @param start * Starting node * @param goal * Goal node * @return Shortest path from start to goal, or null if none found */ public static > List doAStar(T start, T goal) { Set closed = new HashSet(); Map fromMap = new HashMap(); List route = new LinkedList(); Map gScore = new HashMap(); final Map fScore = new HashMap(); PriorityQueue open = new PriorityQueue(11, new Comparator() { public int compare(T nodeA, T nodeB) { return Double.compare(fScore.get(nodeA), fScore.get(nodeB)); } }); gScore.put(start, 0.0); fScore.put(start, start.getHeuristic(goal)); open.offer(start); while (!open.isEmpty()) { T current = open.poll(); if (current.equals(goal)) { while (current != null) { route.add(0, current); current = fromMap.get(current); } return route; } closed.add(current); for (T neighbour : current.getNeighbours()) { if (closed.contains(neighbour)) { continue; } double tentG = gScore.get(current) + current.getTraversalCost(neighbour); boolean contains = open.contains(neighbour); if (!contains || tentG < gScore.get(neighbour)) { gScore.put(neighbour, tentG); fScore.put(neighbour, tentG + neighbour.getHeuristic(goal)); if (contains) { open.remove(neighbour); } open.offer(neighbour); fromMap.put(neighbour, current); } } } return null; } }

### surendra4586059 commented Mar 23, 2016

 please send me struts 2 base example with table score card

### madmenyo commented Nov 28, 2016

 Great algorithm, I usually generate the complete node map including connections/edges and leave that in memory. This gets a much faster algorithm but on large maps takes a lot of memory. This is quick enough on a 32x32 area or less with a occasional hickup and the GC has a ton of work with all those nodes. I am using this in my own library for some quick and dirty pathfinding, is that ok? I altered it to make it more readable for me, it now uses my Coordinate class and I fixed diagonal neighbor lookup since that was broken.

### KimTank commented Jun 4, 2018

 Thank you 💃

### SammyVimes commented Jun 29, 2018

 Diagonal check should be: ``````if (!allowDiagonal && ((i < x && j < y) || (i < x && j > y) || (i > x && j > y) || (i > x && j < y))) { continue; } ``````

### Gregzenegair commented Oct 14, 2019

 it's not `if (map[j][i] < 0) { continue; }` but `if (map[i][j] < 0) { continue; }`