micropather.cpp

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

/*
 * Copyright (c) 2000-2005 Lee Thomason (www.grinninglizard.com)
 *
 * Grinning Lizard Utilities.
 *
 * This software is provided 'as-is', without any express or implied 
 * warranty. In no event will the authors be held liable for any 
 * damages arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any 
 * purpose, including commercial applications, and to alter it and 
 * redistribute it freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must 
 * not claim that you wrote the original software. If you use this 
 * software in a product, an acknowledgment in the product documentation 
 * would be appreciated but is not required.
 *
 * 2. Altered source versions must be plainly marked as such, and 
 * must not be misrepresented as being the original software.
 *
 * 3. This notice may not be removed or altered from any source 
 * distribution.
 */

/*
 * Updated and changed by Tournesol (on the TA Spring client).
 * May (currenty) only be used to compile KAI (AI for TA Spring).
 * Take care when you use it!
 *
 * This notice may not be removed or altered from any source
 *
 * As of 12 march, 2007, the following applies instead of the above notice:
 * (Tournesol gave permission to change it per e-mail)
 *
 * "All parts of the code in this file that are made by 'Tournesol' are
 * released under GPL licence."
 *
 * --Tobi Vollebregt
 */


#include "MicroPather.h"

#define USE_NODEATINDEX100
// #define USE_ASSERTIONS
// #define DEBUG_PATH
// #define DEBUG_PATH_DEEP
// #define USE_LIST



using namespace NSMicroPather;

class OpenQueueBH {
	public:
	
	OpenQueueBH(AIClasses* ai, PathNode** heapArray): ai(ai), size(0) {
		this -> heapArray = heapArray;
	}

	~OpenQueueBH() {}

	void Push(PathNode* pNode) {
		pNode->inOpen = 1;
		// L("Push: " << size);
		if (size) {
			size++;
			heapArray[size] = pNode;
			pNode->myIndex =  size;
			int i = size;

			while((i > 1) && (heapArray[i >> 1]->totalCost > heapArray[i]->totalCost)) {
				// L("Swap them: " << i);
				PathNode* temp = heapArray[i >> 1];
				heapArray[i >> 1] = heapArray[i];
				heapArray[i] = temp;

				temp -> myIndex = i;
				heapArray[i >> 1] -> myIndex = i >> 1;

				i >>= 1;
			}
		}
		else {
			// L("The tree was empty: " );
			size++;
			heapArray[1] = pNode;
			pNode->myIndex = size;
		}
	}


	void Update(PathNode* pNode) {
		// L("Update: " << size);

		if (size > 1) {
			// heapify now
			int i = pNode->myIndex;

			while(i > 1 && ((heapArray[i >> 1] -> totalCost) > (heapArray[i] -> totalCost))) {
				// swap them
				PathNode* temp = heapArray[i >> 1];
				heapArray[i >> 1] = heapArray[i];
				heapArray[i] = temp;

				temp -> myIndex = i;
				heapArray[i >> 1] -> myIndex =  i >> 1;
				i >>= 1;
			}
		}
	}


	PathNode* Pop() {
		// L("Pop: " << size);

		// get the first one
		PathNode* min = heapArray[1];
		min -> inOpen = 0;
		heapArray[1] = heapArray[size];
		size--;

		if (size == 0) {
			return min;
		}

		heapArray[1] -> myIndex = 1;
		# define Parent(x) (x >> 1)
		# define Left(x)  (x << 1)
		# define Right(x) ((x << 1) + 1)

		// fix the heap
		int index = 1;
		int smalest = 1;

		{
			LOOPING_HEAPIFY:
			index = smalest;
			int left = Left(index);
			int right = Right(index);
			// L("left: " << left << ", index: " << index);

			if (left <= size && ((heapArray[left] -> totalCost) < (heapArray[index] -> totalCost)))
				smalest = left;
			else
				smalest = index;

			if (right <= size && ((heapArray[right] -> totalCost) < (heapArray[smalest] -> totalCost)))
				smalest = right;

			// L("index: " << index << ", smalest: " << smalest);
			if (smalest != index) {
				// swap them
				PathNode* temp = heapArray[index];
				heapArray[index] = heapArray[smalest];
				heapArray[smalest] = temp;

				temp -> myIndex = smalest;
				heapArray[index] -> myIndex = index;

				goto LOOPING_HEAPIFY;
			}
		}

		return min;
	}

	bool Empty() {
		return (size == 0);
	}
	
	private:
		PathNode** heapArray;
		AIClasses* ai;
		int size;
};


// logger hack
MicroPather::MicroPather(Graph* _graph, AIClasses* ai, unsigned allocate):
	ALLOCATE(allocate), BLOCKSIZE(allocate - 1), graph(_graph), pathNodeMem(0), availMem(0), pathNodeCount(0), frame(0), checksum(0) {

	this -> ai = ai;
	AllocatePathNode();
	hasStartedARun = false;
}

MicroPather::~MicroPather() {
	free(pathNodeMemForFree);
	free(heapArrayMem);
}


// make sure that costArray doesn't contain values below 1.0 (for speed), and below 0.0 (for eternal loop)
void MicroPather::SetMapData(bool *canMoveArray, float *costArray, int mapSizeX, int mapSizeY) {
	this -> canMoveArray = canMoveArray;
	this -> costArray = costArray;
	this -> mapSizeX = mapSizeX;
	this -> mapSizeY = mapSizeY;

	if (mapSizeY * mapSizeX  > (int) ALLOCATE) {
		// L("Error: 'mapSizeY * mapSizeX  > ALLOCATE' in pather");
		// Stop running:
		assert(!(mapSizeY * mapSizeX  > (int)ALLOCATE));
	}

	// L("pather: mapSizeX: " << mapSizeX << ", mapSizeY: " << mapSizeY << ", ALLOCATE: " << ALLOCATE);

	// Tournesol: make a fixed offset array
	// ***
	// *X*
	// ***

	// smart ordering (do not change)
	offsets[0] = -1;
	offsets[1] = 1;
	offsets[2] = + mapSizeX;
	offsets[3] = - mapSizeX;
	offsets[4] = - mapSizeX - 1;
	offsets[5] = - mapSizeX + 1;
	offsets[6] = + mapSizeX - 1;
	offsets[7] = + mapSizeX + 1;
}


void MicroPather::Reset() {
	// L("Reseting pather, frame is: " << frame);
	for (unsigned i = 0; i < ALLOCATE; i++) {
		PathNode* directNode = &pathNodeMem[i];
		directNode -> Reuse(0);
	}

	frame = 1;
}


// must only be called once...
PathNode* MicroPather::AllocatePathNode() {
	PathNode* result;

	if (availMem == 0) {
		PathNode* newBlock = (PathNode*) malloc(sizeof(PathNode) * ALLOCATE);
		// L("pathNodeMemForFree: " << ((unsigned) newBlock));
		pathNodeMemForFree = newBlock;
		pathNodeMem = newBlock;
		// L(" sizeof(PathNode): " << sizeof(PathNode));
		availMem = BLOCKSIZE;

		// L("availMem == " << (sizeof(PathNode) * ALLOCATE));

		// make all the nodes in one go (step one)
		for (unsigned i = 0; i < ALLOCATE; i++) {
			result = &pathNodeMem[i];
			++pathNodeCount;
			result -> Init(0, FLT_BIG, 0);
			result -> isEndNode = 0;
		}

		result = newBlock;
		heapArrayMem = (PathNode**) malloc(sizeof(PathNode*) * ALLOCATE);
		// L("heapArrayMem: " << heapArrayMem);
	}
	else {
		// this is bad....
		bool AllocatePathNodeCalledTwice = false;
		assert(AllocatePathNodeCalledTwice);
	}

	// assert( availMem );
	// L("AllocatePathNode()");
	return result;
}

void MicroPather::GoalReached(PathNode* node, void* start, void* end, vector<void*>* path) {
	path -> clear();

	// we have reached the goal, how long is the path?
	// (used to allocate the vector which is returned)
	int count = 1;
	PathNode* it = node;

	while (it -> parent) {
		++count;
		it = it -> parent;
	}

	// now that the path has a known length, allocate
	// and fill the vector that will be returned
	if (count < 3) {
		// Handle the short, special case.
		path -> resize(2);
		(*path)[0] = start;
		(*path)[1] = end;
	}
	else {
		path -> resize(count);

		(*path)[0] = start;
		(*path)[count - 1] = end;

		count -= 2;
		it = node -> parent;

		while (it -> parent) {
			(*path)[count] = (void*) ((((unsigned long) it) - ((unsigned long) pathNodeMem)) / sizeof(PathNode));
			it = it -> parent;
			--count;
		}
	}

	#ifdef DEBUG_PATH
	printf("Path: ");
	printf("Cost = %.1f Checksum %d\n", node -> costFromStart, checksum);
	#endif
}

float MicroPather::LeastCostEstimateLocal(int nodeStartIndex) {
	int yStart = nodeStartIndex / mapSizeX;
	int xStart = nodeStartIndex - yStart * mapSizeX;

	int dx = abs(xStart - xEndNode);
	int dy = abs(yStart - yEndNode);
	int strait = abs(dx - dy);

	return (strait + 1.41f * min(dx, dy));
}

void MicroPather::FixStartEndNode(void** startNode, void** endNode) {
	long index = (long) *startNode;
	int y = index / mapSizeX;
	int x = index - y * mapSizeX;
	
	// no node can be at the edge!
	if (x == 0)
		x = 1;
	else if (x == mapSizeX)
		x = mapSizeX - 1;

	if (y == 0)
		y = 1;
	else if (y == mapSizeY)
		y = mapSizeY - 1;

	*startNode = (void*) (y * mapSizeX + x);
	index = (long) *endNode;
	y = index / mapSizeX;
	x = index - y * mapSizeX;

	// no node can be at the edge!
	if (x == 0)
		x = 1;
	else if (x == mapSizeX)
		x = mapSizeX - 1;

	if (y == 0)
		y = 1;
	else if (y == mapSizeY)
		y = mapSizeY - 1;

	xEndNode = x;
	yEndNode = y;
	*endNode = (void*) (y * mapSizeX + x);
}

void MicroPather::FixNode( void** Node) {
	long index = (long) *Node;
	int y = index / mapSizeX;
	int x = index - y * mapSizeX;
	
	assert(index >= 0);
	assert(index <= mapSizeX * mapSizeY);
	
	// no node can be at the edge!
	if (x == 0)
		x = 1;
	else if (x == mapSizeX)
		x = mapSizeX - 1;
	
	if (y == 0)
		y = 1;
	else if (y == mapSizeY)
		y = mapSizeY - 1;
	
	*Node = (void*) (y * mapSizeX + x);
}



int MicroPather::Solve(void* startNode, void* endNode, vector<void*>* path, float* cost) {
	assert(!hasStartedARun);
	hasStartedARun = true;
	*cost = 0.0f;

	if (startNode == endNode) {
		hasStartedARun = false;
		return START_END_SAME;
	}

	{
		FixStartEndNode(&startNode, &endNode);

		if (!canMoveArray[(long) startNode]) {
			// L("Pather: trying to move from a blocked start pos");
		}
		if (!canMoveArray[(long) endNode]) {
			// can't move into the endNode: just fail fast
			hasStartedARun = false;
			return NO_SOLUTION;
		}
	}

	++frame;

	if (frame > 65534) {