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