Stop units trying to pushing into buildings and terrain when they can…

…'t get any closer to the goal.

rts/Sim/MoveTypes/GroundMoveType.cpp

@@ -2142,9 +2142,9 @@ bool CGroundMoveType::CanSetNextWayPoint(int thread) {
 		// bool unitIsBetweenWaypoints = (v0.dot(v1) <= -0.f);
 
 		// The last waypoint on a bad path will never pass a range check so don't try.
-		bool doRangeCheck = !pathManager->NextWayPointIsUnreachable(pathID);
+		//bool doRangeCheck = !pathManager->NextWayPointIsUnreachable(pathID);
 						// && !unitIsBetweenWaypoints;
-		if (doRangeCheck) {
+		//if (doRangeCheck) {
 			const float searchRadius = std::max(WAYPOINT_RADIUS, currentSpeed * 1.05f);
 			const float3 targetPos = nwp;
 
@@ -2163,7 +2163,7 @@ bool CGroundMoveType::CanSetNextWayPoint(int thread) {
 
 			if (!rangeTest)
 				return false;
-		}
+		//}
 	}
 
 	{
@@ -2202,7 +2202,7 @@ void CGroundMoveType::SetNextWayPoint(int thread)
 			bool printMoveInfo = (selectedUnitsHandler.selectedUnits.size() == 1)
 				&& (selectedUnitsHandler.selectedUnits.find(owner->id) != selectedUnitsHandler.selectedUnits.end());
 			if (printMoveInfo) {
-				LOG("%s setting next waypoint", __func__);
+				LOG("%s setting next waypoint (%d:%d)", __func__, owner->id, owner->moveDef->pathType);
 			}
 		}
 		#endif
@@ -2222,11 +2222,11 @@ void CGroundMoveType::SetNextWayPoint(int thread)
 		// Prevent delay repaths because the waypoints have been updated.
 		wantRepath = false;
 
-		lastWaypoint |= pathManager->CurrentWaypointIsUnreachable(pathID);
+		lastWaypoint |= (earlyCurrWayPoint.same(earlyNextWayPoint) && pathManager->CurrentWaypointIsUnreachable(pathID));
 		if (lastWaypoint) {
-			// incomplete path and last valid waypoint has been reached. The last waypoint is
-			// always the point that can't be reached.
-			ReRequestPath(false); //
+			// incomplete path and last valid waypoint has been reached.
+			// ReRequestPath(false);
+			pathingFailed = true;
 		}
 	}
 

rts/Sim/Path/QTPFS/Path.h

@@ -54,6 +54,7 @@ namespace QTPFS {
 			boundingBoxMaxs = other.boundingBoxMaxs;
 
 			repathAtPointIndex = other.repathAtPointIndex;
+			goalPosition = other.goalPosition;
 
 			owner = other.owner;
 			searchTime = other.searchTime;
@@ -83,6 +84,7 @@ namespace QTPFS {
 			boundingBoxMaxs = other.boundingBoxMaxs;
 
 			repathAtPointIndex = other.repathAtPointIndex;
+			goalPosition = other.goalPosition;
 
 			owner = other.owner;
 			searchTime = other.searchTime;
@@ -249,12 +251,15 @@ namespace QTPFS {
 
 		void ClearGetRepathTriggerIndex() { repathAtPointIndex = 0; }
 
+		float3 GetGoalPosition() const { return goalPosition; }
+		void SetGoalPosition(float3 point) { goalPosition = point; }
+
 	private:
 		unsigned int pathID = 0;
 		int pathType = 0;
 
 		unsigned int nextPointIndex = 0; // index of the next waypoint to be visited
-		unsigned int repathAtPointIndex = -1; // minimum index of the waypoint to trigger a repath. -1 == off.
+		unsigned int repathAtPointIndex = 0; // minimum index of the waypoint to trigger a repath. -1 == off.
 		unsigned int numPathUpdates = 0; // number of times this path was invalidated
 
 		// Identifies the layer, target quad and source quad for a search query so that similar
@@ -280,6 +285,9 @@ namespace QTPFS {
 		float3 boundingBoxMins;
 		float3 boundingBoxMaxs;
 
+		// Used for incomplete paths to allow repathing attempts to the real goal.
+		float3 goalPosition;
+
 		// object that requested this path (NULL if none)
 		const CSolidObject* owner = nullptr;
 

rts/Sim/Path/QTPFS/PathManager.cpp

@@ -769,7 +769,7 @@ bool QTPFS::PathManager::InitializeSearch(entt::entity searchEntity) {
 		assert(registry.all_of<IPath>(pathEntity));
 		IPath* path = &registry.get<IPath>(pathEntity);
 		assert(path->GetPathType() == pathType);
-		search->Initialize(&nodeLayer, path->GetSourcePoint(), path->GetTargetPoint(), path->GetOwner());
+		search->Initialize(&nodeLayer, path->GetSourcePoint(), path->GetGoalPosition(), path->GetOwner());
 		path->SetHash(search->GetHash());
 		path->SetVirtualHash(search->GetPartialSearchHash());
 
@@ -1135,6 +1135,7 @@ unsigned int QTPFS::PathManager::QueueSearch(
 	newPath->SetOwner(object);
 	newPath->SetSourcePoint(sourcePoint.cClampInBounds());
 	newPath->SetTargetPoint(targetPoint.cClampInBounds());
+	newPath->SetGoalPosition(newPath->GetTargetPoint());
 	newPath->SetPathType(moveDef->pathType);
 
 	registry.emplace<PathIsTemp>(pathEntity);

rts/Sim/Path/QTPFS/PathSearch.cpp

@@ -9,6 +9,7 @@
 #include "Path.h"
 #include "PathCache.h"
 #include "NodeLayer.h"
+#include "Sim/Misc/CollisionHandler.h"
 #include "Sim/Misc/GlobalConstants.h"
 #include "Sim/Misc/ModInfo.h"
 #include "System/Log/ILog.h"
@@ -60,6 +61,8 @@ void QTPFS::PathSearch::Initialize(
 	fwd.srcPoint = sourcePoint; fwd.srcPoint.ClampInBounds(); fwd.srcPoint.y = 0.f;
 	fwd.tgtPoint = targetPoint; fwd.tgtPoint.ClampInBounds(); fwd.tgtPoint.y = 0.f;
 
+	goalPos = fwd.tgtPoint;
+
 	assert(	fwd.srcPoint.x != 0.f || fwd.srcPoint.z != 0.f );
 
 	auto& bwd = directionalSearchData[SearchThreadData::SEARCH_BACKWARD];
@@ -561,6 +564,12 @@ bool QTPFS::PathSearch::ExecutePathSearch() {
 		// used to trace a bad path to give partial searches a chance of an early out
 		// in reverse searches.
 		bwd.tgtSearchNode = bwd.minSearchNode;
+
+		// Final position needs to be the closest point on the last quad to the goal.
+		fwd.tgtPoint = FindNearestPointOnNodeToGoal(*fwd.tgtSearchNode, goalPos);
+	} else if (badGoal) {
+		// Starting nodw for the reverse search is the nearest node to the goalPos.
+		fwd.tgtPoint = FindNearestPointOnNodeToGoal(*bwd.srcSearchNode, goalPos);
 	}
 	#endif
 
@@ -817,6 +826,7 @@ void QTPFS::PathSearch::Finalize(IPath* path) {
 		path->AllocPoints(2);
 		path->SetSourcePoint({fwd.srcPoint.x, 0.f, fwd.srcPoint.z});
 		path->SetTargetPoint({fwd.tgtPoint.x, 0.f, fwd.tgtPoint.z});
+		path->SetGoalPosition(path->GetTargetPoint());
 		path->SetRepathTriggerIndex(0);
 	}
 
@@ -834,6 +844,40 @@ void QTPFS::PathSearch::Finalize(IPath* path) {
 	path->SetHasPartialPath(havePartPath);
 }
 
+static void GetRectangleCollisionVolume(const QTPFS::SearchNode& node, CollisionVolume& v, float3& rm) {
+	float3 vScales;
+
+	// rectangle dimensions (WS)
+	vScales.x = ((node.xmax - node.xmin) * SQUARE_SIZE);
+	vScales.z = ((node.zmax - node.zmin) * SQUARE_SIZE);
+	vScales.y = 1.0f;
+
+	// rectangle mid-point (WS)
+	rm.x = ((node.xmax + node.xmin) * SQUARE_SIZE) >> 1;
+	rm.z = ((node.zmax + node.zmin) * SQUARE_SIZE) >> 1;
+	rm.y = 0.0f;
+
+	#define CV CollisionVolume
+	v.InitShape(vScales, ZeroVector, CV::COLVOL_TYPE_BOX, CV::COLVOL_HITTEST_CONT, CV::COLVOL_AXIS_Y);
+	#undef CV
+}
+
+float3 QTPFS::PathSearch::FindNearestPointOnNodeToGoal(const QTPFS::SearchNode& node, const float3& goalPos) const {
+	CollisionVolume rv;
+	CollisionQuery cq;
+	float3 rm;
+	GetRectangleCollisionVolume(node, rv, rm);
+
+	const float2& lastPoint2 = node.GetNeighborEdgeTransitionPoint();
+	const float3 lastPoint({lastPoint2.x, 0.f, lastPoint2.y});
+
+	bool collide = CCollisionHandler::IntersectBox(&rv, goalPos - rm, lastPoint - rm, &cq);
+
+	assert(collide);
+
+	return cq.GetHitPos() + rm;
+}
+
 void QTPFS::PathSearch::TracePath(IPath* path) {
 	constexpr uint32_t ONLY_NODE_ID_MASK = 0x80000000;
 	struct TracePoint{
@@ -1032,7 +1076,7 @@ void QTPFS::PathSearch::TracePath(IPath* path) {
 
 	// if source equals target, we need only two points
 	if (!points.empty()) {
-		path->AllocPoints(points.size() + 2 + (-nodesWithoutPoints));
+		path->AllocPoints(points.size() + (-nodesWithoutPoints) + 2);
 		path->AllocNodes(points.size());
 
 		path->SetBoundingBox(boundaryMins, boundaryMaxs);
@@ -1095,7 +1139,7 @@ void QTPFS::PathSearch::TracePath(IPath* path) {
 
 	uint32_t repathIndex = 0;
 	if (!haveFullPath) {
-		constexpr float MIN_REPATH_LENGTH = 500.f;
+		constexpr float MIN_REPATH_LENGTH = 1000.f;
 		bool pathIsBigEnoughForRepath = (pathDist >= MIN_REPATH_LENGTH);
 
 		// This may result in a short path still not finding an index, but that's fine:
@@ -1125,7 +1169,7 @@ void QTPFS::PathSearch::TracePath(IPath* path) {
 	// set the first (0) and last (N - 1) waypoint
 	path->SetSourcePoint(fwd.srcPoint);
 	path->SetTargetPoint(fwd.tgtPoint);
-
+	path->SetGoalPosition(goalPos);
 	path->SetRepathTriggerIndex(repathIndex);
 
 	assert(fwd.srcPoint != ZeroVector);
@@ -1405,7 +1449,7 @@ bool QTPFS::PathSearch::SharedFinalize(const IPath* srcPath, IPath* dstPath) {
 	dstPath->CopyPoints(*srcPath);
 	dstPath->CopyNodes(*srcPath);
 	dstPath->SetSourcePoint(fwd.srcPoint);
-	dstPath->SetTargetPoint(fwd.tgtPoint);
+	dstPath->SetTargetPoint(srcPath->GetTargetPoint());
 	if (srcPath->IsBoundingBoxOverriden()) {
 		const float3& mins = srcPath->GetBoundingBoxMins();
 		const float3& maxs = srcPath->GetBoundingBoxMaxs();
@@ -1417,6 +1461,7 @@ bool QTPFS::PathSearch::SharedFinalize(const IPath* srcPath, IPath* dstPath) {
 	dstPath->SetHasPartialPath(srcPath->IsPartialPath());
 	dstPath->SetSearchTime(srcPath->GetSearchTime());
 	dstPath->SetRepathTriggerIndex(srcPath->GetRepathTriggerIndex());
+	dstPath->SetGoalPosition(srcPath->GetGoalPosition());
 
 	haveFullPath = srcPath->IsFullPath();
 	havePartPath = srcPath->IsPartialPath();

rts/Sim/Path/QTPFS/PathSearch.h

@@ -147,6 +147,8 @@ namespace QTPFS {
 		void IterateNodes(unsigned int searchDir);
 		void IterateNodeNeighbors(const INode* curNode, unsigned int searchDir);
 
+		float3 FindNearestPointOnNodeToGoal(const QTPFS::SearchNode& node, const float3& goalPos) const;
+
 		void TracePath(IPath* path);
 		void SmoothPath(IPath* path);
 		bool SmoothPathIter(IPath* path);
@@ -195,6 +197,7 @@ namespace QTPFS {
 		DirectionalSearchData directionalSearchData[2];
 
 		float2 netPoints[QTPFS_MAX_NETPOINTS_PER_NODE_EDGE];
+		float3 goalPos;
 
 		float gDists[QTPFS_MAX_NETPOINTS_PER_NODE_EDGE];
 		float hDists[QTPFS_MAX_NETPOINTS_PER_NODE_EDGE];