Реализации алгоритмов/Алгоритм поиска A*

Поиск A* (произносится: «А звезда») — алгоритм поиска по первому наилучшему совпадению на графе, который находит маршрут с наименьшей стоимостью от одной вершины (начальной) к другой (целевой, конечной).

Псевдокод с подробными комментариями

 function A*(start,goal)
     closedset := the empty set    // Множество вершин, которые уже были обработаны(раскрыты)
     openset := {start}            // Множество вершин(очередь), которые предстоит обработать(раскрыть). 
	                               // Изначально здесь присутствует только начальная вершина start.
     path_map := the empty set     // Карта пройденных вершин. Используется функцией reconstruct_path
     
     //Заполняем свойства вершины start
     start.g := 0   // g(x). Стоимость пути от начальной вершины. У start g(x) = 0
     start.h := heuristic_cost_estimate(start, goal) // Эвристическая оценка расстояние до цели. h(x)
     start.f := start.g + start.h      // f(x) = g(x) + h(x)

     while openset is not empty
         x := вершина из openset имеющая самую низкую оценку f(x)
         if x = goal 
             return reconstruct_path(start,goal) //заполняем карту path_map
         
         remove x from openset // Вершина x пошла на обработку, а значит её следует удалить из очереди на обработку
         add x to closedset    // И добавить в список уже обработанных
         foreach y in neighbor_nodes(x) // Проверяем каждого соседа x
             if y in closedset          // Пропускаем соседей из закрытого списка
                 continue

             tentative_g_score := x.g + dist_between(x,y)  // Вычисляем g(y) через x для обрабатываемого соседа

             if y not in openset // Если сосед x ещё не в открытом списке - добавим его туда
                 add y to openset
                 tentative_is_better := true
             else               // Сосед был в открытом списке, а значит мы уже знаем его g(y), h(y) и f(y)
                 if tentative_g_score < y.g  
                     // Вычисленная g(y) через x оказалась меньше, а значит нужно будет обновить  g(y), h(y), f(y)
                     tentative_is_better := true   
                 else
                     // Вычисленная g(y) через x оказалась больше, чем имеющаяся в openset. 
                     // Это означает, что из вершины x путь через этого соседа дороже
                     // т.е. существует менее дорогой маршрут, пролегающий через этого соседа (из какой-то другой вершины, не из x)
                     // Поэтому данного соседа мы игнорируем
                     tentative_is_better := false 
             
             // Обновление свойств соседа. 
             if tentative_is_better = true
                 y.came_from := x //Вершина с которой мы пришли. Используется для реконструкции пути.
                 y.g := tentative_g_score
                 y.h := heuristic_cost_estimate(y, goal)
                 y.f := y.g + y.h
             // Обратите внимание, что если происходит обновление свойств - значит y(сосед x) 
             // так или иначе находится в openset.
             // Т.е. при следующей итерации внешнего цикла из openset будет извлечена вершина с наименьшей оценкой f(x)
             // Не исключено, что она окажется соседом нашего x, которого мы только что добавили.
             // В общем это самая важная особенность алгоритма А*

     return failure //управление передаётся сюда когда openset пуст, а goal не найден (путь найти не удалось)

// Заполняет карту path_map
// Путь можно проследить только от заданной вершины(чаще всего это goal) 
// к старту(каждая вершина имеет свойство came_from, чем мы и воспользуемся)
function reconstruct_path(start_node, goal_node)
// Добавляем в карту все вершины от finish_node до start_node.
     current_node := goal_node // поиск начинается от финиша
     while current_node <> NULL 
             path_map.add_node(current_node) // Добавить вершину в карту
             current_node := current_node.came_from
 
     return path_map

Пример на Delphi

procedure SearchPath(x0, y0, x, y: Integer; PrintProc: TCurrentPoint);
type
  pNode = ^TNode;
  TNode = record
    Parent: pNode;
    Pos: TPoint;
    Weight: Integer;
    G: LongInt;
    H, F: Extended;
  end;
var
  i, j, k: Integer;
  OpenList, ClosedList: TList;
  NewNode, Current: pNode;
  FMin: Extended;
  isClosed, isOpen, Complete: Boolean;
begin
  OpenList := TList.Create;
  ClosedList := TList.Create;
  New(NewNode);
  NewNode^.Parent := nil;
  NewNode^.Pos := Point(x0, y0);
  NewNode^.G := 0;
  NewNode^.H := Sqrt(Sqr(x - x0) + Sqr(y - y0));
  NewNode^.F := NewNode^.G + NewNode^.H;
  OpenList.Add(NewNode);
  Complete := False;
  while (OpenList.Count > 0) and not Complete do
  begin
    FMin := 77000;
    Current := nil;
    for i := 0 to OpenList.Count - 1 do
      if pNode(OpenList[i])^.F < FMin then
      begin
        FMin := pNode(OpenList[i])^.F;
        Current := pNode(OpenList[i]);
      end;
    ClosedList.Add(Current);
    OpenList.Remove(Current);
    for i := Current^.Pos.X - 1 to Current^.Pos.X + 1 do
      for j := Current^.Pos.Y - 1 to Current^.Pos.Y + 1 do
      begin
        if (i = Current^.Pos.X) and (j = Current^.Pos.Y) then
          Continue
        else if not Complete then
          Complete := (i = x) and (j = y)
        else
          Break;
        isClosed := False;
        for k := ClosedList.Count - 1 downto 0 do //ищем в закрытом списке
          if (pNode(ClosedList[k])^.Pos.X = i) and (pNode(ClosedList[k])^.Pos.Y = j) then
          begin
            isClosed := True; //в закрытом списке
            Break;
          end;
        if isClosed then
          Continue;
        isOpen := False;
        for k := OpenList.Count - 1 downto 0 do //ищем в открытом списке
          if (pNode(OpenList[k])^.Pos.X = i) and (pNode(OpenList[k])^.Pos.Y = j) then
          begin
            isOpen := True; //в открытом списке
            if pNode(OpenList[k])^.G > (Current^.G + pNode(OpenList[k])^.Weight) then
            begin
              pNode(OpenList[k])^.Parent := Current;
              pNode(OpenList[k])^.G := Current^.G + pNode(OpenList[k])^.Weight;
              pNode(OpenList[k])^.F := pNode(OpenList[k])^.G + pNode(OpenList[k])^.H;
            end;
            Break;
          end;
        if not isOpen then //если еще не открыта
        begin
          //добавляем в открытый список
          New(NewNode);
          NewNode^.Parent := Current;
          NewNode^.Pos := Point(i, j);
          NewNode^.Weight := Weights[i, j];
          NewNode^.G := Current^.G + NewNode^.Weight;
          NewNode^.H := Sqrt(Sqr(x - i) + Sqr(y - j));
          NewNode^.F := NewNode^.G + NewNode^.H;
          OpenList.Add(NewNode);
        end;
      end;
  end;
  for i := OpenList.Count - 1 downto 0 do
    if (pNode(OpenList[i])^.Pos.X = x) and (pNode(OpenList[i])^.Pos.Y = y) then
    begin
      Current := OpenList[i];
      while Current <> nil do
      begin
        PrintProc(Current^.Pos.X, Current^.Pos.Y);
        Current := Current^.Parent;
      end;
    end;
  for i := OpenList.Count - 1 downto 0 do
    Dispose(OpenList[i]);
  OpenList.Free;
  for i := ClosedList.Count - 1 downto 0 do
    Dispose(ClosedList[i]);
  ClosedList.Free;
end;

Пример на BlitzMax

Оптимизированный алгоритм А* с использованием хеш-таблиц с прямой адресацией

Global map:Byte[2048,2048]' Карта проходимости (стены)
map[10,7]=1
Local Astar:TAstar = New TAstar
Local stime:Int = MilliSecs() 'Засекаем время выполнения
Astar.FindPath(1,1, 2000, 2000, 2048)
Print (MilliSecs()-stime)+" ms"

Type Tnode
	Field x:Int,y:Int
	Field Parent:Tnode
	Field Weight:Int
	Field G:Int
	Field H:Double, F:Double
End Type

Type TAstar
	Method FindPath:TPath(x0:Int, y0:Int, x:Int, y:Int, mw:Int)
		Local i:Int, j:Int, k:Int
		Local OpenList:Tmap, ClosedList:Tmap
		Local NewNode:Tnode, CurNode:Tnode, Node:Tnode
		Local hashX:Int, hashY:Int
		Local FMin:Double
		Local isClosed:Byte, isOpen:Byte, Complete:Byte
		
		OpenList = New Tmap
		ClosedList = New Tmap
		
		NewNode = New Tnode
		NewNode.Parent = Null
		NewNode.x = x0
		NewNode.y = y0
		NewNode.G = 0
		NewNode.H = Sqr((x - x0)*(x - x0) + (y - y0)*(y - y0))
		NewNode.F = NewNode.G + NewNode.H
		MapInsert(OpenList, String(NewNode.y*mw+NewNode.x), NewNode)
		Complete = 0
		
		While Not MapIsEmpty(OpenList) And Not Complete
			FMin = 77000
			CurNode = Null
			For Node = EachIn MapValues(OpenList)
				If Node.F < FMin Then
					FMin = Node.F
					CurNode = Node
				End If
			Next

			MapInsert(ClosedList, String(CurNode.y*mw+CurNode.x), CurNode)
			MapRemove(OpenList, String(CurNode.y*mw+CurNode.x))
			For i = CurNode.X - 1 To CurNode.X + 1
				For j = CurNode.Y - 1 To CurNode.Y + 1
					If map[i,j]=0
						If (i = CurNode.X) And (j = CurNode.Y)
							Continue
						ElseIf Complete = 0
							If (i = x) And (j = y) Then Complete = 1'path finded
						Else
							Exit
						End If

						isClosed = 0
	
						If MapContains(ClosedList, String(j*mw+i) ) Then isClosed = 1 ; Continue
	
						isOpen = 0
	
						If MapContains(OpenList, String(j*mw+i) )
						Node = TNode( MapValueForKey(OpenList, String(j*mw+i)) )
						isOpen = 1 'в открытом списке
							If Node.G > (CurNode.G + Node.Weight)
								Node.Parent = CurNode
								Node.G = CurNode.G + Node.Weight
								Node.F = Node.G + Node.H
							End If
						End If

	
						If Not isOpen 'если еще не открыта добавляем в открытый список
							NewNode = New Tnode
							NewNode.Parent = CurNode
							NewNode.X = i
							NewNode.Y = j
							NewNode.Weight = CurNode.Weight
							NewNode.G = CurNode.G + NewNode.Weight
							NewNode.H = Sqr((x - i)*(x - i) + (y - j)*(y - j))
							NewNode.F = NewNode.G + NewNode.H
							MapInsert(OpenList, String(NewNode.y*mw+NewNode.x), NewNode)
						End If
	
					End If
				Next
			Next
		Wend

		If MapContains(OpenList, String(y*mw+x) )
			Node = TNode( MapValueForKey(OpenList, String(y*mw+x)) )
			CurNode = Node
			While CurNode <> Null
				Print ("["+CurNode.X +"."+CurNode.Y+"]")
				CurNode = CurNode.Parent
			Wend
		End If
	
	End Method
End Type

Ссылки

Реализации на различных языках программирования

Доказательство правильности и полноты

COQ