Bar105 qtpfs partial shared searching (#995)

* Removed define for bidirectional search. Partial-shared searching requires it.

* Adjust partial-shared path searches to use a larger combine area.

* partial-shared bad paths will record a partial reverse route to balance between adding early out nodes to speed up bad path detection and keeping memory hammering down - you don't want to fill a portion of the map with bad cells.

* prevent discard path check from infinite looping.

* update previous change to allow previous fix for partial search short cuts.

* move heap priority check.

* pre-emptively cull out-of-date nodes in the open list

* reworking search code for QTPFS.

* use faster method to check for a bad partial path.

* reduced path smoothing passes and added controlled increase in HCost.

* make hcost increase configurable.

* rework path smoothing to allow partial paths to fully path smooth.

* Add a limit to forward searching when reverse search is finished.

* forward limit only needed if reverse path search cannot connect

* avoid updating current search node to avoid cache write-back

rts/Rendering/QTPFSPathDrawer.cpp

@@ -20,7 +20,7 @@
 #include "Sim/Path/QTPFS/PathManager.h"
 
 #include "Sim/Path/QTPFS/Components/Path.h"
-#include "Sim/Path/QTPFS/Components/PathMaxSpeedMod.h"
+#include "Sim/Path/QTPFS/Components/PathSpeedModInfo.h"
 #include "Sim/Path/QTPFS/Registry.h"
 
 #include "Rendering/Fonts/glFont.h"
@@ -36,6 +36,7 @@ static std::vector<const QTPFS::QTNode*> visibleNodes;
 
 static constexpr unsigned char LINK_COLOR[4] = {1 * 255, 0 * 255, 1 * 255, 1 * 128};
 static constexpr unsigned char PATH_COLOR[4] = {0 * 255, 0 * 255, 1 * 255, 1 * 255};
+static constexpr unsigned char BAD_PATH_COLOR[4] = {1 * 255, 0 * 255, 0 * 255, 1 * 255};
 static constexpr unsigned char NODE_COLORS[3][4] = {
 	{1 * 255, 0 * 255, 0 * 255, 1 * 255}, // red --> blocked
 	{0 * 255, 1 * 255, 0 * 255, 1 * 255}, // green --> passable
@@ -116,9 +117,9 @@ void QTPFSPathDrawer::DrawCosts(const std::vector<const QTPFS::QTNode*>& nodes)
 			continue;
 
 		font->SetTextColor(0.0f, 0.0f, 0.0f, 1.0f);
-		font->glWorldPrint(pos, 5.0f, FloatToString(node->GetMoveCost(), "%8.2f"));
+		// font->glWorldPrint(pos, 5.0f, FloatToString(node->GetMoveCost(), "%8.2f"));
 		// font->glWorldPrint(pos, 5.0f, IntToString(node->GetNodeNumber(), "%08x"));
-		// font->glWorldPrint(pos, 5.0f, IntToString(node->GetIndex(), "%d"));
+		font->glWorldPrint(pos, 5.0f, IntToString(node->GetIndex(), "%d"));
 	}
 
 	font->DrawWorldBuffered();
@@ -202,14 +203,22 @@ void QTPFSPathDrawer::DrawPath(const QTPFS::IPath* path, TypedRenderBuffer<VA_TY
 		assert(p1.x / SQUARE_SIZE < mapDims.mapx);
 		assert(p1.z / SQUARE_SIZE < mapDims.mapy);
 
+		assert(p0 != float3());
+		assert(p1 != float3());
+
 		if (!camera->InView(p0) && !camera->InView(p1))
 			continue;
 
 		p0.y = CGround::GetHeightReal(p0.x, p0.z, false);
 		p1.y = CGround::GetHeightReal(p1.x, p1.z, false);
 
-		rb.AddVertex({p0, PATH_COLOR});
-		rb.AddVertex({p1, PATH_COLOR});
+		if (path->GetSearchTime().toMilliSecsi() < 10LL) {
+			rb.AddVertex({p0, PATH_COLOR});
+			rb.AddVertex({p1, PATH_COLOR});
+		} else {
+			rb.AddVertex({p0, BAD_PATH_COLOR});
+			rb.AddVertex({p1, BAD_PATH_COLOR});
+		}
 	}
 
 	rb.Submit(GL_LINES);
@@ -345,8 +354,8 @@ void QTPFSPathDrawer::UpdateExtraTexture(int extraTex, int starty, int endy, int
 			if (md != nullptr) {
 				const QTPFS::NodeLayer& nl = pm->GetNodeLayer(md->pathType);
 
-				auto& speedModComp = QTPFS::systemGlobals.GetSystemComponent<QTPFS::PathMaxSpeedModSystemComponent>();
-				const float smr = 1.0f / ( speedModComp.maxRelSpeedMod[nl.GetNodelayer()] );
+				auto& speedModComp = QTPFS::systemGlobals.GetSystemComponent<QTPFS::PathSpeedModInfoSystemComponent>();
+				const float smr = 1.0f / ( speedModComp.relSpeedModinfos[nl.GetNodelayer()].max );
 				const bool los = (gs->cheatEnabled || gu->spectating);
 
 				for (int ty = starty; ty < endy; ++ty) {

rts/Sim/CMakeLists.txt

@@ -61,7 +61,7 @@ add_library(engineSim STATIC
 		"${CMAKE_CURRENT_SOURCE_DIR}/Path/QTPFS/PathSearch.cpp"
 		"${CMAKE_CURRENT_SOURCE_DIR}/Path/QTPFS/PathManager.cpp"
 		"${CMAKE_CURRENT_SOURCE_DIR}/Path/QTPFS/Registry.cpp"
-		"${CMAKE_CURRENT_SOURCE_DIR}/Path/QTPFS/Systems/PathMaxSpeedModSystem.cpp"
+		"${CMAKE_CURRENT_SOURCE_DIR}/Path/QTPFS/Systems/PathSpeedModInfoSystem.cpp"
 		"${CMAKE_CURRENT_SOURCE_DIR}/Path/QTPFS/Systems/RemoveDeadPathsSystem.cpp"
 		"${CMAKE_CURRENT_SOURCE_DIR}/Path/HAPFS/IPathFinder.cpp"
 		"${CMAKE_CURRENT_SOURCE_DIR}/Path/HAPFS/PathCache.cpp"

rts/Sim/Misc/ModInfo.cpp

@@ -109,6 +109,7 @@ void CModInfo::ResetState()
 		pfRepathDelayInFrames = 60;
 		pfRepathMaxRateInFrames = 150;
 		pfRawMoveSpeedThreshold = 0.f;
+		pfHcostMult = 0.2f;
 
 		enableSmoothMesh = true;
 		quadFieldQuadSizeInElmos = 128;
@@ -158,6 +159,7 @@ void CModInfo::Init(const std::string& modFileName)
 		pfRepathDelayInFrames = std::clamp(system.GetInt("pfRepathDelayInFrames", pfRepathDelayInFrames), 0, 300);
 		pfRepathMaxRateInFrames = std::clamp(system.GetInt("pfRepathMaxRateInFrames", pfRepathMaxRateInFrames), 0, 3600);
 		pfRawMoveSpeedThreshold = std::max(system.GetFloat("pfRawMoveSpeedThreshold", pfRawMoveSpeedThreshold), 0.f);
+		pfHcostMult = std::clamp(system.GetFloat("pfHcostMult", pfHcostMult), 0.0f, 2.0f);
 
 		enableSmoothMesh = system.GetBool("enableSmoothMesh", enableSmoothMesh);
 

rts/Sim/Misc/ModInfo.h

@@ -195,6 +195,18 @@ class CModInfo
 	/// Point at which a region is considered bad for raw path tracing.
 	float pfRawMoveSpeedThreshold;
 
+	/// Increase the strength of distance to the goal when considering the next optimal quad to
+	/// check during QTPFS path searching. The base strength is considered for the quad with least move cost, which
+	/// gives a weak pull. Increase the strength to encourage the QTPFS to more strongly favour
+	/// checking quads nearer the goal rather than on the current shortest path - which may be
+	/// going in the wrong direction. Be careful though, if it is too strong then the pathing
+	/// system will likely produce poor paths that tend to hug walls and obstacles instead of
+	/// cutting past them on approach.
+	/// The increase strength is measured against the difference between the min move cost
+	/// and the mean of all quad move costs. Typically a value near 0 is best.
+	/// 0.0 = no increase, 1.0 = increase to the mean move cost, 2.0 increase to max move cost.
+	float pfHcostMult;
+
 	float pfRawDistMult;
 	float pfUpdateRateScale;
 

rts/Sim/MoveTypes/GroundMoveType.cpp

@@ -740,7 +740,7 @@ void CGroundMoveType::SlowUpdate()
 						// bool printMoveInfo = (selectedUnitsHandler.selectedUnits.size() == 1)
 						// 	&& (selectedUnitsHandler.selectedUnits.find(owner->id) != selectedUnitsHandler.selectedUnits.end());
 						// if (printMoveInfo) {
-							// LOG("%s: failed by idling too long.", __func__);
+						// 	LOG("%s: failed by idling too long.", __func__);
 						// }
 					}
 				}

rts/Sim/Path/QTPFS/Components/Path.h

@@ -20,6 +20,16 @@ struct SharedPathChain {
     entt::entity next{entt::null};
 };
 
+struct PartialSharedPathChain {
+	PartialSharedPathChain() {}
+
+	PartialSharedPathChain(entt::entity initPrev, entt::entity initNext)
+		: prev(initPrev), next(initNext) {}
+
+    entt::entity prev{entt::null};
+    entt::entity next{entt::null};
+};
+
 VOID_COMPONENT(PathIsTemp);
 VOID_COMPONENT(PathIsDirty);
 VOID_COMPONENT(PathIsToBeUpdated);

rts/Sim/Path/QTPFS/Components/PathMaxSpeedMod.h → rts/Sim/Path/QTPFS/Components/PathSpeedModInfo.h

@@ -1,7 +1,7 @@
 /* This file is part of the Spring engine (GPL v2 or later), see LICENSE.html */
 
-#ifndef QTPFS_SYSTEMS_PATH_MAX_SPEED_MOD_H__
-#define QTPFS_SYSTEMS_PATH_MAX_SPEED_MOD_H__
+#ifndef QTPFS_SYSTEMS_PATH_SPEED_MOD_INFO_H__
+#define QTPFS_SYSTEMS_PATH_SPEED_MOD_INFO_H__
 
 #include <deque>
 #include <vector>
@@ -14,19 +14,28 @@ namespace QTPFS {
 
 class INode;
 
-struct NodeLayerMaxSpeedSweep {
+struct NodeLayerSpeedInfoSweep {
 	static constexpr std::size_t page_size = MoveDefHandler::MAX_MOVE_DEFS;
+
 	int updateMaxNodes = 0;
 	float updateCurMaxSpeed = 0.f;
+	float updateCurSumSpeed = 0.f;
+	float updateNumLeafNodes = 0.f;
 	int layerNum = -1;
 	bool updateInProgress = false;
 };
 
 constexpr int NEXT_FRAME_NEVER = std::numeric_limits<decltype(NEXT_FRAME_NEVER)>::max();
 
-struct PathMaxSpeedModSystemComponent {
+struct PathSpeedModInfo {
+	float mean;
+	float max;
+};
+
+struct PathSpeedModInfoSystemComponent {
 	static constexpr std::size_t page_size = 1;
-	std::array<float, MoveDefHandler::MAX_MOVE_DEFS> maxRelSpeedMod;
+
+	std::array<PathSpeedModInfo, MoveDefHandler::MAX_MOVE_DEFS> relSpeedModinfos;
 	int refreshTimeInFrames = 0;
 	int startRefreshOnFrame = 0;
 	int refeshDelayInFrames = 0;

rts/Sim/Path/QTPFS/Node.cpp

@@ -645,7 +645,8 @@ bool QTPFS::QTNode::UpdateNeighborCache(NodeLayer& nodeLayer, UpdateThreadData&
 
 	int newNeighbors = 0;
 
-	constexpr size_t maxNumberOfNeighbours = QTPFS_MAX_NODE_SIZE*4;
+	// number of size-1 quad neighbours around the 4 edges + 4 corners. 
+	constexpr size_t maxNumberOfNeighbours = QTPFS_MAX_NODE_SIZE*4 + 4;
 	std::array<INode*, maxNumberOfNeighbours> neighborCache;
 
 	// if (gs->frameNum > -1 && nodeLayer == 2)
@@ -909,6 +910,8 @@ bool QTPFS::QTNode::UpdateNeighborCache(NodeLayer& nodeLayer, UpdateThreadData&
 		}
 		#endif
 
+		assert(newNeighbors < maxNumberOfNeighbours);
+
 		maxNgbs = neighbours.size() + newNeighbors;
 		neighbours.reserve(maxNgbs);
 

rts/Sim/Path/QTPFS/Node.h

@@ -224,17 +224,22 @@ namespace QTPFS {
 		void SetNeighborEdgeTransitionPoint(const float2& point) { selectedNetpoint = point; }
 		const float2& GetNeighborEdgeTransitionPoint() const { return selectedNetpoint; }
 
+		uint32_t GetStepIndex() const { return stepIndex; }
+		void SetStepIndex(uint32_t idx) { stepIndex = idx; }
+
 		unsigned int index = 0;
 		// unsigned int searchState = 0;
 
-		float fCost = QTPFS_POSITIVE_INFINITY;
+		float fCost = QTPFS_POSITIVE_INFINITY; // TODO: drop this as it is only a derived field?
 		float gCost = QTPFS_POSITIVE_INFINITY;
 		float hCost = QTPFS_POSITIVE_INFINITY;
 
 		// points back to previous node in path
 		SearchNode* prevNode = nullptr;
 
 		float2 selectedNetpoint;
+
+		uint32_t stepIndex = 0;
 	};
 }
 

rts/Sim/Path/QTPFS/NodeLayer.cpp

@@ -64,7 +64,7 @@ void QTPFS::NodeLayer::Init(unsigned int layerNum) {
 		nodeIndcs.clear();
 		nodeIndcs.resize(POOL_TOTAL_SIZE);
 
-		std::for_each(nodeIndcs.begin(), nodeIndcs.end(), [&](const unsigned int& i) { nodeIndcs[&i - &nodeIndcs[0]] = &i - &nodeIndcs[0]; });
+		std::for_each(nodeIndcs.begin(), nodeIndcs.end(), [&](const unsigned int& i) { nodeIndcs[&i - &nodeIndcs[0]] = &i - &nodeIndcs[0]; assert((size_t)(&i - &nodeIndcs[0]) < nodeIndcs.size()); });
 		std::reverse(nodeIndcs.begin(), nodeIndcs.end());
 	}
 

rts/Sim/Path/QTPFS/Path.h

@@ -12,10 +12,18 @@
 
 #include "Map/ReadMap.h"
 
+#include "System/TimeProfiler.h"
+
 class CSolidObject;
 
 namespace QTPFS {
 	struct IPath {
+		struct PathNodeData {
+			uint32_t nodeId;
+			float2 netPoint;
+			int pathPointIndex = -1;
+		};
+
 		IPath() {}
 		IPath(const IPath& other) { *this = other; }
 		IPath& operator = (const IPath& other) {
@@ -28,15 +36,18 @@ namespace QTPFS {
 			numPathUpdates = other.numPathUpdates;
 
 			hash   = other.hash;
+			virtualHash = other.virtualHash;
 			radius = other.radius;
 			synced = other.synced;
 			haveFullPath = other.haveFullPath;
 			points = other.points;
+			nodes = other.nodes;
 
 			boundingBoxMins = other.boundingBoxMins;
 			boundingBoxMaxs = other.boundingBoxMaxs;
 
 			owner = other.owner;
+			searchTime = other.searchTime;
 			return *this;
 		}
 		IPath(IPath&& other) { *this = std::move(other); }
@@ -50,19 +61,22 @@ namespace QTPFS {
 			numPathUpdates = other.numPathUpdates;
 
 			hash   = other.hash;
+			virtualHash = other.virtualHash;
 			radius = other.radius;
 			synced = other.synced;
 			haveFullPath = other.haveFullPath;
 			points = std::move(other.points);
+			nodes = other.nodes;
 
 			boundingBoxMins = other.boundingBoxMins;
 			boundingBoxMaxs = other.boundingBoxMaxs;
 
 			owner = other.owner;
+			searchTime = other.searchTime;
 
 			return *this;
 		}
-		~IPath() { points.clear(); }
+		~IPath() { points.clear(); nodes.clear(); }
 
 		void SetID(unsigned int pathID) { this->pathID = pathID; }
 		unsigned int GetID() const { return pathID; }
@@ -73,13 +87,15 @@ namespace QTPFS {
 		unsigned int GetNumPathUpdates() const { return numPathUpdates; }
 
 		void SetHash(std::uint64_t hash) { this->hash = hash; }
+		void SetVirtualHash(std::uint64_t virtualHash) { this->virtualHash = virtualHash; }
 		void SetRadius(float radius) { this->radius = radius; }
 		void SetSynced(bool synced) { this->synced = synced; }
 		void SetHasFullPath(bool fullPath) { this->haveFullPath = fullPath; }
 		void SetHasPartialPath(bool partialPath) { this->havePartialPath = partialPath; }
 
 		float GetRadius() const { return radius; }
 		std::uint64_t GetHash() const { return hash; }
+		std::uint64_t GetVirtualHash() const { return virtualHash; }
 		bool IsSynced() const { return synced; }
 		bool IsFullPath() const { return haveFullPath; }
 		bool IsPartialPath() const { return havePartialPath; }
@@ -132,8 +148,15 @@ namespace QTPFS {
 		}
 		const float3& GetPoint(unsigned int i) const { return points[std::min(i, NumPoints() - 1)]; }
 
-		void SetSourcePoint(const float3& p) { checkPointInBounds(p); assert(points.size() >= 2); points[                0] = p; }
-		void SetTargetPoint(const float3& p) { checkPointInBounds(p); assert(points.size() >= 2); points[points.size() - 1] = p; }
+		void SetNode(unsigned int i, uint32_t nodeId, float2&& netpoint, int pointIdx) {
+			nodes[i].netPoint = netpoint;
+			nodes[i].nodeId = nodeId;
+			nodes[i].pathPointIndex = pointIdx;
+		}
+		const PathNodeData& GetNode(unsigned int i) const { return nodes[i]; };
+
+		void SetSourcePoint(const float3& p) { /* checkPointInBounds(p); */ assert(points.size() >= 2); points[                0] = p; }
+		void SetTargetPoint(const float3& p) { /* checkPointInBounds(p); */ assert(points.size() >= 2); points[points.size() - 1] = p; }
 		const float3& GetSourcePoint() const { return points[                0]; }
 		const float3& GetTargetPoint() const { return points[points.size() - 1]; }
 
@@ -159,31 +182,60 @@ namespace QTPFS {
 				points[n] = p.GetPoint(n);
 			}
 		}
+		void AllocNodes(unsigned int n) {
+			nodes.clear();
+			nodes.resize(n);
+		}
+		void CopyNodes(const IPath& p) {
+			AllocNodes(p.nodes.size());
+
+			for (unsigned int n = 0; n < p.nodes.size(); n++) {
+				nodes[n] = p.GetNode(n);
+			}
+		}
 
 		void SetPathType(int newPathType) { assert(pathType < moveDefHandler.GetNumMoveDefs()); pathType = newPathType; }
 		int GetPathType() const { return pathType; }
 
+		const std::vector<PathNodeData>& GetNodeList() const { return nodes; };
+
+		void SetSearchTime(spring_time time) { searchTime = time; }
+
+		spring_time GetSearchTime() const { return searchTime; }
+
 	private:
 		unsigned int pathID = 0;
 		int pathType = 0;
 
 		unsigned int nextPointIndex = -1; // index of the next waypoint to be visited
 		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
+		// searches can be combined.
 		std::uint64_t hash = -1;
+
+		// Similar to hash, but the target quad and source quad numbers may not relate to actual
+		// leaf nodes in the quad tree. They repesent the quad that would be there if the leaf node
+		// was exactly the size of QTPFS_PARTIAL_SHARE_PATH_MAX_SIZE. This allows searches that
+		// start and/or end in different, but close, quads. This is used to handle partially-
+		// shared path searches.
+		std::uint64_t virtualHash = -1;
 		float radius = 0.f;
 		bool synced = true;
 		bool haveFullPath = true;
 		bool havePartialPath = false;
 
 		std::vector<float3> points;
+		std::vector<PathNodeData> nodes;
 
 		// corners of the bounding-box containing all our points
 		float3 boundingBoxMins;
 		float3 boundingBoxMaxs;
 
 		// object that requested this path (NULL if none)
 		const CSolidObject* owner = nullptr;
+
+		spring_time searchTime;
 	};
 }
 

rts/Sim/Path/QTPFS/PathCache.cpp

@@ -14,7 +14,7 @@
 
 #include "Registry.h"
 #include "Components/Path.h"
-#include "Components/PathMaxSpeedMod.h"
+#include "Components/PathSpeedModInfo.h"
 
 #include <tracy/Tracy.hpp>