Graph path finding

Jan 1, 0001 programming

A poor mans path finding algorithm. Given a start node, find any path that leads to the target node. I use this piece of code to generate a route from a given planet/star system to another one. The stars are guaranteed to be connected.

Driver:

public List<StarSystem> FindPath(StarSystem source, StarSystem target)
{
	var path = new List<StarSystem>();
	Find(source, target, new List<StarSystem>(), path, new HashSet<StarSystem>());
	return path;
}
private void Find(StarSystem source, StarSystem target, List<StarSystem> path, List<StarSystem> validPath, HashSet<StarSystem> visited)
{
	visited.Add(source);
	if (source == target) validPath.AddRange(path);
	foreach (var neighbor in source.WarpLanes)
	{
		if (visited.Contains(neighbor)) continue;
		path.Add(neighbor);
		Find(neighbor, target, path, validPath, visited);
		path.Remove(neighbor);
	}
	visited.Remove(source);
}

It’s also possible to implement this iteratively. The following implements Lee’s Algorithm (which does find the optimal solution but is rather inefficient)

func find_path(map, src, target):
	var queue = []
	var visited = []
	var expansion = []
	for i in range(map.size()):
		visited.append([])
		expansion.append([])
		for j in range(map[0].size()):
			visited[i].append(false)
			expansion[i].append(0)
			if map[i][j] != 0:
				visited[i][j] = true
			else:
				visited[i][j] = false
	var marker = 1
	queue.push_back({'node': src, 'dist': marker})
	expansion[src.y][src.x] = marker
	visited[src.y][src.x] = true
	while queue.size() > 0:
		var cur = queue.pop_front()
		if cur['node'] == target: break

		var neighbors = get_neighbors(map, cur['node'])
		for adj in neighbors:			
			if visited[adj.y][adj.x]: continue
			visited[adj.y][adj.x] = true
			expansion[adj.y][adj.x] = cur['dist'] + 1
			queue.push_back({'node': adj, 'dist': cur['dist'] + 1})
	var path = [target]
	var cur = target
	marker = expansion[target.y][target.x]
	if marker == 0:
		return []
	while cur != src:
		var neighbors = get_neighbors(expansion, cur)
		for adj in neighbors:
			if expansion[adj.y][adj.x] == marker - 1:
				marker -= 1
				path.push_front(adj)
				cur = adj
				break
	return path

Lee’s algorithm works by expanding a “wave” from the source cells to all the cells in the grid. Assuming s = start, d = destination, 0 = walkable, * = obstacle and movement is permitted in the cardinal directions only:

s 0 0 0 0
* 0 * 0 0
0 0 * 0 0
0 0 0 d 0
0 * 0 0 0

A BFS is done and each neighbors distance from the start cell is recorded.

s 1 0 0 0
* 0 * 0 0
0 0 * 0 0
0 0 0 d 0
0 * 0 0 0

only 1 neighbor cell is walkable

after all the cells have been visited the expansion matrix is like this:

s 1 2 3 4
* 2 * 4 5
4 3 * 5 6
5 4 5 d 7
6 * 6 7 8

The path is found by starting at the target and walking backwards towards the neighbor with the smaller number.