#
#octave_mechanics_doublependulum.m
#https://ix86.org/octave/index-main.html?octave_mechanics_triplependulum
#
function out = octave_mechanics_triplependulum()
dt =  0.01;
m1 = 1;
m2 = 1;
m3 = 1;
l1 = 0.5;
l2 = 0.5;
l3 = 0.5;
g = 9.8;
x_k = [120/180*pi ;
       120/180*pi ;
       120/180*pi ;
          0      ;
          0      ;
          0      ];
x = [];
tim =[];

for t = 0 : dt : 10
    x_k1 = x_k;
    x_tmp = x_k;
    for j =1 : 1 : 4
     A=[(m1+m2+m3)*l1 (m2+m3)*l2*cos(x_tmp(1)-x_tmp(2)) m3*l3*cos(x_tmp(1)-x_tmp(3));
        (m2+m3)*l1*cos(x_tmp(2)-x_tmp(1)) (m2+m3)*l2    m3*l3*cos(x_tmp(2)-x_tmp(3));
         m3*l1*cos(x_tmp(3)-x_tmp(1))      m3*l2*cos(x_tmp(3)-x_tmp(2)) m3*l3       ];
     B=[(m1+m2+m3)*g*sin(x_tmp(1)) (m2+m3)*l2*(x_tmp(5))^2*sin(x_tmp(1)-x_tmp(2)) m3*l3*(x_tmp(6))^2*sin(x_tmp(1)-x_tmp(3));
        (m2+m3)*l1*(x_tmp(4))^2*sin(x_tmp(2)-x_tmp(1)) (m2+m3)*g*sin(x_tmp(2))    m3*l3*(x_tmp(6))^2*sin(x_tmp(2)-x_tmp(3));
         m3*l1*(x_tmp(4))^2*sin(x_tmp(3)-x_tmp(1))      m3*l2*(x_tmp(5))^2*sin(x_tmp(3)-x_tmp(2)) m3*g*sin(x_tmp(3))];
     C=[1;
        1;
        1];
     D=-(inv(A)*B)*C;
     k_j = [ x_tmp(4);
             x_tmp(5); 
             x_tmp(6); 
             D(1);
             D(2);
             D(3)] * dt;
     if j == 1
            x_tmp = x_k + k_j /2;
     endif
     if j == 2
            x_tmp = x_k + k_j /2;
     endif
     if j == 3
            x_tmp = x_k + k_j ;
     endif
     if j == 4
            x_tmp = x_k + k_j /2;
     endif
     if j == 1
            x_k1 = x_k1 + k_j /6;
     endif
     if j == 2
            x_k1 = x_k1 + k_j /3;
     endif
     if j == 3
            x_k1 = x_k1 + k_j /3;
     endif
     if j == 4
            x_k1 = x_k1 + k_j /6;
     endif
    endfor
    x = [x x_k];
    tim = [tim t];
    x_k = x_k1;
endfor

plot(l1*sin(x(1,:)),-l1*cos(x(1,:)),l1*sin(x(1,:))+l2*sin(x(2,:)),-l1*cos(x(1,:))-l2*cos(x(2,:)),l1*sin(x(1,:))+l2*sin(x(2,:))+l3*sin(x(3,:)),-l1*cos(x(1,:))-l2*cos(x(2,:))-l3*cos(x(3,:)));
out=x;
endfunction

