Recast: integrate static pathing generation/parsing code into editor

Instead of building the static pathing data when the NavMesh is getting saved, build it right after the NavMesh has been build. also, parse this data out from NavMesh files loaded from the disk. This patch also documented some extra features and designs. In the future, static pathing logic will also be implemented in the detour path query code.

src/naveditor/Editor.cpp

@@ -18,6 +18,7 @@
 
 #include "Pch.h"
 #include "Recast/Include/Recast.h"
+#include "Detour/Include/DetourAssert.h"
 #include "Detour/Include/DetourNavMesh.h"
 #include "Detour/Include/DetourNavMeshQuery.h"
 #include "DetourCrowd/Include/DetourCrowd.h"
@@ -176,7 +177,6 @@ void Editor::resetCommonSettings()
 	m_detailSampleDist = 6.0f;
 	m_detailSampleMaxError = 1.0f;
 	m_partitionType = EDITOR_PARTITION_WATERSHED;
-	m_reachabilityTableCount = 4;
 }
 void Editor::handleCommonSettings()
 {
@@ -395,11 +395,34 @@ dtNavMesh* Editor::loadAll(std::string path)
 		mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, NULL);
 	}
 
+	// Read read static pathing data.
+	if (header.params.disjointPolyGroupCount >= DT_MIN_POLY_GROUP_COUNT)
+	{
+		for (int i = 0; i < header.params.reachabilityTableCount; i++)
+		{
+			int* reachabilityTable = (int*)dtAlloc(header.params.reachabilityTableSize, DT_ALLOC_PERM);
+			if (!reachabilityTable)
+				break;
+
+			memset(reachabilityTable, 0, header.params.reachabilityTableSize);
+			readLen = fread(reachabilityTable, header.params.reachabilityTableSize, 1, fp);
+
+			if (readLen != 1)
+			{
+				dtFree(reachabilityTable);
+				fclose(fp);
+				return 0;
+			}
+
+			mesh->m_setTables[i] = reachabilityTable;
+		}
+	}
+
 	fclose(fp);
 	return mesh;
 }
 
-void Editor::saveAll(std::string path, dtNavMesh* mesh)
+void Editor::saveAll(std::string path, const dtNavMesh* mesh)
 {
 	if (!mesh)
 		return;
@@ -425,28 +448,20 @@ void Editor::saveAll(std::string path, dtNavMesh* mesh)
 
 	for (int i = 0; i < mesh->getMaxTiles(); ++i)
 	{
-		dtMeshTile* tile = mesh->getTile(i);
+		const dtMeshTile* tile = mesh->getTile(i);
 		if (!tile || !tile->header || !tile->dataSize)
 			continue;
 
 		header.numTiles++;
 	}
+
 	memcpy(&header.params, mesh->getParams(), sizeof(dtNavMeshParams));
-
-	// TODO: this has to be done during navmesh init, not here!!!
-	LinkTableData linkData;
-	const int tableSize = buildLinkTable(mesh, linkData);
-
-	header.params.disjointPolyGroupCount = linkData.setCount;
-	header.params.reachabilityTableCount = m_reachabilityTableCount;
-	header.params.reachabilityTableSize = tableSize;
-
 	fwrite(&header, sizeof(NavMeshSetHeader), 1, fp);
 
 	// Store tiles.
 	for (int i = 0; i < mesh->getMaxTiles(); ++i)
 	{
-		dtMeshTile* tile = mesh->getTile(i);
+		const dtMeshTile* tile = mesh->getTile(i);
 		if (!tile || !tile->header || !tile->dataSize)
 			continue;
 
@@ -458,12 +473,19 @@ void Editor::saveAll(std::string path, dtNavMesh* mesh)
 		fwrite(tile->data, tile->dataSize, 1, fp);
 	}
 
-	std::vector<int> reachability(tableSize, 0);
-	for (int i = 0; i < linkData.setCount; i++)
-		setReachable(reachability, linkData.setCount, i, i, true);
+	// Only store if we have 3 or more poly groups.
+	if (mesh->m_params.disjointPolyGroupCount >= DT_MIN_POLY_GROUP_COUNT)
+	{
+		dtAssert(mesh->m_setTables);
 
-	for (int i = 0; i < header.params.reachabilityTableCount; i++)
-		fwrite(reachability.data(), sizeof(int), (header.params.reachabilityTableSize / 4), fp);
+		for (int i = 0; i < header.params.reachabilityTableCount; i++)
+		{
+			const int* const tableData = mesh->m_setTables[i];
+			dtAssert(tableData);
+
+			fwrite(tableData, sizeof(int), (header.params.reachabilityTableSize/4), fp);
+		}
+	}
 
 	fclose(fp);
 }

src/naveditor/Editor_TileMesh.cpp

@@ -18,6 +18,7 @@
 
 #include "Pch.h"
 #include "Recast/Include/Recast.h"
+#include "Detour/Include/DetourCommon.h"
 #include "Detour/Include/DetourNavMesh.h"
 #include "Detour/Include/DetourNavMeshBuilder.h"
 #include "DebugUtils/Include/RecastDebugDraw.h"
@@ -614,6 +615,10 @@ bool Editor_TileMesh::handleBuild()
 	params.tileHeight = m_tileSize*m_cellSize;
 	params.maxTiles = m_maxTiles;
 	params.maxPolys = m_maxPolysPerTile;
+	params.disjointPolyGroupCount = 0;
+	params.reachabilityTableSize = 0;
+	params.reachabilityTableCount = DT_NUM_REACHABILITY_TABLES;
+	params.allocSize = 0;
 	
 	dtStatus status;
 	
@@ -753,6 +758,11 @@ void Editor_TileMesh::buildAllTiles()
 			}
 		}
 	}
+
+	if (!dtBuildStaticPathingData(m_navMesh))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Failed to build static pathing data.");
+	}
 	
 	// Start the build process.	
 	m_ctx->stopTimer(RC_TIMER_TEMP);

src/naveditor/GameUtils.cpp

@@ -97,92 +97,3 @@ void unpatchTileGame(dtMeshTile* t)
 		coordGameUnswap(t->offMeshCons[i].refPos);
 	}
 }
-
-int buildLinkTable(dtNavMesh* mesh, LinkTableData& data)
-{
-	// Reserve the first 2 poly groups
-	data.insert_new(); // 0 = technically usable for normal poly groups, but for simplicity we reserve it for now.
-	data.insert_new(); // 1 = DT_STRAY_POLY_GROUP.
-
-	//clear all labels
-	for (int i = 0; i < mesh->getMaxTiles(); ++i)
-	{
-		dtMeshTile* tile = mesh->getTile(i);
-		if (!tile || !tile->header || !tile->dataSize) continue;
-		int pcount = tile->header->polyCount;
-		for (int j = 0; j < pcount; j++)
-		{
-			dtPoly& poly = tile->polys[j];
-			poly.disjointSetId = (unsigned short)-1;
-		}
-	}
-	//first pass
-	std::set<int> nlabels;
-	for (int i = 0; i < mesh->getMaxTiles(); ++i)
-	{
-		dtMeshTile* tile = mesh->getTile(i);
-		if (!tile || !tile->header || !tile->dataSize) continue;
-		const int pcount = tile->header->polyCount;
-		for (int j = 0; j < pcount; j++)
-		{
-			dtPoly& poly = tile->polys[j];
-			unsigned int plink = poly.firstLink;
-			while (plink != DT_NULL_LINK)
-			{
-				const dtLink l = tile->links[plink];
-				const dtMeshTile* t;
-				const dtPoly* p;
-				mesh->getTileAndPolyByRefUnsafe(l.ref, &t, &p);
-
-				if (p->disjointSetId != (unsigned short)-1)
-					nlabels.insert(p->disjointSetId);
-
-				plink = l.next;
-			}
-			if (nlabels.empty())
-			{
-				if (poly.firstLink == DT_NULL_LINK)
-					poly.disjointSetId = DT_STRAY_POLY_GROUP;
-				else
-					poly.disjointSetId = (unsigned short)data.insert_new();
-			}
-			else
-			{
-				int l = *nlabels.begin();
-				poly.disjointSetId = (unsigned short)l;
-
-				for (const int nl : nlabels)
-					data.set_union(l, nl);
-			}
-			nlabels.clear();
-		}
-	}
-	//second pass
-	// TODO[ AMOS ]: is this necessary?
-	for (int i = 0; i < mesh->getMaxTiles(); ++i)
-	{
-		dtMeshTile* tile = mesh->getTile(i);
-		if (!tile || !tile->header || !tile->dataSize) continue;
-		const int pcount = tile->header->polyCount;
-		for (int j = 0; j < pcount; j++)
-		{
-			dtPoly& poly = tile->polys[j];
-			int id = data.find(poly.disjointSetId);
-			poly.disjointSetId = (unsigned short)id;
-		}
-	}
-
-	return sizeof(int)*(data.setCount-1) * ((data.setCount + 31) / 32) + (data.setCount-1) / 32;
-}
-void setReachable(std::vector<int>& data, int count, int id1, int id2, bool value)
-{
-	const int w = ((count + 31) / 32);
-	const unsigned int valueMask = ~(1 << (id2 & 0x1f));
-
-	int& cell = data[id1 * w + id2 / 32];
-
-	if (!value)
-		cell = (cell & valueMask);
-	else
-		cell = (cell & valueMask) | (1 << (id2 & 0x1f));
-}

src/naveditor/include/Editor.h

@@ -134,7 +134,6 @@ protected:
 	float m_detailSampleDist;
 	float m_detailSampleMaxError;
 	int m_partitionType;
-	int m_reachabilityTableCount;
 	const char* m_navmeshName = "unnamed";
 	
 	EditorTool* m_tool;
@@ -145,7 +144,7 @@ protected:
 	EditorDebugDraw m_dd;
 	
 	dtNavMesh* loadAll(std::string path);
-	void saveAll(std::string path, dtNavMesh* mesh);
+	void saveAll(std::string path, const dtNavMesh* mesh);
 
 public:
 	std::string m_modelName;

src/naveditor/include/FileTypes.h

@@ -19,49 +19,4 @@ struct NavMeshTileHeader
 	int dataSize;
 };
 
-struct LinkTableData
-{
-	//disjoint set algo from some crappy site because i'm too lazy to think
-	int setCount = 0;
-	std::vector<int> rank;
-	std::vector<int> parent;
-	void init(int size)
-	{
-		rank.resize(size);
-		parent.resize(size);
-
-		for (int i = 0; i < parent.size(); i++)
-			parent[i] = i;
-	}
-	int insert_new()
-	{
-		rank.push_back(0);
-		parent.push_back(setCount);
-		return setCount++;
-	}
-	int find(int id)
-	{
-		if (parent[id] != id)
-			return find(parent[id]);
-		return id;
-	}
-	void set_union(int x, int y)
-	{
-		int sx = find(x);
-		int sy = find(y);
-		if (sx == sy) //same set already
-			return;
-
-		if (rank[sx] < rank[sy])
-			parent[sx] = sy;
-		else if (rank[sx] > rank[sy])
-			parent[sy] = sx;
-		else
-		{
-			parent[sy] = sx;
-			rank[sx] += 1;
-		}
-	}
-};
-
 #endif // FILETYPES_H

src/naveditor/include/GameUtils.h

@@ -14,7 +14,4 @@ void unpatchHeaderGame(NavMeshSetHeader& h);
 void patchTileGame(dtMeshTile* t);
 void unpatchTileGame(dtMeshTile* t);
 
-int buildLinkTable(dtNavMesh* mesh, LinkTableData& data);
-void setReachable(std::vector<int>& data, int count, int id1, int id2, bool value);
-
 #endif // GAMEUTILS_H

src/thirdparty/recast/Detour/Include/DetourNavMesh.h

@@ -67,6 +67,17 @@ static const int DT_VERTS_PER_POLYGON = 6;
 /// For reference, r2 single player NavMeshes also marked everything unconnected as '1'.
 static const unsigned short DT_STRAY_POLY_GROUP = 1;
 
+/// The minimum required number of poly groups for static pathing logic to work.
+/// (E.g. if we have 2 poly groups, group id 1 (DT_STRAY_POLY_GROUP), and group
+/// id 2, then 1 is never reachable as its considered 'trash' by design, and 2
+/// is always reachable as that's the only group id. If group id 3 is involved
+/// then code can use the static patching logic to quickly query if we are even
+/// on the same (or connected) poly island before trying to compute a path).
+static const int DT_MIN_POLY_GROUP_COUNT = 3;
+
+/// The number of reachability tables that will be used for static pathing.
+static const int DT_NUM_REACHABILITY_TABLES = 4;
+
 /// @{
 /// @name Tile Serialization Constants
 /// These constants are used to detect whether a navigation tile's data
@@ -382,10 +393,10 @@ struct dtNavMeshParams
 	int maxPolys;					///< The maximum number of polygons each tile can contain. This and maxTiles are used to calculate how many bits are needed to identify tiles and polygons uniquely.
 //	
 //// i hate this
-	int disjointPolyGroupCount = 0;
-	int reachabilityTableSize = 0;
-	int reachabilityTableCount = 0;
-	int allocSize = 0;
+	int disjointPolyGroupCount;
+	int reachabilityTableSize;
+	int reachabilityTableCount;
+	int allocSize;
 };
 
 #pragma pack(push, 4)
@@ -719,7 +730,7 @@ public:
 	dtMeshTile** m_posLookup;			///< Tile hash lookup.
 	dtMeshTile* m_nextFree;				///< Freelist of tiles.
 	dtMeshTile* m_tiles;				///< List of tiles.
-	dtPolyRef** m_setTables;			///< Array of set tables.
+	int** m_setTables;					///< Array of set tables.
 	void* m_unk0;						///< FIXME: unknown structure pointer.
 
 	char m_meshFlags;	// Maybe.
@@ -743,6 +754,14 @@ public:
 };
 #pragma pack(pop)
 
+/// Returns the total size needed for the static pathing table.
+///  @param[in]	numPolyGroups	The total number of poly groups.
+///  @ingroup detour
+inline int calcStaticPathingTableSize(const int numPolyGroups)
+{
+	return sizeof(int)*numPolyGroups*((numPolyGroups+31)/32);
+}
+
 /// Allocates a navigation mesh object using the Detour allocator.
 /// @return A navigation mesh that is ready for initialization, or null on failure.
 ///  @ingroup detour

src/thirdparty/recast/Detour/Include/DetourNavMeshBuilder.h

@@ -108,6 +108,70 @@ struct dtNavMeshCreateParams
 	/// @}
 };
 
+/// Disjoint set algorithm used to build the static pathing data for the navmesh.
+/// @ingroup detour
+class dtDisjointSet
+{
+public:
+	dtDisjointSet(const int size)
+	{
+		init(size);
+	}
+
+	void init(const int size)
+	{
+		rank.resize(size);
+		parent.resize(size);
+
+		setCount = size;
+
+		for (int i = 0; i < parent.size(); i++)
+			parent[i] = i;
+	}
+	int insertNew()
+	{
+		rank.push_back(0);
+		parent.push_back(setCount);
+
+		return setCount++;
+	}
+	inline int find(const int id) const
+	{
+		if (parent[id] != id)
+			return find(parent[id]);
+
+		return id;
+	}
+	void setUnion(const int x, const int y)
+	{
+		const int sx = find(x);
+		const int sy = find(y);
+
+		if (sx == sy) // Same set already.
+			return;
+
+		if (rank[sx] < rank[sy])
+			parent[sx] = sy;
+		else if (rank[sx] > rank[sy])
+			parent[sy] = sx;
+		else
+		{
+			parent[sy] = sx;
+			rank[sx] += 1;
+		}
+	}
+
+	inline int getSetCount() const { return setCount; }
+
+private:
+	std::vector<int> rank;
+	std::vector<int> parent;
+
+	int setCount = 0;
+};
+
+bool dtBuildStaticPathingData(dtNavMesh* mesh);
+
 /// Builds navigation mesh tile data from the provided tile creation data.
 /// @ingroup detour
 ///  @param[in]		params		Tile creation data.

src/thirdparty/recast/Detour/Source/DetourNavMesh.cpp

@@ -193,7 +193,12 @@ dtNavMesh::dtNavMesh() :
 	m_tileLutMask(0),
 	m_posLookup(0),
 	m_nextFree(0),
-	m_tiles(0)
+	m_tiles(0),
+	m_setTables(0),
+	m_unk0(0),
+	m_meshFlags(0),
+	m_tileFlags(0),
+	m_unk1(0)
 {
 #ifndef DT_POLYREF64
 	m_saltBits = 0;
@@ -217,8 +222,19 @@ dtNavMesh::~dtNavMesh() // TODO: see [r5apex_ds + F43720] to re-implement this c
 			m_tiles[i].dataSize = 0;
 		}
 	}
+
 	dtFree(m_posLookup);
 	dtFree(m_tiles);
+
+	for (int i = 0; i < m_params.reachabilityTableCount; i++)
+	{
+		int* reachabilityTable = m_setTables[i];
+
+		if (reachabilityTable)
+			dtFree(reachabilityTable);
+	}
+
+	dtFree(m_setTables);
 }
 		
 dtStatus dtNavMesh::init(const dtNavMeshParams* params)
@@ -241,8 +257,21 @@ dtStatus dtNavMesh::init(const dtNavMeshParams* params)
 	m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM);
 	if (!m_posLookup)
 		return DT_FAILURE | DT_OUT_OF_MEMORY;
-	memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles);
-	memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize);
+	memset(m_tiles, 0, sizeof(dtMeshTile) * m_maxTiles);
+	memset(m_posLookup, 0, sizeof(dtMeshTile*) * m_tileLutSize);
+
+	const int reachabilityTableCount = params->reachabilityTableCount;
+	if (reachabilityTableCount)
+	{
+		const int setTableBufSize = sizeof(int**)*reachabilityTableCount;
+
+		m_setTables = (int**)dtAlloc(setTableBufSize, DT_ALLOC_PERM);
+		if (!m_setTables)
+			return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+		memset(m_setTables, 0, setTableBufSize);
+	}
+
 	m_nextFree = 0;
 	for (int i = m_maxTiles-1; i >= 0; --i)
 	{
@@ -277,6 +306,10 @@ dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flag
 	params.tileHeight = header->bmax[1] - header->bmin[1];
 	params.maxTiles = 1;
 	params.maxPolys = header->polyCount;
+	params.disjointPolyGroupCount = 0;
+	params.reachabilityTableSize = 0;
+	params.reachabilityTableCount = DT_NUM_REACHABILITY_TABLES;
+	params.allocSize = 0;
 	
 	dtStatus status = init(&params);
 	if (dtStatusFailed(status))

src/thirdparty/recast/Detour/Source/DetourNavMeshBuilder.cpp

@@ -268,6 +268,130 @@ static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, co
 	return 0xff;	
 }
 
+static void setReachable(int* const tableData, const int numPolyGroups,
+	const int polyGroup, const bool isReachable)
+{
+	const int w = ((numPolyGroups + 31) / 32);
+	const unsigned int valueMask = ~(1 << (polyGroup & 0x1f));
+
+	int& cell = tableData[polyGroup * w + polyGroup / 32];
+
+	cell = isReachable 
+		? (cell & valueMask) | (1 << (polyGroup & 0x1f))
+		: (cell & valueMask);
+}
+
+bool dtBuildStaticPathingData(dtNavMesh* mesh)
+{
+	dtAssert(mesh);
+
+	// Reserve the first 2 poly groups
+	// 0 = technically usable for normal poly groups, but for simplicity we reserve it for now.
+	// 1 = DT_STRAY_POLY_GROUP.
+	dtDisjointSet data(2);
+
+	// Clear all labels.
+	for (int i = 0; i < mesh->getMaxTiles(); ++i)
+	{
+		dtMeshTile* tile = mesh->getTile(i);
+		if (!tile || !tile->header || !tile->dataSize) continue;
+		int pcount = tile->header->polyCount;
+		for (int j = 0; j < pcount; j++)
+		{
+			dtPoly& poly = tile->polys[j];
+			poly.disjointSetId = (unsigned short)-1;
+		}
+	}
+
+	// First pass.
+	std::set<int> nlabels;
+	for (int i = 0; i < mesh->getMaxTiles(); ++i)
+	{
+		dtMeshTile* tile = mesh->getTile(i);
+		if (!tile || !tile->header || !tile->dataSize) continue;
+		const int pcount = tile->header->polyCount;
+		for (int j = 0; j < pcount; j++)
+		{
+			dtPoly& poly = tile->polys[j];
+			unsigned int plink = poly.firstLink;
+			while (plink != DT_NULL_LINK)
+			{
+				const dtLink l = tile->links[plink];
+				const dtMeshTile* t;
+				const dtPoly* p;
+				mesh->getTileAndPolyByRefUnsafe(l.ref, &t, &p);
+
+				if (p->disjointSetId != (unsigned short)-1)
+					nlabels.insert(p->disjointSetId);
+
+				plink = l.next;
+			}
+			if (nlabels.empty())
+			{
+				// This poly isn't connected to anything, mark it so the game
+				// won't consider this poly in path generation.
+				if (poly.firstLink == DT_NULL_LINK)
+					poly.disjointSetId = DT_STRAY_POLY_GROUP;
+				else
+					poly.disjointSetId = (unsigned short)data.insertNew();
+			}
+			else
+			{
+				const int l = *nlabels.begin();
+				poly.disjointSetId = (unsigned short)l;
+
+				for (const int nl : nlabels)
+					data.setUnion(l, nl);
+			}
+			nlabels.clear();
+		}
+	}
+
+	// Second pass.
+	for (int i = 0; i < mesh->getMaxTiles(); ++i)
+	{
+		dtMeshTile* tile = mesh->getTile(i);
+		if (!tile || !tile->header || !tile->dataSize) continue;
+		const int pcount = tile->header->polyCount;
+		for (int j = 0; j < pcount; j++)
+		{
+			dtPoly& poly = tile->polys[j];
+			int id = data.find(poly.disjointSetId);
+			poly.disjointSetId = (unsigned short)id;
+		}
+	}
+
+	const int numPolyGroups = data.getSetCount();
+	const int tableSize = calcStaticPathingTableSize(numPolyGroups);
+	const int tableCount = DT_NUM_REACHABILITY_TABLES;
+
+	dtAssert(mesh->m_setTables);
+
+	// NOTE: the game allocates 4 reachability table buffers, original
+	// navmeshes have slightly different data per table. Currently ours are all
+	// the same. Not a big problem as this just-works, but it might be nice to
+	// figure out why we need 4 tables and what the differences are.
+	for (int i = 0; i < tableCount; i++)
+	{
+		int* const reachabilityTable = (int*)dtAlloc(sizeof(int)*tableSize, DT_ALLOC_PERM);
+
+		if (!reachabilityTable)
+			return false;
+
+		mesh->m_setTables[i] = reachabilityTable;
+		memset(reachabilityTable, 0, sizeof(int)*tableSize);
+
+		for (int j = 0; j < numPolyGroups; j++)
+			setReachable(reachabilityTable, numPolyGroups, j, true);
+	}
+
+	mesh->m_params.disjointPolyGroupCount = numPolyGroups;
+	mesh->m_params.reachabilityTableSize = tableSize;
+	mesh->m_params.reachabilityTableCount = tableCount;
+
+	return true;
+}
+
 // TODO: Better error handling.
 
 /// @par