% Vseobecny rovinny pohyb
clear all
clc

ax0 = 0;
ay0 = 0;
az0 = 0;

xbL = 0.15;
ybL = 0.1;
zbL = 0;
r_bL = [xbL ybL zbL 1]';

% SS b
r_bA = [0.3 0 0 1]';
r_bB = [0 0.3 0 1]';

vxb = 0.5;
vyb = 0.3;
w = 2*pi;
v_ab = [vxb vyb 0]';

alpha = 0;
a_ab = [0 0 0]';

dt = 0.05;
tend = 2;
time = 0:dt:tend;

i = 1;
for t = time
    phi(i) = w*t;
    
    r_ab(:,i) = [ax0+vxb*t ay0+vyb*t az0]';
    
    PHI_ab = [cos(phi(i)) -sin(phi(i)) cos(pi/2);
              sin(phi(i))  cos(phi(i)) cos(pi/2);
              0            0           cos(0) ];
          
    T_ab = eye(4);
    T_ab(1:3,1:3) = PHI_ab;
    T_ab(1:3,4) = r_ab(:,i);

    Om_ab = [0 -w 0;
             w 0  0;
             0 0  0];
    Aplha_ab = [0 -alpha 0;   
                alpha 0  0;
                0 0  0];     
         
    V_ab = zeros(4);
    V_ab(1:3,1:3) = Om_ab;
    V_ab(1:3,4) = PHI_ab'*v_ab;
    
    Au_ab = zeros(4);
    Au_ab(1:3,1:3) = Aplha_ab + Om_ab*Om_ab;   
    Au_ab(1:3,4) = PHI_ab'*a_ab;    
    
    r_aL(:,i) = T_ab*r_bL;   
    r_aA(:,i) = T_ab*r_bA;
    r_aB(:,i) = T_ab*r_bB;    
    
    v_aL(:,i) = T_ab*V_ab*r_bL;
    
    a_aL(:,i) = T_ab*Au_ab*r_bL;
    
    i = i+1;
end

figure(1),clf
    subplot(2,1,1)
plot(time,r_aL(1,:),'r','Linewidth',2)
xlabel('t [s]'), ylabel('x_{aL} [m]'), title('Poloha bodu L v smere x')
grid on
    subplot(2,1,2)
plot(time,r_aL(2,:),'b','Linewidth',2)
xlabel('t [s]'), ylabel('y_{aL} [m]'), title('Poloha bodu L v smere y')
grid on

figure(2),clf
    subplot(2,1,1)
plot(time,v_aL(1,:),'r','Linewidth',2)
xlabel('t [s]'), ylabel('v_{aL} [m/s]'), title('Rychlost bodu L v smere x')
grid on
    subplot(2,1,2)
plot(time,v_aL(2,:),'b','Linewidth',2)
xlabel('t [s]'), ylabel('v_{aL} [m/s]'), title('Rychlost bodu L v smere y')
grid on

figure(3),clf
    subplot(2,1,1)
plot(time,a_aL(1,:),'r','Linewidth',2)
xlabel('t [s]'), ylabel('a_{aL} [m/s]'), title('Zrychlenie bodu L v smere x')
grid on
    subplot(2,1,2)
plot(time,a_aL(2,:),'b','Linewidth',2)
xlabel('t [s]'), ylabel('a_{aL} [m/s]'), title('Zrychlenie bodu L v smere y')
grid on


for i = 1:length(phi)
    figure(100), clf
    % SS b
    plot([r_ab(1,i) r_aA(1,i)],[r_ab(2,i) r_aA(2,i)],'b','Linewidth',2), hold on
    plot([r_ab(1,i) r_aB(1,i)],[r_ab(2,i) r_aB(2,i)],'b','Linewidth',2), hold on
    % L (SS b)
    plot([r_ab(1,i) r_aL(1,i)],[r_ab(2,i) r_aL(2,i)],'b','Linewidth',1), hold on
    plot(r_aL(1,i), r_aL(2,i),'.b','MarkerSize',20), hold on
    % SS a
    plot([0 0.3],[0,0],'r','Linewidth',2), hold on
    plot([0 0],[0,0.3],'r','Linewidth',2), hold on
    % Trajektoria L
    plot(r_aL(1,:),r_aL(2,:),'b'), hold on
    % Trajektoria Ob
    %plot(r_ab(1,:),r_ab(2,:),'b')
    
    axis([-0.1 1.4 -0.1 1.4])
    xlabel('x [m]'), ylabel('y [m]')
    %grid on
    axis square
    pause(0.1)
    input('')
end
 
