% Sample Matlab code to construct potential function motion planning
% 
% B. Goodwine

for x=1:100;
    for y=1:100;
        disp(x);
        if((x - y + 59 > 0) & (110 - x - y > 0) & (y - 60 > 0))
             obj(y,x) = 1;
        elseif (30 - y > 0 & x - 20 > 0 & 80 - x > 0 & y - 10 > 0)
             obj(y,x) = 1;
	else
             obj(y,x) = 0;
        end
    end
end

x = linspace(1,100);
y = linspace(1,100);

mesh(x,y,obj);

xlabel('x');
ylabel('y');

q = [1 1];              % start at (1,1)
qgoal = [70 90];        % goal point

xi = 0.1;
eta = 1.0;
delta = 0.25;
rhon = 10;

steps = q;
for step=1:300          % 100 steps max

mindist = 100.0;
     for x=1:100        % brute force find closest object point
          for y=1:100
              if( obj(y,x) > 0 & norm(q - [x y])/10.0 < mindist )
                   mindist = norm(q - [x y])/10.0;
		   min(step) = mindist;
		   qc = [x y];
              end
	  end
     end
     fatt = -xi*(q - qgoal);
     frep = eta*(1/mindist - 1/rhon)*(1/mindist^2)*(q - qc)/norm(q - qc);
     if norm(q - qc) > rhon
        frep = [0.0 0.0];
     end
     q = q + delta*(fatt + frep );
     disp(q);
     steps(step,1) = q(1);
     steps(step,2) = q(2);
end

s = steps';

hold off;
plot(s(1,:),s(2,:));

hold on;

fill([1 25 50],[60 85 60], 'r');
fill([20 20 80 80],[10 30 30 10], 'r');



