/***************************************************************** * GeneratePath() Uses A* PathFinding to generate a path from * the start position to the goal position * * Ins: int * int * * Outs: None * * Returns: vector<CNode*> * * Mod. Date: 08/16/2015 * Mod. Initials: NS *****************************************************************/ vector<XMFLOAT3> CPathFinding::GeneratePath(int _start, int _goal, bool smoothed) { GetReadyForNewSearch(); // Create the first PathNode CPathNode* curPathNode = new CPathNode(m_pGraph->GetNodes()[_start]); m_pqNodeQue.push(curPathNode); m_umVisitedNodes[_start] = curPathNode; // Create all local variables here for attempt at optimization CPathNode* theNode = nullptr; CPathNode* visitedNode = nullptr; CPathNode* nextPathNode = nullptr; CNode* curNode; CNode* adjNode; CEdge* curEdge; float newFinalCost; float newCost; float oldCost; float curCost; float newHeuristic; int adjNodeIndex; int curNodeIndex; vector<XMFLOAT3> path; // Loop while there are edges to explore while (!m_pqNodeQue.empty()) { // Get the next node. curPathNode = m_pqNodeQue.front(); m_pqNodeQue.pop(); // Access the node and it's index for future use curNode = curPathNode->GetNode(); curNodeIndex = curNode->GetIndex(); // If the destination is found if (curNodeIndex == _goal) { // Clear the old path m_lPath.clear(); // Build the path theNode = m_umVisitedNodes[curNode->GetIndex()]; // Untill we trace back to the _start node while (theNode != nullptr) { // Add the node to the path m_lPath.push_back(theNode->GetNode()); // Advance to the next connected node theNode = theNode->m_cpParent; } if (smoothed) path = SmoothPath(m_lPath); else { for (size_t currNode = 0; currNode < m_lPath.size(); currNode++) path.push_back(m_lPath[currNode]->GetPosition()); } /*m_umVisitedNodes.clear(); ^^^^^^^^^^^^^^^^^^^^^^^^^^ This stupid line of code caused us so many headaches that I can't bring myself to actually delete it. Forever it shall live in the belly of our pathfinding code*/ return path; } // For each edge the node has for (size_t edge = 0; edge < curNode->GetEdges().size(); edge++) { // Get the current Edge and the node attatched to it curEdge = curNode->GetEdges()[edge]; adjNodeIndex = curEdge->GetAdjNode(); adjNode = m_pGraph->GetNodes()[adjNodeIndex]; // Calculate the cost to the adj Node from _start Node (Total cost so far) curCost = curNode->GetCost() + curEdge->GetEdgeCost(); // If the node has been visited if (m_umVisitedNodes[adjNodeIndex] != nullptr) { visitedNode = m_umVisitedNodes[adjNodeIndex]; // The cost it took previously to get to the node oldCost = visitedNode->GetNode()->GetCost(); // If the new cost is less the the old one if (curCost < oldCost) { m_pqNodeQue.remove(visitedNode); visitedNode->GetNode()->SetCost(curCost); newFinalCost = visitedNode->GetNode()->GetCost() + visitedNode->GetNode()->GetHeuristic() * m_fHeuristicWeight; visitedNode->GetNode()->SetFinalCost(newFinalCost); visitedNode->m_cpParent = curPathNode; m_pqNodeQue.push(visitedNode); } } else // If it has not been visited { // Set the cost adj notes cost (Total of what it takes to get here) newCost = curNode->GetCost() + curEdge->GetEdgeCost(); adjNode->SetCost(newCost); // Set the HeuristicCost newHeuristic = CalculateCost(m_pGraph->GetNodes()[_goal]->GetPosition(), adjNode->GetPosition()); adjNode->SetHeuristic(newHeuristic); // Set the new FinalCost // ?? newFinalCost = newCost + newHeuristic * m_fHeuristicWeight; adjNode->SetFinalCost(newFinalCost); // Create a PathNode for the adj node (Note adj to the one I'm on) nextPathNode = new CPathNode(adjNode); // Set it's parent to the one I'm on (Coming from) nextPathNode->m_cpParent = curPathNode; // Add it to the que to be checked. m_pqNodeQue.push(nextPathNode); // Mark it as visited m_umVisitedNodes[adjNodeIndex] = nextPathNode; } } } return path; }