YAPF: Reduce need to scan open list queue when moving best node to closed list

src/pathfinder/yapf/nodelist.hpp

@@ -110,6 +110,22 @@ public:
 		return nullptr;
 	}
 
+	inline void DequeueBestOpenNode()
+	{
+		assert(!m_open_queue.IsEmpty());
+		m_open_queue.Shift();
+	}
+
+	inline void ReenqueueOpenNode(Titem_ &item)
+	{
+		m_open_queue.Include(&item);
+	}
+
+	inline Titem_& PopAlreadyDequeuedOpenNode(const Key &key)
+	{
+		return m_open.Pop(key);
+	}
+
 	/** return the open node specified by a key or nullptr if not found */
 	inline Titem_ *FindOpenNode(const Key &key)
 	{

src/pathfinder/yapf/yapf_base.hpp

@@ -140,11 +140,13 @@ public:
 				break;
 			}
 
+			m_nodes.DequeueBestOpenNode();
 			Yapf().PfFollowNode(*n);
 			if (m_max_search_nodes == 0 || m_nodes.ClosedCount() < m_max_search_nodes) {
-				m_nodes.PopOpenNode(n->GetKey());
+				m_nodes.PopAlreadyDequeuedOpenNode(n->GetKey());
 				m_nodes.InsertClosedNode(*n);
 			} else {
+				m_nodes.ReenqueueOpenNode(*n);
 				bDestFound = false;
 				break;
 			}