Monday 15 October 2012

Funnel Algorithm

Red edge-centre path and green optimal path.
When pathfinding for an agent, the typical process is compromised of two steps. First, find a list of nodes leading to the goal using an algorithm like A* or Dijkstra's. Then convert this list into a set of discrete path vertices for the agent to use.

When using triangular nodes, such as those in a navigation mesh, the most commonly suggested method for the second step is to simply use the midpoints of each edge which connects two triangles. This path is also often smoothed by stepping through the list and removing redundant vertices.

The problem with this approach is that, depending on how large the triangles are, this can lead to paths which are inefficient and unnatural looking. Since only the centre points are considered, corners are usually not traversed very closely, causing sub-optimal zig-zag behaviour. The funnel algorithm is an alternative which produces a more efficient path solution by expanding a series of left and right edges (funnels) to find the vertices of the path.

Theory


Image courtesy of Mikko Mononen.
The idea behind the funnel algorithm is that by traversing the connecting edges (portals) in a triangle list (channel) we can find the points where a vertex is necessary (i.e. to traverse a corner).

In the figure provided, we can see that in step A our funnel is initialised with an apex (the start) and our left and right sides (the vertices of the first portal). Steps B through D then step through each of the portals, updating the left and right sides as necessary.

In step E we have a problem. If we continue stepping through the portal vertices as we've been doing previously then the funnel will actually widen and consequently intersect the edges of the channel. As such we skip updating the left edge and move onto step F.

Unfortunately there's now another problem. In updating the right side of the funnel, we've caused it to cross over the left side. As such we know that there is a corner in the channel and a new path vertex is required (the current left position). In step G we set the apex to this position, add it to our list of path vertices, and reinitialise the left and right sides. The algorithm then continues until the goal is found.

To summarise, here's the steps for updating the funnel algorithm:
  • Attempt to narrow funnel.
  • If new endpoint widens funnel, skip.
  • If new endpoint crosses over other side, add new vertex and reinitialise funnel.

Implementation


First we want to abort if the node list is not long enough for a true search. The two cases for this are if we only have one node in the list (both points are in a single triangle) or if there are no nodes (no path found).

For the former, we can simply return a new list containing just the end point (since if they're in the same triangle then there are no static obstacles in the way). For the latter, we should return some sort of error code so that the agent knows no path is available. For my implementation, I've chosen to simply return a list containing only the start point, however you could also send a message to inform the agent.
// Abort if list is empty.
if (nodeList.Count <= 0)
{
    return new List<Vector2>() { start };
}

// Return immediately if list only has one entry.
if (nodeList.Count <= 1)
{
    return new List<Vector2>() { end };
}
Next we need to initialise our variables. We want a list for our path vertices, arrays for our our portal vertices, a vector for our current apex and finally indexes for our current left and right endpoints.

My nodes have 2 properties, the vertices that make up the triangle and a list of references to connected or "local" triangles. As such, I need to loop through my node list to determine what the portal vertices are. I also store the vertices on each end of the channel which aren't part of a portal (lines 25-33 and 36-49). The reasoning for this will be explained later.
List<Vector2> path = new List<Vector2>() { start };
Vector2[] leftVertices = new Vector2[nodeList.Count + 1];
Vector2[] rightVertices = new Vector2[nodeList.Count + 1];
Vector2 apex = start;
int left = 1;
int right = 1;

// Initialise portal vertices.
for (int i = 0; i < nodeList.Count - 1; i++)
{
    for (int j = 0; j < nodeList[i].LocalNodes.Length; j++)
    {
        if (nodeList[i].LocalNodes[j] == nodeList[i + 1])
        {
            int k = j + 1 >= nodeList[i].LocalNodes.Length ? 0 : j + 1;

            leftVertices[i + 1] = ((NavTriangle)nodeList[i]).Points[j];
            rightVertices[i + 1] = ((NavTriangle)nodeList[i]).Points[k];
            break;
        }
    }
}

// Initialise portal vertices first point.
for (int j = 0; j < ((NavTriangle)nodeList[0]).Points.Length; j++)
{
    if (((NavTriangle)nodeList[0]).Points[j] != leftVertices[1]
        && ((NavTriangle)nodeList[0]).Points[j] != rightVertices[1])
    {
        leftVertices[0] = ((NavTriangle)nodeList[0]).Points[j];
        rightVertices[0] = ((NavTriangle)nodeList[0]).Points[j];
    }
}

// Initialise portal vertices last point.
for (int j = 0;
    j < ((NavTriangle)nodeList[nodeList.Count - 1]).Points.Length; j++)
{
    if (((NavTriangle)nodeList[nodeList.Count - 1]).Points[j]
        != leftVertices[nodeList.Count - 1]
        && ((NavTriangle)nodeList[nodeList.Count - 1]).Points[j]
        != rightVertices[nodeList.Count - 1])
    {
        leftVertices[nodeList.Count]
            = ((NavTriangle)nodeList[nodeList.Count - 1]).Points[j];
        rightVertices[nodeList.Count]
            = ((NavTriangle)nodeList[nodeList.Count - 1]).Points[j];
    }
}
Once initialised we begin to loop through the portals, narrowing our current funnel. First, we have to check that the new vertex isn't the same as the current one, in which case we skip. We also want to skip if the new side widens the funnel. Finally, we need to check whether the new side crosses the other, if it does then we need to update our funnel, else we can simply update the current edge and move on.
// Step through channel.
for (int i = 2; i <= nodeList.Count - 1; i++)
{
    // If new left vertex is different, process.
    if (leftVertices[i] != leftVertices[left]
        && i > left)
    {
        Vector2 newSide = leftVertices[i] - apex;

        // If new side does not widen funnel, update.
        if (GeometryHelper.VectorSign(newSide,
                leftVertices[left] - apex) > 0)
        {
            // If new side crosses other side, update apex.
            if (GeometryHelper.VectorSign(newSide,
                rightVertices[right] - apex) > 0)
            {
                // Find next vertex.
                for (int j = next; j <= nodeList.Count; j++)
                {
                    if (rightVertices[j] != rightVertices[next])
                    {
                        next = j;
                        break;
                    }
                }

                path.Add(rightVertices[right]);
                apex = rightVertices[right];
                right = next;
            }
            else
            {
                left = i;
            }
        }
    }

    // If new right vertex is different, process.
    ...
}
VectorSign() is a simple method which checks which side of a line another is on. If the line is on the right then it returns 1, else it returns -1.

Non-Point Agents


Red normal path, green adjusted path.
Assuming your agents are not points and that they do collide with the environment then you will notice a problem with the above implementation. In situations where the corners of the channel define physical obstacles in the environment, you will get odd agent behaviour where they travel straight towards the corner rather than moving to travel around it.

This is because the above implementation does not compensate for the size of the agents, simply using the vertices of the nodes. As such you will often encounter situations where agents end up stuck against corners until they can change vertices or sometimes permantently.

Solving this problem is relatively straightforward though. We simply need to offset the vertex positions from the corners to allow the agents to more easily get around them. You could also imagine it like adding a circle of the same radius to each of the channel vertices and navigating around those.

Implementation


In order to offset the position such that the agent has a clear path to both approach and continue from the vertex, we want to calculate its normal. This way we can simply scale by the agent's radius and add that to the position to correctly offset it.

First, we need to find the edge vertices before and after the current one. This is the reason why we needed to include the end points of the channel in our vertex lists, to account for the first and last portals. In my implementation this simply involves looping backward and forward through my vertices until I find a different one (lines 9-16 and 19-26).

We then calculate the angles of the lines formed between the previous-current and current-next segments. We take the average angle and construct a vector perpendicular to it (either left or right depending on if the vertex is on the right or left edge respectively) to create the vertex normal. We then scale this normal by the agent's radius and add it to the position. The rest of the code is the same as before.
// If new side crosses other side, update apex.
if (GeometryHelper.VectorSign(newSide,
    rightVertices[right] - apex) > 0)
{
    int next = right;
    int prev = right;

    // Find next vertex.
    for (int j = next; j <= nodeList.Count; j++)
    {
        if (rightVertices[j] != rightVertices[next])
        {
            next = j;
            break;
        }
    }

    // Find previous vertex.
    for (int j = prev; j >= 0; j--)
    {
        if (rightVertices[j] != rightVertices[prev])
        {
            prev = j;
            break;
        }
    }

    // Calculate line angles.
    float nextAngle = (float)Math.Atan2(
        rightVertices[next].Y - rightVertices[right].Y,
        rightVertices[next].X - rightVertices[right].X);
    float prevAngle = (float)Math.Atan2(
        rightVertices[right].Y - rightVertices[prev].Y,
        rightVertices[right].X - rightVertices[prev].X);

    // Calculate minimum distance between line angles.
    float distance = nextAngle - prevAngle;

    if (Math.Abs(distance) > MathHelper.Pi)
    {
        distance -= distance > 0 ? MathHelper.TwoPi
            : -MathHelper.TwoPi;
    }

    // Calculate left perpendicular to average angle.
    float angle = prevAngle + (distance / 2)
        + MathHelper.PiOver2;
    Vector2 normal = new Vector2((float)Math.Cos(angle),
        (float)Math.Sin(angle));

    // Add new offset apex to list and update right side.
    path.Add(rightVertices[right] + (normal * size));
    apex = rightVertices[right];
    right = next;
}
else
{
    left = i;
}
Obviously it doesn't hurt to have some additional steering behaviours to further improve performance, but this implementation helps prevent agents from getting hung up on corners as easily.

Further Reading

25 comments:

  1. In the first part, can you please elaborate the definitions of nodeList and LocalNodes?

    ReplyDelete
    Replies
    1. You'll have to excuse me, it's been over a year since I wrote this.

      nodeList is simply a List<> of all the nodes in our graph, in this case triangles, and LocalNodes is an array of nodes that are local (or connected to) the node in question.

      Delete
  2. This algorithm works well for turning around corners, but when objects are moving on an open area and there are vertices in the navmesh that are not from obstacles, the unit makes turns around these vertices which looks extremly wierd. These extra vertices a required, because when triangles get to big the navhmesh A* becomes incorrect. Do you know of a way to circumvent this? Maybe by widening the funnel with more triangles somehow?

    ReplyDelete
  3. This is very interesting content! I have thoroughly enjoyed reading your points and have come to the conclusion that you are right about many of them. You are great. Acknowledges for paper such a beneficial composition, I stumbled beside your blog besides decipher a limited announce. I want your technique of inscription... there to everyone, here everybody is sharing such learning, so it's critical to see this website, and I used to visit this blog day by day . Thanks for picking out the time to discuss this, I feel great about it and love studying more on this topic. It is extremely helpful for me. Thanks for such a valuable help again. 안전지대

    ReplyDelete
  4. Oh my goodness! an excellent article dude. Thank you However I'm experiencing problem with ur rss. Do not know why Cannot register for it. Could there be any person getting identical rss difficulty? Anybody who knows kindly respond. Thnkx Its like you read my mind! You appear to grasp so much approximately this, such as you wrote the ebook in it or something. I believe that you just could do with some p.c. to force the message home a little bit, however instead of that, that is great blog. An excellent read. 먹튀신고

    ReplyDelete
  5. Excellent read, Positive site, where did u come up with the information on this posting?I have read a few of the articles on your website now, and I really like your style. Thanks a million and please keep up the effective work. An interesting dialogue is price comment. I feel that it is best to write more on this matter, it may not be a taboo topic however usually individuals are not enough to talk on such topics. To the next. Cheers. You possess lifted an essential offspring..Blesss for using..I would want to study better latest transactions from this blog..preserve posting. Your blogs further more each else volume is so entertaining further serviceable It appoints me befall retreat encore. I will instantly grab your rss feed to stay informed of any updates. 먹튀검증

    ReplyDelete
  6. Nice information, valuable and excellent design, as share good stuff with good ideas and concepts, lots of great information and inspiration, both of which I need, thanks to offer such a helpful information here. I think this is an educational post and it is extremely helpful and learned. along these lines, I might want to thank you for the endeavors you have made in composing this article. Good post but I was wondering if you could write a litte more on this subject? I’d be very thankful if you could elaborate a little bit further. Appreciate it! I am thinking about how I may be told at whatever point another post has been made. I have subscribed to your RSS which may do the trap? Have an extraordinary day! 먹튀사이다

    ReplyDelete
  7. What I don't comprehended is very you're currently not actually substantially more all around preferred than you might be presently. You're extremely shrewd. You understand along these lines significantly with regards to this subject, created me as far as it matters for me envision it from such countless different points. Its like people are not included except if it is one thing to do with Girl crazy! Your individual stuffs extraordinary. Continuously handle it up! You are very much appreciated than you may be at this moment. 토토안전센터

    ReplyDelete
  8. This novel blog is doubtlessly cool and educational. I have found many intriguing advices out of this source. I promotion love to return again soon. Much appreciated ! You ave made some valid statements there. I kept an eye on the web for extra data about the issue and discovered the vast majority will oblige your perspectives on this site. This specific blog is almost certainly engaging furthermore useful. I have picked a lot of helpful things out of this blog. I advertisement love to visit it over and over. You rock! Only wanna say that this is useful , Thanks for taking as much time as necessary to compose this. 카이소

    ReplyDelete
  9. I really loved reading your blog. It was very well authored and easy to understand. Unlike other blogs I have read which are really not that good.Thanks alot! Truly, this article is really one of the very best in the history of articles. I am a antique ’Article’ collector and I sometimes read some new articles if I find them interesting. And I found this one pretty fascinating and it should go into my collection. Very good work! We have a good connection with the simple actions so we can understand the powers using a variety of piece data. Thanks for your quality input. You have a good point here!I totally agree with what you have said!!Thanks for sharing your views...hope more people will read this article!!! 먹튀연구실

    ReplyDelete
  10. Howdy! I'm grinding away surfing around your blog from my new apple iphone! Simply needed to say I love perusing your blog and anticipate every one of your posts! Keep up the remarkable work!| I simply needed to foster a short note to thank you for the entirety of the incredible strategies you are composing here. My long web query has now been compensated with acceptable understanding to discuss with my companions and colleagues. I 'd say that the majority of us guests are amazingly honored to abide in an eminent spot with numerous unique people with supportive ideas. I feel very fortunate to have utilized your site and anticipate a lot of more fun occasions perusing here. Much obliged to you again for a great deal of things. Woah! I'm truly adoring the layout/subject of this blog. It's straightforward, yet successful. A great deal of times it's exceptionally hard to get that wonderful equilibrium" between heavenly ease of use and appearance. I should say you have worked really hard with this. Also, the blog stacks very quick for me on Chrome. Heavenly Blog! 먹튀연구실

    ReplyDelete
  11. This is an awesome article, Given such an extraordinary measure of data in it, These sort of articles keeps the customers excitement for the site, and keep sharing more ... favorable circumstances. Thanks for writing such a good article, I stumbled onto your blog and read a few post. I like your style of writing... Very interesting, good job and thanks for sharing such a good blog. your article is so convincing that I never stop myself to say something about it. You’re doing a great job. Keep it up. The most intriguing content on this fascinating point that can be found on the net ... 먹튀프렌즈

    ReplyDelete
  12. I’m excited to uncover this page. I need to to thank you for ones time for this particularly fantastic read!! I definitely really liked every part of it and i also have you saved to fav to look at new information in your site. Excellent Blog! I would like to thank for the efforts you have made in writing this post. I am hoping the same best work from you in the future as well. I wanted to thank you for this websites! Thanks for sharing. Great websites! There's no doubt i would fully rate it after i read what is the idea about this article. You did a nice job.. 먹튀신고

    ReplyDelete
  13. Very useful post. This is my first time i visit here. I found so many interesting stuff in your blog especially its discussion. Really its great article. Keep it up. Nice post! This is a very nice blog that I will definitively come back to more times this year! Thanks for informative post. I have read all the comments and suggestions posted by the visitors for this article are very fine,We will wait for your next article so only.Thanks I think this is an informative post and it is very beneficial and knowledgeable. Therefore, I would like to thank you for the endeavors that you have made in writing this article. All the content is absolutely well-researched. Thanks 토토서치

    ReplyDelete
  14. Wonderful web site. A lot of useful info here. I’m sending it to a few pals ans additionally sharing in delicious. And certainly, thank you for your sweat! What a Excellent post. I really found this to much informatics. It is what i was searching for.I would like to suggest you that please keep sharing such type of info.Thanks You have a good point here!I totally agree with what you have said!!Thanks for sharing your views...hope more people will read this article!! Thanks for sharing nice information with us. i like your post and all you share with us is uptodate and quite informative, i would like to bookmark the page so i can come here agaiWonderful web site. A lot of useful info here. I’m sending it to a few pals ans additionally sharing in delicious. And certainly, thank you for your sweat! What a Excellent post. I really found this to much informatics. It is what i was searching for.I would like to suggest you that please keep sharing such type of info.Thanks You have a good point here!I totally agree with what you have said!!Thanks for sharing your views...hope more people will read this article!! Thanks for sharing nice information with us. i like your post and all you share with us is uptodate and quite informative, i would like to bookmark the page so i can come here again to read you, as you have done a wonderful job. 먹튀프렌즈
    n to read you, as you have done a wonderful job. 먹튀프렌즈

    ReplyDelete
  15. I am definitely enjoying your website. You definitely have some great insight and great stories. Your article has piqued a lot of positive interest. I can see why since you have done such a good job of making it interesting. I am incapable of reading articles online very often, but I’m happy I did today. It is very well written, and your points are well-expressed. I request you warmly, please, don’t ever stop writing. Nice post. I was checking constantly this blog and I’m impressed! Extremely useful info specially the last part I care for such information a lot. I was seeking this certain info for a long time. Thank you and good luck. 온카맨

    ReplyDelete
  16. This is a great article and great read for me. It's my first visit to your blog, and I have found it so useful and informative especially this article 먹튀검증

    ReplyDelete
  17. I feel a lot more people need to read this, very good info! 파워사다리

    ReplyDelete
  18. Wow, wonderful blog layout! How long have you ever been blogging for? you made running a blog glance easy. The full look of your website is great, let alone the content! canaan avalon 1246

    ReplyDelete
  19. I simply want to tell you that I am new to weblog and definitely liked this blog site. Very likely I’m going to bookmark your blog . You absolutely have wonderful stories. Cheers for sharing with us your blog. social media management australia

    ReplyDelete
  20. It is somewhat fantastic, and yet check out the advice at this treat. 슬롯사이트

    ReplyDelete
  21. Hi, I also visit this site daily; this webpage is truly pleasant & users are openly sharing fastidious thoughts 온라인바카라

    ReplyDelete
  22. Very interesting, good job and thanks for sharing such a good blog. your article is so convincing that I never stop myself to say something about it. You’re doing a great job. Keep it up 파워사다리

    ReplyDelete
  23. I simply want to tell you that I am new to weblog and definitely liked this blog site. Very likely I’m going to bookmark your blog . You absolutely have wonderful stories. Cheers for sharing with us your blog. 온라인바카라

    ReplyDelete