%[v,f]=read_off('home1_shapes.off');
[v,f]=read_off('home1_cube.off');
figure(1);
clf;
%subplot(1,3,1);
set(gcf,'Color',[1 1 1]);
options.line_width =2;
options.style =['-'];
options.cstyle=['r'];
plot_mesh_edges(v,f,options);
camtarget([0 -0 -0]);
campos([50 0 -25]);
im_plan_size=0.5;
camva(50);
camproj('perspective');
xx=[25 -9 -15
    0 0 0];
%plot3arr(xx);
hh=plot3([25],[-9],[-15],'rx');
set(hh,'MarkerSize',[12]);
z1=[25 -9 -15];
ccenter=z1;

z1=-z1/norm(z1);
x1=-[0 1 0];
x1=x1-(z1*x1')*z1;
x1=x1/norm(x1);
fprintf(1,'%15.14f ', x1);
fprintf(1,'\n');
y1=cross(z1', x1');
y1=y1/norm(y1);
y1=y1';

f1=0.5;
ppoint=ccenter+f1*z1;
t1=ccenter';
R1=[x1;y1;z1]';
K1=[f1 0 0;0 f1 0; 0 0 1];
P1=K1*[R1; (-R1'*t1)']';
v1=v;
for i=1:size(v,2),
    v1(:,i)=P1*[v(:,i);1];
end
axis_ps=[ppoint; ppoint+x1];
plot3arr(axis_ps,'b--');
axis_ps=[ppoint; ppoint+y1];
plot3arr(axis_ps,'g--');
axis_ps=[ccenter; ppoint+80*f1*z1];
plot3arr(axis_ps,'r--');

pcorners=[x1+y1
    x1-y1
    -x1-y1
    -x1+y1
    x1+y1]*im_plan_size;
for i=1:size(pcorners,1),
    pcorners(i,:)=pcorners(i,:)+ppoint;
end
hh=plot3arr(pcorners,'r-');
set(hh,'LineWidth',[2]);
%arrow fixlimits
plot_mesh_edges(v,f,options);
%camtarget([0 -0 -0]);
%campos([25 10 -15]);
%arrow([25 10 -15],[0 0 0]);
xx=[25 -6 -15
    0 0 0];
%plot3arr(xx);
hh=plot3([25],[-6],[-15],'rx');
set(hh,'MarkerSize',[12]);
z1=[25 -6 -15];
ccenter=z1;
z1=-z1/norm(z1);
x1=-[0 1 0];
x1=x1-(z1*x1')*z1;
x1=x1/norm(x1);
fprintf(1,'%15.14f ', x1);
fprintf(1,'\n');

y1=cross(z1', x1');
y1=y1/norm(y1);
y1=y1';

f2=0.25;
ppoint=ccenter+f2*z1;

t2=ccenter';
R2=[x1;y1;z1]';
K2=[f2 0 0;0 f2 0; 0 0 1]*[1 0 0 0; 0 1 0 0; 0 0 1 0];
K2=[f2 0 0;0 f2 0; 0 0 1];
P2=K2*[R2; (-R2'*t2)']';
v2=v;
for i=1:size(v,2),
    v2(:,i)=P2*[v(:,i);1];
end
axis_ps=[ppoint; ppoint+x1];
plot3arr(axis_ps,'b--');
axis_ps=[ppoint; ppoint+y1];
plot3arr(axis_ps,'g--');
axis_ps=[ccenter; ppoint+160*f2*z1];
plot3arr(axis_ps,'r--');

pcorners=[x1+y1
    x1-y1
    -x1-y1
    -x1+y1
    x1+y1]*im_plan_size;
for i=1:size(pcorners,1),
    pcorners(i,:)=pcorners(i,:)+ppoint;
end
hh=plot3arr(pcorners,'r-');
set(hh,'LineWidth',[2]);
%arrow fixlimits
%xlabel('X'); ylabel('Y'); zlabel('Z');
xlabel('X'); ylabel('Y'); zlabel('Z');
axis('equal');
camproj('perspective');
v2_saved=inv(K2)*v2;
v1_saved=inv(K1)*v1;

figure(2);
clf;
subplot(2,2,1);
for i=1:size(v1,2),
    v1(:,i)=v1(:,i)./v1(3,i); 
end
%v1(1,:)=0-v1(1,:);
plot_mesh_edges(v1(1:2,:),f);
%v1(1,:)=0-v1(1,:);
axis('equal');
xlabel('X_{cam}');
ylabel('Y_{cam}');
title(['Left camera image plane']);

subplot(2,2,2);
for i=1:size(v1,2),
    v2(:,i)=v2(:,i)./v2(3,i);
end
%v2(1,:)=-v2(1,:);
plot_mesh_edges(v2(1:2,:),f);
%v2(1,:)=-v2(1,:);
axis('equal');
Kcp1=[-1024/0.1 0 511.5;0 -1024/.1  511.5; 0 0 1];
Kcp2=[-1024/0.1 0 511.5;0 -1024/.1  511.5; 0 0 1];
v1_images=Kcp1*v1;
v2_images=Kcp2*v2;
title(['Right camera image plane']);
xlabel('X_{cam}');
ylabel('Y_{cam}');
subplot(2,2,3);
%cla;

axis('ij');
hold on;
plot_mesh_edges(v1_images(1:2,:),f);
axis('image');
axis('equal');
title(['Image of the left camera']);
xlabel('Column');
ylabel('Row');

subplot(2,2,4);
cla;

axis('ij');
hold on;
plot_mesh_edges(v2_images(1:2,:),f);
axis('equal');
axis('image');

xlabel('Column');
ylabel('Row');
title(['Image of the right camera']);
