1 // Copyright 2007 "Gilles Degottex"
2
3 // This file is part of "Music"
4
5 // "Music" is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation; either version 2.1 of the License, or
8 // (at your option) any later version.
9 //
10 // "Music" is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program; if not, write to the Free Software
17 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18
19 #include "Filter.h"
20
21 // #include <iostream>
22 using namespace std;
23 #include <CppAddons/CAMath.h>
24 using namespace Math;
25 #include "Music.h"
26
27 #include <math.h>
28 #include <stdio.h>
29 #include <stdlib.h>
30
31 // DOESN'T WORK
32 // b = fir1(32,[0.00001 0.23]);
fir1_lowpass(int n,double cutoff)33 vector<double> Music::fir1_lowpass(int n, double cutoff)
34 {
35 vector<double> b(n);
36
37 int d = (n-1)/2;
38
39 for(size_t i=0; i<b.size(); i++)
40 b[i] = cutoff*sinc(cutoff*(i-d));
41
42 return b;
43 }
44
45 // DOESN'T WORK
fir1_highpass(int n,double cutoff)46 vector<double> Music::fir1_highpass(int n, double cutoff)
47 {
48 vector<double> b(n);
49
50 int d = (n-1)/2;
51
52 double s=0.0;
53 for(size_t i=0; i<b.size(); i++)
54 {
55 b[i] = cutoff*sinc(cutoff*(i-d));
56 s += b[i];
57 }
58
59 for(size_t i=0; i<b.size(); i++)
60 b[i] = -b[i];
61 b[d] = s+b[d];
62
63 return b;
64 }
65
66 // cutoff in ]0;0.5[ where 0.5 is the Nyquist frequency
fir1_bandpass(int n,double low_cutoff,double high_cutoff)67 vector<double> Music::fir1_bandpass(int n, double low_cutoff, double high_cutoff)
68 {
69 (void)low_cutoff;
70 (void)high_cutoff;
71
72 double *weights, *desired, *bands;
73 double *h;
74 int i;
75
76 bands = (double *)malloc(6 * sizeof(double));
77 weights = (double *)malloc(3 * sizeof(double));
78 desired = (double *)malloc(3 * sizeof(double));
79 h = (double *)malloc(1000 * sizeof(double));
80
81 desired[0] = 1;
82 desired[1] = 1;
83 desired[2] = 0;
84
85 weights[0] = 10;
86 weights[1] = 1;
87 weights[2] = 10;
88
89 bands[0] = 0;
90 bands[1] = 0.1;
91 bands[2] = 0.2;
92 bands[3] = 0.3;
93 bands[4] = 0.35;
94 bands[5] = 0.5;
95 // bands[0] = 0;
96 // bands[1] = low_cutoff/2;
97 // bands[2] = low_cutoff;
98 // bands[3] = high_cutoff;
99 // bands[4] = 0.5*(high_cutoff+0.5);
100 // bands[5] = 0.5;
101
102 remez(h, n, 3, bands, desired, weights, BANDPASS);
103
104 vector<double> hv(n);
105
106 for (i=0; i<n; i++)
107 {
108 hv[i] = h[i];
109 // printf("%23.20f %23.20f\n", h[i], hv[i]);
110 }
111
112 free(bands);
113 free(weights);
114 free(desired);
115 free(h);
116
117 return hv;
118 }
119
FIRRTFilter(std::vector<double> & imp_res)120 Music::FIRRTFilter::FIRRTFilter(std::vector<double>& imp_res)
121 {
122 assert(imp_res.size()>0);
123
124 m_imp_res = imp_res;
125 // m_to_filter.reserve(imp_res.size());
126 }
127
operator ()(double v)128 double Music::FIRRTFilter::operator()(double v)
129 {
130 double value = 0.0;
131
132 m_to_filter.push_front(v);
133
134 if(m_to_filter.size()>=m_imp_res.size())
135 {
136 // convolve
137 for(size_t i=0; i<m_imp_res.size(); i++)
138 value += m_imp_res[i]*m_to_filter[i];
139
140 // if(decim++%4==0)
141 // m_queue.push_front(value);
142
143 // drop unused data
144 m_to_filter.pop_back();
145 }
146
147 return value;
148 }
149
RectangularHighPassRTFilter(int N)150 Music::RectangularHighPassRTFilter::RectangularHighPassRTFilter(int N)
151 {
152 reset(N);
153 }
reset(int N)154 void Music::RectangularHighPassRTFilter::reset(int N)
155 {
156 if(N<1) N=1;
157
158 m_N = N;
159 m_sum = 0.0;
160 m_summed_values.clear();
161 }
operator ()(double v)162 double Music::RectangularHighPassRTFilter::operator()(double v)
163 {
164 m_summed_values.push_front(v);
165 m_sum += v;
166 while(int(m_summed_values.size())>m_N)
167 {
168 m_sum -= m_summed_values.back();
169
170 m_summed_values.pop_back();
171 }
172
173 double fv;
174 fv = m_summed_values[m_summed_values.size()/2] - m_sum/m_summed_values.size();
175
176 return fv;
177 }
178
operator ()(double v)179 double Music::RectangularLowPassRTFilter::operator()(double v)
180 {
181 return v;
182 }
183
operator ()(double v)184 double Music::RectangularBandPassRTFilter::operator()(double v)
185 {
186 return v;
187 }
188
189 /*
190 LP and HP filter
191 Type : biquad, tweaked butterworthReferences : Posted by Patrice TarrabiaCode : www.musicdsp.org
192 r = rez amount, from sqrt(2) to ~ 0.1
193 f = cutoff frequency
194 (from ~0 Hz to SampleRate/2 - though many
195 synths seem to filter only up to SampleRate/4)
196
197 The filter algo:
198 out(n) = a1 * in + a2 * in(n-1) + a3 * in(n-2) - b1*out(n-1) - b2*out(n-2)
199
200 Lowpass:
201 c = 1.0 / tan(pi * f / sample_rate);
202
203 a1 = 1.0 / ( 1.0 + r * c + c * c);
204 a2 = 2* a1;
205 a3 = a1;
206 b1 = 2.0 * ( 1.0 - c*c) * a1;
207 b2 = ( 1.0 - r * c + c * c) * a1;
208
209 Hipass:
210 c = tan(pi * f / sample_rate);
211
212 a1 = 1.0 / ( 1.0 + r * c + c * c);
213 a2 = -2*a1;
214 a3 = a1;
215 b1 = 2.0 * ( c*c - 1.0) * a1;
216 b2 = ( 1.0 - r * c + c * c) * a1;
217 */
218
219 /**************************************************************************
220 * Parks-McClellan algorithm for FIR filter design (C version)
221 *-------------------------------------------------
222 * Copyright (c) 1995,1998 Jake Janovetz (janovetz@uiuc.edu)
223 *
224 * This library is free software; you can redistribute it and/or
225 * modify it under the terms of the GNU Library General Public
226 * License as published by the Free Software Foundation; either
227 * version 2 of the License, or (at your option) any later version.
228 *
229 * This library is distributed in the hope that it will be useful,
230 * but WITHOUT ANY WARRANTY; without even the implied warranty of
231 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
232 * Library General Public License for more details.
233 *
234 * You should have received a copy of the GNU Library General Public
235 * License along with this library; if not, write to the Free
236 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
237 *
238 *************************************************************************/
239
240
241 /*******************
242 * CreateDenseGrid
243 *=================
244 * Creates the dense grid of frequencies from the specified bands.
245 * Also creates the Desired Frequency Response function (D[]) and
246 * the Weight function (W[]) on that dense grid
247 *
248 *
249 * INPUT:
250 * ------
251 * int r - 1/2 the number of filter coefficients
252 * int numtaps - Number of taps in the resulting filter
253 * int numband - Number of bands in user specification
254 * double bands[] - User-specified band edges [2*numband]
255 * double des[] - Desired response per band [numband]
256 * double weight[] - Weight per band [numband]
257 * int symmetry - Symmetry of filter - used for grid check
258 *
259 * OUTPUT:
260 * -------
261 * int gridsize - Number of elements in the dense frequency grid
262 * double Grid[] - Frequencies (0 to 0.5) on the dense grid [gridsize]
263 * double D[] - Desired response on the dense grid [gridsize]
264 * double W[] - Weight function on the dense grid [gridsize]
265 *******************/
266
CreateDenseGrid(int r,int numtaps,int numband,double bands[],double des[],double weight[],int * gridsize,double Grid[],double D[],double W[],int symmetry)267 void CreateDenseGrid(int r, int numtaps, int numband, double bands[],
268 double des[], double weight[], int *gridsize,
269 double Grid[], double D[], double W[],
270 int symmetry)
271 {
272 int i, j, k, band;
273 double delf, lowf, highf;
274
275 delf = 0.5/(GRIDDENSITY*r);
276
277 /*
278 * For differentiator, hilbert,
279 * symmetry is odd and Grid[0] = max(delf, band[0])
280 */
281
282 if ((symmetry == NEGATIVE) && (delf > bands[0]))
283 bands[0] = delf;
284
285 j=0;
286 for (band=0; band < numband; band++)
287 {
288 Grid[j] = bands[2*band];
289 lowf = bands[2*band];
290 highf = bands[2*band + 1];
291 k = (int)((highf - lowf)/delf + 0.5); /* .5 for rounding */
292 for (i=0; i<k; i++)
293 {
294 D[j] = des[band];
295 W[j] = weight[band];
296 Grid[j] = lowf;
297 lowf += delf;
298 j++;
299 }
300 Grid[j-1] = highf;
301 }
302
303 /*
304 * Similar to above, if odd symmetry, last grid point can't be .5
305 * - but, if there are even taps, leave the last grid point at .5
306 */
307 if ((symmetry == NEGATIVE) &&
308 (Grid[*gridsize-1] > (0.5 - delf)) &&
309 (numtaps % 2))
310 {
311 Grid[*gridsize-1] = 0.5-delf;
312 }
313 }
314
315
316 /********************
317 * InitialGuess
318 *==============
319 * Places Extremal Frequencies evenly throughout the dense grid.
320 *
321 *
322 * INPUT:
323 * ------
324 * int r - 1/2 the number of filter coefficients
325 * int gridsize - Number of elements in the dense frequency grid
326 *
327 * OUTPUT:
328 * -------
329 * int Ext[] - Extremal indexes to dense frequency grid [r+1]
330 ********************/
331
InitialGuess(int r,int Ext[],int gridsize)332 void InitialGuess(int r, int Ext[], int gridsize)
333 {
334 int i;
335
336 for (i=0; i<=r; i++)
337 Ext[i] = i * (gridsize-1) / r;
338 }
339
340
341 /***********************
342 * CalcParms
343 *===========
344 *
345 *
346 * INPUT:
347 * ------
348 * int r - 1/2 the number of filter coefficients
349 * int Ext[] - Extremal indexes to dense frequency grid [r+1]
350 * double Grid[] - Frequencies (0 to 0.5) on the dense grid [gridsize]
351 * double D[] - Desired response on the dense grid [gridsize]
352 * double W[] - Weight function on the dense grid [gridsize]
353 *
354 * OUTPUT:
355 * -------
356 * double ad[] - 'b' in Oppenheim & Schafer [r+1]
357 * double x[] - [r+1]
358 * double y[] - 'C' in Oppenheim & Schafer [r+1]
359 ***********************/
360
CalcParms(int r,int Ext[],double Grid[],double D[],double W[],double ad[],double x[],double y[])361 void CalcParms(int r, int Ext[], double Grid[], double D[], double W[],
362 double ad[], double x[], double y[])
363 {
364 int i, j, k, ld;
365 double sign, xi, delta, denom, numer;
366
367 /*
368 * Find x[]
369 */
370 for (i=0; i<=r; i++)
371 x[i] = cos(Pi2 * Grid[Ext[i]]);
372
373 /*
374 * Calculate ad[] - Oppenheim & Schafer eq 7.132
375 */
376 ld = (r-1)/15 + 1; /* Skips around to avoid round errors */
377 for (i=0; i<=r; i++)
378 {
379 denom = 1.0;
380 xi = x[i];
381 for (j=0; j<ld; j++)
382 {
383 for (k=j; k<=r; k+=ld)
384 if (k != i)
385 denom *= 2.0*(xi - x[k]);
386 }
387 if (fabs(denom)<0.00001)
388 denom = 0.00001;
389 ad[i] = 1.0/denom;
390 }
391
392 /*
393 * Calculate delta - Oppenheim & Schafer eq 7.131
394 */
395 numer = denom = 0;
396 sign = 1;
397 for (i=0; i<=r; i++)
398 {
399 numer += ad[i] * D[Ext[i]];
400 denom += sign * ad[i]/W[Ext[i]];
401 sign = -sign;
402 }
403 delta = numer/denom;
404 sign = 1;
405
406 /*
407 * Calculate y[] - Oppenheim & Schafer eq 7.133b
408 */
409 for (i=0; i<=r; i++)
410 {
411 y[i] = D[Ext[i]] - sign * delta/W[Ext[i]];
412 sign = -sign;
413 }
414 }
415
416
417 /*********************
418 * ComputeA
419 *==========
420 * Using values calculated in CalcParms, ComputeA calculates the
421 * actual filter response at a given frequency (freq). Uses
422 * eq 7.133a from Oppenheim & Schafer.
423 *
424 *
425 * INPUT:
426 * ------
427 * double freq - Frequency (0 to 0.5) at which to calculate A
428 * int r - 1/2 the number of filter coefficients
429 * double ad[] - 'b' in Oppenheim & Schafer [r+1]
430 * double x[] - [r+1]
431 * double y[] - 'C' in Oppenheim & Schafer [r+1]
432 *
433 * OUTPUT:
434 * -------
435 * Returns double value of A[freq]
436 *********************/
437
ComputeA(double freq,int r,double ad[],double x[],double y[])438 double ComputeA(double freq, int r, double ad[], double x[], double y[])
439 {
440 int i;
441 double xc, c, denom, numer;
442
443 denom = numer = 0;
444 xc = cos(Pi2 * freq);
445 for (i=0; i<=r; i++)
446 {
447 c = xc - x[i];
448 if (fabs(c) < 1.0e-7)
449 {
450 numer = y[i];
451 denom = 1;
452 break;
453 }
454 c = ad[i]/c;
455 denom += c;
456 numer += c*y[i];
457 }
458 return numer/denom;
459 }
460
461
462 /************************
463 * CalcError
464 *===========
465 * Calculates the Error function from the desired frequency response
466 * on the dense grid (D[]), the weight function on the dense grid (W[]),
467 * and the present response calculation (A[])
468 *
469 *
470 * INPUT:
471 * ------
472 * int r - 1/2 the number of filter coefficients
473 * double ad[] - [r+1]
474 * double x[] - [r+1]
475 * double y[] - [r+1]
476 * int gridsize - Number of elements in the dense frequency grid
477 * double Grid[] - Frequencies on the dense grid [gridsize]
478 * double D[] - Desired response on the dense grid [gridsize]
479 * double W[] - Weight function on the desnse grid [gridsize]
480 *
481 * OUTPUT:
482 * -------
483 * double E[] - Error function on dense grid [gridsize]
484 ************************/
485
CalcError(int r,double ad[],double x[],double y[],int gridsize,double Grid[],double D[],double W[],double E[])486 void CalcError(int r, double ad[], double x[], double y[],
487 int gridsize, double Grid[],
488 double D[], double W[], double E[])
489 {
490 int i;
491 double A;
492
493 for (i=0; i<gridsize; i++)
494 {
495 A = ComputeA(Grid[i], r, ad, x, y);
496 E[i] = W[i] * (D[i] - A);
497 }
498 }
499
500 /************************
501 * Search
502 *========
503 * Searches for the maxima/minima of the error curve. If more than
504 * r+1 extrema are found, it uses the following heuristic (thanks
505 * Chris Hanson):
506 * 1) Adjacent non-alternating extrema deleted first.
507 * 2) If there are more than one excess extrema, delete the
508 * one with the smallest error. This will create a non-alternation
509 * condition that is fixed by 1).
510 * 3) If there is exactly one excess extremum, delete the smaller
511 * of the first/last extremum
512 *
513 *
514 * INPUT:
515 * ------
516 * int r - 1/2 the number of filter coefficients
517 * int Ext[] - Indexes to Grid[] of extremal frequencies [r+1]
518 * int gridsize - Number of elements in the dense frequency grid
519 * double E[] - Array of error values. [gridsize]
520 * OUTPUT:
521 * -------
522 * int Ext[] - New indexes to extremal frequencies [r+1]
523 ************************/
524
Search(int r,int Ext[],int gridsize,double E[])525 void Search(int r, int Ext[],
526 int gridsize, double E[])
527 {
528 int i, j, k, l, extra; /* Counters */
529 int up, alt;
530 int *foundExt; /* Array of found extremals */
531
532 /*
533 * Allocate enough space for found extremals.
534 */
535 foundExt = (int *)malloc((2*r) * sizeof(int));
536 k = 0;
537
538 /*
539 * Check for extremum at 0.
540 */
541 if (((E[0]>0.0) && (E[0]>E[1])) ||
542 ((E[0]<0.0) && (E[0]<E[1])))
543 foundExt[k++] = 0;
544
545 /*
546 * Check for extrema inside dense grid
547 */
548 for (i=1; i<gridsize-1; i++)
549 {
550 if (((E[i]>=E[i-1]) && (E[i]>E[i+1]) && (E[i]>0.0)) ||
551 ((E[i]<=E[i-1]) && (E[i]<E[i+1]) && (E[i]<0.0)))
552 foundExt[k++] = i;
553 }
554
555 /*
556 * Check for extremum at 0.5
557 */
558 j = gridsize-1;
559 if (((E[j]>0.0) && (E[j]>E[j-1])) ||
560 ((E[j]<0.0) && (E[j]<E[j-1])))
561 foundExt[k++] = j;
562
563
564 /*
565 * Remove extra extremals
566 */
567 extra = k - (r+1);
568
569 while (extra > 0)
570 {
571 if (E[foundExt[0]] > 0.0)
572 up = 1; /* first one is a maxima */
573 else
574 up = 0; /* first one is a minima */
575
576 l=0;
577 alt = 1;
578 for (j=1; j<k; j++)
579 {
580 if (fabs(E[foundExt[j]]) < fabs(E[foundExt[l]]))
581 l = j; /* new smallest error. */
582 if ((up) && (E[foundExt[j]] < 0.0))
583 up = 0; /* switch to a minima */
584 else if ((!up) && (E[foundExt[j]] > 0.0))
585 up = 1; /* switch to a maxima */
586 else
587 {
588 alt = 0;
589 break; /* Ooops, found two non-alternating */
590 } /* extrema. Delete smallest of them */
591 } /* if the loop finishes, all extrema are alternating */
592
593 /*
594 * If there's only one extremal and all are alternating,
595 * delete the smallest of the first/last extremals.
596 */
597 if ((alt) && (extra == 1))
598 {
599 if (fabs(E[foundExt[k-1]]) < fabs(E[foundExt[0]]))
600 l = foundExt[k-1]; /* Delete last extremal */
601 else
602 l = foundExt[0]; /* Delete first extremal */
603 }
604
605 for (j=l; j<k; j++) /* Loop that does the deletion */
606 {
607 foundExt[j] = foundExt[j+1];
608 }
609 k--;
610 extra--;
611 }
612
613 for (i=0; i<=r; i++)
614 {
615 Ext[i] = foundExt[i]; /* Copy found extremals to Ext[] */
616 }
617
618 free(foundExt);
619 }
620
621
622 /*********************
623 * FreqSample
624 *============
625 * Simple frequency sampling algorithm to determine the impulse
626 * response h[] from A's found in ComputeA
627 *
628 *
629 * INPUT:
630 * ------
631 * int N - Number of filter coefficients
632 * double A[] - Sample points of desired response [N/2]
633 * int symmetry - Symmetry of desired filter
634 *
635 * OUTPUT:
636 * -------
637 * double h[] - Impulse Response of final filter [N]
638 *********************/
FreqSample(int N,double A[],double h[],int symm)639 void FreqSample(int N, double A[], double h[], int symm)
640 {
641 int n, k;
642 double x, val, M;
643
644 M = (N-1.0)/2.0;
645 if (symm == POSITIVE)
646 {
647 if (N%2)
648 {
649 for (n=0; n<N; n++)
650 {
651 val = A[0];
652 x = Pi2 * (n - M)/N;
653 for (k=1; k<=M; k++)
654 val += 2.0 * A[k] * cos(x*k);
655 h[n] = val/N;
656 }
657 }
658 else
659 {
660 for (n=0; n<N; n++)
661 {
662 val = A[0];
663 x = Pi2 * (n - M)/N;
664 for (k=1; k<=(N/2-1); k++)
665 val += 2.0 * A[k] * cos(x*k);
666 h[n] = val/N;
667 }
668 }
669 }
670 else
671 {
672 if (N%2)
673 {
674 for (n=0; n<N; n++)
675 {
676 val = 0;
677 x = Pi2 * (n - M)/N;
678 for (k=1; k<=M; k++)
679 val += 2.0 * A[k] * sin(x*k);
680 h[n] = val/N;
681 }
682 }
683 else
684 {
685 for (n=0; n<N; n++)
686 {
687 val = A[N/2] * sin(Pi * (n - M));
688 x = Pi2 * (n - M)/N;
689 for (k=1; k<=(N/2-1); k++)
690 val += 2.0 * A[k] * sin(x*k);
691 h[n] = val/N;
692 }
693 }
694 }
695 }
696
697 /*******************
698 * isDone
699 *========
700 * Checks to see if the error function is small enough to consider
701 * the result to have converged.
702 *
703 * INPUT:
704 * ------
705 * int r - 1/2 the number of filter coeffiecients
706 * int Ext[] - Indexes to extremal frequencies [r+1]
707 * double E[] - Error function on the dense grid [gridsize]
708 *
709 * OUTPUT:
710 * -------
711 * Returns 1 if the result converged
712 * Returns 0 if the result has not converged
713 ********************/
714
isDone(int r,int Ext[],double E[])715 short isDone(int r, int Ext[], double E[])
716 {
717 int i;
718 double min, max, current;
719
720 min = max = fabs(E[Ext[0]]);
721 for (i=1; i<=r; i++)
722 {
723 current = fabs(E[Ext[i]]);
724 if (current < min)
725 min = current;
726 if (current > max)
727 max = current;
728 }
729 if (((max-min)/max) < 0.0001)
730 return 1;
731 return 0;
732 }
733
734 /********************
735 * remez
736 *=======
737 * Calculates the optimal (in the Chebyshev/minimax sense)
738 * FIR filter impulse response given a set of band edges,
739 * the desired reponse on those bands, and the weight given to
740 * the error in those bands.
741 *
742 * INPUT:
743 * ------
744 * int numtaps - Number of filter coefficients
745 * int numband - Number of bands in filter specification
746 * double bands[] - User-specified band edges [2 * numband]
747 * double des[] - User-specified band responses [numband]
748 * double weight[] - User-specified error weights [numband]
749 * int type - Type of filter
750 *
751 * OUTPUT:
752 * -------
753 * double h[] - Impulse response of final filter [numtaps]
754 ********************/
755
remez(double h[],int numtaps,int numband,double bands[],double des[],double weight[],int type)756 void Music::remez(double h[], int numtaps,
757 int numband, double bands[], double des[], double weight[],
758 int type)
759 {
760 double *Grid, *W, *D, *E;
761 int i, iter, gridsize, r, *Ext;
762 double *taps, c;
763 double *x, *y, *ad;
764 int symmetry;
765
766 if (type == BANDPASS)
767 symmetry = POSITIVE;
768 else
769 symmetry = NEGATIVE;
770
771 r = numtaps/2; /* number of extrema */
772 if ((numtaps%2) && (symmetry == POSITIVE))
773 r++;
774
775 /*
776 * Predict dense grid size in advance for memory allocation
777 * .5 is so we round up, not truncate
778 */
779 gridsize = 0;
780 for (i=0; i<numband; i++)
781 {
782 gridsize += (int)(2*r*GRIDDENSITY*(bands[2*i+1] - bands[2*i]) + .5);
783 }
784 if (symmetry == NEGATIVE)
785 {
786 gridsize--;
787 }
788
789 /*
790 * Dynamically allocate memory for arrays with proper sizes
791 */
792 Grid = (double *)malloc(gridsize * sizeof(double));
793 D = (double *)malloc(gridsize * sizeof(double));
794 W = (double *)malloc(gridsize * sizeof(double));
795 E = (double *)malloc(gridsize * sizeof(double));
796 Ext = (int *)malloc((r+1) * sizeof(int));
797 taps = (double *)malloc((r+1) * sizeof(double));
798 x = (double *)malloc((r+1) * sizeof(double));
799 y = (double *)malloc((r+1) * sizeof(double));
800 ad = (double *)malloc((r+1) * sizeof(double));
801
802 /*
803 * Create dense frequency grid
804 */
805 CreateDenseGrid(r, numtaps, numband, bands, des, weight,
806 &gridsize, Grid, D, W, symmetry);
807 InitialGuess(r, Ext, gridsize);
808
809 /*
810 * For Differentiator: (fix grid)
811 */
812 if (type == DIFFERENTIATOR)
813 {
814 for (i=0; i<gridsize; i++)
815 {
816 /* D[i] = D[i]*Grid[i]; */
817 if (D[i] > 0.0001)
818 W[i] = W[i]/Grid[i];
819 }
820 }
821
822 /*
823 * For odd or Negative symmetry filters, alter the
824 * D[] and W[] according to Parks McClellan
825 */
826 if (symmetry == POSITIVE)
827 {
828 if (numtaps % 2 == 0)
829 {
830 for (i=0; i<gridsize; i++)
831 {
832 c = cos(Pi * Grid[i]);
833 D[i] /= c;
834 W[i] *= c;
835 }
836 }
837 }
838 else
839 {
840 if (numtaps % 2)
841 {
842 for (i=0; i<gridsize; i++)
843 {
844 c = sin(Pi2 * Grid[i]);
845 D[i] /= c;
846 W[i] *= c;
847 }
848 }
849 else
850 {
851 for (i=0; i<gridsize; i++)
852 {
853 c = sin(Pi * Grid[i]);
854 D[i] /= c;
855 W[i] *= c;
856 }
857 }
858 }
859
860 /*
861 * Perform the Remez Exchange algorithm
862 */
863 for (iter=0; iter<MAXITERATIONS; iter++)
864 {
865 CalcParms(r, Ext, Grid, D, W, ad, x, y);
866 CalcError(r, ad, x, y, gridsize, Grid, D, W, E);
867 Search(r, Ext, gridsize, E);
868 if (isDone(r, Ext, E))
869 break;
870 }
871 if (iter == MAXITERATIONS)
872 {
873 printf("Reached maximum iteration count.\nResults may be bad.\n");
874 }
875
876 CalcParms(r, Ext, Grid, D, W, ad, x, y);
877
878 /*
879 * Find the 'taps' of the filter for use with Frequency
880 * Sampling. If odd or Negative symmetry, fix the taps
881 * according to Parks McClellan
882 */
883 for (i=0; i<=numtaps/2; i++)
884 {
885 if (symmetry == POSITIVE)
886 {
887 if (numtaps%2)
888 c = 1;
889 else
890 c = cos(Pi * (double)i/numtaps);
891 }
892 else
893 {
894 if (numtaps%2)
895 c = sin(Pi2 * (double)i/numtaps);
896 else
897 c = sin(Pi * (double)i/numtaps);
898 }
899 taps[i] = ComputeA((double)i/numtaps, r, ad, x, y)*c;
900 }
901
902 /*
903 * Frequency sampling design with calculated taps
904 */
905 FreqSample(numtaps, taps, h, symmetry);
906
907 /*
908 * Delete allocated memory
909 */
910 free(Grid);
911 free(W);
912 free(D);
913 free(E);
914 free(Ext);
915 free(x);
916 free(y);
917 free(ad);
918 }
919
920