% funzione che riceve in ingresso una serie di punti e restituisce in
% uscita una serie di punti nei quali i più vicini sono stati mediati

function [punti_finali] = media_punti_3D(epsilon, Punti_tot)
% INGRESSI:
% epsilon: distanza entro la quale due punti nello spazio vengono mediati.
% Punti_tot: matrice dei punti nello spazio in ingresso

% USCITE:
% punti_finali: matrice dei punti utili


cont = 0; %contatore del numero di centri

[tilde l]=size(Punti_tot);

for i = 1:l
    
    if Punti_tot(1,i) ~= 0
        cont = cont + 1;
        num_x = Punti_tot(1,i);
        num_y = Punti_tot(2,i);
        num_z = Punti_tot(3,i);
        den = 1;
        if i ~= l
        for j = i+1:l
            dist = sqrt((Punti_tot(1,i)-Punti_tot(1,j))^2 + (Punti_tot(2,i)-Punti_tot(2,j))^2 + (Punti_tot(3,i)-Punti_tot(3,j))^2);
            if dist < epsilon
                num_x = num_x + Punti_tot(1,j);
                num_y = num_y + Punti_tot(2,j);
                num_z = num_z + Punti_tot(3,j);
                den = den + 1;
                Punti_tot(1,j) = 0;
                
            end
        end
        end
        Punti_tot(1,i) = num_x/den;
        Punti_tot(2,i) = num_y/den;
        Punti_tot(3,i) = num_z/den;
    end
end

punti_finali = zeros(3,cont);

c = 1;
for i = 1:l
    if Punti_tot(1,i) ~= 0
        punti_finali(1,c) = Punti_tot(1,i);
        punti_finali(2,c) = Punti_tot(2,i);
        punti_finali(3,c) = Punti_tot(3,i);
        c = c+1;
    end
end