cpathfinder.cpp

<B>很多DirectX 9.0游戏编程源码例子</B>

#include "CPathFinder.h"

//-------------------------------------------------------------------
//
//							PATH NODE CLASS
//
//-------------------------------------------------------------------
// CONSTRUCTOR
CPathNode::CPathNode()
{
	vReset();
}

// Set values to defaults
void CPathNode::vReset( void )
{
	m_Parent = NULL;
	m_iDistFromStartCost = 0;
	m_iDistFromGoalCost = 0;
	m_iTotalCost = 0;
	m_iEnumerated = 0;
}

//-------------------------------------------------------------------
//
//						PATH FINDER CLASS
//
//-------------------------------------------------------------------
// CONSTRUCTOR
CPathFinder::CPathFinder()
{
	m_OpenList		= new CPathNode[PATHFINDER_MAX_NODES];
	m_ClosedList	= new CPathNode[PATHFINDER_MAX_NODES];
	m_CompletePath	= new CPath;
}

// DESTRUCTOR
CPathFinder::~CPathFinder()
{
	delete [] m_OpenList;
	delete [] m_ClosedList;
	delete m_CompletePath;
}

// Reset the path values
void CPathFinder::vResetPath( void )
{
	int iLoop;

	// Reset the nodes
	for( iLoop = 0; iLoop < PATHFINDER_MAX_NODES; iLoop++ ) {
		m_OpenList[iLoop].vReset();
		m_ClosedList[iLoop].vReset();
	}

	// Reset state
	m_iStartX = 0;
	m_iStartY = 0;
	m_iEndX = 0;
	m_iEndY = 0;
	m_iGoalFound = 0;
	m_iActiveOpenNodes = 0;
	m_iActiveClosedNodes = 0;

	// Set completed path #nodes to 0
	m_CompletePath->m_iNumNodes = 0;
}

//
// Set up the start and end positions.  This is required
// to prep the path for pathfinding.
//
void CPathFinder::vSetStartState( int iStartX, int iStartY, int iEndX, int iEndY )
{
	// Reset the path in-case one already in memory
	vResetPath();
	
	// Setup the start state
	m_iStartX = iStartX;
	m_iStartY = iStartY;
	m_iEndX = iEndX;
	m_iEndY = iEndY;
}

//
// Sets the map function pointer that tells the path finder
// how much a position on the map costs
//
void CPathFinder::vSetCostFunction( int (*ptr)(int,int) )
{
	iGetMapCost = ptr;
}

//
// Get the distance from the node to the goal
//
int CPathFinder::iGetGoalDistCost( int iX, int iY, int iCost )
{
	int iGoal = 0;

	iGoal += abs(m_iEndX - iX);
	iGoal += abs(m_iEndY - iY);

	iGoal *= iCost;

	return( iGoal );
}

//
// Get the distance from the node to the start position
//
int CPathFinder::iGetStartDistCost( int iParentStartCost, int iCost )
{
	int iStart = 0;

	iStart += iParentStartCost + iCost;

	return( iStart );
}

//
// Check to see if the node exists on the open
// path list.
//
bool CPathFinder::bDoesNotExistOnOpenList( int iX, int iY )
{
	int iLoop;
	
	for( iLoop = 0; iLoop < PATHFINDER_MAX_NODES; iLoop++ ) {
		if( m_OpenList[iLoop].m_iEnumerated == 1 ) {
			if( m_OpenList[iLoop].m_iX == iX && 
				m_OpenList[iLoop].m_iY == iY ) 
			{
				return( 1 );	
			}
		}
	}
	
	return( 0 );
}

//
// Check to see if the node exists on the closed
// path list.
//
bool CPathFinder::bDoesNotExistOnClosedList( int iX, int iY )
{
	int iLoop;
	
	for( iLoop = 0; iLoop < PATHFINDER_MAX_NODES; iLoop++ ) {
		if( m_ClosedList[iLoop].m_iEnumerated == 1 ) {
			if( m_ClosedList[iLoop].m_iX == iX && 
				m_ClosedList[iLoop].m_iY == iY ) 
			{
				return( 1 );	
			}
		}
	}

	return( 0 );
}

//
// Create a new node on the closed or open list or
// update a node on the open list
//
// 0 = Open List
// 1 = Closed List
//
int CPathFinder::iAddNodeToList( int iList, int iX, int iY, CPathNode *ParentNode, int iCost )
{
	bool	bExistsOnClosed;
	bool	bExistsOnOpen;
	int		iLoop;

	//
	// Add to the open list or update the open list
	//
	if( iList == 0 ) {
		// Node not blocked
		// Check to see if already on the closed list
		bExistsOnClosed = bDoesNotExistOnClosedList( iX, iY );
		
		// Node does not exist
		// Check to see if already on the closed list
		if( !bExistsOnClosed ) {
			// Node not blocked
			// Check to see if already on the open list
			bExistsOnOpen = bDoesNotExistOnOpenList( iX, iY );
			// Node does not exist
			// Add it to the open list
			if( !bExistsOnOpen ) {
				// Find a non-enumerated node on the open list
				for( iLoop = 0; iLoop < PATHFINDER_MAX_NODES; iLoop++ ) {
					// Check enumeration
					if( m_OpenList[iLoop].m_iEnumerated == 0 ) {
						
						// Set it to active
						m_OpenList[ iLoop ].m_iEnumerated = 1;
						// Set its position on the map
						m_OpenList[ iLoop ].m_iX = iX;
						m_OpenList[ iLoop ].m_iY = iY;
						// Set its distance to the goal cost
						m_OpenList[ iLoop ].m_iDistFromGoalCost = iGetGoalDistCost( iX, iY, iCost );
						// Set its distance from the start cost
						m_OpenList[ iLoop ].m_iDistFromStartCost = iGetStartDistCost( ParentNode->m_iDistFromStartCost, iCost );
						// Set its total cost
						m_OpenList[ iLoop ].m_iTotalCost = m_OpenList[ iLoop ].m_iDistFromGoalCost + m_OpenList[ iLoop ].m_iDistFromStartCost;
						// Set the parent node
						m_OpenList[ iLoop ].m_Parent = ParentNode;

						//
						// Check if goal is found
						//
						if( iX == m_iEndX && iY == m_iEndY ) {
							m_iGoalFound = 1;
							m_iGoalNode = iLoop;
						}

						// Tally open nodes
						m_iActiveOpenNodes++;

						// Done, break out
						return( iLoop );
					}
				}
				// Could not find free node, exit out
				return( -1 );
			}
			// Node already exists on the open list
			// Check to see if the cost from this parent would be cheaper
			// than what is set for the node already
			else {
				// Find the node
				for( iLoop = 0; iLoop < PATHFINDER_MAX_NODES; iLoop++ ) {
					if( m_OpenList[iLoop].m_iEnumerated == 1 ) {
						if( m_OpenList[iLoop].m_iX == iX && 
							m_OpenList[iLoop].m_iY == iY ) 
						{
							// Figure out distance from start cost for
							// the new node
							int iStartCost = iGetStartDistCost( ParentNode->m_iDistFromStartCost, iCost );

							// Check if the "old" node's distance from start cost is greater
							// than the new node.  If so, set the old node's parent to
							// be the parent node.

							if( m_OpenList[ iLoop ].m_iDistFromStartCost > iStartCost ) {

								// Set the parent
								m_OpenList[ iLoop ].m_Parent = ParentNode;
								// Recalculate start cost
								m_OpenList[ iLoop ].m_iDistFromStartCost = iGetStartDistCost( iStartCost, iCost );
								// Recalculate total cost
								m_OpenList[ iLoop ].m_iTotalCost = m_OpenList[ iLoop ].m_iDistFromGoalCost + m_OpenList[ iLoop ].m_iDistFromStartCost;
							}

							// Done, break out
							return( iLoop );
						}
					}
				}
			}
		}
		// Node exists on closed list already
		// so ignore it
		else {
			return( 0 );
		}
	}
	//
	// Transfer the node to the closed list
	//
	else {
		// Remove it from the open list
		ParentNode->m_iEnumerated = 0;
		// Find a non-enumerated node on the closed list
		for( iLoop = 0; iLoop < PATHFINDER_MAX_NODES; iLoop++ ) {
			// Check enumeration
			if( m_ClosedList[iLoop].m_iEnumerated == 0 ) {
				// Set it to active
				m_ClosedList[ iLoop ].m_iEnumerated = 1;
				// Copy the attributes over
				m_ClosedList[ iLoop ].m_iDistFromGoalCost = ParentNode->m_iDistFromGoalCost;
				m_ClosedList[ iLoop ].m_iDistFromStartCost = ParentNode->m_iDistFromStartCost;
				m_ClosedList[ iLoop ].m_iTotalCost = m_ClosedList[ iLoop ].m_iDistFromGoalCost + m_ClosedList[ iLoop ].m_iDistFromStartCost;
				m_ClosedList[ iLoop ].m_iX = ParentNode->m_iX;
				m_ClosedList[ iLoop ].m_iY = ParentNode->m_iY;
				m_ClosedList[ iLoop ].m_Parent = ParentNode->m_Parent;
				
				// Tally open and closed nodes
				m_iActiveOpenNodes--;
				m_iActiveClosedNodes++;

				// Done, break out
				return( iLoop );
			}
		}
		// Could not find free node, exit out
		return( -1 );
	}

	// Invalid request type, error out
	return( -1 );
}

//
// Find all open nodes around the given node and add them
// to the open node list.
//
bool CPathFinder::bOpenNodes( CPathNode *node )
{
	int		iCost;
	bool	bFoundOpenNode = 0;
	int		iRet;

	// Check UP
	if( iCost = (iGetMapCost(	node->m_iX, 
								node->m_iY-1 ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX, node->m_iY-1, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	// Check RIGHT
	if( iCost = (iGetMapCost(	node->m_iX+1, 
								node->m_iY ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX+1, node->m_iY, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	// Check DOWN
	if( iCost = (iGetMapCost(	node->m_iX, 
								node->m_iY+1 ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX, node->m_iY+1, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	// Check LEFT
	if( iCost = (iGetMapCost(	node->m_iX-1, 
								node->m_iY ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX-1, node->m_iY, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	// DIAGONALS

	// Check UP-RIGHT
	if( iCost = (iGetMapCost(	node->m_iX+1, 
								node->m_iY-1 ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX+1, node->m_iY-1, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	// Check DOWN-RIGHT
	if( iCost = (iGetMapCost(	node->m_iX+1, 
								node->m_iY+1 ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX+1, node->m_iY+1, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	// Check DOWN-LEFT
	if( iCost = (iGetMapCost(	node->m_iX-1, 
								node->m_iY+1 ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX-1, node->m_iY+1, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	// Check UP-LEFT
	if( iCost = (iGetMapCost(	node->m_iX-1, 
								node->m_iY+1 ) ) != -1 ) {
		// Add the node to the open list
		iRet = iAddNodeToList( 0, node->m_iX-1, node->m_iY+1, node, iCost );
		// Check for failure, return out if fail
		if( iRet == -1 ) {
			return( 0 );
		}
		bFoundOpenNode = 1;
	}

	return( bFoundOpenNode );
}

//
// Look through the path and find the cheapest node in the closed list
//
int CPathFinder::iFindCheapestNode( void )
{
	int iLoop;
	int iCheapestCost = 9999999;
	int iCheapestNode = -1;
	int iNewNode = -1;
	
	//
	// Loop through every node in the open list
	//
	for( iLoop = 0; iLoop < PATHFINDER_MAX_NODES; iLoop++ ) {
		
		// Check node's cost if it exists
		if( m_OpenList[iLoop].m_iEnumerated == 1 ) {
			
			// Check if node cost is less than current
			// lowest
			if( m_OpenList[iLoop].m_iTotalCost < iCheapestCost ) {
				iCheapestNode = iLoop;
				iCheapestCost = m_OpenList[iLoop].m_iTotalCost;
			}
		}
	}

	// Check if a cheapest node was found
	if( iCheapestNode != -1 ) {
		// Transfer the cheapest node to the closed list
		iNewNode = iAddNodeToList( 1, 0, 0, &m_OpenList[ iCheapestNode ] );
	}
	
	return( iNewNode );
}

//
// Calculate the best path from point A to point B
//
bool CPathFinder::bFindPath( int iMaxNodes )
{
	CPathNode	*GoalNode;
	int			iCurNode = 0;
	
	// Put start node in closed list
	m_ClosedList[ 0 ].m_iEnumerated = 1;
	// Set position
	m_ClosedList[ 0 ].m_iX = m_iStartX;
	m_ClosedList[ 0 ].m_iY = m_iStartY;
	// Set the Goal
	m_ClosedList[ 0 ].m_iDistFromGoalCost = iGetGoalDistCost( m_iStartX, m_iStartY, 0 );
	// Set the total cost of this node
	m_ClosedList[ 0 ].m_iTotalCost = m_ClosedList[ 0 ].m_iDistFromGoalCost;

	// Find the path
	for( ;; ) {

		// Open nodes around the start node
		bOpenNodes( &m_ClosedList[ iCurNode ] );

		// Check if goal state found
		if( m_iGoalFound ) {
			//
			// Store the completed path
			//
			int iTotalNodes = 0;

			//
			// Count total # of nodes
			//
			GoalNode = &m_OpenList[m_iGoalNode];

			while( GoalNode->m_Parent != NULL ) {
				GoalNode = GoalNode->m_Parent;
				iTotalNodes++;
			};
			// Account for the last node
			iTotalNodes++;

			// Store # of nodes
			m_CompletePath->m_iNumNodes = iTotalNodes;

			//
			// Invert the path and store it
			//
			GoalNode = &m_OpenList[m_iGoalNode];

			while( iTotalNodes > 0 ) {
				iTotalNodes--;
				m_CompletePath->m_Path[iTotalNodes] = GoalNode;
				GoalNode = GoalNode->m_Parent;
			};

			return( 1 );
		}
		// Find cheapest open node and add it to the
		// closed list
		iCurNode = iFindCheapestNode();

		// Check to see if no solution is possible
		if( iCurNode == -1 ) {
			return( 0 );
		}

		// Break out if max # of closed nodes reached
		if( m_iActiveClosedNodes >= iMaxNodes ) {
			return( 0 );
		}
	}

	return( 1 );
}