diff --git a/src/pathfinder/astar.cpp b/src/pathfinder/astar.cpp
index 7932fdcea..996bbca12 100644
--- a/src/pathfinder/astar.cpp
+++ b/src/pathfinder/astar.cpp
@@ -263,6 +263,7 @@ void InitAStar(int mapWidth, int mapHeight)
 	memset(AStarMatrix, 0, AStarMatrixSize);
 
 	Threshold = AStarMapWidth * AStarMapHeight / MAX_CLOSE_SET_RATIO;
+	CloseSetSize = Threshold;
 	CloseSet = new int[Threshold];
 
 	OpenSetMaxSize = AStarMapWidth * AStarMapHeight / MAX_OPEN_SET_RATIO;
@@ -307,16 +308,19 @@ static void AStarPrepare()
 /**
 **  Clean up A*
 */
+static void CostMoveToCacheCleanUp();
 static void AStarCleanUp()
 {
 	ProfileBegin("AStarCleanUp");
 
 	if (CloseSetSize >= Threshold) {
 		AStarPrepare();
+		CostMoveToCacheCleanUp();
 	} else {
 		for (int i = 0; i < CloseSetSize; ++i) {
 			AStarMatrix[CloseSet[i]].CostFromStart = 0;
 			AStarMatrix[CloseSet[i]].InGoal = 0;
+			CostMoveToCache[CloseSet[i]] = CacheNotSet;
 		}
 	}
 	ProfileEnd("AStarCleanUp");
@@ -936,7 +940,6 @@ int AStarFindPath(const Vec2i &startPos, const Vec2i &goalPos, int gw, int gh,
 
 	//  Initialize
 	AStarCleanUp();
-	CostMoveToCacheCleanUp();
 
 	OpenSetSize = 0;
 	CloseSetSize = 0;