{{notification.text}}

MirGames

Любой разработчик игр рано или поздно сталкивается с проблемой поиска путей. Конечно же, эта задача решалась уже не раз, и есть несколько простых алгоритмов, которые оптимально позволяют находить путь. В топике в основном рассматриваются точные алгоритмы, т.е. всегда гарантирующие результат. Кроме того, все рассмотренные ниже алгоритмы работают на графах.

Начнем мы с двухмерного поля, некоторые ячейки которого непроходимы. Замечу, что такое поле является частным случаем графов.

АлгоритмСкоростьОграничения
Поиск в глубинуO(V+E)Найденный путь не является кратчайшим
Волновой алгоритмO(V2+E)Работает на невзвешенном графе
ДейкстраO(V2+E)Не должно быть ребер с отрицательным весом
Дейкстра с кучейO(V*logV+E)Не должно быть ребер с отрицательным весом
Матрица расстоянийO(V+E)Нужна память O(V2)
Алгоритм на очередяхO(V*n+E)Не должно быть ребер с отрицательным весом

Примечания: V — число вершин в графе, E — число ребер, n — константа O(n)-графа.

Поиск в глубину
Поиск в глубину — рекурсивный алгоритм. На каждом шаге рекурсии мы передаем текущее положение — в нашем случае координаты X и Y. Если в ячейке (X, Y) мы еще не были и она не является финишной, то для каждого соседа вызываем поиск в глубину. Вот моя реализация:
function SearchWay(StartX, StartY, FinishX, FinishY: Integer): Boolean;
  // Рекурсивная функция, ищущая первый попавшийся путь
  // от (X, Y) до (FinishX, FinishY) и возвращающая успешность
  function DepthSearch(X, Y: Integer): Boolean;
  begin
    Result := False;
    if (X < 0) Or (Y < 0) Or (X > High(Cells)) Or (Y > High(Cells[0])) then
      Exit; // Если (X, Y) вне поля, то путь не ищем
    if Cells[X][Y].Locked Or Cells[X][Y].Marked then
      Exit; // Если (X, Y) заблокировано или мы в нем уже были, путь не ищем
    SetLength(Way, Length(Way) + 1); // Добавляем в будущий путь точку (X, Y)
    Way[High(Way)].X := X;
    Way[High(Way)].Y := Y;
    Cells[X][Y].Marked := True; // Отмечаем, что в (X, Y) мы уже были
    Result := True;
    if (X = FinishX)And(Y = FinishY) then
      Exit; // Если мы на конечной, то путь найден, выходим из рекурсии
    if DepthSearch(X + 1, Y) then
      Exit; // Отправляемся вправо
    if DepthSearch(X - 1, Y) then
      Exit; // ... влево
    if DepthSearch(X, Y + 1) then
      Exit; // ... вниз
    if DepthSearch(X, Y - 1) then
      Exit; // ... вверх
  // Путь не найден
    SetLength(Way, Length(Way) - 1); // Убираем (X, Y) из списка
    Result := False; 
  end;

var
  i, j: Integer;
begin
  for i := 0 to High(Cells) do
    for j := 0 to High(Cells[i]) do
      Cells[i][j].Marked := False; // Мы еще нигде не были
  SetLength(Way, 0); // Обнуляем массив с путем
  Result := DepthSearch(StartX, StartY); // Запускаем поиск в глубину
end;

Главная проблема алгоритма заключается в том, что он не находит кратчайший путь, а находит первый попавшийся. Это, конечно, можно исправить, например, не выходя сразу из функции, а продолжая поиск. Тогда при нахождении пути нужно сравнивать его с найденным ранее и записывать тот, который короче. Но тогда очень остро встает проблема со скоростью, т.к. тогда алгоритм будет просматривать все возможные пути от начала в конец, что неприемлемо для многих задач.

Для решения проблемы кратчайшего пути я видел следующий метод. Будем сравнивать длину пройденного пути, в данном случае Length(Way), с некоторой константой — максимально возможной длиной пути, например 10, и если мы прошли больше, то выходим из функции. Тогда алгоритм будет возвращать путь длиной не больше 10, если такой существует, и не возвращать никакой путь в противном случае.

Вообще обход в глубину (в англоязычной литературе его называют DFSdepth-first search) изначально не является алгоритмом поиска путей. Но он используется для решения ряда других задач. Например, с помощью него можно узнать до каких точек карты можно добраться из данной — это записано в массиве Mark.

Оценим скорость алгоритма. Заметим, что обработка одной ячейки карты может происходить не более одного раза — т.к. мы помечаем все обработанные точки карты. Значит, алгоритм выполняет порядка O(V) операций, где V — число ячеек на карте, что не так уж и плохо.

К достоинствам алгоритма могу отнести его простоту и красоту.

Волновой алгоритм
Предположим, что мы находимся в некоторой точке карты.

Куда мы сможем попасть, сделав не более одного шага?

А не более двух?

А не более трех? четырех? пяти?



Идея волнового алгоритма заключается в том, что мы будем «разбегаться» от стартовой точки. Для этого в каждой ячейке будем хранить число шагов, за которое в нее можно добраться. Изначально добраться можно только до стартовой за 0 шагов. Постепенно увеличивая некоторую переменную Step, мы будем находить ячейки, расстояние (число шагов) до которых равно Step. Это сделать очень просто: такие ячейки являются соседями ячеек, найденных на предыдущем шаге, т.е. число шагов до которых равно Step-1. Вот моя реализация:
function SearchWay(StartX, StartY, FinishX, FinishY: Integer): Boolean;
var
  Angle, X, Y, i, i, Step: Integer;
  Added: Boolean;
  Point: TPoint;
begin
  SetLength(Way, 0); // Обнуляем массив с путем
  for i := 0 to High(Cells) do
    for j := 0 to High(Cells[i]) do
      Cells[i][j].Step := -1; // Мы еще нигде не были
// До финиша ноль шагов - от него будем разбегаться  
  Cells[FinishX][FinishY].Step := 0;
  Step := 0; // Изначально мы сделали ноль шагов
  Added := True; // Для входа в цикл
  
  while Added And (Cells[StartX][StartY].Step = -1) do
  begin
  // Пока вершины добаляются и мы не дошли до старта
    Added := False; // Пока что ничего не добавили
    Inc(Step); // Увеличиваем число шагов
    for i := 0 to High(Cells) do
      for j := 0 to High(Cells[i]) do // Пробегаем по всей карте
        if Cells[i][j].Step = Step - 1 then
        begin
        // Если (i, j) была добавлена на предыдущем шаге
// Пробегаем по всем четырем сторонам света
          for Angle := 0 to 3 do
  begin
            X := i + Round(Cos(Angle/2*pi)); // Вычисляем коор-
            Y := j + Round(Sin(Angle/2*pi)); // динаты соседа
          // Если вышли за пределы поля, (X, Y) не обрабатываем
            if (X < 0) Or (Y < 0) Or (X > High(Cells)) Or (Y > High(Cells[0])) then
              Continue;
          // Если (X, Y) уже добавлено или непроходимо, то не обрабатываем  
            if Cells[X][Y].Locked Or (Cells[X][Y].Step <> -1) then
              Continue;
            Cells[X][Y].Step := Step; // Добав-
            Cells[X][Y].Parent.X := i; // ля-
            Cells[X][Y].Parent.Y := j; // ем
            Added := True; // Что-то добавили
          end;
        end;
  end;
  
// Если до старта не дошли,  
  if Cells[StartX][StartY].Step = -1 then
  begin 
    Result := False; // то пути не существует
    Exit;
  end;
 
  Point.X := StartX;
  Point.Y := StartY;

// Пока не дойдем до финиша  
  while Cells[Point.X][Point.Y].Step <> 0 do
  begin 
    SetLength(Way, Length(Way) + 1);
    Way[High(Way)] := Point; // добавляем текущую вершину
    Point := Cells[Point.X][Point.Y].Parent; // переходим к следующей
  end;
  
  SetLength(Way, Length(Way) + 1); // добавляем финиш
  Way[High(Way)].X := FinishX;
  Way[High(Way)].Y := FinishY;
  Result := True;
end;

Во-первых, в реализации я начинал не от старта, а от финиша. Почему? Потому что если начинать от старта, то в конце мы получим массив Way, в котором все вершины идут в обратном порядке, и нам пришлось бы его разворачивать. Это не сложно, но зачем?

Во-вторых, в алгоритме я хранил в каждой ячейке значение Parent, что сильно упрощает построение конечного пути. Этого можно и не делать, достаточно идти с конца и выбирать соседа с наименьшим значением Step, но учтите, что нужно будет у каждой вершины пробегать соседей. Обход соседей на «голой» (т.е. безо всякого предподсчета) карте вообще неприятная вещь. В алгоритме поиска в глубину я просто рассмотрел четыре случая — благо писать в каждом немного. В приведенной реализации волнового алгоритма я выкрутился тем, что в цикле пробегал по четырем направлениям. Именно из-за нежелания в дальнейшем связываться с обходом соседей, начиная со следующего раздела мы перейдем к работе с полноценными взвешенными графами.

Оценим скорость алгоритма. После каждого увеличения Step мы пробегаем по всей карте за O(V). Step увеличивается до тех пор, пока путь не будет найден, т.е. время работы алгоритма O(W*V), где W — длина пути. В худшем случае (когда путь идет по всей карте) — O(V2).

Работа с графами
В начале немного математики. Графом называют пару G=(V, E), где V — множество вершин, а E — множество упорядоченных пар вида (v, u), называемых ребрами, где v и u принадлежат V. Граф называют неориентированным, если для любых v и u, принадлежащих V, утверждения «ребро (v, u) принадлежит E» и «ребро (u, v) принадлежит E» равносильны. Граф называют взвешенным, если определена функция w:E->R (на множестве ребер принимает значения в действительных числах). Значение этой функции на ребре называют весом этого ребра. Путь в графе — такая последовательность вершин v1, v2, ..., vn, что для любого i ребро (vi, vi+1) принадлежит E. Цикл в графе — это путь, у которого начало совпадает с концом. Стоимостью пути взвешенного графа называют величину w(v1,v2) + w(v2,v3) +… + w(vn-1,vn). Кратчайшим путем между двумя вершинами S и F называют такой путь S,v1,v2,...,vn,F, что его стоимость минимальна. Стоимость кратчайшего пути между S и F называют расстоянием между S и F.

Теперь то же самое, но по-простому. Граф — это множество вершин, некоторые из которых соединены ребрами. Согласно нашему определению пара вершин не может быть соединена несколькими ребрами (хотя в некоторых случаях такое тоже удобно считать графом). Бывают ориентированные графы — это когда каждое ребро имеет направление. (Соответственно, ребра неориентированного графа не имеют направлений.) Если каждое ребро обладает еще и числом (которое называют весом), то такой граф называют взвешенным. Вес можно себе представлять как стоимость перехода по ребру. Тогда минимальный путь от S до F — это путь, стоимость которого минимальна. Замечу, что минимальный путь не определен, если в графе есть цикл с отрицательной стоимостью. Действительно, наворачивая круги на этом цикле мы можем сделать стоимость пути сколь угодно малой.

Теперь о практике. Вершины графа можно использовать для представления множества состояний, а рёбра — для представления возможных переходов между этими состояниями. Скажем, каждую позицию в шахматах можно представлять себе как вершину, а каждый ход как ребро. (Собственно, как и в любой другой математической игре.) В нашем случае вершины — это позиции на карте, а ребра соединяют две соседние вершины, между которыми можно пройти.

Теперь о представлении графов. Способов хранить (во время выполнения алгоритма) граф очень много, но распространены всего два. Первый (его мы рассмотрим чуть позже) — это матрица смежности. Нужно хранить таблицу W, в ячейке W[v][u] которой хранится информация о ребре (v,u). Тогда памяти потребуется O(V2). Второй способ хранения графов (с которого мы и начнем) — массив V, в котором V[i] описывает информацию i-ой вершины. В частности, хранит список вершин, с которыми она соединена.

Откуда же мы будем брать граф для визуализации? Двухмерную карту я просто генерировал случайным образом. Здесь у нас это так просто не выйдет. Поэтому граф будет храниться в обычном текстовом файле в следующем формате:

<n — кол-во вершин>
<описание вершины 0>
<описание вершины 1>

<описание вершины n-1>
<описание вершины i>: <X координата> <Y координата> <e — кол-во ребер, ведущих из вершины><номер вершины, в которую ведет ребро 0> <номер вершины, в которую ведет ребро 1>… <номер вершины, в которую ведет ребро e-1>

X и Y координаты нам нужны для удобства визуализации. Кроме того, в качестве веса ребра мы будем использовать расстояние между вершинами.

Вот и все. Все это (загрузка, хранение и отображение) реализовано в демке вместе с уже известным вам поиском в глубину.

Алгоритм Дейкстры
Алгоритм Дейкстры работает на взвешенном графе с неотрицательными весами и всегда находит кратчайший путь. Если же есть ребро с отрицательным весом, то алгоритм может давать неправильный результат. В нашем случае отрицательных весов нет.

Пусть нам нужно найти путь из вершины Start в вершину Finish. Будем хранить в каждой вершине значение h, равное предполагаемой длине кратчайшего пути от Start до этой вершины. По ходу выполнения алгоритма это значение будет меняться, а изначально положим во всех вершинах h=INF (бесконечность), кроме стартовой: V[Start].h=0. Кроме того, будем хранить S — множество вершин, для которых текущее значение h действительно является расстоянием от Start до нее. Изначально S={} (пусто).

Тогда алгоритм будет следующим:

1. Находим вершину i такую, что (1) она не принадлежит множеству S (2) Значение h в этой этой вершине минимально.

2. Если такой вершины не нашлось, то это означает, что все вершины, до которых можно было добраться лежат в множестве S, а значит до Finish добраться нельзя. Завершаем программу.

3. В противном случае пробегаем по всем соседям вершины i и обновляем значение h в них. А именно, если j — вершина-сосед, то V[j].h := Min(V[j].h, V[i].h + V[i].Adjs[j].w).

4. Добавляем i в S.

5. Если i = Finish, то это значит, что кратчайший путь найден. А именно, для восстановления пути можно идти от вершины Finish и на каждом шаге выбирать соседа с наименьшим значением h. Для упрощения этого процесса можно хранить значение Parent в каждой вершине, и в конце идти по ним. (Так же как мы делали в волновом алгоритме.)

Теперь я приведу доказательство того, что этот алгоритм действительно найдет кратчайшее расстояние.
Утверждение 1. V[i].h для каждого i на каждом шаге алгоритма хранит длину кратчайшего пути от Start до i, если при перемещении в качестве промежуточных вершин можно использовать только вершины, принадлежащие множеству S. Этот инвариант нужно доказывать по индукции: изначально он верен, кроме того легко показать, что после каждого добавления новой вершины инвариант сохраняется.
Утверждение 2. V[i].h хранит кратчайшее расстояние от Start до i, где i — вершина, найденная на шаге 1 алгоритма. Действительно, пусть существует более короткий путь. Очевидно, что он должен содержать вершину, не принадлежащую S и отличную от i, потому что если бы такой вершины не нашлось, то это противоречило бы утверждению 1. (Существовал бы путь в множестве S, длина которого короче V[i].h.) Пусть T — первая вершина в этом пути, не принадлежащая S. Тогда очевидно, что V[T].h < V[i].h, что противоречит алгоритму — значение V[i].h должно быть минимально.

Возможно, алгоритм показался вам знакомым. Это не спроста: волновой алгоритм является частным случаем алгоритма Дейкстры. Оба эти алгоритма берут идеи у так называемого обхода графа в ширину (в англоязычной литературе его называют BFSbreadth-first search).

Теперь поговорим о реализации. При выполнении алгоритма вместо S проще хранить множество необработанных вершин. Делать это можно разными способами. Самый простой — создать массив Used: Array[1..n] Of Boolean и хранить в нем значения обработанности вершин. Тогда выполняться 1 пункт алгоритма будет за O(V). Всего кол-во итераций в алгоритме порядка O(V), а значит в пункте 3 получаем операций порядка O(E), где E — кол-во ребер в графе. А всего алгоритм выполняет порядка O(V2+E) действий.

Другой способ хранения множества — куча или другая подобная структура данных. С помощью кучи можно уменьшить время до O(VlogV + E), что является достаточно существенной оптимизацией при E << V^2. Использовать кучу я не буду, ибо это сильно усложнит код и статью. Вы можете узнать что это такое из любой книжки про алгоритмы или же в интернете — благо это все очень известно.

В своей реализации множество необработанных вершин я представляю при помощи динамического массива, в котором записаны индексы этих вершин. Как было написано выше, я буду хранить в каждой вершине значение Parent. (Номер вершины, из которой мы пришли в данную.) Наконец, чтобы не разворачивать построенный путь, мы будем идти от финиша к старту, но этого нельзя было бы делать, если бы у нас был ориентированный граф. Соответственно, вот реализация (только переменная h, которую я использовал в доказательстве, переименована в Len; V переименовано в Graph, а V представляет собой множество необработанных вершин):
function SearchWay(Start, Finish: Integer): Boolean;
var
  i, j, Min, Vertex: Integer;
  V: Array Of Integer;
  Added: Boolean;
begin
  Result := False;
  SetLength(Way, 0);
  for i := 0 to High(Graph) do
    Graph[i].Len := INF;
  SetLength(V, Length(Graph));
  for i := 0 to High(V) do
    V[i] := i;
  Graph[Finish].Len := 0;
  while True do
  begin // Делать!
    Min := 0;
    for i := 0 to High(V) do
      if Graph[V[i]].Len <= Graph[V[Min]].Len then
        Min := i; // Находим в множестве V самую близкую к Finish вершину
    if Graph[V[Min]].Len = INF then // Если эта вершина на бесконечности,
      Break; // то значит пути не существует
    if Vertex = Start then // Если начало - ближайшая к Finish вершина,
      Break; // то кратчайший путь найден!
    Vertex := V[Min];
    for i := 0 to High(Graph[Vertex].Edges) do // Пробегаем по соседям найденной вершины
      with Graph[Vertex].Edges[i] do
        if Graph[V].Len > Graph[Vertex].Len + W then
        begin // Если можно добраться короче
          Graph[V].Len := Graph[Vertex].Len + W; // Обновляем расстояние
          Graph[V].Parent := Vertex; // Parent нужно для упрошения построения Way
        end;
    V[Min] := V[High(V)]; // Удаляем Vertex из множества V
    SetLength(V, Length(V) - 1);
  end;
  
  if Graph[Start].Len = INF then
    Exit;
  SetLength(Way, 1);
  Way[0] := Start;
  while Way[High(Way)] <> Finish do
  begin
    SetLength(Way, Length(Way) + 1);
    Way[High(Way)] := Graph[Way[High(Way) - 1]].Parent;
  end;
  Result := True;
end;

Использование матрицы смежности и алгоритм Флойда
Теперь предположим, что у нас относительно небольшая карта и поэтому мы без проблем можем хранить таблицу размером O(V2). В моих демках около 100 вершин. Если каждая ячейка таблицы — одна переменная типа Integer, то памяти такая таблица займет 100*100*4 bytes ~= 40KB. Кроме того предположим, что граф статичен и заранее задан. Например, для алгоритма Дейкстры это условие необязательно — его можно адаптировать к работе с генерируемыми на лету позициями.

Пусть у нас есть таблица Table. Для начала построим матрицу смежности, т.е. в Table[i][j] запишем вес ребра, соединяющего i с j, а если такого ребра не существует, то положим Table[i][j] = INF. Если хранить граф как и раньше, то такую таблицу построить очень просто:
// Строит матрицу смежности
procedure BuildTable;
var
  i, j, k: Integer;
begin
// Инициализируем таблицу
  SetLength(Table, Length(Graph), Length(Graph));
  for i := 0 to High(Table) do
    for j := 0 to High(Table[i]) do
      if i = j then
        Table[i][j] := 0
      else
        Table[i][j] := INF;
// Создаем матрицу смежностей
  for i := 0 to High(Graph) do
    for j := 0 to High(Graph[i].Edges) do
      with Graph[i].Edges[j] do
        Table[i][V] := W;
end;

Теперь сделаем следующее — запишем в Table[i][j] длину кратчайшего пути между i и j. Существует несколько алгоритмов выполняющих это. На мой взгляд, самый простой с точки зрения реализации алгоритм — алгоритм Флойда:
// Алгоритм Флоида для построения расстояний между всеми парами вершин
  for k := 0 to High(Graph) do
    for i := 0 to High(Graph) do
      for j := 0 to High(Graph) do
        Table[i][j] := Min(Table[i][j], Table[i][k] + Table[k][j]);

Несмотря на его краткость, сложно сразу понять как он работает. Докажем следующий инвариант внешнего цикла: «Для любых i и j в Table[i][j] записана длина кратчайшего пути, если в качестве промежуточных вершин разрешено использовать вершины из множества {0,1,...,K-1}». Изначально (т.е. при k=0) это верно — матрица смежности как раз удовлетворяет этому свойству. Что происходит, когда мы k увеличиваем на 1? У нас появляется возможность использовать в качестве промежуточной вершину k. Т.е. от вершины i до вершины j мы можем добраться двумя способами: используя вершину k и не используя вершину k. Если мы используем k, то вначале идем до k через {0,1,2,...,k-1}, а потом до j, опять же через {0,1,2,...,k-1}. Т.е. длина такого пути по инварианту будет равна Table[i][k] + Table[k][j]. Если же мы не используем вершину k, то длина кратчайшего пути будет равна Table[i][j]. Осталось только найти минимум от этих двух величин. (Конечно же, еще нужно доказать, что при обновлении таблицы она не «испортиться», т.е. измененные ячейки не повлияют на вычисления других ячеек. Но это достаточно просто, и вы можете сделать это самостоятельно.) Время такого алгоритма, очевидно, O(V3). Но это нужно выполнять только в начале программы.

Теперь перейдем к самому интересному — поиску пути. (Хотя, это скорее будет не поиск, а построение пути.) Если Table[Start][Finish] = INF, то пути не существует, и нужно завершить работу алгоритма. В противном случае путь обязательно есть. Начиная с вершины Start, будем постепенно идти к финишу, выбирая на каждом шаге соседа j с минимальным значением Table[j][Finish]. Стоит отметить, что если в графе есть ребра с нулевыми весами, алгоритм может зациклиться — пример построить достаточно просто. Вместо 0 лучше записать какое-нибудь маленькое число.
function SearchWay(Start, Finish: Integer): Boolean;
var
  i, j, Min: Integer;
begin
  SetLength(Way, 0);
// Проверяем существует ли путь
  if Table[Start][Finish] = INF then
  begin
    Result := False;
    Exit;
  end;
  i := Start;
  while i <> Finish do
  begin
  // Добавляем вершину i
    SetLength(Way, Length(Way) + 1);
    Way[High(Way)] := i;
    Min := Graph[i].Edges[0].V;
  // Ищем соседа, который ближе всего к финишу
    for j := 0 to High(Graph[i].Edges) do
      with Graph[i].Edges[j] do
        if Table[Min][Finish] > Table[V][Finish] then
          Min := V;
  // Переходим к соседу
    i := Min;
  end;
  SetLength(Way, Length(Way) + 1);
  Way[High(Way)] := Finish;
end;

Если пути не существует, то алгоритм работает за время O(1). Если же путь есть, то время выполнения будет порядка O(V + E). Если в графе число ребер очень мало (в полном графе их V2, но в моем примере менее, чем 6V, т.к. из каждой вершины выходит не более шести ребер), то время работы алгоритма можно оценить как O(W), где W — максимальное кол-во вершин в пути. (В моей демке W ~= 20)

Такой вот быстрый и красивый алгоритм.

Алгоритм на очередях
O(n)-граф — это такой взвешенный граф, что вес каждого его ребра принадлежит множеству {0,1,...,n-1}. Предположим, что у нас есть O(n)-граф и нам нужно найти кратчайший путь в нем. Тогда можно воспользоваться следующим алгоритмом.

Во-первых, нам необходимо завести n очередей с номерами вершин. Все операции с очередью (помещение/удаление) мы должны уметь делать за O(1), кроме того нам потребуется удаление произвольного элемента очереди (тоже за O(1)). Поэтому, стандартное решение на динамическом массиве нам не подходит. Мы реализуем очередь двухсвязным списком:
type
// Элемент очереди
  PQue = ^TQue;
  TQue = packed record
    V: Integer; // Vertex
    N: PQue; // Next
    P: PQue; // Prev
  end;

// Удаляет элемент очереди
procedure DeleteQue(const Q: PQue);
begin
// Соседи Q должны ссылаться друг на друга
  Q^.N^.P := Q^.P;
  Q^.P^.N := Q^.N;
// Освобождаем память
  Dispose(Q);
end;

// Извлекает элемент из очереди
function DeQue(var Q: PQue): Integer;
begin
// Возвращаем предыдущий
  Result := Q^.P^.V;
// Если вернулось -1, то значит очередь пуста
  if Result = -1 then
    Exit;
// Если же нет, то удаляем ссылку на элемент
  Graph[Result].Que := nil;
// Удаляем извлеченный элемент
  DeleteQue(Q^.P);
end;

// Вставляет элемент в очередь
procedure EnQue(var Q: PQue; V: Integer);
var
  Temp: PQue;
begin
// Если вершина V помещена в какую-то очередь, то удаляем ее оттуда
  if Graph[V].Que <> nil then
    DeleteQue(Graph[V].Que);
// Создаем новый элемент, вставляем в очередь
  New(Temp);
  Temp^.V := V;
  Temp^.P := Q;
  Temp^.N := Q^.N;
  Temp^.N^.P := Temp;
  Temp^.P^.N := Temp;
// Записываем в V указатель на элемент, который ее содержит
  Graph[V].Que := Temp;
end;

// Инициализирует очередь - создает фиктивный элемент (-1)
procedure CreateQue(var Q: PQue);
begin
  New(Q);
  Q^.V := -1;
  Q^.N := Q;
  Q^.P := Q;
end;

// Уничтожает очередь
procedure DestroyQue(var Q: PQue);
begin
  while DeQue(Q) <> -1 do;
  Dispose(Q);
  Q := nil;
end;

Для инициализации очереди нужно вызвать процедуру CreateQue, которая создает фиктивный элемент (число -1). По этому элементу мы будем определять конец очереди. Конечно, этого можно и не делать, но учтите, что тогда придется рассматривать большое кол-во граничных случаев. Очевидно, что основные операции выполняются за O(1). Кроме того реализован контроль за тем, чтобы каждая вершина лежала ровно в одной очереди.

Во-вторых, в каждой вершине будем хранить (1) Len — предполагаемое расстояние от вершины Start (2) Parent — номер вершины, из которой мы пришли в данную (3) Que — указатель на элемент очереди, в котором находится данная вершина, либо nil, если вершина не помещена ни в какую в очередь. Сам алгоритм такой
1. Если у нас O(MWE)-граф, то создаем MWE очередей. Во все вершины помещаем Len=INF, Parent=-1, Que=Nil.
2. Помещаем Start в нулевую очередь, Graph[Start].Len := 0.
3. Пока у нас что-нибудь добавляется и мы не дошли до финиша делать
  4. For K := 0 to MWE-1 do
    5. До тех пор, пока очередь №K не станет пустой, делать
      6. Извлекаем вершину i из очереди №K
      7. Обходим соседей вершины i, если оценка пути лучше, то помещаем соседа в очередь номер L mod MWE, где L — новая оценка расстояния до соседа.

Ключевой момент алгоритма выделен жирным шрифтом.

Кратко опишу доказательство корректности алгоритма. Во время выполнения алгоритма значения Len обрабатываемых вершин не убывают, а в пределах одной очереди они равны между собой. Эти два утверждения нужно доказывать по индукции. Пусть мы обрабатываем очередь №K и значение Len во всех ее элементах равно L. Тогда во время обработки соседей мы будем добавлять вершины, у которых Len принимает значение от L до L + MWE — 1. Из того, что в одну очередь помещаются элементы с одинаковыми остатками, следует, что после добавления соседей все значения Len в одной очереди останутся прежними. После того, как очередь №K окажется пустой и мы перейдем к следующей, в нее будут добавляться вершины с Len=L+MWE. Выделенное утверждение доказано. Из него следует корректность алгоритма.

Очевидно, что каждая вершина может добавляться в очереди не более MWE раз (т.к. все последующие оценки Len будут хуже). Отсюда оценка по времени O(V*MWE). При небольших MWE алгоритм работает быстрее, чем Дейкстра с кучей, а кроме того реализуется заметно проще. На самом деле, если приглядеться, то описанный алгоритм является частным случаем алгоритма Дейкстры.

Собственно, моя реализация.
function SearchWay(Start, Finish: Integer): Boolean;
var
  i, j, k: Integer;
  Ques: Array Of PQue;
  Added: Boolean;
begin
// Различные инициализации
  Result := False;
  SetLength(Way, 0);
  SetLength(Ques, MWE);
  for i := 0 to High(Ques) do
    CreateQue(Ques[i]);
  for i := 0 to High(Graph) do
  begin
    Graph[i].Len := INF;
    Graph[i].Que := nil;
    Graph[i].Parent := -1;
  end;
// Подготавливаем начальное состояние для алгоритма
  EnQue(Ques[0], Finish);
  Graph[Finish].Len := 0;
  Added := True;
// Главный цикл
  while (Graph[Start].Parent = -1) and Added do
  begin
    Added := False;
  // Пробегаем по всем очередям
    for k := 0 to High(Ques) do
    begin
      i := DeQue(Ques[k]);
    // Опустошаем Ques[k]
      while i <> -1 do
      begin
      // Пробегаем соседей каждой извлеченной вершины
        for j := 0 to High(Graph[i].Edges) do
          with Graph[i].Edges[j] do
          // Если мы добрались до V короче, чем раньше
            if Graph[V].Len > Graph[i].Len + W then
            begin
            // Обновляем V
              Graph[V].Len := Graph[i].Len + W;
              Graph[V].Parent := i;
            // Помещаем в очередь
              EnQue(Ques[Graph[V].Len mod MWE], V);
            // Отмечаем, что что-то в очередь было помещено
              Added := True;
            end;
        i := DeQue(Ques[k]);
      end;
    end
  end;
  
// Удаляем очереди
  for i := 0 to High(Ques) do
    DestroyQue(Ques[i]);
// Если до старта не дошли, то путь не существует
  if Graph[Start].Parent = -1 then
    Exit;
// Строим путь
  SetLength(Way, 1);
  Way[0] := Start;
  while Way[High(Way)] <> Finish do
  begin
    SetLength(Way, Length(Way) + 1);
    Way[High(Way)] := Graph[Way[High(Way)-1]].Parent;
  end;
// Возвращаем успешность
  Result := True;
end;

И конечно же, пример. Обратите внимание на то, что все веса ребер попадают в множество {0,1,...,MWE-1}. Кстати, чуть не забыл, MWE — это сокращение от max weight of edge :)

Ссылки по теме
ALGOLIST.MANUAL.RU
Программирование магических игр
en.wikipedia.org
A* алгоритм и его оптимизации
06.07.08 00:50