/**
* Add node to search queue
*
* @param n Destination node to add
* @param pn Previous node
* @param e Edge distance (previous to this)
*/
void Push(const Node &node, const Node &parent,
const AStarPriorityValue &edge_value) {
// Try to find the given node n in the node_value_map
node_value_iterator it = node_values.find(node);
if (it == node_values.end()) {
// first entry
// If the node wasn't found
// -> Insert a new node into the node_value_map
it = node_values.insert(std::make_pair(node, edge_value)).first;
// Remember the parent node
SetPredecessor(node, parent);
} else if (it->second > edge_value) {
// If the node was found and the new value is smaller
// -> Replace the value with the new one
it->second = edge_value;
// replace, it's bigger
// Remember the new parent node
SetPredecessor(node, parent);
} else
// If the node was found but the value is higher or equal
// -> Don't use this new leg
return;
q.push(NodeValue(edge_value, it));
}
/**
* Obtain the value of this node (accumulated distance to this node)
* Returns 0 on failure to find the node.
*/
gcc_pure
AStarPriorityValue GetNodeValue(const Node &node) const {
node_value_const_iterator it = node_values.find(node);
if (cur->first == node)
return cur->second;
if (it == node_values.end())
return AStarPriorityValue(0);
return it->second;
}
/** Clears the queues */
void Clear() {
// Clear the search queue
q.clear();
// Clear the node_parent_map
node_parents.clear();
// Clear the node_value_map
node_values.clear();
}
/** Clears the queues */
void Clear() {
// Clear the search queue
while (!q.empty())
q.pop();
// Clear the node_parent_map
node_parents.clear();
// Clear the node_value_map
node_values.clear();
}
