If you are using C++, why don't you just use std::priority_queue? Here is how you might initialize and use it:
node temp;std::priority_queue< node, std::vector< node >, std::greater< node > > node_queue;temp = node_queue.top();node_queue.pop();node_queue.push(temp);
You can use any of the comparison operators, but std::greater and std::less are the only ones that would probably make sense.
For searches like A*, I create a node called informed_node that looks a bit like this:
struct informed_node{ node* ptr_node; int cost; int current_cost; int projected_cost; void clone(node& n) { ptr_node = &n cost = 0; current_cost = 0; project_cost = 0; } const informed_node& operator=(cosnt node& n) { clone(n); return *this; } bool operator>(const informed_node& n) { return cost > n.cost; informed_node(const node& n) { clone(n); }};
Then I define my priority queue like this:
std::priority_queue< informed_node, std::vector< informed_node >, std::greater< informed_node > >
When you push regular "node" objects into the priority queue, it creates an informed_node and calls the copy constructor. Why do this? Well, so you can skip having a closed list of nodes. You will have to keep some internal information in the node pointed to by ptr_node. Basically, just the current cost and whether or not it has been visited. You will also need to keep a list of alterned nodes, so you can change the current cost and the visited status back to their defaults. Also, whenever you come to a previously searched node, you can actually change the current cost of EVERY SINGLE NODE pointed to by ptr_node in the informed_nodes.
Doing this removes the need for a closed_list (because you can use the node pointed to by ptr_node to get the current cost and visited status), it also changes having to set all nodes in the search space to default values in the beginning to having to set only searched nodes to default values at the end. This will speed up your searches quite a bit.
Here, I'll just show you mine:
template <class Object, class Cost>int search<Object, Cost>::a_star(node<Object, Cost>& start_node, node<Object, Cost>& goal_node, CostFunc cost_func){ int result; m_nodes_searched = 0; result = a_star_search(&start_node, &goal_node, cost_func); if(result) { this->store_goal(&start_node, m_goal); } this->revert_altered_nodes(); return result;}template <class Object, class Cost>int search<Object, Cost>::a_star_search(node<Object, Cost>* start_node, node<Object, Cost>* goal_node, CostFunc cost_func){ std::priority_queue< informed_node<Object, Cost>, std::vector< informed_node<Object, Cost> >, std::greater< informed_node<Object, Cost> > > node_queue; std::list< node<Object, Cost>* >::const_iterator itr; informed_node<Object, Cost> child; informed_node<Object, Cost> node; Cost current_cost; start_node->m_visited = true; start_node->m_cost = Cost(); m_altered_nodes.push(start_node); node_queue.push(*start_node); while(!node_queue.empty()) { node = node_queue.top(); node_queue.pop(); if(node.m_node->m_object == goal_node->m_object) { m_goal = node.m_node; return 1; } itr = node.m_node->get_connections().begin(); m_nodes_searched++; while(itr != node.m_node->get_connections().end()) { child.m_node = (*itr); if(child.m_node->m_parent != node.m_node) { if(!child.m_node->m_visited) { child.m_current_cost = node.m_current_cost + cost_func(*node.m_node, *child.m_node); child.m_projected_cost = cost_func(*child.m_node, *goal_node); child.m_cost = child.m_current_cost + child.m_projected_cost; child.m_node->m_parent = node.m_node; child.m_node->m_visited = true; child.m_node->m_cost = child.m_current_cost; node_queue.push(child); m_altered_nodes.push(child.m_node); } else { current_cost = node.m_current_cost + cost_func(*node.m_node, *child.m_node); if(current_cost < child.m_node->m_cost) { child.m_current_cost = current_cost; child.m_projected_cost = cost_func(*child.m_node, *goal_node); child.m_cost = child.m_current_cost + child.m_projected_cost; child.m_node->m_parent = node.m_node; child.m_node->m_cost = current_cost; node_queue.push(child); } } } itr++; } } return 0;}