micropather.cpp

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

		// L("frame > 65534, pather reset needed");
		Reset();
	}

	// Make the priority queue
	OpenQueueBH open(ai, heapArrayMem);

	{
		PathNode* tempStartNode = &pathNodeMem[(unsigned long) startNode];
		float estToGoal = LeastCostEstimateLocal( (unsigned long) startNode);
		tempStartNode -> Reuse(frame);
		tempStartNode -> costFromStart = 0;
		tempStartNode -> totalCost = estToGoal;
		open.Push(tempStartNode);
	}

	PathNode* endPathNode = &(pathNodeMem[(unsigned long) endNode]);

	while (!open.Empty()) {
		PathNode* node = open.Pop();

		if (node == endPathNode) {
			GoalReached(node, startNode, endNode, path);
			*cost = node -> costFromStart;
			hasStartedARun = false;
			return SOLVED;
		}
		else {
			// we have not reached the goal, add the neighbors (emulate GetNodeNeighbors)
			int indexStart = (((unsigned long) node) - ((unsigned long) pathNodeMem)) / sizeof(PathNode);

			#ifdef USE_ASSERTIONS
			int ystart = indexStart / mapSizeX;
			int xstart = indexStart - ystart * mapSizeX;
			
			// no node can be at the edge!
			assert(xstart != 0 && xstart != mapSizeX);
			assert(ystart != 0 && ystart != mapSizeY);
			#endif

			float nodeCostFromStart = node -> costFromStart;

			for (int i = 0; i < 8; ++i) {
				int indexEnd = offsets[i] + indexStart;
				
				if (!canMoveArray[indexEnd]) {
					continue;
				}

				PathNode* directNode = &pathNodeMem[indexEnd];

				if (directNode -> frame != frame)
					directNode -> Reuse(frame);

				#ifdef USE_ASSERTIONS
				int yend = indexEnd / mapSizeX;
				int xend = indexEnd - yend * mapSizeX;

				// we can move to that spot
				assert(canMoveArray[yend * mapSizeX + xend]);

				// no node can be at the edge!
				assert(xend != 0 && xend != mapSizeX);
				assert(yend != 0 && yend != mapSizeY);
				#endif

				float newCost = nodeCostFromStart;

				if (i > 3) {
					newCost += costArray[indexEnd] * 1.41f;
				}
				else
					newCost += costArray[indexEnd];

				if (directNode -> costFromStart <= newCost) {
					// do nothing, this path is not better than existing one
					continue;
				}

				// it's better, update its data
				directNode -> parent = node;
				directNode -> costFromStart = newCost;
				directNode -> totalCost = newCost + LeastCostEstimateLocal(indexEnd);
				#ifdef USE_ASSERTIONS
				assert(indexEnd == ((((unsigned) directNode) - ((unsigned) pathNodeMem)) / sizeof(PathNode)));
				#endif

				if (directNode -> inOpen) {
					open.Update(directNode);
				}
				else {
					directNode -> inClosed = 0;
					open.Push(directNode);
				}
			}
		}

		node -> inClosed = 1;
	}

	hasStartedARun = false;
	return NO_SOLUTION;
}


int MicroPather::FindBestPathToAnyGivenPoint(void* startNode, vector<void*> endNodes, vector<void*>* path, float* cost) {
	assert(!hasStartedARun);
	hasStartedARun = true;
	*cost = 0.0f;
	
	for (unsigned i = 0; i < ALLOCATE; i++) {
		PathNode* theNode = &pathNodeMem[i];

		if (theNode -> isEndNode) {
			// L("i: " << i << ", " << theNode->isEndNode);
			theNode -> isEndNode = 0;
			assert(theNode -> isEndNode == 0);
		}
	}

	if (endNodes.size() <= 0) {
		// just fail fast
		hasStartedARun = false;
		return NO_SOLUTION;
	}

	{
		void* endNode = endNodes[0];
		FixStartEndNode(&startNode, &endNode);

		if (!canMoveArray[(long)startNode]) {
			// L("Pather: trying to move from a blocked start pos");
		}
	}

	++frame;

	if (frame > 65534) {
		// L("frame > 65534, pather reset needed");
		Reset();
	}

	// Make the priority queue
	OpenQueueBH open(ai, heapArrayMem);

	{
		PathNode* tempStartNode = &pathNodeMem[(unsigned long) startNode];
		float estToGoal = LeastCostEstimateLocal( (unsigned long) startNode);
		tempStartNode -> Reuse(frame);
		tempStartNode -> costFromStart = 0;
		tempStartNode -> totalCost = estToGoal;
		open.Push( tempStartNode );
	}

	// mark the endNodes
	for (unsigned i = 0; i < endNodes.size(); i++) {
		FixNode(&endNodes[i]);
		PathNode* tempEndNode = &pathNodeMem[(unsigned long) endNodes[i]];
		tempEndNode -> isEndNode = 1;
	}

	while (!open.Empty()) {
		PathNode* node = open.Pop();

		if (node -> isEndNode) {
			void* theEndNode = (void*) ((((unsigned long) node) - ((unsigned long) pathNodeMem)) / sizeof(PathNode));
			GoalReached(node, startNode, theEndNode, path);
			*cost = node -> costFromStart;
			hasStartedARun = false;

			// unmark the endNodes
			for (unsigned i = 0; i < endNodes.size(); i++) {
				PathNode* tempEndNode = &pathNodeMem[(unsigned long) endNodes[i]];
				tempEndNode -> isEndNode = 0;
			}

			return SOLVED;
		}
		else {
			// we have not reached the goal, add the neighbors (emulate GetNodeNeighbors)
			int indexStart = (((unsigned long) node) - ((unsigned long) pathNodeMem)) / sizeof(PathNode);

			#ifdef USE_ASSERTIONS
			int ystart = indexStart / mapSizeX;
			int xstart = indexStart - ystart * mapSizeX;

			// no node can be at the edge!
			assert(xstart != 0 && xstart != mapSizeX);
			assert(ystart != 0 && ystart != mapSizeY);
			#endif

			float nodeCostFromStart = node -> costFromStart;

			for (int i = 0; i < 8; ++i) {
				int indexEnd = offsets[i] + indexStart;

				if (!canMoveArray[indexEnd]) {
					continue;
				}

				PathNode* directNode = &pathNodeMem[indexEnd];

				if (directNode -> frame != frame)
					directNode -> Reuse(frame);

				#ifdef USE_ASSERTIONS
				int yend = indexEnd / mapSizeX;
				int xend = indexEnd - yend * mapSizeX;

				// we can move to that spot
				assert(canMoveArray[yend * mapSizeX + xend]);

				// no node can be at the edge!
				assert(xend != 0 && xend != mapSizeX);
				assert(yend != 0 && yend != mapSizeY);
				#endif

				float newCost = nodeCostFromStart;

				if (i > 3) {
					newCost += costArray[indexEnd] * 1.41f;
				}
				else
					newCost += costArray[indexEnd];

				if (directNode -> costFromStart <= newCost) {
					// do nothing, this path is not better than existing one
					continue;
				}

				// it's better, update its data
				directNode -> parent = node;
				directNode -> costFromStart = newCost;
				directNode -> totalCost = newCost + LeastCostEstimateLocal(indexEnd);

				#ifdef USE_ASSERTIONS
				assert(indexEnd == ((((unsigned) directNode) - ((unsigned) pathNodeMem)) / sizeof(PathNode)));
				#endif

				if (directNode -> inOpen) {
					open.Update(directNode);
				}
				else {
					directNode -> inClosed = 0;
					open.Push(directNode);
				}
			}
		}

		node -> inClosed = 1;
	}

	// unmark the endNodes
	for (unsigned i = 0; i < endNodes.size(); i++) {
		PathNode* tempEndNode = &pathNodeMem[(unsigned long)endNodes[i]];
		tempEndNode -> isEndNode = 0;
	}

	hasStartedARun = false;
	return NO_SOLUTION;
}



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

	if (radius <= 0) {
		// just fail fast
		hasStartedARun = false;
		return NO_SOLUTION;
	}

	{
		FixStartEndNode(&startNode, &endNode);

		if (!canMoveArray[(long)startNode]) {
			// L("Pather: trying to move from a blocked start pos");
		}
	}

	++frame;
	if (frame > 65534) {
		// L("frame > 65534, pather reset needed");
		Reset();
	}

	// make the priority queue
	OpenQueueBH open(ai, heapArrayMem);

	{
		PathNode* tempStartNode = &pathNodeMem[(unsigned long) startNode];
		float estToGoal = LeastCostEstimateLocal( (unsigned long) startNode);
		tempStartNode -> Reuse(frame);
		tempStartNode -> costFromStart = 0;
		tempStartNode -> totalCost = estToGoal;
		open.Push(tempStartNode);
	}
	
	// make the radius
	long indexEnd = (long) endNode;
	int y = indexEnd / mapSizeX;
	int x = indexEnd - y * mapSizeX;
	int* xend = new int[2 * radius + 1];

	for (int a = 0; a < (2 * radius + 1); a++) {
		float z = a - radius;
		float floatsqrradius = radius * radius;
		xend[a] = int(sqrtf(floatsqrradius - z * z));

		// L("xend[a]: " << xend[a]);
		// L("xStart: " << xStart << ", xEnd: " << xEnd);
	}

	// L("yEndNode: " << yEndNode << ", xEndNode: " << xEndNode);

	while (!open.Empty()) {
		PathNode* node = open.Pop();

		int indexStart = (((unsigned long) node) - ((unsigned long) pathNodeMem)) / sizeof(PathNode);
		int ystart = indexStart / mapSizeX;
		int xstart = indexStart - ystart * mapSizeX;
		// L("counter: " << counter << ", ystart: " << ystart << ", xstart: " << xstart);

		// do a box test (slow/test, note that a <= x <= b is the same as x - a <= b - a)
		if ((y - radius <= ystart && ystart <= y + radius) && (x - radius <= xstart && xstart <= x + radius)) {
			// we are in range (x and y direction), find the relative pos from endNode
			int relativeY = ystart - (yEndNode - radius);
			int relativeX = abs(xstart - xEndNode);
			// L("relativeY: " << relativeY << ", relativeX: " << relativeX);

			if (relativeX <= xend[relativeY]) {
				// L("Its a hit: " << counter);

				GoalReached(node, startNode, (void*) (indexStart), path);
				*cost = node -> costFromStart;
				hasStartedARun = false;
				return SOLVED;
			}
		}

		{
			// we have not reached the goal, add the neighbors.
			#ifdef USE_ASSERTIONS
			// no node can be at the edge!
			assert(xstart != 0 && xstart != mapSizeX);
			assert(ystart != 0 && ystart != mapSizeY);
			#endif

			float nodeCostFromStart = node -> costFromStart;
			for (int i = 0; i < 8; ++i) {
				int indexEnd = offsets[i] + indexStart;

				if (!canMoveArray[indexEnd]) {
					continue;
				}

				PathNode* directNode = &pathNodeMem[indexEnd];

				if (directNode->frame != frame)
					directNode -> Reuse(frame);

				#ifdef USE_ASSERTIONS
				int yend = indexEnd / mapSizeX;
				int xend = indexEnd - yend * mapSizeX;

				// we can move to that spot
				assert(canMoveArray[yend*mapSizeX + xend]);

				// no node can be at the edge!
				assert(xend != 0 && xend != mapSizeX);
				assert(yend != 0 && yend != mapSizeY);
				#endif

				float newCost = nodeCostFromStart;

				if (i > 3) {
					newCost += costArray[indexEnd] * 1.41f;
				}
				else
					newCost += costArray[indexEnd];

				if (directNode -> costFromStart <= newCost) {
					// do nothing, this path is not better than existing one
					continue;
				}

				// it's better, update its data
				directNode -> parent = node;
				directNode -> costFromStart = newCost;
				directNode -> totalCost = newCost + LeastCostEstimateLocal(indexEnd);

				#ifdef USE_ASSERTIONS
				assert(indexEnd == ((((unsigned) directNode) - ((unsigned) pathNodeMem)) / sizeof(PathNode)));
				#endif

				if (directNode -> inOpen) {
					open.Update(directNode);
				}
				else {
					directNode -> inClosed = 0;
					open.Push(directNode);
				}
			}
		}

		node -> inClosed = 1;
	}

	hasStartedARun = false;
	return NO_SOLUTION;
}