% Kinematicka analyza priestoroveho otvoreneho mechanizmh maticovou metodou 
% s animaciou pohybu mechanizmu
clear all
clc

% Rozmery mechanizmu
h = 0.5;
a = 0.3;
b = 0.5;
r = 0.3;

% Casovy rozsah
dt = 0.02;
tend = 1;
time = 0:dt:tend;

% Nezavisle suradnice
phi = (90*pi/180)*sin(pi*time);
dphi = pi*(90*pi/180)*cos(pi*time);
ddphi = -pi*pi*(90*pi/180)*sin(pi*time);

x = 0.1 + 0.4*sin(pi*time);
dx = pi*0.4*cos(pi*time);
ddx = -pi*pi*0.4*sin(pi*time);

theta = (90*pi/180)*sin(pi*time);
dtheta = pi*(90*pi/180)*cos(pi*time);
ddtheta = -pi*pi*(90*pi/180)*sin(pi*time);

% Clen 2 - Body A,B,C,D,E,F
r_2A = [0 0 0 1]';
r_2B = [0 0 h 1]';
r_2C = [b a h 1]';
r_2D = [0 a h 1]';
r_2E = [0 -a h 1]';
r_2F = [b -a h 1]';

% Clen 3 - Body G,H 
r_3G = [0 -a 0 1]';
r_3H = [0 a 0 1]';

% Clen 4 - Body I,L
r_4I = [0 0 0 1]';
r_4L = [r 0 0 1]';

% Vypocet polohy, rychlosti a zrychleni bodu vsetkych bodov mechanizmu 
% tranformacnymi maticami zakladnych pohybov

i = 1;
for t = time
    
    % Clen 2
    [Tphi, Dz6] = FCN_basic_mot(phi(i), 6);
    T12 = Tphi;
    r_1A(:,i) = T12*r_2A;
    r_1B(:,i) = T12*r_2B;
    r_1C(:,i) = T12*r_2C;
    r_1D(:,i) = T12*r_2D;
    r_1E(:,i) = T12*r_2E;
    r_1F(:,i) = T12*r_2F;

    % Clen 3
    [Th, Dz3] = FCN_basic_mot(h, 3);
    [Tx, Dz1] = FCN_basic_mot(x(i), 1);
    T23 = Th*Tx;
    T13 = T12*T23;
    r_1G(:,i) = T13*r_3G;
    r_1H(:,i) = T13*r_3H;

    % Clen 4
    [Ttheta, Dz5] = FCN_basic_mot(theta(i), 5);
    T34 = Ttheta;
    T14 = T13*T34;
    r_1I(:,i) = T14*r_4I;
    r_1L(:,i) = T14*r_4L;

    % Poloha bodu L
    r_1L;
    
    % Rychlost bodu L
    T1 = Tphi;      D1 = Dz6;
    T2 = Th;        D2 = Dz3;
    T3 = Tx;        D3 = Dz1;
    T4 = Ttheta;    D4 = Dz5;
    W1 = T1*D1*T2*T3*T4;
    W2 = T1*T2*D2*T3*T3;
    W3 = T1*T2*T3*D3*T4;
    W4 = T1*T2*T3*T4*D4;
    
    v_1L(:,i) = (W1*dphi(i) + W2*0 + W3*dx(i) + W4*dtheta(i))*r_4L;
    
    % Zrychlenie bodu L
    W11 = T1*D1*D1*T2*T3*T4;
    W12 = T1*D1*T2*D2*T3*T4;
    W13 = T1*D1*T2*T3*D3*T4;
    W14 = T1*D1*T2*T3*T4*D4;
    W21 = T1*D1*T2*D2*T3*T4;
    W22 = T1*T2*D2*D2*T3*T4;
    W23 = T1*T2*D2*T3*D3*T4;
    W24 = T1*T2*D2*T3*T4*D4;
    W31 = T1*D1*T2*T3*D3*T4;
    W32 = T1*T2*D2*T3*D3*T4;
    W33 = T1*T2*T3*D3*D3*T4;
    W34 = T1*T2*T3*D3*T4*D4;
    W41 = T1*D1*T2*T3*T4*D4;
    W42 = T1*T2*D2*T3*T4*D4;
    W43 = T1*T2*T3*D3*T4*D4;
    W44 = T1*T2*T3*T4*D4*D4;
    
    a_1L(:,i) = (W1*ddphi(i)   + W11*dphi(i)*dphi(i) + W12*dphi(i)*0   + W13*dphi(i)*dx(i)   + W14*dphi(i)*dtheta(i) + ...
                 0 + ...
                 W3*ddx(i)     + W31*dx(i)*dphi(i)   + W32*dx(i)*0     + W33*dx(i)*dx(i)     + W34*dx(i)*dtheta(i) + ...
                 W4*ddtheta(i) + W41*dtheta(i)*dx(i) + W42*dtheta(i)*0 + W43*dtheta(i)*dx(i) + W44*dtheta(i)*dtheta(i))*r_4L;
        
    i = i+1;   
end

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

% Rychlost
figure(2),clf
    subplot(3,1,1)
plot(time,v_1L(1,:),'r','Linewidth',2)
xlabel('t [s]'), ylabel('vx_{1L} [m/s]'), title('Rychlost bodu L v smere x')
grid on
    subplot(3,1,2)
plot(time,v_1L(2,:),'b','Linewidth',2)
xlabel('t [s]'), ylabel('vy_{1L} [m/s]'), title('Rychlost bodu L v smere y')
grid on
    subplot(3,1,3)
plot(time,v_1L(3,:),'g','Linewidth',2)
xlabel('t [s]'), ylabel('vz_{1L} [m/s]'), title('Rychlost bodu L v smere z')
grid on

% Zrychlenie
figure(3),clf
    subplot(3,1,1)
plot(time,a_1L(1,:),'r','Linewidth',2)
xlabel('t [s]'), ylabel('ax_{1L} [m/s^2]'), title('Zrychlenie bodu L v smere x')
grid on
    subplot(3,1,2)
plot(time,a_1L(2,:),'b','Linewidth',2)
xlabel('t [s]'), ylabel('ay_{1L} [m/s^2]'), title('Zrychlenie bodu L v smere y')
grid on
    subplot(3,1,3)
plot(time,a_1L(3,:),'g','Linewidth',2)
xlabel('t [s]'), ylabel('az_{1L} [m/s^2]'), title('Zrychlenie bodu L v smere z')
grid on

% Polohy bodov v SS ramu
Ax = r_1A(1,:); Ay = r_1A(2,:); Az = r_1A(3,:);
Bx = r_1B(1,:); By = r_1B(2,:); Bz = r_1B(3,:);
Cx = r_1C(1,:); Cy = r_1C(2,:); Cz = r_1C(3,:);
Dx = r_1D(1,:); Dy = r_1D(2,:); Dz = r_1D(3,:);
Ex = r_1E(1,:); Ey = r_1E(2,:); Ez = r_1E(3,:);
Fx = r_1F(1,:); Fy = r_1F(2,:); Fz = r_1F(3,:);
Gx = r_1G(1,:); Gy = r_1G(2,:); Gz = r_1G(3,:);
Hx = r_1H(1,:); Hy = r_1H(2,:); Hz = r_1H(3,:);
Ix = r_1I(1,:); Iy = r_1I(2,:); Iz = r_1I(3,:);
Lx = r_1L(1,:); Ly = r_1L(2,:); Lz = r_1L(3,:);

% Animacia
for i = 1:length(time)
    figure(1), clf
    h4 = plot3([Ax(i) Bx(i)],[Ay(i) By(i)],[Az(i) Bz(i)],'r'); hold on
    h5 = plot3([Cx(i) Dx(i) Ex(i) Fx(i)],[Cy(i) Dy(i) Ey(i) Fy(i)],[Cz(i) Dz(i) Ez(i) Fz(i)],'r'); hold on
    h6 = plot3([Gx(i) Hx(i)],[Gy(i) Hy(i)],[Gz(i) Hz(i)],'k'); hold on
    h7 = plot3([Ix(i) Lx(i)],[Iy(i) Ly(i)],[Iz(i) Lz(i)],'b'); hold on    
    set([h4 h5 h6 h7],'LineWidth',6,'Marker','.','MarkerSize',30)    
    plot3(Lx,Ly,Lz); hold on
    axis([0 0.9 0 0.9 0 0.9])
    xlabel('x [m]'), ylabel('y [m]'), zlabel('z [m]')
    title('Animacia prace otvoreneho mechanizmu')
    grid on
    view([1,1,1])
    pause(0.1)
end