/** * 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; }