gravity2.m

clear variables
global deltat g mass pos2 vel2 nbod ndime nsteps miter color markersize ...
       xmin xmax ymin ymax

close all   

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PROBLEM DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbod = 7; %number of bodies
ndime = 2; %number of dimensions (3 not tested)
nsteps =40000; %number of time steps
g = 0.0001; %gravity value
deltat = 0.05; %time step size

position(1:ndime,1:nbod) = 0.0;
velocity(1:ndime,1:nbod) = 0.0;
accelera(1:ndime,1:nbod) = 0.0;

position(1,1) = 0.0;  %body initial position
position(2,1) = 0.0;
position(1,2) = 1.0;
position(2,2) = 0.0;
position(1,3) = 25.0;
position(1,4) = -20.0;
position(1,5) = -50.0;
position(1,6) = -51;
position(1,7) = -100;
position(2,7) = -25;
position(2,3) = 0.0;

velocity(2,2) = 0.3;  %body initial velocity
velocity(1,2) = 0;
velocity(2,1) = -0.3;
velocity(2,3) = 0.1;
velocity(2,4) = -0.1;
velocity(2,5) = -0.07;
velocity(2,6) = -0.08;
velocity(1,7) = 0.07;

mass(1) = 1000;  %body mass
mass(2) = 1000;
mass(3) = 1;
mass(4) = 1;
mass(5) = 1;
mass(6) = 0.001;
mass(7) = 1;

%NUMERICAL
miter = 60; %maximum number of iterations at each time step
tol = 1e-6; %tolerance for iterations at each time step

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%END PROBLEM DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


iposition = position;
iposold = iposition;
pos2 = zeros(ndime,nbod,nsteps);
vel2 = zeros(ndime,nbod,nsteps);
vold = velocity;
aold = accelera;
matrix = zeros(nbod*ndime,nbod*ndime);
force = zeros(nbod*ndime,1);
%Computing newmark constants
newm.beta  = 0.25;
newm.gamma = 0.5;
newm.b(1) = 1/(newm.beta*deltat*deltat);
newm.b(2) = -1/(newm.beta*deltat);
newm.b(3) = (1-1/(2*newm.beta));
newm.b(4) = newm.gamma/(newm.beta*deltat);
newm.b(5) = 1-newm.gamma/newm.beta;
newm.b(6) = deltat*(1-newm.gamma/(2*newm.beta));
maxiter = 0;

for istep = 1:nsteps
    if rem(istep,nsteps/100) == 0
        disp(['Progress: ', num2str(istep/nsteps*100), '%']);
    end
    error = 1;
    iiter = 0;
    while (error > tol && iiter < miter)
        iiter = iiter+1;
        force(:) = 0.0;
        matrix(:,:) = 0.0;
        rhs_vec = -(iposition-position)*newm.b(1)-velocity*newm.b(2)-accelera*newm.b(3);
        for ibod = 1:nbod
            for idime = 1:ndime
                matrix(ndime*(ibod-1)+idime,ndime*(ibod-1)+idime) = mass(ibod)*newm.b(1);
                force(ndime*(ibod-1)+idime) = mass(ibod)*rhs_vec(idime,ibod);
            end
            for jbod = 1:nbod
                if (jbod ~= ibod)
                    vec = iposition(:,jbod)-iposition(:,ibod);
                    %dist2 = dot(vec,vec);
                    dist2 = 0;
                    for idime = 1:ndime
                        dist2 = dist2 + vec(idime)*vec(idime);
                    end
                    for idime = 1:ndime
                        matrix(ndime*(ibod-1)+idime,ndime*(ibod-1)+idime) = matrix(ndime*(ibod-1)+idime,ndime*(ibod-1)+idime) ...
                            + g*mass(ibod)*mass(jbod)   *dist2^(-3/2);
                        matrix(ndime*(ibod-1)+idime,ndime*(jbod-1)+idime) = matrix(ndime*(ibod-1)+idime,ndime*(jbod-1)+idime) ...
                            - g*mass(ibod)*mass(jbod)   *dist2^(-3/2);
                        for jdime = 1:ndime
                            matrix(ndime*(ibod-1)+idime,ndime*(ibod-1)+jdime) = matrix(ndime*(ibod-1)+idime,ndime*(ibod-1)+jdime) ...
                                - g*mass(ibod)*mass(jbod)   *(-3)*vec(idime)*vec(jdime)*dist2^(-5/2);
                            matrix(ndime*(ibod-1)+idime,ndime*(jbod-1)+jdime) = matrix(ndime*(ibod-1)+idime,ndime*(jbod-1)+jdime) ...
                                + g*mass(ibod)*mass(jbod)   *(-3)*vec(idime)*vec(jdime)*dist2^(-5/2);
                        end
                        force(ndime*(ibod-1)+idime) = force(ndime*(ibod-1)+idime) + vec(idime)*g*mass(ibod)*mass(jbod)/dist2^1.5;
                    end
                end
            end
        end
        u = matrix\force;
        for ibod = 1:nbod
            iposition(:,ibod) = iposition(:,ibod)+u(ndime*(ibod-1)+1:ndime*ibod);
        end
        error = norm(iposition-iposold)/norm(iposition-position);
        iposold = iposition;
    end
    %if iiter == miter
    %    istep
    %end
    maxiter = max(maxiter,iiter);
    %disp(['Number of iterations: ', num2str(iiter)])
    velocity = newm.b(4)*(iposition-position) + newm.b(5)*vold + newm.b(6)*aold;
    accelera = newm.b(1)*(iposition-position) + newm.b(2)*vold + newm.b(3)*aold;
    position = iposition;
    vold = velocity;
    aold = accelera;
    pos2(:,:,istep) = position;
    vel2(:,:,istep) = velocity;
end
disp(['Maximum Number of iterations: ', num2str(maxiter)])

%Post-process
xmin = min(min(pos2(1,:,:)-1));
xmax = max(max(pos2(1,:,:)+1));
ymin = min(min(pos2(2,:,:)-1));
ymax = max(max(pos2(2,:,:)+1));
distx = xmax-xmin;
xmean = 0.5*(xmax+xmin);
disty = ymax-ymin;
ymean = 0.5*(ymax+ymin);
if distx > disty
    ymin = ymean-distx*0.5;
    ymax = ymean+distx*0.5;
else
    xmin = xmean-disty*0.5;
    xmax = xmean+disty*0.5;
end
color(1:3,1:nbod) = 1;
markersize(1:nbod) = 8;
color(:,1) = [1 0 0];
color(:,2) = [0 1 0];
color(:,3) = [0 0 1];
color(:,4) = [0 0 1];
color(:,5) = [1 1 1];
color(:,6) = [0 0 1];
color(:,7) = [0 0 0];
markersize(1) = 8;
markersize(2) = 8;
markersize(3) = 8;
markersize(4) = 8;
markersize(5) = 8;
markersize(6) = 4;
markersize(7) = 6;
figure('color','w')
for istep = 1:30:nsteps
    clf
    axis([xmin, xmax, ymin, ymax])
    axis off
    %axis equal
    hold on
    for ibod = 1:nbod
        plot(pos2(1,ibod,istep),pos2(2,ibod,istep),'-mo',...
            'LineWidth',2,...
            'MarkerEdgeColor','k',...
            'MarkerFaceColor',color(:,ibod),...
            'MarkerSize',markersize(ibod));
        hold on
    end
    pause(0.001)
end