public NavigationCell(Vector3 node1, Vector3 node2, Vector3 node3)
{
NodeVertex = new Vector3[] { node1, node2, node3 };
linkedCells = new NavigationCell[3];
Edges = new Vector3[] { node2 - node1, node3 - node2, node1 - node3 };
Center = CalculateCentre();
}
public NavigationCell[] CalculatePath(Vector3 start, Vector3 finish)
{
costTable.Clear();
parent.Clear();
NavigationCell startCell = navMesh.GetCellFromPosition(start);
NavigationCell finishCell = navMesh.GetCellFromPosition(finish);
if (startCell == null || finishCell == null)
{
return(null);
}
var frontier = new PriorityQueue <NavigationCell>();
frontier.Enqueue(startCell, 0);
parent[startCell] = startCell;
costTable[startCell] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current.Equals(finishCell))
{
break;
}
foreach (var next in current.linkedCells)
{
//skip border edges
if (next == null)
{
continue;
}
//cost as distance between mesh centres
float newCost = costTable[current] + Heuristic.EuclideanDistance(next.Center, current.Center);
if (!costTable.ContainsKey(next) || newCost < costTable[next])
{
costTable[next] = newCost;
float priority = newCost + Heuristic.EuclideanDistance(next.Center, finish);
frontier.Enqueue(next, priority);
parent[next] = current;
}
}
}
//path not found
if (!parent.ContainsKey(finishCell))
{
return(new NavigationCell[0]);
}
var cell = finishCell;
var path = new List <NavigationCell>();
path.Add(finishCell);
while (cell != startCell)
{
cell = parent[cell];
path.Add(cell);
}
path.Reverse();
return(path.ToArray());
}
public NavigationCell Copy()
{
var navcell = new NavigationCell();
navcell.NodeVertex = NodeVertex;
navcell.Edges = Edges;
navcell.linkedCells = linkedCells;
navcell.Center = Center;
return(navcell);
}
void CreateActiveArea(Vertex3D[] mesh, int[] indexes)
{
//cells
for (int i = 0; i < navigationCells.Length; i++)
{
navigationCells[i] = new NavigationCell(mesh[indexes[i * 3]].Position, mesh[indexes[i * 3 + 1]].Position, mesh[indexes[i * 3 + 2]].Position);
navigationCells[i].Index = i;
}
//near cells
for (int i = 0; i < navigationCells.Length; i++)
{
var connection = FindConnectedVertices(i * 3, indexes);
for (int n = 0; n < 3; n++)
{
if (connection[n] == -1)
{
continue;
}
navigationCells[i].linkedCells[n] = navigationCells[connection[n]];
}
}
}
