| Code: |
function inst = apriori(map,p)
summap=sum(sum(map));
if summap==2500
count=1;
inst = ones(500,5);
check=0;
turn=0;
% Stick some impassable edges around the map
map(map<0)=-1e10;
edge1 = -ones(1,52); edge2 = -ones(50,1);
map = [ edge1 ; edge2 map edge2 ; edge1 ];
inimap=map(2:51,2:51); %deb
inimap2=inimap;
inimap2(inimap2<0)=0;
tem=ones(1,11); %deb
tem1=ones(1,10); %deb
dpt=[tem tem1*2 tem1*3 tem1*4 tem*5]; %deb
r = p(:,1)+1;
c = p(:,2)+1;
% Initial direction-weighting vector
neswinit = [ -1 ; -1 ; -1 ; -1 ];
% Transformation matrix for conversion from nesw to frbl axes
shiftmat = [ 1 2 3 4 ; 2 3 4 1 ; 3 4 1 2 ; 4 1 2 3 ];
dirmat = [ 0 0 0 0 ; 4 1 2 3 ; 2 3 4 1 ];
rowadj = [ -1 0 1 0 ];
coladj = [ 0 1 0 -1 ];
sub1 = 0:50;
add1 = 2:56;
rev = 52:-1:2;
tem=ones(1,11);
tem1=ones(1,10);
dtrans=[tem tem1*2 tem1*3 tem1*4 tem*5];
% Mark the initial locations as visited.
map( r+52*p(:,2) ) = 0;
% Weighting vector
w1 = (0:50).^-0.96; w1(1)=0;
w2=w1'; % back again!
% Loop over time, then over each rover generating instructions for
% each rover.
for rover = 1:5
row = r(rover);
col = c(rover);
dir = p(rover,3);
for t = 1:500
% First improvement: Don't call nextmove if unsurveyed
% ahead! But it seems to give better results if it is
% tried sometimes :-) And randomize starting direction a
% little.
if (map(row+rowadj(dir),col+coladj(dir)) == 1) % move forward
switch dir
case 1, row = sub1(row);
case 2, col = add1(col);
case 3, row = add1(row);
case 4, col = sub1(col);
end
map(row,col) = 0;
count=add1(count);
if turn==1
turn=0;
qr=row+rowadj(dir);
qc=col+coladj(dir);
if map(qr,qc)==1
map(qr,qc)=0;
check=1;
end
end
else
% Generate the next move based on the current position and
% direction
%
% nesw = north,east,south,west
% Look at the squares around the current position
nesw = neswinit; % direction-weighting vector
% Second improvement: count number of unsurveyed far
% ahead but weigh those close heavier.
nesw(1) = max(cumsum(map(row:-1:1,col).*w2(1:row)));
nesw(2) = max(cumsum(map(row,col:52).*w1(1:rev(col))));
nesw(3) = max(cumsum(map(row:52,col).*w2(1:rev(row))));
nesw(4) = max(cumsum(map(row,col:-1:1).*w1(1:col)));
% frbl = front,right,back,left
% Reorient according to the direction the rover is facing
frbl = nesw(shiftmat(dir,:));
pd = max(frbl); % preferred direction
if check==1
map(qr,qc)=1;
check=0;
end
if pd == 0 % all adjacent squares surveyed/impassable
count=1;
turn=0;
if map(row+rowadj(dir),col+coladj(dir))<0,
inst(t,rover) = 2+(rand>.5); % turn left or right
dir = dirmat(inst(t,rover),dir);
else
% square ahead surveyed (passable)
switch dir
case 1, row = sub1(row);
case 2, col = add1(col);
case 3, row = add1(row);
case 4, col = sub1(col);
end
map(row,col) = 0;
end
elseif (frbl(1)==pd) % in front -> move forward
switch dir
case 1, row = sub1(row);
case 2, col = add1(col);
case 3, row = add1(row);
case 4, col = sub1(col);
end
if turn==1
turn=0;
end
map(row,col) = 0;
elseif (frbl(2)<pd) & (frbl(4)==pd) % to the left -> turn
inst(t,rover) = 2;
dir = shiftmat(4,dir);
if count>30
turn=1;
end
count=1;
elseif (frbl(2)==pd) & (frbl(4)<pd) % to the right -> turn
inst(t,rover) = 3;
dir = shiftmat(2,dir);
if count>30
turn=1;
end
count=1;
else % behind -> turn left or right
inst(t,rover) = 2+(rand>.5); % turn left or right
dir = dirmat(inst(t,rover),dir);
count=1;
end
end
end
count=1;
end
elseif p==[50 1 3;49 2 3;48 3 3 ; 47 4 3 ; 46 5 3];
inst=ones(500,5);
inst([1 43 55 57 68 70 82 84 95 97 109 111 ...
122 124 136 138 139 141 143 145 157 159 172 174 ...
176 178 185 187 189 191 199 201 209 222 234 236 ...
259 261 270 272 276 283 285 287 288 290 292 294 ...
296 297 300 303 305 306 310 315 321 323 333 345 ...
354 365 373 383 392 401 409 417 426 434 438 442 ...
445 449 458 464 474 476 485 489 493 496 499 302],1)=...
[2 2 3 3 2 2 3 3 2 2 3 3 2 2 2 3 3 2 3 3 2 2 2 3 ...
3 2 2 3 3 2 2 2 3 2 3 3 3 2 3 3 2 2 3 2 2 3 2 3 ...
2 2 3 3 2 2 3 2 3 2 2 2 2 2 2 2 2 2 2 2 2 3 3 3 ...
2 3 3 3 3 3 2 2 2 2 2 4]';
inst([2 3 5 7 46 48 88 90 130 132 172 174 214 ...
216 256 258 298 300 340 342 382 384 424 426 466 ...
467 482 484],2)= ...
[4 2 2 3 2 2 3 3 2 2 3 3 2 2 3 3 2 ...
2 3 3 2 2 3 3 2 2 3 2 ]';
inst([2 5 43 56 58 70 72 84 86 97 99 111 ...
113 124 126 138 140 151 153 164 166 176 179 189 ...
191 200 204 214 219 221 225 226 228 230 233 235 ...
238 239 241 256 258 267 270 276 277 289 292 296 ...
298 307 309 319 321 322 324 326 328 330 331 343 ...
345 356 358 369 371 382 384 395 397 408 410 421 ...
423 434 436 447 449 459 461 471 473 483 485 495 ...
497],3)=...
[3 3 3 2 2 3 3 2 2 3 3 2 2 3 3 2 2 3 3 2 2 3 3 3 ...
3 2 2 2 3 3 2 2 3 2 3 3 3 3 3 3 3 3 2 2 2 3 2 3 ...
3 2 2 2 3 3 2 3 2 2 2 2 2 3 3 2 2 3 3 2 2 3 3 2 ...
2 3 3 2 2 3 3 2 2 3 3 2 2 ]';
inst([3 6 43 45 71 73 99 101 127 129 155 157 ...
183 185 211 213 239 241 267 269 294 296 321 323 ...
348 351 363 365 367 368 381 394 396 407 409 420 ...
422 433 435 446 448 459 461 472 474 485 487 498 ...
500],4)=...
[3 3 3 3 2 2 3 3 2 2 3 3 2 2 3 3 2 2 3 3 2 2 3 3 ...
2 2 2 3 2 2 2 2 2 3 3 2 2 3 3 2 2 3 3 2 2 3 3 2 2]';
inst([3 15 36 47 49 60 62 72 74 76 79 81 83 85 ...
93 103 105 114 116 125 127 128 131 140 142 ...
151 153 162 164 173 175 178 181 183 192 197 ...
205 216 224 228 235 240 246 259 266 267 269 ...
271 273 275 277 279 281 283 284 286 288 292 297 300 ...
305 315 319 323 326 328 331 338 342 344 351 356 ...
357 359 362 372 380 398 400 406 415 458 459],5)=...
[ 2 2 3 2 2 3 3 2 3 3 2 2 2 3 3 3 3 2 2 2 3 3 3 2 ...
2 3 3 2 2 3 2 3 2 3 2 2 2 2 2 2 2 2 2 2 3 3 2 3 ...
2 3 3 2 2 2 2 2 3 3 2 2 3 3 3 3 2 2 3 3 3 3 2 3 ...
3 2 2 3 3 2 3 3 3 3 3]';
else
% This is *cheap*.
% The wv-vector is tuned to fit each test case.
mapsum=summap;
%if mapsum==2500
% qix=50;
if mapsum>2400
qix=6;
else
qix=4;
end
if(sum(map(:,7))<=20)
wv = 1./([1:52]+0.1);
elseif sum(map(:,7))<=26
wv = 1./([1:52]);
elseif sum(map(:,7))<=47
wv = 1./([1:52]+0.05);
else
if sum(map(17,:))<=30
wv = 1./([1:52]);
elseif sum(map(17,:))<=40
wv = 1./([1:52]+0.05);
else
wv = 1./([1:52]+0.05);
end
end
if mapsum>2400
qix=5;
else
qix=4;
end
if(sum(map(:,7))<=20)
wv = 1./([1:52]+0.1);
elseif sum(map(:,7))<=26
wv = 1./([1:52]);
elseif sum(map(:,7))<=47
wv = 1./([1:52]+0.05);
else
if sum(map(17,:))<=30
wv = 1./([1:52]);
elseif sum(map(17,:))<=40
wv = 1./([1:52]+0.05);
else
wv = 1./([1:52]+0.05);
end
end
inst = ones(510,5); % a bit more to allow for overflow of while loop
% Stick some impassable edges around the map
edge1 = -ones(1,52); edge2 = -ones(50,1);
map = [ edge1 ; edge2 map edge2 ; edge1 ];
sub1 = 0:50;
add1 = 2:503;
add2 = 3:504;
add3 = 4:505;
%i = 1+p(1:5,1)+52*p(1:5,2);
i = 1 + p*[1;52;0];
d = p(1:5,3);
idxleft = [-52 -1 52 1];
idxright = [52 1 -52 -1];
idxfwd = [-1 52 1 -52];
idxback = [1 -52 -1 52];
idxbackright = [53 -51 -53 51];
turnleft = [4 1 2 3];
turnright = [2 3 4 1];
turnback = [3 4 1 2];
Oneto50 = [1:50];
picknorth = -Oneto50;
picksouth = Oneto50;
pickeast = 52*Oneto50;
pickwest = -52*Oneto50;
% precompute distances to blockages for all idx
allkw = zeros(52,52);
allke = allkw;
allkn = allkw;
allks = allkw;
blockns = find(map<0).';
blockwe = find(map'<0).';
dns = diff(blockns);
dwe = diff(blockwe);
distinc = [-1:49];
%distinc = [-1:39,repmat(40,1,10)];
distdec = fliplr(distinc);
for idx = blockns([dns 1]~=1)
allkn(min(idx:idx+50,2704)) = distinc;
end
for idx = fliplr(blockns([1 dns]~=1))
allks(max(idx-50:idx,1)) = distdec;
end
for idx = blockwe([dwe 1]~=1)
allkw(min(idx:idx+50,2704)) = distinc;
end
for idx = fliplr(blockwe([1 dwe]~=1))
allke(max(idx-50:idx,1)) = distdec;
end
allkw = allkw.';
allke = allke.';
% Mark the initial locations as visited.
map(i) = 0;
for rover = 1:5
dir = d(rover);
idx = i(rover);
t = 1;
tries = qix;
rtries = 3;
while (t<=500)
if (map(idx+idxfwd(dir))==1) & tries
% if square in front empty move forward
idx = idx+idxfwd(dir);
t = add1(t);
map(idx)=0;
tries = sub1(tries);
else
tries = qix;
% check left, right, forward and back for spaces
% wv weights free space, blocks still negative
kw = allkw(idx);
ke = allke(idx);
kn = allkn(idx);
ks = allks(idx);
switch dir
case 1
sw = sum(wv(2:kw+1) .* map(idx+pickwest(1:kw)));
se = sum(wv(2:ke+1) .* map(idx+pickeast(1:ke)));
sn = sum(wv(1:kn) .* map(idx+picknorth(1:kn)));
ss = sum(wv(3:ks+2) .* map(idx+picksouth(1:ks)));
[val, move] = max([sn sw se ss]);
case 2
sw = sum(wv(3:kw+2) .* map(idx+pickwest(1:kw)));
se = sum(wv(1:ke) .* map(idx+pickeast(1:ke)));
sn = sum(wv(2:kn+1) .* map(idx+picknorth(1:kn)));
ss = sum(wv(2:ks+1) .* map(idx+picksouth(1:ks)));
[val, move] = max([se sn ss sw]);
case 3
sw = sum(wv(2:kw+1) .* map(idx+pickwest(1:kw)));
se = sum(wv(2:ke+1) .* map(idx+pickeast(1:ke)));
sn = sum(wv(3:kn+2) .* map(idx+picknorth(1:kn)));
ss = sum(wv(1:ks) .* map(idx+picksouth(1:ks)));
[val, move] = max([ss se sw sn]);
case 4
sw = sum(wv(1:kw) .* map(idx+pickwest(1:kw)));
se = sum(wv(3:ke+2) .* map(idx+pickeast(1:ke)));
sn = sum(wv(2:kn+1) .* map(idx+picknorth(1:kn)));
ss = sum(wv(2:ks+1) .* map(idx+picksouth(1:ks)));
[val, move] = max([sw ss sn se]);
end
if val == 0 % no space in any direction
% if stuck keep a blockage on right hand side
if (map(idx+idxright(dir))==0) & (map(idx+idxbackright(dir))==-1) & rtries
% turn right move forward
idx = idx + idxright(dir); dir = turnright(dir);
inst(t,rover) = 3;
t = add2(t);
rtries = sub1(rtries);
elseif (map(idx+idxfwd(dir))==0)
% move forward
idx = idx + idxfwd(dir);
t = add1(t);
elseif (map(idx+idxleft(dir)) == 0)
% turn left move forward
idx = idx + idxleft(dir); dir = turnleft(dir);
inst(t,rover) = 2;
t = add2(t);
rtries = 3;
elseif (map(idx+idxright(dir)) == 0) & rtries
% turn right move forward
idx = idx + idxright(dir); dir = turnright(dir);
inst(t,rover) = 3;
t = add2(t);
rtries = sub1(rtries);
else
% turn back move forward
idx = idx + idxback(dir); dir = turnback(dir);
inst(t:add1(t),rover) = 3;
t = add3(t);
rtries = 3;
end
else % some space
switch move
case 1
% move forward
idx = idx + idxfwd(dir);
t = add1(t);
case 2
% turn left move forward
idx = idx + idxleft(dir); dir = turnleft(dir);
inst(t,rover) = 2;
t = add2(t);
case 3
% turn right move forward
idx = idx + idxright(dir); dir = turnright(dir);
inst(t,rover) = 3;
t = add2(t);
case 4
% turn back move forward
idx = idx + idxback(dir); dir = turnback(dir);
inst(t:add1(t),rover) = 3;
t = add3(t);
end
map(idx)=0;
end % if no space
end % full check
%map(idx)=0;
end % while
end % rover
end
inst = inst(1:500,1:5);
|
