Instantly share code, notes, and snippets.

Embed
What would you like to do?
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<MapNode> {
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<MapNode> getNeighbours() {
Set<MapNode> neighbours = new HashSet<MapNode>();
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<MapNode> 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 <T>
* Actual type of the node
*/
public interface Node<T> {
/**
* 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<T> 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 <T extends Node<T>> List<T> doAStar(T start, T goal) {
Set<T> closed = new HashSet<T>();
Map<T, T> fromMap = new HashMap<T, T>();
List<T> route = new LinkedList<T>();
Map<T, Double> gScore = new HashMap<T, Double>();
final Map<T, Double> fScore = new HashMap<T, Double>();
PriorityQueue<T> open = new PriorityQueue<T>(11, new Comparator<T>() {
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

This comment has been minimized.

surendra4586059 commented Mar 23, 2016

please send me struts 2 base example with table score card

@madmenyo

This comment has been minimized.

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

This comment has been minimized.

KimTank commented Jun 4, 2018

Thank you 💃

@SammyVimes

This comment has been minimized.

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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment