Stablize sorts used by path finders (#1805)

* applied std:tie treat to priority queue for QTPFS
* add comment to avoid future confusion.

rts/Sim/Path/HAPFS/PathDataTypes.h

@@ -37,9 +37,13 @@ struct PathNode {
 
 
 /// functor to define node priority
+/// This needs to guarantee that the sorting is stable.
 struct lessCost {
-	inline bool operator() (const PathNode* x, const PathNode* y) const {
-		return (x->fCost == y->fCost) ? (x->gCost < y->gCost) : (x->fCost > y->fCost);
+	inline bool operator() (const PathNode* lhs, const PathNode* rhs) const {
+		// fCost == gCost + hCost.
+		// When fCosts are the same, prioritize the node closest the goal. Since we don't have hCost to hand, we know
+		// that hCost == fCost - gCost. This is why we invert the left/right side for the gCost comparison.
+		return std::tie(lhs->fCost, rhs->gCost, lhs->nodeNum) > std::tie(rhs->fCost, lhs->gCost, rhs->nodeNum);
 	}
 };
 

rts/Sim/Path/QTPFS/PathThreads.h

@@ -90,12 +90,6 @@ namespace QTPFS {
     };
 
     struct SearchQueueNode {
-		bool operator <  (const SearchQueueNode& n) const { return (heapPriority <  n.heapPriority); }
-		bool operator >  (const SearchQueueNode& n) const { return (heapPriority >  n.heapPriority); }
-		bool operator == (const SearchQueueNode& n) const { return (heapPriority == n.heapPriority); }
-		bool operator <= (const SearchQueueNode& n) const { return (heapPriority <= n.heapPriority); }
-		bool operator >= (const SearchQueueNode& n) const { return (heapPriority >= n.heapPriority); }
-
         SearchQueueNode(int index, float newPriorty)
             : heapPriority(newPriorty)
             , nodeIndex(index)
@@ -105,9 +99,18 @@ namespace QTPFS {
         int nodeIndex;
     };
 
+    /// Functor to define node priority.
+    /// Needs to guarantee stable ordering, even if the sorting algorithm itself is not stable.
+    struct ShouldMoveTowardsBottomOfPriorityQueue {
+        inline bool operator() (const SearchQueueNode& lhs, const SearchQueueNode& rhs) const {
+            return std::tie(lhs.heapPriority, lhs.nodeIndex) > std::tie(rhs.heapPriority, rhs.nodeIndex);
+        }
+    };
+
+
     // Reminder that std::priority does comparisons to push element back to the bottom. So using
-    // std::greater here means the smallest value will be top()
-    typedef std::priority_queue<SearchQueueNode, std::vector<SearchQueueNode>, std::greater<SearchQueueNode>> SearchPriorityQueue;
+    // ShouldMoveTowardsBottomOfPriorityQueue here means the smallest value will be top()
+    typedef std::priority_queue<SearchQueueNode, std::vector<SearchQueueNode>, ShouldMoveTowardsBottomOfPriorityQueue> SearchPriorityQueue;
 
 	struct SearchThreadData {