Does NavMeshQuery cache polygon paths?

If a Job is calculating multiple paths from one starting point to multiple destinations packed together such that they reside in the same PolygonId, is NavMeshQuery smart enough to "cache" the polygon path from the starting point to one of the destinations, reusing it for the other destinations?

 

May I request that you look into caching polygon paths when a NavMeshQuery is used in the same job multiple times to calculate paths with closely clustered destinations? I actually wrote a job that does so, though it's messy and bloated due to not having access to lower level APIs. Maybe you'll find it helpful should you ever consider adding that feature? Something like NavMeshQuery.CalculateBatchPath(NativeArray<float2> destinations, int maxWaypointsPerPath, ref NativeArray<float2> allPaths)

Code (CSharp):

1. [BurstCompile]
2.     private struct PolygonPathJob : IJob
3.     {
4.         public Entity userEntity;
5.  
6.         public NavMeshQuery query;
7.         public float middleOfEnvironment;
8.         public float heightOfEnvironment;
9.  
10.         [ReadOnly]
11.         public ComponentDataFromEntity<VantageComponent> vantageFromEntity;
12.         [ReadOnly]
13.         public ComponentDataFromEntity<Translation> translationFromEntity;
14.         [ReadOnly]
15.         public BufferFromEntity<DestinationsLocationElement> destinationsFromEntity;
16.  
17.         [ReadOnly]
18.         public NativeHashMap<float2, PolygonId> destinationsToPolygons;
19.  
20.         public EntityCommandBuffer ecb;
21.  
22.         public void Execute()
23.         {
24.             var userDestinations = destinationsFromEntity[userEntity].AsNativeArray();
25.             var numUserDestinations = userDestinations.Length;
26.             var uniqueDestinationsPolygons = new NativeHashMap<PolygonId, bool>(numUserDestinations, Allocator.Temp);
27.             for (var i = 0; i < numUserDestinations; i++)
28.             {
29.                 uniqueDestinationsPolygons.TryAdd(destinationsToPolygons[userDestinations[i].potentialDestination], true);
30.             }
31.             var destinationPolygons = uniqueDestinationsPolygons.GetKeyArray(Allocator.Temp);
32.             var numDestinationPolygons = destinationPolygons.Length;
33.             var destinationPolygonsSorted = new NativeArray<PolygonProximityContainer>(numDestinationPolygons, Allocator.Temp);
34.             var destinationPolygonsSuccesses = new NativeArray<bool>(numDestinationPolygons, Allocator.Temp);
35.             var destinationPolygonToContainer = new NativeHashMap<PolygonId, PolygonProximityContainer>(numDestinationPolygons, Allocator.Temp);
36.  
37.             var userVantageEntity = vantageFromEntity[userEntity].vantageEntity;
38.             var userVantagePosition = translationFromEntity[userVantageEntity].Value;
39.  
40.             for (var i = 0; i < numDestinationPolygons; i++)
41.             {
42.                 var polygon = destinationPolygons[i];
43.                 var mapLocation = query.CreateLocation(userVantagePosition, polygon);
44.                 destinationPolygonsSorted[i] = new PolygonProximityContainer
45.                 {
46.                     polygon = polygon,
47.                     distanceFromStart = ((float3)mapLocation.position).DistanceFrom(userVantagePosition),
48.                     point = mapLocation.position
49.                 };
50.                 destinationPolygonToContainer.TryAdd(polygon, destinationPolygonsSorted[i]);
51.             }
52.             destinationPolygonsSorted.Sort(new DestinationPolygonsComparator());
53.  
54.             var destinationPolygonToSortedIndex =
55.                 new NativeHashMap<PolygonId, int>(numDestinationPolygons, Allocator.Temp);
56.             for (var i = 0; i < numDestinationPolygons; i++)
57.             {
58.                 var destinationPolygonContainer = destinationPolygonsSorted[i];
59.                 destinationPolygonToSortedIndex.TryAdd(destinationPolygonContainer.polygon, i);
60.             }
61.  
62.             var unprocessedSortedDestinationPolygons =
63.                 new NativeList<PolygonProximityContainer>(numDestinationPolygons, Allocator.Temp);
64.             unprocessedSortedDestinationPolygons.AddRange(destinationPolygonsSorted);
65.             var userStartLocation = query.MapLocation(
66.                 userVantagePosition.To2D().To3D(middleOfEnvironment),
67.                 new Vector3(1, heightOfEnvironment, 1),
68.                 HUMANOID_AGENT_TYPE
69.             );
70.             var destinationsPaths = new NativeArray<PolygonId>(MAX_NUM_POLYGONS_PER_PATH * numUserDestinations, Allocator.Temp);
71.             while (unprocessedSortedDestinationPolygons.Length > 0)
72.             {
73.                 var polygonContainer = unprocessedSortedDestinationPolygons[0];
74.                 var polygonContainerIndex = destinationPolygonToSortedIndex[polygonContainer.polygon];
75.                 unprocessedSortedDestinationPolygons.RemoveAt(0);
76.                 var destinationPolygon = polygonContainer.polygon;
77.                 uniqueDestinationsPolygons.Remove(destinationPolygon);
78.                 var userDestination = polygonContainer.point;
79.                 var userDestinationLocation = query.CreateLocation(userDestination, destinationPolygon);
80.                 var pathBuffer = new NativeArray<PolygonId>(MAX_NUM_POLYGONS_PER_PATH, Allocator.Temp);
81.                 var success = CalculatePath(userStartLocation, userDestinationLocation, ref pathBuffer);
82.                 destinationPolygonsSuccesses[polygonContainerIndex] = success;
83.  
84.                 for (var j = 0; j < MAX_NUM_POLYGONS_PER_PATH; j++)
85.                 {
86.                     var polygonInPath = pathBuffer[j];
87.                     if (polygonInPath.Equals(default))
88.                     {
89.                         break;
90.                     }
91.                     destinationsPaths[polygonContainerIndex * MAX_NUM_POLYGONS_PER_PATH + j] = polygonInPath;
92.                     // if the polygon in the path is the same as another destination
93.                     if (uniqueDestinationsPolygons.ContainsKey(polygonInPath))
94.                     {
95.                         // WARNING: this mutates uniqueDestinationsPolygons
96.                         uniqueDestinationsPolygons.Remove(polygonInPath);
97.                         var polygonContainerInPath = destinationPolygonToContainer[polygonInPath];
98.                         unprocessedSortedDestinationPolygons.Remove(polygonContainerInPath);
99.                         var onTheWayPolygonContainerIndex = destinationPolygonsSorted.IndexOf(polygonContainerInPath);
100.                         destinationPolygonsSuccesses[onTheWayPolygonContainerIndex] = true;
101.                         var onTheWayPolygonStart = onTheWayPolygonContainerIndex * MAX_NUM_POLYGONS_PER_PATH;
102.                         for (var k = 0; k < j; k++)
103.                         {
104.                             var polygonIndex = k;
105.                             var polygonInPathIndex = destinationPolygonsSorted.IndexOf(destinationPolygonToContainer[polygonInPath]);
106.                             var onTheWayPolygonInPath = pathBuffer[polygonIndex];
107.                             destinationsPaths[onTheWayPolygonStart + polygonIndex] = onTheWayPolygonInPath;
108.                         }
109.                     }
110.                 }
111.             }
112.  
113.             var pathSuccess = false;
114.             var straightPath = new NativeArray<NavMeshLocation>(MAX_WAYPOINTS_PER_PATH, Allocator.Temp);
115.             var straightPathFlags = new NativeArray<StraightPathFlags>(MAX_WAYPOINTS_PER_PATH, Allocator.Temp);
116.             var vertexSides = new NativeArray<float>(MAX_WAYPOINTS_PER_PATH, Allocator.Temp);
117.             var straightPathLength = 0;
118.  
119.             for (var i = 0; i < numUserDestinations; i++)
120.             {
121.                 var userDestination = userDestinations[i].potentialDestination;
122.                 var userDestination3D = userDestination.To3D();
123.                 var destinationPolygon = destinationsToPolygons[userDestination];
124.                 var containerIndex = destinationPolygonToSortedIndex[destinationPolygon];
125.                 if (destinationPolygonsSuccesses[containerIndex])
126.                 {
127.                     pathSuccess = true;
128.  
129.                     var polygonPath = destinationsPaths.Slice(
130.                         containerIndex * MAX_NUM_POLYGONS_PER_PATH,
131.                         MAX_NUM_POLYGONS_PER_PATH
132.                     );
133.                     var polygonPathValid = polygonPath.SliceNonDefault(out var numValidPolygons);
134.                     PathUtils.FindStraightPath(
135.                         query,
136.                         userVantagePosition,
137.                         userDestination3D,
138.                         polygonPathValid,
139.                         numValidPolygons,
140.                         ref straightPath,
141.                         ref straightPathFlags,
142.                         ref vertexSides,
143.                         ref straightPathLength,
144.                         MAX_WAYPOINTS_PER_PATH
145.                     );
146.  
147.                     ecb.SetComponent(userEntity, new MovementPlanningDestination
148.                     {
149.                         destinationLocation = ((float3)straightPath[straightPathLength - 1].position).xz
150.                     });
151.  
152.                     var userPaths = ecb.SetBuffer<MovementPlanningWaypointsElement>(userEntity);
153.                     for (var j = 0; j < straightPathLength; j++)
154.                     {
155.                         userPaths.Add(new MovementPlanningWaypointsElement
156.                         {
157.                             waypoint = ((float3)straightPath[j].position).xz
158.                         });
159.                     }
160.  
161.                     break;
162.                 }
163.             }
164.  
165.             ecb.SetComponent(userEntity, new MovementPlanningStatus
166.             {
167.                 success = pathSuccess
168.             });
169.         }
170.  
171.         private struct DestinationPolygonsComparator : IComparer<PolygonProximityContainer>
172.         {
173.             public int Compare(PolygonProximityContainer x, PolygonProximityContainer y)
174.             {
175.                 return (int)-(x.distanceFromStart - y.distanceFromStart);
176.             }
177.         }
178.  
179.         private struct PolygonProximityContainer : IEquatable<PolygonProximityContainer>
180.         {
181.             public PolygonId polygon;
182.             public float distanceFromStart;
183.             public float3 point;
184.  
185.             public bool Equals(PolygonProximityContainer other)
186.             {
187.                 return polygon.Equals(other.polygon);
188.             }
189.         }
190.  
191.         [MethodImpl(MethodImplOptions.AggressiveInlining)]
192.         private bool CalculatePath(NavMeshLocation start, NavMeshLocation end, ref NativeArray<PolygonId> pathBuffer)
193.         {
194.             var status = query.BeginFindPath(start, end, -1);
195.  
196.  
197.             while (true)
198.             {
199.                 switch (status)
200.                 {
201.                     case PathQueryStatus.InProgress:
202.                         status = query.UpdateFindPath(MAX_ITERATIONS_PER_PATH, out int currentIterations);
203.                         break;
204.  
205.                     case PathQueryStatus.Success:
206.                         var finalStatus = query.EndFindPath(out int pathLength);
207.                         var pathResult = query.GetPathResult(pathBuffer);
208.                         return true;
209.  
210.                     case PathQueryStatus.Failure:
211.                         return false;
212.  
213.                     default:
214.                         return false;
215.                 }
216.             }
217.         }
218.     }
219.