This is pzsim.c in view mode; [Download] [Up]
/**********************************************************************
* This program calculates the magnitude of a transfer function
* given all the pole and zero locations. A scalar constant
* K can also be entered.
* Steven Punte
* Sept. 24 1987
***********************************************************************/
#include <stdio.h>
#include <math.h>
#define TRUE 1
#define FALSE 0
#define MAX_ARRAY 50 /* Max number of poles or zeros */
#define PIE 3.141592653
#define STR_LNGTH 20
#define scanf my_scanf
#define abs(X) ( X > 0 ? X : -X )
#define sqr(X) ( X )*( X )
#define scal(X, Y) while( abs( X ) > 1000) { X *= 0.0001; Y += 4; } \
while( abs( X ) < 0.0001) { X *= 10000; Y -= 4; }
#define clear(X) X.real = 0; X.imag = 0
#define cmov(X, Y) Y.real = X.real; Y.imag = X.imag
#define cset(X, Y, Z) X.real = Y; X.imag = Z
#define cmpl(X) X.imag = - X.imag
#define cadd(X, Y, Z) Z.real = X.real + Y.real;\
Z.imag = X.imag + Y.imag
#define csub(X, Y, Z) Z.real = X.real - Y.real;\
Z.imag = X.imag - Y.imag
#define cmul(X, Y, Z) { \
struct cmplx_num tmp; \
tmp.real = X.real * Y.real - X.imag * Y.imag ; \
tmp.imag = X.real * Y.imag + X.imag * Y.real ; \
Z.real = tmp.real; \
Z.imag = tmp.imag; \
}
#define cdiv(X, Y, Z) { \
struct cmplx_num tmp; \
double den; \
den = sqr( Y.real ) + sqr( Y.imag ); \
tmp.real = X.real * Y.real + X.imag * Y.imag ; \
tmp.imag = X.imag * Y.real - X.real * Y.imag ; \
Z.imag = tmp.imag / den; \
Z.real = tmp.real / den; \
}
#define cmag(X) sqrt( sqr( X.real) + sqr( X.imag))
struct cmplx_num {
double real;
double imag;
};
int flag_S = FALSE;
int flag_E = FALSE;
int flag_PT = FALSE;
int flag_LOG = FALSE;
int flag_RADS = FALSE;
int flag_PHASE = FALSE;
int flag_STEP = FALSE;
double start; /* Starting Frequency */
double endd; /* Ending Frequency */
int points; /* Number of partitions */
double K_coeff = 1.0; /* Front Scaler coeffficient (1.0 default value) */
int K_exp = 0;
int numb_poles = 0; /* Number of poles (either real or complex) */
int numb_zeros = 0; /* Number of zeros (either real or complex) */
int i; /* Index for summation calculations */
char str[ STR_LNGTH ] ; /* String buffer for input reading */
int data_set = 0; /* Number of sets of data to be processed */
double ctemp; /* Used for complex divide */
struct cmplx_num poles[ MAX_ARRAY ];
struct cmplx_num zeros[ MAX_ARRAY ];
struct {
double x[500];
double y[500];
int numb_point;
char name[ STR_LNGTH ];
} output[ 5 ];
main()
{
int done_chk;
do {
done_chk = read_set();
check_input();
if( flag_STEP == TRUE ) step_response();
else transfer_respones();
data_set++;
} while( done_chk != EOF );
display();
} /* End of main */
transfer_respones()
{
double delta; /* Frequency increment in hertz */
double x; /* Frequency being analysized in hertz */
double ampl;
double angle;
double gamma;
int pnt;
struct cmplx_num cgamma, tmp;
struct cmplx_num K_scale;
double scale_data();
gamma = scale_data();
cset( K_scale, K_coeff, 0);
delta = (endd - start) / points;
/* Main loop; Calculates magnitude at each frequency */
pnt = 0;
for( x = start; x < endd ; x += delta ) {
struct cmplx_num numerator; /* Numerator of magnitude calculation */
struct cmplx_num denominator; /* Denominator of mangitude calculation */
struct cmplx_num magnitude; /* Final resulting magnitude */
struct cmplx_num temp1, temp2;
struct cmplx_num omega;
if( flag_RADS == FALSE ) { cset( omega, 0, (2*PIE*x / gamma ) ); }
else { cset( omega, 0, ( x / gamma )); }
cset( numerator, 1, 0 );
cset( denominator, 1, 0 );
/* Main calculations here */
for( i = 0; i < numb_zeros ; i++ ) {
csub( omega, zeros[ i ], temp1 );
cmul( temp1, numerator, numerator);
}
for( i = 0; i < numb_poles ; i++ ) {
csub( omega, poles[ i ], temp1 );
cmul( temp1, denominator, denominator);
}
/* In case of underflow */
if( cmag( denominator) == 0.0 ) {
cset( magnitude, 100000000000000.0, 0.0 );
}
else {
cdiv( numerator, denominator, magnitude );
cmul( magnitude, K_scale, magnitude );
}
if( flag_PHASE == TRUE ) {
angle = 180 / PIE * atan2( magnitude.imag, magnitude.real );
/*
printf( "%15.5f %15.5f \n", x, angle );
*/
output[ data_set ].x[ pnt ] = x;
output[ data_set ].y[ pnt ] = angle;
pnt++;
}
else {
ampl = cmag( magnitude );
if( flag_LOG == TRUE ) {
if( ampl == 0.0 ) ampl = -1000;
else ampl = -20 * log10( ampl );
}
/*
printf( "%15.5f %15.5f \n", x, ampl );
*/
output[ data_set ].x[ pnt ] = x;
output[ data_set ].y[ pnt ] = ampl;
pnt++;
}
}
if( flag_PHASE == TRUE ) strcpy( output[ data_set ].name, "Phase" );
else strcpy( output[ data_set ].name, "Magnitude" );
output[ data_set ].numb_point = pnt;
} /* End of transfer response */
step_response()
{
double delta; /* Frequency increment in hertz */
double x; /* Frequency being analysized in hertz */
double gamma ;
struct cmplx_num C[ MAX_ARRAY ];
int k;
int pnt;
gamma = scale_data();
for( k = 0; k < numb_poles ; k++) {
struct cmplx_num numerator; /* Numerator of magnitude calculation */
struct cmplx_num denominator; /* Denominator of mangitude calculation */
struct cmplx_num magnitude; /* Final resulting magnitude */
struct cmplx_num temp1, temp2;
cset( numerator, K_coeff, 0 );
cset( denominator, 1, 0);
for( i = 0; i < numb_zeros ; i++ ) {
csub( poles[k], zeros[i], temp1 );
cmul( temp1, numerator, numerator );
}
for( i = 0; i < numb_poles ; i++ ) {
if( i != k) {
csub( poles[k], poles[i], temp1 );
cmul( temp1, denominator, denominator );
}
}
cdiv( numerator, denominator, C[k]);
}
delta = (endd - start) / points;
/* Main loop; Calculates magnitude at each frequency */
for( x = start; x < endd ; x += delta ) {
struct cmplx_num temp1, temp2, temp3, sum;
cset( sum, 0, 0);
for( k = 0; k < numb_poles ; k++) {
cset( temp1, ( x * gamma ), 0);
cmul( poles[k], temp1, temp2);
cset( temp1, cos( temp2.imag), sin( temp2.imag) );
cset( temp2, exp( temp2.real), 0);
cmul( temp1, temp2, temp3 );
cset( temp1, 1, 0);
csub( temp3, temp1, temp2);
cmul( temp2, C[k], temp3);
cdiv( temp3, poles[k], temp1);
cadd( sum, temp1, temp2);
cmov( temp2, sum);
}
/*
printf( "%15.5f %15.5f %f15.5 \n", x, sum.real, sum.imag );
printf( "%15.5f %15.5f \n", x, sum.real);
*/
output[ data_set ].x[ pnt ] = x;
output[ data_set ].y[ pnt ] = sum.real;
pnt++;
}
strcpy( output[ data_set ].name, "Step" );
output[ data_set ].numb_point = pnt;
}
/* Stack overflow error routing and exit */
overflow_error()
{
fprintf( stderr, "Internal Array Size has been exceeded \n");
fprintf( stderr, "Recompile with larger \"MAX_ARRAY\" size. \n");
fprintf( stderr, "Fatal Error \n");
exit(1);
}
/* Loops over all input lines, and enters data into proper variables */
read_set()
{
double scan_modifier();
int expon; /* The tens power exponent of the metric scale factor */
*str = EOF;
flag_LOG = FALSE;
flag_RADS = FALSE;
flag_PHASE = FALSE;
flag_STEP = FALSE;
K_coeff = 1.0; /* Front Scaler coeffficient (1.0 default value) */
numb_poles = 0; /* Number of poles (either real or complex) */
numb_zeros = 0; /* Number of poles (either real or complex) */
while( (scanf( "%s", str) != EOF) && ( *str != '#') ) {
if( !strcmp( str, "FS") || !strcmp( str, "TS") || !strcmp( str, "S" )) {
scanf( "%f", &start);
start *= scan_modifier( stdin, &expon );
flag_S = TRUE;
}
else if( !strcmp( str, "FE") || !strcmp( str, "TE") || !strcmp( str, "E" )) {
scanf( "%f", &endd);
endd *= scan_modifier( stdin, &expon );
flag_E = TRUE;
}
else if( !strcmp( str, "FP") || !strcmp( str, "TP") || !strcmp( str, "PT" )) {
scanf( "%d", &points);
points *= (int)scan_modifier( stdin, &expon );
flag_PT = TRUE;
}
else if( !strcmp( str, "K") ) {
scanf( "%f", &K_coeff);
scan_modifier( stdin, &expon );
K_exp = expon;
}
else if( !strcmp( str, "P") ) {
if( numb_poles >= MAX_ARRAY) overflow_error();
scanf( "%f", &poles[ numb_poles ].real);
poles[ numb_poles ].real *= scan_modifier( stdin, &expon );
poles[ numb_poles++ ].imag = 0.0;
}
else if( !strcmp( str, "Z") ) {
if( numb_poles >= MAX_ARRAY) overflow_error();
scanf( "%f", &zeros[ numb_zeros ].real);
zeros[ numb_zeros ].real *= scan_modifier( stdin, &expon );
zeros[ numb_zeros++ ].imag = 0.0;
}
else if( !strcmp( str, "PC") ) {
double real, imag, temp;
if( numb_poles >= MAX_ARRAY - 1) overflow_error();
/* Assumes as complex conjugate pair */
scanf( "%f", &real);
scanf( "%f", &imag);
temp = scan_modifier( stdin, &expon );
real *= temp;
imag *= temp;
poles[ numb_poles ].real = real;
poles[ numb_poles++ ].imag = imag;
/* Also enter complex pair */
poles[ numb_poles ].real = real;
poles[ numb_poles++ ].imag = - imag;
}
else if( !strcmp( str, "ZC") ) {
double real, imag, temp;
if( numb_poles >= MAX_ARRAY - 1) overflow_error();
/* Assumes as complex conjugate pair */
scanf( "%f", &real);
scanf( "%f", &imag);
temp = scan_modifier( stdin, &expon );
real *= temp;
imag *= temp;
zeros[ numb_zeros ].real = real;
zeros[ numb_zeros++ ].imag = imag;
/* Also enter complex pair */
zeros[ numb_zeros ].real = real;
zeros[ numb_zeros++ ].imag = - imag;
}
else if( !strcmp( str, "LOG") ) {
scan_modifier( stdin, &expon );
flag_LOG = TRUE;
}
else if( !strcmp( str, "RADS") ) {
scan_modifier( stdin, &expon );
flag_RADS = TRUE;
}
else if( !strcmp( str, "PHASE") ) {
scan_modifier( stdin, &expon );
flag_PHASE = TRUE;
}
else if( !strcmp( str, "STEP") ) {
scan_modifier( stdin, &expon );
flag_STEP = TRUE;
}
else {
fprintf( stderr, "Syntax Error, ");
fprintf( stderr, "Unrecognized input string \"%s\". ", str );
fprintf( stderr, "String ignored. \n");
}
}
if( *str != '#' ) *str = EOF ;
return( *str );
} /* End of read subroutine */
/* Checks if essential input informaiton is present */
check_input()
{
if( flag_S == FALSE ) {
fprintf( stderr, "Starting point unspecified! " );
fprintf( stderr, "Fatal Error. \n");
exit(1);
}
if( flag_E == FALSE ) {
fprintf( stderr, "Ending point unspecified! " );
fprintf( stderr, "Fatal Error. \n");
exit(1);
}
if( flag_PT == FALSE ) {
fprintf( stderr, "Number of data points unspecified! \n" );
fprintf( stderr, "Default of 20 will be used. \n");
points = 20;
}
}
/* Looks for some type of value modifie, such as kilo (x1000)
* or micro (x0.000001). Then will be expecting a colon as endd
* of line marker. Returns a double persion number for
* scaling purposes.
*/
double scan_modifier( input, expon )
FILE *input;
int *expon;
{
double scale = 1.0;
char string[ STR_LNGTH ];
while( (fscanf( input, "%s", string) != EOF) && (*string != ';' ) ) {
switch( *string) {
case 'K':
scale = 1000;
*expon = 3;
break;
case 'M':
scale = 1000000;
*expon = 6;
break;
case 'G':
scale = 1000000000;
*expon = 9;
break;
case 'T':
scale = 1000000000000;
*expon = 12;
break;
case 'm':
scale = 0.001;
*expon = -3;
break;
case 'u':
scale = 0.000001;
*expon = -6;
break;
case 'n':
scale = 0.000000001;
*expon = -9;
break;
case 'p':
scale = 0.000000000001;
*expon = -12;
break;
case 'E':
{
int K;
fscanf( input, "%d", &K);
*expon = K;
if( K > 30 ) K = 30;
if( K < -30) K = -30;
while( K-- > 0 ) scale *= 10;
while( K++ < 0 ) scale *= 0.1;
}
break;
default:
fprintf( stderr, "Unrecognized string \"%s\" \n", string);
fprintf( stderr, "Metric modifying unit expected. \n" );
fprintf( stderr, "String Ignored. \n");
}
}
return( scale );
}
display()
{
int i, j;
/*
printf("%d \n", data_set);
*/
for(i = 0; i < data_set ; i++ ) {
printf( "%d %s \n", output[i].numb_point, output[i].name );
printf( "%d \n", output[i].numb_point);
for(j = 0; j < output[ i ].numb_point ; j++) {
printf( "%15f, %15f \n", output[i].x[j], output[i].y[j]);
}
}
}
double scale_data()
{
double net_mean ;
int mean_order ;
double gamma, mag;
struct cmplx_num cgamma;
net_mean = 0.0;
mean_order = 0;
/* Find geometric mean of all non zeros and poles */
for( i = 0; i < numb_zeros ; i++ ) {
mag = cmag( zeros[i] );
if( mag > 0 ) {
net_mean += log( mag);
mean_order++;
}
}
for( i = 0; i < numb_poles ; i++ ) {
mag = cmag( poles[i] );
if( mag > 0 ) {
net_mean += log( mag);
mean_order++;
}
}
gamma = exp( net_mean / (double)mean_order );
/* Set complex scaling constant, and scale all poles and zeros */
cset( cgamma, (1 / gamma), 0);
for( i = 0; i < numb_zeros ; i++ ) {
cmul( zeros[i], cgamma, zeros[i]);
}
for( i = 0; i < numb_poles ; i++ ) {
cmul( poles[i], cgamma, poles[i]);
}
/* Rescale K_coeff, note: care for overflow must be observed */
K_coeff = exp( (double)(numb_zeros - numb_poles)*log( gamma ) + log( K_coeff ) + 2.30258509 * (double)K_exp );
return( gamma );
} /* End of data scale routine */
#undef scanf
/* A lousy fucking patch for micro-soft C. It doesn't
* seem to read floating point numbers properly.
*/
my_scanf( str, reg)
char *str;
union {
int x;
double y;
char t;
} *reg;
{
double atof();
if( !strcmp( str, "%f") ) {
char temp[40];
scanf( "%s", temp);
reg->y = atof( temp);
}
else scanf( str, reg);
}
These are the contents of the former NiCE NeXT User Group NeXTSTEP/OpenStep software archive, currently hosted by Netfuture.ch.