%% Questa funzione considera il riferimento collocato nel centro ottico della telecamera
%% (convezione utilizzata dai libri)

function [x_p_as, y_p_as, z_p_as, x_f_as , y_f_as, z_f_as] = assoluto(x_t, y_t, z_t, alpha, x_p_rif, length_piano, length_focus)


T_21 = [cos(alpha) -sin(alpha) 0 x_t; sin(alpha) cos(alpha) 0 y_t; 0 0 1 z_t; 0 0 0 1];
 
% Nel caso 2D i centri sono solo sul piano x di riferimento della
% telecamera

y_p_rif = length_focus;
z_p_rif = 0;


m = length(x_p_rif);% numero di centri della telecamera
x_p_as = zeros(1,m);
y_p_as = zeros(1,m);
z_p_as = zeros(1,m);

for i = 1 : m
pos = T_21*[(x_p_rif(i)-(length_piano/2)) y_p_rif z_p_rif 1]';
x_p_as(i) = pos(1);
y_p_as(i) = pos(2);
z_p_as(i) = pos(3);
end

x_f_rif = 0;%length_piano/2;
y_f_rif = 0;%-length_focus;
z_f_rif = 0;


pos_focus = T_21*[x_f_rif y_f_rif z_f_rif 1]';
x_f_as = pos_focus(1);
y_f_as = pos_focus(2);
z_f_as = pos_focus(3);