clf
xG = 60;yG = 60;xS = 35;yS = 35;
rG = 2.5;
sG = 5;
alpha = .25;
xO = 45; yO = 45; 
xO1 = 46; yO1 = 44;
rO = .5; sO=2;
dX = -1;dY = -1;
t=0:1:60;
N=length(t);

xpos = zeros(1, N);ypos = zeros(1, N);
xpos(1) = xS;
ypos(1) = yS;

for i=2:N
	dG = ((xG - xpos(i-1))^2 + (yG - ypos(i-1))^2)^.5;
	dO = ((xO - xpos(i-1))^2 + (yO - ypos(i-1))^2)^.5;
    dO1 = ((xO1 - xpos(i-1))^2 + (yO1 - ypos(i-1))^2)^.5;
	
	thetaG = atan2((yG - ypos(i-1)),(xG - xpos(i-1)));
	thetaO = atan2((yO - ypos(i-1)),(xO - xpos(i-1)));
    thetaO1 = atan2((yO1 - ypos(i-1)),(xO1 - xpos(i-1)));
    %attractive
	if dG < rG
		dX = 0; dY = 0;
	elseif rG <= dG && dG <= sG + rG 
		dX = alpha * (dG - rG) * cos(thetaG);
		dY = alpha * (dG - rG) * sin(thetaG);
    elseif dG > sG + rG
            dX = alpha * sG * cos(thetaG);
            dY = alpha * sG * sin(thetaG);
    end

    %rejective
	if dO < rO
		dX = -sign(cos(thetaO)) * 1;
		dY = -sign(sin(thetaO)) * 1;
	elseif rO <= dO && dO <= sO + rO
		dX = dX + -alpha * (sO + rO - dO) * cos(thetaO);
		dY = dY + -alpha * (sO + rO - dO) * sin(thetaO);
	elseif dO > sO + rO
		dX = dX;dY = dY;
    end
    
    %tangental
    thetaO = thetaO + 90;
    if dO < rO
		dX = -sign(cos(thetaO)) * 1;
		dY = -sign(sin(thetaO)) * 1;
	elseif rO <= dO && dO <= sO + rO
		dX = dX + -alpha * (sO + rO - dO) * cos(thetaO);
		dY = dY + -alpha * (sO + rO - dO) * sin(thetaO);
	elseif dO > sO + rO
		dX = dX;dY = dY;
    end
    
    %rejective
	if dO1 < rO
		dX = -sign(cos(thetaO1)) * 1;
		dY = -sign(sin(thetaO1)) * 1;
	elseif rO <= dO1 && dO1 <= sO + rO
		dX = dX + -alpha * (sO + rO - dO1) * cos(thetaO1);
		dY = dY + -alpha * (sO + rO - dO1) * sin(thetaO1);
	elseif dO1 > sO + rO
		dX = dX;dY = dY;
    end
    
    %tangental
    thetaO1 = thetaO1 + 90;
    if dO1 < rO
		dX = -sign(cos(thetaO1)) * 1;
		dY = -sign(sin(thetaO1)) * 1;
	elseif rO <= dO1 && dO1 <= sO + rO
		dX = dX + -alpha * (sO + rO - dO1) * cos(thetaO1);
		dY = dY + -alpha * (sO + rO - dO1) * sin(thetaO1);
	elseif dO1 > sO + rO
		dX = dX;dY = dY;
    end
	xpos(i) = xpos(i-1) + dX
	ypos(i) = ypos(i-1) + dY
end
hold on
plot(yG, xG, 'bo');
plot(yS, xS, 'go');
plot(yO, xO, 'bo');
plot(yO1, xO1, 'bo');

plot(ypos,xpos,'r-*')