core/vnl/algo/vnl_rnpoly_solve.cxx
Go to the documentation of this file.
00001 // This is core/vnl/algo/vnl_rnpoly_solve.cxx
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005 //:
00006 // \file
00007 #include "vnl_rnpoly_solve.h"
00008 
00009 #include <vnl/vnl_math.h> // for vnl_math::pi
00010 #include <vcl_cmath.h>
00011 #include <vcl_cassert.h>
00012 #ifdef DEBUG
00013 #include <vcl_iostream.h>
00014 #include <vcl_fstream.h>
00015 #endif
00016 
00017 static unsigned int dim_ = 0; // dimension of the problem
00018 static unsigned int max_deg_ = 0; // maximal degree
00019 static unsigned int max_nterms_ = 0; // maximal number of terms
00020 
00021 //: This is a local implementation of a minimal "complex number" class, for internal use only
00022 class vnl_rnpoly_solve_cmplx
00023 {
00024  public:
00025   double R;
00026   double C;
00027   vnl_rnpoly_solve_cmplx(double a=0, double b=0) : R(a), C(b) {}
00028   inline double norm() const { return R*R+C*C; }
00029   inline vnl_rnpoly_solve_cmplx operator-() const
00030   { return vnl_rnpoly_solve_cmplx(-R, -C); }
00031   inline vnl_rnpoly_solve_cmplx operator+(vnl_rnpoly_solve_cmplx const& Y) const
00032   { return vnl_rnpoly_solve_cmplx(R+Y.R, C+Y.C); }
00033   inline vnl_rnpoly_solve_cmplx operator-(vnl_rnpoly_solve_cmplx const& Y) const
00034   { return vnl_rnpoly_solve_cmplx(R-Y.R, C-Y.C); }
00035   inline vnl_rnpoly_solve_cmplx& operator+=(vnl_rnpoly_solve_cmplx const& Y)
00036   { R+=Y.R; C+=Y.C; return *this; }
00037   inline vnl_rnpoly_solve_cmplx& operator-=(vnl_rnpoly_solve_cmplx const& Y)
00038   { R-=Y.R; C-=Y.C; return *this; }
00039   inline vnl_rnpoly_solve_cmplx operator*(vnl_rnpoly_solve_cmplx const& Y) const
00040   { return vnl_rnpoly_solve_cmplx(R*Y.R-C*Y.C, R*Y.C+C*Y.R); }
00041   inline vnl_rnpoly_solve_cmplx operator/(vnl_rnpoly_solve_cmplx const& Y) const
00042   { double N=1.0/Y.norm(); return vnl_rnpoly_solve_cmplx((R*Y.R+C*Y.C)*N, (C*Y.R-R*Y.C)*N); }
00043   inline vnl_rnpoly_solve_cmplx operator*(double T) const
00044   { return vnl_rnpoly_solve_cmplx(R*T, C*T); }
00045   inline vnl_rnpoly_solve_cmplx& operator*=(double T)
00046   { R*=T; C*=T; return *this; }
00047   inline vnl_rnpoly_solve_cmplx& operator*=(vnl_rnpoly_solve_cmplx const& Y)
00048   { double r=R*Y.R-C*Y.C; C=R*Y.C+C*Y.R; R=r; return *this; }
00049   inline vnl_rnpoly_solve_cmplx& operator/=(vnl_rnpoly_solve_cmplx const& Y)
00050   { return *this = operator/(Y); }
00051 };
00052 
00053 static const double twopi = 2.0*vnl_math::pi;
00054 
00055 static const double epsilonB  = 2.e-03;
00056 static const vnl_rnpoly_solve_cmplx  epsilonZ  = vnl_rnpoly_solve_cmplx(1.e-04,1.e-04);
00057 static const double final_eps = 1.e-10;
00058 static const double stepinit  = 1.e-02;
00059 
00060 
00061 vcl_vector<vnl_vector<double>*> vnl_rnpoly_solve::realroots(double tol)
00062 {
00063   tol *= tol; // squared tolerance
00064   vcl_vector<vnl_vector<double>*> rr;
00065   vcl_vector<vnl_vector<double>*>::iterator rp = r_.begin(), ip = i_.begin();
00066   for (; rp != r_.end() && ip != i_.end(); ++rp, ++ip)
00067     if ((*ip)->squared_magnitude() < tol)
00068       rr.push_back(*rp);
00069 
00070   return rr;
00071 }
00072 
00073 
00074 //------------------------- INPTBR ---------------------------
00075 //: Initialize random variables
00076 // This will initialize the random variables which are used
00077 // to perturb the starting point so as to have measure zero
00078 // probability that we will start at a singular point.
00079 static void inptbr(vcl_vector<vnl_rnpoly_solve_cmplx>& p, vcl_vector<vnl_rnpoly_solve_cmplx>& q)
00080 {
00081   vnl_rnpoly_solve_cmplx pp[10],qq[10];
00082 
00083   pp[0] = vnl_rnpoly_solve_cmplx(.12324754231,  .76253746298);
00084   pp[1] = vnl_rnpoly_solve_cmplx(.93857838950,  -.99375892810);
00085   pp[2] = vnl_rnpoly_solve_cmplx(-.23467908356, .39383930009);
00086   pp[3] = vnl_rnpoly_solve_cmplx(.83542556622,  -.10192888288);
00087   pp[4] = vnl_rnpoly_solve_cmplx(-.55763522521, -.83729899911);
00088   pp[5] = vnl_rnpoly_solve_cmplx(-.78348738738, -.10578234903);
00089   pp[6] = vnl_rnpoly_solve_cmplx(.03938347346,  .04825184716);
00090   pp[7] = vnl_rnpoly_solve_cmplx(-.43428734331, .93836289418);
00091   pp[8] = vnl_rnpoly_solve_cmplx(-.99383729993, -.40947822291);
00092   pp[9] = vnl_rnpoly_solve_cmplx(.09383736736,  .26459172298);
00093 
00094   qq[0] = vnl_rnpoly_solve_cmplx(.58720452864,  .01321964722);
00095   qq[1] = vnl_rnpoly_solve_cmplx(.97884134700,  -.14433009712);
00096   qq[2] = vnl_rnpoly_solve_cmplx(.39383737289,  .4154322311);
00097   qq[3] = vnl_rnpoly_solve_cmplx(-.03938376373, -.61253112318);
00098   qq[4] = vnl_rnpoly_solve_cmplx(.39383737388,  -.26454678861);
00099   qq[5] = vnl_rnpoly_solve_cmplx(-.0093837766,  .34447867861);
00100   qq[6] = vnl_rnpoly_solve_cmplx(-.04837366632, .48252736790);
00101   qq[7] = vnl_rnpoly_solve_cmplx(.93725237347,  -.54356527623);
00102   qq[8] = vnl_rnpoly_solve_cmplx(.39373957747,  .65573434564);
00103   qq[9] = vnl_rnpoly_solve_cmplx(-.39380038371, .98903450052);
00104 
00105   p.resize(dim_); q.resize(dim_);
00106   for (unsigned int j=0; j<dim_; ++j) { int jj=j%10; p[j]=pp[jj]; q[j]=qq[jj]; }
00107 }
00108 
00109 //-----------------------------  POWR  -----------------------
00110 //: This returns the complex number y raised to the nth degree
00111 static inline vnl_rnpoly_solve_cmplx powr(int n,vnl_rnpoly_solve_cmplx const& y)
00112 {
00113   vnl_rnpoly_solve_cmplx x(1,0);
00114   if (n>0) while (n--) x *= y;
00115   else     while (n++) x /= y;
00116   return x;
00117 }
00118 
00119 
00120 static void initr(vcl_vector<unsigned int> const& ideg,
00121                   vcl_vector<vnl_rnpoly_solve_cmplx> const& p,
00122                   vcl_vector<vnl_rnpoly_solve_cmplx> const& q,
00123                   vcl_vector<vnl_rnpoly_solve_cmplx>& r,
00124                   vcl_vector<vnl_rnpoly_solve_cmplx>& pdg,
00125                   vcl_vector<vnl_rnpoly_solve_cmplx>& qdg)
00126 {
00127   assert(ideg.size()==dim_);
00128   assert(p.size()==dim_);
00129   assert(q.size()==dim_);
00130   pdg.resize(dim_); qdg.resize(dim_); r.resize(dim_);
00131   for (unsigned int j=0;j<dim_;j++)
00132   {
00133     pdg[j] = powr(ideg[j],p[j]);
00134     qdg[j] = powr(ideg[j],q[j]);
00135     r[j] = q[j] / p[j];
00136   }
00137 }
00138 
00139 
00140 //-------------------------------- DEGREE -------------------------------
00141 //: This will compute the degree of the polynomial based upon the index.
00142 static inline int degree(int index)
00143 {
00144   return (index<0) ? 0 : (index % max_deg_) + 1;
00145 }
00146 
00147 
00148 //-------------------------- FFUNR -------------------------
00149 //: Evaluate the target system component of h.
00150 //  This is the system of equations that we are trying to find the roots.
00151 static void ffunr(vcl_vector<double> const& coeff,
00152                   vcl_vector<int> const& polyn,
00153                   vcl_vector<unsigned int> const& terms,
00154                   vcl_vector<vnl_rnpoly_solve_cmplx> const& x,
00155                   vcl_vector<vnl_rnpoly_solve_cmplx>& pows,
00156                   vcl_vector<vnl_rnpoly_solve_cmplx>& f,
00157                   vcl_vector<vnl_rnpoly_solve_cmplx>& df)
00158 {
00159   assert(terms.size()==dim_);
00160   assert(x.size()==dim_);
00161   // Compute all possible powers for each variable
00162   pows.resize(max_deg_*dim_);
00163   for (unsigned int i=0;i<dim_;i++) // for all variables
00164   {
00165     int index = max_deg_*i;
00166     pows[index]=x[i];
00167     for (unsigned int j=1;j<max_deg_;++j,++index) // for all powers
00168       pows[index+1]= pows[index] * x[i];
00169   }
00170 
00171   // Initialize the new arrays
00172   for (unsigned int i=0; i<dim_; ++i)
00173   {
00174     f[i]=vnl_rnpoly_solve_cmplx(0,0);
00175     for (unsigned int j=0;j<dim_;j++)
00176       df[i*dim_+j]=vnl_rnpoly_solve_cmplx(0,0);
00177   }
00178 
00179   for (unsigned int i=0; i<dim_; ++i) // Across equations
00180     for (unsigned int j=0; j<terms[i]; ++j) // Across terms
00181     {
00182       vnl_rnpoly_solve_cmplx tmp(1,0);
00183       for (unsigned int k=0; k<dim_; ++k) // For each variable
00184       {
00185         int index=polyn[i*dim_*max_nterms_+j*dim_+k];
00186         if (index>=0)
00187           tmp *= pows[index];
00188       }
00189       f[i] += tmp * coeff[i*max_nterms_+j];
00190     }
00191 
00192   // Compute the Derivative!
00193   for (int i=dim_-1;i>=0;i--) // Over equations
00194     for (int l=dim_-1;l>=0;l--) // With respect to each variable
00195     {
00196       vnl_rnpoly_solve_cmplx& df_il = df[i*dim_+l];
00197       for (int j=terms[i]-1;j>=0;j--) // Over terms in each equation
00198         if (polyn[i*dim_*max_nterms_+j*dim_+l]>=0) // if 0 deg in l, df term is 0
00199         {
00200           vnl_rnpoly_solve_cmplx tmp = vnl_rnpoly_solve_cmplx(1,0);
00201           for (int k=dim_-1;k>=0;k--)        // Over each variable in each term
00202           {
00203             int index=polyn[i*dim_*max_nterms_+j*dim_+k];
00204             if (index>=0)
00205             {
00206               if (k==l)
00207               {
00208                 int deg = degree(index);
00209                 if (deg > 1)
00210                   tmp *= pows[index-1];
00211                 tmp *= (double)deg;
00212               }
00213               else
00214                 tmp *= pows[index];
00215             }
00216           } // end for k
00217           df_il += tmp * coeff[i*max_nterms_+j];
00218         }
00219     } // end for l
00220 }
00221 
00222 
00223 //--------------------------- GFUNR --------------------------
00224 //: Evaluate starting system component
00225 // Evaluate the starting system component of h from a system
00226 // of equations that we already know the roots. (ex: x^n - 1)
00227 static void gfunr(vcl_vector<unsigned int> const& ideg,
00228                   vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00229                   vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00230                   vcl_vector<vnl_rnpoly_solve_cmplx> const& pows,
00231                   vcl_vector<vnl_rnpoly_solve_cmplx>& g,
00232                   vcl_vector<vnl_rnpoly_solve_cmplx>& dg)
00233 {
00234   assert(ideg.size()==dim_);
00235   assert(g.size()==dim_);
00236   assert(dg.size()==dim_);
00237   vcl_vector<vnl_rnpoly_solve_cmplx> pxdgm1(dim_), pxdg(dim_);
00238 
00239   for (unsigned int j=0; j<dim_; ++j)
00240   {
00241     vnl_rnpoly_solve_cmplx tmp;
00242     if (ideg[j] <= 1)
00243       tmp = vnl_rnpoly_solve_cmplx(1,0);
00244     else
00245       tmp = pows[j*max_deg_+ideg[j]-2];
00246 
00247     pxdgm1[j] = pdg[j] * tmp;
00248   }
00249 
00250   for (unsigned int j=0; j<dim_; ++j)
00251   {
00252     int index = j*max_deg_+ideg[j]-1;
00253     pxdg[j] = pdg[j] * pows[index];
00254   }
00255 
00256   for (unsigned int j=0; j<dim_; ++j)
00257   {
00258     g[j]  = pxdg[j] - qdg[j];
00259     dg[j] = pxdgm1[j] * ideg[j];
00260   }
00261 }
00262 
00263 
00264 //-------------------------- HFUNR --------------------------
00265 //: This is the routine that traces the curve from the gfunr to the f function
00266 //  (i.e. Evaluate the continuation function)
00267 static void hfunr(vcl_vector<unsigned int> const& ideg,
00268                   vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00269                   vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00270                   double t,
00271                   vcl_vector<vnl_rnpoly_solve_cmplx> const& x,
00272                   vcl_vector<vnl_rnpoly_solve_cmplx>& h,
00273                   vcl_vector<vnl_rnpoly_solve_cmplx>& dhx,
00274                   vcl_vector<vnl_rnpoly_solve_cmplx>& dht,
00275                   vcl_vector<int> const& polyn,
00276                   vcl_vector<double> const& coeff,
00277                   vcl_vector<unsigned int> const& terms)
00278 {
00279   assert(ideg.size()==dim_);
00280   assert(terms.size()==dim_);
00281   assert(x.size()==dim_);
00282   assert(h.size()==dim_);
00283   assert(dht.size()==dim_);
00284   assert(dhx.size()==dim_*dim_);
00285   vcl_vector<vnl_rnpoly_solve_cmplx> df(dim_*dim_),dg(dim_),f(dim_),g(dim_);
00286   vcl_vector<vnl_rnpoly_solve_cmplx> pows;  //  powers of variables [dim_ equations] [max_deg_ possible powers]
00287 
00288   ffunr(coeff,polyn,terms,x,pows,f,df);
00289   gfunr(ideg,pdg,qdg,pows,g,dg);
00290   assert(f.size()==dim_);
00291   assert(g.size()==dim_);
00292   assert(dg.size()==dim_);
00293   assert(df.size()==dim_*dim_);
00294 
00295   double onemt=1.0 - t;
00296   for (unsigned int j=0; j<dim_; ++j)
00297   {
00298     for (unsigned int i=0; i<dim_; ++i)
00299       dhx[j*dim_+i] = df[j*dim_+i] * t;
00300 
00301     dhx[j*dim_+j] += dg[j]*onemt;
00302     dht[j] = f[j] - g[j];
00303     h[j] = f[j] * t + g[j] * onemt;
00304   }
00305 }
00306 
00307 
00308 //------------------------ LU DECOMPOSITION --------------------------
00309 //: This performs LU decomposition on a matrix.
00310 static int ludcmp(vcl_vector<vnl_rnpoly_solve_cmplx>& a, vcl_vector<int>& indx)
00311 {
00312   vcl_vector<double> vv(dim_);
00313 
00314   // Loop over rows to get the implicit scaling information
00315   for (unsigned int i=0; i<dim_; ++i)
00316   {
00317     double big = 0.0;
00318     for (unsigned int j=0; j<dim_; ++j)
00319     {
00320       double temp = a[i*dim_+j].norm();
00321       if (temp > big) big = temp;
00322     }
00323     if (big == 0.0) return 1;
00324     vv[i]=1.0/vcl_sqrt(big);
00325   }
00326 
00327   // This is the loop over columns of Crout's method
00328   for (unsigned int j=0; j<dim_; ++j)
00329   {
00330     for (unsigned int i=0; i<j; ++i)
00331       for (unsigned int k=0; k<i; ++k)
00332         a[i*dim_+j] -= a[i*dim_+k] * a[k*dim_+j];
00333 
00334     // Initialize for the search for largest pivot element
00335     double big = 0.0;
00336     unsigned int imax = 0;
00337 
00338     for (unsigned int i=j; i<dim_; ++i)
00339     {
00340       for (unsigned int k=0; k<j; ++k)
00341         a[i*dim_+j] -= a[i*dim_+k] * a[k*dim_+j];
00342 
00343       // Is the figure of merit for the pivot better than the best so far?
00344       double rdum = vv[i]*a[i*dim_+j].norm();
00345       if (rdum >= big) { big = rdum; imax = i; }
00346     }
00347 
00348     // Do we need to interchange rows?
00349     if (j != imax)
00350     {
00351       // Yes, do so...
00352       for (unsigned int k=0; k<dim_; ++k)
00353       {
00354         vnl_rnpoly_solve_cmplx dum = a[imax*dim_+k];
00355         a[imax*dim_+k] = a[j*dim_+k]; a[j*dim_+k] = dum;
00356       }
00357 
00358       // Also interchange the scale factor
00359       vv[imax]=vv[j];
00360     }
00361     indx[j]=imax;
00362 
00363     vnl_rnpoly_solve_cmplx& ajj = a[j*dim_+j];
00364     if (ajj.norm() == 0.0)
00365       ajj = epsilonZ;
00366 
00367     // Now, finally, divide by the pivot element
00368     if (j+1 != dim_)
00369     {
00370       vnl_rnpoly_solve_cmplx dum = vnl_rnpoly_solve_cmplx(1,0) / ajj;
00371 
00372       // If the pivot element is zero the matrix is singular.
00373       for (unsigned int i=j+1; i<dim_; ++i)
00374         a[i*dim_+j] *= dum;
00375     }
00376   }
00377   return 0;
00378 }
00379 
00380 
00381 // ------------------------- LU Back Substitution -------------------------
00382 static void lubksb(vcl_vector<vnl_rnpoly_solve_cmplx> const& a,
00383                    vcl_vector<int> const& indx,
00384                    vcl_vector<vnl_rnpoly_solve_cmplx> const& bb,
00385                    vcl_vector<vnl_rnpoly_solve_cmplx>& b)
00386 {
00387   int ii=-1;
00388   for (unsigned int k=0; k<dim_; ++k)
00389     b[k] = bb[k];
00390 
00391   for (unsigned int i=0; i<dim_; ++i)
00392   {
00393     int ip = indx[i];
00394     vnl_rnpoly_solve_cmplx sum = b[ip];
00395     b[ip] = b[i];
00396 
00397     if (ii>=0)
00398       for (unsigned int j=ii;j<i;++j)
00399         sum -= a[i*dim_+j] * b[j];
00400     else
00401       // A nonzero element was encountered, so from now on we
00402       // will have to do the sums in the loop above
00403       if (sum.norm() > 0) ii = i;
00404 
00405     b[i] = sum;
00406   }
00407 
00408   // Now do the backsubstitution
00409   for (int i=dim_-1;i>=0;i--)
00410   {
00411     for (unsigned int j=i+1; j<dim_; ++j)
00412       b[i] -= a[i*dim_+j] * b[j];
00413 
00414     b[i] /= a[i*dim_+i];
00415   }
00416 }
00417 
00418 
00419 //-------------------------- LINNR -------------------
00420 //: Solve a complex system of equations by using l-u decomposition and then back substitution.
00421 static int linnr(vcl_vector<vnl_rnpoly_solve_cmplx>& dhx,
00422                  vcl_vector<vnl_rnpoly_solve_cmplx> const& rhs,
00423                  vcl_vector<vnl_rnpoly_solve_cmplx>& resid)
00424 {
00425   vcl_vector<int> irow(dim_);
00426   if (ludcmp(dhx,irow)==1) return 1;
00427   lubksb(dhx,irow,rhs,resid);
00428   return 0;
00429 }
00430 
00431 
00432 //-----------------------  XNORM  --------------------
00433 //: Finds the unit normal of a vector v
00434 static double xnorm(vcl_vector<vnl_rnpoly_solve_cmplx> const& v)
00435 {
00436   assert(v.size()==dim_);
00437   double txnorm=0.0;
00438   for (unsigned int j=0; j<dim_; ++j)
00439     txnorm += vcl_fabs(v[j].R) + vcl_fabs(v[j].C);
00440   return txnorm;
00441 }
00442 
00443 //---------------------- PREDICT ---------------------
00444 //: Predict new x vector using Taylor's Expansion.
00445 static void predict(vcl_vector<unsigned int> const& ideg,
00446                     vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00447                     vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00448                     double step, double& t,
00449                     vcl_vector<vnl_rnpoly_solve_cmplx>& x,
00450                     vcl_vector<int> const& polyn,
00451                     vcl_vector<double> const& coeff,
00452                     vcl_vector<unsigned int> const& terms)
00453 {
00454   assert(ideg.size()==dim_);
00455   assert(terms.size()==dim_);
00456   assert(x.size()==dim_);
00457 
00458   double maxdt =.2; // Maximum change in t for a given step.  If dt is
00459                     // too large, there seems to be greater chance of
00460                     // jumping to another path.  Set this to 1 if you
00461                     // don't care.
00462   vcl_vector<vnl_rnpoly_solve_cmplx> dht(dim_),dhx(dim_*dim_),dz(dim_),h(dim_),rhs(dim_);
00463   // Call the continuation function that we are tracing
00464   hfunr(ideg,pdg,qdg,t,x,h,dhx,dht,polyn,coeff,terms);
00465 
00466   for (unsigned int j=0; j<dim_; ++j)
00467     rhs[j] = - dht[j];
00468 
00469   // Call the function that solves a complex system of equations
00470   if (linnr(dhx,rhs,dz) == 1) return;
00471 
00472   // Find the unit normal of a vector and normalize our step
00473   double factor = step/(1+xnorm(dz));
00474   if (factor>maxdt) factor = maxdt;
00475 
00476   bool tis1=true;
00477   if (t+factor>1) { tis1 = false; factor = 1.0 - t; }
00478 
00479   // Update this path with the predicted next point
00480   for (unsigned int j=0; j<dim_; ++j)
00481     x[j] += dz[j] * factor;
00482 
00483   if (tis1) t += factor;
00484   else      t = 1.0;
00485 }
00486 
00487 
00488 //------------------------- CORRECT --------------------------
00489 //: Correct the predicted point to lie near the actual curve
00490 // Use Newton's Method to do this.
00491 // Returns:
00492 // 0: Converged
00493 // 1: Singular Jacobian
00494 // 2: Didn't converge in 'loop' iterations
00495 // 3: If the magnitude of x > maxroot
00496 static int correct(vcl_vector<unsigned int> const& ideg, int loop, double eps,
00497                    vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00498                    vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00499                    double t,
00500                    vcl_vector<vnl_rnpoly_solve_cmplx>& x,
00501                    vcl_vector<int> const& polyn,
00502                    vcl_vector<double> const& coeff,
00503                    vcl_vector<unsigned int> const& terms)
00504 {
00505   double maxroot= 1000;// Maximum size of root where it is considered heading to infinity
00506   vcl_vector<vnl_rnpoly_solve_cmplx> dhx(dim_*dim_),dht(dim_),h(dim_),resid(dim_);
00507 
00508   assert(ideg.size()==dim_);
00509   assert(terms.size()==dim_);
00510   assert(x.size()==dim_);
00511 
00512   for (int i=0;i<loop;i++)
00513   {
00514     hfunr(ideg,pdg,qdg,t,x,h,dhx,dht,polyn,coeff,terms);
00515 
00516     // If linnr = 1, error
00517     if (linnr(dhx,h,resid)==1) return 1;
00518 
00519     for (unsigned int j=0; j<dim_; ++j)
00520       x[j] -= resid[j];
00521 
00522     double xresid = xnorm(resid);
00523     if (xresid < eps) return 0;
00524     if (xresid > maxroot) return 3;
00525   }
00526   return 2;
00527 }
00528 
00529 
00530 //-------------------------- TRACE ---------------------------
00531 //: This is the continuation routine.
00532 // It will trace a curve from a known point in the complex plane to an unknown
00533 // point in the complex plane.  The new end point is the root
00534 // to a polynomial equation that we are trying to solve.
00535 // It will return the following codes:
00536 //      0: Maximum number of steps exceeded
00537 //      1: Path converged
00538 //      2: Step size became too small
00539 //      3: Path Heading to infinity
00540 //      4: Singular Jacobian on Path
00541 static int trace(vcl_vector<vnl_rnpoly_solve_cmplx>& x,
00542                  vcl_vector<unsigned int> const& ideg,
00543                  vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
00544                  vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
00545                  vcl_vector<int> const& polyn,
00546                  vcl_vector<double> const& coeff,
00547                  vcl_vector<unsigned int> const& terms)
00548 {
00549   assert(ideg.size()==dim_);
00550   assert(terms.size()==dim_);
00551   assert(x.size()==dim_);
00552 
00553   int maxns=500;  // Maximum number of path steps
00554   int maxit=5;    // Maximum number of iterations to correct a step.
00555                   // For each step, Newton-Raphson is used to correct
00556                   // the step.  This should be at least 3 to improve
00557                   // the chances of convergence. If function is well
00558                   // behaved, fewer than maxit steps will be needed
00559 
00560   double eps=0;                     // epsilon value used in correct
00561   double epsilonS=1.0e-3 * epsilonB;// smallest path step for t>.95
00562   double stepmin=1.0e-5 * stepinit; // Minimum stepsize allowed
00563   double step=stepinit;             // stepsize
00564   double t=0.0;                     // Continuation parameter 0<t<1
00565   double oldt=0.0;                  // The previous t value
00566   vcl_vector<vnl_rnpoly_solve_cmplx> oldx = x; // the previous path value
00567   int nadv=0;
00568 
00569   for (int numstep=0;numstep<maxns;numstep++)
00570   {
00571     // Taylor approximate the next point
00572     predict(ideg,pdg,qdg,step,t,x,polyn,coeff,terms);
00573 
00574     //if (t>1.0) t=1.0;
00575 
00576     if (t > .95)
00577     {
00578       if (eps != epsilonS) step = step/4.0;
00579       eps = epsilonS;
00580     }
00581     else
00582       eps = epsilonB;
00583 #ifdef DEBUG
00584     vcl_cout << "t=" << t << vcl_endl;
00585 #endif
00586 
00587     if (t>=.99999)                      // Path converged
00588     {
00589 #ifdef DEBUG
00590       vcl_cout << "path converged\n" << vcl_flush;
00591 #endif
00592       double factor = (1.0-oldt)/(t-oldt);
00593       for (unsigned int j=0; j<dim_; ++j)
00594         x[j] = oldx[j] + (x[j]-oldx[j]) * factor;
00595       t = 1.0;
00596       int cflag=correct(ideg,10*maxit,final_eps,pdg,qdg,t,x, polyn, coeff,terms);
00597       if ((cflag==0) ||(cflag==2))
00598         return 1;       // Final Correction converged
00599       else if (cflag==3)
00600         return 3;       // Heading to infinity
00601       else return 4;    // Singular solution
00602     }
00603 
00604     // Newton's method brings us back to the curve
00605     int cflag=correct(ideg,maxit,eps,pdg,qdg,t,x,polyn, coeff,terms);
00606     if (cflag==0)
00607     {
00608       // Successful step
00609       if ((++nadv)==maxit) { step *= 2; nadv=0; }   // Increase the step size
00610       // Make note of our new location
00611       oldt = t;
00612       oldx = x;
00613     }
00614     else
00615     {
00616       nadv=0;
00617       step /= 2.0;
00618 
00619       if (cflag==3) return 3;           // Path heading to infinity
00620       if (step<stepmin) return 2;       // Path failed StepSizeMin exceeded
00621 
00622       // Reset the values since we stepped to far, and try again
00623       t = oldt;
00624       x = oldx;
00625     }
00626   }// end of the loop numstep
00627 
00628   return 0;
00629 }
00630 
00631 
00632 //-------------------------- STRPTR ---------------------------
00633 //: This will find a starting point on the 'g' function circle.
00634 // The new point to start tracing is stored in the x array.
00635 static void strptr(vcl_vector<unsigned int>& icount,
00636                    vcl_vector<unsigned int> const& ideg,
00637                    vcl_vector<vnl_rnpoly_solve_cmplx> const& r,
00638                    vcl_vector<vnl_rnpoly_solve_cmplx>& x)
00639 {
00640   assert(ideg.size()==dim_);
00641   assert(r.size()==dim_);
00642   x.resize(dim_);
00643 
00644   for (unsigned int i=0; i<dim_; ++i)
00645     if (icount[i] >= ideg[i]) icount[i] = 1;
00646     else                    { icount[i]++; break; }
00647 
00648   for (unsigned int j=0; j<dim_; ++j)
00649   {
00650     double angle = twopi / ideg[j] * icount[j];
00651     x[j] = r[j] * vnl_rnpoly_solve_cmplx (vcl_cos(angle), vcl_sin(angle));
00652   }
00653 }
00654 
00655 
00656 static vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> >
00657 Perform_Distributed_Task(vcl_vector<unsigned int> const& ideg,
00658                          vcl_vector<unsigned int> const& terms,
00659                          vcl_vector<int> const& polyn,
00660                          vcl_vector<double> const& coeff)
00661 {
00662   assert(ideg.size()==dim_);
00663 
00664   vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> > sols;
00665   vcl_vector<vnl_rnpoly_solve_cmplx> pdg, qdg, p, q, r, x;
00666   vcl_vector<unsigned int> icount(dim_,1); icount[0]=0;
00667   bool solflag; // flag used to remember if a root is found
00668 #ifdef DEBUG
00669   char const* FILENAM = "/tmp/cont.results";
00670   vcl_ofstream F(FILENAM);
00671   if (!F)
00672   {
00673     vcl_cerr<<"could not open "<<FILENAM<<" for writing\nplease erase old file first\n";
00674     F = vcl_cerr;
00675   }
00676   else
00677     vcl_cerr << "Writing to " << FILENAM << '\n';
00678 #endif
00679   // Initialize some variables
00680   inptbr(p,q);
00681   initr(ideg,p,q,r,pdg,qdg);
00682 
00683   // int Psize = 2*dim_*sizeof(double);
00684   int totdegree = 1;            // Total degree of the system
00685   for (unsigned int j=0;j<dim_;j++)  totdegree *= ideg[j];
00686 
00687   // *************  Send initial information ****************
00688   //Initialize(dim_,maxns,maxdt,maxit,maxroot,
00689   //           terms,ideg,pdg,qdg,coeff,polyn);
00690   while ((totdegree--) > 0)
00691   {
00692     // Compute path to trace
00693     strptr(icount,ideg,r,x);
00694 
00695     // Tell the client which path you want it to trace
00696     solflag = 1 == trace(x,ideg,pdg,qdg,polyn,coeff,terms);
00697     // Save the solution for future reference
00698     if (solflag)
00699     {
00700 #ifdef DEBUG
00701       for (unsigned int i=0; i<dim_; ++i)
00702         F << '<' << x[dim_-i-1].R << ' ' << x[dim_-i-1].C << '>';
00703       F << vcl_endl;
00704 #endif
00705       sols.push_back(x);
00706     }
00707 #ifdef DEBUG
00708     // print something out for each root
00709     if (solflag) vcl_cout << '.';
00710     else         vcl_cout << '*';
00711     vcl_cout.flush();
00712 #endif
00713   }
00714 
00715 #ifdef DEBUG
00716   vcl_cout<< vcl_endl;
00717 #endif
00718 
00719   return sols;
00720 }
00721 
00722 
00723 //----------------------- READ INPUT ----------------------
00724 //: This will read the input polynomials from a data file.
00725 void vnl_rnpoly_solve::Read_Input(vcl_vector<unsigned int>& ideg,
00726                                   vcl_vector<unsigned int>& terms,
00727                                   vcl_vector<int>& polyn,
00728                                   vcl_vector<double>& coeff)
00729 {
00730   // Read the number of equations
00731   dim_ = ps_.size();
00732 
00733   ideg.resize(dim_); terms.resize(dim_);
00734   // Start reading in the array values
00735   max_deg_=0;
00736   max_nterms_=0;
00737   for (unsigned int i=0;i<dim_;i++)
00738   {
00739     ideg[i] = ps_[i]->ideg_;
00740     terms[i] = ps_[i]->nterms_;
00741     if (ideg[i] > max_deg_)
00742       max_deg_ = ideg[i];
00743     if (terms[i] > max_nterms_)
00744       max_nterms_ = terms[i];
00745   }
00746   coeff.resize(dim_*max_nterms_);
00747   polyn.resize(dim_*max_nterms_*dim_);
00748   for (unsigned int i=0;i<dim_;i++)
00749   {
00750     for (unsigned int k=0;k<terms[i];k++)
00751     {
00752       coeff[i*max_nterms_+k] = ps_[i]->coeffs_(k);
00753       for (unsigned int j=0;j<dim_;j++)
00754       {
00755         int deg = ps_[i]->polyn_(k,j);
00756         polyn[i*dim_*max_nterms_+k*dim_+j] = deg ? int(j*max_deg_)+deg-1 : -1;
00757       }
00758     }
00759   }
00760 }
00761 
00762 
00763 vnl_rnpoly_solve::~vnl_rnpoly_solve()
00764 {
00765   while (r_.size() > 0) { delete r_.back(); r_.pop_back(); }
00766   while (i_.size() > 0) { delete i_.back(); i_.pop_back(); }
00767 }
00768 
00769 bool vnl_rnpoly_solve::compute()
00770 {
00771   vcl_vector<unsigned int> ideg, terms;
00772   vcl_vector<int> polyn;
00773   vcl_vector<double> coeff;
00774 
00775   Read_Input(ideg,terms,polyn,coeff); // returns number of equations
00776   assert(ideg.size()==dim_);
00777   assert(terms.size()==dim_);
00778   assert(polyn.size()==dim_*max_nterms_*dim_);
00779   assert(coeff.size()==dim_*max_nterms_);
00780 
00781   int totdegree = 1;
00782   for (unsigned int j=0; j<dim_; ++j) totdegree *= ideg[j];
00783 
00784   vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> > ans = Perform_Distributed_Task(ideg,terms,polyn,coeff);
00785 
00786   // Print out the answers
00787   vnl_vector<double> * rp, *ip;
00788 #ifdef DEBUG
00789   vcl_cout << "Total degree: " << totdegree << vcl_endl
00790            << "# solutions : " << ans.size() << vcl_endl;
00791 #endif
00792   for (unsigned int i=0; i<ans.size(); ++i)
00793   {
00794     assert(ans[i].size()==dim_);
00795     rp=new vnl_vector<double>(dim_); r_.push_back(rp);
00796     ip=new vnl_vector<double>(dim_); i_.push_back(ip);
00797     for (unsigned int j=0; j<dim_; ++j)
00798     {
00799 #ifdef DEBUG
00800       vcl_cout << ans[i][j].R << " + j " << ans[i][j].C << vcl_endl;
00801 #endif
00802       (*rp)[j]=ans[i][j].R; (*ip)[j]=ans[i][j].C;
00803     }
00804 #ifdef DEBUG
00805     vcl_cout<< vcl_endl;
00806 #endif
00807   }
00808   return true;
00809 }