%Solar system simulation 
% Usage: solar(g,m,x,dx,tfin,h,p)
% Inputs: g = the gravitational constant
% m = masses of the objects
% x = initial coordinates
% dx = initial velocities
% tfin = simulation time
% h = stepsize, p = steps per point plotted
% Calls a one-step method such as trapstep.m
% Example usage: solar(g,m,x,dx,1000,10,3)
function solar(g,mm,x,tfin,h,p)
global m;
m=mm*g;
n=round(tfin/(p*h));        % plot n points

set(gca,'XLim',[-40 40],'YLim',[-40 40],'ZLim',[-40 40],...
    'XTick',[],'YTick',[],'ZTick',[],...
    'Drawmode','fast','Visible','on','NextPlot','add','View',[-37.5,0]);
body = line('color','b','LineStyle','-','erase','none','xdata',[],'ydata',[],'zdata',[]);

for k=1:n
  y=x;  
  for i=1:p
    y=eulerstep(y,h);
  end
  for i=1:3:length(x/2)
    set(body,'xdata',[x(i) y(i)],'ydata',[x(i+1) y(i+1)],'zdata',[x(i+2) y(i+2)]);
    drawnow;
  end
  x=y;
end

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

function y=trapstep(x,h)
%one step of the Trapezoid method
u=rhs(x);
y=x+0.5*h*(u+rhs(x+h*u));

function z = rhs(x)
global m;
z=zeros(length(x),1);
n=length(m)*3;
r=zeros(n,length(m));
ii=1;
for i=1:length(m)
    jj=ii+3;
    for j=i+1:length(m)
        r(jj:jj+2,i)=x(jj:jj+2)-x(ii:ii+2);
        r(ii:ii+2,j)=-r(jj:jj+2,i);
        jj=jj+3;
    end
    ii=ii+3;
end
d=zeros(length(m));
ii=1;
for i=1:length(m)
    jj=ii+3;
    for j=i+1:length(m)
        d(j,i)=sqrt(sum(r(jj:jj+2,i).*r(jj:jj+2,i)));
        d(j,i)=1/d(j,i)^3;
        d(i,j)=d(j,i);
        jj=jj+3;
    end
    ii=ii+3;
end
z(1:n)=x(n+1:length(x));
ii=1;
for i=1:length(m)
    jj=1;
    for j=1:i-1
        z(n+ii:n+ii+2)=z(n+ii:n+ii+2)+m(j)*d(j,i)*r(jj:jj+2,i);
        jj=jj+3;
    end
    for j=i+1:length(m)
        jj=jj+3;
        z(n+ii:n+ii+2)=z(n+ii:n+ii+2)+m(j)*d(j,i)*r(jj:jj+2,i);
    end
    ii=ii+3;
end