function Fig_EpipolarLine
% Figure to illustrate projecting the point P onto the two retina:
% show two different choices of retinal coordinates

clear all
close all

fs=14; % fontsize to use for axis labels
fsa=18; % fontsize to use for panel labels
fst = 18; % fontsize to use for titles

% Head-centered coordinates of viewed object
objectP = [-6 7 10];

% Coordinates of real eye:
interoculardistance = 6;
eyes.radius = 1.75;
eyes.eyeballcolor = [0.75 0.75 0.5];
eyes.eyeballtransparency = 0.7;
% Specify eye positions
eyes.eyepos(1).nodal = [1; 0 ; 0]*interoculardistance/2 ;
eyes.eyepos(1).azimuth = -15;
eyes.eyepos(1).elevation = 0;
eyes.eyepos(1).torsion = 0;
% Colors in which to draw coordinate systems
eyes.azcoordcol = 'k' ;
eyes.elcoordcol = 'k' ;
eyes.eyepos(2) = eyes.eyepos(1);
eyes.eyepos(2).azimuth = -eyes.eyepos(1).azimuth;
eyes.eyepos(2).nodal = [-1; 0 ; 0]*interoculardistance/2;

% Both retinae superimposed
jointeyes = eyes;
jointeyes = rmfield(jointeyes,'eyepos');
jointeyes.eyepos.nodal = [ 0 ; 0 ; 0];
jointeyes.eyepos.azimuth = 0;
jointeyes.eyepos.elevation = 0;
jointeyes.eyepos.torsion = 0;

figure('pos',[ 479   161   686   529])
arrowheadlength = 0.5;
% Draw interocular axis
Xaxis = [ [-1 1]*6;[0 0];[0 0] ] + [1 1 ; 0 0 ; 0 0];
plotvector(Xaxis,'k-','linew',2)
hold on
% Add on arrowhead
plot3(Xaxis(1,2) + [-1 0 -1]*arrowheadlength , Xaxis(2,2) * [1 1 1], [-1 0 1]*arrowheadlength,'col','k','linew',2)
text(Xaxis(1,2)-1,1,-2,'X','fonts',fs)
% Mark Y axis
Yaxis = [0 0;-1 1;0 0]*4 + [0 0; 1 1 ; 0 0];
plotvector(Yaxis,'k-','linew',2)
% Add on arrowhead
plot3( [-1 0 1]*arrowheadlength , Yaxis(2,2) + [-1 0 -1]*arrowheadlength, Yaxis(3,2) * [1 1 1],'col','k','linew',2)
text(0,Yaxis(2,2)+1,0,'Y','fonts',fs)
% Mark Z axis
Zaxis = [0 0;0 0;-1 20];
plotvector(Zaxis,'k-','linew',2)
% Add on arrowhead
plot3( [-1 0 1]*arrowheadlength , [0 0 0], Zaxis(3,2) + [-1 0 -1]*arrowheadlength,'col','k','linew',2)
text(-0.4,-0.3,Zaxis(3,2),'Z','fonts',fs)

% Mark on horiozntal plane (XZ plane)
patch([-1 1 1 -1 -1]*(eyes.radius+interoculardistance/2),[0 0 0 0 0],[-1 -1 0 0 -1]*eyes.radius + [0 0 1 1 0]*15,[0.8 0.8 0.8],'edgecol','none');

% Draw the eyes
DrawEyes(eyes);

% Plot object in space, P:
% Calculate
imagecoords = ProjectToRetina(eyes,objectP');
plotvector(imagecoords.headcentered{1},'ro','markerfacecol','r')
imL = imagecoords.headcentered{1};
%
camup([0 1 0])
camtarget([-0.9806    3.0488    3.0955])
campos([-6.4609    6.2447   -9.9213])
camva(40)
lighting phong
light('pos',Zaxis(:,2)/2+Yaxis(:,2)*2)
camlight
axis equal off
% Calculate epipolar line
unitvector = eyes.eyepos(1).nodal - imagecoords.headcentered{1}';
unitvector = unitvector/norm(unitvector);
dists = eyes.radius + [0:1:20];
for j=1:length(dists)
    thisP = eyes.eyepos(1).nodal  + dists(j) * unitvector;
    imagecoords = ProjectToRetina(eyes,thisP);
    plotvector([imagecoords.headcentered{2}'  thisP],'b-')
     epiRX(j) = imagecoords.headcentered{2}(1);
    epiRY(j) = imagecoords.headcentered{2}(2);
    epiRZ(j) = imagecoords.headcentered{2}(3);

end
plot3(epiRX,epiRY,epiRZ,'linew',2,'col','b')
plotvector([imL' thisP],'r-')
set(gcf,'color','w')
print -opengl -djpeg100 tmp

function imagecoords = ProjectToRetina(eyes,point)
% Calculate retinal coordinates of a point in space.
% "eyes" is a structure giving the binocular eye position.
% "points" is a 3-row vector, [X;Y;Z]. Each column is assumed to specify a
% different point in space.
% NB this is exact; no approximations of any kind are used.


for thiseye=1:length(eyes.eyepos);
    n = eyes.eyepos(thiseye).nodal;
    Rot = RotationMatrix(eyes.eyepos(thiseye));
    nrm = dot(point-n , Rot*[0;0;1]);
    xonf = - dot(point-n , Rot*[1;0;0]) / nrm;
    yonf = - dot(point-n , Rot*[0;1;0]) / nrm;

    imagecoords.AzLong(thiseye) = atan(xonf);
    imagecoords.AzLat(thiseye) = atan(xonf/sqrt(yonf^2+1));
    imagecoords.ElLong(thiseye) = atan(yonf);
    imagecoords.ElLat(thiseye) = atan(yonf/sqrt(xonf^2+1));
    % Position of the image on the rotated eye, in head-centered
    % coords:
    lam = eyes.radius/norm(point-n);
    imagecoords.headcentered{thiseye} = n'-lam*(point-n)';
end

