 |
| Red edge-centre path and green optimal path. |
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. |
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. |
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.