%Program 6.4 Plotting program for two-body problem 
% Inputs: int=[a b] time interval, initial conditions
% ic = [x1 vx1 y1 vy1 x2 vx2 y2 vy2]
% x position, x velocity, y pos, y vel, of body 1 and body 2
% h = stepsize, p = steps per point plotted
% Calls a one-step method such as trapstep.m
% Example usage: orbit2([0 100],[0 1 2 0 0 -0.1 0 0],0.01,5)
function z=orbit2(int,ic,h,p)
n=round((int(2)-int(1))/(p*h));        % plot n points 
x1=ic(1);vx1=ic(2);y1=ic(3);vy1=ic(4); % grab initial conds for body 1
x2=ic(5);vx2=ic(6);y2=ic(7);vy2=ic(8); % grab initial conds for body 2
y(1,:)=[x1 vx1 y1 vy1 x2 vx2 y2 vy2];t(1)=int(1);    % build y vector
set(gca,'XLim',[-5 5],'YLim',[-5 5],'XTick',[-5 0 5],'YTick',...
  [-5 0 5],'Drawmode','fast','Visible','on','NextPlot','add');
cla;
head1=line('color','r','Marker','.','markersize',20,...
  'erase','xor','xdata',[],'ydata',[]);
tail1=line('color','b','LineStyle','-','erase','none',...
  'xdata',[],'ydata',[]);
head2=line('color','b','Marker','.','markersize',30,...
  'erase','xor','xdata',[],'ydata',[]);
tail2=line('color','r','LineStyle','-','erase','none',...
  'xdata',[],'ydata',[]);
%[px,py,button]=ginput(1);         % include these three lines
%[px1,py1,button]=ginput(1);       % to enable mouse support
%[px2,py2,button]=ginput(1);         % include these five lines
%[px3,py3,button]=ginput(1);       % to enable mouse support
%y(1,:)=[px px1-px py py1-py px2 px3-px2 py2 py3-py2];     % 2 clicks set direction
for k=1:n
  for i=1:p
    t(i+1)=t(i)+h;
    y(i+1,:)=singlestep(t(i),y(i,:),h);
  end
  y(1,:)=y(p+1,:);t(1)=t(p+1);
  set(head1,'xdata',y(1,1),'ydata',y(1,3));
  set(tail1,'xdata',y(2:p,1),'ydata',y(2:p,3));
  set(head2,'xdata',y(1,5),'ydata',y(1,7));
  set(tail2,'xdata',y(2:p,5),'ydata',y(2:p,7));
  drawnow;
end

function y=singlestep(t,x,h)
%one step of the Euler method
%y=x+h*ydot(t,x);
%one step of the Trapezoid method
u=ydot(t,x);
y=x+0.5*h*(u+ydot(t+h,x+h*u));

function z = ydot(t,x)
g=1;
m1=0.3; mg1=m1*g;
m2=3; mg2=m2*g;
px1=x(1);py1=x(3);vx1=x(2);vy1=x(4);
px2=x(5);py2=x(7);vx2=x(6);vy2=x(8);
dist=sqrt((px2-px1)^2+(py2-py1)^2);
z=zeros(1,8);
z(1)=vx1;
z(2)=(mg2*(px2-px1))/(dist^3);
z(3)=vy1;
z(4)=(mg2*(py2-py1))/(dist^3);
z(5)=vx2;
z(6)=(mg1*(px1-px2))/(dist^3);
z(7)=vy2;
z(8)=(mg1*(py1-py2))/(dist^3);