%-------------------------------------------------------------------------%
%         This software is licensed by Creative Commons BY-NC-SA:         %
%      http://creativecommons.org/licenses/by-nc-sa/3.0/it/legalcode      %
%-------------------------------------------------------------------------%
%
% File: kNearestNeighbors(dataMatrix,queryMatrix,k)
%
% Goal: computes the k-nearest neighbors within a set of points
%
% Inputs: dataMatrix:  matrix of points Mx3 
%         queryMatrix: matrix of query points Mx3 (in our case is a point)
%         k:           number of nearest neighbors desired  
%  
% Outputs: neighbors:         k nearest neighbors
%          neighborIds:       indices of k nearest neighbors
%          neighborDistances: distances between neighbors
%
% Comment: distance metric used: Euclidean distance
%
% Remark:  the original routine comes from matlab central file exchange, 
%          provided by Ani, 14/01/14. This program has been 
%          modified in order to return also neighbors coordinates
%
%-------------------------------------------------------------------------%
function [neighbors neighborIds neighborDistances] = ...
    kNearestNeighbors(dataMatrix,queryMatrix,k)
neighborIds = zeros(size(queryMatrix,1),k);
neighborDistances = neighborIds;
numDataVectors = size(dataMatrix,1);
numQueryVectors = size(queryMatrix,1);
for i=1:numQueryVectors,
    dist = sum((repmat(queryMatrix(i,:),numDataVectors,1)...
        -dataMatrix).^2,2);
    [sortval sortpos] = sort(dist,'ascend');
    neighborIds(i,:) = sortpos(1:k);
    neighborDistances(i,:) = sqrt(sortval(1:k));
end
for i=1:k
      neighbors(i,:) = dataMatrix(neighborIds(1,i),:); 
end