Cost of a NavMeshPath

I am currently working on a project, where it would be quite useful to know the cost of a NavMeshPath. Sadly the NavMeshPath does not contain a field for this information. I have written a small piece of code, which works, (and should be no noticeable performance hit on my game, due to its nature), but to me it feels like unnecessary work for a piece of information that the A* that calculates the path in the first place should provide for free (if Unity would store this information with the path).

My question: is there a more intelligent way to do it than this?

Code (CSharp):

1. public static float Cost(this NavMeshPath path) {
2.         if (path.corners.Length < 2) return 0;
3.  
4.         float cost = 0;
5.  
6.         for (int i = 1; i < path.corners.Length; ++i) {
7.             float length = Vector3.Distance(path.corners[i], path.corners[i - 1]);
8.             NavMeshHit hit1, hit2;
9.  
10.             NavMesh.SamplePosition(path.corners[i], out hit1, 1, NavMesh.AllAreas);
11.             NavMesh.SamplePosition(path.corners[i-1], out hit2, 1, NavMesh.AllAreas);
12.  
13.             int index1 = 0, index2 = 0;
14.             int mask1 = hit1.mask, mask2 = hit2.mask;
15.  
16.             while ((mask1 >>= 1) > 0) {
17.                 index1++;
18.             }
19.  
20.             while((mask2 >>= 1) > 0) {
21.                 index2++;
22.             }
23.  
24.             cost += (NavMesh.GetAreaCost(index1) + NavMesh.GetAreaCost(index2))*length*0.5f;
25.         }
26.  
27.         return cost;
28.     }

 

Currently there's no easy way using the existing API. We should add it - note taken!

From script it would be more accurate to do a raycast between the corners of the found path.
Noting that a path segment may cover different area types. One example - using the current cost for each (sub)segment:

Code (CSharp):

1.     // return index for mask if it has exactly one bit set
2.     // otherwise returns -1
3.     int IndexFromMask(int mask)
4.     {
5.         for (int i = 0; i < 32; ++i)
6.         {
7.             if ((1 << i) == mask)
8.                 return i;
9.         }
10.         return -1;
11.     }
12.  
13.     float CalculatePathCost(NavMeshPath path) {
14.         var corners = path.corners;
15.         if (corners.Length < 2)
16.             return Mathf.Infinity;
17.  
18.         var hit = new NavMeshHit();
19.         NavMesh.SamplePosition(corners[0], out hit, 0.1f, NavMesh.AllAreas);
20.  
21.         var pathCost = 0.0f;
22.         var costMultiplier = NavMesh.GetAreaCost(IndexFromMask(hit.mask));
23.         int mask = hit.mask;
24.         var rayStart = corners[0];
25.  
26.         for (int i = 1; i < corners.Length; ++i)
27.         {
28.             // the segment may contain several area types - iterate over each
29.             while (NavMesh.Raycast(rayStart, corners[i], out hit, hit.mask))
30.             {
31.                 pathCost += costMultiplier * hit.distance;
32.                 costMultiplier = NavMesh.GetAreaCost(IndexFromMask(hit.mask));
33.                 mask = hit.mask;
34.                 rayStart = hit.position;
35.             }
36.      
37.             // advance to next segment
38.             pathCost += costMultiplier * hit.distance;
39.             costMultiplier = NavMesh.GetAreaCost(IndexFromMask(hit.mask));
40.             mask = hit.mask;
41.             rayStart = hit.position;
42.         }
43.  
44.         return pathCost;
45.     }
46.  

 

Thank you for your reply and for taking the time to write that script. I'll be going to test it once the other elements are in place.

 

I did a quick test and this script is definitely more accurate than mine, when it works, but I encountered an Index out of bounds Exception with a very simple path that had to avoid one obstacle (A path from (-10,0,0) to (10,0,0), obstacle at (0,0,0), a single 25x25 floor set to 'walkable'). I moved the points a bit, and the error did not occur, but I can't see a pattern.

I attached a sample project that shows the error.

 

I had a closer look at the code. If the raycast hits a NavMesh boundary, the hit.mask is 0. This leads to the IndexFromMask function returning -1, which results in the GetAreaCost function to fail.

A bigger problem is, that any further raycast from that point on the border either returns itself again (if the raycast uses the previous mask, which is unexpected, since this area should be traversable without the ray registering a hit) or a miss at infinity (with any other mask, including 0 or the inverse of the previous mask). This means the function will either return infinity or result in a infinite loop, if the path touches a NavMesh border.

Independent of if the path.cost will be included in the API, this issue should be addressed.

I modified the function above. It sacrifices a bit of accuracy, but it at least always returns a valid value.

Code (CSharp):

1. /// <summary>
2. /// returns the index of the least significant bit of a bitmask; -1 if mask is 0
3. /// </summary>
4. /// <param name="mask"></param>
5. /// <returns></returns>
6. public static int IndexFromMask(int mask) {
7.     for(int i = 0; i < 32; ++i) {
8.         if((1 << i & mask) != 0) {
9.             return i;
10.         }
11.     }
12.     return -1;
13. }
14.  
15. public static float Cost(this NavMeshPath path) {
16.     if (path.corners.Length < 2) return 0;
17.  
18.     float cost = 0;
19.     NavMeshHit hit;
20.     NavMesh.SamplePosition(path.corners[0], out hit, 0.1f, NavMesh.AllAreas);
21.     Vector3 rayStart = path.corners[0];
22.     int mask = hit.mask;
23.     int index = IndexFromMask(mask);
24.  
25.     for (int i = 1; i < path.corners.Length; ++i) {
26.  
27.         while(true) {
28.             NavMesh.Raycast(rayStart, path.corners[i], out hit, mask);
29.  
30.             cost += NavMesh.GetAreaCost(index) * hit.distance;
31.  
32.             if (hit.mask != 0) mask = hit.mask;
33.  
34.             index = IndexFromMask(mask);
35.             rayStart = hit.position;
36.  
37.             if (hit.mask == 0) { //hit boundary; move startPoint of ray a bit closer to endpoint
38.                 rayStart += (path.corners[i] - rayStart).normalized * 0.01f;
39.             }
40.  
41.             if (!hit.hit) break;
42.         }
43.     }
44.  
45.     return cost;
46. }

 

You are correct - the raycast has floating point precision problems at this vertex. Numerically it hits the outer edge before crossing to the next polygon.

 

Is it intended behaviour, that the ray hits the outer edge at all? I can see the value of it hitting the edge from inside, but why would it need to even register from outside the NavMesh? Registering a hit against a NavMesh polygon should cover all cases, and once you hit a polygon, you have a valid mask to ignore that area for further rays.

 

Wanted to reactivate this old thread to check if there is a faster/better way of calculating the path cost @adriant.

Both of the above methods are not working correctly and can lead to infinite loops. I can obviously fix this, but it seems really complicated and slow to get the navigation costs like this.

 

@DwinTeimlon The corrections by Piflik don't work?
Without path cost, the area cost feature is very limited

 

Piflik corrections don't work in my case. I get inifite loops in some cases. So I introduced a maxTries variable to avoid it. It's suboptimal to say the least, but I haven't find a another way around it yet.

 

Work for me (without maxTries):

Code (CSharp):

1.     public static int IndexFromMask(int mask)
2.     {
3.         if (mask == 0)
4.             return -1;
5.  
6.         int i = 0;
7.         while(mask != 1)
8.         {
9.             mask >>= 1;
10.             ++i;
11.         }
12.         return i;
13.     }
14.  
15.     public static int GetAreaMask(ref Vector3 point)
16.     {
17.         NavMeshHit hit;
18.         NavMesh.SamplePosition(point, out hit, 0.1f, NavMesh.AllAreas);
19.         if (!hit.hit)
20.         {
21.             Debug.Log("GetAreaMask failed!");
22.             return 0;
23.         }
24.  
25.         point = hit.position;
26.         return hit.mask;
27.     }
28.  
29.     public static float GetPathCost(in NavMeshPath path)
30.     {
31.         if (path.corners.Length < 2) return 0;
32.  
33.         NavMeshHit hit;
34.         Vector3 rayStart = path.corners[0];
35.         int mask = GetAreaMask(ref rayStart);
36.         if (mask == 0)
37.             return Mathf.Infinity;
38.  
39.         int index = IndexFromMask(mask);
40.  
41.         float cost = 0;
42.         for (int i = 1; i < path.corners.Length; ++i)
43.         {
44.             NavMesh.Raycast(rayStart, path.corners[i], out hit, mask);
45.  
46.             // last raycast
47.             if (!hit.hit)
48.             {
49.                 cost += NavMesh.GetAreaCost(index) * Vector3.Distance(rayStart, path.corners[i]);
50.                 break;
51.             }
52.  
53.             cost += NavMesh.GetAreaCost(index) * hit.distance;
54.  
55.             if (hit.mask != 0)
56.             {
57.                 mask = hit.mask;
58.             }
59.             else
60.             {
61.                 mask = GetAreaMask(ref rayStart);
62.                 if (mask == 0)
63.                     return Mathf.Infinity;
64.             }
65.  
66.             index = IndexFromMask(mask);
67.             rayStart = hit.position;
68.         }
69.  
70.         return cost;
71.     }

 

Any updates here? Cause all those solutions still ain't working right

 

This is a version that should work and doesn't crash.
(Unity Version 2021.3.16f1)

Code (CSharp):

1. using UnityEngine.AI;
2. using UnityEngine;
3.  
4. public static class CaculatePathCost
5. {
6.     public static int IndexFromMask(int mask)
7.     {
8.         for (int i = 0; i < 32; ++i)
9.         {
10.             if ((1 << i & mask) != 0)
11.             {
12.                 return i;
13.             }
14.         }
15.         return -1;
16.     }
17.  
18.     public static float Cost(this NavMeshPath path)
19.     {
20.         if (path.corners.Length < 2) return 0;
21.  
22.         float cost = 0;
23.         NavMeshHit hit;
24.         NavMesh.SamplePosition(path.corners[0], out hit, 0.1f, NavMesh.AllAreas);
25.         Vector3 rayStart = path.corners[0];
26.         int mask = hit.mask;
27.         int index = IndexFromMask(mask);
28.  
29.         for (int i = 1; i < path.corners.Length; ++i)
30.         {
31.             //The 100 is just a random value I chose
32.             for(int x = 0; x < 100; x++)
33.             {
34.                 NavMesh.Raycast(rayStart, path.corners[i], out hit, mask);
35.  
36.                 cost += NavMesh.GetAreaCost(index) * hit.distance;
37.  
38.                 if (hit.mask != 0) mask = hit.mask;
39.  
40.                 index = IndexFromMask(mask);
41.                 rayStart = hit.position;
42.  
43.                 if (hit.mask == 0)
44.                 { //hit boundary; move startPoint of ray a bit closer to endpoint
45.                     rayStart += (path.corners[i] - rayStart).normalized * 0.01f;
46.                 }
47.  
48.                 if (!hit.hit) break;
49.             }
50.         }
51.  
52.         return cost;
53.     }
54. }