pathestimator.cpp

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

	if(goalBlock.x == startBlock.x && goalBlock.y == startBlock.y)
		return CantGetCloser;
	else
		return result;
}


/*
Performs the actual search.
*/
IPath::SearchResult CPathEstimator::DoSearch(const MoveData& moveData, const CPathFinderDef& peDef) {
	bool foundGoal = false;
	while(!openBlocks.empty() && (openBlockBufferPointer - openBlockBuffer) < (maxBlocksToBeSearched - 8)) {
		//Get the open block with lowest cost.
		OpenBlock* ob = openBlocks.top();
		openBlocks.pop();

		//Check if the block has been marked as unaccessable during it's time in queue.
		if(blockState[ob->blocknr].options & (PATHOPT_BLOCKED | PATHOPT_CLOSED | PATHOPT_FORBIDDEN))
			continue;

		//Check if the goal is reached.
		int xBSquare = blockState[ob->blocknr].sqrCenter[moveData.pathType].x;
		int zBSquare = blockState[ob->blocknr].sqrCenter[moveData.pathType].y;
		int xGSquare = ob->block.x * BLOCK_SIZE + goalSqrOffset.x;
		int zGSquare = ob->block.y * BLOCK_SIZE + goalSqrOffset.y;
		if(peDef.IsGoal(xBSquare, zBSquare) || peDef.IsGoal(xGSquare, zGSquare)) {
			goalBlock = ob->block;
			goalHeuristic = 0;
			foundGoal = true;
			break;
		}

		//Test the 8 surrounding blocks.
		TestBlock(moveData, peDef, *ob, PATHDIR_LEFT);
		TestBlock(moveData, peDef, *ob, PATHDIR_LEFT_UP);
		TestBlock(moveData, peDef, *ob, PATHDIR_UP);
		TestBlock(moveData, peDef, *ob, PATHDIR_RIGHT_UP);
		TestBlock(moveData, peDef, *ob, PATHDIR_RIGHT);
		TestBlock(moveData, peDef, *ob, PATHDIR_RIGHT_DOWN);
		TestBlock(moveData, peDef, *ob, PATHDIR_DOWN);
		TestBlock(moveData, peDef, *ob, PATHDIR_LEFT_DOWN);

		//Mark this block as closed.
		blockState[ob->blocknr].options |= PATHOPT_CLOSED;
	}

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

	//Could not reach the goal.
	if(openBlockBufferPointer - openBlockBuffer >= (maxBlocksToBeSearched - 8))
		return GoalOutOfRange;

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

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


/*
Test the accessability of a block and it's value
and possibly add it to the open-blocks-queue.
*/
void CPathEstimator::TestBlock(const MoveData& moveData, const CPathFinderDef &peDef, OpenBlock& parentOpenBlock, unsigned int direction) {
	testedBlocks++;

	//Initial calculations of the new block.
	int2 block;
	block.x = parentOpenBlock.block.x + directionVector[direction].x;
	block.y = parentOpenBlock.block.y + directionVector[direction].y;
	int vertexNbr = moveData.pathType * nbrOfBlocks * PATH_DIRECTION_VERTICES + parentOpenBlock.blocknr * PATH_DIRECTION_VERTICES + directionVertex[direction];

	//Outside map?
	if(/*block.x < 0 || block.x >= nbrOfBlocksX		//the blocks should never be able to become wrong due to the infinite vertices at the edges
	|| block.y < 0 || block.y >= nbrOfBlocksZ
	||*/ vertexNbr < 0 || vertexNbr >= nbrOfVertices)
		return;

	int blocknr = block.y * nbrOfBlocksX + block.x;
	float blockCost = vertex[vertexNbr];
	if(blockCost >= PATHCOST_INFINITY)
		return;

	//Check if the block is unavailable.
	if(blockState[blocknr].options & (PATHOPT_FORBIDDEN | PATHOPT_BLOCKED | PATHOPT_CLOSED))
		return;

	int xSquare = blockState[blocknr].sqrCenter[moveData.pathType].x;
	int zSquare = blockState[blocknr].sqrCenter[moveData.pathType].y;

	//Check if the block is blocked or out of constraints.
	if(!peDef.WithinConstraints(xSquare, zSquare)) {
		blockState[blocknr].options |= PATHOPT_BLOCKED;
		dirtyBlocks.push_back(blocknr);
		return;
	}

	//Evaluate this node.
	float heuristicCost = peDef.Heuristic(xSquare, zSquare);
	float currentCost = parentOpenBlock.currentCost + blockCost;
	float cost = currentCost + heuristicCost;

	//Check if the block is already in queue and better, then keep it.
	if(blockState[blocknr].options & PATHOPT_OPEN) {
		if(blockState[blocknr].cost <= cost)
			return;
		blockState[blocknr].options &= 255-7;
	}

	//Looking for improvements.
	if(heuristicCost < goalHeuristic) {
		goalBlock = block;
		goalHeuristic = heuristicCost;
	}

	//Store this block as open.
	OpenBlock* ob = ++openBlockBufferPointer;
	ob->block = block;
	ob->blocknr = blocknr;
	ob->cost = cost;
	ob->currentCost = currentCost;
	openBlocks.push(ob);

	//Mark the block as open, and it's parent.
	blockState[blocknr].cost = cost;
	blockState[blocknr].options |= (direction | PATHOPT_OPEN);
	blockState[blocknr].parentBlock = parentOpenBlock.block;
	dirtyBlocks.push_back(blocknr);
}


/*
Recreate the path taken to the goal.
*/
void CPathEstimator::FinishSearch(const MoveData& moveData, Path& path) {
	int2 block = goalBlock;
	while(block.x != startBlock.x || block.y != startBlock.y) {
		int blocknr = block.y * nbrOfBlocksX + block.x;
/*		int xGSquare = block.x * BLOCK_SIZE + goalSqrOffset.x;
		int zGSquare = block.y * BLOCK_SIZE + goalSqrOffset.y;
		//In first case trying to use a by goal defined offset...
		if(!moveData.moveMath->IsBlocked(moveData, moveMathOptions, xGSquare, zGSquare)) {
			float3 pos = SquareToFloat3(xGSquare, zGSquare);
			pos.y = moveData.moveMath->yLevel(xGSquare, zGSquare);
			path.path.push_back(pos);
		}
		//...if not possible, then use offset defined by the block.
		else */{
			int xBSquare = blockState[blocknr].sqrCenter[moveData.pathType].x;
			int zBSquare = blockState[blocknr].sqrCenter[moveData.pathType].y;
			float3 pos = SquareToFloat3(xBSquare, zBSquare);
			path.path.push_back(pos);
		}

		//Next step backwards.
		block = blockState[blocknr].parentBlock;
	}

	//Additional information.
	path.pathCost = blockState[goalBlock.y * nbrOfBlocksX + goalBlock.x].cost - goalHeuristic;
	path.pathGoal = path.path.front();
}


/*
Cleaning lists from last search.
*/
void CPathEstimator::ResetSearch() {
	while(!openBlocks.empty())
		openBlocks.pop();
	while(!dirtyBlocks.empty()) {
		blockState[dirtyBlocks.back()].cost = PATHCOST_INFINITY;
		blockState[dirtyBlocks.back()].parentBlock.x = -1;
		blockState[dirtyBlocks.back()].parentBlock.y = -1;
		blockState[dirtyBlocks.back()].options &= 128;//~PATHOPT_SEARCHRELATED;
		dirtyBlocks.pop_back();
	}
	testedBlocks = 0;
}


/*
Trying to read offset and vertices data from file.
Return false if failed.
TODO: Read-error-check.
*/
bool CPathEstimator::ReadFile(string name)
{
	unsigned int hash=Hash();
	char hashString[50];
	sprintf(hashString,"%u",hash);

	string filename = string("maps/paths/") + stupidGlobalMapname.substr(0, stupidGlobalMapname.find_last_of('.') + 1) + hashString + "." + name + ".zip";

	// open file for reading from a suitable location (where the file exists)
	CArchiveZip* pfile = SAFE_NEW CArchiveZip(filesystem.LocateFile(filename));

	if (!pfile || !pfile->IsOpen()) {
		delete pfile;
		return false;
	}

	std::auto_ptr<CArchiveZip> auto_pfile(pfile);
	CArchiveZip& file(*pfile);

	int fh = file.OpenFile("pathinfo");

	if (fh) {
		unsigned int filehash = 0;
 		//Check hash.
//		logOutput.Print("%i",hash);
		file.ReadFile(fh, &filehash, 4);
		if(filehash != hash)
			return false;

		//Read block-center-offset data.
		int blocknr;
		for(blocknr = 0; blocknr < nbrOfBlocks; blocknr++) {
			file.ReadFile(fh, blockState[blocknr].sqrCenter, moveinfo->moveData.size() * sizeof(int2));
		}

		//Read vertices data.
		file.ReadFile(fh, vertex, nbrOfVertices * sizeof(float));

		//File read successful.
		return true;
	} else {
		return false;
	}
}


/*
Trying to write offset and vertices data to file.
*/
void CPathEstimator::WriteFile(string name) {
	// We need this directory to exist
	if (!filesystem.CreateDirectory("maps/paths"))
		return;

	unsigned int hash = Hash();
	char hashString[50];
	sprintf(hashString,"%u",hash);

	string filename = string("maps/paths/") + stupidGlobalMapname.substr(0, stupidGlobalMapname.find_last_of('.') + 1) + hashString + "." + name + ".zip";
	zipFile file;

	// open file for writing in a suitable location
	file = zipOpen(filesystem.LocateFile(filename, FileSystem::WRITE).c_str(), APPEND_STATUS_CREATE);

	if (file) {
		zipOpenNewFileInZip(file, "pathinfo", NULL, NULL, 0, NULL, 0, NULL, Z_DEFLATED, Z_BEST_COMPRESSION);

		//Write hash.
		unsigned int hash = Hash();
		zipWriteInFileInZip(file, (void*)&hash, 4);

		//Write block-center-offsets.
		int blocknr;
		for(blocknr = 0; blocknr < nbrOfBlocks; blocknr++) {
			zipWriteInFileInZip(file, (void*)blockState[blocknr].sqrCenter, moveinfo->moveData.size() * sizeof(int2));
			//file.write((char*)blockState[blocknr].sqrCenter, moveinfo->moveData.size() * sizeof(int2));
		}

		//Write vertices.
		zipWriteInFileInZip(file, (void*)vertex, nbrOfVertices * sizeof(float));
		//file.write((char*)vertex, nbrOfVertices * sizeof(float));

		zipCloseFileInZip(file);
		zipClose(file, NULL);
	}
}


/*
Gives a hash-code identifying the dataset of this estimator.
*/
unsigned int CPathEstimator::Hash() {
	return readmap->mapChecksum + moveinfo->moveInfoChecksum + BLOCK_SIZE + moveMathOptions + PATHESTIMATOR_VERSION;
}

void CPathEstimator::Draw(void)
{
	MoveData* md=moveinfo->GetMoveDataFromName("TANKSH2");
	if(!selectedUnits.selectedUnits.empty() && (*selectedUnits.selectedUnits.begin())->unitDef->movedata)
		md=(*selectedUnits.selectedUnits.begin())->unitDef->movedata;

	glDisable(GL_TEXTURE_2D);
	glColor3f(1,1,0);
/*	float blue=BLOCK_SIZE==32?1:0;
	glBegin(GL_LINES);
	for(int z = 0; z < nbrOfBlocksZ; z++) {
		for(int x = 0; x < nbrOfBlocksX; x++) {
			int blocknr = z * nbrOfBlocksX + x;
			float3 p1;
			p1.x=(blockState[blocknr].sqrCenter[md->pathType].x)*8;
			p1.z=(blockState[blocknr].sqrCenter[md->pathType].y)*8;
			p1.y=ground->GetHeight(p1.x,p1.z)+10;

			glColor3f(1,1,blue);
			glVertexf3(p1);
			glVertexf3(p1-UpVector*10);
			for(int dir = 0; dir < PATH_DIRECTION_VERTICES; dir++){
				int obx = x + directionVector[dir].x;
				int obz = z + directionVector[dir].y;

				if(obx >= 0 && obz >= 0 && obx < nbrOfBlocksX && obz < nbrOfBlocksZ) {
					float3 p2;
					int obblocknr = obz * nbrOfBlocksX + obx;

					p2.x=(blockState[obblocknr].sqrCenter[md->pathType].x)*8;
					p2.z=(blockState[obblocknr].sqrCenter[md->pathType].y)*8;
					p2.y=ground->GetHeight(p2.x,p2.z)+10;

					int vertexNbr = md->pathType * nbrOfBlocks * PATH_DIRECTION_VERTICES + blocknr * PATH_DIRECTION_VERTICES + directionVertex[dir];
					float cost=vertex[vertexNbr];

					glColor3f(1/(sqrt(cost/BLOCK_SIZE)),1/(cost/BLOCK_SIZE),blue);
					glVertexf3(p1);
					glVertexf3(p2);
				}
			}
		}

	}
	glEnd();/**/
/*	glEnable(GL_TEXTURE_2D);
	for(int z = 0; z < nbrOfBlocksZ; z++) {
		for(int x = 0; x < nbrOfBlocksX; x++) {
			int blocknr = z * nbrOfBlocksX + x;
			float3 p1;
			p1.x=(blockState[blocknr].sqrCenter[md->pathType].x)*SQUARE_SIZE;
			p1.z=(blockState[blocknr].sqrCenter[md->pathType].y)*SQUARE_SIZE;
			p1.y=ground->GetHeight(p1.x,p1.z)+10;

			glColor3f(1,1,blue);
			for(int dir = 0; dir < PATH_DIRECTION_VERTICES; dir++){
				int obx = x + directionVector[dir].x;
				int obz = z + directionVector[dir].y;

				if(obx >= 0 && obz >= 0 && obx < nbrOfBlocksX && obz < nbrOfBlocksZ) {
					float3 p2;
					int obblocknr = obz * nbrOfBlocksX + obx;

					p2.x=(blockState[obblocknr].sqrCenter[md->pathType].x)*SQUARE_SIZE;
					p2.z=(blockState[obblocknr].sqrCenter[md->pathType].y)*SQUARE_SIZE;
					p2.y=ground->GetHeight(p2.x,p2.z)+10;

					int vertexNbr = md->pathType * nbrOfBlocks * PATH_DIRECTION_VERTICES + blocknr * PATH_DIRECTION_VERTICES + directionVertex[dir];
					float cost=vertex[vertexNbr];

					glColor3f(1,1/(cost/BLOCK_SIZE),blue);

					p2=(p1+p2)/2;
					if(camera->pos.distance(p2)<500){
						glPushMatrix();
						glTranslatef3(p2);
						glScalef(5,5,5);
						font->glWorldPrint("%.0f",cost);
						glPopMatrix();
					}
				}
			}
		}
	}
*/
	if(BLOCK_SIZE==8)
		glColor3f(0.2f,0.7f,0.2f);
	else
		glColor3f(0.2f,0.2f,0.7f);
	glDisable(GL_TEXTURE_2D);
	glBegin(GL_LINES);
	for(OpenBlock*  ob=openBlockBuffer;ob!=openBlockBufferPointer;++ob){
		int blocknr = ob->blocknr;
		float3 p1;
		p1.x=(blockState[blocknr].sqrCenter[md->pathType].x)*SQUARE_SIZE;
		p1.z=(blockState[blocknr].sqrCenter[md->pathType].y)*SQUARE_SIZE;
		p1.y=ground->GetHeight(p1.x,p1.z)+15;

		float3 p2;
		int obx=blockState[ob->blocknr].parentBlock.x;
		int obz=blockState[ob->blocknr].parentBlock.y;
		int obblocknr =  obz * nbrOfBlocksX + obx;

		if(obblocknr>=0){
			p2.x=(blockState[obblocknr].sqrCenter[md->pathType].x)*SQUARE_SIZE;
			p2.z=(blockState[obblocknr].sqrCenter[md->pathType].y)*SQUARE_SIZE;
			p2.y=ground->GetHeight(p2.x,p2.z)+15;

			glVertexf3(p1);
			glVertexf3(p2);
		}
	}
	glEnd();
/*	glEnable(GL_TEXTURE_2D);
	glEnable(GL_BLEND);
	glColor4f(1,0,blue,0.7f);
	glAlphaFunc(GL_GREATER,0.05f);
	int a=0;
	for(OpenBlock*  ob=openBlockBuffer;ob!=openBlockBufferPointer;++ob){
		int blocknr = ob->blocknr;
		float3 p1;
		p1.x=(ob->block.x * BLOCK_SIZE + blockState[blocknr].sqrCenter[md->pathType].x)*SQUARE_SIZE;
		p1.z=(ob->block.y * BLOCK_SIZE + blockState[blocknr].sqrCenter[md->pathType].y)*SQUARE_SIZE;
		p1.y=ground->GetHeight(p1.x,p1.z)+15;

		if(camera->pos.distance(p1)<500){
			glPushMatrix();
			glTranslatef3(p1);
			glScalef(5,5,5);
			font->glWorldPrint("%.0f %.0f",ob->cost,ob->currentCost);
			glPopMatrix();
		}
		++a;
	}
	glDisable(GL_BLEND);*/
}

float3 CPathEstimator::FindBestBlockCenter(const MoveData* moveData, float3 pos)
{
	int pathType=moveData->pathType;
	CRangedGoalWithCircularConstraint rangedGoal(pos,pos, 0,0,SQUARE_SIZE*BLOCK_SIZE*SQUARE_SIZE*BLOCK_SIZE*4);
	IPath::Path path;

	std::vector<float3> startPos;

	int xm=(int)(pos.x/(SQUARE_SIZE*BLOCK_SIZE));
	int ym=(int)(pos.z/(SQUARE_SIZE*BLOCK_SIZE));

	for(int y=max(0,ym-1);y<=min(nbrOfBlocksZ-1,ym+1);++y){
		for(int x=max(0,xm-1);x<=min(nbrOfBlocksX-1,xm+1);++x){
			startPos.push_back(float3(blockState[y*nbrOfBlocksX+x].sqrCenter[pathType].x*SQUARE_SIZE,0,blockState[y*nbrOfBlocksX+x].sqrCenter[pathType].y*SQUARE_SIZE));
		}
	}
	IPath::SearchResult result = pathFinder->GetPath(*moveData, startPos, rangedGoal, path);
	if(result == IPath::Ok && !path.path.empty()) {
		return path.path.back();
	}
	return ZeroVector;
}