Páginas

segunda-feira, 26 de maio de 2014

Towards a Modified Simple Stupid Funnel Algorithm with Agent Radius

So, you have your pathfinding working. It returns a channel (series of polygons, edges or portals) that you can execute a funnel algorithm on to return the shortest path from start to end in that channel. If you are using a navmesh, its is often (if not nearly always) a better idea to simply grow your obstacles (shrink the navmesh) with a simple convex polygon that represents your agent size. Of course, there is the need of a navmesh specifically to each desired agent radius.

That being said, there is the famous Simple Stupid Funnel Algorithm posted by Mikko Mononen in the Digesting Duck blog. You won't need to account for agent radius since your channel already has it calculated for you, assuming that your navmesh has been shrunk by the agent radius beforehand. Even if it doesn't, there are ways to "fix" the path by using steering movements or other techniques. The internet is full of ideas.

Now, lets say you truly want your funnel algorithm to use the agent radius "on the fly". Things got really trickier, and chances are that have already read "Efficient Triangulation-Based Pathfinding", by Douglas Jon Demyen. In that thesis there is the pseudocode for the Modified Funnel Algorithm that uses agent radius, but if you are like me, you will have a hard time to understand and implement it from that. Another version was posted by jack_1313 on gamedev forums (http://www.gamedev.net/topic/548610-modified-funnel-algorithm/#entry4538426). Its pretty neat and should work fine - as long as your language allows using booleans as integers. There are some neat ways to solve this problem if it does not, but that is not important here.

Truth is, I found it really hard to find more examples of funnel algorithms that consider the agent radius. Based on the funnel algorithms cited above, I decided to try to make a version of my own. I used the Simple Stupid Funnel Algorithm as a starting point, and came back to the Modified Funnel Algorithm and jack_1313's code every time I hit a problem that I didn't know how to solve. As a result, I ended up with an algorithm that seems to work properly. I am unable to think in more tests situation in which it might break, right now. The code will be posted here in the hope that it helps other people to find different ways to solve this problem and share their code and/or ideas, so that this technique won't be such a "secret" anymore.

First I assume that the channel is always valid to that agent radius - that is, the pathfinding already has returned a channel where the agent is able to move. Since I am using pathfinding on a Constrained Delaunay Triangulation, I used the pseudocodes given by Demyen in his thesis during the pathfinding to know if the moving agent could or could not walk into a triangle from a given edge.

My channel is structured as an array of portals, or edges. Every (triangle) edge that the agent must cross to travel from starting position to destination is a portal. Portals are stored in pairs of consecutive elements in the array: even elements are 'left' vertex of the edge, and odd elements are the 'right' vertex of the edge. Left and right here are in channel's viewpoint. Since my edges are stored as counter-clockwise, it is easy to build this channel. The first portal (array indexes 0 and 1) are equal and the starting position of the channel. The last portal (last two indexes) are equal and the final position of the channel. I allow repeated vertices for channel building and traveling simplicity (as with the original Simple Stupid Funnel Algorithm).


List<HalfEdgeVertex> portals


The next problem is the return data of the funnel algorithm. It seems that many people desires that the funnel algorithm itself returns a path. I have no strong opinion on this matter, but I decided that my version won't return a path - it will return the apexes from the channel that the agent will "touch" while traveling the path. I then use this information to process the path the way I want. This gives some flexibility such as using the same funnel algorithm (with different paths processing, e.g flock movement or steering algorithms) for different objects types. With this information, it is entirely possible to plot an exact path while keeping a "radius" distance from the walls, too!

My return data is an array of instances of a data class:


public enum ApexTypes { Point, Left, Right }

public struct Apex {
    public HalfEdgeVertex vertex;
    public ApexTypes type;
}


The apex type flags if the touched apex was on the left or right side of the channel. The 'Point' type is special, used for the first and last apexes in the array (the starting and ending points of the channel). As the apex position, I store the navmesh vertex itself. Not really elegant, but...

First, I will post a direct conversion of the Simple Stupid Funnel Algorithm to my project, so that the changes made to force up agent radius in this becomes easier to spot.


// Using lists since they can be cleared, and will not need
// reallocation of size until it grows bigger than it ever
// was during the game execution. End position is added twice (left and right). Note
// That the first two and last two elements of the portals list are NOT valid mesh vertices.
// Portals format: for a portal i
//     portal[i] = left side
//     portal[i+1] = right side
//
// This algorithm is basically the Simple Stupid Funnel Algorithm posted by
// Mikko in the Digesting Duck blog.
static public void 
Funnel(List<HalfEdgeVertex> portals, ref List<Apex> contactVertices) {
    HalfEdgeVertex portalApex = portals[0];
    HalfEdgeVertex portalLeft = portals[0];
    HalfEdgeVertex portalRight = portals[1];
    
    int portalLeftIndex = 0;
    int portalRightIndex = 0;
    
    // Put the first point into the contact list
    Apex startApex = new Apex();
    startApex.vertex = portalApex;
    startApex.type = ApexTypes.Point;
    
    contactVertices.Clear();
    contactVertices.Add(startApex);

    Vector2 previousValidLSegment = Vector2.zero;
    Vector2 previousValidRSegment = Vector2.zero;
    
    
    
    for(int i = 2; i < portals.Count; i += 2) {
        HalfEdgeVertex left = portals[i];
        HalfEdgeVertex right = portals[i+1];
        
        Vector2 currentLSegment = left.position - portalApex.position;
        Vector2 currentRSegment = right.position - portalApex.position;
        
        
        //Right side
        // Does new 'right' reduce the funnel?
        if(MyMath2D.CrossProduct2D(previousValidRSegment, currentRSegment) > -MyMath2D.tolerance) {
            // Does it NOT cross the left side?
            // Is the apex the same as portal right? (if true, no chance but to move)
            if(
                portalApex == portalRight ||
                MyMath2D.CrossProduct2D(previousValidLSegment, currentRSegment) < MyMath2D.tolerance
            ) {
                portalRight = right;
                previousValidRSegment = currentRSegment;
                portalRightIndex = i;
            } else {
                // Collapse
                portalApex = portalLeft;
                portalRight = portalApex;
                
                Apex apex = new Apex();
                apex.vertex = portalApex;
                apex.type = ApexTypes.Left;
                contactVertices.Add(apex);
                
                portalRightIndex = portalLeftIndex;
                i = portalLeftIndex;
                
                previousValidLSegment = Vector2.zero;
                previousValidRSegment = Vector2.zero;
                
                continue;
            }
        }
        
        // Left Side
        // Does new 'left' reduce the funnel?
        if(MyMath2D.CrossProduct2D(previousValidLSegment, currentLSegment) < MyMath2D.tolerance) {
            // Does it NOT cross the right side?
            // Is the apex the same as portal left? (if true, no chance but to move)
            if(
                portalApex == portalLeft ||
                MyMath2D.CrossProduct2D(previousValidRSegment, currentLSegment) > -MyMath2D.tolerance
            ) {
                portalLeft = left;
                previousValidLSegment = currentLSegment;
                portalLeftIndex = i;
            } else {
                // Collapse
                portalApex = portalRight;
                portalLeft = portalApex;
                
                Apex apex = new Apex();
                apex.vertex = portalApex;
                apex.type = ApexTypes.Right;
                contactVertices.Add(apex);
                
                portalLeftIndex = portalRightIndex;
                i = portalRightIndex;
                
                previousValidLSegment = Vector2.zero;
                previousValidRSegment = Vector2.zero;
                
                continue;
            }
        }
    }
    
    // Put the first point into the contact list
    Apex endApex = new Apex();
    endApex.vertex = portals[portals.Count - 1];
    endApex.type = ApexTypes.Point;
    contactVertices.Add(endApex);
}


If you actually had read this code and the original Simple Stupid Funnel Algorithm, you will see some differences. First, I use a "2d cross product", to know if a vector is clockwise or counter-clockwise from another. Second, I use vectors as in jack_1313's code. This is probably slower than using direct coordinates, but it made the code easier to debug.

Now, to put agent radius in this, its as simple as "considering each apex a circle". It was a long way until I got convinced of this. Really. Pure math fun. We need a way to calculate the tangents from one apex to another. Basically, let A and B be circle center positions, and P a point, you will need the following math methods:
- Inner tangent from A to B
- Outer tangent from A to B (this one is easy)
- Tangent that touches circle (A or B) and passes through P (start and end of channel).

I put all these hours of pure fun using vector math (to avoid using atan2, though vectors normalization can pile up and be as slow) in a single mighty method called GetTangentPoints. The rest of the algorithm is basically the same as the original one, but now double-sized to consider each special case that the agent radius brought to us. Naturally, this algorithm is slower than the original one, too.


// This algorithm is basically the Simple Stupid Funnel Algorithm posted by
// Mikko in the Digesting Duck blog. This one has been modified to account for agent radius.
static public void Funnel(float radius, List<HalfEdgeVertex> portals, ref List<Apex> contactVertices) {
    // In some special cases, it is possible that the tangents of the apexes will
    // cause the funnel to collapse to the left or right portal right before going to
    // the final position. This happens when the final position is more 'outward' than
    // the vector from the apex to the portal extremity, and the final position is
    // actually 'closer' to the previous portal than the 'current' portal extremity.
    // If that happens, we remove the portal before the last from the list. I have no
    // proof that this guarantees the correct behavior, though.
    if(portals.Count >= 8) {
        // This seems to be possible to happen only when there are 4 or more
        // portals (first and last are start and destination)
        int basePortal = portals.Count - 6;
        int lastPortal = portals.Count - 4;
        int destinationPortal = portals.Count - 2;

        // First, check left
        Vector2 baseLast = portals[lastPortal].position - portals[basePortal].position;
        Vector2 baseDest = portals[destinationPortal].position - portals[basePortal].position;
        if(baseDest.sqrMagnitude < baseLast.sqrMagnitude) {
            portals.RemoveRange(lastPortal, 2);
        } else {
            // Now check right
            baseLast = portals[lastPortal+1].position - portals[basePortal+1].position;
            baseDest = portals[destinationPortal+1].position - portals[basePortal+1].position;
            if(baseDest.sqrMagnitude < baseLast.sqrMagnitude) {
                portals.RemoveRange(lastPortal, 2);
            }
        }
    }


    HalfEdgeVertex portalApex = portals[0];
    HalfEdgeVertex portalLeft = portals[0];
    HalfEdgeVertex portalRight = portals[1];
    
    int portalLeftIndex = 0;
    int portalRightIndex = 0;
    
    // Put the first point into the contact list
    Apex startApex = new Apex();
    startApex.vertex = portalApex;
    startApex.type = ApexTypes.Point;
    
    contactVertices.Clear();
    contactVertices.Add(startApex);
    
    ApexTypes currentType = ApexTypes.Point;
    Vector2 previousValidLSegment = Vector2.zero;
    Vector2 previousValidRSegment = Vector2.zero;
    
    
    
    for(int i = 2; i < portals.Count; i += 2) {
        HalfEdgeVertex left = portals[i];
        HalfEdgeVertex right = portals[i+1];

        ApexTypes nextLeft = ApexTypes.Left;
        ApexTypes nextRight = ApexTypes.Right;
        if(i >= portals.Count - 2) {
            // Correct next apex type if we are at the end of the channel
            nextLeft = ApexTypes.Point;
            nextRight = ApexTypes.Point;
        }

        // Build radius-inflated line segments
        Vector2 tempA = portalApex.position, tempB = left.position;
        GetTangentPoints(
            tempA, tempB, currentType, nextLeft, radius, out tempA, out tempB
        );
        Vector2 currentLSegment = tempB - tempA;

        tempA = portalApex.position; tempB = right.position;
        GetTangentPoints(
            tempA, tempB, currentType, nextRight, radius, out tempA, out tempB
        );
        Vector2 currentRSegment = tempB - tempA;
        
        
        //Right side
        // Does new 'right' reduce the funnel?
        if(MyMath2D.CrossProduct2D(previousValidRSegment, currentRSegment) > -MyMath2D.tolerance) {
            // Does it NOT cross the left side?
            // Is the apex the same as portal right? (if true, no chance but to move)
            if(
                portalApex == portalRight ||
                MyMath2D.CrossProduct2D(previousValidLSegment, currentRSegment) < MyMath2D.tolerance
            ) {
                portalRight = right;
                previousValidRSegment = currentRSegment;
                portalRightIndex = i;
            } else {
                // Collapse
                if(currentRSegment.sqrMagnitude > previousValidLSegment.sqrMagnitude) {
                    portalApex = portalLeft;
                    portalRight = portalApex;
                    
                    Apex apex = new Apex();
                    apex.vertex = portalApex;
                    apex.type = ApexTypes.Left;
                    contactVertices.Add(apex);

                    currentType = ApexTypes.Left;
                    
                    portalRightIndex = portalLeftIndex;
                    i = portalLeftIndex;
                } else {
                    portalRight = right;
                    previousValidRSegment = currentRSegment;
                    portalRightIndex = i;

                    portalApex = portalRight;
                    portalLeft = portalApex;
                    
                    Apex apex = new Apex();
                    apex.vertex = portalApex;
                    apex.type = ApexTypes.Right;
                    contactVertices.Add(apex);
                    
                    currentType = ApexTypes.Right;
                    
                    portalLeftIndex = portalRightIndex;
                    i = portalRightIndex;
                }

                previousValidLSegment = Vector2.zero;
                previousValidRSegment = Vector2.zero;

                continue;
            }
        }
        
        // Left Side
        // Does new 'left' reduce the funnel?
        if(MyMath2D.CrossProduct2D(previousValidLSegment, currentLSegment) < MyMath2D.tolerance) {
            // Does it NOT cross the right side?
            // Is the apex the same as portal left? (if true, no chance but to move)
            if(
                portalApex == portalLeft ||
                MyMath2D.CrossProduct2D(previousValidRSegment, currentLSegment) > -MyMath2D.tolerance
            ) {
                portalLeft = left;
                previousValidLSegment = currentLSegment;
                portalLeftIndex = i;
            } else {
                // Collapse
                if(currentLSegment.sqrMagnitude > previousValidRSegment.sqrMagnitude) {
                    portalApex = portalRight;
                    portalLeft = portalApex;
                    
                    Apex apex = new Apex();
                    apex.vertex = portalApex;
                    apex.type = ApexTypes.Right;
                    contactVertices.Add(apex);

                    currentType = ApexTypes.Right;
                    
                    portalLeftIndex = portalRightIndex;
                    i = portalRightIndex;
                } else {
                    portalLeft = left;
                    previousValidLSegment = currentLSegment;
                    portalLeftIndex = i;

                    portalApex = portalLeft;
                    portalRight = portalApex;
                    
                    Apex apex = new Apex();
                    apex.vertex = portalApex;
                    apex.type = ApexTypes.Left;
                    contactVertices.Add(apex);
                    
                    currentType = ApexTypes.Left;
                    
                    portalRightIndex = portalLeftIndex;
                    i = portalLeftIndex;
                }

                previousValidLSegment = Vector2.zero;
                previousValidRSegment = Vector2.zero;

                continue;
            }
        }
    }
    
    // Put the last point into the contact list
    if(contactVertices[contactVertices.Count - 1].vertex == portals[portals.Count-1]) {
        // Last point was added to funnel, so we need to change its type to point
        Apex endApex = new Apex();
        endApex.vertex = portals[portals.Count-1];
        endApex.type = ApexTypes.Point;
        contactVertices[contactVertices.Count-1] = endApex;
    } else {
        // Last point was not added to funnel, so we add it
        Apex endApex = new Apex();
        endApex.vertex = portals[portals.Count - 1];
        endApex.type = ApexTypes.Point;
        contactVertices.Add(endApex);
    }
}


That's it. A channel special case treating (right at the start of the method), new conditionals to know if we are arriving at the end of the channel, two new "collapsing" codes (one for left apex, another for right apex) and a final correction if the last point is or is not in the contact list, that I don't know what causes it to sometimes be there and other times not.

Finally, my method to build the path with agent radius. Yeah, I calculate the tangents all over again...


static public void BuildPath(float radius, List<Apex> contactVertices, ref List<Vector2> path) {
    path.Clear();

    // My channel actually goes from path end to path start, so I need to
    // invert all the apexes sides...

    // Add first node
    path.Add(contactVertices[contactVertices.Count - 1].vertex.position);
    
    for(int i = contactVertices.Count - 2; i >= 0; --i) {
        Vector2 positionA = Vector2.zero, positionB = Vector2.zero;

        ApexTypes invertedA = contactVertices[i+1].type;
        ApexTypes invertedB = contactVertices[i].type;

        if(invertedA == ApexTypes.Left) invertedA = ApexTypes.Right;
        else if(invertedA == ApexTypes.Right) invertedA = ApexTypes.Left;

        if(invertedB == ApexTypes.Left) invertedB = ApexTypes.Right;
        else if(invertedB == ApexTypes.Right) invertedB = ApexTypes.Left;


        GetTangentPoints(
            contactVertices[i+1].vertex.position, contactVertices[i].vertex.position,
            invertedA, invertedB, radius, out positionA, out positionB
        );
        path.Add(positionA);
        path.Add(positionB);
    }
}


This post got quite long already. Way longer than I intended it to be. In the next post, I will write about the special situations that appeared while trying to make the funnel algorithm to use the agent radius, and relate them with the code posted here. As I said previously, this algorithm is not guaranteed to be fail proof as there might be special cases that I didn't cross or think while testing. I hope that this helps someone to come up with different funnel algorithms and maybe share through the internet.

Nenhum comentário:

Postar um comentário