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