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