procedure Astar (open : in Element_List;
close : in 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(close);
Close_Curs := Element_Lists.First(close);
Adjust(Initial_Node, Element_Lists.First_Element(open));
Node_Lists.Append(Openned, Initial_Node);
Element_Lists.Delete_First(Close);
Text_Io.Put_Line("initialized !" );
Astar_Loop :
while not Is_Empty(Openned) loop
U_Sorting.Sort(Openned);
H_Sorting.Sort(Openned);
declare
Openned_Find_Curs : Node_Lists.Cursor :=
Node_Lists.First(Openned);
Best_Node : Node_Record;
F : Float := 0.0;
begin
Best_Node := Node_Lists.Element(Openned_Find_Curs);
Node_Lists.Delete_First(Openned);
declare
Best_Successors : El_Array := Successors(Best_Node.E);
begin
for Succ in Best_Successors ' range loop
declare
E : Element := Best_Successors(Succ);
begin
if Equal(E, Goal) then
Adjust(Node, E);
Node_Lists.Append(Closed, Node);
--Adjust(Node, Goal);
Text_Io.Put_Line("Count : " & Integer'Image(Integer(Node_Lists.Length (Closed))));
--Node_Lists.Append(Closed, Node);
Text_Io.Put_Line("Goal !" );
exit Astar_Loop;
else
Adjust(Node, E);
Node.Ucost := Best_Node.Ucost + heuristic(Node.E) + Best_Node.Hcost;
Text_Io.Put_Line("Ucost : " & float'Image(Node.ucost));
Node.Hcost := Best_Node.Hcost + Heuristic(Node.E) - Uniform(Node.E);
Text_Io.Put_Line("Hcost : " & float'Image(Node.Hcost));
F := Node.Hcost + Best_Node.ucost + Uniform(Node.E);
Text_Io.Put_Line("F : " & Float'Image(F));
if (Node.Hcost + (Node.Ucost) + Best_Node.ucost) < F then
Node_Lists.Append(closed, node);
end if;
if not Node_Lists.Is_Empty(openned) then
Text_Io.Put_Line("Openned length : " & Integer'Image(Integer(Node_Lists.Length(openned))));
declare
Open_Curs : Node_Lists.Cursor := Node_Lists.First(Openned);
begin
declare
Open_Node : Node_Record := Node_Lists.Element(Open_Curs);
begin
if Equal(Open_Node.E, Node.E)
and (Open_Node.Ucost + Open_Node.Hcost) > F then
Done := True;
end if;
end;
if not Done then
for Open in 1..Node_Lists.Length(openned) - 1 loop
Open_Curs := Node_Lists.Next(Open_Curs);
declare
Open_Node : Node_Record := Node_Lists.Element(Open_Curs);
begin
if Equal(Open_Node.E, Node.E)
and (Open_Node.Ucost + Open_Node.Hcost) > F then
Done := True;
end if;
end;
exit when Open = Node_Lists.Length(Openned);
end loop;
end if;
end;
end if;
if not Done then
Node_Lists.Append(Openned, Node);
end if;
Done := False;
end if;
end;
end loop;
end;
end;
end loop Astar_Loop;
Openned_Finalization(close, closed);
--Element_Lists.Reverse_Elements(Close);
end Astar;