function [N,k1,k2,T1,T2] = find_darboux_frame(P,PS)
% An algorithm to estimate the darboux frame (normal, principal directions and principal 
% curvatures) when given a point and its neighbours.
% This algorithm was developed by Eyal Hameiri and Ilan Shimshoni
% For details please refer to 
% IEEE Trans. Sys. Man and Cybernetics B, Vol 33, No 4, 626-637,2003 .

for i=1:size(PS,1)
    ps2(i,1:3) = PS(i,1:3)-P;
end;
[U,S,V] = svd(ps2);
N = V(:,3);
if(N(3) < 0)
    N= -N;
end;
Mv = zeros(3,3);
for i=1:size(PS,1)
    Kp(i) = 2*N'*ps2(i,:)'/norm(ps2(i,:))^2;
    T(i,1:3) = ps2(i,:)-ps2(i,:)*N*N';
    T(i,:) = T(i,:)/norm(T(i,:))';
    Mv = Mv + Kp(i) * T(i,:)'*T(i,:);
end;

[U,S,V] = svd(Mv);
e1 = S(1,1);
e2 = S(2,2);
v1 = V(:,1);
v2 = V(:,2);
A=0;
B=0;
C=0;
for i=1:size(PS,1)
    theta(i) = atan2(T(i,:)*v2,T(i,:)*v1);
    A=A+cos(theta(i))^4;
    B=B+(cos(theta(i))*sin(theta(i)))^2;
   C=C+sin(theta(i))^4;
end;
 m= [A,B
    B,C];
k12=m\[e1,e2]';
if(k12(1) > k12(2))
    k1 = k12(1);
    k2 = k12(2);
    T1 = v1;
    T2 = v2;
else
    k1 = k12(2);
    k2 = k12(1);
    T1 = v2;
    T2 = v1;
end;