CPathfinder: implement priority queue and node locking

lib/CPathfinder.cpp

@@ -84,12 +84,13 @@ void CPathfinder::calculatePaths()
 	CGPathNode *initialNode = out.getNode(out.hpos, hero->boat ? EPathfindingLayer::SAIL : EPathfindingLayer::LAND);
 	initialNode->turns = 0;
 	initialNode->moveRemains = hero->movement;
-	mq.push_back(initialNode);
+	pq.push(initialNode);
 
-	while(!mq.empty())
+	while(!pq.empty())
 	{
-		cp = mq.front();
-		mq.pop_front();
+		cp = pq.top();
+		pq.pop();
+		cp->locked = true;
 
 		int movement = cp->moveRemains, turn = cp->turns;
 		if(!movement)
@@ -110,6 +111,9 @@ void CPathfinder::calculatePaths()
 				if(dp->accessible == CGPathNode::NOT_SET)
 					continue;
 
+				if(dp->locked)
+					continue;
+
 				if(cp->layer != i && !isLayerTransitionPossible())
 					continue;
 
@@ -142,7 +146,7 @@ void CPathfinder::calculatePaths()
 					dp->theNodeBefore = cp;
 
 					if(isMovementAfterDestPossible())
-						mq.push_back(dp);
+						pq.push(dp);
 				}
 			}
 		} //neighbours loop
@@ -154,12 +158,15 @@ void CPathfinder::calculatePaths()
 			for(auto & neighbour : neighbours)
 			{
 				dp = out.getNode(neighbour, cp->layer);
+				if(dp->locked)
+					continue;
+
 				if(isBetterWay(movement, turn))
 				{
 					dp->moveRemains = movement;
 					dp->turns = turn;
 					dp->theNodeBefore = cp;
-					mq.push_back(dp);
+					pq.push(dp);
 				}
 			}
 		}
@@ -386,6 +393,7 @@ void CPathfinder::initializeGraph()
 	auto updateNode = [&](int3 pos, EPathfindingLayer layer, const TerrainTile *tinfo)
 	{
 		auto node = out.getNode(pos, layer);
+		node->locked = false;
 		node->accessible = evaluateAccessibility(pos, tinfo);
 		node->turns = 0xff;
 		node->moveRemains = 0;
@@ -513,6 +521,7 @@ bool CPathfinder::canVisitObject() const
 CGPathNode::CGPathNode()
 	: coord(-1,-1,-1)
 {
+	locked = false;
 	accessible = NOT_SET;
 	land = 0;
 	moveRemains = 0;

lib/CPathfinder.h

@@ -5,6 +5,8 @@
 #include "IGameCallback.h"
 #include "int3.h"
 
+#include <boost/heap/priority_queue.hpp>
+
 /*
  * CPathfinder.h, part of VCMI engine
  *
@@ -30,6 +32,7 @@ struct DLL_LINKAGE CGPathNode
 		BLOCKED //tile can't be entered nor visited
 	};
 
+	bool locked;
 	EAccessibility accessible;
 	ui8 land;
 	ui8 turns; //how many turns we have to wait before reachng the tile - 0 means current turn
@@ -96,7 +99,19 @@ private:
 	CPathsInfo &out;
 	const CGHeroInstance *hero;
 
-	std::list<CGPathNode*> mq; //BFS queue -> nodes to be checked
+	struct NodeComparer
+	{
+		bool operator()(const CGPathNode * lhs, const CGPathNode * rhs) const
+		{
+			if(rhs->turns > lhs->turns)
+				return false;
+			else if(rhs->turns == lhs->turns && rhs->moveRemains < lhs->moveRemains)
+				return false;
+
+			return true;
+		}
+	};
+	boost::heap::priority_queue<CGPathNode *, boost::heap::compare<NodeComparer> > pq;
 
 	std::vector<int3> neighbours;