%% Funzione Assoluto 3D

% INPUT
% x_t,y_t,z_t coordinate centro piano immagine rispetto al sistema di riferimento ambiente
% R matrice di rotazione della telecamera rispetto
% coordinate condinate dei marker sul piano immagine 
% height_piano altezza del piano immagine in pixels
% width_piano larghezza del piano immagine in pixels
% f lunghezza focale telecamera
% OUTPUT
% x_p_as,y_p_as,z_p_as cordinate del piano immagine riportate nel sistema di riferimento assoluto


function [x_p_as, y_p_as, z_p_as] = assoluto3D(x_t,y_t,z_t,R,coordinate,heigth_piano,width_piano,f)

x_tmp = [x_t y_t z_t]';

T_21 = [R x_tmp;zeros(1,3) 1];

% numero di centri della telecamera
[n m] = size(coordinate);

x_p_as = zeros(1,m);
y_p_as = zeros(1,m);
z_p_as = zeros(1,m);

z_p_rif = 0;

for i = 1 : m
x_p_rif = coordinate(1,i);
y_p_rif = coordinate(2,i);
pos = T_21*[(x_p_rif-(width_piano/2)) (y_p_rif-(heigth_piano/2)) z_p_rif+f 1]';
x_p_as(i) = pos(1);
y_p_as(i) = pos(2);
z_p_as(i) = pos(3);
end

