Алгоритм A-star (А*), оптимизация - C#

Формулировка задачи:

Всем привет пытаюсь допилить алгоритм Astar и подстроить его под свои нужды. Я добавил почти весь код, если вдруг кому то будет интересно глянуть.

while (contCheck)
            {
                int x = closedGridTIle[closedGridTIle.Count - 1].TileCoordinates.X;
                int z = closedGridTIle[closedGridTIle.Count - 1].TileCoordinates.Y;

                if (!gridTIle.ContainsKey(new Point(x, z)))
                {
                    Tile myTile = new Tile();
 
                    if (uberGrid[x, z] == 255 || uberGrid[x, z] == 1)                   
                        myTile.Walkable = false;
                    else
                        myTile.Walkable = true;
 
                    myTile.Checked = false;
                    myTile.BaseCost = 0;
                    gridTIle.Add(new Point(x, z), myTile);
                }
 
                //Console.WriteLine(LINQTIle.Count());
 
                FuncA(x, z);
                FuncB(x, z);
                FuncC(x, z);
                FuncD(x, z);
 
                LastFuc(x, z);
            }
 
            t.Stop();
            Console.WriteLine("\nTime {0}", t.Elapsed);

private static void FuncA(int x, int z)
        {
            #region FuncA
            if (x - 1 >= 0)
            {
                if (!gridTIle.ContainsKey(new Point(x - 1, z)))
                {
                    Tile myTile = new Tile();
 
                    if (uberGrid[x - 1, z] == 255 || uberGrid[x - 1, z] == 1)  
                        myTile.Walkable = false;
                    else
                        myTile.Walkable = true;
 
                    myTile.Checked = false;
                    myTile.BaseCost = 0;
                    gridTIle.Add(new Point(x - 1, z), myTile);
                }

                if (gridTIle[new Point(x - 1, z)].Walkable &&
                     !gridTIle[new Point(x - 1, z)].Checked &&
                     !openedGridTIle.ContainsKey(new Point(x - 1, z)))
                {
 
                    gridTIle[new Point(x - 1, z)].ParentTileCoordinates = new Point(x, z); // Родительская ячейка.
                    gridTIle[new Point(x - 1, z)].TileCoordinates = new Point(x - 1, z);
 
                    gridTIle[new Point(x - 1, z)].GCost = CoefficientPath + gridTIle[new Point(x, z)].GCost;   // стоимость передвижения из стартовой точки A к данной клетке, следуя найденному пути к этой клетке.
                    gridTIle[new Point(x - 1, z)].HCost = CalcHCost(x - 1, z, endPosX, endPosZ, size);  // приблизительное количество ходов до конечной позиции ортогональное вычисление
 
                    gridTIle[new Point(x - 1, z)].TotalCost =
                        gridTIle[new Point(x - 1, z)].GCost +
                        gridTIle[new Point(x - 1, z)].HCost +
                        gridTIle[new Point(x - 1, z)].BaseCost;
 
                    //Console.WriteLine("Working = {0} {1}", x, z);
 
                    openedGridTIle.Add(new Point(x - 1, z), gridTIle[new Point(x - 1, z)].Checked);
 
                    // ------------------------- Test -------------------------
                    LINQTIle.Add(gridTIle[new Point(x - 1, z)]);
                    // ------------------------- Test -------------------------
 
                }
            }
            #endregion
        }
 
        private static void FuncB(int x, int z)
        {
            #region FuncB
            if (x + 1 <= size - 1)
            {
                if (!gridTIle.ContainsKey(new Point(x + 1, z)))
                {
                    Tile myTile = new Tile();
 
                    if (uberGrid[x + 1, z] == 255 || uberGrid[x + 1, z] == 1)  
                        myTile.Walkable = false;
                    else
                        myTile.Walkable = true;
 
                    myTile.Checked = false;
                    myTile.BaseCost = 0;
                    gridTIle.Add(new Point(x + 1, z), myTile);
                }
 
                if (gridTIle[new Point(x + 1, z)].Walkable &&
                    !gridTIle[new Point(x + 1, z)].Checked &&
                    !openedGridTIle.ContainsKey(new Point(x + 1, z)))
                {
 
                    gridTIle[new Point(x + 1, z)].ParentTileCoordinates = new Point(x, z); // Родительская ячейка.
                    gridTIle[new Point(x + 1, z)].TileCoordinates = new Point(x + 1, z);
 
                    gridTIle[new Point(x + 1, z)].GCost = CoefficientPath + gridTIle[new Point(x, z)].GCost; // стоимость передвижения из стартовой точки A к данной клетке, следуя найденному пути к этой клетке.
                    gridTIle[new Point(x + 1, z)].HCost = CalcHCost(x + 1, z, endPosX, endPosZ, size); // приблизительное количество ходов до конечной позиции ортогональное вычисление
 
                    gridTIle[new Point(x + 1, z)].TotalCost =
                        gridTIle[new Point(x + 1, z)].GCost +
                        gridTIle[new Point(x + 1, z)].HCost +
                        gridTIle[new Point(x + 1, z)].BaseCost;
 
                    //Console.WriteLine("Working = {0} {1}", x, z);
 
                    openedGridTIle.Add(new Point(x + 1, z), gridTIle[new Point(x + 1, z)].Checked);
 
                    // ------------------------- Test -------------------------
                    LINQTIle.Add(gridTIle[new Point(x + 1, z)]);
                    // ------------------------- Test -------------------------
                }
            }
            #endregion
        }
 
        private static void FuncC(int x, int z)
        {
            #region FuncC
            if (z - 1 >= 0)
            {
                if (!gridTIle.ContainsKey(new Point(x, z - 1)))
                {
                    Tile myTile = new Tile();
 
                    if (uberGrid[x, z - 1] == 255 || uberGrid[x, z - 1] == 1)  
                        myTile.Walkable = false;
                    else
                        myTile.Walkable = true;
 
                    myTile.Checked = false;
                    myTile.BaseCost = 0;
                    gridTIle.Add(new Point(x, z - 1), myTile);
                }
 
                if (gridTIle[new Point(x, z - 1)].Walkable &&
                    !gridTIle[new Point(x, z - 1)].Checked &&
                    !openedGridTIle.ContainsKey(new Point(x, z - 1)))
                {
                    gridTIle[new Point(x, z - 1)].ParentTileCoordinates = new Point(x, z); // Родительская ячейка.
                    gridTIle[new Point(x, z - 1)].TileCoordinates = new Point(x, z - 1);
 
                    gridTIle[new Point(x, z - 1)].GCost = CoefficientPath + gridTIle[new Point(x, z)].GCost;  // стоимость передвижения из стартовой точки A к данной клетке, следуя найденному пути к этой клетке.
                    gridTIle[new Point(x, z - 1)].HCost = CalcHCost(x, z - 1, endPosX, endPosZ, size); // приблизительное количество ходов до конечной позиции ортогональное вычисление
 
                    gridTIle[new Point(x, z - 1)].TotalCost =
                        gridTIle[new Point(x, z - 1)].GCost +
                        gridTIle[new Point(x, z - 1)].HCost +
                        gridTIle[new Point(x, z - 1)].BaseCost;
 
                    //Console.WriteLine("Working = {0} {1}", x, z);
 
                    openedGridTIle.Add(new Point(x, z - 1), gridTIle[new Point(x, z - 1)].Checked);
 
                    // ------------------------- Test -------------------------
                    LINQTIle.Add(gridTIle[new Point(x, z - 1)]);
                    // ------------------------- Test -------------------------
                }
            }
            #endregion
        }
 
        private static void FuncD(int x, int z)
        {
            #region FuncD
            if (z + 1 <= size - 1)
            {
                if (!gridTIle.ContainsKey(new Point(x, z + 1)))
                {
                    Tile myTile = new Tile();
 
                    if (uberGrid[x, z + 1] == 255 || uberGrid[x, z + 1] == 1) 
                        myTile.Walkable = false;
                    else
                        myTile.Walkable = true;
 
                    myTile.Checked = false;
                    myTile.BaseCost = 0;
                    gridTIle.Add(new Point(x, z + 1), myTile);
                }

                if (gridTIle[new Point(x, z + 1)].Walkable &&
                    !gridTIle[new Point(x, z + 1)].Checked &&
                    !openedGridTIle.ContainsKey(new Point(x, z + 1)))
                {
                    gridTIle[new Point(x, z + 1)].ParentTileCoordinates = new Point(x, z); // Родительская ячейка.
                    gridTIle[new Point(x, z + 1)].TileCoordinates = new Point(x, z + 1);
 
                    gridTIle[new Point(x, z + 1)].GCost = CoefficientPath + gridTIle[new Point(x, z)].GCost;  // стоимость передвижения из стартовой точки A к данной клетке, следуя найденному пути к этой клетке.
                    gridTIle[new Point(x, z + 1)].HCost = CalcHCost(x, z + 1, endPosX, endPosZ, size); // приблизительное количество ходов до конечной позиции ортогональное вычисление
 
                    gridTIle[new Point(x, z + 1)].TotalCost =
                        gridTIle[new Point(x, z + 1)].GCost +
                        gridTIle[new Point(x, z + 1)].HCost +
                        gridTIle[new Point(x, z + 1)].BaseCost;
 
                    //Console.WriteLine("Working = {0} {1}", x, z);
 
                    openedGridTIle.Add(new Point(x, z + 1), gridTIle[new Point(x, z + 1)].Checked);
 
                    // ------------------------- Test -------------------------
                    LINQTIle.Add(gridTIle[new Point(x, z + 1)]);
                    // ------------------------- Test -------------------------
                }
            }
            #endregion
        }

   private static void LastFuc(int x, int z)
        {
            #region LastFuc
            //var uncheckedTiles = from k in openedGridTIle where !k.Value.Checked select k;  // поиск всех ячеейк которые еще не чекнуты.
            // -------------------------------------------------------------- Test --------------------------------------------------------------
            //var uncheckedTiles = from k in LINQTIle where !k.Checked select k;  // поиск всех ячеейк которые еще не чекнуты.
            // -------------------------------------------------------------- Test --------------------------------------------------------------
 
            if (LINQTIle.Count() > 0)
            {
                if (openedGridTIle.ContainsKey(new Point(endPosX, endPosZ)))
                {
                    Tile lastTile = new Tile();
 
                    lastTile.ParentTileCoordinates = new Point(x, z);
                    lastTile.TileCoordinates = new Point(endPosX, endPosZ);
 
                    closedGridTIle.Add(lastTile);
 
                    Console.WriteLine("Путь найден");
                    openedGridTIle.Clear();
                    gridTIle.Clear();
                    contCheck = false;
                }
                else
                {
                    // Поиск ячеек наименьшей стоимости.
                    var bestTile = (from i in LINQTIle
                                    let minCost = LINQTIle.Min(m => m.TotalCost)
                                    where i.TotalCost == minCost
                                    select i).FirstOrDefault();
 
                    bestTile.Checked = true;
 
                    closedGridTIle.Add(bestTile);
                    // Поиск ячеек наименьшей стоимости.
 
                    LINQTIle.Remove(bestTile);
 
                }
            }
            else if (LINQTIle.Count() == 0)
            {
                contCheck = false;  // если количество не чекнутых ячеек 0 то путь невозможно найти.
                Console.WriteLine("Путь не найден, его не может быть.");
                openedGridTIle.Clear();
                gridTIle.Clear();
                closedGridTIle.Clear();
 
                // Если путь не найден возвращаем стартовую позицию
                List<Path> startPath = new List<Path>();
                Path startPos = new Path();
                startPos.PosX = startPosX;
                startPos.PosZ = startPosZ;
                startPos.TileCost = 0;
                startPath.Add(startPos);
                // Если путь не найден возвращаем стартовую позицию
 
                return;
 
                //Console.WriteLine(LINQTIle.Count());
            }
            #endregion
        }

Практически все функции однотипные они чекают клетки x -1, x + 1, z +1, z - 1 и проверяют есть ли эти клетки в списке, если есть то не добавляют их. А последняя функция проверяет дошли ли мы до конечной точки. При массиве 200 х 200 если я беру конечную позицию 190 на 190 он ищет от 0.9 до 1.1 секунды. если массив 300 х 300 ищет 3.5. Пока не перестроил запросы LINQ и не исправил коэффициенты, все искало еще дольше поиск занимал от 10 до 15 и больше секунд. Я никогда не занимался такими вещами я уже много чего попробовал для оптимизации и так далее. Чекал через профайлер, Самое проблемное место это последняя функция, когда уже список растет хоть я и удаляю из него элементы все равно при сравнении тратится много ресурсов.

// Поиск ячеек наименьшей стоимости.
                    var bestTile = (from i in LINQTIle
                                    let minCost = LINQTIle.Min(m => m.TotalCost)
                                    where i.TotalCost == minCost
                                    select i).FirstOrDefault();

Собственно вопрос можно как то ускорить выполнение работы? Дожать выполнение до 1 секунды, что то перестроить у меня уже нет идей. Я тут прочел в инете что есть разные версии сборок что типо если релиз есть настройки оптимизации и так далее но нигде не нашел. Что посоветуете?

Решение задачи: «Алгоритм A-star (А*), оптимизация»

textual
                            Листинг программы 
                        

                            using System;
using System.Collections.Generic;
 
namespace Dijkstra
{
    class Program
    {
        private static void Main()
        {
 
            const int n = 5;
            var adjacencyMatrix = new int[n, n];
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    adjacencyMatrix[i, j] = -1;
                }
            }
            adjacencyMatrix[0, 1] = 10;
            adjacencyMatrix[0, 3] = 30;
            adjacencyMatrix[0, 4] = 100;
            adjacencyMatrix[1, 2] = 50;
            adjacencyMatrix[2, 0] = 70;
            adjacencyMatrix[2, 4] = 10;
            adjacencyMatrix[3, 2] = 20;
            adjacencyMatrix[4, 3] = 60;
 
            var costs = new int[n];
            for (int i = 1; i < costs.Length; i++)
            {
                costs[i] = int.MaxValue;
            }
 
            for (int i = 0; i < n; i++)
            {
                var list = new List<int>();
                for (int j = i; j < n; j++)
                {
                    if (adjacencyMatrix[i, j] != -1)
                        list.Add(j);
                }
                list.Sort((x, y) => adjacencyMatrix[i, x].CompareTo(adjacencyMatrix[i, y])); //Сортируем по мин. стоимости пути
                foreach (var j in list)
                {
                    var newcost = costs[i] + adjacencyMatrix[i, j];
                    if (newcost < costs[j])
                        costs[j] = newcost;
                }
            }
            for (int i = 0; i < costs.Length; i++)
            {
                Console.WriteLine("Пункт {0}, стоимость пути = {1}", i, costs[i]);
            }
            Console.ReadKey();
        }
    }
}
                        