Linux Cross Reference
cvs/emstar/libnl/nr/mnewt.c

   
   /*
    *  mnewt.c
    *
    *  Newton Raphson Method from Numerical Recipes
    */
   
   #include <math.h>
   #include "nr.h"
  
  // A small number.
  #define TINY 1.0e-20;
  
  /*
   * Given a matrix a[1..n][1..n], this routine replaces it by the LU
   * decomposition of a rowwise permutation of itself. a and n are input. a
   * is output, arranged as in equation (2.3.14) above; indx[1..n] is an
   * output vector that records the row permutation eected by the partial
   * pivoting; d is output as  1 depending on whether the number of row
   * interchanges was even or odd, respectively. This routine is used in
   * combination with lubksb to solve linear equations or invert a matrix.
   */
  
  void ludcmp(float **a, int n, int *indx, float *d)
  {
    int i,imax=0,j,k;
    float big,dum,sum,temp;
  
    // vv stores the implicit scaling of each row.
    float *vv; 
  
    vv=vector(1,n);
  
    // No row interchanges yet.
    *d=1.0; 
  
    // Loop over rows to get the implicit scaling information
    for (i=1;i<=n;i++) { 
      big=0.0;
      for (j=1;j<=n;j++)
        if ((temp=fabs(a[i][j])) > big) big=temp;
      if (big == 0.0) nrerror("Singular matrix in routine ludcmp");
      // No nonzero largest element.
      // Save the scaling.
      vv[i]=1.0/big; 
    }
  
    // This is the loop over columns of Crout's method.
    for (j=1;j<=n;j++) { 
      
      // This is equation (2.3.12) except for i = j.
      for (i=1;i<j;i++) { 
        sum=a[i][j];
        for (k=1;k<i;k++) sum -= a[i][k]*a[k][j];
        a[i][j]=sum;
      }
  
      // Initialize for the search for largest pivot element.
      big=0.0; 
  
      // This is i = j of equation (2.3.12) and i = j +1: ::N
      // of equation (2.3.13). 
      for (i=j;i<=n;i++) { 
        sum=a[i][j];
        for (k=1;k<j;k++)
          sum -= a[i][k]*a[k][j];
        a[i][j]=sum;
        if ( (dum=vv[i]*fabs(sum)) >= big) {
          // Is the Figure of merit for the pivot better than the best so far?
          big=dum;
          imax=i;
        }
      }
  
      // Do we need to interchange rows?
      if (j != imax) { 
        // Yes, do so...
        for (k=1;k<=n;k++) { 
          dum=a[imax][k];
          a[imax][k]=a[j][k];
          a[j][k]=dum;
        }
  
        // ...and change the parity of d.
        *d = -(*d); 
  
        // Also interchange the scale factor.
        vv[imax]=vv[j]; 
      }
      indx[j]=imax;
      if (a[j][j] == 0.0) a[j][j]=TINY;
      // If the pivot element is zero the matrix is singular (at least to the precision of the
      // algorithm). For some applications on singular matrices, it is desirable to substitute
      // TINY for zero.
  
      // Now, Finally, divide by the pivot element.
      if (j != n) { 
        dum=1.0/(a[j][j]);
        for (i=j+1;i<=n;i++) a[i][j] *= dum;
     }
 
     // Go back for the next column in the reduction.
   } 
   free_vector(vv,1,n);
 }
 
 
 /*
  * Here is the routine for forward substitution and backsubstitution,
  * implementing equations (2.3.6) and (2.3.7).
  *
  * Solves the set of n linear equations A 
 X = B. Herea[1..n][1..n] is
  * input, not as the matrix A but rather as its LU decomposition,
  * determined by the routine ludcmp. indx[1..n] is input as the
  * permutation vector returned by ludcmp. b[1..n] is input as the
  * right-hand side vector B, and returns with the solution vector X. a,
  * n, andindx are not modied by this routine and can be left in place
  * for successive calls with dierent right-hand sides b. This routine
  * takes into account the possibility that b will begin with many zero
  * elements, so it is efficient for use in matrix inversion.
  */
 
 void lubksb(float **a, int n, int *indx, float b[])
 {
   int i,ii=0,ip,j;
   float sum;
 
   // When ii is set to a positive value, it will become the
   // index of the rst nonvanishing element of b. Wenow
   // do the forward substitution, equation (2.3.6). The
   // only new wrinkle is to unscramble the permutation
   // as we go.
 
   for (i=1;i<=n;i++) { 
     ip=indx[i];
     sum=b[ip];
     b[ip]=b[i];
     if (ii)
       for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
     else 
       if (sum) ii=i;
     // A nonzero element was encountered, so from now on we
     // will have to do the sums in the loop above. b[i]=sum;
   }
   
   // Now we do the backsubstitution, equation (2.3.7).
   for (i=n;i>=1;i--) { 
     sum=b[i];
     for (j=i+1;j<=n;j++) sum -= a[i][j]*b[j];
 
     // Store a component of the solution vector X.
     b[i]=sum/a[i][i]; 
   } 
   // All done!
 }
 
 
 
 
 
 // Set to maximum number of iterations.
 #define JMAX 20
 
 /*
  * Using the Newton-Raphson method, nd the root of a function known to
  * lie in the interval [x1; x2]. The root rtnewt will be refined until its
  * accuracy is known within fixacc. funcd is a user-supplied routine
  * that returns both the function value and the first derivative of the
  * function at the point x.
  */
 
 float rtnewt(void (*funcd)(float, float *, float *), float x1, float x2,
              float xacc)
 {
   int j;
   float df,dx,f,rtn;
   // Initial guess.
   rtn=0.5*(x1+x2); 
   for (j=1;j<=JMAX;j++) {
     (*funcd)(rtn,&f,&df);
     dx=f/df;
     rtn -= dx;
     if ((x1-rtn)*(rtn-x2) < 0.0)
       nrerror("Jumped out of brackets in rtnewt");
     
     // Convergence.
     if (fabs(dx) < xacc) return rtn; 
   }
 
   // Never get here.
   nrerror("Maximum number of iterations exceeded in rtnewt");
   return 0.0; 
 }
 
 /*
  * While Newton-Raphson’s global convergence properties are poor, it
  * is fai easy to design a fail-safe routine that utilizes a combination
  * of bisection and Newton-Raphson.  The hybrid algorithm takes a
  * bisection step whenever Newton-Raphson would take the solution out of
  * bounds, or whenever Newton-Raphson is not reducing the size of the
  * brackets rapidly enough.
  */
 
 // Maximum allowed number of iterations.
 #define MAXIT 100 
 
 /*
  * Using a combination of Newton-Raphson and bisection, find the root of
  * a function bracketed between x1 and x2. The root, returned as the
  * function value rtsafe, will be refined until its accuracy is known
  * within  xacc. funcd is a user-supplied routine that returns both the
  * function value and the first derivative of the function.
  */
 
 float rtsafe(void (*funcd)(float, float *, float *), float x1, float x2,
              float xacc)
 {
   int j;
   float df,dx,dxold,f,fh,fl;
   float temp,xh,xl,rts;
   (*funcd)(x1,&fl,&df);
   (*funcd)(x2,&fh,&df);
   if ((fl > 0.0 && fh > 0.0) || (fl < 0.0 && fh < 0.0))
     nrerror("Root must be bracketed in rtsafe");
   if (fl == 0.0) return x1;
   if (fh == 0.0) return x2;
 
   // Orient the search so that f(xl) < 0.
   if (fl < 0.0) { 
     xl=x1;
     xh=x2;
   } 
   
   else {
     xh=x1;
     xl=x2;
   }
 
   // Initialize the guess for root,
   rts=0.5*(x1+x2); 
 
   // the stepsize before last,
   // and the last step.
   dxold=fabs(x2-x1); 
   dx=dxold; 
   (*funcd)(rts,&f,&df);
 
   // Loop over allowed iterations.
   for (j=1;j<=MAXIT;j++) { 
 
     // Bisect if Newton out of range,
     // or not decreasing fast enough.
     if ((((rts-xh)*df-f)*((rts-xl)*df-f) > 0.0) 
         || (fabs(2.0*f) > fabs(dxold*df))) { 
       dxold=dx;
       dx=0.5*(xh-xl);
       rts=xl+dx;
       
       // Change in root is negligible.
       if (xl == rts) return rts; 
     } 
     
     // Newton step acceptable. Take it.
     else { 
       dxold=dx;
       dx=f/df;
       temp=rts;
       rts -= dx;
       if (temp == rts) return rts;
     }
 
     // Convergence criterion.
     if (fabs(dx) < xacc) return rts; 
 
     // The one new function evaluation per iteration.
     (*funcd)(rts,&f,&df);
     
     // Maintain the bracket on the root.
     if (f < 0.0) 
       xl=rts;
     else
       xh=rts;
   }
 
   // Never get here.
   nrerror("Maximum number of iterations exceeded in rtsafe");
   return 0.0; 
 }
 
 
 
 /*
  * Given an initial guess x[1..n] for a root in n dimensions, take ntrial
  * Newton-Raphson steps to improve the root. Stop if the root converges
  * in either summed absolute variable increments tolx or summed absolute
  * function values tolf.
  */
 
 void mnewt(int ntrial, float x[], int n, float tolx, float tolf, mnewt_func func)
 {
   int k,i,*indx;
   float errx,errf,d,*fvec,**fjac,*p;
 
   indx=ivector(1,n);
   p=vector(1,n);
   fvec=vector(1,n);
   fjac=matrix(1,n,1,n);
 
   for (k=1;k<=ntrial;k++) {
 
     // User function supplies function values at x in
     // fvec and Jacobian matrix in fjac. 
     func(x,n,fvec,fjac); 
     
     // Check function convergence.
     errf=0.0;
     for (i=1;i<=n;i++) errf += fabs(fvec[i]); 
     if (errf <= tolf) 
       goto out;
     
     // Right-hand side of linear equations.
     for (i=1;i<=n;i++) p[i] = -fvec[i]; 
 
     // Solve linear equations using LU decomposition.
     ludcmp(fjac,n,indx,&d); 
     lubksb(fjac,n,indx,p);
 
     // Check root convergence.
     errx=0.0; 
     for (i=1;i<=n;i++) { 
       // Update solution.
       errx += fabs(p[i]);
       x[i] += p[i];
     }
     if (errx <= tolx) 
       goto out;
   }
 
  out:
   free_matrix(fjac,1,n,1,n);
   free_vector(fvec,1,n);
   free_vector(p,1,n);
   free_ivector(indx,1,n);
 }