% A matrix with distances between n 3D points is given (d2). 
% The problem is to find cartesian coordinates of the points.
% For further explanations look at the end of the file

% set path:
% path(path, 'D:\LAZA\DICKST~1');

n = 32;
% These are test points
x0 = 100*randn(n,1);
y0 = 100*randn(n,1);
z0 = 100*randn(n,1);

x=x0; y=y0; z=z0;
deltax = x*ones(1,n) - ones(n,1)*x';
deltay = y*ones(1,n) - ones(n,1)*y';
deltaz = z*ones(1,n) - ones(n,1)*z';
d2 = deltax.^2 + deltay.^2 + deltaz.^2;
epsilon = 0.1*ErrorF(round(x0),round(y0),round(z0),d2);
disp(sprintf('epsilon=%f',epsilon));

%Setting initial points randomly
noise=10;
x = x0 + noise*rand(n,1);
y = y0 + noise*rand(n,1);
z = z0 + noise*rand(n,1);

% Newton method - works when close to solution
% Algorithm:
% 1. Calculate g, G where g=d(e^2)/dx, G=d^2(e^2)/(dxi dxj),  e is error
% 2. While G+ni*I is not positive definite, ni=ni*4
% 3. Solve (G + ni*I) * d = -g    for d
% 4. r = (f(x+d)-f(x))/(q(x+d)-q(x)), where f is squared error, q is quadratic approximation of f
% 5. if r<0.25, ni=4*ni; elseif r>0.75, ni=0.5*ni;
% 6. if r>0, x = x + d
% 7. Goto 1 until error<epsilon
% 
%
nix = 10;	niy = 10; 	niz = 10;

% Solving for changes
[e, gx,gy,gz, Gx,Gy,Gz] = ErrorF(x,y,z,d2);
disp(e);

s = 'y';
while( s == 'y' & e>epsilon ),
while( det(Gx+nix*eye(n))<0 ), nix = 4*nix;	end;
while( det(Gy+niy*eye(n))<0 ), niy = 4*niy;	end;
while( det(Gz+niz*eye(n))<0 ), niz = 4*niz;	end;
dx = - (Gx+nix*eye(n))\gx; 
dy = - (Gy+niy*eye(n))\gy;
dz = - (Gz+niz*eye(n))\gz;
rx = ( ErrorF(x+dx,y,z,d2)-e )/( dx'*gx + 0.5*dx'*Gx*dx );
ry = ( ErrorF(x,y+dy,z,d2)-e )/( dy'*gy + 0.5*dy'*Gy*dy );
rz = ( ErrorF(x,y,z+dz,d2)-e )/( dz'*gz + 0.5*dz'*Gz*dz );
if (rx<0.25), nix=4*nix; elseif (rx>0.75), nix=0.5*nix; end;
if (ry<0.25), niy=4*niy; elseif (ry>0.75), niy=0.5*niy; end;
if (rz<0.25), niz=4*niz; elseif (rz>0.75), niz=0.5*niz; end;
if (rx>0), x = x + dx;  end;
if (ry>0), y = y + dy;  end;
if (rz>0), z = z + dz;  end;
[e, gx,gy,gz, Gx,Gy,Gz] = ErrorF(x,y,z,d2);
disp([e, nix, niy, niz] );

s = input('Continue (y/n)?','s'); 
if (length(s)==0), s='y'; else s=lower(s(1)); end;
end; %while

%Additional coments:
% Problem definition:
%	A n*n matrix (d) with distances between the points is given so that
%	d(i,j) = distance between point i and point j
%	d2(i,j) = d(i,j)^2 = (x(i)-x(j))^2+(y(i)-y(j))^2+(z(i)-z(j))^2 + noise
%	matrix d2 should be idealy symetric, but due to the noise, it is not.
%
%       The minimization function is squared error: 
%		f = sum_i(sum_j( e(i,j)^2 ))
%	The first derivative 
%		df(x)/dx(i) = g(i) = sum_j( 4*(x(i)-x(j))*e(i,j) ); 
%	The second derivative 
%		d^2 f(x)/ dx(i)dx(j) = G(i,j) = -4*[ e(i,j) + 2*(x(i)-x(j))^2 ]
%	Taylor series is:
%		f(x+h) = f(x) + h'*g(x) + 1/2*h'*G(x)*h + o(h'*h)
%	The quadratic approximation is:
%		q(x)   = f(x)
%		q(x+h) = f(x) + h'*g(x) + 1/2*h'*G(x)*h
%	=>	Dq     = h'*g(x) + 1/2*h'*G(x)*h
%	Convergence ratio:
%		r = Df / Dq = ( f(x+h)-f(x) ) / Dq