NodePath TreeSearcher::FindPath(Node* src, Node* dest) { src = src ? src : this->src; dest = dest ? dest : this->dest; std::deque<Node *> srcQueue; std::stack<Node *> destStack; NodePath path; Node *currentNode = src; while (currentNode) { srcQueue.push_back(currentNode); currentNode = currentNode -> Parent(); } currentNode = dest; while(currentNode) { destStack.push(currentNode); currentNode = currentNode -> Parent(); } Node* common; while(srcQueue.size() > 1 && destStack.size() > 1 && srcQueue.back() == destStack.top()) { common = destStack.top(); srcQueue.pop_back(); destStack.pop(); } if (srcQueue.back() == destStack.top()) { common = destStack.top(); if (srcQueue.size() > 1) srcQueue.pop_back(); else if (destStack.size() > 1) destStack.pop(); } for(Node *node : srcQueue) { path.AddPathNode(node); } if (common != src && common != dest) path.AddPathNode(common); while (!destStack.empty()) { path.AddPathNode(destStack.top()); destStack.pop(); } return path; }