function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Basic solver, does nothing
% Copyright 2010 The MathWorks, Inc.
[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)),bIndex);
maxThrottle;
chart(:,:,1);
chartsize = size(chart(:,:,1));
goal = [bRow, bCol];
curPos= [aRow, aCol];
curVel = [0 0];
thrustRow = [];
thrustCol =[];
posvec = []
goalv = []
avoidmat = landmines(chart, maxThrottle);
maxaproach = 1
if ( avoidmat(bRow, bCol)==1 )
maxaproach = 4
end
for (i = 1:100)
dVelW = [chart(curPos(1),curPos(2),1), chart(curPos(1),curPos(2),2)];
% actThrust = [ 1 1];
veldrift = dVelW + curVel;
actThrust = thrustcalc( chart, veldrift, curPos, goal, maxThrottle, avoidmat );
% thrustRow = [thrustRow ,actThrust(1) ];
% thrustCol = [thrustCol ,actThrust(2) ];
newVel = dVelW + curVel +actThrust;
newPos = curPos+ newVel;
% avoid loop
% if ( newVel(1) ==0 && newVel(2) == 0)
% disp('-------!!! will stay in a loop! ')
% if( rand(1) >0.9)
% actThrust(1) = sign(actThrust(1)) * (abs( actThrust(1))+1);
% else
% actThrust(2) = sign(actThrust(2)) * (abs( actThrust(2))+1);
% end
% newVel = dVelW + curVel +actThrust;
% newPos = curPos+ newVel;
% end
trackv = [ newPos(1), newPos(2) , newVel(1), newVel(2), goal(1), goal(2) ]
%posvx = [posvx, newPos(1) ];
%posvy = [posvy, newPos(2) ];
%goalv = [ goalv , goal']
if (i>1)
h = trackv' * ones(1,size(posvec,2));
% size(h)
% size(posvec)
checkbit = prod (sum(abs(posvec-h),1));
if (checkbit==0) disp('loop found');
% disp('-------!!! will stay in a loop! ')
if( rand(1) >0.8)
actThrust(1) = sign(actThrust(1)) * (abs( actThrust(1))+1);
else
actThrust(2) = sign(actThrust(2)) * (abs( actThrust(2))+1);
end
newVel = dVelW + curVel +actThrust;
newPos = curPos+ newVel;
end
trackv = [ newPos(1), newPos(2) , newVel(1), newVel(2), goal(1), goal(2) ];
end
posvec= [posvec ,trackv' ];
thrustRow = [thrustRow ,actThrust(1) ];
thrustCol = [thrustCol ,actThrust(2) ];
curVel = newVel;
curPos = newPos;
[distv,dx,dy] = distf(curPos, goal);
distance= distv
if (distance <= maxaproach )
goal = [aRow, aCol] ;
[distance,dx,dy] = distf(curPos, goal);
disp('turn around, go home! ')
end
if (goal(1) == aRow && goal(2) == aCol && distance== 0 )
fprintf ('done, finish!')
break
end
outofmap(newPos, chartsize);
if outofmap(newPos, chartsize)
disp('boat will leave the map! ')
fprintf( 'nxet bad postion: %d ,%d', newPos)
thrustRow = thrustRow(1:end-1);
thrustCol = thrustCol(1:end-1);
break
end
goal;
fprintf ('-------end cycle--------')
fprintf ('i: %d, pos: %d,%d \n', i , curPos )
end % end for cycle
thrustRow;
thrustCol;
thrustvec = [thrustRow; thrustCol]
maxThrottle
posvec;
avoidmat;
fprintf('fatal fields: %d \n', sum(sum(avoidmat)))
%goalv
%posvec= [posvx ; posvy; goalv(1,:) ; goalv(2,:) ]
%posvec == posvec(60,:)
%acttrust= [1, 1]
%thrustRow = [0 0 0 0 ];
%thrustCol = [0 0 0 0 ];
%thrustRow = [thrustRow ,acttrust(1) ]
%thrustCol = [thrustCol ,acttrust(2) ]
end
function [distance, dx, dy] = distf( pos1, pos2 )
distance = ( abs(pos2(1)- pos1(1)) + abs(pos2(2)- pos1(2)) );
dx= pos2(1)- pos1(1);
dy= pos2(2)- pos1(2);
end
function thrust = thrustcalc ( chart, veldrift, curPos, goal, maxThrottle, avoidm )
% here comes the algorithm
swsaver=0;
[distv,dx,dy] = distf(curPos, goal);
mt = min(abs(dx),abs(dy));
if (distv==0)
thrust= 0;
return
elseif (distv >0 && mt==0)
dir = [ sign(dx) sign(dy) ];
else
dir = [ round(dx/mt), round(dy/mt) ];
end
if (distv > 20)
dir = dir*3;
elseif (distv > 18)
dir = dir*2;
end
if (velnorm (dir) > maxThrottle && (distv < 3 * maxThrottle) )
dir= round( (maxThrottle*0.5)* dir/velnorm(dir))
disp('dir was tto large, modified');
end
if ( distv < 10 )
spd = round(distv/5)+1;
dir= round( spd * dir/ velnorm(dir)) ;
fprintf('dir was too large, modified %d,%d\n',spd, distv );
end
fprintf('dir %d %d, veldrift %d %d \n', dir, veldrift )
thrguess = dir-veldrift
if ( velnorm(thrguess) < maxThrottle )
thrust = thrguess;
% if( (velnorm(thrguess) < maxThrottle-1) && rand(1) > 1 )
% disp('random term activated')
% if( rand(1) > 0.8) % bring in random term to avoid loops
% thrust(1) = sign(thrguess(1)) * (abs(thrguess(1))+1);
% else
% thrust(2) = sign(thrguess(2)) * (abs( thrguess(2))+1);
% end
% end
else
disp('thrust too big , reduce it')
thrust = round (thrguess./2)
if ( velnorm(thrust) > maxThrottle )
thrustn = renorm(thrust, maxThrottle);
thrust = thrustn;
end
end
% avoid fatal situations
nextpos = curPos+ veldrift + thrust;
chsize = size(chart(:,:,1));
thrustorig = thrust;
if ( outofmap(nextpos, chsize) )
if ( nextpos(1) > chsize(1) )
maxmove = chsize(1) - curPos(1)
thrust(1) = -veldrift(1)+maxmove
fprintf('danger +x dir:pos %d, veldrift: %d, maxm:%d',curPos(1), veldrift(1), maxmove )
%min( abs(maxThrottle-thrust(1)), abs(thrustorig(2)) )
thrust(2) = sign(thrustorig(2))* min( abs(maxThrottle-abs(thrust(1))), abs(thrustorig(2)) )
nextpos = curPos+ veldrift + thrust;
thrustorig = thrust;
elseif ( nextpos(1) < 1 )
maxmove = 1 - curPos(1)
fprintf('danger -x dir:pos %d, veldrift: %d, maxm:%d',curPos(1), veldrift(1), maxmove )
thrust(1) = -veldrift(1)+maxmove
thrust(2) = sign(thrustorig(2))* min( abs(maxThrottle-abs(thrust(1))), abs(thrustorig(2)) )
nextpos = curPos+ veldrift + thrust;
thrustorig = thrust;
end
if ( nextpos(2) > chsize(2) )
maxmove = chsize(2) - curPos(2)
fprintf('danger +y dir:pos %d, veldrift: %d, maxm:%d',curPos(2), veldrift(2), maxmove )
thrust(2) = -veldrift(2)+maxmove
thrust(1) = sign(thrustorig(1))* min( abs(maxThrottle-abs(thrust(2))), abs(thrustorig(1)) )
nextpos = curPos+ veldrift + thrust;
thrustorig = thrust;
elseif ( nextpos(2) < 1 )
maxmove = 1 - curPos(2)
fprintf('danger -y dir:pos %d, veldrift: %d, maxm:%d',curPos(2), veldrift(2), maxmove )
thrust(2) = -veldrift(2)+maxmove
thrust(1) = sign(thrustorig(1))* min( abs(maxThrottle-abs(thrust(2))), abs(thrustorig(1)) )
nextpos = curPos+ veldrift + thrust;
thrustorig = thrust;
end
if (velnorm(thrust) > maxThrottle )
thrust = [0 0]
swsaver=1
end
end
size(avoidm)
nextpos
if ( outofmap(nextpos, chsize) || swsaver==1)
disp('outofmap was not enough! ')
thrust = calcSaver(curPos, veldrift, maxThrottle, chsize,avoidm )
elseif ( avoidm(nextpos(1), nextpos(2)) )
disp('avoid land mines! ')
thrust = calcSaver(curPos, veldrift, maxThrottle, chsize,avoidm )
end
nextpos = curPos+ veldrift + thrust;
% avoid fatal situations
end
function vabs = velnorm (vel)
vabs = abs(vel(1))+abs(vel(2));
end
function sw = outofmap (pos, chsize)
if ( pos(1)<1 || pos(1)> chsize(1) || pos(2)<1 || pos(2)> chsize(2) )
sw= 1;
else
sw= 0;
end
end
function v =renorm(thrust, maxThrottle)
v= thrust ;
while ( velnorm(v) > maxThrottle )
v= round (v * 0.8);
end
end
function avoidm = landmines(chart, maxThrottle )
xlim= size(chart(:,:,1),1);
ylim= size(chart(:,:,1),2);
xv= 1:xlim;
yv= 1:ylim;
distfxlim = (fliplr(xv)'*ones(1,ylim))-1;
distfx0 = ((xv)'*ones(1,ylim))-1;
distfylim = (ones(1,xlim)' * fliplr(yv))-1;
distfy0 = (ones(1,xlim)' * yv )-1;
flydown= chart(:,:,1)- distfxlim;
flyup = -chart(:,:,1) - distfx0;
flyright= chart(:,:,2)- distfylim;
flyleft = -chart(:,:,2) - distfy0;
flydown(flydown<0)= 0;
flyup(flyup<0)= 0;
flyright(flyright<0)= 0;
flyleft(flyleft<0)= 0;
% avoidm = ( ( abs( chart(:,:,1)) + abs( chart(:,:,2)) ) > maxThrottle );
avoidm = ( ( flydown + flyup + flyright + flyleft ) > (maxThrottle -2))
end
function thrustok = calcSaver( curPos, veldrift, maxThrottle, chsize, avoidm)
disp (' INSIDE SAVER FUNCTION')
tv = 0:maxThrottle
% postx = [ fliplr(tv), -(tv(2:end)), -fliplr(tv) ]
% posty = [ (tv), fliplr(tv(1:end)) ]
% post= [postx;posty]
thrustok = [0 0]
for tvx= -maxThrottle: maxThrottle
for k=-1:2:1
tvy = k* (maxThrottle -abs(tvx))
thrust = [ tvx,tvy ]
nextpos = curPos+ veldrift + thrust;
if (~ outofmap (nextpos, chsize))
if( avoidm(nextpos(1),nextpos(2))==1 )
fprintf('thrust %d , %d, not accaptable \n', tvx, tvy )
else
thrustok = thrust;
return
end
% fprintf(' %d , %d, %d \n', tvx, tvy1, tvy2 )
end
end
end
disp('no solution found, you will die!! :(')
end
|
