Search Unity

  1. We are migrating the Unity Forums to Unity Discussions. On July 12, the Unity Forums will become read-only. On July 15, Unity Discussions will become read-only until July 18, when the new design and the migrated forum contents will go live. Read our full announcement for more information and let us know if you have any questions.

Cost of a NavMeshPath

Discussion in 'Navigation' started by Piflik, Oct 21, 2016.

  1. Piflik

    Piflik

    Joined:
    Sep 11, 2011
    Posts:
    293
    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.     }
     
  2. Jakob_Unity

    Jakob_Unity

    Joined:
    Dec 25, 2011
    Posts:
    269
    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.  
     
    Mycroft, Piflik and JBR-games like this.
  3. Piflik

    Piflik

    Joined:
    Sep 11, 2011
    Posts:
    293
    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.
     
  4. Piflik

    Piflik

    Joined:
    Sep 11, 2011
    Posts:
    293
    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.
     

    Attached Files:

  5. Piflik

    Piflik

    Joined:
    Sep 11, 2011
    Posts:
    293
    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. }
     
    Last edited: Oct 25, 2016
    JBR-games likes this.
  6. Jakob_Unity

    Jakob_Unity

    Joined:
    Dec 25, 2011
    Posts:
    269
    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.
     
  7. Piflik

    Piflik

    Joined:
    Sep 11, 2011
    Posts:
    293
    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.
     
  8. DwinTeimlon

    DwinTeimlon

    Joined:
    Feb 25, 2016
    Posts:
    304
    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.
     
    Last edited: Apr 9, 2019
  9. josh_rake

    josh_rake

    Joined:
    Jan 4, 2017
    Posts:
    15
    @DwinTeimlon The corrections by Piflik don't work?
    Without path cost, the area cost feature is very limited
     
  10. DwinTeimlon

    DwinTeimlon

    Joined:
    Feb 25, 2016
    Posts:
    304
    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.
     
  11. shudrikn

    shudrikn

    Joined:
    Oct 18, 2019
    Posts:
    1
    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.     }
     
  12. nikofon007

    nikofon007

    Joined:
    Sep 10, 2020
    Posts:
    38
    Any updates here? Cause all those solutions still ain't working right
     
  13. VeryRude

    VeryRude

    Joined:
    Jul 24, 2022
    Posts:
    1
    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. }
     
    Jinxology likes this.