%-------------------------------------------------------------------------%
%         This software is licensed by Creative Commons BY-NC-SA:         %
%      http://creativecommons.org/licenses/by-nc-sa/3.0/it/legalcode      %
%-------------------------------------------------------------------------%
%
% File: PointCloud3D_PUCS(A,ep,npu,k)
%
% Goal: script that fits a surface to 3D point cloud using partition of 
%       unity approximation with sparse matrices
%
% Inputs: A:    Nx3 matrix representing a set of N points in R^3
%         ep:   determines size of support of basis function. Small ep
%               yields wide function, i.e., supportsize = 1/ep
%         npu:  parameter for npu-by-npu-by-npu grid of PU cells
%         k:    number of nearest points used for estimate the normals 
%
% Outputs:  the plot of the surface
%
% Calls on: unoriented_KNN.m, DistanceMatrixCSRBF.m, 
%           pcloud_normal_estimate.m 
%
% Remark:  the original routine comes from the book: 
%          [G. E. Fasshauer, Meshfree approximation methods with Matlab, 
%          World Scientific, Singapore, 2007]. 
%          The program has been modified for the computation of 
%          the attraction basins. Moreover it differs from the original 
%          one for the kd-tree package. This program uses the so-called 
%          Kdtree implementation in Matlab, written by P. Vemulapalli,
%          since it executable in the recent versions of Matlab 
%
%-------------------------------------------------------------------------%
function PointCloud3D_PUCS(A,ep,npu,k)
% Weight function for global Shepard partition of unity weighting
wf = @(e,r) r.^4.*(5*spones(r)-4*r);
% The RBF basis function for local RBF interpolation
rbf = @(e,r) r.^4.*(5*spones(r)-4*r);
% Parameter for neval-by-neval-by-neval grid evaluation points
neval = 10;
% Declare data and compute bounding box
N = size(A,1); bmin = min(A,[],1);  bmax = max(A,[],1);
bdim = max(bmax-bmin); wep = npu/(2*bdim);
% Estimate the normals
normals = unoriented_KNN(A,k);
% Oriente the normals using Riemann graph
normals = pcloud_normal_estimate(A,normals,k);
% Find points with nonzero normal vectors and count them
withnormals = find(normals(:,1)|normals(:,2)|normals(:,3));
addpoints = length(withnormals); normals(withnormals,:);
% Distance along normals at which we place new points
delta = bdim/100;
% Add auxiliary points along normals "inside" and "outside"
A(N+1:N+addpoints,:) = A(withnormals,:) + delta*normals(withnormals,:);
A(N+addpoints+1:N+2*addpoints,:) = A(withnormals,:) - ...
    delta*normals(withnormals,:);
% Interpolant is implicit surface, i.e., "original" points have rhs=0,
% "inside" rhs=-1, "outside" rhs=1
rhs = [zeros(N,1); +ones(addpoints,1); -ones(addpoints,1)];
% Compute new bounding box
bmin = min(A,[],1);  bmax = max(A,[],1); ctrs = A;
% Create neval-by-neval-by-neval equally spaced evaluation locations in
% bounding box
xgrid = linspace(bmin(1),bmax(1),neval);
ygrid = linspace(bmin(2),bmax(2),neval);
zgrid = linspace(bmin(3),bmax(3),neval);
[xe,ye,ze] = meshgrid(xgrid,ygrid,zgrid); epoints = [xe(:) ye(:) ze(:)];
% Create npu-by-npu-by-npu equally spaced centers of PU cells in
% the bounding box
puxgrid = linspace(bmin(1),bmax(1),npu);
puygrid = linspace(bmin(2),bmax(2),npu);
puzgrid = linspace(bmin(3),bmax(3),npu);
[xpu,ypu,zpu] = meshgrid(puxgrid,puygrid,puzgrid);
cellctrs = [xpu(:) ypu(:) zpu(:)]; cellradius = 1/wep;
% Compute Shepard evaluation matrix
DM_eval = DistanceMatrixCSRBF(epoints,cellctrs,wep);
SEM = wf(wep,DM_eval);
SEM = spdiags(1./(SEM*ones(npu^3,1)),0,neval^3,neval^3)*SEM;
% Build k-D trees for data sites and evaluation points
datatree = kd_buildtree(A,[]);
evaltree = kd_buildtree(epoints,[]);
Pf = zeros(neval^3,1); % Initialize
for j = 1:npu^3
    % Find data sites in cell j
    [idx,dist,pts] = kd_rangequery_ball(datatree,cellctrs(j,:), ...
        cellradius);
    if (~isempty(idx))
        % Build local interpolation matrix for cell j
        DM_data = DistanceMatrixCSRBF(A(idx,:),ctrs(idx,:),ep);
        IM = rbf(ep,DM_data);
        % Find evaluation points in cell j
        [eidx,edist,epts] = kd_rangequery_ball(evaltree, ...
            cellctrs(j,:),cellradius);
        % Compute local evaluation matrix
        DM_eval = DistanceMatrixCSRBF(epoints(eidx,:),ctrs(idx,:),ep);
        EM = rbf(ep,DM_eval);
        % Compute local RBF interpolant
        localfit = EM * (IM\rhs(idx));
        % Accumulate global fit
        Pf(eidx) = Pf(eidx) + localfit.*SEM(eidx,j);
    end
end
% Plot the interpolant (zero contour of 3D-fit Pf)
hold on
pfit = patch(isosurface(xe,ye,ze,reshape(Pf,neval,neval,neval),0));
isonormals(xe,ye,ze,reshape(Pf,neval,neval,neval),pfit)
set(pfit,'FaceLighting','gouraud','FaceColor','r','EdgeColor','none');
light('Position',[0 0 1],'Style','infinite');
daspect([1 1 1]); view([0,90]);
end