/** * Connects the node. This will connect the node to the previous node along the path to @a target * and update the pathfinding information. * @param tuCost The total cost of the path so far. * @param prevNode The previous node along the path. * @param prevDir The direction FROM the previous node. * @param target The target position (used to update our guess cost). */ void PathfindingNode::connect(int tuCost, PathfindingNode* prevNode, int prevDir, const Position &target) { _tuCost = tuCost; _prevNode = prevNode; _prevDir = prevDir; if (!inOpenSet()) // Otherwise we have this already. { Position d = target - _pos; d *= d; _tuGuess = 4 * sqrt((double)d.x + d.y + d.z); } }
vector<D3DXVECTOR3> AStar::findPath(PathNode* _start, PathNode* _end) { //time it for debugging clock_t clockStart = clock(); //empty the closed set vector vector<PathNode*>().swap(mClosedSet); //empty the open set and put in only the start node vector<PathNode*>().swap(mOpenSet); mOpenSet.push_back(_start); //set all node gScores to max for (PathNode* PN : mPathNodes) PN->setGScore(INT_MAX); //set start node properties _start->setGScore(0); _start->setFScore(findHeuristic(_start, _end)); //while the open set is not empty while (mOpenSet.size() > 0) { //the node in open set having the lowest f score PathNode* current; findLowestFScore(current); //if we found the goal, return the path if (current == _end || ((clock() - clockStart) / (float)CLOCKS_PER_SEC) > MAX_PATH_TIME_LOW) { vector<D3DXVECTOR3> rePath; reconstructPath(rePath, _start, current); return rePath; } //save current to closed set mClosedSet.push_back(current); //remove current from open set for (vector<PathNode*>::iterator iter = mOpenSet.begin(); iter != mOpenSet.end(); ++iter) { if (*iter == current) { mOpenSet.erase(iter); break; } } //for each linked node in the current node for (PathNode* PN : current->getLinks()) { //if it is already in the closed set, continue if (inClosedSet(PN)) continue; //find tentative gScore int tempGScore = current->getGScore() + findHeuristic(current, _end); //if link node is not in open set or tempGScore < present GScore if (!inOpenSet(PN) || tempGScore < PN->getGScore()) { //set the came from of the link to the current node PN->setCameFrom(current); //set g score PN->setGScore(tempGScore); PN->setFScore(PN->getGScore() + findHeuristic(PN, _end)); //if link is not in open set, add to it if (!inOpenSet(PN)) mOpenSet.push_back(PN); } } } //finished loop without finding goal OutputDebugString(L"ERROR: No path found."); return vector<D3DXVECTOR3>(); }