clear all
close all
clc

%% CARREGAR VARIAVEIS SEM TER A CAMARA
load("Workspace_App.mat")

%% RODAR A NUVEM DE PONTOS PARA FICAR PARALELA COM EIXO DOS XX
%ANGULO DE ROTAÇAO
theta = -0.04;

% MATRIZ DE ROTAÇAO EM X
Rx = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)];
trans = [0, 0, 0];
tform = rigid3d(Rx,trans);

% NOVA NUVEM DE PONTOS RODADA
ptCloud = pctransform(ptCloudOriginal,tform);

% MOSTAR NUVEM DE PONTOS
PointCloudBrowser = figure("Name",'Point Cloud Browser','NumberTitle','off');
pcshow(ptCloud,"VerticalAxis","Y",VerticalAxisDir="Down");
title('Full Point Cloud')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% LIMITES TARGET AREA
Xmin = 1;
Xmax = 2;
Ymin = 1;
Ymax = 2;
Zmin = 1;
Zmax = 2;
% DEFINE CUBIC ROI AS TARGET AREA
roi = [Xmin Xmax Ymin Ymax Zmin Zmax];

% FIND POINTS OF THE ORIGINAL CLOUD WITHIN ROI PREVIOUSLY DEFINED 
indices = findPointsInROI(ptCloud,roi);

% SAVE THE POINTS INTO A NEW POINT CLOUD
ptTargetArea = select(ptCloud,indices);

% MOSTRAR A NUVEM DE PONTOS COM A TARGER AREA
figure
pcshow(ptCloud,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
pcshow(ptTargetArea.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")
title('Full Point Cloud with Target Area')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

roi = [Xmin Xmax Ymin Ymax Zmin Zmax];

% CREATE THE COORDINATES CUBE'S VERTICES
p1 = [Xmin Ymax Zmax]';
p2 = [Xmin Ymax Zmin]';
p3 = [Xmax Ymax Zmin]';
p4 = [Xmax Ymax Zmax]';
p5 = [Xmin Ymin Zmax]';
p6 = [Xmin Ymin Zmin]';
p7 = [Xmax Ymin Zmin]';
p8 = [Xmax Ymin Zmax]';

% CREATE THE FACES OF THE CUBE
Cube=[p1 p2 p3 p4; p4 p3 p7 p8; p7 p8 p5 p6; p7 p6 p2 p3; p6 p5 p1 p2; p8 p5 p1 p4];

% DISPLAY THE TARGET AREA WITH COLOR AND TRANSPARENCY SET
p=fill3(Cube(1,:),Cube(2,:),Cube(3,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(Cube(4,:),Cube(5,:),Cube(6,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(Cube(7,:),Cube(8,:),Cube(9,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(Cube(10,:),Cube(11,:),Cube(12,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(Cube(13,:),Cube(14,:),Cube(15,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(Cube(16,:),Cube(17,:),Cube(18,:),'--w');
p.FaceAlpha = 0.3;



