Daniel Soltyka | Game Programmer

//==========================================================================//
// Ogre A* Example - December 2009                                          //
//                                                                          //
// Author: Daniel Soltyka                                                   //
// E-Mail: dsoltyka [at] gmail [dot] com                                    //
//                                                                          //
// This example demonstrates the A* pathfinding algorithm in a 3D           //
// environment.                                                             //
//                                                                          //
// Modified A* routines based off of A* Algorithm Implementation using STL  //
// by Justin Heyes-Jones.                                                   //
//                                                                          //
// Original Routines can be found at http://www.heyes-jones.com/astar.html  //
//==========================================================================//

#include "PathFinder.h"

using namespace std;

PathFinder::PathFinder(WorldMap* map):mMap(map)
{
	// seed random number gen
	srand(time(0));

	mStartNode = MapSearchNode(mMap);
	mEndNode = MapSearchNode(mMap);
	mSearchCount = 0;

	// set start and end nodes
 	SetStartEndNodes();
}

PathFinder::PathFinder(WorldMap *map, Ogre::Vector3 *startingPoint) : mMap(map)
{
	// seed random number gen
	srand(time(0));

	mStartNode = MapSearchNode(mMap);
	mEndNode = MapSearchNode(mMap);
	mSearchCount = 0;

	SetStartEndNodes(startingPoint);
}

PathFinder::~PathFinder()
{
	// Once you're done with the solution you can free the nodes up
	mAStarSearch.FreeSolutionNodes();

	// free memory
	mAStarSearch.EnsureMemoryFreed();
}

// Run the path finder
void PathFinder::Run()
{
	// set flag
	mFoundSolution = false;

	// try new paths until we have a solution
	do
	{
		// try and find a path
		FindPath();

		// if we haven't found a solution, we will reset everything
		// before the loop tries again
		if (!mFoundSolution)
		{
			// create a new end point and reset
			GetNewEndNode();
			ResetSearch();
		}
	} while (!mFoundSolution);
}

// Convert nodes into Ogre::Vector3 objects and fill
// the deque passed to this function with those Vectors
void PathFinder::GetResults(deque* waypoints)
{
	MapSearchNode* node = mAStarSearch.GetSolutionStart();

	Ogre::Vector3 waypointStart = Ogre::Vector3((Ogre::Real)(node->x * 64.0f), 0.0f, (Ogre::Real)(node->y * 64.0f));
	waypoints->push_back(waypointStart);

	int steps = 0;
	for( ;; )
	{
		node = mAStarSearch.GetSolutionNext();

		if( !node )
		{
			break;
		}
		else
		{
			Ogre::Vector3 waypoint = Ogre::Vector3((Ogre::Real)(node->x * 64.0f), 0.0f, (Ogre::Real)(node->y * 64.0f));
			waypoints->push_back(waypoint);
		}
		steps ++;
	};
}

void PathFinder::FindPath()
{
	while(mSearchCount < NUM_SEARCHES)
	{
		// Set Start and goal states
		mAStarSearch.SetStartAndGoalStates( mStartNode, mEndNode );

		unsigned int SearchState;
		unsigned int SearchSteps = 0;

		// step through our path and find a solution
		do
		{
			SearchState = mAStarSearch.SearchStep();
			SearchSteps++;
		}
		while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING );

		if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED )
		{
			// search succeeded
			mFoundSolution = true;

		}
		else if( SearchState == AStarSearch::SEARCH_STATE_FAILED )
		{
			// search failed
			mFoundSolution = false;
		}
		mSearchCount++;
	}
}

// Generate random start and end nodes
void PathFinder::SetStartEndNodes()
{
	// define start state
	do
	{
		mStartNode = MapSearchNode(mMap);
		mStartNode.x = rand()%mMap->GetWidth();
		mStartNode.y = rand()%mMap->GetHeight();
	} while( mMap->GetTile(mStartNode.x, mStartNode.y) == 9);

	// define the goal state
	do
	{
		mEndNode = MapSearchNode(mMap);
		mEndNode.x = rand()%mMap->GetWidth();
		mEndNode.y = rand()%mMap->GetHeight();
	} while( (mMap->GetTile(mEndNode.x, mEndNode.y) == 9) || (mStartNode == mEndNode) );
}

// Generate a random end node, using a known vector as a starting location
void PathFinder::SetStartEndNodes(Ogre::Vector3* startingPoint)
{

	mStartNode.x = (int)(startingPoint->x / 64);
	mStartNode.y = (int)(startingPoint->z / 64);

	// define the goal state
	do
	{
		mEndNode.x = rand()%mMap->GetWidth();
		mEndNode.y = rand()%mMap->GetHeight();
	} while( (mMap->GetTile(mEndNode.x, mEndNode.y) == 9) || (mStartNode == mEndNode) );
}

// Generate a new random end node
void PathFinder::GetNewEndNode()
{
	// define the goal state
	do
	{
		mEndNode.x = rand()%mMap->GetWidth();
		mEndNode.y = rand()%mMap->GetHeight();
	} while( (mMap->GetTile(mEndNode.x, mEndNode.y) == 9) || (mStartNode == mEndNode) );
}

// Reset the search
void PathFinder::ResetSearch()
{
	mSearchCount = 0;
	mAStarSearch.FreeSolutionNodes();
}