From 6cef06254765e9915a088d83993fc39ad0fbaa4f Mon Sep 17 00:00:00 2001
From: Kalle Olavi Niemitalo <kon@iki.fi>
Date: Wed, 14 Apr 2010 23:58:21 +0300
Subject: [PATCHv9] Speed up pathfinder by caching islands

When AStarFindPath decides that the goal is unreachable,
it has already done a flood fill from the source position
to all reachable tiles.  Save the filled island in a cache.
Then, if another unit in that island wants to move out of it,
consider the goal unreachable right away.

In this version, the cache is flushed on every game cycle, so that it
need not be updated when units move out of the way.  Also, groups of
jet fighters do not benefit from the cache because each jet fighter is
3x3 tiles large and A* looking for a path for one jet fighter does not
get close enough to another one to mark it as being on the same island.
However, the goals of aircraft are almost always reachable, so this
weakness probably doesn't matter.

This version of the patch is still somewhat sloppy code and should not
be pushed to svn.  Comments and proper initialization are missing.
---
 engine/include/pathfinder.h      |    5 +
 engine/pathfinder/astar.cpp      |   50 ++++++++++
 engine/pathfinder/pathfinder.cpp |  194 +++++++++++++++++++++++++++++++++++++-
 3 files changed, 244 insertions(+), 5 deletions(-)

diff --git a/engine/include/pathfinder.h b/engine/include/pathfinder.h
index 7183a15..d17fca4 100644
--- a/engine/include/pathfinder.h
+++ b/engine/include/pathfinder.h
@@ -125,6 +125,11 @@ extern void FreeAStar(void);
 extern int AStarFindPath(int sx, int sy, int gx, int gy, int gw, int gh,
 	int tilesizex, int tilesizey, int minrange, int maxrange, char *path, int pathlen, void *data);
 
+extern bool AStarWasSlow(void);
+extern void AStarForEachTileInIsland(void (STDCALL *fn)(int x, int y,
+							void *data),
+				     void *data);
+
 extern void SetAStarFixedUnitCrossingCost(int cost);
 extern int GetAStarFixedUnitCrossingCost();
 
diff --git a/engine/pathfinder/astar.cpp b/engine/pathfinder/astar.cpp
index 49668fb..cced194 100644
--- a/engine/pathfinder/astar.cpp
+++ b/engine/pathfinder/astar.cpp
@@ -853,6 +853,15 @@ int AStarFindPath(int sx, int sy, int gx, int gy, int gw, int gh,
 			minrange, maxrange, path, pathlen, data);
 	if (i != PF_FAILED) {
 		ret = i;
+		// CloseSetSize may have been left from the previous call.
+		// Clear it so AStarWasSlow and AStarForEachTileInIsland
+		// will behave correctly.  This may take some time but
+		// that's OK because the next AStarCleanup will then be
+		// correspondingly faster.
+		if (i == PF_UNREACHABLE && CloseSetSize != 0) {
+			AStarCleanUp();
+			CloseSetSize = 0;
+		}
 		goto Cleanup;
 	}
 
@@ -1023,6 +1032,47 @@ Cleanup:
 	return ret;
 }
 
+/**
+**  Check whether the last AStarFindPath call took so much time
+**  that the caller should consider caching the the results.
+**
+**  This should be called only after AStarFindPath returns PF_UNREACHABLE.
+*/
+bool AStarWasSlow(void)
+{
+	return CloseSetSize >= Threshold;
+}
+
+/**
+**  Call the specified function for each map tile that A* examined
+**  before it decided the goal is unreachable.
+**
+**  This should be called only after AStarFindPath returns PF_UNREACHABLE.
+*/
+void AStarForEachTileInIsland(void (STDCALL *fn)(int x, int y, void *data),
+			      void *data)
+{
+	if (CloseSetSize < Threshold) {
+		for (int i = 0; i < CloseSetSize; i++) {
+			int node = CloseSet[i];
+			if (AStarMatrix[node].CostFromStart > 0) {
+				(*fn)(node % AStarMapWidth,
+				      node / AStarMapWidth,
+				      data);
+			}
+		}
+	} else {
+		for (int node = 0; node < AStarMapWidth * AStarMapHeight;
+		     node++) {
+			if (AStarMatrix[node].CostFromStart > 0) {
+				(*fn)(node % AStarMapWidth,
+				      node / AStarMapWidth,
+				      data);
+			}
+		}
+	}
+}
+
 struct StatsNode
 {
 	StatsNode() : Direction(0), InGoal(0), CostFromStart(0), Costs(0) {}
diff --git a/engine/pathfinder/pathfinder.cpp b/engine/pathfinder/pathfinder.cpp
index 4898826..06b7a0d 100644
--- a/engine/pathfinder/pathfinder.cpp
+++ b/engine/pathfinder/pathfinder.cpp
@@ -34,11 +34,15 @@
 --  Includes
 ----------------------------------------------------------------------------*/
 
+#include <algorithm>
 #include "stratagus.h"
 #include "map.h"
 #include "unittype.h"
 #include "unit.h"
 #include "pathfinder.h"
+#include "player.h"
+
+// #define VERIFY_ISLAND_CACHE
 
 /*----------------------------------------------------------------------------
 --  Variables
@@ -56,6 +60,36 @@ static unsigned char Matrix[(MaxMapWidth + 2) * (MaxMapHeight + 3) + 2];  /// Pa
 
 static int STDCALL CostMoveTo(int x, int y, void *data);
 
+class IslandCache
+{
+public:
+	int FindPath(const CUnit *src,
+		     int gx, int gy, int gw, int gh,
+		     int minrange, int maxrange,
+		     char *path, int pathlen);
+
+private:
+	typedef unsigned char IslandNumber;
+
+	IslandNumber Islands[(MaxMapWidth + 2) * (MaxMapHeight + 2)];
+
+	struct {
+		unsigned long GameCycle;
+		int PlayerIndex;
+		unsigned MovementMask;
+	} CacheKey;
+
+	bool MarkedAnythingThisCall;
+	IslandNumber IslandBeingMarked;
+	inline IslandNumber& IslandAt(int x, int y);
+	bool GoalMightBeInIsland(IslandNumber island,
+				 int gx, int gy, int gw, int gh,
+				 int maxrange);
+	static void STDCALL MarkIslandTile(int x, int y, void *self);
+};
+
+static class IslandCache IslandCache;
+
 /*----------------------------------------------------------------------------
 --  Functions
 ----------------------------------------------------------------------------*/
@@ -135,6 +169,157 @@ unsigned char *MakeMatrix(void)
 --  PATH-FINDER USE
 ----------------------------------------------------------------------------*/
 
+inline IslandCache::IslandNumber& IslandCache::IslandAt(int x, int y)
+{
+	Assert(x >= -1);
+	Assert(x <= Map.Info.MapWidth);
+	Assert(y >= -1);
+	Assert(y <= Map.Info.MapHeight);
+	return this->Islands[(y + 1) * (Map.Info.MapWidth + 2) + (x + 1)];
+}
+
+bool IslandCache::GoalMightBeInIsland(IslandNumber island,
+				      int gx, int gy, int gw, int gh,
+				      int maxrange)
+{
+
+	int xbeg = std::max(gx - maxrange, 0);
+	int ybeg = std::max(gy - maxrange, 0);
+	int xend = std::min(gx + std::max(gw, 1) + maxrange,
+			    Map.Info.MapWidth);
+	int yend = std::min(gy + std::max(gh, 1) + maxrange,
+			    Map.Info.MapHeight);
+
+	for (int y = ybeg; y < yend; y++) {
+		for (int x = xbeg; x < xend; x++) {
+			if (this->IslandAt(x, y) == island)
+				return true;
+		}
+	}
+
+	return false;
+}
+
+int IslandCache::FindPath(const CUnit *src,
+			  int gx, int gy, int gw, int gh,
+			  int minrange, int maxrange,
+			  char *path, int pathlen)
+{
+	// The island cache supports only 1x1 units for now.
+	if (src->Type->TileWidth != 1 || src->Type->TileHeight != 1) {
+		return AStarFindPath(src->X, src->Y, gx, gy, gw, gh,
+				     src->Type->TileWidth,
+				     src->Type->TileHeight,
+				     minrange, maxrange,
+				     path, pathlen,
+				     const_cast<CUnit *>(src));
+	}
+
+	bool keyOK = (this->CacheKey.GameCycle == GameCycle
+		      && this->CacheKey.PlayerIndex == src->Player->Index
+		      && this->CacheKey.MovementMask == src->Type->MovementMask);
+#ifdef VERIFY_ISLAND_CACHE
+	bool shouldBeUnreachable = false;
+#endif
+	if (keyOK) {
+		IslandNumber srcIslands[9];
+		int numSrcIslands = 0;
+
+		for (int heading = 0; heading < 8; ++heading) {
+			int x = src->X + Heading2X[heading];
+			int y = src->Y + Heading2Y[heading];
+			if (x < 0 || x >= Map.Info.MapWidth
+			    || y < 0 || y >= Map.Info.MapWidth)
+				continue; // out of map range
+			if (CostMoveTo(x, y, const_cast<CUnit *>(src)) == -1)
+				continue; // impassable tile
+			IslandNumber srcIsland = this->IslandAt(x, y);
+
+			// The following test can be omitted
+			// if testing shows that is faster.
+			if (srcIsland == 0)
+				goto MayBeReachable;
+
+			if (std::find(srcIslands, srcIslands + numSrcIslands,
+				      srcIsland) == srcIslands + numSrcIslands)
+				srcIslands[numSrcIslands++] = srcIsland;
+		}
+
+		for (int i = 0; i < numSrcIslands; ++i) {
+			if (this->GoalMightBeInIsland(srcIslands[i],
+						      gx, gy, gw, gh,
+						      maxrange))
+				goto MayBeReachable;
+		}
+
+		if (numSrcIslands == 0) {
+			// The unit cannot move at all.  However, if
+			// maxrange is large enough, then the unit may
+			// already be in the goal; we didn't call
+			// GoalMightBeInIsland, so we don't know.  Let
+			// AStarFindPath figure it out.  It will be
+			// fast because there is not much to check.
+			goto MayBeReachable;
+		}
+
+#ifdef VERIFY_ISLAND_CACHE
+		shouldBeUnreachable = true;
+#else
+		return PF_UNREACHABLE;
+#endif
+	}
+
+MayBeReachable:
+	int pf = AStarFindPath(src->X, src->Y, gx, gy, gw, gh,
+			       src->Type->TileWidth, src->Type->TileHeight,
+			       minrange, maxrange,
+			       path, pathlen,
+			       const_cast<CUnit *>(src));
+#ifdef VERIFY_ISLAND_CACHE
+	Assert(pf == PF_UNREACHABLE || !shouldBeUnreachable);
+#endif
+
+	// If we'd have to clear this->Islands with memset to cache
+	// the result, and AStarFindPath wasn't too slow in the first
+	// place, then don't bother caching it.
+	if (pf == PF_UNREACHABLE && (keyOK || AStarWasSlow())) {
+		if (!keyOK) {
+			memset(this->Islands, 0,
+			       Map.Info.MapWidth * Map.Info.MapHeight
+			       * sizeof(IslandNumber));
+			this->CacheKey.GameCycle = GameCycle;
+			this->CacheKey.PlayerIndex = src->Player->Index;
+			this->CacheKey.MovementMask = src->Type->MovementMask;
+		}
+
+		// If AStarMarkGoal decides the goal itself is unreachable
+		// (e.g. in the sea, where land units cannot move),
+		// then the close set is empty.
+		this->MarkedAnythingThisCall = false;
+		if (this->IslandBeingMarked == 0)
+			this->IslandBeingMarked = 1;
+		AStarForEachTileInIsland(MarkIslandTile, this);
+		if (this->MarkedAnythingThisCall) {
+			Assert(this->IslandAt(src->X, src->Y)
+			       == this->IslandBeingMarked);
+			// When IslandBeingMarked wraps around, no problem;
+			// some islands will then get the same number,
+			// and that may cause extra AStarFindPath calls,
+			// but the result will still be correct.
+			this->IslandBeingMarked++;
+		}
+	}
+
+	return pf;
+}
+
+void STDCALL IslandCache::MarkIslandTile(int x, int y, void *data)
+{
+	IslandCache *self = static_cast<IslandCache *>(data);
+	self->IslandAt(x, y) = self->IslandBeingMarked;
+	self->MarkedAnythingThisCall = true;
+}
+
 /**
 **  Can the unit 'src' reach the place x,y.
 **
@@ -150,9 +335,8 @@ unsigned char *MakeMatrix(void)
 */
 int PlaceReachable(const CUnit *src, int x, int y, int w, int h, int minrange, int range)
 {
-	int i = AStarFindPath(src->X, src->Y, x, y, w, h,
-		src->Type->TileWidth, src->Type->TileHeight, minrange, range, NULL, 0, (void *)src);
-
+	int i = IslandCache.FindPath(src, x, y, w, h, minrange, range,
+				     NULL, 0);
 	switch (i) {
 		case PF_FAILED:
 		case PF_UNREACHABLE:
@@ -318,8 +502,8 @@ int NewPath(CUnit *unit)
 		gy = unit->Orders[0]->Y;
 	}
 	path = unit->Data.Move.Path;
-	i = AStarFindPath(unit->X, unit->Y, gx, gy, gw, gh,
-		unit->Type->TileWidth, unit->Type->TileHeight, minrange, maxrange, path, MAX_PATH_LENGTH, unit);
+	i = IslandCache.FindPath(unit, gx, gy, gw, gh, minrange, maxrange,
+				 path, MAX_PATH_LENGTH);
 	if (i == PF_FAILED) {
 		i = PF_UNREACHABLE;
 	}
-- 
1.7.1

