Add multithreading to GenerateWaypointNodes

Author: dbierek

include/NavKit/adapter/RecastAdapter.h

@@ -66,7 +66,8 @@ class RecastAdapter {
 
     dtPolyRef getPoly(int tileIndex, int polyIndex) const;
 
-    dtStatus findNearestPoly(const float *recastPos, dtPolyRef *polyRef, float *nearestPt, bool includeExcludedAreas) const;
+    dtStatus findNearestPoly(const float *recastPos, dtPolyRef *polyRef, float *nearestPt,
+                             bool includeExcludedAreas) const;
 
     void findPfSeedPointAreas();
 
@@ -112,9 +113,13 @@ class RecastAdapter {
 
     Vec3 calculateCentroid(dtPolyRef polyRef) const;
 
-    std::vector<dtPolyRef> getClosestReachablePolys(Vec3 navpowerPos, dtPolyRef navpowerStart, int maxPolys) const;
+    const dtNavMesh *getNavMesh() const;
 
-    std::vector<dtPolyRef> getClosestPolys(Vec3 navPowerPos, int maxPolys) const;
+    std::vector<dtPolyRef> getClosestReachablePolys(
+        dtNavMeshQuery *navQuery, Vec3 navpowerPos, dtPolyRef navpowerStart, int maxPolys) const;
+
+    std::vector<dtPolyRef> getClosestPolys(
+        dtNavMeshQuery *navQuery, Vec3 navPowerPos, int maxPolys) const;
 
     Sample_TileMesh *sample;
     BuildContext *buildContext;

include/NavKit/util/GridGenerator.h

@@ -3,6 +3,7 @@
 #include "../model/ReasoningGrid.h"
 #include "../util/Pathfinding.h"
 #include <DetourNavMesh.h>
+#include <unordered_map>
 
 class Properties;
 
@@ -15,7 +16,7 @@ class GridGenerator {
 
     static bool initRecastAirgAdapter();
 
-    std::map<int, std::vector<int> > m_WaypointMap{};
+    std::unordered_map<int, std::vector<int> > m_WaypointMap{};
     std::map<int, std::vector<Pathfinding::SGCell> > waypointCells{};
 
     static void build();
@@ -36,17 +37,17 @@ class GridGenerator {
 
     void GenerateLayerIndices();
 
-    static Pathfinding::ZPFLocation *MapLocation_Internal(Pathfinding::ZPFLocation *result, const float4 *vPosNavPower,
-                                                   float fAcceptance, dtPolyRef startPolyRef);
+    static Pathfinding::ZPFLocation *MapLocation_Internal(dtNavMeshQuery *navQuery, Pathfinding::ZPFLocation *result,
+                                                          const float4 *vPosNavPower, float fAcceptance, dtPolyRef startPolyRef);
 
-    static bool MapLocation(const float4 *vNavPowerPos, Pathfinding::ZPFLocation *lMapped);
+    static bool MapLocation(dtNavMeshQuery *navQuery, const float4 *vNavPowerPos, Pathfinding::ZPFLocation *lMapped);
 
-    static float4 MapToCell(const float4 *vCellNavPowerUpperLeft, const NavPower::Area &area);
+    static float4 MapToCell(dtNavMeshQuery *navQuery, const float4 *vCellNavPowerUpperLeft, const NavPower::Area &area);
 
-    static bool IsInside(Pathfinding::ZPFLocation *location);
+    static bool IsInside(dtNavMeshQuery *navQuery, Pathfinding::ZPFLocation *location);
 
-    static bool NearestOuterEdge(Pathfinding::ZPFLocation &location, float tolerance, float4 *edgeNavPowerResult,
-                          float4 *edgeNavPowerNormal);
+    static bool NearestOuterEdge(dtNavMeshQuery *navQuery, Pathfinding::ZPFLocation &lFrom, float fRadius,
+                                 float4 *edgeNavPowerResult, float4 *edgeNavPowerNormal);
 
     static void buildVisionAndDeadEndData();
 

src/adapter/RecastAdapter.cpp

@@ -842,6 +842,9 @@ Vec3 RecastAdapter::calculateCentroid(const dtPolyRef polyRef) const {
     return {x, y, z};
 }
 
+const dtNavMesh* RecastAdapter::getNavMesh() const {
+    return sample->getNavMesh();
+}
 /**
  * Find up to the maxPolys closest polys within the specified radius of the position which are reachable from the starting area.
  * @param navpowerPos
@@ -850,12 +853,12 @@ Vec3 RecastAdapter::calculateCentroid(const dtPolyRef polyRef) const {
  * @return
  */
 std::vector<dtPolyRef> RecastAdapter::getClosestReachablePolys(
+    dtNavMeshQuery* navQuery,
     Vec3 navpowerPos,
     const dtPolyRef start,
     const int maxPolys) const {
     std::vector<dtPolyRef> polys;
     const dtNavMesh *navMesh = sample->getNavMesh();
-    const dtNavMeshQuery *navQuery = sample->getNavMeshQuery();
     if (!navMesh || !navQuery || start == 0) {
         return polys;
     }
@@ -914,10 +917,9 @@ std::vector<dtPolyRef> RecastAdapter::getClosestReachablePolys(
  * @param maxPolys
  * @return
  */
-std::vector<dtPolyRef> RecastAdapter::getClosestPolys(Vec3 navPowerPos, const int maxPolys) const {
+std::vector<dtPolyRef> RecastAdapter::getClosestPolys(dtNavMeshQuery* navQuery, Vec3 navPowerPos, const int maxPolys) const {
     std::vector<dtPolyRef> polys;
     const dtNavMesh *navMesh = sample->getNavMesh();
-    const dtNavMeshQuery *navQuery = sample->getNavMeshQuery();
     if (!navMesh || !navQuery) {
         return polys;
     }