I'm having trouble getting my A* algorithm to work on a nav mesh. The A* implementation is quite similar with few differences.. Euclidean heuristic and G cost calculated by world-space distance. I'm not sure if that is correct.

This is how my nav mesh looks:

http://puu.sh/s2kbM/3eebcbbc12.png

This is the path generated when inputting the green dot as start location and brown dot as end location

http://puu.sh/s2kiB/cfeed04822.png (white cubes represent points on the calculated path)

There should only be 2 or 3 cubes in this case, a pretty straight-forward path. The pathing is always wacky as seen in the above screenshot. What do I need to change to make the algorithm suitable for a nav mesh? What neighbors do I need to return from NodeGraph, simply the next or previous 2 indexes?

A* calculation:

// triA = first vertex index of triangle on starting point of path
// triB = first vertex index of triangle on path destination
var graph = Client.GameView.Instance.NodeGraph;
var start = graph.Nodes[triA];
var end = graph.Nodes[triB];

var openSet = new HeapPriorityQueue<Node>(graph.Nodes.Length);

graph.ResetNodes();

openSet.Enqueue(start, graph.NodeDistance(a, start.vector));
start.gCost = graph.NodeDistance(a, start.vector);
start.opened = true;

while(openSet.Count > 0)
{
    var current = openSet.Dequeue();
    current.closed = true;

    if(current.index == navMesh.triangles[triB]
        || current.index == navMesh.triangles[triB + 1]
        || current.index == navMesh.triangles[triB + 2])
    {
        Node parent = end;
        while(parent != null)
        {
            GameObject.CreatePrimitive(PrimitiveType.Cube).transform.position = parent.vector;
            parent = parent.parent;
        }
        break;
    }

    foreach(Node n in graph.Neighbors(current.index))
    {
        int tentativeG = current.gCost + graph.NodeDistance(current.vector, n.vector);

        if (!n.opened
            || tentativeG < n.gCost)
        {
            n.parent = current;
            n.gCost = tentativeG;
            n.hCost = graph.Heuristic(n.vector, b);

            if (!n.opened)
            {
                // fCost is calculated g + h
                openSet.Enqueue(n, n.fCost);
                n.opened = true;
            }
        }
    }
}

Node class:

public class Node : PriorityQueueNode
{
    public Node(int index, Vector3 vector, int tri)
    {
        this.index = index;
        this.vector = vector;
        this.tri = tri;
    }

    public Node parent { get; set; }
    public int index { get; set; }
    public int tri { get; set; }
    public int hCost { get; set; }
    public int gCost { get; set; }
    public int fCost { get { return hCost + gCost; } }
    public bool opened { get; set; }
    public bool closed { get; set; }
    public Vector3 vector { get; set; }
}

NodeGraph:

public class NodeGraph
{

    public NodeGraph(Mesh navMesh)
    {
        nodes = new Node[navMesh.vertices.Length];

        for(int i = 0; i < navMesh.vertices.Length; i++)
        {
            nodes[i] = new Node(i, navMesh.vertices[i], i);
        }
    }

    private Node[] nodes;

    public Node[] Nodes { get { return nodes; } }

    public void ResetNodes()
    {
        for(int i = 0; i < nodes.Length; i++)
        {
            nodes[i].parent = null;
            nodes[i].opened = false;
            nodes[i].closed = false;
            nodes[i].hCost = 0;
            nodes[i].gCost = 0;
        }
    }

    public Node[] Neighbors(int a)
    {
        List<Node> n = new List<Node>();

        // what do I do here?
        n.Add(nodes[a - 1]);
        n.Add(nodes[a - 2]);
        n.Add(nodes[a - 3]);

        return n.ToArray();
    }

    public int NodeDistance(Vector3 a, Vector3 b)
    {
        return Mathf.RoundToInt(Vector3.Distance(a, b));
    }

    public int Heuristic(Vector3 a, Vector3 b)
    {
        int dx = Mathf.RoundToInt(Mathf.Abs(a.x - b.x));
        int dy = Mathf.RoundToInt(Mathf.Abs(a.z - b.z));
        return Mathf.RoundToInt(10 * Mathf.Sqrt(dx * dx + dy * dy));
    }
}

Neighbors: The neighbors you can return can be anything that you could travel in a straight line without hitting an obstacle. HOWEVER, you'd usually want to only go to the opposite end of every edge that your current vertex is connected to, and you DON'T want to do actual raycasting to determine whether there are obstacles in the way. That's the point of having the NavMesh in the first place; Its edges deal with the "obstacle" checking for you. Since you will be getting neighbors a LOT, I recommend calculating your neighbors for every vertex ahead of time and storing them in the navmesh.

Heuristic: You can just use actual distance here instead of Abs, RoundToInt, etc. I don't know if you can use squared distance (maybe someone else here knows).

After you finish with the A*, you'll want to do "string pulling" (using http://ahamnett.blogspot.ie/2012/10/funnel-algorithm.html or some similar algorithm) in order to straighten the path out so that it goes in straight lines across unobstructed triangles rather than following vertices.

To add to what was said above, it looks like you're trying to navigate via the vertices of your navmesh, but that is not what you want - you want to navigate from edge to edge, or perhaps from centroid to centroid.

You'll want to pre-process the mesh to find the neighbours, and any 2 triangles which share 2 vertices (or vertex indices) share an edge and are therefore neighbours, if you're moving from triangle to triangle. By extension each inner edge is part of 2 triangles and therefore has 4 neighbour edges, i.e. the other edges in those adjacent triangles - and that's for if you're moving from edge to edge.

One way to test to see if your path is straight, is that you can actually just "ray cast" along your polygons. If the ray is able to go to the desired point without any interruptions, than you know for sure that your path is straight.

This is also an early optimization to prevent unneeded path finding searches.

Take a look at the Recast Detour library. In the Detour section, there is a function that does just this.

I found the main source of all my frustration.. the generated mesh had duplicate vertices in some locations as well as shared vertices. Shared vertexes are good, but I assumed because there were shared vertices there wouldn't be any duplicates :angry:

The A* is running almost as expected now, I'm going look into the methods you guys posted. Thanks!