% Script: rootfind
% Programmer: t. shores
% Last Rev: 7/1/02
% Description:
% A package of functions for solving the vector equation f(x)=0.
% Some routines may be oriented towards optimization.

% not a function file:
1;

% preliminary tools

function retval = arrayf(fname,arr)
% usage: arrayf(fname,arr)
% description: this returns the matrix resulting from replacing
% each coordinate of arr by its evaluation by the function with
% name fname.

% local variables
% ii,jj: index variables
% nr,nc: number of rows, cols

[nr,nc] = size(arr);
retval = zeros(nr,nc);
for ii = 1:nr
  for jj = 1:nc
    retval(ii,jj) = feval(fname,arr(ii,jj));
  end;
end;

endfunction

%

function retval = testfn(x)

retval = zeros(3,1);
retval(1) = 3*x(1)-cos(x(2)*x(3))-0.5;
retval(2) = x(1)^2-81*(x(2)+0.1)^2+sin(x(3))+1.06;
retval(3) = exp(-x(1)*x(2))+20*x(3)+(10*pi-3)/3;

endfunction

%

function retval = testf(x)

retval = testfn(x)'*testfn(x);

endfunction

%

function retval = jacob(fname,x,h,m)
% usage: retval = jacob(fname,x,h,m)
% description: An unsophisticated Jacobian calculation using
% certered differences. fname(x) and x are *assumed* vector and
% fname(x) is *assumed* a column vector of length m.

% local variables
% jj: index variable
% nx: length of x

nx = length(x);
retval = zeros(m,nx);
for jj = 1:nx
  dx = zeros(size(x));
  dx(jj) = h;
  retval(:,jj) = (feval(fname,x+dx)-feval(fname,x-dx))/(2*h);
end;

endfunction

% rootfinders

function [x,resid] = newton(fname,x0,iterlim,h)
% usage: [x,resid] = newton(fname,x0,iterlim,h)
% description: Classical Newton method for vector equation
% f(x)=0, with f,x n-vectors. Input function name, initial
% vector guess, iteration limit and stepsize for numerical
% derivative. Output final x, optionally norm of residual
% at final x. Stopping criterion is reaching iterlim.

% local variables
% kk: iteration counter
% x: looping values of x iterates
% n: length of x and f

x = x0;
n = length(x);
for ii = 1:iterlim
  x = x - jacob(fname,x,h,n)\feval(fname,x);
end;
resid = norm(feval(fname,x),inf);

endfunction

%

function [xnew,resid,kk,nn] = modnewton(fname,x0,iterlim,h)
% usage: [x,res,kk,nn] = modnewton(fname,x0,iterlim,h)
% description: a modified Newton's method applied to
% nonlinear equation fn(x) = 0. Input function name, initial
% vector guess, iteration limit and stepsize for numerical
% derivative. Output final x, optionally norm of residual
% at final x, iteration number and restart number.
% Uses Armijo search, restart and stopping criteria. Stores
% full Jacobian.

% local variables:
% stopr,stopa: relative and absolute stopping coefficients
% kk,nn: iteration counter, restart counter
% xold, xnew: vectors for iteration
% n: length of x and f
% B: inverse of Jacobian or approximation matrix
% s: search direction
% lambda: search direction parameter
% p: difference in x steps
% q: difference of residuals
% restartf: restart flag
% normf0: norm of initial function value

% initialization
stopr = 1e-7;
stopa = 1e-11;
xold = x0;
n = length(xold);
normf0 = norm(feval(fname,x0),inf);
B = inv(jacob(fname,xold,h,n));
nn = 1;
restartf = 1;
for kk = 1:iterlim
  s = -B*feval(fname,xold);
  lambda = search(fname,xold,s);
  if (lambda == 0)
    if (restartf ==1)
      disp('Failure of Newton direction to reduce ||f(x)||');
      resid = norm(feval(fname,xold),inf);
      return;
    else
      % need to really do a restart
      B = inv(jacob(fname,xold,h,n));
      nn = nn + 1;
      restart = 1;
      continue;
    end
  end
  % can now assume search direction succeeded
  xnew = xold + lambda*s;
  % so do a Broyden update
  p = xnew - xold;
  q = feval(fname,xnew) - feval(fname,xold);
  u = B*q-p;
  v = p/(p'*p);
  B = (eye(n) - u*v'/(1+v'*u))*B;
  restart = 0;
  xold = xnew;
  % test for termination
  if (norm(feval(fname,xnew),inf) <= stopr*normf0 + stopa)
    break;
  end
end
resid = norm(feval(fname,xold),inf);

endfunction

%

function lambda = search(fname,x,d)
% usage: lambda = search(fname,x,d)
% a routine to calculate search direction parameter lambda<=1
% for a modified Newton method. Failure signalled by lambda=0.

% local variables:
% alpha: sufficient decrease parameter
% maxhalf: maximum number of halvings
% jj: index variable
% xtmp,ftmp,normfx: temporaries

alpha = 1e-4;
maxhalf = 8;
jj = 0;
lambda = 2;
normfx = norm(feval(fname,x),inf);
while (jj < maxhalf)
  lambda = lambda/2;
  xtmp = x + lambda*d;
  ftmp = feval(fname,xtmp);
  if (norm(ftmp,inf) < (1-alpha*lambda)*normfx)
    break;
  end;
  jj = jj + 1;
end
if (jj == maxhalf)
  lambda = 0;
end

endfunction

%