% Problem 3 teles v 2D rovine
% Reseni pomoci metody Runge-Kutta 4.radu
% Pro tento typ problemu je lepsi pouzivat
% integratory ktere zachovavaji clekovou
% energii mechanicke soustavy, resp. Hamiltonian
% tj. tzv. symplekticke integratory viz napr.
% https://en.wikipedia.org/wiki/Symplectic_integrator

clear all; clc;

function f = F(X)
% funkce prave strany (viz https://en.wikipedia.org/wiki/Three-body_problem) 
% hmotnosti 3 teles/planet
m1 = 5; m2 = 5; m3 = 8;  
% gravitacni konstanta (zmenit tak aby vypocet hezky vychazel)
G = 100;
% tyto promene (r1,r2,...) jsou definovany pouze uvnitr funkce
% v norm(...) pricitame nejake male cislo, napr 0.001 abychom
% se vyhnuli deleni nulou, popr. velmi malym cislem ... 
r1 = X(1:2); r2 = X(3:4); r3 = X(5:6);
v1 = X(7:8); v2 = X(9:10);v3 = X(11:12);
f = [ v1; -G*m2*(r1-r2)/(0.001+norm(r1-r2))^1.5 - G*m3*(r1-r3)/(0.001+norm(r1-r3))^1.5;
      v2; -G*m3*(r2-r3)/(0.001+norm(r2-r3))^1.5 - G*m1*(r1-r2)/(0.001+norm(r1-r2))^1.5;
      v3; -G*m1*(r3-r1)/(0.001+norm(r3-r1))^1.5 - G*m2*(r3-r2)/(0.001+norm(r3-r2))^1.5];
endfunction

function f = RK4(X,h)
  % prirustkova funkce pro RK4
  k1 = F(X);
  k2 = F(X+h/2*k1);
  k3 = F(X+h/2*k2);
  k4 = F(X+h*k3);
  f  = h/6*(k1+2*k2+2*k3+k4);
endfunction

%----------------------------------------------%
% pocatecni poloha planet (v x-y rovine)
r1 = [90;380]; r2 = [116;58];  r3 = [25;-140];
% pocatecni rychlost planet (v x-y rovine)
v1 = [0;0]; v2 = [0;0]; v3 = [0;0];
% cas
t0 = 0;
t  = t0;
X = [r1;r2;r3;v1;v2;v3];
n = 10000;
% casovy krok
h = 0.015;
sz = 15;

figure (1);
close(1);
figure(1);

for i=1:n
  % toto vykresli koncovy bod
  plot(X(1,i),X(2,i),"o",'MarkerFaceColor', 'b','MarkerSize',sz); % fazovy portret
  hold on
  plot(X(3,i),X(4,i),"o",'MarkerFaceColor', 'r','MarkerSize',sz);
  hold on
  plot(X(5,i),X(6,i),"o",'MarkerFaceColor', 'y','MarkerSize',sz);
  hold on;
  % toto vykresli drahu
  plot(X(1,:),X(2,:)); 
  hold on
  plot(X(3,:),X(4,:));
  hold on
  plot(X(5,:),X(6,:));
  hold off
  axis square
  % mozno zadat i fixni okno pro vykresleni dat
##  axis([-1*10^3 1*10^3 -1*10^3 1*10^3])
  X(:,i+1) = X(:,i) + RK4(X(:,i),h);
  pause(0.001)
endfor

