poly_roots.cc

Go to the documentation of this file.

/* Copyright (C) 2002-2012 Oliver Lemke <olemke@core-dump.info>
 
   This program is free software; you can redistribute it and/or modify it
   under the terms of the GNU General Public License as published by the
   Free Software Foundation; either version 2, or (at your option) any
   later version.
 
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
 
   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
   USA. */
 
 
//   File description
 
//   External declarations
 
#include <iostream>
#include <cmath>
#include <stdexcept>
#include "poly_roots.h"
 
 
/* C-style matrix elements */
#define MAT(m,i,j,n) ((m)[(i)*(n) + (j)])
 
/* Fortran-style matrix elements */
#define FMAT(m,i,j,n) ((m)[((i)-1)*(n) + ((j)-1)])
 
 
#define GSL_DBL_EPSILON        2.2204460492503131e-16
 
#define RADIX 2
#define RADIX2 (RADIX*RADIX)
 
#define GSL_SUCCESS 0
#define GSL_FAILURE -1
#define GSL_EINVAL 4
#define GSL_EFAILED 5
 
#define GSL_SET_COMPLEX_PACKED(zp,n,x,y) do {*((zp)+2*(n))=(x); *((zp)+(2*(n)+1))=(y);} while(0)
 
 
typedef double *gsl_complex_packed_ptr ;
 
typedef struct {
      size_t nc;
      double * matrix;
} gsl_poly_complex_workspace ;
 
 
/* Begin Internal GSL function prototypes */
 
static gsl_poly_complex_workspace *
gsl_poly_complex_workspace_alloc (size_t n);
 
static void
gsl_poly_complex_workspace_free (gsl_poly_complex_workspace * w);
 
static void
set_companion_matrix (const double *a, size_t n, double *m);
 
static int
qr_companion (double *h, size_t nc, gsl_complex_packed_ptr z);
 
static void
balance_companion_matrix (double *m, size_t n);
 
static int
gsl_poly_complex_solve (const double *a, size_t n,
                        gsl_poly_complex_workspace * w,
                        gsl_complex_packed_ptr z);
 
/* End Internal GSL function prototypes */
 
 
int
poly_root_solve (Matrix& roots, Vector& coeffs)
{
  Index a;
  double *c;
  double *s;
 
  a = coeffs.nelem();
 
  assert (roots.nrows() == a - 1);
  assert (roots.ncols() == 2);
  assert (coeffs[a - 1] != 0);
 
#ifdef USE_DOUBLE
  c = coeffs.mdata;
  s = roots.mdata;
#else
  c = (double *)malloc (a * sizeof (double));
  for (Index i = 0; i < a; i++)
    c[i] = (double)coeffs[i];
 
  s = (double *)malloc ((a-1) * 2 * sizeof (double));
#endif
 
  gsl_poly_complex_workspace *w = gsl_poly_complex_workspace_alloc (a);
 
  int status = gsl_poly_complex_solve (c, a, w, s);
 
#ifndef USE_DOUBLE
  if (status == GSL_SUCCESS)
    {
      for (Index i = 0; i < a - 1; i++)
        {
          roots (i, 0) = (Numeric)s[i * 2];
          roots (i, 1) = (Numeric)s[i * 2 + 1];
        }
    }
 
  free (c);
  free (s);
#endif
 
  gsl_poly_complex_workspace_free (w);
 
  return status;
}
 
 
static gsl_poly_complex_workspace *
gsl_poly_complex_workspace_alloc (size_t n)
{
  size_t nc ;
 
  gsl_poly_complex_workspace * w ;
 
  if (n == 0)
    {
      cerr << "matrix size n must be positive integer" << endl;
 
      return (NULL);
    }
 
  w = (gsl_poly_complex_workspace *)
    malloc (sizeof(gsl_poly_complex_workspace));
 
  if (w == 0)
    {
      cerr << "failed to allocate space for struct" << endl;
 
      return (NULL);
    }
 
  nc = n - 1;
 
  w->nc = nc;
 
  w->matrix = (double *) malloc (nc * nc * sizeof(double));
 
  if (w->matrix == 0)
    {
      free (w) ;       /* error in constructor, avoid memory leak */
 
      cerr << "failed to allocate space for workspace matrix" << endl;
 
      return (NULL);
    }
 
  return w ;
}
 
 
static void
gsl_poly_complex_workspace_free (gsl_poly_complex_workspace * w)
{
  free(w->matrix) ;
  free(w);
}
 
 
static void
balance_companion_matrix (double *m, size_t nc)
{
  int not_converged = 1;
 
  double row_norm = 0;
  double col_norm = 0;
 
  while (not_converged)
    {
      size_t i, j;
      double g, f, s;
 
      not_converged = 0;
 
      for (i = 0; i < nc; i++)
        {
          /* column norm, excluding the diagonal */
 
          if (i != nc - 1)
            {
              col_norm = fabs (MAT (m, i + 1, i, nc));
            }
          else
            {
              col_norm = 0;
 
              for (j = 0; j < nc - 1; j++)
                {
                  col_norm += fabs (MAT (m, j, nc - 1, nc));
                }
            }
 
          /* row norm, excluding the diagonal */
 
          if (i == 0)
            {
              row_norm = fabs (MAT (m, 0, nc - 1, nc));
            }
          else if (i == nc - 1)
            {
              row_norm = fabs (MAT (m, i, i - 1, nc));
            }
          else
            {
              row_norm = (fabs (MAT (m, i, i - 1, nc)) 
                          + fabs (MAT (m, i, nc - 1, nc)));
            }
 
          if (col_norm == 0 || row_norm == 0)
            {
              continue;
            }
 
          g = row_norm / RADIX;
          f = 1;
          s = col_norm + row_norm;
 
          while (col_norm < g)
            {
              f *= RADIX;
              col_norm *= RADIX2;
            }
 
          g = row_norm * RADIX;
 
          while (col_norm > g)
            {
              f /= RADIX;
              col_norm /= RADIX2;
            }
 
          if ((row_norm + col_norm) < 0.95 * s * f)
            {
              not_converged = 1;
 
              g = 1 / f;
 
              if (i == 0)
                {
                  MAT (m, 0, nc - 1, nc) *= g;
                }
              else
                {
                  MAT (m, i, i - 1, nc) *= g;
                  MAT (m, i, nc - 1, nc) *= g;
                }
 
              if (i == nc - 1)
                {
                  for (j = 0; j < nc; j++)
                    {
                      MAT (m, j, i, nc) *= f;
                    }
                }
              else
                {
                  MAT (m, i + 1, i, nc) *= f;
                }
            }
        }
    }
}
 
 
static int
qr_companion (double *h, size_t nc, gsl_complex_packed_ptr zroot)
{
  double t = 0.0;
 
  size_t iterations, e, i, j, k, m;
 
  double w, x, y, s, z;
 
  double p = 0, q = 0, r = 0; 
 
  /* FIXME: if p,q,r, are not set to zero then the compiler complains
     that they ``might be used uninitialized in this
     function''. Looking at the code this does seem possible, so this
     should be checked. */
 
  int notlast;
 
  size_t n = nc;
 
next_root:
 
  if (n == 0)
    return GSL_SUCCESS ;
 
  iterations = 0;
 
next_iteration:
 
  for (e = n; e >= 2; e--)
    {
      double a1 = fabs (FMAT (h, e, e - 1, nc));
      double a2 = fabs (FMAT (h, e - 1, e - 1, nc));
      double a3 = fabs (FMAT (h, e, e, nc));
 
      if (a1 <= GSL_DBL_EPSILON * (a2 + a3))
        break;
    }
 
  x = FMAT (h, n, n, nc);
 
  if (e == n)
    {
      GSL_SET_COMPLEX_PACKED (zroot, n-1, x + t, 0); /* one real root */
      n--;
      goto next_root;
      /*continue;*/
    }
 
  y = FMAT (h, n - 1, n - 1, nc);
  w = FMAT (h, n - 1, n, nc) * FMAT (h, n, n - 1, nc);
 
  if (e == n - 1)
    {
      p = (y - x) / 2;
      q = p * p + w;
      y = sqrt (fabs (q));
 
      x += t;
 
      if (q > 0)    /* two real roots */
        {
          if (p < 0)
            y = -y;
          y += p;
 
          GSL_SET_COMPLEX_PACKED (zroot, n-1, x - w / y, 0);
          GSL_SET_COMPLEX_PACKED (zroot, n-2, x + y, 0);
        }
      else
        {
          GSL_SET_COMPLEX_PACKED (zroot, n-1, x + p, -y);
          GSL_SET_COMPLEX_PACKED (zroot, n-2, x + p, y);
        }
      n -= 2;
 
      goto next_root;
      /*continue;*/
    }
 
  /* No more roots found yet, do another iteration */
 
  if (iterations == 60)  /* increased from 30 to 60 */
    {
      /* too many iterations - give up! */
 
      return GSL_FAILURE ;
    }
 
  if (iterations % 10 == 0 && iterations > 0)
    {
      /* use an exceptional shift */
 
      t += x;
      for (i = 1; i <= n; i++)
        {
          FMAT (h, i, i, nc) -= x;
        }
 
      s = fabs (FMAT (h, n, n - 1, nc)) + fabs (FMAT (h, n - 1, n - 2, nc));
      y = 0.75 * s;
      x = y;
      w = -0.4375 * s * s;
    }
 
  iterations++;
 
  for (m = n - 2; m >= e; m--)
    {
      double a1, a2, a3;
 
      z = FMAT (h, m, m, nc);
      r = x - z;
      s = y - z;
      p = FMAT (h, m, m + 1, nc) + (r * s - w) / FMAT (h, m + 1, m, nc);
      q = FMAT (h, m + 1, m + 1, nc) - z - r - s;
      r = FMAT (h, m + 2, m + 1, nc);
      s = fabs (p) + fabs (q) + fabs (r);
      p /= s;
      q /= s;
      r /= s;
 
      if (m == e)
        break;
 
      a1 = fabs (FMAT (h, m, m - 1, nc));
      a2 = fabs (FMAT (h, m - 1, m - 1, nc));
      a3 = fabs (FMAT (h, m + 1, m + 1, nc));
 
      if (a1 * (fabs (q) + fabs (r)) <= GSL_DBL_EPSILON * fabs (p) * (a2 + a3))
        break;
    }
 
  for (i = m + 2; i <= n; i++)
    {
      FMAT (h, i, i - 2, nc) = 0;
    }
 
  for (i = m + 3; i <= n; i++)
    {
      FMAT (h, i, i - 3, nc) = 0;
    }
 
  /* double QR step */
 
  for (k = m; k <= n - 1; k++)
    {
      notlast = (k != n - 1);
 
      if (k != m)
        {
          p = FMAT (h, k, k - 1, nc);
          q = FMAT (h, k + 1, k - 1, nc);
          r = notlast ? FMAT (h, k + 2, k - 1, nc) : 0.0;
 
          x = fabs (p) + fabs (q) + fabs (r);
 
          if (x == 0)
            continue;   /* FIXME????? */
 
          p /= x;
          q /= x;
          r /= x;
        }
 
      s = sqrt (p * p + q * q + r * r);
 
      if (p < 0)
        s = -s;
 
      if (k != m)
        {
          FMAT (h, k, k - 1, nc) = -s * x;
        }
      else if (e != m)
        {
          FMAT (h, k, k - 1, nc) *= -1;
        }
 
      p += s;
      x = p / s;
      y = q / s;
      z = r / s;
      q /= p;
      r /= p;
 
      /* do row modifications */
 
      for (j = k; j <= n; j++)
        {
          p = FMAT (h, k, j, nc) + q * FMAT (h, k + 1, j, nc);
 
          if (notlast)
            {
              p += r * FMAT (h, k + 2, j, nc);
              FMAT (h, k + 2, j, nc) -= p * z;
            }
 
          FMAT (h, k + 1, j, nc) -= p * y;
          FMAT (h, k, j, nc) -= p * x;
        }
 
      j = (k + 3 < n) ? (k + 3) : n;
 
      /* do column modifications */
 
      for (i = e; i <= j; i++)
        {
          p = x * FMAT (h, i, k, nc) + y * FMAT (h, i, k + 1, nc);
 
          if (notlast)
            {
              p += z * FMAT (h, i, k + 2, nc);
              FMAT (h, i, k + 2, nc) -= p * r;
            }
          FMAT (h, i, k + 1, nc) -= p * q;
          FMAT (h, i, k, nc) -= p;
        }
    }
 
  goto next_iteration;
}
 
 
static void
set_companion_matrix (const double *a, size_t nc, double *m)
{
  size_t i, j;
 
  for (i = 0; i < nc; i++)
    for (j = 0; j < nc; j++)
      MAT (m, i, j, nc) = 0.0;
 
  for (i = 1; i < nc; i++)
    MAT (m, i, i - 1, nc) = 1.0;
 
  for (i = 0; i < nc; i++)
    MAT (m, i, nc - 1, nc) = -a[i] / a[nc];
}
 
 
static int
gsl_poly_complex_solve (const double *a, size_t n,
                        gsl_poly_complex_workspace * w,
                        gsl_complex_packed_ptr z)
{
  int status;
  double *m;
 
  if (n == 0)
    {
      cerr << "number of terms must be a positive integer" << endl;
 
      return (GSL_FAILURE);
    }
 
  if (n == 1)
    {
      cerr << "cannot solve for only one term" << endl;
 
      return (GSL_FAILURE);
    }
 
  if (a[n - 1] == 0)
    {
      cerr << "leading term of polynomial must be non-zero" << endl;
 
      return (GSL_FAILURE);
    }
 
  if (w->nc != n - 1)
    {
      cerr << "size of workspace does not match polynomial" << endl;
 
      return (GSL_FAILURE);
    }
 
  m = w->matrix;
 
 
  set_companion_matrix (a, n - 1, m);
 
  balance_companion_matrix (m, n - 1);
 
  status = qr_companion (m, n - 1, z);
 
  if (status)
    {
      //cerr << "root solving qr method failed to converge" << endl;
 
      return (GSL_FAILURE);
    }
 
  return GSL_SUCCESS;
}