pathfinder.cpp

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

		if(down){
			if(right)
				TestSquare(moveData, pfDef, os, (PATHOPT_RIGHT | PATHOPT_DOWN));
			if(left)
				TestSquare(moveData, pfDef, os, (PATHOPT_LEFT | PATHOPT_DOWN));
		}

		//Mark this square as closed.
		squareState[os->sqr].status |= PATHOPT_CLOSED;
	}

	//Returning search-result.
	if(foundGoal)
		return Ok;

	//Could not reach the goal.
	if(openSquareBufferPointer - openSquareBuffer >= (maxNodesToBeSearched - 8))
		return GoalOutOfRange;

	//Search could not reach the goal, due to the unit being locked in.
	if(openSquares.empty())
		return GoalOutOfRange;

	//Below shall never be runned.
	logOutput << "ERROR: CPathFinder::DoSearch() - Unhandled end of search!\n";
	return Error;
}


/**
Test the availability and value of a square,
and possibly add it to the queue of open squares.
*/
bool CPathFinder::TestSquare(const MoveData& moveData, const CPathFinderDef& pfDef, OpenSquare* parentOpenSquare, unsigned int enterDirection) {
	testedNodes++;

	//Calculate the new square.
	int2 square;
	square.x = parentOpenSquare->square.x + directionVector[enterDirection].x;
	square.y = parentOpenSquare->square.y + directionVector[enterDirection].y;

	//Inside map?
	if(square.x < 0 || square.y < 0	|| square.x >= gs->mapx || square.y >= gs->mapy)
		return false;
	int sqr = square.x + square.y * gs->mapx;

	//Check if the square is unaccessable or used.
	if(squareState[sqr].status & (PATHOPT_CLOSED | PATHOPT_FORBIDDEN | PATHOPT_BLOCKED)){
		if(squareState[sqr].status & PATHOPT_BLOCKED)
			return false;
		else
			return false;
	}

	int blockStatus=moveData.moveMath->IsBlocked2(moveData, square.x, square.y);
	//Check if square are out of constraints or blocked by something.
	//Don't need to be done on open squares, as whose are already tested.
	if(!(squareState[sqr].status & PATHOPT_OPEN) && (!pfDef.WithinConstraints(square.x, square.y) || (blockStatus & (CMoveMath::BLOCK_STRUCTURE | CMoveMath::BLOCK_TERRAIN)))){
		squareState[sqr].status |= PATHOPT_BLOCKED;
		dirtySquares.push_back(sqr);
		return false;
	}

	//Evaluate this node.
	float squareSpeedMod = moveData.moveMath->SpeedMod(moveData, square.x, square.y);

	if(squareSpeedMod==0){
		squareState[sqr].status |= PATHOPT_FORBIDDEN;
		dirtySquares.push_back(sqr);
		return false;
	}
	if(testMobile && (blockStatus & (CMoveMath::BLOCK_MOBILE | CMoveMath::BLOCK_MOVING | CMoveMath::BLOCK_MOBILE_BUSY))){
		if(blockStatus & CMoveMath::BLOCK_MOVING)
			squareSpeedMod*=0.65f;
		else if(blockStatus & CMoveMath::BLOCK_MOBILE)
			squareSpeedMod*=0.35f;
		else
			squareSpeedMod*=0.10f;
	}
	float squareCost = moveCost[enterDirection] / squareSpeedMod;
	float heuristicCost = pfDef.Heuristic(square.x, square.y);

	//Summarize cost.
	float currentCost = parentOpenSquare->currentCost + squareCost;
	float cost = currentCost + heuristicCost;

	//Checks if this square are in que already.
	//If the old one is better then keep it, else change it.
	if(squareState[sqr].status & PATHOPT_OPEN) {
		if(squareState[sqr].cost <= cost)
			return true;
		squareState[sqr].status &= ~PATHOPT_DIRECTION;
	}

	//Looking for improvements.
	if(!exactPath	&& heuristicCost < goalHeuristic) {
		goalSquare = sqr;
		goalHeuristic = heuristicCost;
	}

	//Store this square as open.
	OpenSquare *os = ++openSquareBufferPointer;		//Take the next OpenSquare in buffer.
	os->square = square;
	os->sqr = sqr;
	os->currentCost = currentCost;
	os->cost = cost;
	openSquares.push(os);

	//Set this one as open and the direction from which it was reached.
	squareState[sqr].cost = os->cost;
	squareState[sqr].status |= (PATHOPT_OPEN | enterDirection);
	dirtySquares.push_back(sqr);
	return true;
}


/**
Recreates the path found by pathfinder.
Starting at goalSquare and tracking backwards.
*/
void CPathFinder::FinishSearch(const MoveData& moveData, Path& foundPath) {
	//Backtracking the path.
	if(needPath)
		{
		int2 square;
		square.x = goalSquare % gs->mapx;
		square.y = goalSquare / gs->mapx;
		do {
			int sqr = square.x + square.y * gs->mapx;
			if(squareState[sqr].status & PATHOPT_START)
				break;
			float3 cs;
				cs.x = (square.x/2/* + 0.5f*/) * SQUARE_SIZE*2+SQUARE_SIZE;
				cs.z = (square.y/2/* + 0.5f*/) * SQUARE_SIZE*2+SQUARE_SIZE;
			cs.y = moveData.moveMath->yLevel(square.x, square.y);
			foundPath.path.push_back(cs);
			int2 oldSquare;
			oldSquare.x = square.x;
			oldSquare.y = square.y;
			square.x -= directionVector[squareState[sqr].status & PATHOPT_DIRECTION].x;
			square.y -= directionVector[squareState[sqr].status & PATHOPT_DIRECTION].y;
			} 
		while(true);
		if (foundPath.path.size() > 0)
			{
			foundPath.pathGoal = foundPath.path.front();
			}
		}
	//Adds the cost of the path.
	foundPath.pathCost = squareState[goalSquare].cost;
}


/**
Clear things up from last search.
*/
void CPathFinder::ResetSearch() {
	openSquares.DeleteAll();
//	while(!openSquares.empty())
//		openSquares.pop();
	while(!dirtySquares.empty()){
		int lsquare = dirtySquares.back();
		squareState[lsquare].status = 0;
		squareState[lsquare].cost = PATHCOST_INFINITY;
		dirtySquares.pop_back();
	}
	testedNodes = 0;
}



////////////////////
// CPathFinderDef //
////////////////////

/**
Constructor.
*/
CPathFinderDef::CPathFinderDef(float3 goalCenter, float goalRadius) :
goal(goalCenter),
sqGoalRadius(goalRadius)
{
	//Makes sure that the goal could be reached with 2-square resolution.
	if(sqGoalRadius < SQUARE_SIZE*SQUARE_SIZE*2)
		sqGoalRadius = SQUARE_SIZE*SQUARE_SIZE*2;
	goalSquareX=(int)(goalCenter.x/SQUARE_SIZE);
	goalSquareZ=(int)(goalCenter.z/SQUARE_SIZE);
}


/**
Tells whenever the goal is in range.
*/
bool CPathFinderDef::IsGoal(int xSquare, int zSquare) const {
	return ((SquareToFloat3(xSquare, zSquare)-goal).SqLength2D() <= sqGoalRadius);
}


/**
Distance to goal center in mapsquares.
*/
float CPathFinderDef::Heuristic(int xSquare, int zSquare) const
{
	int min=abs(xSquare-goalSquareX);
	int max=abs(zSquare-goalSquareZ);
	if(min>max){
		int temp=min;
		min=max;
		max=temp;
	}
	return max*0.5f+min*0.2f;
}


/**
Tells if the goal are is unaccessable.
If the goal area is small and blocked then it's considered blocked, else not.
*/
bool CPathFinderDef::GoalIsBlocked(const MoveData& moveData, unsigned int moveMathOptions) const {
	if((sqGoalRadius < SQUARE_SIZE*SQUARE_SIZE*4 || sqGoalRadius <= (moveData.size/2)*(moveData.size/2)*1.5f*SQUARE_SIZE*SQUARE_SIZE)
	&& (moveData.moveMath->IsBlocked(moveData, goal) & moveMathOptions))
		return true;
	else
		return false;
}

int2 CPathFinderDef::GoalSquareOffset(int blockSize) const {
	int blockPixelSize = blockSize * SQUARE_SIZE;
	int2 offset;
	offset.x = ((int)goal.x % blockPixelSize) / SQUARE_SIZE;
	offset.y = ((int)goal.z % blockPixelSize) / SQUARE_SIZE;
	return offset;
}

/**
Draw a circle around the goal, indicating the goal area.
*/
void CPathFinderDef::Draw() const {
	glColor4f(0, 1, 1, 1);
	glSurfaceCircle(goal, sqrt(sqGoalRadius), 20);
}

void CPathFinder::Draw(void)
{
	glColor3f(0.7f,0.2f,0.2f);
	glDisable(GL_TEXTURE_2D);
	glBegin(GL_LINES);
	for(OpenSquare* os=openSquareBuffer;os!=openSquareBufferPointer;++os){
		int2 sqr=os->square;
		int square = os->sqr;
		if(squareState[square].status & PATHOPT_START)
			continue;
		float3 p1;
		p1.x=sqr.x*SQUARE_SIZE;
		p1.z=sqr.y*SQUARE_SIZE;
		p1.y=ground->GetHeight(p1.x,p1.z)+15;
		float3 p2;
		int obx=sqr.x-directionVector[squareState[square].status & PATHOPT_DIRECTION].x;
		int obz=sqr.y-directionVector[squareState[square].status & PATHOPT_DIRECTION].y;
		int obsquare =  obz * gs->mapx + obx;

		if(obsquare>=0){
			p2.x=obx*SQUARE_SIZE;
			p2.z=obz*SQUARE_SIZE;
			p2.y=ground->GetHeight(p2.x,p2.z)+15;

			glVertexf3(p1);
			glVertexf3(p2);
		}
	}
	glEnd();
}

//////////////////////////////////////////
// CRangedGoalWithCircularConstraintPFD //
//////////////////////////////////////////

/**
Constructor
Calculating the center and radius of the constrainted area.
*/
CRangedGoalWithCircularConstraint::CRangedGoalWithCircularConstraint(float3 start, float3 goal, float goalRadius,float searchSize,int extraSize) :
CPathFinderDef(goal, goalRadius)
{
	int startX=(int)(start.x/SQUARE_SIZE);
	int startZ=(int)(start.z/SQUARE_SIZE);
	float3 halfWay = (start + goal) / 2;

	halfWayX = (int)(halfWay.x/SQUARE_SIZE);
	halfWayZ = (int)(halfWay.z/SQUARE_SIZE);

	int dx=startX-halfWayX;
	int dz=startZ-halfWayZ;

	searchRadiusSq=dx*dx+dz*dz;
	searchRadiusSq*=(int)(searchSize*searchSize);
	searchRadiusSq+=extraSize;
}


/**
Tests if a square is inside is the circular constrainted area.
*/
bool CRangedGoalWithCircularConstraint::WithinConstraints(int xSquare, int zSquare) const
{
	int dx=halfWayX-xSquare;
	int dz=halfWayZ-zSquare;

	return (dx*dx+dz*dz <= searchRadiusSq);
}

void CPathFinder::myPQ::DeleteAll()
{
//	while(!c.empty())
//		c.pop_back();
	c.clear();
//	c.reserve(1000);
}

CPathFinderDef::~CPathFinderDef() {
}
CRangedGoalWithCircularConstraint::~CRangedGoalWithCircularConstraint() {
}