Fix issues with units getting stuck when given unreachable pathing go…

…als.

rts/Sim/MoveTypes/GroundMoveType.cpp

@@ -2038,24 +2038,28 @@ bool CGroundMoveType::CanSetNextWayPoint(int thread) {
 
 		}
 
-		const float searchRadius = std::max(WAYPOINT_RADIUS, currentSpeed * 1.05f);
-		const float3 targetPos = nwp;
-
-		// check the between pos and cwp for obstacles
-		// if still further than SS elmos from waypoint, disallow skipping
-		const bool rangeTest = owner->moveDef->DoRawSearch(owner, pos, targetPos, owner->speed, true, true, true, nullptr, nullptr, thread);
-
-		// {
-		// bool printMoveInfo = (selectedUnitsHandler.selectedUnits.size() == 1)
-		// 	&& (selectedUnitsHandler.selectedUnits.find(owner->id) != selectedUnitsHandler.selectedUnits.end());
-		// if (printMoveInfo)
-		// 	LOG("%s: test move square (%f,%f)->(%f,%f) = %f (range=%d skip=%d)", __func__
-		// 			, pos.x, pos.z, targetPos.x, targetPos.z, math::sqrtf(cwpDistSq)
-		// 			, int(rangeTest), int(allowSkip));
-		// }
+		// The last waypoint on a bad path will never pass a range check so don't try.
+		bool doRangeCheck = !pathManager->NextWayPointIsUnreachable(pathID);
+		if (doRangeCheck) {
+			const float searchRadius = std::max(WAYPOINT_RADIUS, currentSpeed * 1.05f);
+			const float3 targetPos = nwp;
+
+			// check the between pos and cwp for obstacles
+			// if still further than SS elmos from waypoint, disallow skipping
+			const bool rangeTest = owner->moveDef->DoRawSearch(owner, pos, targetPos, owner->speed, true, true, true, nullptr, nullptr, thread);
+
+			// {
+			// bool printMoveInfo = (selectedUnitsHandler.selectedUnits.size() == 1)
+			// 	&& (selectedUnitsHandler.selectedUnits.find(owner->id) != selectedUnitsHandler.selectedUnits.end());
+			// if (printMoveInfo)
+			// 	LOG("%s: test move square (%f,%f)->(%f,%f) = %f (range=%d skip=%d)", __func__
+			// 			, pos.x, pos.z, targetPos.x, targetPos.z, math::sqrtf(cwpDistSq)
+			// 			, int(rangeTest), int(allowSkip));
+			// }
 
-		if (!rangeTest)
-			return false;
+			if (!rangeTest)
+				return false;
+		}
 	}
 
 	{
@@ -2333,7 +2337,9 @@ bool CGroundMoveType::HandleStaticObjectCollision(
 	// while being built, units that overlap their factory yardmap should not be moved at all
 	assert(!collider->beingBuilt);
 
-	if (checkTerrain && (!collider->IsMoving() || collider->IsInAir()))
+	// Even units standing still can be pushed into terrain and so need to be able to be pushed
+	// back out.
+	if (checkTerrain && (/*!collider->IsMoving() ||*/ collider->IsInAir()))
 		return false;
 
 	// for factories, check if collidee's position is behind us (which means we are likely exiting)
@@ -3071,6 +3077,10 @@ void CGroundMoveType::UpdateOwnerPos(const float3& oldSpeedVector, const float3&
 									&& !isTerrainSquareOpen(owner->pos);
 		bool isSquareBlocked = isTerrainSquareBlocked || !isObjectsSquareOpen(owner->pos);
 		if (isSquareBlocked) {
+			// Sometimes now collisions won't happen due to this code preventing that.
+			// so units need to be able to get themselves out of stuck situations. So adding
+			// a rerequest path check here.
+			ReRequestPath(false);
 			bool updatePos = false;
 
 			// This attempts to slide units around obstructions. The effect is reduced around

rts/Sim/Path/IPathManager.h

@@ -82,6 +82,7 @@ class IPathManager {
 	}
 
 	virtual bool CurrentWaypointIsUnreachable(unsigned int pathID) { return false; }
+	virtual bool NextWayPointIsUnreachable(unsigned int pathID) { return false; }
 
 
 	/**

rts/Sim/Path/QTPFS/PathManager.cpp

@@ -1315,7 +1315,12 @@ float3 QTPFS::PathManager::NextWayPoint(
 	}
 
 	unsigned int nextPointIndex = livePath->GetNextPointIndex() + 1;
-	livePath->SetNextPointIndex(nextPointIndex);
+	unsigned int lastPointIndex = livePath->NumPoints() - 1;
+	if (nextPointIndex > lastPointIndex) {
+		nextPointIndex = lastPointIndex;
+	} else {
+		livePath->SetNextPointIndex(nextPointIndex);
+	}
 	return livePath->GetPoint(nextPointIndex);
 }
 
@@ -1337,6 +1342,22 @@ bool QTPFS::PathManager::CurrentWaypointIsUnreachable(unsigned int pathID) {
 }
 
 
+bool QTPFS::PathManager::NextWayPointIsUnreachable(unsigned int pathID) {
+	entt::entity pathEntity = entt::entity(pathID);
+	if (!registry.valid(pathEntity))
+		return true;
+
+	IPath* livePath = registry.try_get<IPath>(pathEntity);
+	if (livePath == nullptr)
+		return true;
+
+	unsigned int lastWaypoint = livePath->NumPoints() - 1;
+	unsigned int nextWaypoint = livePath->GetNextPointIndex() + 1;
+
+	return ( nextWaypoint >= lastWaypoint ) && ( !livePath->IsFullPath() );
+}
+
+
 void QTPFS::PathManager::GetPathWayPoints(
 	unsigned int pathID,
 	std::vector<float3>& points,

rts/Sim/Path/QTPFS/PathManager.h

@@ -71,6 +71,7 @@ namespace QTPFS {
 		) override;
 
 		bool CurrentWaypointIsUnreachable(unsigned int pathID) override;
+		bool NextWayPointIsUnreachable(unsigned int pathID) override;
 
 		void GetPathWayPoints(
 			unsigned int pathID,

rts/Sim/Path/QTPFS/PathSearch.cpp

@@ -487,19 +487,17 @@ bool QTPFS::PathSearch::ExecutePathSearch() {
 		}
 	}
 
-	havePartPath = (fwd.minSearchNode != fwd.srcSearchNode);
+	havePartPath = (fwd.minSearchNode != fwd.srcSearchNode) | (badGoal & haveFullPath);
+	haveFullPath = (badGoal == true) ? false : haveFullPath;
 
 	#ifdef QTPFS_SUPPORT_PARTIAL_SEARCHES
 	// adjust the target-point if we only got a partial result
 	// NOTE:
-	//   should adjust GMT::goalPos accordingly, otherwise
-	//   units will end up spinning in-place over the last
-	//   waypoint (since "atGoal" can never become true)
+	//   movement code has handles special cases where the last waypoint isn't reachable so there
+	//   is no need to change the target point anymore.
 	if (!haveFullPath && havePartPath) {
 		fwd.tgtSearchNode = fwd.minSearchNode;
 		auto* minNode = nodeLayer->GetPoolNode(fwd.minSearchNode->GetIndex());
-		fwd.tgtPoint.x = minNode->xmid() * SQUARE_SIZE;
-		fwd.tgtPoint.z = minNode->zmid() * SQUARE_SIZE;
 
 		// used to trace a bad path to give partial searches a chance of an early out
 		// in reverse searches.
@@ -751,8 +749,8 @@ void QTPFS::PathSearch::Finalize(IPath* path) {
 	}
 
 	path->SetBoundingBox();
-	path->SetHasFullPath(haveFullPath & !badGoal);
-	path->SetHasPartialPath(havePartPath | (haveFullPath & badGoal));
+	path->SetHasFullPath(haveFullPath);
+	path->SetHasPartialPath(havePartPath);
 }
 
 void QTPFS::PathSearch::TracePath(IPath* path) {
@@ -767,7 +765,6 @@ void QTPFS::PathSearch::TracePath(IPath* path) {
 	auto& fwd = directionalSearchData[SearchThreadData::SEARCH_FORWARD];
 	auto& bwd = directionalSearchData[SearchThreadData::SEARCH_BACKWARD];
 
-	//if (fwd.srcSearchNode != fwd.tgtSearchNode || bwd.srcSearchNode != bwd.tgtSearchNode)
 	{
 		// Only a bad path will be associated with the reverse path
 		if (!haveFullPath)
@@ -806,7 +803,6 @@ void QTPFS::PathSearch::TracePath(IPath* path) {
 				//   one exception: tgtPoint can legitimately coincide
 				//   with first transition-point, which we must ignore
 				assert(tmpNode != prvNode);
-
 				assert(prvPoint != float3());
 
 				// This check is not helpful. In bigger maps this triggers for points on small (1x1) quads
@@ -850,7 +846,6 @@ void QTPFS::PathSearch::TracePath(IPath* path) {
 			//   one exception: tgtPoint can legitimately coincide
 			//   with first transition-point, which we must ignore
 			assert(tmpNode != prvNode);
-
 			assert(tmpPoint != float3());
 
 			// assert(tmpPoint != prvPoint || tmpNode == fwd.tgtSearchNode || doPartialSearch);
@@ -873,6 +868,7 @@ void QTPFS::PathSearch::TracePath(IPath* path) {
 			prvNode = tmpNode->GetPrevNode();
 		}
 
+		// ensure the starting quad is shared with other path searches.
 		if (tmpNode != nullptr) {
 			points.emplace_front(float3(), tmpNode->GetIndex() | ONLY_NODE_ID_MASK);
 			// LOG("%s: [%d] tgtNode=%d point ", __func__, SearchThreadData::SEARCH_FORWARD