Fix rail pathfinding with YAPF

Partial revert of 30e1a61c04.

See: https://github.com/OpenTTD/OpenTTD/issues/12250

src/pathfinder/yapf/yapf_base.hpp

@@ -117,8 +117,8 @@ public:
 			Node *best_open_node = m_nodes.GetBestOpenNode();
 			if (best_open_node == nullptr) break;
 
-			if (Yapf().PfDetectDestination(*best_open_node)) {
-				m_pBestDestNode = best_open_node;
+			/* if the best open node was worse than the best path found, we can finish */
+			if (m_pBestDestNode != nullptr && m_pBestDestNode->GetCost() < best_open_node->GetCostEstimate()) {
 				break;
 			}
 
@@ -246,6 +246,16 @@ public:
 		/* have the cost or estimate callbacks marked this node as invalid? */
 		if (!bValid) return;
 
+		/* detect the destination */
+		bool bDestination = Yapf().PfDetectDestination(n);
+		if (bDestination) {
+			if (m_pBestDestNode == nullptr || n < *m_pBestDestNode) {
+				m_pBestDestNode = &n;
+			}
+			m_nodes.FoundBestNode(n);
+			return;
+		}
+
 		/* The new node can be set as the best intermediate node only once we're
 		 * certain it will be finalized by being inserted into the open list. */
 		bool set_intermediate = m_max_search_nodes > 0 && (m_pBestIntermediateNode == nullptr || (m_pBestIntermediateNode->GetCostEstimate() - m_pBestIntermediateNode->GetCost()) > (n.GetCostEstimate() - n.GetCost()));