laguerre.cpp

Digital filter designer s handbook C++ code source

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//
//  File = laguerre.cpp
//
//  Laguerre method for finding polynomial roots
//

#include <math.h>
#include <stdlib.h>
#include <iostream.h>
#include <fstream.h>
#include "d_cmplx.h"
#include "laguerre.h"
extern ofstream DebugFile;

int LaguerreMethod( int order,
                    double_complex *coef,
                    double_complex *root_ptr,
                    double epsilon,
                    double epsilon2,
                    int max_iter)
{
int iter, j;
double_complex p_eval, p_prime, p_double_prime;
double_complex root, f, f_sqrd, g, radical;
double_complex f_plus_rad, f_minus_rad, delta_root;
double old_delta_mag, root_mag, error;

root = *root_ptr;
old_delta_mag = 1000.0;

for(iter=1; iter<=max_iter; iter++)
  {
  p_double_prime = double_complex(0.0,0.0);
  p_prime = double_complex(0.0,0.0);
  p_eval = coef[order];
  error = mag(p_eval);
  root_mag = mag(root);

  for( j=order-1; j>=0; j--)
    {
    //DebugFile << "trial root = " << root << endl;
    p_double_prime = p_prime + root * p_double_prime;
    p_prime = p_eval + root * p_prime;
    p_eval = coef[j] + root * p_eval;
    error = mag(p_eval) + root_mag * error;
    }
  error = epsilon2 * error;
  p_double_prime = 2.0 * p_double_prime;
  //DebugFile << "P(root) = " << p_eval << endl;
  //DebugFile << "P'(root) = " << p_prime << endl;
  //DebugFile << "P''(root) = " << p_double_prime << endl;
  //DebugFile << "error = " << error << endl;

  if(mag(p_eval) < error)
    {
    *root_ptr = root;
    return(1);
    }
  f = p_prime/p_eval;
  //DebugFile << "F = " << f << endl;
  f_sqrd = f * f;
  //DebugFile << "F**2 = " << f_sqrd << endl;
  g = f_sqrd - p_double_prime/p_eval;
  //DebugFile << "G = " << g << endl;
  radical = (order-1)*(order * g - f_sqrd);
  //DebugFile << "argument of sqrt = " << radical << endl;
  radical = sqrt(radical);
  //DebugFile << "result of sqrt = " << radical << endl;
  f_plus_rad = f + radical;
  f_minus_rad = f - radical;
  if( mag(f_plus_rad) > mag(f_minus_rad) )
    {
    delta_root = double_complex(double(order), 0.0) /
                 f_plus_rad;
    //DebugFile << "using F + rad, delta_root = " 
    //          << delta_root << endl;
    }
  else
    {
    delta_root = double_complex(double(order), 0.0) /
                 f_minus_rad;
    //DebugFile << "using F - rad, delta_root = " 
    //          << delta_root << endl;
    }
  root = root - delta_root;
  //DebugFile << "root changed to " << root << endl;
  if( (iter > 6) && (mag(delta_root) > old_delta_mag) )
    {
    *root_ptr = root;
    return(2);
    }
  if( mag(delta_root) < (epsilon * mag(root)))
    {
    *root_ptr = root;
    return(3);
    }
  old_delta_mag = mag(delta_root);
  }
DebugFile << "Laguerre method failed to converge" << endl;
return(-1);
}