I would recommend an alternative approach: the rapidly exploring random tree (RRT). One cool thing about it is you can get it to go around corners, or explode in all directions.
The algorithm is really basic:
// Returns a random tree containing the start and the goal.
// Grows the tree for a maximum number of iterations.
Tree RRT(Node start, Node goal, int maxIters)
{
// Initialize a tree with a root as the start node.
Tree t = new Tree();
t.Root = start;
bool reachedGoal = false;
int iter = 0;
// Keep growing the tree until it contains the goal and we've
// grown for the required number of iterations.
while (!reachedGoal || iter < maxIters)
{
// Get a random node somewhere near the goal
Node random = RandomSample(goal);
// Get the closest node in the tree to the sample.
Node closest = t.GetClosestNode(random);
// Create a new node between the closest node and the sample.
Node extension = ExtendToward(closest, random);
// If we managed to create a new node, add it to the tree.
if (extension)
{
closest.AddChild(extension);
// If we haven't yet reached the goal, and the new node
// is very near the goal, add the goal to the tree.
if(!reachedGoal && extension.IsNear(goal))
{
extension.AddChild(goal);
reachedGoal = true;
}
}
iter++;
}
return t;
}
By modifying the RandomSample
and ExtendToward
functions, you can get very different trees. If RandomSample
just uniformly samples everywhere, the tree will grow uniformly in all directions. If its biased toward the goal, the tree will tend to grow towards the goal. If it always samples the goal, the tree will be a straight line from the start to the goal.
ExtendToward
can allow you to do interesting things to the tree as well. For one thing, if you have obstacles (such as walls), you can get the tree to grow around them simply by rejecting extensions that collide with walls.
This is what it looks like when you don't bias the sampling toward the goal:

(source: uiuc.edu)
Some cool properties of the RRT once its finished:
- The RRT will never cross itself
- The RRT will eventually cover the entire space with smaller and smaller branches
- The path from the start to the goal can be completely random and weird.