function pointCloudIndex = find_closest(pointCloud, ax)
% Find the point closest to the point clicked in the axes.
%
% The transformation between the viewing frame and the point cloud frame
% is calculated using the camera viewing direction and the 'up' vector.
% Then, the point cloud is transformed into the viewing frame. Finally,
% the z coordinate in this frame is ignored and the x and y coordinates
% of all the points are compared with the mouse click location and the
% closest point is selected.

% Copyright 2012 Levente Hunyadi

if nargin < 2
    ax = gca;
end
point = get(ax, 'CurrentPoint');  % mouse click position
switch size(pointCloud,1)  % dimensionality
    case 2
        distsq = sum(bsxfun(@minus, pointCloud, [point(1,1) ; point(1,2)]).^2, 1);
        [~,pointCloudIndex] = min(distsq);  % closest point
    case 3
        camPos = get(ax, 'CameraPosition');  % camera position
        camTgt = get(ax, 'CameraTarget');    % where the camera is pointing to

        camDir = camPos - camTgt;  % camera direction
        camUpVect = get(ax, 'CameraUpVector');  % camera 'up' vector

        % build an orthonormal frame based on the viewing direction and the up vector (the "view frame")
        zAxis = camDir/norm(camDir);
        upAxis = camUpVect/norm(camUpVect);
        xAxis = cross(upAxis, zAxis);
        yAxis = cross(zAxis, xAxis);

        rot = [xAxis; yAxis; zAxis];  % view rotation

        % the point cloud represented in the view frame
        rotatedPointCloud = rot * pointCloud;

        % the clicked point represented in the view frame
        rotatedPointFront = rot * point' ;

        % find the nearest neighbour to the clicked point
        pointCloudIndex = dsearchn(rotatedPointCloud(1:2,:)', rotatedPointFront(1:2));
end
