C++ CNode::GetCost方法代码示例

/*****************************************************************
* 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();
//.........这里部分代码省略.........