function [st]=tps_func(X,Y,lamda)

% tps_func
%
%	Description:
%   Calculate Thin Plate Spline for arbitray dimension >= 2
%   modified from 2-D TPS (Serge Belongie (sjb@eecs.berkeley.edu) Sept. 4, 2000)
%   touched up by Gary K.L. Tam (2008)
% Input:
%   X - input points (N x dim matrix)
%   Y - output corresponding points (N x dim matrix)
%   lamda : 0 - thin plate spline
%         : (0,1] - regularized thin plate spline
% Output:
%   st - struct (w,a,X)
%


assert(size(X,1) == size(Y,1));
assert(size(X,2) == size(Y,2));

%   dim - nbr of dimension
dim = size(X,2);
%   N - nbr of points
N = size(X,1);
assert(dim >= 2);
%assert(N > dim);

r2=dist2(X,X);


%---- increase stability ----
ind=find(r2<1e-4);
%ind=find(r2==0);
r2(ind)=1e-4;
i=1:N; j=1:N;
ind=sub2ind(size(r2),i,j);
r2(ind)=0;
%---- increase stability ----


K=r2.*log(r2+eye(N,N)); % add identity matrix to make K zero on the diagonal


P=[ones(N,1) X];
L=[K  P
   P' zeros(size(P,2),size(P,2))];
V=[Y' zeros(size(Y,2),size(P,2))];
if nargin>2
   % regularization
   L(1:N,1:N)=L(1:N,1:N)+lamda*eye(N,N);
end

%tic, c = L \ V'; toc;   %or this?
%tic;
c = pinv(L)*V';
c = real(c);
%toc;

a = c(size(r2,1)+1:end,:);
w = c(1:size(r2,1),:);
st = struct('w',w,'a',a,'X',X);
return;
