close all
figure(1)
view(3)
grid on
axis equal 
rotate3d on;
space=[5 5 5]
xlim([-2 space(1)+2]);
ylim([-2 space(1)+2]);
zlim([-2 space(1)+2]);

latovoxel=.5;
d_min=2;
d_max=5;
cams=auto_position1(20,space,1,4);
mark=marker_generator(space,20);
DrawCuboid( space',space'/2,...
            [0,0,0], 'b', 0.05);
[cams voxels]=space_partition2(cams,space,pi/3,latovoxel,d_min,d_max);

figure(2)
view(3)
grid on
axis equal 
rotate3d on;
space=[5 5 5]
xlim([-2 space(1)+2]);
ylim([-2 space(1)+2]);
zlim([-2 space(1)+2]);
DrawCuboid( space',space'/2,...
            [0,0,0], 'b', 0.05);
        hold on
temp=0;
        while temp<=space(1)
            h=patch([temp temp temp temp],[0 0 space(2) space(2)],[0 space(3) space(3) 0],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
        temp=0;
        while temp<=space(3)
            h=patch([0 space(1) space(1) 0],[0 0 space(2) space(2)],[temp temp temp temp],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
         temp=0;
        while temp<=space(2)
            h=patch([0 space(1) space(1) 0],[temp temp temp temp],[0 0 space(3) space(3)],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
figure(3)
view(3)
grid on
axis equal 
rotate3d on;
space=[5 5 5]
xlim([-2 space(1)+2]);
ylim([-2 space(1)+2]);
zlim([-2 space(1)+2]);

temp=0;
        while temp<=space(1)
            h=patch([temp temp temp temp],[0 0 space(2) space(2)],[0 space(3) space(3) 0],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
        temp=0;
        while temp<=space(3)
            h=patch([0 space(1) space(1) 0],[0 0 space(2) space(2)],[temp temp temp temp],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
         temp=0;
        while temp<=space(2)
            h=patch([0 space(1) space(1) 0],[temp temp temp temp],[0 0 space(3) space(3)],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
m=mark(:,1);

hold on
%plot3(m(1),m(2),m(3),'r.');
DrawCuboid( [0.2 0.2 .2]',m,[0,0,0], 'r', 0.8);
figure(4)
view(3)
grid on
axis equal 
rotate3d on;
space=[5 5 5]
xlim([-2 space(1)+2]);
ylim([-2 space(1)+2]);
zlim([-2 space(1)+2]);
% DrawCuboid( space',space'/2,...
%             [0,0,0], 'c', 0.05);
temp=0;
        while temp<=space(1)
            h=patch([temp temp temp temp],[0 0 space(2) space(2)],[0 space(3) space(3) 0],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
        temp=0;
        while temp<=space(3)
            h=patch([0 space(1) space(1) 0],[0 0 space(2) space(2)],[temp temp temp temp],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
         temp=0;
        while temp<=space(2)
            h=patch([0 space(1) space(1) 0],[temp temp temp temp],[0 0 space(3) space(3)],'b');
            set(h,'facecolor','w','FaceAlpha',0.05,'EdgeColor', 'b');
            temp=temp+latovoxel;
        end
        
latox=space(1);
latoy=space(2);
latoz=space(3);

vox_x= latox/latovoxel;
vox_y= latoy/latovoxel;
vox_z= latoz/latovoxel;

pos     =  floor(mark(:,1)/latovoxel)+1;
   %allora l'etichetta del voxel a cui corrispondono tali coordinate è
etichetta= pos(1)+(pos(2)-1)*vox_x+(pos(3)-1)*vox_x*vox_y;
% for i=1:size(voxels(etichetta).setcameras)
%     plotcam3(cams,space,voxels,latovoxel,[1 1 0 0 0 0 0])
% end
DrawCuboid( [latovoxel latovoxel latovoxel]',voxels(etichetta).position,...
            [0,0,0], 'g', 0.5);
        
figure(5)
view(3)
grid on
axis equal 
rotate3d on;
space=[5 5 5]
xlim([-2 space(1)+2]);
ylim([-2 space(1)+2]);
zlim([-2 space(1)+2]);
DrawCuboid( space',space'/2,...
            [0,0,0], 'w', 0.05);

DrawCuboid( [latovoxel latovoxel latovoxel]',voxels(etichetta).position,...
            [0,0,0], 'g', 0.5);

for i=1:size(voxels(etichetta).setcameras)
    plotcam4(cams,space,voxels,latovoxel,[1 1 0 0 0 0 0],['w'])
end

for i=1:size(voxels(etichetta).setcameras)
    plotcam4(cams,space,voxels,latovoxel,[1 1 0 0 0 0 0],['w'])
end
a=randint(1,size(cams,2),[1 size(cams,2)])
for i=1:size(voxels(etichetta).setcameras)
    plotcam4(cams(a),space,voxels,latovoxel,[1 1 0 0 0 0 0],['b'])
end

figure(6)
view(3)
grid on
axis equal 
rotate3d on;
space=[5 5 5]
xlim([-2 space(1)+2]);
ylim([-2 space(1)+2]);
zlim([-2 space(1)+2]);
DrawCuboid( space',space'/2,...
            [0,0,0], 'w', 0.05);

DrawCuboid( [latovoxel latovoxel latovoxel]',voxels(etichetta).position,...
            [0,0,0], 'g', 0.5);

% for i=1:size(voxels(etichetta).setcameras)
%     plotcam4(cams,space,voxels,latovoxel,[1 0 0 0 0 0 0],['w'])
% end

a=randint(1,size(cams,2),[1 size(cams,2)])
for i=1:size(voxels(etichetta).setcameras)
    plotcam4(cams(a),space,voxels,latovoxel,[1 1 0 0 0 0 0],['b'])
end

figure(7)
view(3)
grid on
axis equal 
rotate3d on;
space=[5 5 5]
xlim([-2 space(1)+2]);
ylim([-2 space(1)+2]);
zlim([-2 space(1)+2]);
DrawCuboid( space',space'/2,...
            [0,0,0], 'w', 0.05);

DrawCuboid( [latovoxel latovoxel latovoxel]',voxels(etichetta).position,...
            [0,0,0], 'g', 0.5);

% for i=1:size(voxels(etichetta).setcameras)
%     plotcam4(cams,space,voxels,latovoxel,[1 0 0 0 0 0 0],['w'])
% end
DrawCuboid( space',space'/2,...
            [0,0,0], 'b', 0.05);
%a=randint(1,size(cams,2),[1 size(cams,2)])
for i=1:size(voxels(etichetta).setcameras)
    plotcam4(cams(a),space,voxels,latovoxel,[1 1 0 0 0 0 0],['b'])
end

