## 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;
HalfEdgeVertex portalLeft = portals;
HalfEdgeVertex portalRight = portals;

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();

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;

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;

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;
}``````

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;
HalfEdgeVertex portalLeft = portals;
HalfEdgeVertex portalRight = portals;

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();

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;
}

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;

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;

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;

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;

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;
}
}``````

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...

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
);
}
}``````

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.

## segunda-feira, 19 de maio de 2014

### Why would we like dot product? Part 3

Finally, this is the third part of things you can do with dot product. I had some difficulty in deciding what to post here. Most tricks would either be slight variations of what has been said in part two, or dot product would be only a helping tool in a complex set of calculations (take Separating Axis Tests, for example). As a result, I have a feeling that this post will be the last post about dot product alone.

### Closest point of a line segment to another point

Sometimes, you will find a problem that can be solved (or reduced) by knowing the point of a line segment that is closest to another point. With this information, you can build up usable data as distance and direction between the points. Suppose, for example, a 2d top view game. As long as obstacles can be modeled as line segments, you can calculate the distance from an object (i.e the player) from the obstacle. If the object is too close, you move it a little so the object and the obstacle do not overlap.

There are numerous way of calculating the closest point of a point to a line segment, and one of them is with dot products. I will post here code for 3d checking, as converting it to 2d, if needed, is really simple. What we will need to find the closest point is:

- Vector3 testPoint: the point that we will check the segment against.
- Vector3 segmentStart: starting position of the line segment. This is one of the segment extremity points.
- Vector3 segmentEnd: ending position of the line segment. This is the other segment extremity point.

Since the calculation is simple, the requisites are also simple - however the magic behind them can be rather tricky to understand. When you calculate a dot product between two normalized vectors, the resulting value is the cosine between them. If the vectors aren't normalized, you don't have the cosine, but you can still get some useful information about their relationship:

- Product is a positive value, so the angle between the vectors is less than 90 degrees.
- Product is zero, so the angle between the vectors is exactly 90 degrees (aka never in computer calculations).
- Product is a negative number, so the angle between the vectors is more than 90 degrees (not bigger than 180 degrees).

So, what we do is calculate the dot product between the line segment against two vectors created with the test point and each of the line segment extremities. If, when calculating, the dot product is zero or negative, then we know that the segment extremity currently being used in the calculations is also the closest point to the testing one. If both dot products are positive, then the closest point is somewhere between the the two points that defines the segment.

We first calculate our line segment as a vector, to do the calculations:

``Vector3 segment = segmentEnd - segmentStart;``

Next, we create new vectors with the testing point and the segment extremities, in order to check if any of them is also the closest point:

``````Vector3 pointToSegmentStart = testPoint - segmentStart;
float dotPointToStart = Vector3.Dot(pointToSegmentStart, segment);
if(dotPointToStart <= 0) return(segmentStart);

Vector3 pointToSegmentEnd = segmentEnd - testPoint;
float dotPointToEnd = Vector3.Dot(pointToSegmentEnd, segment);
if(dotPointToEnd <= 0) return(segmentEnd);``````

Notice that none of the vectors are normalized. This is important when the closest point actually is between the two segment extremities. Now curse the math. You can actually use the dot products (that came from non normalized vectors) to calculate the closest point, by means of a interpolation.

``````Vector3 resultingPoint = Vector3.zero;

resultingPoint.x = segmentStart.x + ((segment.x * dotPointToStart) /
(dotPointToStart + dotPointToEnd));
resultingPoint.y = segmentStart.y + ((segment.y * dotPointToStart) /
(dotPointToStart + dotPointToEnd));
resultingPoint.z = segmentStart.z + ((segment.z * dotPointToStart) /
(dotPointToStart + dotPointToEnd));

return(resultingPoint);``````

The interpolation will return the point of the line segment that is closest to our testing point. The full code follows:

``````static public Vector3 ClosestPointToSegment(
Vector3 testPoint, Vector3 segmentStart, Vector3 segmentEnd
) {
Vector3 segment = segmentEnd - segmentStart;

Vector3 pointToSegmentStart = testPoint - segmentStart;
float dotPointToStart = Vector3.Dot(pointToSegmentStart, segment);
if(dotPointToStart <= 0) return(segmentStart);

Vector3 pointToSegmentEnd = segmentEnd - testPoint;
float dotPointToEnd = Vector3.Dot(pointToSegmentEnd, segment);
if(dotPointToEnd <= 0) return(segmentEnd);

Vector3 resultingPoint = Vector3.zero;
resultingPoint.x = segmentStart.x + ((segment.x * dotPointToStart) /
(dotPointToStart + dotPointToEnd));
resultingPoint.y = segmentStart.y + ((segment.y * dotPointToStart) /
(dotPointToStart + dotPointToEnd));
resultingPoint.z = segmentStart.z + ((segment.z * dotPointToStart) /
(dotPointToStart + dotPointToEnd));
return(resultingPoint);
}``````

Efficiency wise, this way of calculating distance can be pretty fast on gpus, since they are magical with dot products. However, if you are truly worried with performance and/or precision, testing and profiling is strongly advised.