QTPFS Path Repair

rts/Rendering/QTPFSPathDrawer.cpp

@@ -118,9 +118,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->GetDepth(), "%d"));
 	}
 
 	font->DrawWorldBuffered();

rts/Sim/Misc/ModInfo.cpp

@@ -110,7 +110,7 @@ void CModInfo::ResetState()
 		pfRepathMaxRateInFrames = 150;
 		pfRawMoveSpeedThreshold = 0.f;
 		qtMaxNodesSearched = 8192;
-		qtRefreshPathMinDist = 2000.f;
+		qtRefreshPathMinDist = 512.f;
 		qtMaxNodesSearchedRelativeToMapOpenNodes = 0.25;
 		qtLowerQualityPaths = false;
 

rts/Sim/Path/QTPFS/Node.cpp

@@ -205,7 +205,7 @@ void QTPFS::QTNode::InitStatic() {
 }
 
 void QTPFS::QTNode::Init(
-	const QTNode* /*parent*/,
+	const QTNode* parent,
 	unsigned int nn,
 	unsigned int x1, unsigned int z1,
 	unsigned int x2, unsigned int z2,
@@ -231,7 +231,8 @@ void QTPFS::QTNode::Init(
 	assert(zsize() != 0);
 
 	moveCostAvg = -1.0f;
-	index = idx;
+	uint32_t depth = (parent != nullptr) ? (parent->GetDepth() + 1) : 0;
+	index = (idx & NODE_INDEX_MASK) + ((depth << DEPTH_BIT_OFFSET) & DEPTH_MASK);
 
 	neighbours.clear();
 }

rts/Sim/Path/QTPFS/Node.h

@@ -45,7 +45,9 @@ namespace QTPFS {
 		float2 GetNeighborEdgeTransitionPoint(const INode* ngb, const float3& pos, float alpha) const;
 		SRectangle ClipRectangle(const SRectangle& r) const;
 
-		unsigned int GetIndex() const { return index; }
+		unsigned int GetIndex() const { return index & NODE_INDEX_MASK; }
+		unsigned int GetDepth() const { return (index & DEPTH_MASK) >> DEPTH_BIT_OFFSET; }
+		unsigned int GetRawIndex() const { return index; }
 
 		~QTNode() = default;
 		QTNode() = default;
@@ -69,9 +71,10 @@ namespace QTPFS {
 		//     root-node identifier is always 0
 		//     <i> is a NODE_IDX index in [0, 3]
 		unsigned int GetChildID(unsigned int i, uint32_t rootMask) const {
-			uint32_t rootId = rootMask & nodeNumber;
-			uint32_t nodeId = ((~rootMask) & nodeNumber);
-			return rootId | ((nodeId << 2) + (i + 1));
+			// uint32_t rootId = rootMask & nodeNumber;
+			// uint32_t nodeId = ((~rootMask) & nodeNumber);
+			uint32_t shift = (MAX_DEPTH - (GetDepth() + 1)) * QTPFS_NODE_NUMBER_SHIFT_STEP;
+			return nodeNumber + ((i + 1) << shift);
 		}
 
 		std::uint64_t GetCheckSum(const NodeLayer& nl) const;
@@ -153,6 +156,16 @@ namespace QTPFS {
 		static unsigned int MAX_DEPTH;
 
 	private:
+		// Mask off the bits for node index, we can use higher bits for other purposes. We wouldn't normally, worry
+		// about so much about this, but when you have hundreds of thousands of nodes per movetype, the memory used
+		// adds up rather quickly so it is important to keep these objects as small as possible. 
+		static constexpr unsigned int NODE_INDEX_MASK = 0x000fffff;
+
+		static constexpr unsigned int EXIT_ONLY_BIT_OFFSET = 31;
+		static constexpr unsigned int EXIT_ONLY_MASK = (0x1 << EXIT_ONLY_BIT_OFFSET);
+		static constexpr unsigned int DEPTH_BIT_OFFSET = 20;
+		static constexpr unsigned int DEPTH_MASK = (0xf << DEPTH_BIT_OFFSET);
+
 		unsigned int nodeNumber = -1u;
 		unsigned int index = 0;
 
@@ -180,10 +193,12 @@ namespace QTPFS {
 
 		SearchNode(INode& srcNode)
 			: index(srcNode.index)
+			, nodeNumber(srcNode.nodeNumber)
 			{}
 
 		SearchNode(INode* srcNode)
 			: index(srcNode->index)
+			, nodeNumber(srcNode->nodeNumber)
 			{}
 
 		SearchNode(int nodeId)
@@ -202,10 +217,21 @@ namespace QTPFS {
 			zmin = other.zmin;
 			xmax = other.xmax;
 			zmax = other.zmax;
+			nodeNumber = other.nodeNumber;
+			badNode = other.badNode;
 			// searchState = other.searchState;
 			return *this;
 		}
 
+		void CopyGeneralNodeData(const SearchNode& other) {
+			index = other.index;
+			xmin = other.xmin;
+			zmin = other.zmin;
+			xmax = other.xmax;
+			zmax = other.zmax;
+			nodeNumber = other.nodeNumber;
+		}
+
 		float GetHeapPriority() const { return GetPathCost(NODE_PATH_COST_F); }
 		// unsigned int GetSearchState() const { return searchState; }
 
@@ -231,6 +257,9 @@ namespace QTPFS {
 		uint32_t GetStepIndex() const { return stepIndex; }
 		void SetStepIndex(uint32_t idx) { stepIndex = idx; }
 
+		bool isNodeBad() const { return badNode; /*selectedNetpoint.x == -1.f;*/ }
+		void SetNodeBad(bool state) { badNode = state; }
+
 		unsigned int index = 0;
 		// unsigned int searchState = 0;
 
@@ -249,6 +278,9 @@ namespace QTPFS {
 		int zmin = 0;
 		int xmax = 0;
 		int zmax = 0;
+
+		unsigned int nodeNumber = -1;
+		bool badNode = false;
 	};
 }
 

rts/Sim/Path/QTPFS/NodeLayer.h

@@ -191,6 +191,12 @@ namespace QTPFS {
 			return GetPoolNode(i);
 		}
 
+		const QTNode* GetRootNode(int x, int z) const {
+			// This is fine, the class doesn't get modified in the process of the call.
+			// This call is to add const to the return value.
+			return const_cast<QTPFS::NodeLayer *>(this)->GetRootNode(x, z);
+		}
+
 public:
 
 		// NOTE:

rts/Sim/Path/QTPFS/Path.h

@@ -15,18 +15,68 @@
 
 #include "System/TimeProfiler.h"
 
+#include "System/Log/ILog.h"
+
 class CSolidObject;
 
 namespace QTPFS {
 	struct IPath {
 		struct PathNodeData {
-			uint32_t nodeId;
+			uint32_t nodeId = -1U;
+			uint32_t nodeNumber = -1U;
 			float2 netPoint;
 			int pathPointIndex = -1;
 			int xmin = 0;
 			int zmin = 0;
 			int xmax = 0;
 			int zmax = 0;
+			bool badNode = false;
+
+			bool IsNodeBad() const { return badNode; }
+			void SetNodeBad(bool state) { badNode = state; }
+
+			int getWidth() const { return xmax - xmin; }
+
+			static constexpr int NEIGHBOUR_LEFT = 0x0;
+			static constexpr int NEIGHBOUR_RIGHT = 0x1;
+			static constexpr int NEIGHBOUR_TOP = 0x0;
+			static constexpr int NEIGHBOUR_BOTTOM = 0x2;
+			static constexpr int NEIGHBOUR_NO_TOUCH = 0x0;
+			static constexpr int NEIGHBOUR_LONGITUDE_TOUCH = 0x4;
+			static constexpr int NEIGHBOUR_LATITUDE_TOUCH = 0x8;
+
+			static constexpr int NEIGHBOUR_TOUCH = (NEIGHBOUR_LONGITUDE_TOUCH|NEIGHBOUR_LATITUDE_TOUCH);
+
+			static int GetLongitudeNeighbourSide(int neighbourRelationStatus) {
+				return (neighbourRelationStatus & NEIGHBOUR_RIGHT);
+			}
+
+			static int GetLatitudeNeighbourSide(int neighbourRelationStatus) {
+				return (neighbourRelationStatus & NEIGHBOUR_BOTTOM) >> 1;
+			}
+
+			static int IsNeighbourCorner(int neighbourRelationStatus) {
+				return (neighbourRelationStatus & NEIGHBOUR_TOUCH) == NEIGHBOUR_TOUCH;
+			}
+
+			static int IsNeighbourLongitude(int neighbourRelationStatus) {
+				return (neighbourRelationStatus & NEIGHBOUR_TOUCH) == NEIGHBOUR_LONGITUDE_TOUCH;
+			}
+
+			static int IsNeighbourLatitude(int neighbourRelationStatus) {
+				return (neighbourRelationStatus & NEIGHBOUR_TOUCH) == NEIGHBOUR_LATITUDE_TOUCH;
+			}
+
+			int GetNeighborRelationStatus(const PathNodeData& other) const {
+				bool top = (zmin == other.zmax);
+				bool bottom = (zmax == other.zmin);
+				bool left = (xmin == other.xmax);
+				bool right = (xmax == other.xmin);
+
+				return (NEIGHBOUR_BOTTOM * bottom) + (NEIGHBOUR_RIGHT * right)
+						+ (NEIGHBOUR_LONGITUDE_TOUCH * (left|right))
+						+ (NEIGHBOUR_LATITUDE_TOUCH  * (top|bottom));
+			}
 		};
 
 		IPath() {}
@@ -39,6 +89,7 @@ namespace QTPFS {
 
 			nextPointIndex = other.nextPointIndex;
 			numPathUpdates = other.numPathUpdates;
+			firstNodeIdOfCleanPath = other.firstNodeIdOfCleanPath;
 
 			hash   = other.hash;
 			virtualHash = other.virtualHash;
@@ -47,6 +98,7 @@ namespace QTPFS {
 			haveFullPath = other.haveFullPath;
 			havePartialPath = other.havePartialPath;
 			boundingBoxOverride = other.boundingBoxOverride;
+			isRawPath = other.isRawPath;
 			points = other.points;
 			nodes = other.nodes;
 
@@ -69,6 +121,7 @@ namespace QTPFS {
 
 			nextPointIndex = other.nextPointIndex;
 			numPathUpdates = other.numPathUpdates;
+			firstNodeIdOfCleanPath = other.firstNodeIdOfCleanPath;
 
 			hash   = other.hash;
 			virtualHash = other.virtualHash;
@@ -77,6 +130,7 @@ namespace QTPFS {
 			haveFullPath = other.haveFullPath;
 			havePartialPath = other.havePartialPath;
 			boundingBoxOverride = other.boundingBoxOverride;
+			isRawPath = other.isRawPath;
 			points = std::move(other.points);
 			nodes = std::move(other.nodes);
 
@@ -120,8 +174,9 @@ namespace QTPFS {
 			boundingBoxMins.z = 1e6f; boundingBoxMaxs.z = -1e6f;
 
 			const unsigned int begin = (nextPointIndex >= 2U) ? nextPointIndex - 2U : 0U;
-			const unsigned int end = (repathAtPointIndex > 0U) ? repathAtPointIndex + 1U : points.size();
-
+			// const unsigned int end = (repathAtPointIndex > 0U) ? repathAtPointIndex + 1U : points.size();
+			const unsigned int end = points.size();
+
 			for (unsigned int n = begin; n < end; n++) {
 				boundingBoxMins.x = std::min(boundingBoxMins.x, points[n].x);
 				boundingBoxMins.z = std::min(boundingBoxMins.z, points[n].z);
@@ -187,18 +242,33 @@ namespace QTPFS {
 			if (index < repathAtPointIndex) repathAtPointIndex--;
 		}
 
-		void SetNode(unsigned int i, uint32_t nodeId, float2&& netpoint, int pointIdx) {
+		void SetNode(unsigned int i, uint32_t nodeId, uint32_t nodeNumber, float2&& netpoint, int pointIdx, bool isBad) {
 			nodes[i].netPoint = netpoint;
+			nodes[i].nodeNumber = nodeNumber;
 			nodes[i].nodeId = nodeId;
 			nodes[i].pathPointIndex = pointIdx;
+			nodes[i].SetNodeBad(isBad);
 		}
+
+		const PathNodeData& GetNode(unsigned int i) const {
+			return nodes[i];
+		};
+
+		void RemoveNode(size_t index) {
+			if (index >= nodes.size()) { return; }
+			unsigned int start = index, end = nodes.size() - 1;
+			for (unsigned int i = start; i < end; ++i) { nodes[i] = nodes[i+1]; }
+			nodes.pop_back();
+		}
+
 		void SetNodeBoundary(unsigned int i, int xmin, int zmin, int xmax, int zmax) {
 			nodes[i].xmin = xmin;
 			nodes[i].zmin = zmin;
 			nodes[i].xmax = xmax;
 			nodes[i].zmax = zmax;
 		}
-		const PathNodeData& GetNode(unsigned int i) const { return nodes[i]; };
+		// There are always (points - 1) valid path nodes.
+		uint32_t GetGoodNodeCount() const { return points.size() - 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; }
@@ -257,13 +327,20 @@ namespace QTPFS {
 		float3 GetGoalPosition() const { return goalPosition; }
 		void SetGoalPosition(float3 point) { goalPosition = point; }
 
+		unsigned int GetFirstNodeIdOfCleanPath() const { return firstNodeIdOfCleanPath; }
+		void SetFirstNodeIdOfCleanPath(int nodeId) { firstNodeIdOfCleanPath = nodeId; }
+
+		bool IsRawPath() const { return isRawPath; }
+		void SetIsRawPath(bool enable) { isRawPath = enable; }
+
 	private:
 		unsigned int pathID = 0;
 		int pathType = 0;
 
 		unsigned int nextPointIndex = 0; // index of the next waypoint to be visited
 		unsigned int repathAtPointIndex = 0; // minimum index of the waypoint to trigger a repath.
 		unsigned int numPathUpdates = 0; // number of times this path was invalidated
+		unsigned int firstNodeIdOfCleanPath = 0;
 
 		// Identifies the layer, target quad and source quad for a search query so that similar
 		// searches can be combined.
@@ -280,6 +357,7 @@ namespace QTPFS {
 		bool haveFullPath = true;
 		bool havePartialPath = false;
 		bool boundingBoxOverride = false;
+		bool isRawPath = false;
 
 		std::vector<float3> points;
 		std::vector<PathNodeData> nodes;