vnl_rnpoly_solve.cxx

DTMK软件开发包,此为开源软件,是一款很好的医学图像开发资源.

  }

  // Now do the backsubstitution
  for (int i=dim_-1;i>=0;i--)
  {
    for (unsigned int j=i+1; j<dim_; ++j)
      b[i] -= a[i*dim_+j] * b[j];

    b[i] /= a[i*dim_+i];
  }
}


//-------------------------- LINNR -------------------
//: Solve a complex system of equations by using l-u decomposition and then back substitution.
static int linnr(vcl_vector<vnl_rnpoly_solve_cmplx>& dhx,
                 vcl_vector<vnl_rnpoly_solve_cmplx> const& rhs,
                 vcl_vector<vnl_rnpoly_solve_cmplx>& resid)
{
  vcl_vector<int> irow(dim_);
  if (ludcmp(dhx,irow)==1) return 1;
  lubksb(dhx,irow,rhs,resid);
  return 0;
}


//-----------------------  XNORM  --------------------
//: Finds the unit normal of a vector v
static double xnorm(vcl_vector<vnl_rnpoly_solve_cmplx> const& v)
{
  assert(v.size()==dim_);
  double txnorm=0.0;
  for (unsigned int j=0; j<dim_; ++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(vcl_vector<unsigned int> const& ideg,
                    vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
                    vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
                    double step, double& t,
                    vcl_vector<vnl_rnpoly_solve_cmplx>& x,
                    vcl_vector<int> const& polyn,
                    vcl_vector<double> const& coeff,
                    vcl_vector<unsigned int> const& terms)
{
  assert(ideg.size()==dim_);
  assert(terms.size()==dim_);
  assert(x.size()==dim_);

  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.
  vcl_vector<vnl_rnpoly_solve_cmplx> dht(dim_),dhx(dim_*dim_),dz(dim_),h(dim_),rhs(dim_);
  // Call the continuation function that we are tracing
  hfunr(ideg,pdg,qdg,t,x,h,dhx,dht,polyn,coeff,terms);

  for (unsigned int j=0; j<dim_; ++j)
    rhs[j] = - dht[j];

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

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

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

  // Update this path with the predicted next point
  for (unsigned int j=0; j<dim_; ++j)
    x[j] += dz[j] * factor;

  if (tis1) 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(vcl_vector<unsigned int> const& ideg, int loop, double eps,
                   vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
                   vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
                   double t,
                   vcl_vector<vnl_rnpoly_solve_cmplx>& x,
                   vcl_vector<int> const& polyn,
                   vcl_vector<double> const& coeff,
                   vcl_vector<unsigned int> const& terms)
{
  double maxroot= 1000;// Maximum size of root where it is considered heading to infinity
  vcl_vector<vnl_rnpoly_solve_cmplx> dhx(dim_*dim_),dht(dim_),h(dim_),resid(dim_);

  assert(ideg.size()==dim_);
  assert(terms.size()==dim_);
  assert(x.size()==dim_);

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

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

    for (unsigned int j=0; j<dim_; ++j)
      x[j] -= resid[j];

    double xresid = xnorm(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(vcl_vector<vnl_rnpoly_solve_cmplx>& x,
                 vcl_vector<unsigned int> const& ideg,
                 vcl_vector<vnl_rnpoly_solve_cmplx> const& pdg,
                 vcl_vector<vnl_rnpoly_solve_cmplx> const& qdg,
                 vcl_vector<int> const& polyn,
                 vcl_vector<double> const& coeff,
                 vcl_vector<unsigned int> const& terms)
{
  assert(ideg.size()==dim_);
  assert(terms.size()==dim_);
  assert(x.size()==dim_);

  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=1.0e-3 * epsilonB;// smallest path step for t>.95
  double stepmin=1.0e-5 * stepinit; // Minimum stepsize allowed
  double step=stepinit;             // stepsize
  double t=0.0;                     // Continuation parameter 0<t<1
  double oldt=0.0;                  // The previous t value
  vcl_vector<vnl_rnpoly_solve_cmplx> oldx = x; // the previous path value
  int nadv=0;

  for (int numstep=0;numstep<maxns;numstep++)
  {
    // Taylor approximate the next point
    predict(ideg,pdg,qdg,step,t,x,polyn,coeff,terms);

    //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_cout << "t=" << t << vcl_endl;
#endif

    if (t>=.99999)                      // Path converged
    {
#ifdef DEBUG
      vcl_cout << "path converged\n" << vcl_flush;
#endif
      double factor = (1.0-oldt)/(t-oldt);
      for (unsigned int j=0; j<dim_; ++j)
        x[j] = oldx[j] + (x[j]-oldx[j]) * factor;
      t = 1.0;
      int cflag=correct(ideg,10*maxit,final_eps,pdg,qdg,t,x, polyn, coeff,terms);
      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
    int cflag=correct(ideg,maxit,eps,pdg,qdg,t,x,polyn, coeff,terms);
    if (cflag==0)
    {
      // Successful step
      if ((++nadv)==maxit) { step *= 2; nadv=0; }   // Increase the step size
      // Make note of our new location
      oldt = t;
      oldx = x;
    }
    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;
      x = oldx;
    }
  }// 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(vcl_vector<unsigned int>& icount,
                   vcl_vector<unsigned int> const& ideg,
                   vcl_vector<vnl_rnpoly_solve_cmplx> const& r,
                   vcl_vector<vnl_rnpoly_solve_cmplx>& x)
{
  assert(ideg.size()==dim_);
  assert(r.size()==dim_);
  x.resize(dim_);

  for (unsigned int i=0; i<dim_; ++i)
    if (icount[i] >= ideg[i]) icount[i] = 1;
    else                    { icount[i]++; break; }

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


static vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> >
Perform_Distributed_Task(vcl_vector<unsigned int> const& ideg,
                         vcl_vector<unsigned int> const& terms,
                         vcl_vector<int> const& polyn,
                         vcl_vector<double> const& coeff)
{
  assert(ideg.size()==dim_);

  vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> > sols;
  vcl_vector<vnl_rnpoly_solve_cmplx> pdg, qdg, p, q, r, x;
  vcl_vector<unsigned int> icount(dim_,1); icount[0]=0;
  bool solflag; // flag used to remember if a root is found
#ifdef DEBUG
  char const* FILENAM = "/tmp/cont.results";
  vcl_ofstream F(FILENAM);
  if (!F)
  {
    vcl_cerr<<"could not open "<<FILENAM<<" for writing\nplease erase old file first\n";
    F = vcl_cerr;
  }
  else
    vcl_cerr << "Writing to " << FILENAM << '\n';
#endif
  // Initialize some variables
  inptbr(p,q);
  initr(ideg,p,q,r,pdg,qdg);

  // int Psize = 2*dim_*sizeof(double);
  int totdegree = 1;            // Total degree of the system
  for (unsigned int j=0;j<dim_;j++)  totdegree *= ideg[j];

  // *************  Send initial information ****************
  //Initialize(dim_,maxns,maxdt,maxit,maxroot,
  //           terms,ideg,pdg,qdg,coeff,polyn);
  while ((totdegree--) > 0)
  {
    // Compute path to trace
    strptr(icount,ideg,r,x);

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

#ifdef DEBUG
  vcl_cout<< vcl_endl;
#endif

  return sols;
}


//----------------------- READ INPUT ----------------------
//: This will read the input polynomials from a data file.
void vnl_rnpoly_solve::Read_Input(vcl_vector<unsigned int>& ideg,
                                  vcl_vector<unsigned int>& terms,
                                  vcl_vector<int>& polyn,
                                  vcl_vector<double>& coeff)
{
  // Read the number of equations
  dim_ = ps_.size();

  ideg.resize(dim_); terms.resize(dim_);
  // Start reading in the array values
  max_deg_=0;
  max_nterms_=0;
  for (unsigned int i=0;i<dim_;i++)
  {
    ideg[i] = ps_[i]->ideg_;
    terms[i] = ps_[i]->nterms_;
    if (ideg[i] > max_deg_)
      max_deg_ = ideg[i];
    if (terms[i] > max_nterms_)
      max_nterms_ = terms[i];
  }
  coeff.resize(dim_*max_nterms_);
  polyn.resize(dim_*max_nterms_*dim_);
  for (unsigned int i=0;i<dim_;i++)
  {
    for (unsigned int k=0;k<terms[i];k++)
    {
      coeff[i*max_nterms_+k] = ps_[i]->coeffs_(k);
      for (unsigned int j=0;j<dim_;j++)
      {
        int deg = ps_[i]->polyn_(k,j);
        polyn[i*dim_*max_nterms_+k*dim_+j] = deg ? int(j*max_deg_)+deg-1 : -1;
      }
    }
  }
}


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()
{
  vcl_vector<unsigned int> ideg, terms;
  vcl_vector<int> polyn;
  vcl_vector<double> coeff;

  Read_Input(ideg,terms,polyn,coeff); // returns number of equations
  assert(ideg.size()==dim_);
  assert(terms.size()==dim_);
  assert(polyn.size()==dim_*max_nterms_*dim_);
  assert(coeff.size()==dim_*max_nterms_);

  int totdegree = 1;
  for (unsigned int j=0; j<dim_; ++j) totdegree *= ideg[j];

  vcl_vector<vcl_vector<vnl_rnpoly_solve_cmplx> > ans = Perform_Distributed_Task(ideg,terms,polyn,coeff);

  // Print out the answers
  vnl_vector<double> * rp, *ip;
#ifdef DEBUG
  vcl_cout << "Total degree: " << totdegree << vcl_endl
           << "# solutions : " << ans.size() << vcl_endl;
#endif
  for (unsigned int i=0; i<ans.size(); ++i)
  {
    assert(ans[i].size()==dim_);
    rp=new vnl_vector<double>(dim_); r_.push_back(rp);
    ip=new vnl_vector<double>(dim_); i_.push_back(ip);
    for (unsigned int j=0; j<dim_; ++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;
}