public static List <Vector3> GetPath(Vector3 start, Vector3 exit, bool[] obstaclesArray, MapGrid grid) { VertexPosition startVertex = new VertexPosition(start); VertexPosition exitVertex = new VertexPosition(exit); List <Vector3> path = new List <Vector3>(); List <VertexPosition> openedList = new List <VertexPosition>(); HashSet <VertexPosition> closedList = new HashSet <VertexPosition>(); startVertex.estimatedCost = ManhattanDistance(startVertex, exitVertex); openedList.Add(startVertex); VertexPosition currentVertex = null; while (openedList.Count > 0) { openedList.Sort(); currentVertex = openedList[0]; if (currentVertex.Equals(exitVertex)) { while (currentVertex != startVertex) { path.Add(currentVertex.Position); currentVertex = currentVertex.previousVertex; } path.Reverse(); break; } var arrayOfNeighbours = FindNeighboursFor(currentVertex, grid, obstaclesArray); foreach (var neighbour in arrayOfNeighbours) { if (neighbour == null || closedList.Contains(neighbour)) { continue; } if (neighbour.IsTaken == false) { var totalCost = currentVertex.totalCost + 1; var neighbourEstimatedCost = ManhattanDistance(neighbour, exitVertex); neighbour.totalCost = totalCost; neighbour.previousVertex = currentVertex; neighbour.estimatedCost = totalCost + neighbourEstimatedCost; if (openedList.Contains(neighbour) == false) { openedList.Add(neighbour); } } } closedList.Add(currentVertex); openedList.Remove(currentVertex); } return(path); }
private static VertexPosition[] FindNeighboursFor(VertexPosition currentVertex, MapGrid grid, bool[] obstaclesArray) { VertexPosition[] arrayOfNeighbours = new VertexPosition[4]; int arrayIndex = 0; foreach (var possibleNeihgbour in VertexPosition.possibleNeighbours) { Vector3 position = new Vector3(currentVertex.X + possibleNeihgbour.x, 0, currentVertex.Z + possibleNeihgbour.y); if (grid.IsCellValid(position.x, position.z)) { int index = grid.CalculateIndexFromCoordinates(position.x, position.z); arrayOfNeighbours[arrayIndex] = new VertexPosition(position, obstaclesArray[index]); arrayIndex++; } } return(arrayOfNeighbours); }
private static float ManhattanDistance(VertexPosition startVertex, VertexPosition exitVertex) { return(Mathf.Abs(startVertex.X - exitVertex.X) + Mathf.Abs(startVertex.Z - exitVertex.Z)); }