% animation_patch.m

clear; close all;

% Define Geometry Points (Vertices for the top surface of the block)
Lx = 0.15; %[m]
Ly = 0.05; %[m]
Lz = 0.30; %[m]

p1 = [  Lx/2,  Ly/2, Lz/2 ];
p2 = [  Lx/2, -Ly/2, Lz/2 ];
p3 = [ -Lx/2, -Ly/2, Lz/2 ];
p4 = [ -Lx/2,  Ly/2, Lz/2 ];

% Load Data
[LABEL,DATA] = MBDynLoad('free_rotating_block.mov');

r = DATA(:,[2:4],LABEL==1);        % Node Position [m]
A = DATA(:,[5:7],LABEL==1)*pi/180; % Node Orientation [rad] (x-y-z Euler angle)

n = size(r,1); % Data Length

% Euler Angle -> Orientation Matrix
for i=1:n
    a1 = A(i,1);
    a2 = A(i,2);
    a3 = A(i,3);
    
    R1 = [1, 0, 0;
        0, cos(a1), -sin(a1);
        0, sin(a1), cos(a1)];
    
    R2 = [cos(a2), 0, sin(a2);
        0, 1, 0;
        -sin(a2), 0, cos(a2)];
    
    R3 = [cos(a3), -sin(a3), 0;
        sin(a3), cos(a3), 0;
        0, 0, 1];
    
    R(:,:,i) = R1*R2*R3;
end

% Compute Propagation of Geometry Points
for i=1:n 
    r1(i,:) = r(i,:) + p1*R(:,:,1)*R(:,:,i)';
    r2(i,:) = r(i,:) + p2*R(:,:,1)*R(:,:,i)';
    r3(i,:) = r(i,:) + p3*R(:,:,1)*R(:,:,i)';
    r4(i,:) = r(i,:) + p4*R(:,:,1)*R(:,:,i)';
end

% Compile Data for Drawing Patch
PatchData_X = [r1(:,1),r2(:,1),r3(:,1),r4(:,1)];
PatchData_Y = [r1(:,2),r2(:,2),r3(:,2),r4(:,2)];
PatchData_Z = [r1(:,3),r2(:,3),r3(:,3),r4(:,3)];

% Draw Initial Figure
figure(1);
set(gcf,'Renderer','OpenGL');
h = patch(PatchData_X(1,:),PatchData_Y(1,:),PatchData_Z(1,:),'y');
set(h,'EraseMode','normal');
axis vis3d equal;
view([-37.5,30]);
camlight;
grid on;
xlim([-0.2,0.2]);
ylim([-0.2,0.2]);
zlim([-0.2,0.2]);

% Animation Loop
for i=1:n
    set(h,'XData',PatchData_X(i,:)');
    set(h,'YData',PatchData_Y(i,:)');
    set(h,'ZData',PatchData_Z(i,:)');
    drawnow;
end