home *** CD-ROM | disk | FTP | other *** search
/ AmigActive 15 / AACD15.ISO / AACD / System / MorphOS / SoX / Source / resample.c < prev    next >
Encoding:
C/C++ Source or Header  |  1999-07-18  |  20.2 KB  |  680 lines
  1.  
  2. /*
  3.  * July 5, 1991
  4.  * Copyright 1991 Lance Norskog And Sundry Contributors
  5.  * This source code is freely redistributable and may be used for
  6.  * any purpose.  This copyright notice must be maintained. 
  7.  * Lance Norskog And Sundry Contributors are not responsible for 
  8.  * the consequences of using this software.
  9.  */
  10.  
  11. /*
  12.  * Sound Tools rate change effect file.
  13.  * Spiffy rate changer using Smith & Wesson Bandwidth-Limited Interpolation.
  14.  * The algorithm is described in "Bandlimited Interpolation -
  15.  * Introduction and Algorithm" by Julian O. Smith III.
  16.  * Available on ccrma-ftp.stanford.edu as
  17.  * pub/BandlimitedInterpolation.eps.Z or similar.
  18.  *
  19.  * The latest stand alone version of this algorithm can be found
  20.  * at ftp://ccrma-ftp.stanford.edu/pub/NeXT/
  21.  * under the name of resample-version.number.tar.Z
  22.  *
  23.  * NOTE: This source badly needs to be updated to reflect the latest
  24.  * version of the above software!  Someone please perform this and
  25.  * send patches to cbagwell@sprynet.com.
  26.  */
  27. /* Fixed bug: roll off frequency was wrong, too high by 2 when upsampling,
  28.  * too low by 2 when downsampling.
  29.  * Andreas Wilde, 12. Feb. 1999, andreas@eakaw2.et.tu-dresden.de
  30. */
  31. #include <math.h>
  32. #include <stdlib.h>
  33. #ifdef HAVE_MALLOC_H
  34. #include <malloc.h>
  35. #endif
  36. #include "st.h"
  37.  
  38. /* resample includes */
  39. #include "resdefs.h"
  40. #include "resampl.h"
  41.  
  42. #define IBUFFSIZE 4096                         /* Input buffer size */
  43. #define OBUFFSIZE (IBUFFSIZE*MAXFACTOR+2)      /* Calc'd out buffer size */
  44.  
  45. /* Private data for Lerp via LCM file */
  46. typedef struct resamplestuff {
  47.    double Factor;               /* Factor = Fout/Fin sample rates */
  48.    double rolloff;              /* roll-off frequency */
  49.    double beta;                 /* passband/stopband tuning magic */
  50.    short InterpFilt;          /* TRUE means interpolate filter coeffs */
  51.    UHWORD Oskip;        /* number of bogus output samples at start */
  52.    UHWORD LpScl, Nmult, Nwing;
  53.    HWORD *Imp;                 /* impulse [MAXNWING] Filter coefficients */
  54.    HWORD *ImpD;                /* [MAXNWING] ImpD[n] = Imp[n+1]-Imp[n] */
  55.    /* for resample main loop */
  56.    UWORD Time;                  /* Current time/pos in input sample */
  57.    UHWORD Xp, Xoff, Xread;
  58.    HWORD *X, *Y;         /* I/O buffers */
  59. } *resample_t;
  60.  
  61. int makeFilter(P6(HWORD Imp[],
  62.           HWORD ImpD[],
  63.           UHWORD *LpScl,
  64.           UHWORD Nwing,
  65.           double Froll,
  66.           double Beta));
  67. HWORD SrcUp(P10(HWORD X[],
  68.         HWORD Y[],
  69.         double Factor,
  70.         UWORD *Time,
  71.         UHWORD Nx,
  72.         UHWORD Nwing,
  73.         UHWORD LpScl,
  74.         HWORD Imp[],
  75.         HWORD ImpD[],
  76.         BOOL Interp));
  77. HWORD SrcUD(P10(HWORD X[],
  78.         HWORD Y[],
  79.         double Factor,
  80.         UWORD *Time,
  81.         UHWORD Nx,
  82.         UHWORD Nwing,
  83.         UHWORD LpScl,
  84.         HWORD Imp[],
  85.         HWORD ImpD[],
  86.         BOOL Interp));
  87. IWORD FilterUp(P7(HWORD Imp[],
  88.           HWORD ImpD[],
  89.           UHWORD Nwing,
  90.           BOOL Interp,
  91.           HWORD *Xp,
  92.           HWORD Ph,
  93.           HWORD Inc));
  94. IWORD FilterUD(P8(HWORD Imp[],
  95.           HWORD ImpD[],
  96.           UHWORD Nwing,
  97.           BOOL Interp,
  98.           HWORD *Xp,
  99.           HWORD Ph,
  100.           HWORD Inc,
  101.           UHWORD dhb));
  102.  
  103. /*
  104.  * Process options
  105.  */
  106. void resample_getopts(effp, n, argv) 
  107. eff_t effp;
  108. int n;
  109. char **argv;
  110. {
  111.     resample_t resample = (resample_t) effp->priv;
  112.  
  113.     /* These defaults are conservative with respect to aliasing. */
  114.     resample->rolloff = 0.8;
  115.     resample->beta = 17.5;
  116.  
  117.     /* This used to fail, but with sox-12.15 it works. AW */
  118.     if ((n >= 1) && !sscanf(argv[0], "%lf", &resample->rolloff))
  119.         fail("Usage: resample [ rolloff [ beta ] ]");
  120.     else if ((resample->rolloff < 0.01) || (resample->rolloff > 1.0))
  121.         fail("resample: rolloff factor (%f) no good, should be 0.01<x<1.0", 
  122.             resample->rolloff);
  123.     if ((n >= 2) && !sscanf(argv[1], "%lf", &resample->beta))
  124.         fail("Usage: resample [ rolloff [ beta ] ]");
  125.     else if (resample->beta < 1.0)
  126.             fail("resample: beta factor (%f) no good, should be >= 1.0", 
  127.             resample->beta);
  128.     report("resample opts: %f, %f\n", 
  129.         resample->rolloff, resample->beta);
  130. }
  131.  
  132. /*
  133.  * Prepare processing.
  134.  */
  135. void resample_start(effp)
  136. eff_t effp;
  137. {
  138.     resample_t resample = (resample_t) effp->priv;
  139.     int i;
  140.     
  141.     resample->InterpFilt = 1;    /* interpolate filter: slower */
  142.     resample->Factor = 
  143.         (double)effp->outinfo.rate / (double)effp->ininfo.rate;
  144.     
  145.     /* Check for illegal constants */
  146.     if (Np >= 16)
  147.         fail("Error: Np>=16");
  148.     if (Nb+Nhg+NLpScl >= 32)
  149.         fail("Error: Nb+Nhg+NLpScl>=32");
  150.     if (Nh+Nb > 32)
  151.           fail("Error: Nh+Nb>32");
  152.  
  153.  
  154.     resample->Imp = (HWORD *) malloc(sizeof(HWORD) * MAXNWING);
  155.     resample->ImpD = (HWORD *) malloc(sizeof(HWORD) * MAXNWING);
  156.     resample->X = (HWORD *) malloc(sizeof(HWORD) * IBUFFSIZE);
  157.     resample->Y = (HWORD *) malloc(sizeof(HWORD) * OBUFFSIZE);
  158.  
  159.     /* upsampling requires smaller Nmults */
  160.     for(resample->Nmult = 37; resample->Nmult > 1; resample->Nmult -= 2) {
  161.         /* # of filter coeffs in right wing */
  162.         resample->Nwing = Npc*(resample->Nmult+1)/2;     
  163.         /* This prevents just missing last coeff */
  164.         /*   for integer conversion factors  */
  165.         resample->Nwing += Npc/2 + 1;      
  166.  
  167.         /* returns error # or 0 for success */
  168.         if (makeFilter(resample->Imp, resample->ImpD, 
  169.                 &resample->LpScl, resample->Nwing, 
  170.                 resample->rolloff, resample->beta))
  171.                 continue;
  172.             else
  173.                 break;
  174.             
  175.     }
  176.  
  177.     if(resample->Nmult == 1)
  178.         fail("resample: Unable to make filter\n");
  179.  
  180.     if (resample->Factor < 1)
  181.         resample->LpScl = resample->LpScl*resample->Factor + 0.5;
  182.     /* Calc reach of LP filter wing & give some creeping room */
  183.     resample->Xoff = ((resample->Nmult+1)/2.0) * 
  184.         MAX(1.0,1.0/resample->Factor) + 10;
  185.     if (IBUFFSIZE < 2*resample->Xoff)      /* Check input buffer size */
  186.         fail("IBUFFSIZE (or Factor) is too small");
  187.  
  188.     /* Current "now"-sample pointer for input */
  189.     resample->Xp = resample->Xoff;             
  190.     /* Position in input array to read into */
  191.     resample->Xread = resample->Xoff;          
  192.     /* Current-time pointer for converter */
  193.     resample->Time = (resample->Xoff<<Np);     
  194.  
  195.     /* Set sample drop at beginning */
  196.     resample->Oskip = resample->Xread * resample->Factor;
  197.  
  198.     /* Need Xoff zeros at begining of sample */
  199.     for (i=0; i<resample->Xoff; i++)
  200.         resample->X[i] = 0;
  201. }
  202.  
  203. /*
  204.  * Processed signed long samples from ibuf to obuf.
  205.  * Return number of samples processed.
  206.  */
  207.  
  208. void resample_flow(effp, ibuf, obuf, isamp, osamp)
  209. eff_t effp;
  210. LONG *ibuf, *obuf;
  211. LONG *isamp, *osamp;
  212. {
  213.     resample_t resample = (resample_t) effp->priv;
  214.     LONG i, last, creep, Nout, Nx;
  215.     UHWORD Nproc;
  216.  
  217.     /* constrain amount we actually process */
  218.     Nproc = IBUFFSIZE - resample->Xp;
  219.     if (Nproc * resample->Factor >= OBUFFSIZE)
  220.         Nproc = OBUFFSIZE / resample->Factor;
  221.     if (Nproc * resample->Factor >= *osamp)
  222.         Nproc = *osamp / resample->Factor;
  223.     
  224.     Nx = Nproc - resample->Xread;
  225.     if (Nx <= 0)
  226.         fail("Nx negative: %d", Nx);
  227.     if (Nx > *isamp) {
  228.         Nx = *isamp;
  229.     }
  230.     for(i = resample->Xread; i < Nx + resample->Xread  ; i++) 
  231.         resample->X[i] = RIGHT(*ibuf++ + 0x8000, 16);
  232.     last = i;
  233.     Nproc = last - (resample->Xoff * 2);
  234.     for(; i < last + resample->Xoff  ; i++) 
  235.         resample->X[i] = 0;
  236.  
  237.     /* If we're draining out a buffer tail, 
  238.      * just do it next time or in drain.
  239.      */
  240.     if ((Nx == *isamp) && (Nx <= resample->Xoff)) {
  241.         /* fill in starting here next time */
  242.         resample->Xread = last;
  243.         /* leave *isamp alone, we consumed it */
  244.         *osamp = 0;
  245.         return;
  246.     }
  247.  
  248.  
  249.         /* SrcUp() is faster if we can use it */
  250.     if (resample->Factor > 1)       /* Resample stuff in input buffer */
  251.         Nout = SrcUp(resample->X, resample->Y,
  252.         resample->Factor, &resample->Time, Nproc,
  253.         resample->Nwing, resample->LpScl,
  254.         resample->Imp, resample->ImpD, 
  255.         resample->InterpFilt);      
  256.     else
  257.             Nout = SrcUD(resample->X, resample->Y,
  258.         resample->Factor, &resample->Time, Nproc,
  259.         resample->Nwing, resample->LpScl,
  260.         resample->Imp, resample->ImpD,
  261.         resample->InterpFilt);
  262.  
  263.     /* Move converter Nproc samples back in time */
  264.     resample->Time -= (Nproc<<Np); 
  265.         /* Advance by number of samples processed */
  266.     resample->Xp += Nproc;
  267.     /* Calc time accumulation in Time */
  268.     creep = (resample->Time>>Np) - resample->Xoff; 
  269.     if (creep)
  270.     {
  271.         resample->Time -= (creep<<Np);   /* Remove time accumulation */
  272.         resample->Xp += creep;     /* and add it to read pointer */
  273.     }
  274.  
  275.     /* Copy back portion of input signal that must be re-used */
  276.     for (i=0; i<last - resample->Xp + resample->Xoff; i++) 
  277.         resample->X[i] = resample->X[i + resample->Xp - resample->Xoff];
  278.  
  279.     /* Pos in input buff to read new data into */
  280.     resample->Xread = i;                 
  281.     resample->Xp = resample->Xoff;
  282.  
  283.     /* copy to output buffer, zero-filling beginning */
  284.     /* zero-fill to preserve length and loop points */
  285.     for(i = 0; i < resample->Oskip; i++) {
  286.         *obuf++ = 0;
  287.     }
  288.     for(i = resample->Oskip; i < Nout + resample->Oskip; i++) {
  289.         *obuf++ = LEFT(resample->Y[i], 16);
  290.     }
  291.  
  292.     *isamp = Nx;
  293.     *osamp = Nout;
  294.  
  295.     resample->Oskip = 0;
  296. }
  297.  
  298. /*
  299.  * Process tail of input samples.
  300.  */
  301. void resample_drain(effp, obuf, osamp)
  302. eff_t effp;
  303. ULONG *obuf;
  304. ULONG *osamp;
  305. {
  306.     resample_t resample = (resample_t) effp->priv;
  307.     LONG i, Nout;
  308.     UHWORD Nx;
  309.     
  310.     Nx = resample->Xread - resample->Xoff;
  311.     if (Nx <= resample->Xoff * 2) {
  312.         /* zero-fill end */
  313.         for(i = 0; i < resample->Xoff; i++)
  314.             *obuf++ = 0;
  315.         *osamp = resample->Xoff;
  316.         return;
  317.     }
  318.  
  319.     if (Nx * resample->Factor >= *osamp)
  320.         fail("resample_drain: Overran output buffer!\n");
  321.  
  322.     /* fill out end with zeros */
  323.     for(i = 0; i < resample->Xoff; i++)
  324.         resample->X[i + resample->Xread] = 0;
  325.         /* SrcUp() is faster if we can use it */
  326.     if (resample->Factor >= 1)       /* Resample stuff in input buffer */
  327.         Nout = SrcUp(resample->X, resample->Y,
  328.         resample->Factor, &resample->Time, Nx,
  329.         resample->Nwing, resample->LpScl,
  330.         resample->Imp, resample->ImpD, 
  331.         resample->InterpFilt);      
  332.     else
  333.             Nout = SrcUD(resample->X, resample->Y,
  334.         resample->Factor, &resample->Time, Nx,
  335.         resample->Nwing, resample->LpScl,
  336.         resample->Imp, resample->ImpD,
  337.         resample->InterpFilt);
  338.     
  339.     for(i = resample->Oskip; i < Nout; i++) {
  340.         *obuf++ = LEFT(resample->Y[i], 16);
  341.     }
  342.     *osamp = Nout - resample->Oskip;
  343. }
  344.  
  345. /*
  346.  * Do anything required when you stop reading samples.  
  347.  * Don't close input file! 
  348.  */
  349. void resample_stop(effp)
  350. eff_t effp;
  351. {
  352.     resample_t resample = (resample_t) effp->priv;
  353.     
  354.     free(resample->Imp);
  355.     free(resample->ImpD);
  356.     free(resample->X);
  357.     free(resample->Y);
  358. }
  359.  
  360. /* From resample:filters.c */
  361.  
  362. /* Sampling rate up-conversion only subroutine;
  363.  * Slightly faster than down-conversion;
  364.  */
  365. HWORD SrcUp(X, Y, Factor, Time, Nx, Nwing, LpScl, Imp, ImpD, Interp)
  366. HWORD X[], Y[];
  367. double Factor;
  368. UWORD *Time;
  369. UHWORD Nx, Nwing, LpScl;
  370. HWORD Imp[], ImpD[];
  371. BOOL Interp;
  372. {
  373.    HWORD *Xp, *Ystart;
  374.    IWORD v;
  375.  
  376.    double dt;                  /* Step through input signal */ 
  377.    UWORD dtb;                  /* Fixed-point version of Dt */
  378.    UWORD endTime;              /* When Time reaches EndTime, return to user */
  379.  
  380.    dt = 1.0/Factor;            /* Output sampling period */
  381.    dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */
  382.  
  383.    Ystart = Y;
  384.    endTime = *Time + (1<<Np)*(IWORD)Nx;
  385.    while (*Time < endTime)
  386.       {
  387.       Xp = &X[*Time>>Np];      /* Ptr to current input sample */
  388.       v = FilterUp(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask),
  389.          -1);                  /* Perform left-wing inner product */
  390.       v += FilterUp(Imp, ImpD, Nwing, Interp, Xp+1, (HWORD)((-*Time)&Pmask),
  391.          1);                   /* Perform right-wing inner product */
  392.       v >>= Nhg;               /* Make guard bits */
  393.       v *= LpScl;              /* Normalize for unity filter gain */
  394.       *Y++ = v>>NLpScl;        /* Deposit output */
  395.       *Time += dtb;            /* Move to next sample by time increment */
  396.       }
  397.    return (Y - Ystart);        /* Return the number of output samples */
  398. }
  399.  
  400.  
  401. /* Sampling rate conversion subroutine */
  402.  
  403. HWORD SrcUD(X, Y, Factor, Time, Nx, Nwing, LpScl, Imp, ImpD, Interp)
  404. HWORD X[], Y[];
  405. double Factor;
  406. UWORD *Time;
  407. UHWORD Nx, Nwing, LpScl;
  408. HWORD Imp[], ImpD[];
  409. BOOL Interp;
  410. {
  411.    HWORD *Xp, *Ystart;
  412.    IWORD v;
  413.  
  414.    double dh;                  /* Step through filter impulse response */
  415.    double dt;                  /* Step through input signal */
  416.    UWORD endTime;              /* When Time reaches EndTime, return to user */
  417.    UWORD dhb, dtb;             /* Fixed-point versions of Dh,Dt */
  418.  
  419.    dt = 1.0/Factor;            /* Output sampling period */
  420.    dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */
  421.  
  422.    dh = MIN(Npc, Factor*Npc);  /* Filter sampling period */
  423.    dhb = dh*(1<<Na) + 0.5;     /* Fixed-point representation */
  424.  
  425.    Ystart = Y;
  426.    endTime = *Time + (1<<Np)*(IWORD)Nx;
  427.    while (*Time < endTime)
  428.       {
  429.       Xp = &X[*Time>>Np];      /* Ptr to current input sample */
  430.       v = FilterUD(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask),
  431.           -1, dhb);            /* Perform left-wing inner product */
  432.       v += FilterUD(Imp, ImpD, Nwing, Interp, Xp+1, (HWORD)((-*Time)&Pmask),
  433.            1, dhb);            /* Perform right-wing inner product */
  434.       v >>= Nhg;               /* Make guard bits */
  435.       v *= LpScl;              /* Normalize for unity filter gain */
  436.       *Y++ = v>>NLpScl;        /* Deposit output */
  437.       *Time += dtb;            /* Move to next sample by time increment */
  438.       }
  439.    return (Y - Ystart);        /* Return the number of output samples */
  440. }
  441.  
  442. void LpFilter();
  443.  
  444. int makeFilter(Imp, ImpD, LpScl, Nwing, Froll, Beta)
  445. HWORD Imp[], ImpD[];
  446. UHWORD *LpScl, Nwing;
  447. double Froll, Beta;
  448. {
  449.    double DCgain, Scl, Maxh;
  450.    double *ImpR;
  451.    HWORD Dh;
  452.    LONG i, temp;
  453.  
  454.    if (Nwing > MAXNWING)                      /* Check for valid parameters */
  455.       return(1);
  456.    if ((Froll<=0) || (Froll>1))
  457.       return(2);
  458.    if (Beta < 1)
  459.       return(3);
  460.  
  461.    ImpR = (double *) malloc(sizeof(double) * MAXNWING);
  462.    LpFilter(ImpR, (int)Nwing, Froll, Beta, Npc); /* Design a Kaiser-window */
  463.                                                  /* Sinc low-pass filter */
  464.  
  465.    /* Compute the DC gain of the lowpass filter, and its maximum coefficient
  466.     * magnitude. Scale the coefficients so that the maximum coeffiecient just
  467.     * fits in Nh-bit fixed-point, and compute LpScl as the NLpScl-bit (signed)
  468.     * scale factor which when multiplied by the output of the lowpass filter
  469.     * gives unity gain. */
  470.    DCgain = 0;
  471.    Dh = Npc;                       /* Filter sampling period for factors>=1 */
  472.    for (i=Dh; i<Nwing; i+=Dh)
  473.       DCgain += ImpR[i];
  474.    DCgain = 2*DCgain + ImpR[0];    /* DC gain of real coefficients */
  475.  
  476.    for (Maxh=i=0; i<Nwing; i++)
  477.       Maxh = MAX(Maxh, fabs(ImpR[i]));
  478.  
  479.    Scl = ((1<<(Nh-1))-1)/Maxh;     /* Map largest coeff to 16-bit maximum */
  480.    temp = fabs((1<<(NLpScl+Nh))/(DCgain*Scl));
  481.    if (temp >= (1L<<16)) {
  482.       free(ImpR);
  483.       return(4);                   /* Filter scale factor overflows UHWORD */
  484.     }
  485.    *LpScl = temp;
  486.  
  487.    /* Scale filter coefficients for Nh bits and convert to integer */
  488.    if (ImpR[0] < 0)                /* Need pos 1st value for LpScl storage */
  489.       Scl = -Scl;
  490.    for (i=0; i<Nwing; i++)         /* Scale them */
  491.       ImpR[i] *= Scl;
  492.    for (i=0; i<Nwing; i++)         /* Round them */
  493.       Imp[i] = ImpR[i] + 0.5;
  494.  
  495.    /* ImpD makes linear interpolation of the filter coefficients faster */
  496.    for (i=0; i<Nwing-1; i++)
  497.       ImpD[i] = Imp[i+1] - Imp[i];
  498.    ImpD[Nwing-1] = - Imp[Nwing-1];      /* Last coeff. not interpolated */
  499.  
  500.    free(ImpR);
  501.    return(0);
  502. }
  503.  
  504.  
  505.  
  506. /* LpFilter()
  507.  *
  508.  * reference: "Digital Filters, 2nd edition"
  509.  *            R.W. Hamming, pp. 178-179
  510.  *
  511.  * Izero() computes the 0th order modified bessel function of the first kind.
  512.  *    (Needed to compute Kaiser window).
  513.  *
  514.  * LpFilter() computes the coeffs of a Kaiser-windowed low pass filter with
  515.  *    the following characteristics:
  516.  *
  517.  *       c[]  = array in which to store computed coeffs
  518.  *       frq  = roll-off frequency of filter
  519.  *       N    = Half the window length in number of coeffs
  520.  *       Beta = parameter of Kaiser window
  521.  *       Num  = number of coeffs before 1/frq
  522.  *
  523.  * Beta trades the rejection of the lowpass filter against the transition
  524.  *    width from passband to stopband.  Larger Beta means a slower
  525.  *    transition and greater stopband rejection.  See Rabiner and Gold
  526.  *    (Theory and Application of DSP) under Kaiser windows for more about
  527.  *    Beta.  The following table from Rabiner and Gold gives some feel
  528.  *    for the effect of Beta:
  529.  *
  530.  * All ripples in dB, width of transition band = D*N where N = window length
  531.  *
  532.  *               BETA    D       PB RIP   SB RIP
  533.  *               2.120   1.50  +-0.27      -30
  534.  *               3.384   2.23    0.0864    -40
  535.  *               4.538   2.93    0.0274    -50
  536.  *               5.658   3.62    0.00868   -60
  537.  *               6.764   4.32    0.00275   -70
  538.  *               7.865   5.0     0.000868  -80
  539.  *               8.960   5.7     0.000275  -90
  540.  *               10.056  6.4     0.000087  -100
  541.  */
  542.  
  543.  
  544. #define IzeroEPSILON 1E-21               /* Max error acceptable in Izero */
  545.  
  546. double Izero(x)
  547. double x;
  548. {
  549.    double sum, u, halfx, temp;
  550.    LONG n;
  551.  
  552.    sum = u = n = 1;
  553.    halfx = x/2.0;
  554.    do {
  555.       temp = halfx/(double)n;
  556.       n += 1;
  557.       temp *= temp;
  558.       u *= temp;
  559.       sum += u;
  560.       } while (u >= IzeroEPSILON*sum);
  561.    return(sum);
  562. }
  563.  
  564.  
  565. void LpFilter(c,N,frq,Beta,Num)
  566. double c[], frq, Beta;
  567. int N, Num;
  568. {
  569.    double IBeta, temp;
  570.    int i;
  571.  
  572.    /* Calculate filter coeffs: */
  573.    c[0] = frq;
  574.    for (i=1; i<N; i++)
  575.       {
  576.       temp = PI*(double)i/(double)Num;
  577.       c[i] = sin(temp*frq)/temp;
  578.       }
  579.  
  580.    /* Calculate and Apply Kaiser window to filter coeffs: */
  581.    IBeta = 1.0/Izero(Beta);
  582.    for (i=1; i<N; i++)
  583.       {
  584.       temp = (double)i / ((double)N * (double)1.0);
  585.       c[i] *= Izero(Beta*sqrt(1.0-temp*temp)) * IBeta;
  586.       }
  587. }
  588.  
  589.  
  590.  
  591.  
  592. IWORD FilterUp(Imp, ImpD, Nwing, Interp, Xp, Ph, Inc)
  593. HWORD Imp[], ImpD[];
  594. UHWORD Nwing;
  595. BOOL Interp;
  596. HWORD *Xp, Ph, Inc;
  597. {
  598.    HWORD a=0, *Hp, *Hdp=0, *End;
  599.    IWORD v, t;
  600.  
  601.    v=0;
  602.    Hp = &Imp[Ph>>Na];
  603.    End = &Imp[Nwing];
  604.    if (Interp)
  605.       {
  606.       Hdp = &ImpD[Ph>>Na];
  607.       a = Ph & Amask;
  608.       }
  609.    /* Possible Bug: Hdp and a are not initialized if Interp == 0 */
  610.    if (Inc == 1)                     /* If doing right wing...              */
  611.       {                              /* ...drop extra coeff, so when Ph is  */
  612.       End--;                         /*    0.5, we don't do too many mult's */
  613.       if (Ph == 0)                   /* If the phase is zero...           */
  614.          {                           /* ...then we've already skipped the */
  615.          Hp += Npc;                  /*    first sample, so we must also  */
  616.          Hdp += Npc;                 /*    skip ahead in Imp[] and ImpD[] */
  617.          }
  618.       }
  619.    while (Hp < End)
  620.       {
  621.       t = *Hp;                       /* Get filter coeff */
  622.       if (Interp)
  623.          {
  624.          t += (((IWORD)*Hdp)*a)>>Na;  /* t is now interp'd filter coeff */
  625.          Hdp += Npc;                 /* Filter coeff differences step */
  626.      }
  627.       t *= *Xp;      /* Mult coeff by input sample */
  628.       if (t & (1<<(Nhxn-1)))  /* Round, if needed */
  629.          t += (1<<(Nhxn-1));
  630.       t >>= Nhxn;    /* Leave some guard bits, but come back some */
  631.       v += t;        /* The filter output */
  632.       Hp += Npc;     /* Filter coeff step */
  633.       Xp += Inc;     /* Input signal step. NO CHECK ON ARRAY BOUNDS */
  634.       }
  635.    return(v);
  636. }
  637.  
  638.  
  639. IWORD FilterUD(Imp, ImpD, Nwing, Interp, Xp, Ph, Inc, dhb)
  640. HWORD Imp[], ImpD[];
  641. UHWORD Nwing;
  642. BOOL Interp;
  643. HWORD *Xp, Ph, Inc;
  644. UHWORD dhb;
  645. {
  646.    HWORD a, *Hp, *Hdp, *End;
  647.    IWORD v, t;
  648.    UWORD Ho;
  649.  
  650.    v=0;
  651.    Ho = (Ph*(UWORD)dhb)>>Np;
  652.    End = &Imp[Nwing];
  653.    if (Inc == 1)                     /* If doing right wing...              */
  654.       {                              /* ...drop extra coeff, so when Ph is  */
  655.       End--;                         /*    0.5, we don't do too many mult's */
  656.       if (Ph == 0)                   /* If the phase is zero...           */
  657.          Ho += dhb;                  /* ...then we've already skipped the */
  658.       }                              /*    first sample, so we must also  */
  659.                                      /*    skip ahead in Imp[] and ImpD[] */
  660.    while ((Hp = &Imp[Ho>>Na]) < End)
  661.       {
  662.       t = *Hp;       /* Get IR sample */
  663.       if (Interp)
  664.          {
  665.          Hdp = &ImpD[Ho>>Na]; /* get interp (lower Na) bits from diff table */
  666.          a = Ho & Amask;                  /* a is logically between 0 and 1 */
  667.          t += (((IWORD)*Hdp)*a)>>Na;      /* t is now interp'd filter coeff */
  668.      }
  669.       t *= *Xp;      /* Mult coeff by input sample */
  670.       if (t & (1<<(Nhxn-1)))  /* Round, if needed */
  671.          t += (1<<(Nhxn-1));
  672.       t >>= Nhxn;    /* Leave some guard bits, but come back some */
  673.       v += t;        /* The filter output */
  674.       Ho += dhb;     /* IR step */
  675.       Xp += Inc;     /* Input signal step. NO CHECK ON ARRAY BOUNDS */
  676.       }
  677.    return(v);
  678. }
  679.  
  680.