%Program 6.4 Plotting program for one-body problem 
% Inputs: time interval inter, initial conditions
% masses = [m1 m2]
% ic = [x0 vx0 y0 vy0], x position, x velocity, y pos, y vel,
% number of steps n, steps per point plotted p
% Calls a one-step method such as trapstep.m
% Example call: orbitNbody3D([0 200],[0.03,0.3,0.03],[2 0.2 2 -0.2 0 0 0 0 0 0.01 0 -0.01 -2 -0.2 0 0 -2 0.2],10000,15)

function z=orbitNbody3D(inter,m,ic,n,p)
clf;
h=(inter(2)-inter(1))/n;        % plot n points
t(1)=inter(1);
for j = 0:length(m)-1
    x0(1+j)=ic(1+j*6);vx0(1+j)=ic(2+j*6);y0(1+j)=ic(3+j*6);vy0(1+j)=ic(4+j*6);z0(1+j)=ic(5+j*6);vz0(1+j)=ic(6+j*6);% grab initial conds
    y(1,1+j*6:6+j*6)=[x0(1+j) vx0(1+j) y0(1+j) vy0(1+j) z0(1+j) vz0(1+j)];% build y vector
    head(1+j)=animatedline('color','r','Marker','.','markersize',35);
    tail(1+j)=animatedline('color','b','LineStyle','-');
end

set(gca,'XLim',[-5 5],'YLim',[-5 5],'ZLim',[-5 5],...
    'XTick',[-5 0 5],'YTick',[-5 0 5],'ZTick',[-5 0 5])
view(-134,20);
a = 5; axis([-a a -a a -a a]);
axis vis3d

%[px,py]=ginput(1);                % include these three lines
%[px1,py1]=ginput(1);              % to enable mouse support
%y(1,:)=[px px1-px py py1-py];     % 2 clicks set direction
for k=1:n/p
  for i=1:p
    t(i+1)=t(i)+h;
    y(i+1,:)=RungeKuttaStep(t(i),y(i,:),h,m);
  end
  y(1,:)=y(p+1,:);t(1)=t(p+1);
  for j = 0:length(m)-1
	clearpoints(head(1+j));addpoints(head(1+j),y(1,1+j*6),y(1,3+j*6),y(1,5+j*6))
	addpoints(tail(1+j),y(1,1+j*6),y(1,3+j*6),y(1,5+j*6))
  end
  drawnow;
end

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

function y=RungeKuttaStep(t,x,h,m)
s1=ydot(t,x,m);
s2=ydot(t+h/2,x+h/2*s1,m);
s3=ydot(t+h/2,x+h/2*s2,m);
s4=ydot(t+h,x+h*s3,m);

y = x+h/6*(s1+2*s2+2*s3+s4);

function z = ydot(t,x,m)
g = 1;
z = zeros(1,length(m)*6);
for j = 0:length(m)-1
	i = 1+j*6:6+j*6;
	x1 = x(i);
	x2 = x; x2(i) = [];
	m2 = m; m2(1+j) = [];
	px1 = x1(1); py1 = x1(3); pz1 = x1(5); vx1 = x1(2); vy1 = x1(4); vz1 = x1(6);
    px2 = zeros(1,length(m2)); py2 = px2; pz2 = px2;
    for i = 0:length(m2)-1
		px2(1+i) = x2(1+i*6);
		py2(1+i) = x2(3+i*6);
        pz2(1+i) = x2(5+i*6);
    end
    
	z(1+j*6)=vx1;
	z(3+j*6)=vy1;
    z(5+j*6)=vz1;
	z(2+j*6)=0;
	z(4+j*6)=0;
    z(6+j*6)=0;
    
    for i = 1:length(m2)
		dist=sqrt((px2(i)-px1)^2+(py2(i)-py1)^2+(pz2(i)-pz1)^2);
		z(2+j*6)=z(2+j*6)+(m2(i)*g*(px2(i)-px1))/(dist^3);
		z(4+j*6)=z(4+j*6)+(m2(i)*g*(py2(i)-py1))/(dist^3);
        z(6+j*6)=z(6+j*6)+(m2(i)*g*(pz2(i)-pz1))/(dist^3);
    end
end