Path finding function

Could this be made more efficient, and/or simpler? It's a path-finding function. It seems to work, but do you see any potential cases where it could fail?

// Searches for a path from [start] to [end].
// Predicate [passable] should take an std::pair<int,int> and return true if the node is passable.
// Nodes in path are stored in [out].
// Return value is a pair.  With first being a bool to indicate if a path was found,
// and second an iterator for the end of the path
template<typename OutputIterator, typename PassablePR>
std::pair<bool, OutputIterator> ShortestPath(std::pair<int,int> start, std::pair<int,int> end, PassablePR passable, OutputIterator out)
{
typedef std::pair<int,int> node;

std::pair<bool, OutputIterator> ret(false, out);

// queue of nodes expanding out from the starting point
std::queue<node> q;

// keep track of visited nodes so we don't visit them twice
std::vector<node> visited_nodes;
auto visited = [&visited_nodes] (node n) {
return std::find(visited_nodes.begin(), visited_nodes.end(), n) != visited_nodes.end();
};

// link child nodes to parents
std::map<node,node> path_tree;

q.push(start);
while(q.empty() == false)
{
auto parent = q.front();
if(passable(parent) && !visited(parent))
{
visited_nodes.push_back(parent);

if(parent == end)
{
ret.first = true;
std::vector<std::pair<int,int>> path;
auto i = path_tree.find(parent);
while(i != path_tree.end())
{
path.push_back(i->first);
parent = i->second;
path_tree.erase(i);
i = path_tree.find(parent);
}

// path is currently from end to start, so reverse it
std::copy(path.rbegin(), path.rend(), ret.second);

return ret;
}

auto child(parent);

// node to the left
--child.first;
q.push(child);
if(path_tree.find(child) == path_tree.end())
path_tree[child] = parent;

// right
child.first += 2;
q.push(child);
if(path_tree.find(child) == path_tree.end())
path_tree[child] = parent;

// above
--child.first;
--child.second;
q.push(child);
if(path_tree.find(child) == path_tree.end())
path_tree[child] = parent;

// and below
child.second += 2;
q.push(child);
if(path_tree.find(child) == path_tree.end())
path_tree[child] = parent;
}
q.pop();
}
return ret;
}


Testing it out:

int main()
{
int grid[5][5] =
{ { 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1, 0 },
{ 0, 1, 0, 1, 0 },
{ 0, 1, 0, 1, 0 },
{ 0, 0, 0, 1, 0 } };

std::vector<std::pair<int,int>> path;

auto ret = ShortestPath(std::pair<int,int>(0,0), std::pair<int,int>(4,4),
[&grid] (std::pair<int,int> n) -> bool {
if(ben::WithinBoundsInclusive(std::pair<int,int>(0,0), std::pair<int,int>(4,4), n) == false)
return false;
return grid[n.first][n.second] == 0;
},
std::back_inserter(path));

if(ret.first)
{
std::cout << "Path found\n";

std::for_each(path.begin(), path.end(),
[](std::pair<int,int> n) {
std::cout << '(' << n.first << ',' << n.second << ")\n";
});
}
else
}


// keep track of visited nodes so we don't visit them twice
std::vector<node> visited_nodes;
auto visited = [&visited_nodes] (node n) {
return std::find(visited_nodes.begin(), visited_nodes.end(), n) != visited_nodes.end();
};


Every time you have to check whether a point has been visited, this is going to scan over the entire list of visited_nodes. Better to do a std::set

// path is currently from end to start, so reverse it
std::copy(path.rbegin(), path.rend(), ret.second);


Rather then constructing the path and reversing it, you could construct the path backwards.

When it comes to constructing all of the child nodes, I think your approach is confusing. You'd be better of constructing a new node rather then reusing the node. As it is, its not clear what you are doing. That section of code is also repetitive. I'd do something like

int x_dir = {0, 0, 1, -1}
int y_dir = {1, -1, 0, 0}

for(int pos = 0; pos < 4; pos++)
{
node child = parent;
child.first += x_dir[pos];
child.second += y_dir[pos];
...
}


Other notes:

The use of map is going to O(log), if you use a multidimensional array to hold the information for each point you can do it in O(1) time.

Using pairs to hold co-ordinates is easy. However, I suggest that its better to have a point class with x and y members. I think it makes what is going on clearer. Additionally, the point class can encapsulate the neighboring cell logic.

• Iterating over a vector may be surprisingly quick compared to finding a node in a set, even if the former is O(n) and the latter is O(log n)... I'd rather test with realistic datasets... – Xavier Nodet Feb 12 '11 at 19:10