pathfinder.cpp

来自「这是整套横扫千军3D版游戏的源码」· C++ 代码 · 共 648 行 · 第 1/2 页

#include "StdAfx.h"
#include "PathFinder.h"
#include "Sim/MoveTypes/MoveMath/MoveMath.h"
#include "Map/ReadMap.h"
#include "LogOutput.h"
#include "Rendering/GL/glExtra.h"
#include <ostream>
#include "Sim/MoveTypes/MoveInfo.h"
#include "Map/Ground.h"
#include "mmgr.h"

#define PATHDEBUG false


//Option constants.
const unsigned int PATHOPT_RIGHT = 1;		//-x
const unsigned int PATHOPT_LEFT = 2;		//+x
const unsigned int PATHOPT_UP = 4;			//+z
const unsigned int PATHOPT_DOWN = 8;		//-z
const unsigned int PATHOPT_DIRECTION = (PATHOPT_RIGHT | PATHOPT_LEFT | PATHOPT_UP | PATHOPT_DOWN);
const unsigned int PATHOPT_START = 16;
const unsigned int PATHOPT_OPEN = 32;
const unsigned int PATHOPT_CLOSED = 64;
const unsigned int PATHOPT_FORBIDDEN = 128;
const unsigned int PATHOPT_BLOCKED = 256;

//Cost constants.
const float PATHCOST_INFINITY = 10000000;

void* pfAlloc(size_t n)
{
	char* ret=SAFE_NEW char[n];
	for(int a=0;a<n;++a)
		ret[a]=0;

	return ret;
}

void pfDealloc(void *p,size_t n)
{
	delete[] ((char*)p);
}

/**
Constructor.
Building tables and precalculating data.
*/
CPathFinder::CPathFinder()
: openSquareBufferPointer(openSquareBuffer)
{
	//Creates and init all square states.
	squareState = SAFE_NEW SquareState[gs->mapSquares];
	for(int a = 0; a < gs->mapSquares; ++a){
		squareState[a].status = 0;
		squareState[a].cost = PATHCOST_INFINITY;
	}
	for(int a=0;a<MAX_SEARCHED_SQUARES;++a){
		openSquareBuffer[a].cost=0;
		openSquareBuffer[a].currentCost=0;
		openSquareBuffer[a].sqr=0;
		openSquareBuffer[a].square.x=0;
		openSquareBuffer[a].square.y=0;
	}

/*	//Create border-constraints all around the map.
	//Need to be 2 squares thick.
	for(int x = 0; x < gs->mapx; ++x) {
		for(int y = 0; y < 2; ++y)
			squareState[y*gs->mapx+x].status |= PATHOPT_FORBIDDEN;
		for(int y = gs->mapy-2; y < gs->mapy; ++y)
			squareState[y*gs->mapx+x].status |= PATHOPT_FORBIDDEN;
	}
	for(int y = 0; y < gs->mapy; ++y){
		for(int x = 0; x < 2; ++x)
			squareState[y*gs->mapx+x].status |= PATHOPT_FORBIDDEN;
		for(int x = gs->mapx-2; x < gs->mapx; ++x)
			squareState[y*gs->mapx+x].status |= PATHOPT_FORBIDDEN;
	}
*/
	//Precalculated vectors.
	directionVector[PATHOPT_RIGHT].x = -2;
	directionVector[PATHOPT_LEFT].x = 2;
	directionVector[PATHOPT_UP].x = 0;
	directionVector[PATHOPT_DOWN].x = 0;
	directionVector[(PATHOPT_RIGHT | PATHOPT_UP)].x = directionVector[PATHOPT_RIGHT].x + directionVector[PATHOPT_UP].x;
	directionVector[(PATHOPT_LEFT | PATHOPT_UP)].x = directionVector[PATHOPT_LEFT].x + directionVector[PATHOPT_UP].x;
	directionVector[(PATHOPT_RIGHT | PATHOPT_DOWN)].x = directionVector[PATHOPT_RIGHT].x + directionVector[PATHOPT_DOWN].x;
	directionVector[(PATHOPT_LEFT | PATHOPT_DOWN)].x = directionVector[PATHOPT_LEFT].x + directionVector[PATHOPT_DOWN].x;

	directionVector[PATHOPT_RIGHT].y = 0;
	directionVector[PATHOPT_LEFT].y = 0;
	directionVector[PATHOPT_UP].y = 2;
	directionVector[PATHOPT_DOWN].y = -2;
	directionVector[(PATHOPT_RIGHT | PATHOPT_UP)].y = directionVector[PATHOPT_RIGHT].y + directionVector[PATHOPT_UP].y;
	directionVector[(PATHOPT_LEFT | PATHOPT_UP)].y = directionVector[PATHOPT_LEFT].y + directionVector[PATHOPT_UP].y;
	directionVector[(PATHOPT_RIGHT | PATHOPT_DOWN)].y = directionVector[PATHOPT_RIGHT].y + directionVector[PATHOPT_DOWN].y;
	directionVector[(PATHOPT_LEFT | PATHOPT_DOWN)].y = directionVector[PATHOPT_LEFT].y + directionVector[PATHOPT_DOWN].y;

	moveCost[PATHOPT_RIGHT] = 1;
	moveCost[PATHOPT_LEFT] = 1;
	moveCost[PATHOPT_UP] = 1;
	moveCost[PATHOPT_DOWN] = 1;
	moveCost[(PATHOPT_RIGHT | PATHOPT_UP)] = 1.42f;
	moveCost[(PATHOPT_LEFT | PATHOPT_UP)] = 1.42f;
	moveCost[(PATHOPT_RIGHT | PATHOPT_DOWN)] = 1.42f;
	moveCost[(PATHOPT_LEFT | PATHOPT_DOWN)] = 1.42f;
}


/**
Destructor.
Free used memory.
*/
CPathFinder::~CPathFinder()
{
	ResetSearch();
	delete[] squareState;
}


/**
Search with several start positions
*/
IPath::SearchResult CPathFinder::GetPath(const MoveData& moveData, const std::vector<float3>& startPos, const CPathFinderDef& pfDef, Path& path) {
	//Clear the given path.
	path.path.clear();
	path.pathCost = PATHCOST_INFINITY;

	//Store som basic data.
	maxNodesToBeSearched = MAX_SEARCHED_SQUARES;
	testMobile=false;
	exactPath = true;
	needPath=true;

	//If exact path is reqired and the goal is blocked, then no search is needed.
	if(exactPath && pfDef.GoalIsBlocked(moveData, (CMoveMath::BLOCK_STRUCTURE | CMoveMath::BLOCK_TERRAIN)))
		return CantGetCloser;

	//If the starting position is a goal position, then no search need to be performed.
	if(pfDef.IsGoal(startxSqr, startzSqr))
		return CantGetCloser;

	//Clearing the system from last search.
	ResetSearch();

	openSquareBufferPointer = &openSquareBuffer[0];

	for(std::vector<float3>::const_iterator si=startPos.begin();si!=startPos.end();++si){
		start = *si;
		startxSqr = (int(start.x) / SQUARE_SIZE)|1;
		startzSqr = (int(start.z) / SQUARE_SIZE)|1;
		startSquare = startxSqr + startzSqr * gs->mapx;

		squareState[startSquare].status = (PATHOPT_START | PATHOPT_OPEN);
		squareState[startSquare].cost = 0;
		dirtySquares.push_back(startSquare);

		goalSquare = startSquare;

		OpenSquare *os = ++openSquareBufferPointer;	//Taking first OpenSquare in buffer.
		os->currentCost = 0;
		os->cost = 0;
		os->square.x = startxSqr;
		os->square.y = startzSqr;
		os->sqr = startSquare;
		openSquares.push(os);
	}

	//Performs the search.
	SearchResult result = DoSearch(moveData, pfDef);

	//Respond to the success of the search.
	if(result == Ok) {
		FinishSearch(moveData, path);
		if(PATHDEBUG) {
			logOutput << "Path found.\n";
			logOutput << "Nodes tested: " << (int)testedNodes << "\n";
			logOutput << "Open squares: " << (float)(openSquareBufferPointer - openSquareBuffer) << "\n";
			logOutput << "Path steps: " << (int)(path.path.size()) << "\n";
			logOutput << "Path cost: " << path.pathCost << "\n";
		}
	} else {
		if(PATHDEBUG) {
			logOutput << "Path not found!\n";
			logOutput << "Nodes tested: " << (int)testedNodes << "\n";
			logOutput << "Open squares: " << (float)(openSquareBufferPointer - openSquareBuffer) << "\n";
		}
	}
	return result;
}

/**
Store som data and doing some basic top-administration.
*/
IPath::SearchResult CPathFinder::GetPath(const MoveData& moveData, const float3 startPos, const CPathFinderDef& pfDef, Path& path, bool testMobile, bool exactPath, unsigned int maxNodes,bool needPath) {
	//Clear the given path.
	path.path.clear();
	path.pathCost = PATHCOST_INFINITY;

	//Store som basic data.
	maxNodesToBeSearched = min((unsigned int)MAX_SEARCHED_SQUARES, maxNodes);
	this->testMobile=testMobile;
	this->exactPath = exactPath;
	this->needPath=needPath;
	start = startPos;
	startxSqr = (int(start.x) / SQUARE_SIZE)|1;
	startzSqr = (int(start.z) / SQUARE_SIZE)|1;

	//Clamp the start position
	if (startxSqr < 0) startxSqr=0;
	if (startxSqr >= gs->mapx) startxSqr = gs->mapx-1;
	if (startzSqr < 0) startzSqr =0;
	if (startzSqr >= gs->mapy) startzSqr = gs->mapy-1;

	startSquare = startxSqr + startzSqr * gs->mapx;

	//Start up the search.
	SearchResult result = InitSearch(moveData, pfDef);

	//Respond to the success of the search.
	if(result == Ok || result == GoalOutOfRange) {
		FinishSearch(moveData, path);
		if(PATHDEBUG) {
			logOutput << "Path found.\n";
			logOutput << "Nodes tested: " << (int)testedNodes << "\n";
			logOutput << "Open squares: " << (float)(openSquareBufferPointer - openSquareBuffer) << "\n";
			logOutput << "Path steps: " << (int)(path.path.size()) << "\n";
			logOutput << "Path cost: " << path.pathCost << "\n";
		}
	} else {
		if(PATHDEBUG) {
			logOutput << "Path not found!\n";
			logOutput << "Nodes tested: " << (int)testedNodes << "\n";
			logOutput << "Open squares: " << (float)(openSquareBufferPointer - openSquareBuffer) << "\n";
		}
	}
	return result;
}


/**
Setting up the starting point of the search.
*/
IPath::SearchResult CPathFinder::InitSearch(const MoveData& moveData, const CPathFinderDef& pfDef) {
	//If exact path is reqired and the goal is blocked, then no search is needed.
	if(exactPath && pfDef.GoalIsBlocked(moveData, (CMoveMath::BLOCK_STRUCTURE | CMoveMath::BLOCK_TERRAIN)))
		return CantGetCloser;

	//Clamp the start position
	if (startxSqr < 0) startxSqr=0;
	if (startxSqr >= gs->mapx) startxSqr = gs->mapx-1;
	if (startzSqr < 0) startzSqr =0;
	if (startzSqr >= gs->mapy) startzSqr = gs->mapy-1;

	//If the starting position is a goal position, then no search need to be performed.
	if(pfDef.IsGoal(startxSqr, startzSqr))
		return CantGetCloser;

	//Clearing the system from last search.
	ResetSearch();

	//Marks and store the start-square.
	squareState[startSquare].status = (PATHOPT_START | PATHOPT_OPEN);
	squareState[startSquare].cost = 0;
	dirtySquares.push_back(startSquare);

	//Make the beginning the fest square found.
	goalSquare = startSquare;
	goalHeuristic = pfDef.Heuristic(startxSqr, startzSqr);

	//Adding the start-square to the queue.
	openSquareBufferPointer = &openSquareBuffer[0];
	OpenSquare *os = openSquareBufferPointer;	//Taking first OpenSquare in buffer.
	os->currentCost = 0;
	os->cost = 0;
	os->square.x = startxSqr;
	os->square.y = startzSqr;
	os->sqr = startSquare;
	openSquares.push(os);

	//Performs the search.
	SearchResult result = DoSearch(moveData, pfDef);

	//If no improvement has been found then return CantGetCloser instead.
	if(goalSquare == startSquare || goalSquare == 0) {
		return CantGetCloser;
	} else
		return result;
}


/**
Performs the actual search.
*/
IPath::SearchResult CPathFinder::DoSearch(const MoveData& moveData, const CPathFinderDef& pfDef) {
	bool foundGoal = false;
	while(!openSquares.empty() && openSquareBufferPointer - openSquareBuffer < (maxNodesToBeSearched - 8)){
		//Gets the open square with lowest expected path-cost.
		OpenSquare* os = (OpenSquare*)openSquares.top();
		openSquares.pop();

		//Check if this OpenSquare-holder have become obsolete.
		if(squareState[os->sqr].cost != os->cost)
			continue;

		//Check if the goal is reached.
		if(pfDef.IsGoal(os->square.x, os->square.y)) {
			goalSquare = os->sqr;
			goalHeuristic = 0;
			foundGoal = true;
			break;
		}

		//Testing the 8 surrounding squares.
		bool right=TestSquare(moveData, pfDef, os, PATHOPT_RIGHT);
		bool left=TestSquare(moveData, pfDef, os, PATHOPT_LEFT);
		bool up=TestSquare(moveData, pfDef, os, PATHOPT_UP);
		bool down=TestSquare(moveData, pfDef, os, PATHOPT_DOWN);
		if(up){		//we dont want to search diagonally if there is a blocking object (not blocking terrain) in one of the two side squares
			if(right)
				TestSquare(moveData, pfDef, os, (PATHOPT_RIGHT | PATHOPT_UP));
			if(left)
				TestSquare(moveData, pfDef, os, (PATHOPT_LEFT | PATHOPT_UP));
		}