I once wrote this. It should work fairly well, but it is a primitive breadth-first search, so it will examine more squares than e.g. a good A* will do. It was good enough for my purpose though.
The "step" functions are used to make something follow the path. findPath() does the actual pathfinding.
You'll need a replacement for my Map class that defines the needed methods ... this code won't compile all by itself. Particularly you must implement the map.isMoveAllowed() method which checks if a move from one square to another is allowed; this is too project specific to be coded in a generic fashion
/*
* Path.java
*
* Created on 9. December 2008
*
* Copyright (c) Hansjoerg Malthaner
* hansjoerg.malthaner@gmx.de
*
* This file is available under the GPL v2 or newer.
* http://www.gnu.org/licenses/gpl-2.0.html
*/
import java.util.ArrayList;
import java.util.Stack;
/**
* A path of discrete steps from one location to another.
* This pathfinding implementation is not reentrant, so each thread
* needs it's own instance of a path for pathfinding.
*
* @author Hj. Malthaner
*/
public class Path {
/**
* One step of the path. The path as a whole is a linked list of these nodes.
* @author Hj. Malthaner
*/
public class Node {
public int x, y;
Node link;
Node(int x, int y) {
this.x = x;
this.y = y;
}
Node(int x, int y, Node link) {
this.x = x;
this.y = y;
this.link = link;
}
}
/** Result path of pathfinding */
private ArrayList path;
/** Current step in stepping through the path */
private int step;
/** Marks used in pathfinding. */
private boolean [] marks;
/** Look ahead on next step */
public Node nextStep()
{
return getStep(step+1);
}
/** Retrieve current step */
public Node currentStep()
{
return getStep(step);
}
/** Random access for a step */
public Node getStep(int n)
{
if(n >= 0 && n < path.size()) {
return (Node)path.get(n);
}
return null;
}
/** Advance current step by one. */
public void advance()
{
step ++;
}
/** Add a step to this path */
public void addStep(int x, int y)
{
path.add(new Node(x, y));
}
/** Reset path, remove all nodes.*/
public void clear()
{
path = new ArrayList();
step = 0;
}
/**
* Breadth first pathfinding.
*
* @param map the map to search
* @param sx Source x-coordinate
* @param sy Source y-coordinate
* @param dx Destination x-coordinate
* @param dy Destination y-coordinate
*
* @return true if path was found, false otherwise
*
* @author Hj. Malthaner
*/
public boolean findPath(Map map, int sx, int sy, int dx, int dy)
{
clear();
marks = new boolean[map.getWidth()*map.getHeight()];
ArrayList queue = new ArrayList();
queue.add(new Node(sx, sy));
marks[sy*map.getWidth() + sx] = true;
Node p;
do {
p = (Node)queue.remove(0);
if(p.x == dx && p.y == dy) {
// Hajo: destination found
// unwind path
Stack stack = new Stack();
while(p.link != null) {
stack.push(p);
p = p.link;
}
// Hajo: Push starting node
stack.push(p);
// Hajo: Setup path
while(stack.isEmpty() == false) {
p = (Node)stack.pop();
addStep(p.x, p.y);
// System.err.println("x=" + p.x + " y=" + p.y);
}
return true;
} else {
checkMove(map, queue, p, 1, 0);
checkMove(map, queue, p, 0, 1);
checkMove(map, queue, p, -1, 0);
checkMove(map, queue, p, 0, -1);
}
} while(queue.isEmpty() == false);
return false;
}
/** Recursive pathfinding in the 4 cardinal directions. */
private void checkMove(Map map, ArrayList queue, Node p, int w, int h)
{
final int dx = p.x+w;
final int dy = p.y+h;
if(map.isMoveAllowed(p.x, p.y, dx, dy) &&
marks[dy*map.getWidth() + dx] == false)
{
queue.add(new Node(dx, dy, p));
marks[dy*map.getWidth() + dx] = true;
}
}
/** Creates a new instance of Path */
public Path()
{
clear();
}
}
Edit: Fixed some typos.
Edit 2: Added more comments.
