Simple Stupid Funnel Algorithm

[EDIT] Fixed that example scenario below.

There's a nice master class over at AIGameDev.com about pathfinding: Hierarchical Pathfinding Tips & Tricks with Alexander Kring. That slide about Left 4 Dead path smoothing hit my pet peeve. It makes me sad that people still use "let's trace the polygon edge midpoints" as the reference algorithm for path smoothing. Really sad.

Funnel algorithm is simple algorithm to find straight path along portals. Of the most detailed descriptions can be found in Efficient Triangulation-Based Pathfinding.

Here's implementation of simple stupid funnel algorithm. Instead of using queue and all that fancy stuff, simple stupid funnel algorithm runs a loop and restarts the loop from earlier location when new corner is added. This means that some portals are calculated a bit more often than potentially necessary, but it makes the implementation a whole lot simpler.



The above image shows six steps of the algorithm. Every time we process a new portal edge (the dashed lines highlighted in yellow), we first:

• Check if the left and right points are inside the current funnel (described by the blue and red lines), if they are, we simple narrow the funnel (A-D).
• If the new left endpoint is outside the funnel, the funnel is not update (E-F)
  
• If the new left end point is over right funnel edge (F), we add the right funnel as a corner in the path and place the apex of the funnel at the right funnel point location and restart the algorithm from there (G).

The same logic goes for left edge too. This is repeated until all the portal edges are processed.

As seen in the above example, the algorithm recalculates some edge several times because of the restart, but since the calculations are so simple, this simple stupid trick makes the implementation much simpler and in practice is faster too.

inline float triarea2(const float* a, const float* b, const float* c)
{
 const float ax = b[0] - a[0];
 const float ay = b[1] - a[1];
 const float bx = c[0] - a[0];
 const float by = c[1] - a[1];
 return bx*ay - ax*by;
}

inline bool vequal(const float* a, const float* b)
{
 static const float eq = 0.001f*0.001f;
 return vdistsqr(a, b) < eq;
}

int stringPull(const float* portals, int nportals,
         float* pts, const int maxPts)
{
     // Find straight path.
 int npts = 0;
     // Init scan state
 float portalApex[2], portalLeft[2], portalRight[2];
 int apexIndex = 0, leftIndex = 0, rightIndex = 0;
 vcpy(portalApex, &portals[0]);
 vcpy(portalLeft, &portals[0]);
 vcpy(portalRight, &portals[2]);

     // Add start point.
 vcpy(&pts[npts*2], portalApex);
 npts++;

 for (int i = 1; i < nportals && npts < maxPts; ++i)
 {
     const float* left = &portals[i*4+0];
     const float* right = &portals[i*4+2];

         // Update right vertex.
        if (triarea2(portalApex, portalRight, right) <= 0.0f)
  {
         if (vequal(portalApex, portalRight) || triarea2(portalApex, portalLeft, right) > 0.0f)
         {
              // Tighten the funnel.
             vcpy(portalRight, right);
             rightIndex = i;
         }
         else
         {
                 // Right over left, insert left to path and restart scan from portal left point.
             vcpy(&pts[npts*2], portalLeft);
             npts++;
                 // Make current left the new apex.
             vcpy(portalApex, portalLeft);
             apexIndex = leftIndex;
                 // Reset portal
             vcpy(portalLeft, portalApex);
             vcpy(portalRight, portalApex);
             leftIndex = apexIndex;
             rightIndex = apexIndex;
                 // Restart scan
             i = apexIndex;
             continue;
         }
     }

         // Update left vertex.
        if (triarea2(portalApex, portalLeft, left) >= 0.0f)
  {
         if (vequal(portalApex, portalLeft) || triarea2(portalApex, portalRight, left) < 0.0f)
         {
                 // Tighten the funnel.
             vcpy(portalLeft, left);
             leftIndex = i;
         }
         else
         {
              // Left over right, insert right to path and restart scan from portal right point.
             vcpy(&pts[npts*2], portalRight);
             npts++;
                 // Make current right the new apex.
             vcpy(portalApex, portalRight);
             apexIndex = rightIndex;
                 // Reset portal
             vcpy(portalLeft, portalApex);
             vcpy(portalRight, portalApex);
             leftIndex = apexIndex;
             rightIndex = apexIndex;
                 // Restart scan
             i = apexIndex;
             continue;
         }
     }
 }
     // Append last point to path.
 if (npts < maxPts)
 {
     vcpy(&pts[npts*2], &portals[(nportals-1)*4+0]);
     npts++;
 }

 return npts;
}

The array portals has all the portal segments of the path to simplify, first left edge point and the right edge point. The first portal's left and right location are set to start location, and the last portals left and right location is set to end location of the path. Something like this:

// Start portal
vcpy(&portals[nportals*4+0], startPos);
vcpy(&portals[nportals*4+2], startPos);
nportals++;
// Portal between navmesh polygons
for (int i = 0; i < path->npolys-1; ++i)
{
 getPortalPoints(mesh, path->poly[i], path->poly[i+1], &portals[nportals*4+0], &portals[nportals*4+2]);
 nportals++;
}
// End portal
vcpy(&portals[nportals*4+0], endPos);
vcpy(&portals[nportals*4+2], endPos);
nportals++;

The cool thing is that this algorithm can be used with pretty much any navigation format. Let it be a grid, quad-tree, connected squares, way-points or navigation mesh. As long as you pass it a list of line segments that describe the portal from one node to another.

This algorithm is also awesome when combined with steering. You can calculate the desired movement velocity by calculating the next few turns and the steer towards the next corner. It is so fast that you can calculate this every iteration just before you use your favorite steering code.

So there you have it. The next person who goes public and suggest to use edge midpoints and raycasting to find more straight path after A* will get his inbox flooded with Cute Overload weekly selection to instigate his pet hate too.