function [x,y,z,e] = dMinDirect( x0,y0,z0, d, teta )
%This does NOT work
% dMinDirect - minimizes square error of distances 
%	[x,y,z, e,counter] = dMinDirect( x0,y0,z0, d, teta )
%	x0, y0, z0 - 1D column vectors, initial estimates of the points coordinates.
%	d -  matrix of distances between points, d(i,j)=distance between points i and j
%	teta  - NxN matrix with values [0..1], 0 representing bad  data in matrix d
%                                              1 representing good data in matrix d
%	x,y,z - result points
%	e - sum of errors
%	counter - number of iterations done

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

n = size(d,1);
l = ones(size(x0));

deltax2 = (x0*l'-l*x0').^2;	deltay2 = (y0*l'-l*y0').^2;	deltaz2 = (z0*l'-l*z0').^2;
e = deltax2 + deltay2 + deltaz2 - d.^2;
e1 = sum(sum(e.^2)); 
disp(sprintf('e1 = %10.2f',e1));

x1 = (e*x0)./(e*l);
y1 = (e*y0)./(e*l);
z1 = (e*z0)./(e*l);
deltax2 = (x1*l'-l*x1').^2;	deltay2 = (y1*l'-l*y1').^2;	deltaz2 = (z1*l'-l*z1').^2;
e = deltax2 + deltay2 + deltaz2 - d.^2;
e2 = sum(sum(e.^2));
disp(sprintf('e2 = %10.2f',e2));

x2 = (e*x1)./(e*l);
y2 = (e*y1)./(e*l);
z2 = (e*z1)./(e*l);
deltax2 = (x2*l'-l*x2').^2;	deltay2 = (y2*l'-l*y2').^2;	deltaz2 = (z2*l'-l*z2').^2;
e = deltax2 + deltay2 + deltaz2 - d.^2;
e3 = sum(sum(e.^2));
disp(sprintf('e3 = %10.2f',e3));

x = (x0.*x2 - x1.^2) ./ (x0 - 2*x1 + x2);
y = (y0.*y2 - y1.^2) ./ (y0 - 2*y1 + y2);
z = (z0.*z2 - z1.^2) ./ (z0 - 2*z1 + z2);
deltax2 = (x*l'-l*x').^2;	deltay2 = (y*l'-l*y').^2;	deltaz2 = (z*l'-l*z').^2;
e = deltax2 + deltay2 + deltaz2 - d.^2;
e = sum(sum(e.^2));
%disp(sprintf('e = %10.2f',e));



return;