%[v,f]=read_off('home1_shapes.off');
%[v,f]=read_off('cube.off');

ncameras=6;
cameras=cell([ncameras 1]);
%Here we setup the cameras along a circle
c_radius = 10;
height = 5;
c_start = 0.0;
c_target=[0 0 0]';
im_height_pixels = 1024;
im_width_pixels = 1024;
pl_height= 1;
pl_width = 1;
bk_color=[255 255 255];
for c_cam=1:ncameras,
   c_angle=(c_cam-1+c_start)*2*pi/ncameras;
   cameras{c_cam}.center=[c_radius*cos(c_angle) c_radius*sin(c_angle) height]';
   cameras{c_cam}.target=c_target;
   cameras{c_cam}.fLen=1.50;
   cameras{c_cam}.upvector=[0 0 1]';
   cameras{c_cam}.im_height = im_height_pixels;
   cameras{c_cam}.im_width = im_width_pixels;
   cameras{c_cam}.pl_height = pl_height;
   cameras{c_cam}.pl_width = pl_width;
   cameras{c_cam}.bkcolor=bk_color;
   cameras{c_cam}.ray_len=3.5;
   cameras{c_cam}.ray_style='k-';
   cameras{c_cam}.drawing=0;
   cameras{c_cam}.draw_scale=0.5;
end
clf;
figure(1);
hold on;
axis('off');
axis('equal');
for i=1:ncameras,
   cameras{i}.z=cameras{i}.target-cameras{i}.center;
   cameras{i}.z=cameras{i}.z/norm(cameras{i}.z);
   cameras{i}.y=cameras{i}.upvector-dot(cameras{i}.upvector,cameras{i}.z)*cameras{i}.z;
   cameras{i}.y=cameras{i}.y/norm(cameras{i}.y);
   cameras{i}.x=cross(cameras{i}.y,cameras{i}.z);
   %This is the pose of the camera in the world coordinate
   cameras{i}.Rw=[cameras{i}.x,cameras{i}.y,cameras{i}.z];
   %This is the position of the camera in the world coordinate
   cameras{i}.Tw=cameras{i}.center;
   %Note that given a point Xw in the world coordinate, its coorindate in the camera Xc is given by
   % Xc = cameras{i}.Rw'*Xw-cameras{i}.Rw'*cameras{i}.center
   % The following is the matrix that converts a point in the image plane to the corresponding point
   % in the pixel coordinate
   %
   cameras{i}.world2cam=[cameras{i}.Rw',-cameras{i}.Rw'*cameras{i}.Tw; 0 0 0 1];
   cameras{i}.cam2image=[-cameras{i}.im_width/cameras{i}.pl_width 0 (cameras{i}.im_width+1)/2
      0   -cameras{i}.im_height/cameras{i}.pl_height (cameras{i}.im_height+1)/2
      0                 0                                1];
   cameras{i}.f_K=diag([cameras{i}.f cameras{i}.f 1]);
   cameras{i}.K=cameras{i}.cam2image*cameras{i}.f_K;
   %Given a point in the pixel coordinate xim, we can get the point on the normalized image plane
   %xn = inv(cameras{i}.K) * xim;
   cameras{i}.P=cameras{i}.K*[1 0 0 0;0 1 0 0; 0 0 1 0]*cameras{i}.world2cam;
   %Checking, the target point should be mapped to the center of the image
   target_cam=cameras{i}.P*[cameras{i}.target;1];
   target_cam=target_cam./target_cam(end);
   if norm(target_cam-cameras{i}.cam2image(:,end))>0.00001,
      fprintf(1,'Something is wrong for camera %d. Its target should be mapped to ', i);
      fprintf(1,'%6.2f %6.2f instead of ', cameras{i}.cam2image(1,end),cameras{i}.cam2image(2,end));
      fprintf(1,'%6.2f %6.2f\n',target_cam(1),target_cam(2));
   end
   target_z=cameras{i}.Rw'*[cameras{i}.z];
   if norm(target_z-[0 0 1]')>0.00001,
      fprintf(1,'Something is wrong for camera %d. Its z axis should be mapped to ', i);
      fprintf(1,'[0 0 1] instead of ');
      target_cam=cameras{i}.world2cam*[cameras{i}.z; 1];
      fprintf(1,'[%6.2f %6.2f %6.2f]\n',target_z(1),target_z(2),target_z(3));
   end
   center_cam=cameras{i}.world2cam*[cameras{i}.center;1];
   center_cam=center_cam/center_cam(end);
   center_cam=center_cam(1:3);
   
   if norm(center_cam-[0 0 0]') > 0.00001,
      fprintf(1,'Something is wrong for camera %d. Its center should be mapped to ', i);
      fprintf(1,'[0 0 0] instead of ');
      fprintf(1,'%6.2f %6.2f %6.2f\n',center_cam(1),center_cam(2),center_cam(3));
   end
   image_plane_center=cameras{i}.Tw+cameras{i}.f*cameras{i}.z;
   cameras{i}.v=[cameras{i}.pl_width/2*cameras{i}.x+cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.v=[cameras{i}.v,cameras{1}.pl_width/2*cameras{i}.x-cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.v=[cameras{i}.v, -cameras{1}.pl_width/2*cameras{i}.x-cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.v=[cameras{i}.v, -cameras{1}.pl_width/2*cameras{i}.x+cameras{i}.pl_height/2*cameras{i}.y+image_plane_center];
   cameras{i}.f=[1 2 4;2 3 4];
   if cameras{i}.drawing,
      f_3Dcamera([cameras{i}.Rw,cameras{i}.Tw],'k-',cameras{i}.draw_scale,2,3,1.5);
   else
      
   plot_mesh(cameras{i}.v,cameras{i}.f);
   for ii=1:size(cameras{i}.v,2),
      dir_v=cameras{i}.v(:,ii)-cameras{i}.Tw;
      dir_v=dir_v/norm(dir_v);

      plot3arr([cameras{i}.Tw';(cameras{i}.Tw+cameras{i}.ray_len*dir_v)'],cameras{i}.ray_style,2);
   end
   plot3arr([cameras{i}.Tw';(cameras{i}.Tw+cameras{i}.ray_len*cameras{i}.z)'],cameras{i}.ray_style,4);
   hh=plot3arr(cameras{i}.Tw','ko',4);
   set(hh,'MarkerSize',[6]);
   hh=plot3arr(cameras{i}.Tw','k+',4);
   set(hh,'MarkerSize',[6]);
   end
end


