clear all; close all; clc;
% Lens shift and tilt phenomena
% by Maarten Dekker


% inputs
o = 150; % object distance
f = 50;
O = 40;
Oo = 5;

%% 3 Phenomena
sx = 0; % Lens x-shift
sy = 0; % Lens y-shift
Lt = 0; % Lens tilt in [deg]
Ot = 0; % Object tilt in [deg]

%% nominal positions computations
i = 1/(1/f-1/o);
m = -i/o;
I = O*m;
Io = Oo*m;

%% --- initial plot ---
figure
% object
plot([-o -o],[Oo Oo+O],'k','LineWidth',2); hold on; axis equal;
% lens
plot([0 0],[-O +O],'k:');
% focus point
plot(f,0,'ko','MarkerFaceColor','k')
% image
plot([i i],[Io Io+I],'k','LineWidth',2); hold on;
% lens axis
plot([-1.1*o 1.1*i],[0 0],'k:')
% top-ray
plot([-o 0 i -o],[Oo+O Oo+O Io+I Oo+O],'Color',1/255*[127 127 127])
% bottom ray
plot([-o 0 i -o],[Oo Oo Io Oo],'--','Color',1/255*[127 127 127])

%% --- lens x shift ---
os = o+sx;
is = 1/(1/(f)-1/os);
ms = -is/os;
Is = O*ms;
Ios = Oo*ms;

% x shift plot
% lens
plot([0+sx 0+sx],[-O +O],'Color',1/255*[228 108 10]);
% focus point
plot(f+sx,0,'o','Color',1/255*[228 108 10],'MarkerFaceColor',1/255*[228 108 10])
% image
plot([is+sx is+sx],[Ios Ios+Is],'Color',1/255*[0 158 214],'LineWidth',2); hold on;
% top-ray
plot([-o sx is+sx -o],[Oo+O Oo+O Ios+Is Oo+O],'Color',1/255*[228 108 10])
% bottom ray
plot([-o sx is+sx -o],[Oo Oo Ios Oo],'--','Color',1/255*[228 108 10])

%% --- lens y shift ---
Is = O*ms;
Ios = (Oo-sy)*ms;

% y shift plot
% lens
plot([0+sx 0+sx],[-O+sy +O+sy],'Color',1/255*[146 208 80]);
% focus point
plot(f+sx,sy,'o','Color',1/255*[146 208 80],'MarkerFaceColor',1/255*[146 208 80])
% image
plot([is+sx is+sx],[sy+Ios sy+Ios+Is],'Color',1/255*[146 208 80],'LineWidth',2); hold on;
% top-ray
plot([-o sx is+sx -o],[Oo+O Oo+O sy+Ios+Is Oo+O],'Color',1/255*[146 208 80])
% bottom ray
plot([-o sx is+sx -o],[Oo Oo sy+Ios Oo],'--','Color',1/255*[146 208 80])

%% --- Lens tilt ---
Lt = Lt/180*pi;
% focus point
xft = sx+f*cos(Lt);
yft = sy+f*sin(Lt);
% top ray crossing with lense
xt1 = -o+cos(Lt)^2*(os-tan(Lt)*(Oo+O-sy));
yt1 = Oo+O+sin(Lt)*cos(Lt)*(os-tan(Lt)*(Oo+O-sy));
% top ray image crossing
RCt1 = (yft-yt1)/(xft-xt1);
RCt2 = (sy-(Oo+O))/(sx+o);
xt2 = (sy-RCt2*sx-yft+RCt1*xft)/(RCt1-RCt2);
yt2 = RCt1*xt2+yft-RCt1*xft;
% bottom ray crossing with lense
xb1 = -o+cos(Lt)^2*(os-tan(Lt)*(Oo-sy));
yb1 = Oo+sin(Lt)*cos(Lt)*(os-tan(Lt)*(Oo-sy));
% bottom ray image crossing
RCb1 = (yft-yb1)/(xft-xb1);
RCb2 = (sy-(Oo))/(sx+o);
xb2 = (sy-RCb2*sx-yft+RCb1*xft)/(RCb1-RCb2);
yb2 = RCb1*xb2+yft-RCb1*xft;

% Lens tilt plot
% lens
plot(sx+[-sin(Lt)*f +sin(Lt)*f],sy+[+cos(Lt)*f -cos(Lt)*f],'r-.')
% lens axis
plot(sx+[-f*cos(Lt) f*cos(Lt)],sy+[-f*sin(Lt) f*sin(Lt)],'r-.')
% focus point
plot(xft,yft,'ro','MarkerFaceColor','r')
% top ray
plot([-o xt1 xt2 -o],[Oo+O yt1 yt2 Oo+O],'r')
% bottom ray
plot([-o xb1 xb2 -o],[Oo yb1 yb2 Oo],'r--')
% image
plot([xt2 xb2],[yt2 yb2],'r','LineWidth',2)

%% --- Object tilt ---
Ot = Ot/180*pi;
% Object points
Oxt = o+O/2*sin(Ot);
Oxb = o-O/2*sin(Ot);
Oyt = Oo+O/2+O/2*cos(Ot);
Oyb = Oo+O/2-O/2*cos(Ot);

% top ray crossing with lense
xt3 = -Oxt+cos(Lt)^2*(Oxt+sx-tan(Lt)*(Oyt-sy));
yt3 = Oyt+sin(Lt)*cos(Lt)*(Oxt+sx-tan(Lt)*(Oyt-sy));
% top ray image crossing
RCt1a = (yft-yt3)/(xft-xt3);
RCt2a = (sy-Oyt)/(sx+Oxt);
b1 = yft-RCt1a*xft;
b2 = sy-RCt2a*sx;
xt4 = (b2-b1)/(RCt1a-RCt2a);
yt4 = RCt1a*xt4+b1;

% bottom ray crossing with lense
xb3 = -Oxb+cos(Lt)^2*(Oxb+sx-tan(Lt)*(Oyb-sy));
yb3 = Oyb+sin(Lt)*cos(Lt)*(Oxb+sx-tan(Lt)*(Oyb-sy));
% bottom ray image crossing
RCb1a = (yft-yb3)/(xft-xb3);
RCb2a = (sy-Oyb)/(sx+Oxb);
b1 = yft-RCb1a*xft;
b2 = sy-RCb2a*sx;
xb4 = (b2-b1)/(RCb1a-RCb2a);
yb4 = RCb1a*xb4+b1;

% Object tilt plot
% Object
plot([-Oxt -Oxb],[Oyt Oyb],'Color',1/255*[0 158 214],'LineWidth',2)
% top ray
plot([-Oxt xt3 xt4 -Oxt],[Oyt yt3 yt4 Oyt],'Color',1/255*[0 158 214])
% Bottom ray
plot([-Oxb xb3 xb4 -Oxb],[Oyb yb3 yb4 Oyb],'--','Color',1/255*[0 158 214])
% image
plot([xt4 xb4],[yt4 yb4],'Color',1/255*[0 158 214],'LineWidth',2)
% focus point
plot(xft,yft,'o','Color',1/255*[0 158 214],'MarkerFaceColor',1/255*[0 158 214])
% lens
plot(sx+[-sin(Lt)*f +sin(Lt)*f],sy+[+cos(Lt)*f -cos(Lt)*f],'-.','Color',1/255*[0 158 214])
% lens axis
plot(sx+[-f*cos(Lt) f*cos(Lt)],sy+[-f*sin(Lt) f*sin(Lt)],'-.','Color',1/255*[0 158 214])

%% New image w.r.t Told image
Image = sqrt((yb4-yt4)^2+(xb4-xt4)^2)
theta_image = atan((xt4-xb4)/(yb4-yt4))/pi*180
Image_magnification_to_object = -Image/O
Image_magnification_to_old_image = -Image/I
Location_of_new_image_midpoint = [xb4+(xt4-xb4)/2 yt4+(yb4-yt4)/2]
Shift_of_image_midpoint = [Location_of_new_image_midpoint(1)-i Location_of_new_image_midpoint(2)-(Io+(I)/2)]

% image midpoint plot
plot(i,Io+I/2,'kx','MarkerSize',10)
plot(Location_of_new_image_midpoint(1),Location_of_new_image_midpoint(2),'x','Color',1/255*[0 158 214],'MarkerSize',10)