home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: Multimed / Multimed.zip / rxwavsrc.zip / RxWavVocoder.c < prev   
C/C++ Source or Header  |  2000-03-06  |  16KB  |  637 lines
  1. /*  RxWav
  2.    Copyright (C) 1999 2000  Giorgio Vicario
  3.  
  4.    This program is free software; you can redistribute it and/or modify
  5.    it under the terms of the GNU General Public License as published by
  6.    the Free Software Foundation; either version 2 of the License, or
  7.    (at your option) any later version.
  8.  
  9.    This program is distributed in the hope that it will be useful,
  10.    but WITHOUT ANY WARRANTY; without even the implied warranty of
  11.    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12.    GNU General Public License for more details.
  13.  
  14.    You should have received a copy of the GNU General Public License
  15.    along with this program; if not, write to the Free Software
  16.    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA     */
  17.  
  18. #define INCL_REXXSAA
  19. #include <os2emx.h>
  20. #include <stdlib.h>
  21. #include <stdio.h>
  22. #include <string.h>
  23. #include <regexp.h>
  24. #include <math.h>
  25. #include <float.h>
  26. #include "RxWav.h"
  27.  
  28. #define FORWARD 1
  29. #define INVERSE 0
  30. #define K_PUNTI 4096
  31. #define K_PUNTI1 (K_PUNTI + 1)
  32.  
  33.  
  34. double P, Pinc, synt, *WAnalisi, *WSintesi, *inout, *buffer, *dativoc;
  35. double lastphase[2][K_PUNTI1], lastamp[2][K_PUNTI1], lastfreq[2][K_PUNTI1],
  36.   index[2][K_PUNTI1];
  37.  
  38. /***********************************************************************
  39. Vocoder:
  40. tratto da Ceres di (oyvindha@notam.uio.no)
  41.  
  42.  
  43. ***********************************************************************/
  44. ULONG
  45. WavVocoder (PCSZ name, LONG argc, const RXSTRING * argv,
  46.         PCSZ queuename, PRXSTRING retstr)
  47. {
  48.   PSHORT pCh, pCh2, ptemp;
  49.   APIRET rc;
  50.   int i, frame, FDec, nCamp;
  51.   int campioni, puntifft, Kn2, Koverlap;
  52.   double soglia;
  53.  
  54.   if (argc != 4)
  55.     {
  56.       SendMsg (FUNC_VOCODER, ERR_NUMERO_PARAMETRI);
  57.       return INVALID_ROUTINE;
  58.     }
  59.  
  60.   if (!sscanf (argv[0].strptr, "%d", &pCh))
  61.     {
  62.       SendMsg (FUNC_VOCODER, ERR_PUNTATORE_ERRATO);
  63.       return INVALID_ROUTINE;
  64.     }
  65.  
  66.   if (!sscanf (argv[1].strptr, "%d", &pCh2))
  67.     {
  68.       SendMsg (FUNC_VOCODER, ERR_PUNTATORE_ERRATO);
  69.       return INVALID_ROUTINE;
  70.     }
  71.  
  72.   nCamp = atol (argv[2].strptr);
  73.   if (nCamp < 1)
  74.     {
  75.       SendMsg (FUNC_VOCODER, ERR_VALORE_INVALIDO);
  76.       return INVALID_ROUTINE;
  77.     }
  78.  
  79.   pCh = AllineaCh (pCh, (ULONG) nCamp, FUNC_VOCODER);
  80.   if (!pCh)
  81.     return INVALID_ROUTINE;
  82.  
  83.   pCh2 = AllineaCh (pCh2, (ULONG) nCamp, FUNC_VOCODER);
  84.   if (!pCh2)
  85.     return INVALID_ROUTINE;
  86.  
  87.   soglia = atof (argv[3].strptr) / 1000000;
  88.   if ((soglia < 0) | (soglia > 1))
  89.     {
  90.       SendMsg (FUNC_VOCODER, ERR_VALORE_INVALIDO);
  91.       return INVALID_ROUTINE;
  92.     }
  93.  
  94.  
  95.   fvec (inout, K_PUNTI);
  96.   fvec (buffer, K_PUNTI);
  97.   fvec (dativoc, K_PUNTI);
  98.   fvec (WAnalisi, K_PUNTI);
  99.   fvec (WSintesi, K_PUNTI);
  100.  
  101.   campioni = K_PUNTI;
  102.   puntifft = K_PUNTI / 2;
  103.   Kn2 = K_PUNTI / 2;
  104.   Koverlap = 2;
  105.   FDec = campioni / Koverlap;
  106.  
  107.   makewindows (WAnalisi, WSintesi, campioni, campioni, FDec);
  108.  
  109.   for (frame = 0; frame < (10 * nCamp / K_PUNTI); frame++)
  110.     {
  111.       for (i = 0; i < campioni; i++)
  112.     inout[i] = (double) *pCh++ / (double) MAX_CAMPIONE;
  113.  
  114.       fold (inout, WAnalisi, campioni, buffer, campioni, frame * campioni);
  115.       for (i = 0; i < campioni; i++)
  116.     inout[i] = (double) 0;
  117. /*
  118.  */
  119.       rfft (buffer, puntifft, FORWARD);
  120.       convert (buffer, dativoc, Kn2, FDec, FreqCamp, 1);
  121.       /*
  122.          if(frame==100) for (i=0; i<Kn2; i++) {
  123.          printf("freq %f, amp %f\n", dativoc[i+i+1], dativoc[i+i] * 1000);
  124.          }
  125.  
  126.        */
  127.       for (i = 0; i < Kn2; i++)
  128.     {
  129.       if (dativoc[i + i] < soglia)
  130.         dativoc[i + i] = 0;
  131.     }
  132.  
  133.       unconvert (dativoc, buffer, Kn2, FDec, FreqCamp, 1);
  134.       rfft (buffer, puntifft, INVERSE);
  135.       overlapadd (buffer, campioni, WSintesi, inout, campioni, frame * campioni);
  136.  
  137.       for (i = 0; i < campioni; i++)
  138.     *pCh2++ = *pCh2 + (SHORT) (inout[i] * MAX_CAMPIONE);
  139.       pCh = pCh - (SHORT) (K_PUNTI * 0.9);
  140.       pCh2 = pCh2 - (SHORT) (K_PUNTI * 0.9);
  141.     }
  142.  
  143.   free (inout);
  144.   free (buffer);
  145.   free (dativoc);
  146.   free (WAnalisi);
  147.   free (WSintesi);
  148.   sprintf (retstr->strptr, "%f", ERR_OK);
  149.   retstr->strlength = strlen (retstr->strptr);
  150.   return VALID_ROUTINE;
  151. }
  152.  
  153.  
  154.  
  155.  
  156.  
  157.  
  158. /* FFT ROUTINES */
  159.  
  160. /* If forward is true, rfft replaces 2*N real data points in x with
  161.    N complex values representing the positive frequency half of their
  162.    Fourier spectrum, with x[1] replaced with the real part of the Nyquist
  163.    frequency value.  If forward is false, rfft expects x to contain a
  164.    positive frequency spectrum arranged as before, and replaces it with
  165.    2*N real values.  N MUST be a power of 2. */
  166.  
  167. rfft (x, N, forward)
  168.      double x[];
  169.      int N, forward;
  170. {
  171.   double c1, c2, h1r, h1i, h2r, h2i, wr, wi, wpr, wpi, temp, theta;
  172.   double xr, xi;
  173.   int i, i1, i2, i3, i4, N2p1;
  174.  
  175.   theta = PI / N;
  176.   wr = 1.;
  177.   wi = 0.;
  178.   c1 = 0.5;
  179.   if (forward)
  180.     {
  181.       c2 = -0.5;
  182.       cfft (x, N, forward);
  183.       xr = x[0];
  184.       xi = x[1];
  185.     }
  186.   else
  187.     {
  188.       c2 = 0.5;
  189.       theta = -theta;
  190.       xr = x[1];
  191.       xi = 0.;
  192.       x[1] = 0.;
  193.     }
  194.   wpr = -2. * pow (sin (0.5 * theta), 2.);
  195.   wpi = sin (theta);
  196.   N2p1 = (N << 1) + 1;
  197.   for (i = 0; i <= N >> 1; i++)
  198.     {
  199.       i1 = i << 1;
  200.       i2 = i1 + 1;
  201.       i3 = N2p1 - i2;
  202.       i4 = i3 + 1;
  203.       if (i == 0)
  204.     {
  205.       h1r = c1 * (x[i1] + xr);
  206.       h1i = c1 * (x[i2] - xi);
  207.       h2r = -c2 * (x[i2] + xi);
  208.       h2i = c2 * (x[i1] - xr);
  209.       x[i1] = h1r + wr * h2r - wi * h2i;
  210.       x[i2] = h1i + wr * h2i + wi * h2r;
  211.       xr = h1r - wr * h2r + wi * h2i;
  212.       xi = -h1i + wr * h2i + wi * h2r;
  213.     }
  214.       else
  215.     {
  216.       h1r = c1 * (x[i1] + x[i3]);
  217.       h1i = c1 * (x[i2] - x[i4]);
  218.       h2r = -c2 * (x[i2] + x[i4]);
  219.       h2i = c2 * (x[i1] - x[i3]);
  220.       x[i1] = h1r + wr * h2r - wi * h2i;
  221.       x[i2] = h1i + wr * h2i + wi * h2r;
  222.       x[i3] = h1r - wr * h2r + wi * h2i;
  223.       x[i4] = -h1i + wr * h2i + wi * h2r;
  224.     }
  225.       wr = (temp = wr) * wpr - wi * wpi + wr;
  226.       wi = wi * wpr + temp * wpi + wi;
  227.     }
  228.   if (forward)
  229.     x[1] = xr;
  230.   else
  231.     cfft (x, N, forward);
  232. }
  233.  
  234. /* cfft replaces double array x containing NC complex values
  235.    (2*NC double values alternating real, imagininary, etc.)
  236.    by its Fourier transform if forward is true, or by its
  237.    inverse Fourier transform if forward is false, using a
  238.    recursive Fast Fourier transform method due to Danielson
  239.    and Lanczos.  NC MUST be a power of 2. */
  240.  
  241. cfft (x, NC, forward)
  242.      double x[];
  243.      int NC, forward;
  244. {
  245.   double wr, wi, wpr, wpi, theta, scale;
  246.   int mmax, ND, m, i, j, delta;
  247.  
  248.   ND = NC << 1;
  249.   bitreverse (x, ND);
  250.   for (mmax = 2; mmax < ND; mmax = delta)
  251.     {
  252.       delta = mmax << 1;
  253.       theta = TWOPI / (forward ? mmax : -mmax);
  254.       wpr = -2. * pow (sin (0.5 * theta), 2.);
  255.       wpi = sin (theta);
  256.       wr = 1.;
  257.       wi = 0.;
  258.       for (m = 0; m < mmax; m += 2)
  259.     {
  260.       double rtemp, itemp;
  261.       for (i = m; i < ND; i += delta)
  262.         {
  263.           j = i + mmax;
  264.           rtemp = wr * x[j] - wi * x[j + 1];
  265.           itemp = wr * x[j + 1] + wi * x[j];
  266.           x[j] = x[i] - rtemp;
  267.           x[j + 1] = x[i + 1] - itemp;
  268.           x[i] += rtemp;
  269.           x[i + 1] += itemp;
  270.         }
  271.       wr = (rtemp = wr) * wpr - wi * wpi + wr;
  272.       wi = wi * wpr + rtemp * wpi + wi;
  273.     }
  274.     }
  275.  
  276. /* scale output */
  277.  
  278.   scale = forward ? 1. / ND : 2.;
  279.   for (i = 0; i < ND; i++)
  280.     x[i] *= scale;
  281. }
  282.  
  283. /* bitreverse places double array x containing N/2 complex values
  284.    into bit-reversed order */
  285.  
  286. bitreverse (x, N)
  287.      double x[];
  288.      int N;
  289. {
  290.   double rtemp, itemp;
  291.   int i, j, m;
  292.  
  293.   for (i = j = 0; i < N; i += 2, j += m)
  294.     {
  295.       if (j > i)
  296.     {
  297.       rtemp = x[j];
  298.       itemp = x[j + 1];    /* complex exchange */
  299.       x[j] = x[i];
  300.       x[j + 1] = x[i + 1];
  301.       x[i] = rtemp;
  302.       x[i + 1] = itemp;
  303.     }
  304.       for (m = N >> 1; m >= 2 && j >= m; m >>= 1)
  305.     j -= m;
  306.     }
  307. }
  308.  
  309.  
  310. /* PHASE VOCODER INTERNALS */
  311.  
  312. /*
  313.  * make balanced pair of analysis (A) and synthesis (S) windows;
  314.  * window lengths are Nw, FFT length is N, synthesis interpolation
  315.  * factor is I
  316.  */
  317. makewindows (A, S, Nw, N, I)
  318.      double A[], S[];
  319.      int Nw, N, I;
  320. {
  321.   int i;
  322.   double sum;
  323.  
  324.   for (i = 0; i < Nw; i++)
  325.     A[i] = S[i] = 0.54 - 0.46 * cos (TWOPI * i / (Nw - 1));
  326.  
  327. /*
  328.  * when Nw > N, also apply interpolating (sinc) windows to
  329.  * ensure that window are 0 at increments of N (the FFT length)
  330.  * away from the center of the analysis window and of I away
  331.  * from the center of the synthesis window
  332.  */
  333.   if (Nw > N)
  334.     {
  335.       double x;
  336.  
  337. /*
  338.  * take care to create symmetrical windows
  339.  */
  340.       x = -(Nw - 1) / 2.;
  341.       for (i = 0; i < Nw; i++, x += 1.)
  342.     if (x != 0.)
  343.       {
  344.         A[i] *= N * sin (PI * x / N) / (PI * x);
  345.         S[i] *= I * sin (PI * x / I) / (PI * x);
  346.       }
  347.     }
  348. /*
  349.  * normalize windows for unity gain across unmodified
  350.  * analysis-synthesis procedure
  351.  */
  352.   for (sum = i = 0; i < Nw; i++)
  353.     sum += A[i];
  354.  
  355.   for (i = 0; i < Nw; i++)
  356.     {
  357.       double afac = 2. / sum;
  358.       double sfac = Nw > N ? 1. / afac : afac;
  359.       A[i] *= afac;
  360.       S[i] *= sfac;
  361.     }
  362.  
  363.   if (Nw <= N)
  364.     {
  365.       for (sum = i = 0; i < Nw; i += I)
  366.     sum += S[i] * S[i];
  367.       for (sum = 1. / sum, i = 0; i < Nw; i++)
  368.     S[i] *= sum;
  369.     }
  370. }
  371.  
  372.  
  373. /*
  374.  * multiply current input I by window W (both of length Nw);
  375.  * using modulus arithmetic, fold and rotate windowed input
  376.  * into output array O of (FFT) length N according to current
  377.  * input time n
  378.  */
  379. fold (I, W, Nw, O, N, n)
  380.      double I[], W[], O[];
  381.      int Nw, N, n;
  382. {
  383.  
  384.   int i;
  385.  
  386.   for (i = 0; i < N; i++)
  387.     O[i] = 0.;
  388.  
  389.   while (n < 0)
  390.     n += N;
  391.   n %= N;
  392.   for (i = 0; i < Nw; i++)
  393.     {
  394.       O[n] += I[i] * W[i];
  395.       if (++n == N)
  396.     n = 0;
  397.     }
  398. }
  399.  
  400.  
  401. /* S is a spectrum in rfft format, i.e., it contains N real values
  402.    arranged as real followed by imaginary values, except for first
  403.    two values, which are real parts of 0 and Nyquist frequencies;
  404.    convert first changes these into N/2+1 PAIRS of magnitude and
  405.    phase values to be stored in output array C; the phases are then
  406.    unwrapped and successive phase differences are used to compute
  407.    estimates of the instantaneous frequencies for each phase vocoder
  408.    analysis channel; decimation rate D and sampling rate R are used
  409.    to render these frequency values directly in Hz. */
  410.  
  411. convert (S, C, N2, D, R, ch)
  412.      double S[], C[];
  413.      int N2, D, R, ch;
  414. {
  415.   static int first = 1;
  416.   static double fundamental, factor;
  417.   double phase, phasediff;
  418.   int real, imag, amp, freq;
  419.   double a, b;
  420.   int i;
  421.  
  422. /* first pass: allocate zeroed space for previous phase
  423.    values for each channel and compute constants */
  424.  
  425.   if (first)
  426.     {
  427.       first = 0;
  428.       fundamental = (double) R / (N2 << 1);
  429.       factor = R / (D * TWOPI);
  430.     }
  431.  
  432. /* unravel rfft-format spectrum: note that N2+1 pairs of
  433.    values are produced */
  434.  
  435.   for (i = 0; i <= N2; i++)
  436.     {
  437.       imag = freq = (real = amp = i << 1) + 1;
  438.       a = (i == N2 ? S[1] : S[real]);
  439.       b = (i == 0 || i == N2 ? 0. : S[imag]);
  440.  
  441. /* compute magnitude value from real and imaginary parts */
  442.  
  443.       C[amp] = hypot (a, b);
  444.  
  445. /* compute phase value from real and imaginary parts and take
  446.    difference between this and previous value for each channel */
  447.  
  448.       if (C[amp] == 0.)
  449.     phasediff = 0.;
  450.       else
  451.     {
  452.       phasediff = (phase = -atan2 (b, a)) - lastphase[ch][i];
  453.       lastphase[ch][i] = phase;
  454.  
  455. /* unwrap phase differences */
  456.  
  457.       while (phasediff > PI)
  458.         phasediff -= TWOPI;
  459.       while (phasediff < -PI)
  460.         phasediff += TWOPI;
  461.     }
  462.  
  463. /* convert each phase difference to Hz */
  464.  
  465.       C[freq] = phasediff * factor + i * fundamental;
  466.     }
  467. }
  468.  
  469. /* oscillator bank resynthesizer for phase vocoder analyzer
  470.    uses sum of N+1 cosinusoidal table lookup oscillators to
  471.    compute I (interpolation factor) samples of output O
  472.    from N+1 amplitude and frequency value-pairs in C;
  473.    frequencies are scaled by P */
  474.  
  475. oscbank (C, N, R, I, O, ch, Nw, coef, Np, a0)
  476.      double C[], O[], coef[], a0;
  477.      int N, R, I, ch, Nw, Np;
  478. {
  479.   static int L = 8192, first = 1;
  480.   static double *table;
  481.   int amp, freq, n, chan;
  482.   double Iinv;
  483.  
  484. /* first pass: allocate memory for and compute cosine table */
  485.  
  486.   if (first)
  487.     {
  488.       first = 0;
  489.       fvec (table, L);
  490.  
  491.       for (n = 0; n < L; n++)
  492.     table[n] = N * cos (TWOPI * (double) n / L);
  493.     }
  494.  
  495.   Iinv = 1. / I;
  496.  
  497. /* for each channel, compute I samples using linear
  498.    interpolation on the amplitude and frequency
  499.    control values */
  500.  
  501.   for (chan = 0; chan < K_PUNTI; chan++)
  502.     {
  503.  
  504.       double a, ainc, f, finc, address;
  505.  
  506.       freq = (amp = (chan << 1)) + 1;
  507.       C[freq] *= Pinc;
  508.       finc = (C[freq] - (f = lastfreq[ch][chan])) * Iinv;
  509.       if (C[amp] < synt)
  510.     C[amp] = 0.;
  511.       else if (Np)
  512.     C[amp] *= lpamp (chan * P * PI / N, a0, coef, Np) /
  513.       lpamp (chan * PI / N, a0, coef, Np);
  514.       ainc = (C[amp] - (a = lastamp[ch][chan])) * Iinv;
  515.       address = index[ch][chan];
  516.  
  517. /* accumulate the I samples from each oscillator into
  518.    output array O (initially assumed to be zero);
  519.    f is frequency in Hz scaled by oscillator increment
  520.    factor and pitch (Pinc); a is amplitude; */
  521.  
  522.       if (ainc != 0. || a != 0.)
  523.     for (n = 0; n < I; n++)
  524.       {
  525.         O[n] += a * table[(int) address];
  526.  
  527.         address += f;
  528.  
  529.         while (address >= L)
  530.           address -= L;
  531.  
  532.         while (address < 0)
  533.           address += L;
  534.  
  535.         a += ainc;
  536.         f += finc;
  537.       }
  538.  
  539. /* save current values for next iteration */
  540.       lastfreq[ch][chan] = C[freq];
  541.       lastamp[ch][chan] = C[amp];
  542.       index[ch][chan] = address;
  543.     }
  544. }
  545.  
  546.  
  547. /* unconvert essentially undoes what convert does, i.e., it
  548.    turns N2+1 PAIRS of amplitude and frequency values in
  549.    C into N2 PAIR of complex spectrum data (in rfft format)
  550.    in output array S; sampling rate R and interpolation factor
  551.    I are used to recompute phase values from frequencies */
  552.  
  553. unconvert (C, S, N2, I, R, ch)
  554.      double C[], S[];
  555.      int N2, I, R, ch;
  556. {
  557.   static int first = 1;
  558.   static double fundamental, factor;
  559.   int i, real, imag, amp, freq;
  560.   double mag, phase;
  561.  
  562. /* first pass: allocate memory and compute constants */
  563.  
  564.   if (first)
  565.     {
  566.       first = 0;
  567.       fundamental = (double) R / (N2 << 1);
  568.       factor = TWOPI * I / R;
  569.     }
  570.  
  571. /* subtract out frequencies associated with each channel,
  572.    compute phases in terms of radians per I samples, and
  573.    convert to complex form */
  574.  
  575.   for (i = 0; i <= N2; i++)
  576.     {
  577.       imag = freq = (real = amp = i << 1) + 1;
  578.       if (i == N2)
  579.     real = 1;
  580.       mag = C[amp];
  581.       lastphase[ch][i] += C[freq] - i * fundamental;
  582.       phase = lastphase[ch][i] * factor;
  583.       S[real] = mag * cos (phase);
  584.       if (i != N2)
  585.     S[imag] = -mag * sin (phase);
  586.     }
  587. }
  588.  
  589.  
  590. /*
  591.  * input I is a folded spectrum of length N; output O and
  592.  * synthesis window W are of length Nw--overlap-add windowed,
  593.  * unrotated, unfolded input data into output O
  594.  */
  595. overlapadd (I, N, W, O, Nw, n)
  596.      double I[], W[], O[];
  597.      int N, Nw, n;
  598. {
  599.   int i;
  600.   while (n < 0)
  601.     n += N;
  602.   n %= N;
  603.   for (i = 0; i < Nw; i++)
  604.     {
  605.       O[i] += I[n] * W[i];
  606.       if (++n == N)
  607.     n = 0;
  608.     }
  609. }
  610.  
  611.  
  612. double
  613. lpamp (double omega, double a0, double *coef, int M)
  614. {
  615.   double wpr, wpi, wr, wi, re, im, temp, lp;
  616.   int i;
  617.  
  618.   if (a0 == 0.)
  619.     return (0.);
  620.   wpr = cos (omega);
  621.   wpi = sin (omega);
  622.   re = wr = 1.;
  623.   im = wi = 0.;
  624.   for (coef++, i = 1; i <= M; i++, coef++)
  625.     {
  626.       wr = (temp = wr) * wpr - wi * wpi;
  627.       wi = wi * wpr + temp * wpi;
  628.       re += *coef * wr;
  629.       im += *coef * wi;
  630.     }
  631.   if (re == 0. && im == 0.)
  632.     lp = 0.;
  633.   else
  634.     lp = sqrt (a0 / (re * re + im * im));
  635.   return (lp);
  636. }
  637.