% anim_block_translate.m

clear; close all;

% Block spec
Lx = 0.15;
Ly = 0.05;
Lz = 0.30;

% Motion data
t = (0:0.001:1)';                       % Time data
position = [0.5*sin(2*pi*t), 0*t, 0*t]; % Position data
angles = [0*t, 0*t, 0*t];               % Orientation data (XYZ Euler angles)

% Initial vertices and faces
r_ini = position(1,:)';
R_ini = eulerXYZ(angles(1,1), angles(1,2), angles(1,3));
[vertices_ini, faces_ini] = calcBlock(r_ini, R_ini, Lx, Ly, Lz);

% Draw initial figure
figure(1);
h = patch('Faces', faces_ini, 'Vertices', vertices_ini, 'FaceColor', 'y');

% Axes settings
xlabel('x'); ylabel('y'); zlabel('z');
axis vis3d equal;
view([-37.5,30]);
camlight;
grid on;
xlim([-0.7,0.7]);
ylim([-0.7,0.7]);
zlim([-0.7,0.7]);

% Compute vertices at each time
vertices = zeros(8,3,length(t));
for i = 1:length(t)
    r = position(i,:)';
    R = eulerXYZ(angles(i,1), angles(i,2), angles(i,3));
    [vertices(:,:,i), ~] = calcBlock(r, R, Lx, Ly, Lz);
end

% Animation Loop
for i = 1:length(t)
    set(h, 'Vertices', vertices(:,:,i));
    drawnow;
end