RGBdevice = videoinput('kinect',1,'RGB_640x480');
DEPTHdevice = videoinput('kinect',2,'Depth_640x480');

preview(DEPTHdevice)

pause(20)
%% INICIAR AS CAMARAS
RGBdevice();
DEPTHdevice();

%% CAPTURAR UM FRAMDE DOS DISPOSITIVOS
colorImg = getsnapshot(RGBdevice);
depthImg = getsnapshot(DEPTHdevice);

ptCloud = pcfromkinect(DEPTHdevice,depthImg,colorImg);

color=ptCloud.Color; % Cor da nuvem de pontos

r=color(:,:,1);
g=color(:,:,2);
b=color(:,:,3);


r=im2double(reshape(r,[],1));
g=im2double(reshape(g,[],1));
b=im2double(reshape(b,[],1));

cor=[r g b];



points=ptCloud.Location; % Localização dos pontos

x=points(:,:,1); % Distancias Z dos pontos
x=reshape(x,[],1);

y=points(:,:,2); % Distancias Z dos pontos
y=reshape(y,[],1);

z=points(:,:,3); % Distancias Z dos pontos
z=reshape(z,[],1);

figure(1)
s=scatter3(x,y,z,[],cor,'.');
xlabel('X');
ylabel('Y');
zlabel('Z');


figure(2)
rotate3d('on');
direction = [0 0 1];
rotate(s,direction,90);


% 
% 
% x1=[0.5*x(:); 0.75*x(:); x(:)];
% y1=[0.0*y(:); 0.0*y(:); y(:)];
% z1=[0.0*z(:); 0.0*z(:); z(:)];
% scatter3(x1,y1,z1);
% p = plot3(x,y,z);
% direction = [0 0 1];
% rotate(p,direction,90);
% xx =p.XData;
% yy = p.YData;
% zz = p.ZData;




% 
% 
% figure(10);
% scatter3(xx,yy,zz,[],cor,'.')


% h.axes = axes;
% set(h.axes,'CameraUpVector',[0 1 0])        % Set Y axis as vertical
% set(h.axes,'XDir','reverse')   
% scatter3(x1,y1,z1,[],cor1,'.');
% ylabel('Y');
% xlabel('X');
% zlabel('Z');
% figure(2)
% scatter3(x,y,z,[],cor,'.');
% 
% scatter3(x,y,z-0.8,[],cor,'.');
% view([ -0,-77.4400]);
% 
% pause(0.2);