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