Source Code: https://bitbucket.

org/twntduff/navmesh/src

NavMesh
The meshes that graphically display the navmesh object were created
in Blender and loaded in OBJ form. The same vertices and indices lists that
were created during this process are then used to create and connect the
triangle nodes that make up the eNavMesh object. Each triangle node has an
array of pointers that point to adjacent triangles for use in pathfinding, and a
pointer to a parent triangle that is later used to trace back through the
triangles which represent the optimal path, and G/H values that are used
during the A-Star algorithm.

A Star
The A-Star algorithm is used to determine the optimal path of triangles
to reach a goal point. Before the algorithm runs, it is first determined if it is
necessary to do so. If the start point and the goal point are contained in the
same triangle, then there is no point in using the A-Star algorithm to find an
optimal path and the only point that is needed in the path list is the goal
point. If either the start or the goal point is not contained within a triangle in
the navmesh, then we do nothing because the point is either unreachable or
the entity is located outside of the navmesh.

Simple Stupid Funnel Algorithm

Once an optimal path of triangles is found, then the Simple Stupid
Algorithm goes to work finding the points which represent a corner in the
path to the goal. First, the portals are identified by looping through the
pointers to parent triangles and locating the shared vertices of each root and
parent triangle. Then, the vertices on the right and left, with respect to the
characters position as it moves through the path, are identified and stored in
two separate arrays for easy access. The algorithm then begins by assigning
the start point as the initial apex and loops through the left and right
vertices, one at a time calculating the angle between the two at each
iteration until there is an overlap or the angle is larger than on the previous
iteration. When this occurs the side that was advanced is brought back to the
previous vertex and the opposite side is advanced and the algorithm
continues until it finds that the left and right vertices have overlapped. When
an overlap is detected, the left or right vertex that was stationary on the last
iteration is stored in the apex array and it becomes the current apex that is

used in further iterations. The algorithm is completed when the goal triangle
is reached or the left and right array has reached the last item.

Changes in Y
If the project that the navmesh was being used in had no changes in
height from one edge to another, we could stop here and this would be
adequate enough for smooth pathfinding. If the project does require a
navmesh with changes in height, then we have one more step to ensure that
the path that is returned is correct and there are no anomalies. Some
anomalies that may occur is that the entity will go through the floor or hover
above the floor and not follow along the navmesh as it rises and falls. To
account for the changes in the Y direction, instead of returning the apex
array that contains the start, corners, and goal position, the path smoothing
algorithm takes this array and casts a 2D line (X, Z) from one apex to
another and uses the left and right vertices array to plot a point on the edges
that they frame. Once a 2D apex line no longer crosses with an edge, the
apex line is advanced to the next two apexes in the array. Once a point is
located on a line in the X and Z coordinates, it is then plotted on the 3D line
using the parametric equation of a line in 3D space to calculate the height of
which the point should lie at the X and Z coordinates.

3D Picking
The start point of the path is located at the current position of the
entity and the goal point is located at the point where the mouse is located
when the left mouse button is clicked. The 2D point where the mouse is
located when a left click input is received is projected onto the navmesh in
3D space. This is accomplished by casting a ray from the cameras location
towards a position in 3D space. If the ray intersects with a triangle on the
navmesh, the point of intersection is returned and used as the goal point for
pathfinding.

System Limitations
The way that the changes in Y is solved does not allow for edges to
pass under a potential apex line. Therefore, it is important to ensure that a
triangle node that might lie in the optimal path list does not run under a
current apex line when it is constructed. This means that there can be no
sharp turns that run under another turn on the mesh.