Node<Plot>* search(std::map<Plot*, std::vector<Plot*> >& path, Plot& start, Plot& goal) { Node<Plot>* node_goal = new Node<Plot>(goal); Node<Plot>* node_start = new Node<Plot>(start); PriorityQueue queue; std::unordered_map<Plot*, int> closeList; queue.insert(node_start); int iter = 0; while (!queue.empty()) { iter++; Node<Plot> *current(*(queue.begin())); queue.erase(queue.begin()); std::vector<Plot*>& neighbours(path[current->node]); for (Plot *p : neighbours) { Node<Plot>* next = new Node<Plot>(*p); next->g = current->g + manhattan_distance(current->node, p); next->h = manhattan_distance(p, &goal); next->f = next->g + next->h; next->x = current->x + 1; // nb turns next->parent = current; if (p == node_goal->node) { std::cout << "nbIter: " << iter << std::endl; return next; } if (isWorthTrying(next, queue, closeList)) { queue.insert(next); } } closeList[current->node] = current->f; delete current; } return nullptr; }
bool isWorthTrying(Node<Plot>* node, PriorityQueue& pqueue, std::unordered_map<Plot*, int>& closeList) { auto found(pqueue.find(node)); if (found != pqueue.end()) { if ((*found)->f < node->f) return false; else { Node<Plot>* n(*found); pqueue.erase(found); delete n; } } auto cl_found(closeList.find(node->node)); if (cl_found != closeList.end()) { if (cl_found->second < node->f) return false; else closeList.erase(cl_found); } return true; }