planning-wit-heuristic.pl

e(a,b). e(b,c). e(c,f).
e(f,e). e(e,d). e(d,h).
e(f,g). e(g,i). e(i,h).

% after(X,Y)
%
%after(X,Y):-e(X,Y).
%after(X,Y):-e(Y,X).

%p(h).
%p(g).

df(V, []):-p(V).
df(V, [V-Y|T]):-after(V,Y),df(Y,T).

df1(V, [], _):-p(V).
df1(V, [V-Y|T], N):-N>0, after(V,Y), N1 is N-1, df1(Y,T, N1).

idf(V, S, N):- df1(V,S,N),!.
idf(V, S, N):- N1 is N+1, idf(V,S,N1).

df2(V, [], _):-p(V).
df2(V, [V-Y|T], L):-after(V,Y), \+ member(Y,L), df2(Y,T, [Y|L]).



% [[a],[],[]]

app([],L,L).
app([X|T],L,[X|R]):-app(T,L,R).

bf([[V-X|T]|_],[V-X|T]):-p(V),!.
bf([[V-X|T]|Ws],S):- \+ p(V),
	findall([Y-V,V-X|T],
		(after(V,Y),
		 \+ member(Y-V, [V-X|T])
		), L),
	app(Ws,L,NWs),
	bf(NWs,S).


% [a]         [c]
% [b]         [b]    |    [a]|
% [c]  ---->  [a]    |[c] [b]|
%
%

del(X,[X|T],T).
del(X,[Y|T],[Y|R]):-del(X,T,R).

move(I, T1, X, P2, I2) :-
	del([X|T1],I,I1),
	del(P2,I1,I2).

down(I, T1, X, I1):-
	del([X|T1],I,I1).

after(I,[[X|P2]|I2]):-
	move(I, [], X, P2, I2).

after(I,[[A|T],[X|P2]|I2]):-
	move(I, [A|T], X, P2, I2).

after(I,[[X]|T]):-
	down(I, [], X, T).

after(I,[[Y|T1],[X]|T]):-
	down(I, [Y|T1], X,  T).

%p(L):-G=[a,b,c],
%	del(G,L,_).


% after(X,Y, C) <-> "Можно совершить дейсвтие по переводу
% состояния X в состояние Y, при этом стоимость этого действия = C."



p(zerind).

astar1([_-gh(G,_,[V-X|T])|_],G-[V-X|T]):-p(V),!.

astar1([_-gh(GV,_,[V-X|T])|Ws],FR-S):- \+ p(V),
	p(GoalNode),
	findall(F-gh(GY,H,[Y-V,V-X|T]),
		(after(V,Y,C),
		 \+ member(Y-V, [V-X|T]),
		 GY is GV + C,
		 heuristic(Y,GoalNode,H),
		 F is GY+H
		), L),
	app(Ws,L,NWs),
	keysort(NWs,SNWs),
	tr(SNWs),nl,
	astar1(SNWs,FR-S).

astar(V, S):-
	p(T),
	heuristic(V, T, H),
	astar1([H-gh(0,H,[V-0])], S).

tr([]).
tr([X|T]):-
	write(X),nl,
	tr(T).






heuristic(Y, T, H):-
	coord(Y, XY,YY),
	coord(T, XT,YT),
	DX is XT-XY,
	DY is YT-YY,
	H is sqrt(DX*DX+DY*DY).

%    g(x) - стоимость части *решающего* пути из нач. вершины в x.
%    r(x) - стоимость части *решающего* пути из х в цел. вершину.
%    f(x)=g(x)+r(x), как правило, мы вычислить не можем.
%    f(x)\approx g(x)+W*h(x), \forall x h(x)<=r(x).
%    A*
%
%    F-gh(g,h,Path)
%    Path = [V-_|T]
%    keysort(L1, S1).

after(X,Y,C):-e(X,Y,C).
after(X,Y,C):-e(Y,X,C).

e(oradea,sibiu,151).
e(oradea,zerind,71).
e(zerind,ar,75).
e(ar,sibiu,140).
e(ar,timisoara,118).
e(sibiu,faragas,99).
e(faragas,buharest,211).
e(buharest,pitesti,101).
e(pitesti,rimmicu,97).
e(rimmicu,sibiu,80).
coord(buharest, 300,0).
coord(pitesti, 240,50).
coord(faragas, 230, 100).
coord(rimmicu, 150, 75).
coord(sibiu, 140,110).
coord(ar, 0, 175).
coord(zerind, 10, 180).
coord(oradea, 20, 200).
coord(timisoara, 5,75).