fix(pathfinder): Implement robust forward insertion sort for PathfindCell::putOnSortedOpenList() (TheSuperHackers#2432)

Author: Mauller

Core/GameEngine/Include/GameLogic/AIPathfind.h

@@ -326,6 +326,14 @@ class PathfindCell
 
 	UnsignedInt costSoFar( PathfindCell *parent );
 
+#if RETAIL_COMPATIBLE_PATHFINDING
+	// Forward insertion sort that is 100% retail compatible
+	void forwardInsertionSortRetailCompatible(PathfindCellList& list);
+#endif
+
+	// Forward insertion sort, in ascending cost order
+	void forwardInsertionSort(PathfindCellList& list);
+
 	/// put self on "open" list in ascending cost order
 	void putOnSortedOpenList( PathfindCellList &list );
 

Core/GameEngine/Source/GameLogic/AI/AIPathfind.cpp

@@ -1680,8 +1680,9 @@ Bool PathfindCell::removeObstacle( Object *obstacle )
 	return true;
 }
 
-/// put self on "open" list in ascending cost order, return new list
-void PathfindCell::putOnSortedOpenList( PathfindCellList &list )
+#if RETAIL_COMPATIBLE_PATHFINDING
+// Retail compatible insertion sort
+void PathfindCell::forwardInsertionSortRetailCompatible(PathfindCellList& list)
 {
 	DEBUG_ASSERTCRASH(m_info, ("Has to have info."));
 	DEBUG_ASSERTCRASH(m_info->m_closed == FALSE && m_info->m_open == FALSE, ("Serious error - Invalid flags. jba"));
@@ -1701,18 +1702,10 @@ void PathfindCell::putOnSortedOpenList( PathfindCellList &list )
 	// insertion sort
 	PathfindCell* currentCell = list.m_head;
 	PathfindCell* previousCell = nullptr;
-#if RETAIL_COMPATIBLE_PATHFINDING
-	// TheSuperHackers @bugfix In the retail compatible pathfinding, on rare occasions, we get stuck in an infinite loop
-	// External code should pickup on the bad behaviour and cleanup properly, but we need to explicitly break out here
-	// The fixed pathfinding does not have this issue due to the proper cleanup of pathfindCells and their pathfindCellInfos
 	UnsignedInt cellCount = 0;
 	while (currentCell && cellCount < PATHFIND_CELLS_PER_FRAME && currentCell->m_info->m_totalCost <= m_info->m_totalCost)
 	{
 		cellCount++;
-#else
-	while (currentCell && currentCell->m_info->m_totalCost <= m_info->m_totalCost)
-	{
-#endif
 		previousCell = currentCell;
 		currentCell = currentCell->getNextOpen();
 	}
@@ -1739,6 +1732,61 @@ void PathfindCell::putOnSortedOpenList( PathfindCellList &list )
 		m_info->m_nextOpen = nullptr;
 	}
 }
+#endif
+
+// Forward insertion sort, returns early if the list is being initialised or we are prepending the list
+void PathfindCell::forwardInsertionSort(PathfindCellList& list)
+{
+	DEBUG_ASSERTCRASH(m_info, ("Has to have info."));
+	DEBUG_ASSERTCRASH(m_info->m_closed == FALSE && m_info->m_open == FALSE, ("Serious error - Invalid flags. jba"));
+
+	// mark the new cell as being on the open list
+	m_info->m_open = true;
+	m_info->m_closed = false;
+
+	if (list.m_head == nullptr) {
+		m_info->m_prevOpen = nullptr;
+		m_info->m_nextOpen = nullptr;
+		list.m_head = this;
+		return;
+	}
+
+	// If the node needs inserting before the current list head
+	if (m_info->m_totalCost < list.m_head->m_info->m_totalCost) {
+		m_info->m_prevOpen = nullptr;
+		list.m_head->m_info->m_prevOpen = this->m_info;
+		m_info->m_nextOpen = list.m_head->m_info;
+		list.m_head = this;
+		return;
+	}
+
+	// Traverse the list to find correct position
+	PathfindCell* current = list.m_head;
+	while (current->m_info->m_nextOpen && current->m_info->m_nextOpen->m_totalCost <= m_info->m_totalCost) {
+		current = current->getNextOpen();
+	}
+
+	// Insert the new node in the correct position
+	m_info->m_nextOpen = current->m_info->m_nextOpen;
+	if (current->m_info->m_nextOpen != nullptr) {
+		current->m_info->m_nextOpen->m_prevOpen = this->m_info;
+	}
+	current->m_info->m_nextOpen = this->m_info;
+	m_info->m_prevOpen = current->m_info;
+}
+
+/// put self on "open" list in ascending cost order, return new list
+void PathfindCell::putOnSortedOpenList( PathfindCellList &list )
+{
+#if RETAIL_COMPATIBLE_PATHFINDING
+	if (!s_useFixedPathfinding) {
+		forwardInsertionSortRetailCompatible(list);
+		return;
+	}
+#endif
+
+	forwardInsertionSort(list);
+}
 
 /// remove self from "open" list
 void PathfindCell::removeFromOpenList( PathfindCellList &list )