quarkkeulchen wenden r2f2

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Basic solver, does nothing

% Copyright 2010 The MathWorks, Inc.

  sz = [size(chart,1) size(chart,2)];
 
  [ay,ax] = ind2sub(sz,aIndex);
  [by,bx] = ind2sub(sz,bIndex);
 
  [ty1,tx1,vy1,vx1,by,bx] = oneway(chart,ay,ax,by,bx,0,0,maxThrottle);
  [ty2,tx2,vy2,vx2,by,bx] = oneway(chart,by,bx,ay,ax,vy1,vx1,maxThrottle);

  thrustRow = [ty1 ty2];
  thrustCol = [tx1 tx2];
  
function [ty,tx,vy,vx,by,bx] = ...
      oneway(chart,ay,ax,by,bx,vy,vx,maxThrottle)
  % constants
  r = 2; % neighbor radius
  f = 2; % neighbor offset factor

  rows = size(chart,1);
  cols = size(chart,2);

  % --------------------------------------
  % dijkstra

  Dis = inf(rows,cols);
  Vel = zeros(rows,cols,2);
  Vel(ay,ax,:) = [vy vx];
  Dis(ay,ax) = 0;
  
  % create differences to neighbors
  nb = ones(r*2+1);
  r = (size(nb,1)-1)/2;
  [nby,nbx] = find(nb);
  nby = nby-r-1;
  nbx = nbx-r-1;
  
  nbmap = cell(rows,cols);
  for y=1:rows
    for x=1:cols
      nb = zeros(2,0);
      nby2 = y+nby;
      nbx2 = x+nbx;
      for j=1:numel(nby)
	ny = nby2(j);
	nx = nbx2(j);
	if nx >= 1 && ny >= 1 && nx <= cols && ny <= rows 
	  nb(:,end+1) = [ny; nx];
	end
      end
      nbmap{y,x} = nb;
    end
  end      

  % do the dijkstra
  Q = zeros(2,2*rows*cols);
  Q(:,1) = [ay; ax];
  bottom = 1;
  top = 1;
  inQ = zeros(rows,cols);
  inQ(ay,ax) = 1;
  while top >= bottom
    % pop
    x = Q(2,bottom);
    y = Q(1,bottom);
    bottom = bottom + 1;
    inQ(y,x) = inQ(y,x)-1;
    
    % this pos occurs again later in queue
    if inQ(y,x)
      continue
    end
    
    % do nothing velocity
    dny = Vel(y,x,1)+chart(y,x,1);
    dnx = Vel(y,x,2)+chart(y,x,2);
    
    % find neighbor offset
    dny2 = by-y;
    if sign(dny2) ~= sign(dny)
      dny2 = 0;
    elseif abs(dny2) >=  f*r*abs(dny)
      dny2 = dny;
    else
      dny2 = floor(dny2 / (f*r));
    end
    dnx2 = bx-x;
    if sign(dnx2) ~= sign(dnx)
      dnx2 = 0;
    elseif abs(dnx2) >= f*r*abs(dnx)
      dnx2 = dnx;
    else
      dnx2 = floor(dnx2 / (f*r));
    end
    %dny2=0;
    %dnx2=0;
    
    nb2 = nbmap{y+dny2,x+dnx2};
          
    % neighbors
    for j=1:size(nb2,2)
      ny = nb2(1,end-j+1);
      nx = nb2(2,end-j+1);
      
      if nx == y && ny == y
	continue;
      end     
      
      my = ny-y-dny;
      mx = nx-x-dnx;
      
      % check motor crash
      mtot = abs(my)+abs(mx);
      if mtot > maxThrottle
	continue;
      end
      
      % better path?
      dnew = Dis(y,x)+mtot; 
      if Dis(ny,nx) > dnew	
	Vel(ny,nx,1) = dny+my;
	Vel(ny,nx,2) = dnx+mx;
	Dis(ny,nx) = dnew;
	
	if top == size(Q,2)
	  Q(:,1:top-bottom+1) = Q(:,bottom:top);
	  top = top - bottom + 1;
	  bottom = 1;
	end
	
	Q(:,top+1) = [ny; nx];
	top = top+1;
	inQ(ny,nx) = inQ(ny,nx) + 1;
      end
    end
  end
  
  %Dis
  %Vel

  % --------------------------------------
  % extract motor power

  ty = [];
  tx = [];

  % find best turning spot
  Dis = Dis ...
	+ repmat(abs((1:cols)  - bx),rows,1).^2 ...
	+ repmat(abs((1:rows)' - by),1,cols).^2;
  [dummy,targetIndex] = min(Dis(:));
  [by,bx] = ind2sub([rows cols],targetIndex);
  vy = Vel(by,bx,1);
  vx = Vel(by,bx,2);
  
  % walk the path backwards
  curPos = [by; bx];
  while curPos(1) ~= ay || curPos(2) ~= ax
    x = curPos(2);
    y = curPos(1);
    prevPos = curPos - permute(Vel(y,x,:),[3 1 2]);
    py = prevPos(1);
    px = prevPos(2);
    m = Vel(y,x,:)-chart(py,px,:)-Vel(py,px,:);
    ty(end+1) = m(1);
    tx(end+1) = m(2);
    curPos = prevPos;
  end  
  
  % reverse motor power b/c of backwards walk
  ty = fliplr(ty);
  tx = fliplr(tx);