1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126 | with Ada.Containers.Ordered_Multisets,
Ada.Containers.Indefinite_Ordered_Maps,
Agpl.Trace,
System.Pool_Local;
use Agpl.Trace;
package body Agpl.Search.A_Star is
---------------
-- Best_Path --
---------------
procedure Best_Path (Ini,
Fin : State;
Route : out Path;
Cost : out Costs)
is
Local_Pool : System.Pool_Local.Unbounded_Reclaim_Pool;
type Node;
type Node_Access is access Node;
for Node_Access'Storage_Pool use Local_Pool; -- Automatic reclaim
type Node is record
Cost : Costs; -- Includes real + estimation, for sorting in pending
Real : Costs; -- The real cost till Curr without estimation
Prev : Node_Access;
Curr : State;
Len : Positive;
end record;
function Image (N : Node) return String is
begin
return Image (N.Curr) & N.Len'Img & Image (N.Real) & Image (N.Cost);
end Image;
---------
-- "<" --
---------
function "<" (L, R : Node_Access) return Boolean is
begin
return L.Cost < R.Cost;
end "<";
package Node_Sets is new Ada.Containers.Ordered_Multisets (Node_Access);
package String_Cost_Maps is new
Ada.Containers.Indefinite_Ordered_Maps (String, Costs);
----------------
-- Build_Path --
----------------
procedure Build_Path (Fin : Node_Access) is
I : Node_Access := Fin;
begin
Cost := Fin.Cost;
loop
Prepend (Route, I.Curr);
exit when I.Prev = null;
I := I.Prev;
end loop;
end Build_Path;
Pending : Node_Sets.Set;
-- Visited : Containers.String_Sets.Set;
Candids : String_Cost_Maps.Map;
Success : Boolean := False;
Iter : Positive := 1;
begin
Pending.Insert (new Node'(Zero, Zero, null, Ini, 1));
Candids.Insert (Image (Pending.First_Element.Curr), Zero);
while not Pending.Is_Empty loop
Log ("::" & Iter'Img, Debug, Log_Section);
Iter := Iter + 1;
declare
Curr : constant Node_Access := Pending.First_Element;
begin
Pending.Delete_First;
Log ("At node " & Image (Curr.all), Debug, Log_Section);
if Image (Curr.Curr) = Image (Fin) then
Log ("Fin reached", Debug, Log_Section);
Build_Path (Curr);
Success := True;
exit;
else
for I in 1 .. Num_Next (Curr.Curr) loop
declare
S : constant State := Next (Curr.Curr, I);
G : constant Costs := Real_Cost (Curr.Curr, S);
H : constant Costs := Estimate (S, Fin);
N : constant Node := (Curr.Real + G + H,
Curr.Real + G,
Curr,
S,
Curr.Len + 1);
begin
if
(not Candids.Contains (Image (S))) or else
N.Cost < Candids.Element (Image (S))
then
Log ("Added neighbor #" & I'Img & ":" & Image (N),
Debug, Log_Section);
Candids.Include (Image (S), N.Cost);
Pending.Insert (new Node'(N));
else
Log ("Rejected neighbor #" & I'Img, Debug, Log_Section);
end if;
end;
end loop;
end if;
end;
end loop;
if not Success then
raise Constraint_Error with
"No A* route: " & Image (Ini) & " --> " & Image (Fin);
end if;
end Best_Path;
end Agpl.Search.A_Star;
|