Path finding function

Question

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
 std::cout << "Path not found\n";
}

Answer 1

// 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.

• Xavier Nodet: 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...