stpinvar.cpp

Digital filter designer s handbook C++ code source

//
//  File = stpinvar.cpp
//

#include "stpinvar.h"

void StepInvar( double_complex *pole,
                int num_poles,
                double_complex *zero,
                int num_zeros,
                double h_sub_zero,
                double big_t,
                double_complex *a,
                double_complex *b)
{
int k, n, j, max_coef;
double_complex *delta, *big_a;
double_complex beta, denom, numer, work2;

delta = new double_complex[num_poles+1];
big_a = new double_complex[num_poles+1];

for(j=0; j<=num_poles; j++) {
	delta[j] = double_complex(0.0,0.0);
	a[j] = double_complex(0.0,0.0);
	b[j] = double_complex(0.0,0.0);
	}
pole[0] = double_complex(0.0,0.0);

//---------------------------------------------------
//  compute partial fraction expansion coefficients

for( k=0; k<=num_poles; k++)
  {
	numer = double_complex(h_sub_zero,0.0);
	for(n=1; n<=num_zeros; n++) 
		{
    //numer = cMult(numer, cSub(pole[n], zero[n]));
    numer *= (pole[n] - zero[n]);
    }
	denom = double_complex(1.0,0.0);
	for( n=0; n<=num_poles; n++)
    {
		if(n==k) continue;
		  //denom = cMult(denom, cSub(pole[k],pole[n]));
      denom *= (pole[k] - pole[n]);
		}
	//big_a[k] = cDiv(numer,denom);
  big_a[k] = numer/denom;
	}

//-------------------------------------
//  compute numerator coefficients 

for( k=1; k<=num_poles; k++)
  {
	delta[0] = double_complex(1.0, 0.0);
	for(n=1; n<=num_poles; n++)
		{delta[n] = double_complex(0.0,0.0);}
	max_coef = 0;
	for( n=0; n<=num_poles; n++) {
		if(n==k) continue;
		max_coef++;
		//beta = sMult(-1.0, cExp(sMult(big_t,pole[n])));
    beta = -cexp(big_t * pole[n]);

		for(j=max_coef; j>=1; j--) 
			{
      //delta[j] = cAdd( delta[j], cMult( beta, delta[j-1]));
      delta[j] += (beta * delta[j-1]);
      }
		}
	for( j=0; j<num_poles; j++) 
		{ 
    //b[j] = cAdd(b[j], cMult( big_a[k], delta[j]));
    b[j] += (big_a[k] * delta[j]);
    }

// multiply by 1-z**(-1) 
	beta = double_complex(-1.0,0.0);
	for(j=num_poles+1; j>=1; j--)
    {
    //b[j] = cAdd(b[j], cMult(beta, b[j-1]));
    b[j] += (beta * b[j-1]);
    }
	}

//-------------------------------------
//  compute denominator coefficients 

a[0] = double_complex(1.0,0.0);
for( n=1; n<=num_poles; n++)
  {
	//beta = sMult(-1.0, cExp(sMult(big_t,pole[n])));
  beta = -cexp(big_t * pole[n]);
	for( j=n; j>=1; j--)
		{
    //a[j] = cAdd( a[j], cMult( beta, a[j-1]));
    a[j] += (beta * a[j-1]);
    }
	}
for( j=1; j<=num_poles; j++)
	{
  //a[j] = sMult(-1.0,a[j]);
  a[j] = -a[j];
  }
return;
}