vnl_rnpoly_solve.cxx

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

// This is core/vnl/algo/vnl_rnpoly_solve.cxx
#ifdef VCL_NEEDS_PRAGMA_INTERFACE
#pragma implementation
#endif
//:
// \file

#include <vcl_cmath.h>
#ifdef DEBUG
#include <vcl_cstdio.h>
#include <vcl_iostream.h>
#endif

#include "vnl_rnpoly_solve.h"

// fsm: moved ::M and ::T into the namespace of vnl_rnpoly_solve, as they
// were causing multiply defined symbols for static builds. if your compiler cannot
// cope with the next two lines, replace them with #defines.
static const unsigned int M = vnl_rnpoly_solve::M;
static const unsigned int T = vnl_rnpoly_solve::T;

static const unsigned int P  = 10;   // Maximum power for any variable in a term
static const unsigned int LEN= 3080; // Maximum number of roots

//: This is a local implementation of a "complex number" class, for internal use only
class vnl_rnpoly_solve_cmplx {
public:
  double R;
  double C;
  vnl_rnpoly_solve_cmplx(double a=0, double b=0) : R(a), C(b) {}
  inline double norm() const { return R*R+C*C; }
  inline vnl_rnpoly_solve_cmplx operator-() const { return vnl_rnpoly_solve_cmplx(-R, -C); }
  inline vnl_rnpoly_solve_cmplx operator+(vnl_rnpoly_solve_cmplx const& Y) const
                                { return vnl_rnpoly_solve_cmplx(R+Y.R, C+Y.C); }
  inline vnl_rnpoly_solve_cmplx operator-(vnl_rnpoly_solve_cmplx const& Y) const
                                { return vnl_rnpoly_solve_cmplx(R-Y.R, C-Y.C); }
  inline vnl_rnpoly_solve_cmplx& operator+=(vnl_rnpoly_solve_cmplx const& Y)
                                { R+=Y.R; C+=Y.C; return *this; }
  inline vnl_rnpoly_solve_cmplx& operator-=(vnl_rnpoly_solve_cmplx const& Y)
                                { R-=Y.R; C-=Y.C; return *this; }
  inline vnl_rnpoly_solve_cmplx operator*(vnl_rnpoly_solve_cmplx const& Y) const
                                { return vnl_rnpoly_solve_cmplx(R*Y.R-C*Y.C, R*Y.C+C*Y.R); }
  inline vnl_rnpoly_solve_cmplx operator/(vnl_rnpoly_solve_cmplx const& Y) const
                                { 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); }
  inline vnl_rnpoly_solve_cmplx operator*(double T) const
                                { return vnl_rnpoly_solve_cmplx(R*T, C*T); }
  inline vnl_rnpoly_solve_cmplx& operator*=(double T)
                                { R*=T; C*=T; return *this; }
  inline vnl_rnpoly_solve_cmplx& operator*=(vnl_rnpoly_solve_cmplx const& Y)
                                { double r=R*Y.R-C*Y.C; C=R*Y.C+C*Y.R; R=r; return *this; }
  inline vnl_rnpoly_solve_cmplx& operator/=(vnl_rnpoly_solve_cmplx const& Y)
                                { return *this = operator/(Y); }
};

static const double twopi = 6.2831853071795864769;

static const double epsilonB  = 2.e-03;
static const vnl_rnpoly_solve_cmplx  epsilonZ  = vnl_rnpoly_solve_cmplx(1.e-04,1.e-04);
static const double final_eps = 1.e-10;
static const double stepinit  = 1.e-02;


vcl_vector<vnl_vector<double>*> vnl_rnpoly_solve::realroots(double tol)
{
  tol *= tol; // squared tolerance
  vcl_vector<vnl_vector<double>*> rr;
  vcl_vector<vnl_vector<double>*>::iterator rp = r_.begin(), ip = i_.begin();
  for (; rp != r_.end() && ip != i_.end(); ++rp, ++ip) {
    if ((*ip)->squared_magnitude() < tol)
      rr.push_back(*rp);
  }
  return rr;
}


//------------------------- INPTBR ---------------------------
//: Initialize random variables
// This will initialize the random variables which are used
// to preturb the starting point so as to have measure zero
// probability that we will start at a singular point.
static void inptbr(int n, vnl_rnpoly_solve_cmplx p[M], vnl_rnpoly_solve_cmplx q[M])
{ vnl_rnpoly_solve_cmplx pp[10],qq[10];

  pp[0] = vnl_rnpoly_solve_cmplx(.12324754231,  .76253746298);
  pp[1] = vnl_rnpoly_solve_cmplx(.93857838950,  -.99375892810);
  pp[2] = vnl_rnpoly_solve_cmplx(-.23467908356, .39383930009);
  pp[3] = vnl_rnpoly_solve_cmplx(.83542556622,  -.10192888288);
  pp[4] = vnl_rnpoly_solve_cmplx(-.55763522521, -.83729899911);
  pp[5] = vnl_rnpoly_solve_cmplx(-.78348738738, -.10578234903);
  pp[6] = vnl_rnpoly_solve_cmplx(.03938347346,  .04825184716);
  pp[7] = vnl_rnpoly_solve_cmplx(-.43428734331, .93836289418);
  pp[8] = vnl_rnpoly_solve_cmplx(-.99383729993, -.40947822291);
  pp[9] = vnl_rnpoly_solve_cmplx(.09383736736,  .26459172298);

  qq[0] = vnl_rnpoly_solve_cmplx(.58720452864,  .01321964722);
  qq[1] = vnl_rnpoly_solve_cmplx(.97884134700,  -.14433009712);
  qq[2] = vnl_rnpoly_solve_cmplx(.39383737289,  .4154322311);
  qq[3] = vnl_rnpoly_solve_cmplx(-.03938376373, -.61253112318);
  qq[4] = vnl_rnpoly_solve_cmplx(.39383737388,  -.26454678861);
  qq[5] = vnl_rnpoly_solve_cmplx(-.0093837766,  .34447867861);
  qq[6] = vnl_rnpoly_solve_cmplx(-.04837366632, .48252736790);
  qq[7] = vnl_rnpoly_solve_cmplx(.93725237347,  -.54356527623);
  qq[8] = vnl_rnpoly_solve_cmplx(.39373957747,  .65573434564);
  qq[9] = vnl_rnpoly_solve_cmplx(-.39380038371, .98903450052);

  for (int j=n-1;j>=0;--j) { int jj=j%10; p[j]=pp[jj]; q[j]=qq[jj]; }
}

//-----------------------------  POWR  -----------------------
//: This returns the complex number y raised to the nth degree
static inline vnl_rnpoly_solve_cmplx powr(int n,vnl_rnpoly_solve_cmplx const& y)
{
    vnl_rnpoly_solve_cmplx x (1,0);
    if (n>0) while (n--) x *= y;
    else     while (n++) x /= y;
    return x;
}


static void initr(int n,int ideg[M], vnl_rnpoly_solve_cmplx p[M], vnl_rnpoly_solve_cmplx q[M],
                  vnl_rnpoly_solve_cmplx r[M], vnl_rnpoly_solve_cmplx pdg[M], vnl_rnpoly_solve_cmplx qdg[M])
{
  for (int j=0;j<n;j++)
   { pdg[j] = powr(ideg[j],p[j]);
     qdg[j] = powr(ideg[j],q[j]);
     r[j] = q[j] / p[j];
   }
}


//-------------------------------- DEGREE -------------------------------
//: This will compute the degree of the polynomial based upon the index.
static inline int degree(int index)
{
 return (index<0) ? 0 : (index % P) + 1;
}


//-------------------------- FFUNR -------------------------
//: Evaluate the target system component of h.
//  This is the system of equations that we are trying to find the roots.
static void ffunr(double coeff[M][T], int polyn[M][T][M], int n,
                  int terms[M], vnl_rnpoly_solve_cmplx x[M], vnl_rnpoly_solve_cmplx pows[M*P],
                  int max_deg, vnl_rnpoly_solve_cmplx f[M], vnl_rnpoly_solve_cmplx df[M][M])
{ int i,k,l,deg;
  vnl_rnpoly_solve_cmplx *df_il_ptr;

  // Compute all possible powers for each variable
  for (i=0;i<n;i++) { // for all variables
    int index = P*i;
    pows[index]=x[i];
    for (int j=1;j<max_deg;++j,++index) { // for all powers
      pows[index+1]= pows[index] * x[i];
    }}

  // Initialize the new arrays
  for (i=0;i<n;i++) {
    f[i]=vnl_rnpoly_solve_cmplx(0,0);
    for (int j=0;j<n;j++)
      df[i][j]=vnl_rnpoly_solve_cmplx(0,0);
  }

  for (i=n-1;i>=0;i--) // Across equations
    for (int j=terms[i]-1;j>=0;j--) { // Across terms
      vnl_rnpoly_solve_cmplx tmp (1,0);
      for (k=n-1;k>=0;k--) { // For each variable
        int index=polyn[i][j][k];
        if (index>=0)
          tmp *= pows[index];
      }
      f[i] += tmp * coeff[i][j];
    }

  // Compute the Derivative!
  for (i=n-1;i>=0;i--) // Over equations
    for (l=n-1;l>=0;l--) { // With respect to each variable
      df_il_ptr = &df[i][l];
      for (int j=terms[i]-1;j>=0;j--) // Over terms in each equation
        if (polyn[i][j][l]>=0) {      // if 0 deg in l, df term is 0
          vnl_rnpoly_solve_cmplx tmp = vnl_rnpoly_solve_cmplx(1,0);
          for (k=n-1;k>=0;k--) {      // Over each variable in each term
            int index=polyn[i][j][k];
            if (index>=0) {
              if (k==l) {
                deg = degree(index);
                if (deg > 1)
                  tmp *= pows[index-1];
                tmp *= (double)deg;
              } else
                tmp *= pows[index];
            }
          } // end for k
          *df_il_ptr += tmp * coeff[i][j];
        }
    } // end for l
}


//--------------------------- GFUNR --------------------------
//: Evaluate starting system component
// Evaluate the starting system component of h from a system
// of equations that we already know the roots. (ex: x^n - 1)
static void gfunr(int len, int ideg[M], vnl_rnpoly_solve_cmplx pdg[M], vnl_rnpoly_solve_cmplx qdg[M],
                  vnl_rnpoly_solve_cmplx /*x*/ [M], vnl_rnpoly_solve_cmplx pows[M*P],
                  vnl_rnpoly_solve_cmplx g[M], vnl_rnpoly_solve_cmplx dg[M])
{ int j;
  vnl_rnpoly_solve_cmplx pxdgm1[M], pxdg[M];
  vnl_rnpoly_solve_cmplx tmp;

  for (j=0;j<len;j++) {
    if (ideg[j] == 1)
      tmp = vnl_rnpoly_solve_cmplx(1,0);
    else
      tmp = pows[j*P+(ideg[j]-2)];

    pxdgm1[j] = pdg[j] * tmp;
  }

  for (j=0;j<len;j++) {
    int index = j*P+(ideg[j]-1);
    pxdg[j] = pdg[j] * pows[index];
  }

  for (j=len-1;j>=0;j--) {
    g[j]   = pxdg[j] - qdg[j];
    dg[j]  = pxdgm1[j] * ideg[j];
  }
}


//-------------------------- HFUNR --------------------------
//: This is the routine that traces the curve from the gfunr to the f function
//  (i.e. Evaluate the continuation function)
static void hfunr(int len,int ideg[M], vnl_rnpoly_solve_cmplx pdg[M], vnl_rnpoly_solve_cmplx qdg[M],
                  double t, vnl_rnpoly_solve_cmplx x[M], vnl_rnpoly_solve_cmplx h[M], vnl_rnpoly_solve_cmplx dhx[M][M],
                  vnl_rnpoly_solve_cmplx dht[M], int polyn[M][T][M], double coeff[M][T],
                  int terms[M],int max_deg)
{
  vnl_rnpoly_solve_cmplx df[M][M],dg[M],f[M],g[M];
  vnl_rnpoly_solve_cmplx pows[M*P];  //  powers of variables [M equations] [P possible powers]
  vnl_rnpoly_solve_cmplx *dhx_ptr, *end_ptr, *df_jk_ptr;

  ffunr(coeff,polyn,len,terms,x,pows,max_deg,f,df);
  gfunr(len,ideg,pdg,qdg,x,pows,g,dg);

  double onemt=1.0 - t;
  for (int j=0;j<len;j++) {
    end_ptr = &dhx[j][len]; df_jk_ptr= &df[j][0];
    for (dhx_ptr = &dhx[j][0]; dhx_ptr< end_ptr;dhx_ptr++,df_jk_ptr++)
      (*dhx_ptr) = (*df_jk_ptr) * t;

    dhx[j][j] += dg[j]*onemt;
    dht[j] = f[j] - g[j];
    h[j] = f[j] * t + g[j] * onemt;
  }
}


//------------------------ LU DECOMPOSITION --------------------------
//: This performs LU decomposition on a matrix.
static int ludcmp(vnl_rnpoly_solve_cmplx a[M][M], int n, int indx[M])
{
  int imax = 0;
  double rdum,temp;
  double vv[M];
  vnl_rnpoly_solve_cmplx *a_ij_ptr, *a_ik_ptr, *a_kj_ptr, *a_jk_ptr;

  // Loop over rows to get the implicit scaling information
  for (int i=0;i<n;i++) {
    double big = 0.0;
    vnl_rnpoly_solve_cmplx *endptr = &a[i][0] + n;
    for (vnl_rnpoly_solve_cmplx *aptr=&a[i][0]; aptr<endptr; ++aptr)
      if ((temp=aptr->norm()) > big)
        big = temp;
    if (big == 0.0) return 1;
    vv[i]=1.0/vcl_sqrt(big);}

  // This is the loop over columns of Crout's method
  for (int j=0;j<n;++j) {
    a_ij_ptr = &a[0][j];

    {for (int i=0;i<j;++i,a_ij_ptr+=M) {
      a_ik_ptr = &a[i][0];
      a_kj_ptr = &a[0][j];
      for (int k=0;k<i;++k,++a_ik_ptr,a_kj_ptr+=M)
        (*a_ij_ptr) -= (*a_ik_ptr) * (*a_kj_ptr);
    }}

    // Initialize for the search for largest pivot element
    double big = 0.0;
    a_ij_ptr= &a[j][j];

    for (int i=j;i<n;++i,a_ij_ptr+=M) {
      a_ik_ptr = &a[i][0];
      a_kj_ptr = &a[0][j];
      for (int k=0;k<j;++k,++a_ik_ptr,a_kj_ptr+=M)
        (*a_ij_ptr) -= (*a_ik_ptr) * (*a_kj_ptr);

      // Is the figure of merit for the pivot better than the best so far?
      if ((rdum=vv[i]*a_ij_ptr->norm()) >= big) { big = rdum; imax = i; }
    }

    // Do we need to interchange rows?
    if (j != imax) {
      // Yes, do so...
      vnl_rnpoly_solve_cmplx *endptr = &a[imax][0] + n;
      a_jk_ptr = &a[j][0];
      for (vnl_rnpoly_solve_cmplx *aptr=&a[imax][0];aptr<endptr;++aptr,++a_jk_ptr) {
        vnl_rnpoly_solve_cmplx dum = *aptr; *aptr=*a_jk_ptr; *a_jk_ptr = dum;}

      // Also interchange the scale factor
      vv[imax]=vv[j]; }
    indx[j]=imax;

    vnl_rnpoly_solve_cmplx* aptr = &a[j][j];
    if (aptr->norm() == 0.0)
      *aptr = epsilonZ;

    // Now, finally, divide by the pivot element
    if (j != (n-1)) {
      vnl_rnpoly_solve_cmplx dum = vnl_rnpoly_solve_cmplx(1,0) / (*aptr);

      // If the pivot element is zero the matrix is singular.
      a_ij_ptr=&a[j+1][j];
      for (int i=j+1;i<n;++i,a_ij_ptr+=M)
        (*a_ij_ptr) = (*a_ij_ptr) * dum;
    }
  }
  return 0;
}


// ------------------------- LU Back Subsitution -------------------------
static void lubksb(vnl_rnpoly_solve_cmplx a[M][M], int n, int indx[M],
                   vnl_rnpoly_solve_cmplx bb[M], vnl_rnpoly_solve_cmplx b[M])
{ int i;
  int ii=-1;
  vnl_rnpoly_solve_cmplx *bbptr = &bb[0];
  vnl_rnpoly_solve_cmplx *endptr = &b[0] + n;
  for (vnl_rnpoly_solve_cmplx* bptr= &b[0]; bptr < endptr; bptr++,++bbptr)
    *bptr= *bbptr;

  for (i=0;i<n;i++) {
    int ip = indx[i];
    vnl_rnpoly_solve_cmplx sum = b[ip];
    b[ip] = b[i];

    if (ii>=0)
      for (int j=ii;j<i;++j)
        sum -= a[i][j] * b[j];
    else
      // A nonzero element was encountered, so from now on we
      // will have to do the sums in the loop above
      if (sum.norm() > 0) ii = i;

    b[i] = sum;
  }

  // Now do the backsubstitution
  for (i=n-1;i>=0;i--) {
    vnl_rnpoly_solve_cmplx *endptr = &b[n];
    vnl_rnpoly_solve_cmplx *a_ij_ptr = &a[i][i+1];
    for (vnl_rnpoly_solve_cmplx *bptr=&b[i+1]; bptr<endptr; ++bptr,++a_ij_ptr)
      b[i] -= (*a_ij_ptr) * (*bptr);

    b[i] = b[i] / a[i][i];