function [f, gx,gy,gz] = dErrorNoZero(x,y,z, d, teta)

% dErrorF is error function for set of points and distances between them
%	[error, gx,gy,gz] = ErrorF(x,y,z, d, teta)
%	x,y,z - vectors of cortesian coordinates of the points
%	d     - NxN matrix of distances between points
%	teta  - NxN matrix with values [0..1], 0 representing bad  data in matrix d
%                                              1 representing good data in matrix d
%	e = sum(sum(error*error'))
%	gx,gy,gz - first  derivative of squared error with respect to x(i), y(i), z(i)
%	error = sum_i sum_j [(xi-xj)^2+(yi-yj)^2+(zi-zj)^2 - d2(i,j)^2]^2

n = size(teta,1);
alpha = n*n / sum(sum(teta));
deltax = x*ones(1,n) - ones(n,1)*x';	%creating matrix: deltax(i,j)=x(i)-x(j)
deltay = y*ones(1,n) - ones(n,1)*y';
deltaz = z*ones(1,n) - ones(n,1)*z';
pp = sqrt(deltax.^2 + deltay.^2 + deltaz.^2) + eye(n); % Point to point distance
e  = 1- d./pp;                                  %error
f  = alpha*sum(sum(teta.*(e.^2)));            %error function

if (nargout > 1 ),

ed = teta.*e.*d + (teta.*e.*d)';
pp3 = pp.^3;

gx = - alpha * 2*sum( deltax .* ed ./ pp3)';        %MATLAB sums comlumnwise
gy = - alpha * 2*sum( deltay .* ed ./ pp3)';	   
gz = - alpha * 2*sum( deltaz .* ed ./ pp3)';	   

end; %if

return
% error function:
% F = sum_i sum_j teta(i,j) e(i,j)^2
% error:
% e(i,j) = 1 - d(i,j)/pp(i,j)
% d(i,j) is given distance
% pp(i,j) is absolute distance between points i and j
% pp(i,j) = || p(i) - p(j) || = sqrt( (xi-xj)^2+(yi-yj)^2+(zi-zj)^2 )