procedure Astar (Open : in Element_List;
Close : out Element_List) is
Openned : Node_List;
Closed : Node_List;
Openned_Curs : Node_Lists.Cursor;
Close_Curs : Element_Lists.Cursor;
Done : Boolean := False;
Initial_Node : Node_Record;
Goal : Element;
Node : Node_Record;
begin
Goal := Element_Lists.Last_Element(Open);
Close_Curs := Element_Lists.First(Open);
Adjust(Initial_Node, Element_Lists.First_Element(Open));
Node_Lists.Append(Openned, Initial_Node);
Astar_Loop :
while not Is_Empty(Openned) loop
Nodes_Sorting.Sort(Openned);
declare
Openned_Find_Curs : Node_Lists.Cursor :=
Node_Lists.First(Openned);
Best_Node : Node_Record;
F : Float := 0.0; -- successor.f = successor.g + successor.h
begin
Best_Node := Node_Lists.Element(Openned_Find_Curs);
Best_Node.Ucost := Uniform(Best_Node.E);
Best_Node.Hcost := Heuristic(Best_Node.E);
Node_Lists.Delete_First(Openned);
--Text_Io.Put_Line("New successors :" );
declare
Best_Successors : El_Array := Successors(Best_Node.E);
begin
--Text_Io.Put_Line("Nb successors : " & Integer'Image(Best_Successors'Length));
for Succ in Best_Successors ' range loop
declare
E : Element := Best_Successors(Succ);
begin
--Text_Io.Put_Line("Successor " & Integer'Image(Succ) & " : " );
if Equal(E, Goal) then -- i) if successor is the goal, stop search
Adjust(Node, E);
Node_Lists.Append(Closed, Node);
exit Astar_Loop;
else
Adjust(Node, E);
-- ii) else, compute both g and h for successor
-- successor.g = q.g + distance between
-- successor and q
Node.Ucost := Best_Node.ucost + uniform(Node.E);
-- successor.h = distance from goal to
-- successor (This can be done using many
--Text_Io.Put_Line(" - ucost = " & float'Image(Node.Ucost));
Node.Hcost := Heuristic(Goal) - Heuristic(Node.E);
--Text_Io.Put_Line(" - hcost = " & float'Image(Node.Hcost));
-- successor.f = successor.g + successor.h
F := Node.Ucost + Node.Hcost;
--Text_Io.Put_Line(" - F = " & float'Image(F));
declare
begin
if not Node_Lists.Is_Empty(Openned) then
declare
Open_curs : Node_Lists.Cursor := Node_Lists.First(Openned);
begin
for Open in 1..Node_Lists.Length(Closed) loop
declare
Open_Node : Node_Record;
begin
Open_Node := Node_Lists.First_Element(Openned);
if Equal(Open_Node.E, Node.E) and then
(Open_Node.Ucost + Open_Node.Hcost) < F then
Done := True;
end if;
end;
exit when Open = Node_Lists.Length(Openned);
Open_Curs := Node_Lists.Next(Open_Curs);
end loop;
end;
if not Done then
if not Node_Lists.Is_Empty(Closed) then
declare
Close_curs : Node_Lists.Cursor := Node_Lists.First(Closed);
begin
for close in 1..Node_Lists.Length(Closed) loop
declare
Close_Node : Node_Record := Node_Lists.Element(Close_Curs);
begin
if Equal(Close_Node.E, Goal) and then
(Close_Node.Ucost + Close_Node.Hcost) < F then
Done := True;
end if;
end;
exit when Close = Node_Lists.Length(Closed);
Close_Curs := Node_Lists.Next(Close_Curs);
end loop;
end;
end if;
end if;
if not Done then
--Text_Io.Put_Line("Adding new node in openned" );
Node_Lists.Append(Openned, Node);
end if;
Done := False;
end if;
end;
end if;
end;
end loop;
--Text_Io.Put_Line("Adding new node in closed" ); -- e) push q on the closed list
Node_Lists.Append(Closed, Best_Node);
end;
end;
end loop Astar_Loop;
Openned_Finalization(Close, Closed);
--Element_Lists.Reverse_Elements(Close);
end Astar;
