vnl_rnpoly_solve.cxx

InsightToolkit-1.4.0(有大量的优化算法程序)

  }
}


//-------------------------- LINNR -------------------
//: Solve a complex system of equations by using l-u decomposition and then back subsitution.
static int linnr(int len,vnl_rnpoly_solve_cmplx dhx[M][M],
                 vnl_rnpoly_solve_cmplx rhs[M],
                 vnl_rnpoly_solve_cmplx resid[M])
{ int irow[M];
  if (ludcmp(dhx,len,irow)==1) return 1;
  lubksb(dhx,len,irow,rhs,resid);
  return 0;
}


//-----------------------  XNORM  --------------------
//: Finds the unit normal of a vector v
static double xnorm(int n, vnl_rnpoly_solve_cmplx v[])
{
  double txnorm=0.0;
  for (int j=n-1;j>=0; --j)
    txnorm += vcl_fabs(v[j].R) + vcl_fabs(v[j].C);
  return txnorm;
}

//---------------------- PREDICT ---------------------
//: Predict new x vector using Taylor's Expansion.
static void predict(int len, int ideg[M], vnl_rnpoly_solve_cmplx pdg[M], vnl_rnpoly_solve_cmplx qdg[M],
                    double step, double& t, vnl_rnpoly_solve_cmplx x[M], int polyn[M][T][M],
                    double coeff[M][T], int terms[M], int max_deg)
{
  double maxdt =.2; // Maximum change in t for a given step.  If dt is
                    // too large, there seems to be greater chance of
                    // jumping to another path.  Set this to 1 if you
                    // don't care.
  int j;
  double factor;
  vnl_rnpoly_solve_cmplx dht[M],dhx[M][M],dz[M],h[M],rhs[M];
  int tis1=0;
  // Call the continuation function that we are tracing
  hfunr(len,ideg,pdg,qdg,t,x,h,dhx,dht,polyn,coeff,terms,max_deg);

  for (j=len-1;j>=0;j--)
    rhs[j] = - dht[j];

  // Call the function that solves a complex system of equations
  if (linnr(len,dhx,rhs,dz) == 1) return;

  // Find the unit normal of a vector and normalize our step
  factor = step/(1+xnorm(len,dz));
  if (factor>maxdt) factor = maxdt;

  if ((t+factor)>1) {tis1 = 1; factor = 1.0 - t;}

  // Update this path with the predicted next point
  for (j=len-1;j>=0;j--)
    x[j] += dz[j] * factor;

  if (tis1==0) t += factor;
  else         t = 1.0;
}


//------------------------- CORRECT --------------------------
//: Correct the predicted point to lie near the actual curve
// Use Newton's Method to do this.
// Returns:
// 0: Converged
// 1: Singular Jacobian
// 2: Didn't converge in 'loop' iterations
// 3: If the magnitude of X > maxroot
static int correct(int len,int ideg[M], int loop, double eps,
                   vnl_rnpoly_solve_cmplx pdg[M], vnl_rnpoly_solve_cmplx qdg[M], double t,
                   vnl_rnpoly_solve_cmplx x[M], int polyn[M][T][M], double coeff[M][T],
                   int terms[M], int max_deg)
{
  double maxroot= 1000;// Maximum size of root where it is considered heading to infinity
  int i,j;
  vnl_rnpoly_solve_cmplx dhx[M][M],dht[M], h[M],resid[M];

  double xresid;

  for (i=0;i<loop;i++) {
    hfunr(len,ideg,pdg,qdg,t,x,h,dhx,dht,polyn,coeff,terms,max_deg);

    // If linnr = 1, error
    if (linnr(len,dhx,h,resid)==1) return 1;

    for (j=len-1;j>=0;j--)
      x[j] -= resid[j];

    xresid = xnorm(len,resid);
    if (xresid < eps) return 0;
    if (xresid > maxroot) return 3;
  }
  return 2;
}


//-------------------------- TRACE ---------------------------
//: This is the continuation routine.
// It will trace a curve from a known point in the complex plane to an unknown
// point in the complex plane.  The new end point is the root
// to a polynomial equation that we are trying to solve.
// It will return the following codes:
//      0: Maximum number of steps exceeded
//      1: Path converged
//      2: Step size became too small
//      3: Path Heading to infinity
//      4: Singular Jacobian on Path
static int trace (int len, vnl_rnpoly_solve_cmplx x[M], int ideg[M],
                  vnl_rnpoly_solve_cmplx pdg[M], vnl_rnpoly_solve_cmplx qdg[M],
                  int polyn[M][T][M], double coeff[M][T],
                  int terms[M], int max_deg)
{
  int maxns=500;  // Maximum number of path steps
  int maxit=5;    // Maximum number of iterations to correct a step.
                  // For each step, Newton-Raphson is used to correct
                  // the step.  This should be at least 3 to improve
                  // the chances of convergence. If function is well
                  // behaved, fewer than maxit steps will be needed

  double eps=0;   // epsilon value used in correct
  double epsilonS;// smallest path step for t>.95
  double stepmin; // Minimum stepsize allowed
  double step;    // stepsize
  double t;       // Continuation parameter 0<t<1
  double oldt;    // The previous t value
  vnl_rnpoly_solve_cmplx oldx[M];       // the previous path value
  double factor;
  int j,numstep,cflag;
  int nadv;

  t=oldt=0.0;
  nadv=0;
  // int n2 = 2*len;
  epsilonS=1.0e-3 * epsilonB;
  stepmin=1.0e-5 * stepinit;
  step=stepinit;

  // Remember the original point
  for (j=len-1;j>=0;j--)
    oldx[j] = x[j];

  for (numstep=0;numstep<maxns;numstep++) {

    // Taylor approximate the next point
    predict(len,ideg,pdg,qdg,step,t,x,polyn,coeff,terms,max_deg);

    //if (t>1.0) t=1.0;

    if (t > .95) {
      if (eps != epsilonS) step = step/4.0;
      eps = epsilonS;
    }else
      eps = epsilonB;
#ifdef DEBUG
    vcl_printf ("t=%.15f\n",t); fflush(stdout);
#endif

    if (t>=.99999) {                    // Path converged
#ifdef DEBUG
      vcl_printf ("path converged\n");
#endif
      factor = (1.0-oldt)/(t-oldt);
      for (j=len-1;j>=0;j--)
        x[j] = oldx[j] + (x[j]-oldx[j]) * factor;
      t = 1.0;
      cflag=correct(len,ideg,10*maxit,final_eps,pdg,qdg,t,x, polyn, coeff,terms, max_deg);
      if ((cflag==0) ||(cflag==2))
        return 1;       // Final Correction converged
      else if (cflag==3)
        return 3;       // Heading to infinity
      else return 4;    // Singular solution
    }

    // Newton's method brings us back to the curve
    cflag=correct(len,ideg,maxit,eps,pdg,qdg,t,x,polyn, coeff,terms,max_deg);
    if (cflag==0) {
      // Successful step
      if ((++nadv)==5) { step *= 2; nadv=0; }   // Increase the step size
      // Make note of our new location
      oldt=t;
      for (j=len-1;j>=0;j--)
        oldx[j] = x[j];
    } else {
      nadv=0;
      step /= 2.0;

      if (cflag==3) return 3;           // Path heading to infinity
      if (step<stepmin) return 2;       // Path failed StepSizeMin exceeded

      // Reset the values since we stepped to far, and try again
      t = oldt;
      for (j=len-1;j>=0;j--)
        x[j] = oldx[j];
    }
  }// end of the loop numstep

  return 0;
}


//-------------------------- STRPTR ---------------------------
//: This will find a starting point on the 'g' function circle.
// The new point to start tracing is stored in the x array.
static void strptr(int n,int icount[M],int ideg[M], vnl_rnpoly_solve_cmplx r[M],vnl_rnpoly_solve_cmplx x[M])
{
  for (int i=0;i<n;i++)
    if (icount[i] >= ideg[i]) icount[i] = 1;
    else                    { icount[i]++; break; }

  for (int j=0;j<n;j++) {
    double angle = twopi / ideg[j] * icount[j];
    x[j] = r[j] * vnl_rnpoly_solve_cmplx (vcl_cos(angle), vcl_sin(angle));
  }
}


static int Perform_Distributed_Task(int points,vnl_rnpoly_solve_cmplx sols[LEN][M],
                                    int ideg[M],int terms[M],
                                    int polyn[M][T][M],double coeff[M][T])
{
  vnl_rnpoly_solve_cmplx p[M], q[M], r[M], pdg[M], qdg[M], x[M];
  int icount[M];
  int j,i;
  int NumSols=0;
  bool solflag;           // flag used to remember if a root is found
  int max_deg=P;
#ifdef DEBUG
  char const* FILENAM = "/tmp/cont.results";
  FILE *F = vcl_fopen(FILENAM,"w");
  if (!F) {
    vcl_cerr<<"could not open "<<FILENAM<<"\nplease erase old file first\n";
    F = stderr;
  }
  else
    vcl_fprintf(stderr, "Writing to %s\n", FILENAM);
#endif
  // Initialize some variables
  inptbr(points,p,q);
  initr(points,ideg,p,q,r,pdg,qdg);

  // int Psize = 2*points*sizeof(double);
  int totdegree = 1;            // Total degree of the system
  for (j=0;j<points;j++)  totdegree *= ideg[j];
  icount[0]=0;
  for (j=points-1;j>0;j--) icount[j]=1;
  for (i=0;i<points;i++)
    if (ideg[i] > max_deg)
      max_deg = ideg[i];

  // *************  Send initial information ****************
  //Initialize(points,maxns,maxdt,maxit,maxroot,
  //           terms,ideg,pdg,qdg,coeff,polyn);
  while ((totdegree--) > 0) {

    // Compute path to trace
    strptr(points,icount,ideg,r,x);

    // Tell the client which path you want it to trace
    solflag = 1 == trace (points,x,ideg,pdg,qdg,polyn,coeff,terms,max_deg);
    // Save the solution for future reference
    if (solflag) {
      for (i=points-1;i>=0;i--) {
        sols[NumSols][i] = x[i];
#ifdef DEBUG
        vcl_fprintf(F,"<%f  %f>",x[points-i-1].R,x[points-i-1].C);
#endif
      }
      ++NumSols;
#ifdef DEBUG
      vcl_fprintf(F,"\n");
      vcl_fflush(F);
#endif
    }
#ifdef DEBUG
    // print something out for each root
    if (solflag) vcl_cout << ".";
    else vcl_cout << '*';
    vcl_cout.flush();
#endif
  }

#ifdef DEBUG
  if (F != stderr) vcl_fclose(F);
  vcl_cout<< vcl_endl;
#endif

  return NumSols;
}


//----------------------- READ INPUT ----------------------
//: This will read the input polynomials from a data file.
int vnl_rnpoly_solve::Read_Input(int ideg[M], int terms[M],
                                 int polyn[M][T][M], double coeff[M][T])
{
  // Read the number of equations
  unsigned int n = ps_.size();

  // Initialize the array's to zero
  for (unsigned int i=0;i<n;i++) for (unsigned int k=0;k<T;k++)
    coeff[i][k] = 0.0;
  for (unsigned int i=0;i<n;i++)
    ideg[i] = terms[i] = 0;
  for (unsigned int i=0;i<n;i++) for (unsigned int k=0;k<T;k++) for (unsigned int j=0;j<n;j++)
    polyn[i][k][j]=0;
  // Start reading in the array values
  for (unsigned int i=0;i<n;i++)
  {
    ideg[i]  = ps_[i]->ideg_;
    terms[i] = ps_[i]->nterms_;

    for (int k=0;k<terms[i];k++)
    {
      coeff[i][k] = ps_[i]->coeffs_(k);
      for (unsigned int j=0;j<n;j++) {
        int deg = ps_[i]->polyn_(k,j);
        if (deg) polyn[i][k][j] = (j*P)+(deg-1);
        else     polyn[i][k][j] = -1;
      }
    }
  }
  return n;
}


vnl_rnpoly_solve::~vnl_rnpoly_solve()
{
  while (r_.size() > 0)
    { delete r_.back(); r_.pop_back(); }
  while (i_.size() > 0)
    { delete i_.back(); i_.pop_back(); }
}

bool vnl_rnpoly_solve::compute()
{

  int i,j;
  int ideg[M], terms[M], polyn[M][T][M];
  vnl_rnpoly_solve_cmplx ans[LEN][M];
  double coeff[M][T];
  int p = Read_Input(ideg,terms,polyn,coeff);
  int NumSols = Perform_Distributed_Task(p,ans,ideg,terms,polyn,coeff);
  // Print out the answers
  vnl_vector<double> * rp, *ip;
#ifdef DEBUG
  vcl_cout << "Numsolutions are: " << NumSols << vcl_endl;
#endif
for (i=0;i<NumSols;i++) {
    rp=new vnl_vector<double>(p);  r_.push_back(rp);
    ip=new vnl_vector<double>(p);  i_.push_back(ip);
    for (j=0;j<p;j++) {
#ifdef DEBUG
      vcl_cout << ans[i][j].R << " + j " << ans[i][j].C << vcl_endl;
#endif
      (*rp)[j]=ans[i][j].R; (*ip)[j]=ans[i][j].C;
    }
#ifdef DEBUG
    vcl_cout<< vcl_endl;
#endif
  }
  return true;
}