Fix PQ issues while calculating paths

AI/VCAI/AIUtility.cpp

@@ -191,7 +191,7 @@ bool CDistanceSorter::operator()(const CGObjectInstance * lhs, const CGObjectIns
 	const CGPathNode * ln = ai->myCb->getPathsInfo(hero)->getPathInfo(lhs->visitablePos());
 	const CGPathNode * rn = ai->myCb->getPathsInfo(hero)->getPathInfo(rhs->visitablePos());
 
-	return ln->cost < rn->cost;
+	return ln->getCost() < rn->getCost();
 }
 
 bool isSafeToVisit(HeroPtr h, crint3 tile)

AI/VCAI/FuzzyEngines.cpp

@@ -98,7 +98,7 @@ float HeroMovementGoalEngineBase::calculateTurnDistanceInputValue(const Goals::A
 	else
 	{
 		auto pathInfo = ai->myCb->getPathsInfo(goal.hero.h)->getPathInfo(goal.tile);
-		return pathInfo->cost;
+		return pathInfo->getCost();
 	}
 }
 

AI/VCAI/Pathfinding/AINodeStorage.cpp

@@ -119,7 +119,7 @@ CGPathNode * AINodeStorage::getInitialNode()
 	initialNode->turns = 0;
 	initialNode->moveRemains = hero->movement;
 	initialNode->danger = 0;
-	initialNode->cost = 0.0;
+	initialNode->setCost(0.0);
 
 	return initialNode;
 }
@@ -146,7 +146,7 @@ void AINodeStorage::commit(CDestinationNodeInfo & destination, const PathNodeInf
 	{
 		dstNode->moveRemains = destination.movementLeft;
 		dstNode->turns = destination.turn;
-		dstNode->cost = destination.cost;
+		dstNode->setCost(destination.cost);
 		dstNode->danger = srcNode->danger;
 		dstNode->action = destination.action;
 		dstNode->theNodeBefore = srcNode->theNodeBefore;
@@ -305,7 +305,7 @@ bool AINodeStorage::hasBetterChain(const PathNodeInfo & source, CDestinationNode
 
 		if(node.danger <= destinationNode->danger && destinationNode->chainMask == 1 && node.chainMask == 0)
 		{
-			if(node.cost < destinationNode->cost)
+			if(node.getCost() < destinationNode->getCost())
 			{
 #ifdef VCMI_TRACE_PATHFINDER
 				logAi->trace(
@@ -349,7 +349,7 @@ std::vector<AIPath> AINodeStorage::getChainInfo(const int3 & pos, bool isOnLand)
 		while(current != nullptr && current->coord != initialPos)
 		{
 			AIPathNodeInfo pathNode;
-			pathNode.cost = current->cost;
+			pathNode.cost = current->getCost();
 			pathNode.turns = current->turns;
 			pathNode.danger = current->danger;
 			pathNode.coord = current->coord;

lib/CPathfinder.cpp

@@ -161,7 +161,7 @@ CGPathNode * NodeStorage::getInitialNode()
 
 	initialNode->turns = 0;
 	initialNode->moveRemains = out.hero->movement;
-	initialNode->cost = 0.0;
+	initialNode->setCost(0.0);
 
 	return initialNode;
 }
@@ -169,7 +169,7 @@ CGPathNode * NodeStorage::getInitialNode()
 void NodeStorage::commit(CDestinationNodeInfo & destination, const PathNodeInfo & source)
 {
 	assert(destination.node != source.node->theNodeBefore); //two tiles can't point to each other
-	destination.node->cost = destination.cost;
+	destination.node->setCost(destination.cost);
 	destination.node->moveRemains = destination.movementLeft;
 	destination.node->turns = destination.turn;
 	destination.node->theNodeBefore = source.node;
@@ -289,6 +289,28 @@ CPathfinder::CPathfinder(
 	initializeGraph();
 }
 
+
+void CPathfinder::push(CGPathNode * node)
+{
+	if(node && !node->inPQ)
+	{
+		node->inPQ = true;
+		node->pq = &this->pq;
+		auto handle = pq.push(node);
+		node->pqHandle = handle;
+	}
+}
+
+CGPathNode * CPathfinder::topAndPop()
+{
+	auto node = pq.top();
+
+	pq.pop();
+	node->inPQ = false;
+	node->pq = nullptr;
+	return node;
+}
+
 void CPathfinder::calculatePaths()
 {
 	//logGlobal->info("Calculating paths for hero %s (adress  %d) of player %d", hero->name, hero , hero->tempOwner);
@@ -305,19 +327,19 @@ void CPathfinder::calculatePaths()
 	if(isHeroPatrolLocked())
 		return;
 
-	pq.push(initialNode);
+	push(initialNode);
+
 	while(!pq.empty())
 	{
-		auto node = pq.top();
+		auto node = topAndPop();
 		auto excludeOurHero = node->coord == initialNode->coord;
 
 		source.setNode(gs, node, excludeOurHero);
-		pq.pop();
 		source.node->locked = true;
 
 		int movement = source.node->moveRemains;
 		uint8_t turn = source.node->turns;
-		float cost = source.node->cost;
+		float cost = source.node->getCost();
 
 		hlp->updateTurnInfo(turn);
 		if(!movement)
@@ -368,7 +390,7 @@ void CPathfinder::calculatePaths()
 			}
 
 			if(!destination.blocked)
-				pq.push(destination.node);
+				push(destination.node);
 
 		} //neighbours loop
 
@@ -403,7 +425,7 @@ void CPathfinder::calculatePaths()
 				config->nodeStorage->commit(destination, source);
 
 				if(destination.node->action == CGPathNode::TELEPORT_NORMAL)
-					pq.push(destination.node);
+					push(destination.node);
 			}
 		}
 	} //queue loop
@@ -1367,7 +1389,7 @@ bool CDestinationNodeInfo::isBetterWay() const
 	if(node->turns == 0xff) //we haven't been here before
 		return true;
 	else
-		return cost < node->cost; //this route is faster
+		return cost < node->getCost(); //this route is faster
 }
 
 bool PathNodeInfo::isNodeObjectVisitable() const

lib/CPathfinder.h

@@ -14,7 +14,8 @@
 #include "HeroBonus.h"
 #include "int3.h"
 
-#include <boost/heap/priority_queue.hpp>
+#include <boost/heap/fibonacci_heap.hpp>
+
 
 class CGHeroInstance;
 class CGObjectInstance;
@@ -26,6 +27,17 @@ class CPathfinderHelper;
 class CPathfinder;
 class PathfinderConfig;
 
+
+template<typename N>
+struct DLL_LINKAGE NodeComparer
+{
+	STRONG_INLINE
+	bool operator()(const N * lhs, const N * rhs) const
+	{
+		return lhs->getCost() > rhs->getCost();
+	}
+};
+
 struct DLL_LINKAGE CGPathNode
 {
 	typedef EPathfindingLayer ELayer;
@@ -58,16 +70,17 @@ struct DLL_LINKAGE CGPathNode
 	int3 coord; //coordinates
 	ELayer layer;
 	ui32 moveRemains; //remaining movement points after hero reaches the tile
-	float cost; //total cost of the path to this tile measured in turns with fractions
 	ui8 turns; //how many turns we have to wait before reaching the tile - 0 means current turn
 
 	EAccessibility accessible;
 	ENodeAction action;
 	bool locked;
+	bool inPQ;
 
 	CGPathNode()
 		: coord(-1),
-		layer(ELayer::WRONG)
+		layer(ELayer::WRONG),
+		pqHandle(nullptr)
 	{
 		reset();
 	}
@@ -82,6 +95,36 @@ struct DLL_LINKAGE CGPathNode
 		turns = 255;
 		theNodeBefore = nullptr;
 		action = UNKNOWN;
+		inPQ = false;
+		pq = nullptr;
+	}
+
+	STRONG_INLINE
+	float getCost() const
+	{
+		return cost;
+	}
+
+	STRONG_INLINE
+	void setCost(float value)
+	{
+		if(value == cost)
+			return;
+
+		bool getUpNode = value < cost;
+		cost = value;
+		// If the node is in the heap, update the heap.
+		if(inPQ && pq != nullptr)
+		{
+			if(getUpNode)
+			{
+				pq->increase(this->pqHandle, this);
+			}
+			else
+			{
+				pq->decrease(this->pqHandle, this);
+			}
+		}
 	}
 
 	STRONG_INLINE
@@ -105,6 +148,26 @@ struct DLL_LINKAGE CGPathNode
 	{
 		return turns < 255;
 	}
+
+	boost::heap::detail::node_handle
+	<
+		boost::heap::detail::marked_heap_node<CGPathNode *>*,
+		boost::heap::detail::make_fibonacci_heap_base
+		<
+			CGPathNode *,
+			boost::parameter::aux::arg_list
+			<
+				boost::heap::compare<NodeComparer<CGPathNode>>,
+				boost::parameter::aux::empty_arg_list
+			>
+		>::type,
+		CGPathNode *&
+	> pqHandle;
+
+	boost::heap::fibonacci_heap< CGPathNode *, boost::heap::compare<NodeComparer<CGPathNode>> >* pq;
+
+private:
+	float cost; //total cost of the path to this tile measured in turns with fractions
 };
 
 struct DLL_LINKAGE CGPath
@@ -415,15 +478,7 @@ private:
 	} patrolState;
 	std::unordered_set<int3, ShashInt3> patrolTiles;
 
-	struct NodeComparer
+	boost::heap::fibonacci_heap<CGPathNode *, boost::heap::compare<NodeComparer<CGPathNode>> > pq;
-	{
-		STRONG_INLINE
-		bool operator()(const CGPathNode * lhs, const CGPathNode * rhs) const
-		{
-			return lhs->cost > rhs->cost;
-		}
-	};
-	boost::heap::priority_queue<CGPathNode *, boost::heap::compare<NodeComparer> > pq;
 
 	PathNodeInfo source; //current (source) path node -> we took it from the queue
 	CDestinationNodeInfo destination; //destination node -> it's a neighbour of source that we consider
@@ -441,6 +496,12 @@ private:
 
 	void initializePatrol();
 	void initializeGraph();
+
+	STRONG_INLINE
+	void push(CGPathNode * node);
+
+	STRONG_INLINE
+	CGPathNode * topAndPop();
 };
 
 struct DLL_LINKAGE TurnInfo