clear all
close all
clc

% %% LIGAR CAMARAS
% % LIGAR CAMARA RGB
% RGBDisp = videoinput('kinect',1,'RGB_640x480');
% 
% % LIGAR CAMARA DEPTH
% DEPTHDisp = videoinput('kinect',2,'Depth_640x480');
% 
% preview(DEPTHDisp)
% pause(20)
% 
% RGBFrameFlipped = flip(getsnapshot(RGBDisp),2);
% DEPTHFrameFlipped = flip(getsnapshot(DEPTHDisp),2);
% 
% %% NUVEM DE PONTOS COMPLETA
% % EXTRAIR A NUVEM DE PONTOS
% RGBFrame1 = getsnapshot(RGBDisp);
% DEPTHFrame1 = getsnapshot(DEPTHDisp);
% 
% ptCloudOriginal = pcfromkinect(DEPTHDisp, DEPTHFrame1, RGBFrame1);

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
ptCloudRot = pctransform(ptCloudOriginal,tform);

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


%% DEFINIR UMA ROI CUBICA PARA ASSUMIR A TARGET AREA
% ESPECIFICAR O STEP SIZE
stepSize = 100;

% AMOSTRA A NUVEM DE PONTOS DE ENTRADA E ARMAZENA AS COORDENADAS DE PONTOS COMO UM OBJETO DE NUVEM DE PONTOS
indices = 1:stepSize:ptCloudRot.Count;
tempPtCloud = select(ptCloudRot,indices);

% REMOVER OS PONTOS INVALIDOS DENTRO DA AMOSTRA ANTERIOR
[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);

% OBTER AS COORDENADAS DE LOCALIZAÇAO 3D DOS PONTOS DA NUVEM DE PONTOS
worldPoints = tempPtCloud.Location;

% CONVERTER E ENCONTRAR AS COORDENADAS DOS PONTOS 2D CORRESPONDENTES AOS PONTOS 3D DA NUVEM
[Y,X] = ind2sub([size(ptCloudRot.Location,1),size(ptCloudRot.Location,2)],indices);
imagePoints = [X(validIndices)' Y(validIndices)'];

% ESTIMATIVA DA MATRIZ DE PROJEÇAO DA CAMARA A PARTIR DA IMAGEM E DAS COORDENADAS DOS PONTOS DE LOCALIZAÇAO 3D
camMatrix = estimateCameraMatrix(imagePoints,worldPoints);

% DEFINIR A ROI CUBICA PARA A TARGET AREA
roi = [-0.35 0.15 0.17 0.41 1.1 1.6];

% ENCONTRAR OS INDICES DOS PONTOS DA NUVEM QUE ESTAO DENTRO DA ROI CUBICA
indices = findPointsInROI(ptCloudRot,roi,camMatrix);

% SELECIONAR OS PONTOS DA NUVEM QUE SE ENCONTRAM DENTRO DA ROI
ptcloudB = select(ptCloudRot,indices);

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

% DEFINIÇÃO DOS PONTOS DOS VERTICES DO CUBO DA ROI
p1 = [roi(1) roi(4) roi(6)]';
p2 = [roi(1) roi(4) roi(5)]';
p3 = [roi(2) roi(4) roi(5)]';
p4 = [roi(2) roi(4) roi(6)]';
p5 = [roi(1) roi(3) roi(6)]';
p6 = [roi(1) roi(3) roi(5)]';
p7 = [roi(2) roi(3) roi(5)]';
p8 = [roi(2) roi(3) roi(6)]';

% DEFINIR AS FACES DO CUBO ATRAVES DOS VERTICES ANTERIORES
A1=[p1 p2 p3 p4;p4 p3 p7 p8;p7 p8 p5 p6;p7 p6 p2 p3;p6 p5 p1 p2;p8 p5 p1 p4];

% CORES DAS FACES
p=fill3(A1(1,:),A1(2,:),A1(3,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(4,:),A1(5,:),A1(6,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(7,:),A1(8,:),A1(9,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(10,:),A1(11,:),A1(12,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(13,:),A1(14,:),A1(15,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(16,:),A1(17,:),A1(18,:),'--w');
p.FaceAlpha = 0.3;

hold off

%% REMOVER O PLANO DO CHAO E MOSTRAR OBJETO
% DEFINIR A DISTANCIA MAXIMA PONTO-PLANO PARA A MONTAGEM DO PLANO
maxDistance = 0.0005;     % (m)

% DEFINIR O VETOR NORMAL AO PLANO A DEFINIR (NESTE CASO VETOR PREPENDICULAR AO PLANO Y)
referenceVector = [0,1,0];

% DETETAR O PLANO E EXTRAI-LO DA NUVEM DE PONTOS
[~,~,outlierIndices] = pcfitplane(ptcloudB,maxDistance,referenceVector);
remainPtCloud = select(ptcloudB,outlierIndices);

% REMOVER O BARULHO DA NUVEM DE PONTOS (ELIMINAR "IMPUREZAS")
pclimpa = pcdenoise(remainPtCloud,"NumNeighbors",100);

% MOSTRAR A NUVEM DE PONTOS LIMPA E SEM O PLANO DO CHAO
figure("Name",'Object Cloud Viewer','NumberTitle','off');
pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

%% CONTORNOS
% RETIRAR AS COORDENADAS DOS PONTOS DO OBJETO
xnuvem = pclimpa.Location(:,1);
ynuvem = pclimpa.Location(:,2);

% FAZER PLOT DOS PONTOS X E Y DO OBJETO E RODAR O PLOT
figure(100);
plot(xnuvem,ynuvem,'k.')
set(gca,'ydir','reverse')
axis 'off'

% RETIRAR FRAME DO PLOT ANTERIOR
% FramePlot = frame2im(getframe(figure(100)));
FramePlot = getframe(figure(100));
imgFramePlot = FramePlot.cdata;

figure;
imshow(imgFramePlot)

% BINARIZAR IMG E INVERTER COR
ImgBin = imbinarize(im2gray(imgFramePlot));
InverseImg = uint8(255) - im2uint8(ImgBin);

% PREENCHER ESPAÇOS ABERTOS NO OBJETO
raio_strel = 8;
se = strel('disk',raio_strel);
ImgClosed = imclose(InverseImg,se);
figure
imshow(ImgClosed)

% FAZER CONTORNOS DO OBJETO
ImgEdge = edge(ImgClosed,"canny");
figure
imshow(ImgEdge)

%% MESH
% RETIRAR AS COORDENADAS DOS PONTOS DO OBJETO
xmesh = double(pclimpa.Location(:,1));
ymesh = double(pclimpa.Location(:,2));
zmesh = double(pclimpa.Location(:,3));

% MULTIPLICAR OS PONTOS POR 1000 PARA OBTER A MESMA ORDEM DE GRANDEZA
matrixmesh = [xmesh ymesh]*1000;
zmesh = zmesh*1000;

% CENTRAR A MESH COM A ORIGEM DO REFERENCIAL (0,0)
xmeshnova = matrixmesh(:,1)+abs(min(matrixmesh(:,1)));
ymeshnova = abs(matrixmesh(:,2)-abs(max(matrixmesh(:,2))));
zmesh = zmesh-min(zmesh);

%% INDICES DOS PONTOS DOS CONTORNOS
[y,x] = find(ImgEdge); 

% CENTRAR OS CONTORNOS COM A ORIGEM DO REFERENCIAL (0,0)
x1 = x-abs(min(x));
y1 = y-abs(max(y));

% MAX MALHA
max_x_malha = max(xmeshnova);
max_y_malha = max(ymeshnova);

% MAX CONTORNOS
max_x_contorno = max(x1);
max_y_contorno = abs(min(y1));

% FATOR DE MULTIPLICAÇAO DAS MATRIZES ANTERIORES
% X
fator_x = max_x_malha/max_x_contorno;

% Y
fator_y = max_y_malha/max_y_contorno;

% MATRIZ DOS CONTORNOS DO TAMANHO DA MESH
x2 = x1*fator_x;
y2 = abs(y1*fator_y);

figure
plot(x2,y2,'k.')

%% CRIAR A MESH A PARTIR DA TRIANGULAÇAO DE DELAUNAY
DT = delaunayTriangulation(xmeshnova,ymeshnova);

%% DELAUNAY MESH'S ORIGINAIS
% 2D DELAUNAY MESH OBJETO
figure;
triplot(DT);
xlabel('x')
ylabel('y')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(ymeshnova)-5 max(ymeshnova)+5])
title('2D Original Delaunay Mesh')


% 3D DELAUNAY MESH OBJETO
% figure(500)
figure
trimesh(DT.ConnectivityList,xmeshnova,zmesh,ymeshnova)
xlabel('x')
ylabel('z')
zlabel('y')
title('3D Original Delaunay Mesh')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(zmesh) max(zmesh)])
zlim([min(ymeshnova)-5 max(ymeshnova)+5])



%% AREAS DOS TRIANGULOS DA MESH 2D ORIGINAL
NVertices = size(DT.ConnectivityList,1);
% calcular as areas dos triangulos da DT
Areas = zeros(NVertices,1);
for i = 1:NVertices
    PointIndexes = DT.ConnectivityList(i,:);
    Areas(i) = polyarea(DT.Points(PointIndexes,1),DT.Points(PointIndexes,2));
end


%% VERTICES EM COORDENADAS DA MESH 2D ORIGINAL
% CALCULAR (x,y) DOS VERTICES DA CONNECTIVITY LIST
% NVertices = size(DT.ConnectivityList,1);
matrix_vertices_coordenadas = zeros([size(DT.ConnectivityList,1) 6]);
for i=1:NVertices
    matrix_vertices_coordenadas(i,:) = [xmeshnova(DT.ConnectivityList(i,1)) ymeshnova(DT.ConnectivityList(i,1)) ...
        xmeshnova(DT.ConnectivityList(i,2)) ymeshnova(DT.ConnectivityList(i,2)) ...
        xmeshnova(DT.ConnectivityList(i,3)) ymeshnova(DT.ConnectivityList(i,3))];
end

% TRES MATRIZES COM OS (x,y) DOS VERTICES DE CADA TRIANGULO
primeiro_vertice_coord = [matrix_vertices_coordenadas(:,1) matrix_vertices_coordenadas(:,2)];
segundo_vertice_coord = [matrix_vertices_coordenadas(:,3) matrix_vertices_coordenadas(:,4)];
terceiro_vertice_coord = [matrix_vertices_coordenadas(:,5) matrix_vertices_coordenadas(:,6)];


%% COMPRIMENTO DAS ARESTAS DA MESH 2D ORIGINAL
% CALCULAR AS DISTANCIAS ENTRE VERTICES
distancias_arestas = zeros([size(DT.ConnectivityList,1) 3]);
for i=1:NVertices
    distancias_arestas(i,:) = [norm(primeiro_vertice_coord(i,:)-segundo_vertice_coord(i,:))...
        norm(primeiro_vertice_coord(i,:)-terceiro_vertice_coord(i,:)) ...
        norm(segundo_vertice_coord(i,:)-terceiro_vertice_coord(i,:))];
end

%% ARESTAS GRANDES E AREAS GRANDES DA MESH 2D ORIGINAL
% ROW E COL (INDEX) DAS ARESTAS GRANDES
[arestas_grandes_row, arestas_grandes_col] = find(distancias_arestas(:,:) > 10);

% triangulos com areas muito grandes
areas_grandes = find(Areas > 10);

% INDICES DOS TRIANGULOS COM AREAS E ARESTAS GRANDES
triangulos_remove = [arestas_grandes_row;areas_grandes];

%% NOVA CONNECTIVITY LIST DA 2D MESH ORIGINAL
NConnectivity = size(DT.ConnectivityList,1);
nova_connectivityList2 = zeros(size(DT.ConnectivityList,1),3);
for i=1:NConnectivity
    if i ~= triangulos_remove   
        nova_connectivityList2(i,:) = [DT.ConnectivityList(i,:)];
    end
end

nova_connectivityList2(~any(nova_connectivityList2,2),:) = [];

%% MESH 2D MODIFICADA
figure
triplot(nova_connectivityList2,DT.Points(:,1),DT.Points(:,2))
xlabel('x')
ylabel('y')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(ymeshnova)-5 max(ymeshnova)+5])
title('2D Constrained Delaunay Triangulation')
hold on
plot(x2,y2,'.k','MarkerSize',4)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%% MESH 3D
figure
mesh3d = trimesh(nova_connectivityList2,xmeshnova,zmesh,ymeshnova);
xlabel('x')
ylabel('y')
zlabel('z')
title('3D Constrained Delaunay Mesh')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(zmesh) max(zmesh)])
zlim([min(ymeshnova)-5 max(ymeshnova)+5])

hold on

roi3d = [100 175 75 175 75 110];


% DEFINIÇÃO DOS PONTOS DOS VERTICES DO CUBO DA ROI
p1_3d = [roi3d(1) roi3d(4) roi3d(6)]';
p2_3d = [roi3d(1) roi3d(4) roi3d(5)]';
p3_3d = [roi3d(2) roi3d(4) roi3d(5)]';
p4_3d = [roi3d(2) roi3d(4) roi3d(6)]';
p5_3d = [roi3d(1) roi3d(3) roi3d(6)]';
p6_3d = [roi3d(1) roi3d(3) roi3d(5)]';
p7_3d = [roi3d(2) roi3d(3) roi3d(5)]';
p8_3d = [roi3d(2) roi3d(3) roi3d(6)]';

% DEFINIR AS FACES DO CUBO ATRAVES DOS VERTICES ANTERIORES
A2=[p1_3d p2_3d p3_3d p4_3d;
    p4_3d p3_3d p7_3d p8_3d;
    p7_3d p8_3d p5_3d p6_3d;
    p7_3d p6_3d p2_3d p3_3d;
    p6_3d p5_3d p1_3d p2_3d;
    p8_3d p5_3d p1_3d p4_3d];

% CORES DAS FACES
p_3d=fill3(A2(1,:),A2(2,:),A2(3,:),'--w');
p_3d.FaceAlpha = 0.3;
p_3d=fill3(A2(4,:),A2(5,:),A2(6,:),'--w');
p_3d.FaceAlpha = 0.3;
p_3d=fill3(A2(7,:),A2(8,:),A2(9,:),'--w');
p_3d.FaceAlpha = 0.3;
p_3d=fill3(A2(10,:),A2(11,:),A2(12,:),'--w');
p_3d.FaceAlpha = 0.3;
p_3d=fill3(A2(13,:),A2(14,:),A2(15,:),'--w');
p_3d.FaceAlpha = 0.3;
p_3d=fill3(A2(16,:),A2(17,:),A2(18,:),'--w');
p_3d.FaceAlpha = 0.3;

hold off

%% TENTAR MODIFICAR A 3D MESH JA ALTERADA DEPOIS DE MODIFICAR A MESH 2D
faces_mesh3D = [xmeshnova(mesh3d.Faces(:,1)),zmesh(mesh3d.Faces(:,1)),ymeshnova(mesh3d.Faces(:,1)),...
                xmeshnova(mesh3d.Faces(:,2)),zmesh(mesh3d.Faces(:,2)),ymeshnova(mesh3d.Faces(:,2)),...
                xmeshnova(mesh3d.Faces(:,3)),zmesh(mesh3d.Faces(:,3)),ymeshnova(mesh3d.Faces(:,3))];

faces = mesh3d.Faces;



[indexmesh3drow,indexmesh3dcol] = find(faces_mesh3D(:,[1,4,7]) > roi3d(1) & faces_mesh3D(:,[1,4,7]) < roi3d(2) & ...
                                       faces_mesh3D(:,[2,5,8]) > roi3d(3) & faces_mesh3D(:,[2,5,8]) < roi3d(4) & ...
                                       faces_mesh3D(:,[3,6,9]) > roi3d(5) & faces_mesh3D(:,[3,6,9]) < roi3d(6));


% vetores
Vetores3d = abs([faces_mesh3D(:,[1:3])-faces_mesh3D(:,[4:6]),faces_mesh3D(:,[4:6])-faces_mesh3D(:,[7:9]),faces_mesh3D(:,[7:9])-faces_mesh3D(:,[1:3])]);

% maxvect = 0.025;
% [vecrow1,veccol1] = find(Vetores3d(:,2) > maxvect);
% [vecrow2,veccol2] = find(Vetores3d(:,5) > maxvect);
% [vecrow3,veccol3] = find(Vetores3d(:,8) > maxvect);

% vetoresremove = unique([vecrow1;vecrow2;vecrow3]);

indexmesh3drow = unique(indexmesh3drow);


% vetores=zeros(size(indexmesh3drow,1),9);
for i=1:size(Vetores3d,1)
    if i~=indexmesh3drow
        Vetores3d(i,:) = 0;
    end
end

maxvect = 10;
[vecrow1,veccol1] = find(Vetores3d(:,2) > maxvect);
[vecrow2,veccol2] = find(Vetores3d(:,5) > maxvect);
[vecrow3,veccol3] = find(Vetores3d(:,8) > maxvect);

vetoresremove = unique([vecrow1;vecrow2;vecrow3]);

connectivityList3D = zeros(size(faces,1),3);
for i=1:size(faces,1)
    if i~= vetoresremove
        connectivityList3D(i,:) = nova_connectivityList2(i,:);
    end
end

connectivityList3D(~any(connectivityList3D,2),:) = [];

figure
h=trimesh(connectivityList3D,xmeshnova,zmesh,ymeshnova);
xlabel('x')
ylabel('y')
zlabel('z')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(zmesh) max(zmesh)])
zlim([min(ymeshnova)-5 max(ymeshnova)+5])


%% NORMAIS DOS PONTOS DA NUVEM DE PONTOS DO OBJETO
% NORMAIS DOS PONTOS DA NUVEM DE PONTOS DO OBJETO
normals = pcnormals(pclimpa);

% RETIRAR COORDENADAS x y E z DOS PONTOS DA NUVEM DO OBJETO
xnuvem = pclimpa.Location(:,1);
ynuvem = pclimpa.Location(:,2);
znuvem = pclimpa.Location(:,3);

% MOSTRAR OS PONTOS DA NUVEM DO OBJETO
figure
pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
u = normals(:,1);
v = normals(:,2);
w = normals(:,3);

% MOSTRAR AS NORMAIS DE CADA PONTO
q = quiver3(xnuvem,ynuvem,znuvem,u,v,w);





% ImgClosed=imbinarize(ImgClosed);
[B,L,N,A] = bwboundaries(ImgClosed); 
figure
plot(xmeshnova,ymeshnova,'.k')
set(gca,'ydir','reverse')
hold on 
for k = 1:N
    % Boundary k is the parent of a hole if the k-th column of the adjacency matrix A contains a non-zero element
    if (nnz(A(:,k)) > 0)
        boundary = B{k};
        redline = plot(boundary(:,2),boundary(:,1),'r','LineWidth',2);
        % Loop through the children of boundary k
        for l = find(A(:,k))'
            boundary = B{l};
            greenline(l,:) = plot(boundary(:,2),boundary(:,1),'g','LineWidth',2);
        end
    end
end


% PONTOS DA NUVEM
xnuvemnova = xnuvem+abs(min(xnuvem));
ynuvemnova = abs(ynuvem-abs(max(ynuvem)));

% CONTORNO EXTERIOR
redlinex = ((redline.XData')-min((redline.XData')));
redliney = abs(((redline.YData')-max((redline.YData'))));

redlinex = redlinex/((max(redlinex))/max(xnuvemnova));
redliney = redliney/((max(redliney))/max(ynuvemnova));

% % CONTORNO INTERIOR
% greenlinex = ((greenline.XData')-min((greenline.XData')))/1092;
% greenliney = ((greenline.YData')-max((greenline.YData')))/1380;
% greenline1 = [greenlinex greenliney];

greenlinex = (greenline(2,:).XData)';
greenliney = (greenline(2,:).YData)';

greenlinex = greenlinex/((max(greenlinex))/max(xnuvemnova));
greenliney = greenliney/((max(greenliney))/max(ynuvemnova));


% MOSTRAR CONTORNOS E PONTOS DA NUVEM
% PONTOS DA NUVEM
figure
plot(xnuvemnova,ynuvemnova,'.k')
hold on

% CONTORNO EXTERIOR
plot(redlinex,redliney,'LineWidth',2,'Color','r')
hold on

%CONTORNO INTERIOR
plot(greenlinex,greenliney,'LineWidth',2,'Color','g')




% INVERTER IMAGEM BINARIA PARA COINCIDIR COORDENADAS
imgflip = flip(ImgClosed);
figure
imshow(imgflip)

% RETIRAR E ALINHAR CONTORNOS
bwflip = bwboundaries(imgflip);
figure
hold on
for i=1:size(bwflip,1)
    if i==1
        bw1flip = bwflip{i};
        bw1flip = [bw1flip(:,2),(bw1flip(:,1))];
        
        plot(bw1flip(:,1)-min(bw1flip(:,1)),(bw1flip(:,2))-min(bw1flip(:,2)))
%         plot(bw1flip(:,1)-min(bw1flip(:,1)),(bw1flip(:,2))-min(bw1flip(:,2)))
    elseif i>1
        bw2flip=bwflip{i};
        bw2flip = [bw2flip(:,2),(bw2flip(:,1))];
%         max(xnuvem) max(bw1flip(:,1))
        plot(bw2flip(:,1)-min(bw1flip(:,1)),bw2flip(:,2)-min(bw1flip(:,2)))
    end
end
plot(xnuvemnova,ynuvemnova,'.k')
hold off








bw = bwboundaries(ImgClosed);
figure
hold on
for i=1:size(bw,1)
    if i==1
        bw1 = bw{i};
        bw1 = [bw1(:,1)*-1,(bw1(:,2))];
        plot(bw1(:,2),bw1(:,1))
    elseif i>1
        bw2=bw{i};
        fato = min(bw1(:,1))+max(bw2(:,1));
        plot(bw2(:,2)+fato,bw2(:,1)*-1)
    end
end













bw2 = bw{1};
figure
plot(bw2(:,2),bw2(:,1))
title('Original Contour')

figure
for theta1=0:0.01:pi
    R = [cos(theta1) -sin(theta1); sin(theta1) cos(theta)];
    vr = bw{1}*R;
    xteste = vr(:,2);
    yteste = vr(:,1);
    plot(xteste,yteste)
    pause(0.01)
    title('Rotated Contour')
end

