function mv = preproc(ai,af,w)
% set up some arrays
rand('state',100);
mv=[];
nBlocks = max(ai(:));
[m,n] = size(ai);
% Make increment tables
% N=1, E=2, S=3, W=4
I = [0 1 0 -1];
J = [1 0 -1 0];
%find locations and targets
nBlocks=sum(sum(ai>0));
itarg=w;
jtarg=w;
iinit=w;
jinit=w;
for bid=1:nBlocks
[itarg(bid),jtarg(bid)] = find(af==bid);
end
for bid=1:nBlocks
[iinit(bid),jinit(bid)] = find(ai==bid);
end
icurr=iinit;
jcurr=jinit;
% try to move blocks nearer targets
[wsort,isort]=sort(-w);
a=ai;
for k=1:nBlocks
bid=isort(k);
[mvn,a,icurr(bid),jcurr(bid)] = findpath(a,I,J,m,n,icurr(bid),jcurr(bid),itarg(bid),jtarg(bid));
mv = [mv; mvn];
end
r=rand(1,nBlocks);
[wsort,isort]=sort(r);
for k=1:nBlocks
bid=isort(k);
[mvn,a,icurr(bid),jcurr(bid)] = findpath(a,I,J,m,n,icurr(bid),jcurr(bid),itarg(bid),jtarg(bid));
mv = [mv; mvn];
end
r=rand(1,nBlocks);
[wsort,isort]=sort(r);
for k=1:nBlocks
bid=isort(k);
[mvn,a,icurr(bid),jcurr(bid)] = findpath(a,I,J,m,n,icurr(bid),jcurr(bid),itarg(bid),jtarg(bid));
mv = [mv; mvn];
end
mvn = solver(a,af,w,nBlocks,I,J,m,n,itarg,jtarg,icurr,jcurr,1,1);
if ~mvn
mvn = solver(a,af,w,nBlocks,I,J,m,n,itarg,jtarg,icurr,jcurr,0,2);
end
if ~mvn
mv = solver(ai,af,w,nBlocks,I,J,m,n,itarg,jtarg,iinit,jinit,0,0);
%mv = nq1(ai,af,w);
else
mv = [mv; mvn];
end
% Remove moves that are immediately reversed.
mv = postproc(mv);
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mv=postproc (mv)
nMoves = size(mv,1);
k=1;
while k<nMoves
if mv(k,1)==mv(k+1,1) && ...
( mv(k,2)==mv(k+1,2)-2 || mv(k,2)==mv(k+1,2)+2 )
mv(k,1)=0;
mv(k+1,1)=0;
k=k+2;
else
k=k+1;
end
end
delmove=(mv(:,1)==0);
mv(delmove,:)=[];
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [mv,a,in,jn] = findpath(a,I,J,m,n,istart,jstart,i2,j2)
% Try to find a path fromi1,j1 to i2, j2 in the current map a
bid=a(istart,jstart);
a0=a;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% move in j first, then i
i1=istart; j1=jstart;
mv=[]; a=a0;
in=i1; jn=j1;
if i1==i2 && j1==j2
return
end
dj=0;
if (j2>j1)
col = a(i1,j1+1:j2);
if ~any(col)
dj=j2-j1;
r=1;
jn=j2;
elseif col(1)>0
dj=0;
jn=j1;
else
dj=min(find(col>0))-1; % as far as we can go in positive i dir
r=1;
jn = j1+dj;
end
end
if (j2<j1)
col = a(i1,j1-1:-1:j2);
if ~any(col)
dj=j1-j2;
r=3;
jn=j2;
elseif col(1)>0
dj=0;
jn=j1;
else
dj=(min(find(col>0))-1); % as far as we can go in negative i dir
r = 3;
jn = j1-dj;
end
end
if dj>0
mv(end+1:end+dj,[1 2])=ones(dj,1)*[bid r];
a(i1,j1)=0;
a(i1,jn)=bid;
j1=jn;
end
di=0;
if (i2>i1)
row = a(i1+1:i2,j1);
if ~any(row)
di=i2-i1;
r=2;
in=i2;
elseif row(1)>0
di=0;
in=i1;
else
di=min(find(row>0))-1; % as far as we can go in positive i dir
r=2;
in = i1+di;
end
end
if (i2<i1)
row = a(i1-1:-1:i2,j1);
if ~any(row)
di=i1-i2;
r=4;
in = i2;
elseif row(1)>0
di=0;
in=i1;
else
di=(min(find(row>0))-1); % as far as we can go in negative i dir
r = 4;
in = i1-di;
end
end
if di>0
mv(end+1:end+di,[1 2])=ones(di,1)*[bid r];
a(i1,j1)=0;
a(in,j1)=bid;
i1=in;
end
if (jn==j2)&&(in==i2)
return
end
aj=a; mvj=mv; inj=in; jnj=jn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% move in i first, then in j
i1=istart; j1=jstart;
mv=[]; a=a0;
in=i1; jn=j1;
if i1==i2 && j1==j2
return
end
di=0;
if (i2>i1)
row = a(i1+1:i2,j1);
if ~any(row)
di=i2-i1;
r=2;
in=i2;
elseif row(1)>0
di=0;
in=i1;
else
di=min(find(row>0))-1; % as far as we can go in positive i dir
r=2;
in = i1+di;
end
end
if (i2<i1)
row = a(i1-1:-1:i2,j1);
if ~any(row)
di=i1-i2;
r=4;
in = i2;
elseif row(1)>0
di=0;
in=i1;
else
di=(min(find(row>0))-1); % as far as we can go in negative i dir
r = 4;
in = i1-di;
end
end
if di>0
mv(end+1:end+di,[1 2])=ones(di,1)*[bid r];
a(i1,j1)=0;
a(in,j1)=bid;
i1=in;
end
dj=0;
if (j2>j1)
col = a(i1,j1+1:j2);
if ~any(col)
dj=j2-j1;
r=1;
jn=j2;
elseif col(1)>0
dj=0;
jn=j1;
else
dj=min(find(col>0))-1; % as far as we can go in positive i dir
r=1;
jn = j1+dj;
end
end
if (j2<j1)
col = a(i1,j1-1:-1:j2);
if ~any(col)
dj=j1-j2;
r=3;
jn=j2;
elseif col(1)>0
dj=0;
jn=j1;
else
dj=(min(find(col>0))-1); % as far as we can go in negative i dir
r = 3;
jn = j1-dj;
end
end
if dj>0
mv(end+1:end+dj,[1 2])=ones(dj,1)*[bid r];
a(i1,j1)=0;
a(i1,jn)=bid;
j1=jn;
end
if (jn==j2)&&(in==i2)
return
end
ai=a; mvi=mv; ini=in; jni=jn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% finished going in j then i, now see which was better:
if abs(jnj-j2)+abs(inj-i2) < abs(jni-j2)+abs(ini-i2)
mv=mvj;
a=aj;
in=inj;
jn=jnj;
else
mv=mvi;
a=ai;
in=ini;
jn=jni;
end
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mv = solver(ai,af,w,nBlocks,I,J,m,n,itarg,jtarg,iinit,jinit,probmode,timemode)
rand('state',0);
a = ai;
mv = [];
icurr=iinit;
jcurr=jinit;
outofplace = ~((icurr==itarg)&(jcurr==jtarg));
unfinished = any(outofplace);
switch timemode
case 1
maxmoves = 100+0.736*nBlocks^2.14;
case 2
maxmoves = 2*(100+0.736*nBlocks^2.14);
otherwise
maxmoves=Inf;
end
%calculate state of solution - should tend to zero as solution improves
dist = abs(icurr-itarg) + abs(jcurr-jtarg);
wdist = sum ((abs(icurr-itarg) + abs(jcurr-jtarg)) .* w);
nmoves=0;
%while (~isequal(icurr,itarg) || ~isequal(jcurr,jtarg)) && nmoves<maxmoves
while (unfinished) && nmoves<maxmoves
% weight block probabilities
switch probmode
case 1
% p = cumsum(0.5/nBlocks + 0.25*dist/sum(dist) + 0.25*w/sum(w));
p = cumsum(0.5/nBlocks + 0.5 * (dist.*(w+4))/sum(dist.*(w+4)) );
% p = cumsum(0.5/nBlocks + 0.5*dist/sum(dist));
otherwise
p = [1:nBlocks]/nBlocks;
end
% Pick a random block
rr=rand; bid=1;
while p(bid)<=rr
bid=bid+1;
end
% Get the target location
ti = itarg(bid);
tj = jtarg(bid);
% Get the current lcoation
i = icurr(bid);
j = jcurr(bid);
% % is it already at home?
% if (i==ti) && (j==tj)
% if i>1 && i<m && j>1 && j<n ...
% && sum(sum(a(i-1:i+1,j-1:j+1)))==bid % does it have neighbours?
% continue
% end
% end
% % is it already at home?
% if (i==ti) && (j==tj) && (rand>0.05)
% continue
% end
% Move it in a random direction
r = ceil(rand*4);
ni = i + I(r);
nj = j + J(r);
% Is it a legal move? Check edges
if (ni<1) || (ni>m) || (nj<1) || (nj>n)
continue
end
% Is it a legal move? Check for collision
if a(ni,nj)>0
continue
end
% Check distance
nwdist = wdist - (abs(i-ti) + abs(j-tj)) .* w(bid) + (abs(ni-ti) + abs(nj-tj)) .* w(bid);
% d = (ti-i)^2 + (tj-j)^2;
% dn = (ti-ni)^2 + (tj-nj)^2;
d = abs(ti-i) + abs(tj-j);
dn = abs(ti-ni) + abs(tj-nj);
% Have we moved closer to the target location?
if (d<dn) && (rand>0.05)
continue
end
% Move the block
nmoves=nmoves+1;
a(ni,nj) = bid;
a(i,j) = 0;
icurr(bid) = ni;
jcurr(bid) = nj;
if (ni==ti)&&(nj==tj)
outofplace(bid)=0;
else
outofplace(bid)=1;
end
wdist=nwdist;
dist(bid)=abs(ti-ni)+abs(tj-nj);
% Record the move
mv(end+1,[1 2]) = [bid r];
bid=0;
unfinished=false;
while bid<nBlocks
bid=bid+1;
if outofplace(bid)
unfinished=true;
break
end
end
end
if nmoves>=maxmoves && ~isequal(af,a)
% fprintf(1,'abort...');
mv = 0;
end
return
|
