Волновой алгоритм

Наступит время, и нужно будет реализовать алгоритм нахождения пути. Если покопаться - то можно найти различные решения: А-звездочка, волновой алгоритм, Дейскстри…
Выбор будет зависеть от прославленной задачи. Мне пришлось реализовать алгоритм самому ничего короткого и на нужном языке C# мне не подошло. Поэтому опишу здесь как суть алгоритма (Волновой алгоритм), так и основу кода (ничего лишнего).

Суть алгоритма.
Существует 2-е точки одна старт другая финиш. Эти 2-е точки находятся в лабиринте нужно найти путь от старта к финишу.
Представление данных.
За лабиринт сойдет матрица (двухмерный массив) int. Если значение ячейки = 1 то это стена, если = 0 то ячейка проходима. Объект может двигаться в 4-х направлениях: вверх, вниз, влево, вправо.
Основной цикл (теория).
Начинаем со старта. Заносим в соседние 4-е ячейки значение шага. На следующем инкременте цикла увеличиваем значение шага на 1. Находим значение шаг-1 и заполняем соседние ячейки значением шага. Заполнять соседние ячейки нужно осторожно, ячейка не должна быть пройдена ранее или не должна быть стеной (непроходимой). И так пока не достигнем финиша…

Если решения нет – то нужно в основном цикле учитывать, что количество шагов не должно превышать количество ячеек на поле. Попробуйте несколько раз нарисовать на листочке «волны».

Реализация.
Первым делом заполняем копию матрицы таким образом, чтобы -2 это значение стены, а -1 это значение не пройденной ячейки. Я решил идти от финиша. Значение финиша = 0 = значение шага. Далее как по теории перебираем все ячейки пока не находим те которые == шагу. Когда находим – ставим в соседние ячейки в соответствии с правилами значение шага (еще не пройдена и не стена). Выходим из основного цикла когда значение ячейки старта не равно -1 или когда нет решения (количество шагов больше количества ячеек). В конце выводим полученную матрицу. Я не просчитываю сам путь в данном примере, для этого просто пройдитесь от финиша к старту по меньшей стоимости шага пока не достигните старта.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace AStarInMatrix
{
    class AStar
    {
        int[,] Map;
        int MapWidht;
        int MapHeight;

        int[,] WayMap;
        /// <summary>
        /// Конструктор
        /// </summary>
        public void ReadMap()
        {
            MapWidht = 16;
            MapHeight = 9;
            Map = new int[,]{
                {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1},
                {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
                {1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1},
                {1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1},
                {1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1},
                {1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1},
                {1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1},
                {1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1},
                {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1}};
            WayMap = new int[10, 10];
        }
        /// <summary>
        /// Отображение карты
        /// </summary>
        public void DrawMap()
        {
            for (int y = 0; y < MapHeight; y++)
            {
                Console.WriteLine();
                for (int x = 0; x < MapWidht; x++)
                    if (Map[y,x]==1)
                        Console.Write("+");
                    else
                        Console.Write(" ");
            }
            Console.ReadKey();
            FindWave(1, 1, 3, 4);
        }
        /// <summary>
        /// Поиск пути
        /// </summary>
        /// <param name="startX">Координата старта X</param>
        /// <param name="startY">Координата старта Y</param>
        /// <param name="targetX">Координата финиша X</param>
        /// <param name="targetY">Координата финиша Y</param>
        public void FindWave(int startX, int startY, int targetX, int targetY)
        {
            bool add=true;
            int[,] cMap = new int[MapHeight,MapWidht];
            int x, y,step=0;
            for (y = 0; y < MapHeight; y++)
                for (x = 0; x < MapWidht; x++)
                {
                    if (Map[y, x] == 1)
                        cMap[y, x] = -2;//индикатор стены
                    else
                        cMap[y, x] = -1;//индикатор еще не ступали сюда
                }
            cMap[targetY,targetX]=0;//Начинаем с финиша
            while (add==true)
            {
                add = false;
                for (y = 0; y < MapWidht; y++)
                    for (x = 0; x < MapHeight; x++)
                    {
                        if (cMap[x, y] == step)
                        {
                            //Ставим значение шага+1 в соседние ячейки (если они проходимы)
                            if (y - 1 >= 0 && cMap[x - 1, y] != -2 && cMap[x - 1, y] == -1)
                                cMap[x - 1, y] = step + 1;
                            if (x - 1 >= 0 && cMap[x, y - 1] != -2 && cMap[x, y - 1] == -1)
                                cMap[x, y - 1] = step + 1;
                            if (y + 1 < MapWidht && cMap[x + 1, y] != -2 && cMap[x + 1, y] == -1)
                                cMap[x + 1, y] = step + 1;
                            if (x + 1 < MapHeight && cMap[x, y + 1] != -2 && cMap[x, y + 1] == -1)
                                cMap[x, y + 1] = step + 1;
                        }
                     }
                step++;
                add = true;
                if (cMap[startY,startX] != -1)//решение найдено
                    add = false;
                if (step > MapWidht * MapHeight)//решение не найдено
                    add = false;
            }
            //Отрисовываем карты
            for (y = 0; y < MapHeight; y++)
            {
                Console.WriteLine();
                for (x = 0; x < MapWidht; x++)
                    if (cMap[y, x] == -1)
                        Console.Write(" ");
                    else
                    if (cMap[y, x] == -2)
                            Console.Write("#");
                    else
                    if (y == startY && x == startX)
                            Console.Write("S");
                    else
                    if (y == targetY && x == targetX)
                            Console.Write("F");
                    else
                    if (cMap[y, x] > -1)
                        Console.Write("{0}", cMap[y, x]);

            }
            Console.ReadKey();
        }
    }
}