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;
Exhausted : boolean := false;
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
print(E);
Adjust(Node, E);
Node.Ucost := Best_Node.ucost + (Uniform(Node.E));
Node.Hcost := (Heuristic(Node.E) + Best_Node.Hcost) + best_node.ucost;
F := (best_node.ucost + heuristic(Node.E)) ;
Text_Io.Put_Line("Ucost : " & float'Image(Node.ucost));
Text_Io.Put_Line("Hcost : " & float'Image(Node.Hcost));
Text_Io.Put_Line("F : " & Float'Image(F));
Node_Lists.Append(Closed, Node);
Text_Io.Put_Line("Goal !" );
exit Astar_Loop;
else
print(E);
Adjust(Node, E);
Node.Ucost := Best_Node.ucost + (Uniform(Node.E));
Text_Io.Put_Line("Ucost : " & float'Image(Node.ucost));
Node.Hcost := (Heuristic(Node.E) + Best_Node.Hcost) + best_node.ucost;
Text_Io.Put_Line("Hcost : " & float'Image(Node.Hcost));
F := (best_node.ucost + node.hcost + heuristic(Node.E)) ;
Text_Io.Put_Line("F : " & Float'Image(F));
if Node.Ucost + heuristic(node.E) < F then
Node_Lists.Append(closed, node);
if not Node_Lists.Is_Empty(openned) then
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) 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) 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 if;
end;
end loop;
end;
end;
end loop Astar_Loop;
Openned_Finalization(close, closed);
end Astar;