├── GCC_PHAT.h
├── LICENSE.md
├── README.md
├── STFT
    └── C
    │   ├── STFT.h
    │   └── ooura_fft.h
├── WAV
    ├── WAV.cpp
    └── WAV.h
├── include
    ├── HannWindow.h
    ├── Ooura_FFT.h
    ├── PostProcessor.h
    └── STFT.h
└── main.cpp


/GCC_PHAT.h:
--------------------------------------------------------------------------------
  1 | #ifndef _GCC_PHAT_
  2 | #define _GCC_PHAT_
  3 | /*
  4 |  * Ver 1.0 2021-01-25 init
  5 |  */
  6 | #define _USE_MATH_DEFINES // to use M_PI
  7 | #include <math.h>
  8 | #include <tgmath.h>
  9 | #include <stdlib.h>
 10 | #include <string.h>
 11 | #include <stdio.h>
 12 | #include <complex>
 13 | #include "STFT/cpp/STFT.h"
 14 | #include "STFT/cpp/Ooura_FFT.h"
 15 | 
 16 | class GCC_PHAT {
 17 | private:
 18 |     const double eps = 2.2204e-16;
 19 |     const double sound_speed = 343.3;
 20 |     const double smooth_factor = 0.50;
 21 |     const double IntpRatio = 16.0;
 22 |     const int corr_opt = 1;
 23 |     const int phat_opt = 1;
 24 |     const int interest_freq_opt = 0;
 25 |     const double pi = 3.1415926535897;
 26 |     
 27 |     int ch;
 28 |     int fs;
 29 |     int nfft;
 30 |     int nhfft;
 31 |     int shift;
 32 |     int frame;
 33 |     double max_idx;
 34 |     double max_delay;
 35 |     double max;
 36 |     double mic_dist;
 37 | 
 38 |     std::complex<double>* R_tmp;
 39 |     std::complex<double>* R_tmp1;
 40 |     std::complex<double>* R_tmp2;
 41 |     double* R_PHAT_tmp3;
 42 |     double* R_PHAT;
 43 |     double* R_f_tmp;
 44 |     double* R_f_input;
 45 |     double* R_f;
 46 | 
 47 |     Ooura_FFT* fft = nullptr;
 48 |     std::complex<double> i1;
 49 | 
 50 | public:
 51 | 
 52 |     inline GCC_PHAT(int ch, int nfft, int fs, int shift, double mic_dist);
 53 |     inline ~GCC_PHAT();
 54 |     inline void Process(double**);
 55 | 
 56 |     double Estimated_Sample_delay;
 57 |     std::complex<double> Estimated_azimuth;
 58 |     double* R_f_output;
 59 | };
 60 | 
 61 | GCC_PHAT::GCC_PHAT(
 62 |     int ch_,
 63 |     int nfft_,
 64 |     int fs_,
 65 |     int shift_,
 66 |     double  mic_dist_):i1(0,1){
 67 | 
 68 |     ch = ch_;
 69 |     nfft = nfft_;
 70 |     fs = fs_;
 71 |     shift = shift_;
 72 |     frame = nfft_;
 73 |     mic_dist = mic_dist_;
 74 |     
 75 |     nhfft = nfft / 2 + 1;
 76 |     max_delay = ceil(mic_dist * double(fs) / sound_speed);
 77 |     
 78 |     R_tmp = new std::complex<double>[nhfft];
 79 |     R_tmp1 = new std::complex<double>[nhfft];
 80 |     R_tmp2 = new std::complex<double>[nfft / 2 * (IntpRatio)+1];
 81 |     R_PHAT_tmp3 = new double[nfft * IntpRatio + 2]; // for transformation right before iFFT
 82 |     R_PHAT = new double[nfft * IntpRatio];
 83 |     R_f_tmp = new double[max_delay * IntpRatio * 2 + 1];
 84 |     R_f_input = new double [(max_delay * IntpRatio) * 2 + 1]; // the very before frame
 85 |     R_f = new double[(max_delay * IntpRatio) * 2 + 1];
 86 |     R_f_output = new double[(max_delay * IntpRatio) * 2 + 1];
 87 |     memset(R_f_output, 0, sizeof(double) * ((max_delay * IntpRatio) * 2 + 1));
 88 |     fft = new Ooura_FFT(nfft * IntpRatio, ch);
 89 | }
 90 | 
 91 | void GCC_PHAT::Process(double** X) {
 92 |     int i, j, k, l;
 93 |     double weight_PHAT;
 94 | 
 95 |     /* Cross-Correlation */
 96 |     for (j = 0; j < nhfft; j++) {
 97 |         R_tmp[j] = (X[0][j + j] * X[1][j + j]) + (X[0][j + j + 1] * X[1][j + j + 1]) + i1*((X[0][j + j + 1] * X[1][j + j]) - (X[0][j + j ] * X[1][j + j + 1]));   // (ac+bd) + (bc -ad)i
 98 |     }
 99 | 
100 |     /* Interest Frequency Limit */
101 |     if (interest_freq_opt == 1) {
102 |         R_tmp[0] = 0;
103 |         for (i = 99; i < nhfft; i++) {
104 |             R_tmp[i] = 0;
105 |         }
106 |     }
107 | 
108 |     /* PHAT */
109 |     if (phat_opt == 1) {
110 |         for (j = 0; j < nhfft; j++) {
111 |             weight_PHAT = sqrt((R_tmp[j].real() * R_tmp[j].real()) + (R_tmp[j].imag() * R_tmp[j].imag()))+eps;
112 |             R_tmp1[j] = R_tmp[j] / weight_PHAT;
113 |         }
114 |     }
115 |     else {
116 |         for (j = 0; j < nhfft; j++) {
117 |             R_tmp1[j] = R_tmp[j];
118 |         }
119 |     }
120 | 
121 |     /* Interpolation (Zero Padding) */
122 |     for (j = 0; j < nfft/2 ; j++) {
123 |         R_tmp2[j] = R_tmp1[j];
124 |     }
125 |     R_tmp2[nfft/2* int(IntpRatio)] = R_tmp1[nfft/2]; 
126 | 
127 |     /* Transform double complex to double*/
128 |     for (j = 0; j < (nfft/2 * IntpRatio)+1; j++) {
129 |         R_PHAT_tmp3[j + j] = R_tmp2[j].real();
130 |         R_PHAT_tmp3[j + j + 1] = R_tmp2[j].imag();
131 |     }
132 | 
133 |     /*Inverse FFT*/
134 |     fft->SingleiFFT(R_PHAT_tmp3);
135 |     
136 |     ///*Half Flip*/  마지막 두개는 버려
137 |     for (j = 0; j < nfft * IntpRatio / 2; j++) {
138 |         R_PHAT[nfft * int(IntpRatio) / 2 + j] = R_PHAT_tmp3[j];
139 |         R_PHAT[j] = R_PHAT_tmp3[nfft * int(IntpRatio) / 2 + j];
140 |     }
141 | 
142 |     ///* Sample Delay Estimation */
143 |     for (j = 0; j < max_delay * IntpRatio * 2 + 1; j++) {
144 |         R_f_tmp[j] = R_PHAT[(nfft * int(IntpRatio) / 2) - 1 - int(max_delay * IntpRatio) + j];
145 |     }
146 | 
147 |     /* Smoothing Factor */
148 |     if (corr_opt == 1) {
149 |         for (j = 0; j < (max_delay * IntpRatio) * 2 + 1; j++) {
150 |             R_f[j] = R_f_tmp[j];
151 |         }
152 |     }
153 |     else if (corr_opt == 2) {
154 |         for (j = 0; j < (max_delay * IntpRatio) * 2 + 1; j++) {
155 |             R_f[j] = smooth_factor * R_f_output[j] + (1 - smooth_factor) * R_f_tmp[j];
156 |         }
157 |     }
158 |     else if (corr_opt == 3) {
159 |         for (j = 0; j < (max_delay * IntpRatio) * 2 + 1; j++) {
160 |             R_f[j] = R_f_output[j]+ R_f_tmp[j];
161 |         }
162 |     }
163 | 
164 |     /* Sample Delay Estimation */
165 |     max = R_f[0];
166 |     max_idx = 0;
167 |     for (j = 1; j < (max_delay * IntpRatio) * 2 + 1 ; j++) {
168 |         if (R_f[j] > max) {
169 |             max_idx = j;
170 |             max = R_f[j];
171 |         }
172 |     }
173 | 
174 |     /* Public Variable for return */
175 |     Estimated_Sample_delay = (max_idx) / IntpRatio - max_delay;
176 |     //Estimated_azimuth = (asin(Estimated_Sample_delay * sound_speed / double(fs) / mic_dist)) * 180 / pi;
177 |     //printf("(max_delay*IntpRatio)*2 : %f\n", (max_delay*IntpRatio)*2);
178 |     //printf("max_delay : %f\n", max_delay);
179 |     //printf("Estimated_Sample_delay : %f\n", Estimated_Sample_delay);
180 |     Estimated_azimuth = (asin(Estimated_Sample_delay / max_delay)) * 180 / pi;
181 |     for (j = 0; j < (max_delay * IntpRatio) * 2 + 1; j++) {
182 |         R_f_output[j] = R_f[j];
183 |     }
184 | }
185 | 
186 | /* Delete Function */
187 | GCC_PHAT::~GCC_PHAT() {
188 |     int i, j, k, l;
189 |     delete[] R_f_input;
190 |     delete[] R_f;
191 |     delete[] R_tmp;
192 |     delete[] R_tmp1;
193 |     delete[] R_tmp2;
194 |     delete[] R_PHAT;
195 |     delete[] R_f;
196 |     delete[] R_PHAT_tmp3;
197 | }
198 | #endif
199 | 


--------------------------------------------------------------------------------
/LICENSE.md:
--------------------------------------------------------------------------------
 1 | (c) 2022 NCSOFT Corporation & mpWAV Inc. All Rights Reserved.
 2 | 
 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 4 | 
 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 6 | 
 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 8 | 
 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
10 | 
11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
12 | 
13 | Any questions about our licensed work can be sent to opensource@ncsoft.com.
14 | ________________________________________
15 | 
16 | This software uses Open Source Software (OSS). You can find the link for the source code of these open source projects, along with applicable license information, below.
17 | 
18 | ooura_fft <br>
19 | https://github.com/biotrump/OouraFFT <br>
20 | Copyright(C) 1996-2001 Takuya OOURA <br>
21 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # mpWAV-Sound-Source-Localization
 2 | Sound Source Localization study for AI Grand Challenge 2021 (sponsored by NC Soft Vision Lab)
 3 | 
 4 | 
 5 | ## Overall Schematic
 6 | ![image](https://user-images.githubusercontent.com/57610448/152844613-dd3eadf9-7b56-42ca-8a35-ae769b07972f.png)
 7 | 
 8 | 
 9 | 
10 | 
11 | 
12 | ## Acknowledgement 
13 | 이 데이터는 2021년도 정부(과학기술정보통신부)의 재원으로 정보통신기획평가원의 지원을 받아 수행된 연구의 결과물임 (No.171125972, 인명 구조용 드론을 위한 영상/음성 인지 기술 개발)
14 | 
15 | This work was supported by Institute of Information & communications Technology Planning & Evaluation (IITP) grant funded by the Korea government(MSIT) (No.171125972, Audio-Visual Perception for Autonomous Rescue Drones)
16 | 


--------------------------------------------------------------------------------
/STFT/C/STFT.h:
--------------------------------------------------------------------------------
  1 | #include <stdlib.h>
  2 | #include <string.h>
  3 | #include <math.h>
  4 | #include "ooura_fft.h"
  5 | 
  6 | struct STFT{
  7 | 
  8 |   int channels;
  9 |   int frame;
 10 |   int shift;
 11 | 
 12 |   double**raw,**data;
 13 | 
 14 |   // hann window
 15 |   double*hann;
 16 | 
 17 |   //fft
 18 |   double *a,*w;
 19 |   int *ip;
 20 | 
 21 | }
 22 | 
 23 | void STFT_init(struct STFT*stft,int ch_,int frame_,int shift_){
 24 | 
 25 |   int channels = ch_;
 26 |   int frame = frame_;
 27 |   int shift = shift_;
 28 |   int ol= frame - shift;
 29 | 
 30 |   double**raw,**data;
 31 |   double*hann;
 32 | 
 33 |   int i,j;
 34 | 
 35 |   stft->channels = channels;
 36 |   stft->frame= frame_;
 37 |   stft->shift= shift_;
 38 | 
 39 |   hann = stft->hann;
 40 |   raw = stft->raw;
 41 |   data = stft->data;
 42 | 
 43 |   raw = (double**)malloc(sizeof(double*)*ch);
 44 |   for(i=0;i<ch;i++){
 45 |      raw[i] = (double*)malloc(sizeof(double)*frame);
 46 |      memset(raw[i],0,sizeof(double)*frame);
 47 |   }
 48 |   data = (double**)malloc(sizeof(double*)*ch);
 49 |   for(i=0;i<ch;i++){
 50 |      data[i] = (double*)malloc(sizeof(double)*(frame+2));
 51 |      memset(data[i],0,sizeof(double)*(frame+2));
 52 |   }
 53 | 
 54 |   hann = (double*)malloc(sizeof(double)*frame);
 55 |   switch((int)(frame/shift)){
 56 |     case 4:
 57 |         hann[0] = 0.0;
 58 |         for (i = 1; i < frame; ++i)
 59 |             hann[i] = 0.5 * (1.0 - cos(2.0 * M_PI * (double)i / (double)frame));
 60 | 
 61 |         tmp = sqrt((double)2 / 3);
 62 |         for (i = 1; i < frame; i++)
 63 |             hann[i] *= tmp;
 64 | 
 65 |         break;
 66 | 
 67 |     case 2:
 68 |         for (i = 0; i < frame; i++) {
 69 |             hann[i] = sin(M_PI * (i + 0.5) / frame);
 70 |         }
 71 | 
 72 |         break;
 73 |     default:
 74 |         printf("ERROR::frame/shift(%d) is Not Supported\n",
 75 |                frame / shift);
 76 |         exit(-1);
 77 |   }
 78 | 
 79 |   stft->a = (double*)malloc(sizeof(double)*frame);
 80 |   stft->w = (double*)malloc(sizeof(double)*frame);
 81 |   stft->ip = (int*)malloc(sizeof(int)*(int)(sqrt(frame/2)+1));
 82 | 
 83 | }
 84 | 
 85 | void STFT_free(struct STFT *stft){
 86 |   int i;
 87 | 
 88 |   free(stft->a);
 89 |   free(stft->w);
 90 |   free(stft->ip);
 91 | 
 92 |   for(i=0;i<stft->channels){
 93 |     free(stft->raw[i]);
 94 |   }
 95 |   free(stft->raw);
 96 |   free(stft->hann);
 97 | }
 98 | 
 99 | /* buf_in : input of STFT, wav file or record input stream format
100 |  *          ex) for 4 channels input
101 |  *              1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4 ...
102 |  * length : length of input
103 |  * data   : FFT output, [channels][frame]
104 |  * */
105 | void STFT(struct STFT *stft,short * buf_in,int length,double**data){
106 |   int i,j;
107 |   int channels,frame,shift,ol;
108 |   double **raw,*hann;
109 | 
110 |   /*** INIT ***/
111 |   channels = stft->channels;
112 |   frame = stft->frame;
113 |   shift = stft->shift;
114 |   ol = frame - shift;
115 | 
116 |   raw = stft->raw;
117 |   hann = stft->hann;
118 | 
119 | 
120 |   /*** SHIFT ***/
121 |   for (j = 0; j < channels; j++) {
122 |     for (i = 0; i < ol; i++) {
123 |       raw[j][i] = raw[j][i + shift];
124 |     }
125 |   }
126 | 
127 |   /*** COPY ***/
128 |   //// EOF
129 |   if(length!=shift*channels){
130 | 
131 |     length = length/channels;
132 |     for (i = 0; i < length; i++) {
133 |       for (j = 0; j < channels; j++)
134 |         raw[j][i + ol]
135 |           =  (double)(buf_in[i * channels+ j]);
136 |     }
137 |     for (i = length; i < shift; i++) {
138 |       for (j = 0; j < channels; j++)
139 |         raw[j][i + ol] = 0;
140 |     }
141 |     //// continue
142 |   }else{
143 |     for (i = 0; i < shift; i++) {
144 |       for (j = 0; j < channels; j++){
145 |         raw[j][i + ol] 
146 |           = (double)(buf_in[i * channels+ j]);
147 |       }
148 |     }
149 |   }
150 | 
151 |   //shifted raw -> data
152 |   for(i=0;i<channels;i++)
153 |     memcpy(data[i],raw[i],sizeof(double)*frame);
154 | 
155 |   /*** Windowing ***/
156 |   for(i=0;i<channels;i++){
157 |     for(j=0;j<frame;j++)
158 |       data[i][j] *= hann[j];
159 |   }
160 | 
161 |   /*** FFT ***/
162 |   for (j = 0; j < channels; j++) {
163 |     ip[0] = 0;
164 |     for (i = 0; i < frame; i++)
165 |       a[i] = data[j][i];
166 | 
167 |     ooura_rdft(frame, 1, a, ip, w);
168 | 
169 |     for (i = 0; i < frame; i += 2) {
170 |       data[j][i] = a[i];
171 |       data[j][i + 1] = -a[i + 1];
172 |     }
173 |     data[j][1] = 0;
174 |     data[j][frame] = a[1];
175 |     data[j][frame+ 1] = 0;
176 |   }
177 | 
178 | }
179 | 
180 | void ISTFT(struct STFT *stft,double **data, short * buf_out){
181 | 
182 |   /*** IFFT ***/
183 |     for (j = 0; j < channels; j++) {
184 |         double *t;
185 |         t = data[j];
186 |         ip[j][0] = 0;
187 |         for (int i = 0; i < frame_size; i += 2) {
188 |             a[j][i] = t[i];
189 |             a[j][i + 1] = -t[i + 1];
190 |         }
191 |         a[j][1] = t[frame_size];
192 | 
193 |         rdft(frame_size, -1, a[j], ip[j], w[j]);
194 |         for (int i = 0; i < frame_size; i++) {
195 |             a[j][i] *= 2.0;
196 |             a[j][i] /= frame_size;
197 |         }
198 | 
199 |         for (int i = 0; i < frame_size; i++) {
200 |             t[i] = a[j][i];
201 |         }
202 |     }
203 | 
204 |   /*** iFFT ***/
205 |   fft_out->iFFT(pool);
206 | 
207 |   /*** Window ***/
208 |   hw->Process(pool, channels);
209 | 
210 |   /*** Output ***/
211 |   if (do_mvdr)
212 |     out->Append(ap->Overlap(pool), shift_size * 1);
213 |   else if(do_gcp)
214 |     out->Append(ap->OverlapSingle(pool), shift_size);
215 |   else
216 |     out->Append(ap->Overlap(pool), shift_size * channels);
217 |   }
218 | 
219 | }
220 | 


--------------------------------------------------------------------------------
/STFT/C/ooura_fft.h:
--------------------------------------------------------------------------------
   1 | #include <math.h>
   2 | 
   3 | /*
   4 |   Copyright:
   5 |   Copyright(C) 1996-2001 Takuya OOURA
   6 |   email: ooura@mmm.t.u-tokyo.ac.jp
   7 |   download: http://momonga.t.u-tokyo.ac.jp/~ooura/fft.html
   8 |   You may use, copy, modify this code for any purpose and
   9 |   without fee. You may distribute this ORIGINAL package.
  10 | */
  11 |  void ooura_cdft(int, int, double *, int *, double *);
  12 |  void ooura_rdft(int, int, double *, int *, double *);
  13 |  void ooura_ddct(int, int, double *, int *, double *);
  14 |  void ooura_ddst(int, int, double *, int *, double *);
  15 |  void ooura_dfct(int, double *, double *, int *, double *);
  16 |  void ooura_dfst(int, double *, double *, int *, double *);
  17 | 
  18 |  void ooura_cdft(int n, int isgn, double *a, int *ip, double *w) {
  19 |     void ooura_makewt(int nw, int *ip, double *w);
  20 |     void ooura_bitrv2(int n, int *ip, double *a);
  21 |     void ooura_bitrv2conj(int n, int *ip, double *a);
  22 |     void ooura_cftfsub(int n, double *a, double *w);
  23 |     void ooura_cftbsub(int n, double *a, double *w);
  24 | 
  25 |     if (n > (ip[0] << 2)) {
  26 |         ooura_makewt(n >> 2, ip, w);
  27 |     }
  28 |     if (n > 4) {
  29 |         if (isgn >= 0) {
  30 |             ooura_bitrv2(n, ip + 2, a);
  31 |             ooura_cftfsub(n, a, w);
  32 |         } else {
  33 |             ooura_bitrv2conj(n, ip + 2, a);
  34 |             ooura_cftbsub(n, a, w);
  35 |         }
  36 |     } else if (n == 4) {
  37 |         ooura_cftfsub(n, a, w);
  38 |     }
  39 | }
  40 | 
  41 |  void ooura_rdft(int n, int isgn, double *a, int *ip, double *w) {
  42 |     void ooura_makewt(int nw, int *ip, double *w);
  43 |     void ooura_makect(int nc, int *ip, double *c);
  44 |     void ooura_bitrv2(int n, int *ip, double *a);
  45 |     void ooura_cftfsub(int n, double *a, double *w);
  46 |     void ooura_cftbsub(int n, double *a, double *w);
  47 |     void ooura_rftfsub(int n, double *a, int nc, double *c);
  48 |     void ooura_rftbsub(int n, double *a, int nc, double *c);
  49 |     int nw, nc;
  50 |     double xi;
  51 | 
  52 |     nw = ip[0];
  53 |     if (n > (nw << 2)) {
  54 |         nw = n >> 2;
  55 |         ooura_makewt(nw, ip, w);
  56 |     }
  57 |     nc = ip[1];
  58 |     if (n > (nc << 2)) {
  59 |         nc = n >> 2;
  60 |         ooura_makect(nc, ip, w + nw);
  61 |     }
  62 |     if (isgn >= 0) {
  63 |         if (n > 4) {
  64 |             ooura_bitrv2(n, ip + 2, a);
  65 |             ooura_cftfsub(n, a, w);
  66 |             ooura_rftfsub(n, a, nc, w + nw);
  67 |         } else if (n == 4) {
  68 |             ooura_cftfsub(n, a, w);
  69 |         }
  70 |         xi = a[0] - a[1];
  71 |         a[0] += a[1];
  72 |         a[1] = xi;
  73 |     } else {
  74 |         a[1] = 0.5 * (a[0] - a[1]);
  75 |         a[0] -= a[1];
  76 |         if (n > 4) {
  77 |             ooura_rftbsub(n, a, nc, w + nw);
  78 |             ooura_bitrv2(n, ip + 2, a);
  79 |             ooura_cftbsub(n, a, w);
  80 |         } else if (n == 4) {
  81 |             ooura_cftfsub(n, a, w);
  82 |         }
  83 |     }
  84 | }
  85 | 
  86 |  void ooura_ddct(int n, int isgn, double *a, int *ip, double *w) {
  87 |     void ooura_makewt(int nw, int *ip, double *w);
  88 |     void ooura_makect(int nc, int *ip, double *c);
  89 |     void ooura_bitrv2(int n, int *ip, double *a);
  90 |     void ooura_cftfsub(int n, double *a, double *w);
  91 |     void ooura_cftbsub(int n, double *a, double *w);
  92 |     void ooura_rftfsub(int n, double *a, int nc, double *c);
  93 |     void ooura_rftbsub(int n, double *a, int nc, double *c);
  94 |     void ooura_dctsub(int n, double *a, int nc, double *c);
  95 |     int j, nw, nc;
  96 |     double xr;
  97 | 
  98 |     nw = ip[0];
  99 |     if (n > (nw << 2)) {
 100 |         nw = n >> 2;
 101 |         ooura_makewt(nw, ip, w);
 102 |     }
 103 |     nc = ip[1];
 104 |     if (n > nc) {
 105 |         nc = n;
 106 |         ooura_makect(nc, ip, w + nw);
 107 |     }
 108 |     if (isgn < 0) {
 109 |         xr = a[n - 1];
 110 |         for (j = n - 2; j >= 2; j -= 2) {
 111 |             a[j + 1] = a[j] - a[j - 1];
 112 |             a[j] += a[j - 1];
 113 |         }
 114 |         a[1] = a[0] - xr;
 115 |         a[0] += xr;
 116 |         if (n > 4) {
 117 |             ooura_rftbsub(n, a, nc, w + nw);
 118 |             ooura_bitrv2(n, ip + 2, a);
 119 |             ooura_cftbsub(n, a, w);
 120 |         } else if (n == 4) {
 121 |             ooura_cftfsub(n, a, w);
 122 |         }
 123 |     }
 124 |     ooura_dctsub(n, a, nc, w + nw);
 125 |     if (isgn >= 0) {
 126 |         if (n > 4) {
 127 |             ooura_bitrv2(n, ip + 2, a);
 128 |             ooura_cftfsub(n, a, w);
 129 |             ooura_rftfsub(n, a, nc, w + nw);
 130 |         } else if (n == 4) {
 131 |             ooura_cftfsub(n, a, w);
 132 |         }
 133 |         xr = a[0] - a[1];
 134 |         a[0] += a[1];
 135 |         for (j = 2; j < n; j += 2) {
 136 |             a[j - 1] = a[j] - a[j + 1];
 137 |             a[j] += a[j + 1];
 138 |         }
 139 |         a[n - 1] = xr;
 140 |     }
 141 | }
 142 | 
 143 |  void ooura_ddst(int n, int isgn, double *a, int *ip, double *w) {
 144 |     void ooura_makewt(int nw, int *ip, double *w);
 145 |     void ooura_makect(int nc, int *ip, double *c);
 146 |     void ooura_bitrv2(int n, int *ip, double *a);
 147 |     void ooura_cftfsub(int n, double *a, double *w);
 148 |     void ooura_cftbsub(int n, double *a, double *w);
 149 |     void ooura_rftfsub(int n, double *a, int nc, double *c);
 150 |     void ooura_rftbsub(int n, double *a, int nc, double *c);
 151 |     void ooura_dstsub(int n, double *a, int nc, double *c);
 152 |     int j, nw, nc;
 153 |     double xr;
 154 | 
 155 |     nw = ip[0];
 156 |     if (n > (nw << 2)) {
 157 |         nw = n >> 2;
 158 |         ooura_makewt(nw, ip, w);
 159 |     }
 160 |     nc = ip[1];
 161 |     if (n > nc) {
 162 |         nc = n;
 163 |         ooura_makect(nc, ip, w + nw);
 164 |     }
 165 |     if (isgn < 0) {
 166 |         xr = a[n - 1];
 167 |         for (j = n - 2; j >= 2; j -= 2) {
 168 |             a[j + 1] = -a[j] - a[j - 1];
 169 |             a[j] -= a[j - 1];
 170 |         }
 171 |         a[1] = a[0] + xr;
 172 |         a[0] -= xr;
 173 |         if (n > 4) {
 174 |             ooura_rftbsub(n, a, nc, w + nw);
 175 |             ooura_bitrv2(n, ip + 2, a);
 176 |             ooura_cftbsub(n, a, w);
 177 |         } else if (n == 4) {
 178 |             ooura_cftfsub(n, a, w);
 179 |         }
 180 |     }
 181 |     ooura_dstsub(n, a, nc, w + nw);
 182 |     if (isgn >= 0) {
 183 |         if (n > 4) {
 184 |             ooura_bitrv2(n, ip + 2, a);
 185 |             ooura_cftfsub(n, a, w);
 186 |             ooura_rftfsub(n, a, nc, w + nw);
 187 |         } else if (n == 4) {
 188 |             ooura_cftfsub(n, a, w);
 189 |         }
 190 |         xr = a[0] - a[1];
 191 |         a[0] += a[1];
 192 |         for (j = 2; j < n; j += 2) {
 193 |             a[j - 1] = -a[j] - a[j + 1];
 194 |             a[j] -= a[j + 1];
 195 |         }
 196 |         a[n - 1] = -xr;
 197 |     }
 198 | }
 199 | 
 200 |  void ooura_dfct(int n, double *a, double *t, int *ip, double *w) {
 201 |     void ooura_makewt(int nw, int *ip, double *w);
 202 |     void ooura_makect(int nc, int *ip, double *c);
 203 |     void ooura_bitrv2(int n, int *ip, double *a);
 204 |     void ooura_cftfsub(int n, double *a, double *w);
 205 |     void ooura_rftfsub(int n, double *a, int nc, double *c);
 206 |     void ooura_dctsub(int n, double *a, int nc, double *c);
 207 |     int j, k, l, m, mh, nw, nc;
 208 |     double xr, xi, yr, yi;
 209 | 
 210 |     nw = ip[0];
 211 |     if (n > (nw << 3)) {
 212 |         nw = n >> 3;
 213 |         ooura_makewt(nw, ip, w);
 214 |     }
 215 |     nc = ip[1];
 216 |     if (n > (nc << 1)) {
 217 |         nc = n >> 1;
 218 |         ooura_makect(nc, ip, w + nw);
 219 |     }
 220 |     m = n >> 1;
 221 |     yi = a[m];
 222 |     xi = a[0] + a[n];
 223 |     a[0] -= a[n];
 224 |     t[0] = xi - yi;
 225 |     t[m] = xi + yi;
 226 |     if (n > 2) {
 227 |         mh = m >> 1;
 228 |         for (j = 1; j < mh; j++) {
 229 |             k = m - j;
 230 |             xr = a[j] - a[n - j];
 231 |             xi = a[j] + a[n - j];
 232 |             yr = a[k] - a[n - k];
 233 |             yi = a[k] + a[n - k];
 234 |             a[j] = xr;
 235 |             a[k] = yr;
 236 |             t[j] = xi - yi;
 237 |             t[k] = xi + yi;
 238 |         }
 239 |         t[mh] = a[mh] + a[n - mh];
 240 |         a[mh] -= a[n - mh];
 241 |         ooura_dctsub(m, a, nc, w + nw);
 242 |         if (m > 4) {
 243 |             ooura_bitrv2(m, ip + 2, a);
 244 |             ooura_cftfsub(m, a, w);
 245 |             ooura_rftfsub(m, a, nc, w + nw);
 246 |         } else if (m == 4) {
 247 |             ooura_cftfsub(m, a, w);
 248 |         }
 249 |         a[n - 1] = a[0] - a[1];
 250 |         a[1] = a[0] + a[1];
 251 |         for (j = m - 2; j >= 2; j -= 2) {
 252 |             a[2 * j + 1] = a[j] + a[j + 1];
 253 |             a[2 * j - 1] = a[j] - a[j + 1];
 254 |         }
 255 |         l = 2;
 256 |         m = mh;
 257 |         while (m >= 2) {
 258 |             ooura_dctsub(m, t, nc, w + nw);
 259 |             if (m > 4) {
 260 |                 ooura_bitrv2(m, ip + 2, t);
 261 |                 ooura_cftfsub(m, t, w);
 262 |                 ooura_rftfsub(m, t, nc, w + nw);
 263 |             } else if (m == 4) {
 264 |                 ooura_cftfsub(m, t, w);
 265 |             }
 266 |             a[n - l] = t[0] - t[1];
 267 |             a[l] = t[0] + t[1];
 268 |             k = 0;
 269 |             for (j = 2; j < m; j += 2) {
 270 |                 k += l << 2;
 271 |                 a[k - l] = t[j] - t[j + 1];
 272 |                 a[k + l] = t[j] + t[j + 1];
 273 |             }
 274 |             l <<= 1;
 275 |             mh = m >> 1;
 276 |             for (j = 0; j < mh; j++) {
 277 |                 k = m - j;
 278 |                 t[j] = t[m + k] - t[m + j];
 279 |                 t[k] = t[m + k] + t[m + j];
 280 |             }
 281 |             t[mh] = t[m + mh];
 282 |             m = mh;
 283 |         }
 284 |         a[l] = t[0];
 285 |         a[n] = t[2] - t[1];
 286 |         a[0] = t[2] + t[1];
 287 |     } else {
 288 |         a[1] = a[0];
 289 |         a[2] = t[0];
 290 |         a[0] = t[1];
 291 |     }
 292 | }
 293 | 
 294 |  void ooura_dfst(int n, double *a, double *t, int *ip, double *w) {
 295 |     void ooura_makewt(int nw, int *ip, double *w);
 296 |     void ooura_makect(int nc, int *ip, double *c);
 297 |     void ooura_bitrv2(int n, int *ip, double *a);
 298 |     void ooura_cftfsub(int n, double *a, double *w);
 299 |     void ooura_rftfsub(int n, double *a, int nc, double *c);
 300 |     void ooura_dstsub(int n, double *a, int nc, double *c);
 301 |     int j, k, l, m, mh, nw, nc;
 302 |     double xr, xi, yr, yi;
 303 | 
 304 |     nw = ip[0];
 305 |     if (n > (nw << 3)) {
 306 |         nw = n >> 3;
 307 |         ooura_makewt(nw, ip, w);
 308 |     }
 309 |     nc = ip[1];
 310 |     if (n > (nc << 1)) {
 311 |         nc = n >> 1;
 312 |         ooura_makect(nc, ip, w + nw);
 313 |     }
 314 |     if (n > 2) {
 315 |         m = n >> 1;
 316 |         mh = m >> 1;
 317 |         for (j = 1; j < mh; j++) {
 318 |             k = m - j;
 319 |             xr = a[j] + a[n - j];
 320 |             xi = a[j] - a[n - j];
 321 |             yr = a[k] + a[n - k];
 322 |             yi = a[k] - a[n - k];
 323 |             a[j] = xr;
 324 |             a[k] = yr;
 325 |             t[j] = xi + yi;
 326 |             t[k] = xi - yi;
 327 |         }
 328 |         t[0] = a[mh] - a[n - mh];
 329 |         a[mh] += a[n - mh];
 330 |         a[0] = a[m];
 331 |         ooura_dstsub(m, a, nc, w + nw);
 332 |         if (m > 4) {
 333 |             ooura_bitrv2(m, ip + 2, a);
 334 |             ooura_cftfsub(m, a, w);
 335 |             ooura_rftfsub(m, a, nc, w + nw);
 336 |         } else if (m == 4) {
 337 |             ooura_cftfsub(m, a, w);
 338 |         }
 339 |         a[n - 1] = a[1] - a[0];
 340 |         a[1] = a[0] + a[1];
 341 |         for (j = m - 2; j >= 2; j -= 2) {
 342 |             a[2 * j + 1] = a[j] - a[j + 1];
 343 |             a[2 * j - 1] = -a[j] - a[j + 1];
 344 |         }
 345 |         l = 2;
 346 |         m = mh;
 347 |         while (m >= 2) {
 348 |             ooura_dstsub(m, t, nc, w + nw);
 349 |             if (m > 4) {
 350 |                 ooura_bitrv2(m, ip + 2, t);
 351 |                 ooura_cftfsub(m, t, w);
 352 |                 ooura_rftfsub(m, t, nc, w + nw);
 353 |             } else if (m == 4) {
 354 |                 ooura_cftfsub(m, t, w);
 355 |             }
 356 |             a[n - l] = t[1] - t[0];
 357 |             a[l] = t[0] + t[1];
 358 |             k = 0;
 359 |             for (j = 2; j < m; j += 2) {
 360 |                 k += l << 2;
 361 |                 a[k - l] = -t[j] - t[j + 1];
 362 |                 a[k + l] = t[j] - t[j + 1];
 363 |             }
 364 |             l <<= 1;
 365 |             mh = m >> 1;
 366 |             for (j = 1; j < mh; j++) {
 367 |                 k = m - j;
 368 |                 t[j] = t[m + k] + t[m + j];
 369 |                 t[k] = t[m + k] - t[m + j];
 370 |             }
 371 |             t[0] = t[m + mh];
 372 |             m = mh;
 373 |         }
 374 |         a[l] = t[0];
 375 |     }
 376 |     a[0] = 0;
 377 | }
 378 | 
 379 | /* -------- initializing routines -------- */
 380 | 
 381 |  void ooura_makewt(int nw, int *ip, double *w) {
 382 |     void ooura_bitrv2(int n, int *ip, double *a);
 383 |     int j, nwh;
 384 |     double delta, x, y;
 385 | 
 386 |     ip[0] = nw;
 387 |     ip[1] = 1;
 388 |     if (nw > 2) {
 389 |         nwh = nw >> 1;
 390 |         delta = atan(1.0) / nwh;
 391 |         w[0] = 1;
 392 |         w[1] = 0;
 393 |         w[nwh] = cos(delta * nwh);
 394 |         w[nwh + 1] = w[nwh];
 395 |         if (nwh > 2) {
 396 |             for (j = 2; j < nwh; j += 2) {
 397 |                 x = cos(delta * j);
 398 |                 y = sin(delta * j);
 399 |                 w[j] = x;
 400 |                 w[j + 1] = y;
 401 |                 w[nw - j] = y;
 402 |                 w[nw - j + 1] = x;
 403 |             }
 404 |             ooura_bitrv2(nw, ip + 2, w);
 405 |         }
 406 |     }
 407 | }
 408 | 
 409 |  void ooura_makect(int nc, int *ip, double *c) {
 410 |     int j, nch;
 411 |     double delta;
 412 | 
 413 |     ip[1] = nc;
 414 |     if (nc > 1) {
 415 |         nch = nc >> 1;
 416 |         delta = atan(1.0) / nch;
 417 |         c[0] = cos(delta * nch);
 418 |         c[nch] = 0.5 * c[0];
 419 |         for (j = 1; j < nch; j++) {
 420 |             c[j] = 0.5 * cos(delta * j);
 421 |             c[nc - j] = 0.5 * sin(delta * j);
 422 |         }
 423 |     }
 424 | }
 425 | 
 426 | /* -------- child routines -------- */
 427 | 
 428 |  void ooura_bitrv2(int n, int *ip, double *a) {
 429 |     int j, j1, k, k1, l, m, m2;
 430 |     double xr, xi, yr, yi;
 431 | 
 432 |     ip[0] = 0;
 433 |     l = n;
 434 |     m = 1;
 435 |     while ((m << 3) < l) {
 436 |         l >>= 1;
 437 |         for (j = 0; j < m; j++) {
 438 |             ip[m + j] = ip[j] + l;
 439 |         }
 440 |         m <<= 1;
 441 |     }
 442 |     m2 = 2 * m;
 443 |     if ((m << 3) == l) {
 444 |         for (k = 0; k < m; k++) {
 445 |             for (j = 0; j < k; j++) {
 446 |                 j1 = 2 * j + ip[k];
 447 |                 k1 = 2 * k + ip[j];
 448 |                 xr = a[j1];
 449 |                 xi = a[j1 + 1];
 450 |                 yr = a[k1];
 451 |                 yi = a[k1 + 1];
 452 |                 a[j1] = yr;
 453 |                 a[j1 + 1] = yi;
 454 |                 a[k1] = xr;
 455 |                 a[k1 + 1] = xi;
 456 |                 j1 += m2;
 457 |                 k1 += 2 * m2;
 458 |                 xr = a[j1];
 459 |                 xi = a[j1 + 1];
 460 |                 yr = a[k1];
 461 |                 yi = a[k1 + 1];
 462 |                 a[j1] = yr;
 463 |                 a[j1 + 1] = yi;
 464 |                 a[k1] = xr;
 465 |                 a[k1 + 1] = xi;
 466 |                 j1 += m2;
 467 |                 k1 -= m2;
 468 |                 xr = a[j1];
 469 |                 xi = a[j1 + 1];
 470 |                 yr = a[k1];
 471 |                 yi = a[k1 + 1];
 472 |                 a[j1] = yr;
 473 |                 a[j1 + 1] = yi;
 474 |                 a[k1] = xr;
 475 |                 a[k1 + 1] = xi;
 476 |                 j1 += m2;
 477 |                 k1 += 2 * m2;
 478 |                 xr = a[j1];
 479 |                 xi = a[j1 + 1];
 480 |                 yr = a[k1];
 481 |                 yi = a[k1 + 1];
 482 |                 a[j1] = yr;
 483 |                 a[j1 + 1] = yi;
 484 |                 a[k1] = xr;
 485 |                 a[k1 + 1] = xi;
 486 |             }
 487 |             j1 = 2 * k + m2 + ip[k];
 488 |             k1 = j1 + m2;
 489 |             xr = a[j1];
 490 |             xi = a[j1 + 1];
 491 |             yr = a[k1];
 492 |             yi = a[k1 + 1];
 493 |             a[j1] = yr;
 494 |             a[j1 + 1] = yi;
 495 |             a[k1] = xr;
 496 |             a[k1 + 1] = xi;
 497 |         }
 498 |     } else {
 499 |         for (k = 1; k < m; k++) {
 500 |             for (j = 0; j < k; j++) {
 501 |                 j1 = 2 * j + ip[k];
 502 |                 k1 = 2 * k + ip[j];
 503 |                 xr = a[j1];
 504 |                 xi = a[j1 + 1];
 505 |                 yr = a[k1];
 506 |                 yi = a[k1 + 1];
 507 |                 a[j1] = yr;
 508 |                 a[j1 + 1] = yi;
 509 |                 a[k1] = xr;
 510 |                 a[k1 + 1] = xi;
 511 |                 j1 += m2;
 512 |                 k1 += m2;
 513 |                 xr = a[j1];
 514 |                 xi = a[j1 + 1];
 515 |                 yr = a[k1];
 516 |                 yi = a[k1 + 1];
 517 |                 a[j1] = yr;
 518 |                 a[j1 + 1] = yi;
 519 |                 a[k1] = xr;
 520 |                 a[k1 + 1] = xi;
 521 |             }
 522 |         }
 523 |     }
 524 | }
 525 | 
 526 |  void ooura_bitrv2conj(int n, int *ip, double *a) {
 527 |     int j, j1, k, k1, l, m, m2;
 528 |     double xr, xi, yr, yi;
 529 | 
 530 |     ip[0] = 0;
 531 |     l = n;
 532 |     m = 1;
 533 |     while ((m << 3) < l) {
 534 |         l >>= 1;
 535 |         for (j = 0; j < m; j++) {
 536 |             ip[m + j] = ip[j] + l;
 537 |         }
 538 |         m <<= 1;
 539 |     }
 540 |     m2 = 2 * m;
 541 |     if ((m << 3) == l) {
 542 |         for (k = 0; k < m; k++) {
 543 |             for (j = 0; j < k; j++) {
 544 |                 j1 = 2 * j + ip[k];
 545 |                 k1 = 2 * k + ip[j];
 546 |                 xr = a[j1];
 547 |                 xi = -a[j1 + 1];
 548 |                 yr = a[k1];
 549 |                 yi = -a[k1 + 1];
 550 |                 a[j1] = yr;
 551 |                 a[j1 + 1] = yi;
 552 |                 a[k1] = xr;
 553 |                 a[k1 + 1] = xi;
 554 |                 j1 += m2;
 555 |                 k1 += 2 * m2;
 556 |                 xr = a[j1];
 557 |                 xi = -a[j1 + 1];
 558 |                 yr = a[k1];
 559 |                 yi = -a[k1 + 1];
 560 |                 a[j1] = yr;
 561 |                 a[j1 + 1] = yi;
 562 |                 a[k1] = xr;
 563 |                 a[k1 + 1] = xi;
 564 |                 j1 += m2;
 565 |                 k1 -= m2;
 566 |                 xr = a[j1];
 567 |                 xi = -a[j1 + 1];
 568 |                 yr = a[k1];
 569 |                 yi = -a[k1 + 1];
 570 |                 a[j1] = yr;
 571 |                 a[j1 + 1] = yi;
 572 |                 a[k1] = xr;
 573 |                 a[k1 + 1] = xi;
 574 |                 j1 += m2;
 575 |                 k1 += 2 * m2;
 576 |                 xr = a[j1];
 577 |                 xi = -a[j1 + 1];
 578 |                 yr = a[k1];
 579 |                 yi = -a[k1 + 1];
 580 |                 a[j1] = yr;
 581 |                 a[j1 + 1] = yi;
 582 |                 a[k1] = xr;
 583 |                 a[k1 + 1] = xi;
 584 |             }
 585 |             k1 = 2 * k + ip[k];
 586 |             a[k1 + 1] = -a[k1 + 1];
 587 |             j1 = k1 + m2;
 588 |             k1 = j1 + m2;
 589 |             xr = a[j1];
 590 |             xi = -a[j1 + 1];
 591 |             yr = a[k1];
 592 |             yi = -a[k1 + 1];
 593 |             a[j1] = yr;
 594 |             a[j1 + 1] = yi;
 595 |             a[k1] = xr;
 596 |             a[k1 + 1] = xi;
 597 |             k1 += m2;
 598 |             a[k1 + 1] = -a[k1 + 1];
 599 |         }
 600 |     } else {
 601 |         a[1] = -a[1];
 602 |         a[m2 + 1] = -a[m2 + 1];
 603 |         for (k = 1; k < m; k++) {
 604 |             for (j = 0; j < k; j++) {
 605 |                 j1 = 2 * j + ip[k];
 606 |                 k1 = 2 * k + ip[j];
 607 |                 xr = a[j1];
 608 |                 xi = -a[j1 + 1];
 609 |                 yr = a[k1];
 610 |                 yi = -a[k1 + 1];
 611 |                 a[j1] = yr;
 612 |                 a[j1 + 1] = yi;
 613 |                 a[k1] = xr;
 614 |                 a[k1 + 1] = xi;
 615 |                 j1 += m2;
 616 |                 k1 += m2;
 617 |                 xr = a[j1];
 618 |                 xi = -a[j1 + 1];
 619 |                 yr = a[k1];
 620 |                 yi = -a[k1 + 1];
 621 |                 a[j1] = yr;
 622 |                 a[j1 + 1] = yi;
 623 |                 a[k1] = xr;
 624 |                 a[k1 + 1] = xi;
 625 |             }
 626 |             k1 = 2 * k + ip[k];
 627 |             a[k1 + 1] = -a[k1 + 1];
 628 |             a[k1 + m2 + 1] = -a[k1 + m2 + 1];
 629 |         }
 630 |     }
 631 | }
 632 | 
 633 |  void ooura_cftfsub(int n, double *a, double *w) {
 634 |     void ooura_cft1st(int n, double *a, double *w);
 635 |     void ooura_cftmdl(int n, int l, double *a, double *w);
 636 |     int j, j1, j2, j3, l;
 637 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
 638 | 
 639 |     l = 2;
 640 |     if (n > 8) {
 641 |         ooura_cft1st(n, a, w);
 642 |         l = 8;
 643 |         while ((l << 2) < n) {
 644 |             ooura_cftmdl(n, l, a, w);
 645 |             l <<= 2;
 646 |         }
 647 |     }
 648 |     if ((l << 2) == n) {
 649 |         for (j = 0; j < l; j += 2) {
 650 |             j1 = j + l;
 651 |             j2 = j1 + l;
 652 |             j3 = j2 + l;
 653 |             x0r = a[j] + a[j1];
 654 |             x0i = a[j + 1] + a[j1 + 1];
 655 |             x1r = a[j] - a[j1];
 656 |             x1i = a[j + 1] - a[j1 + 1];
 657 |             x2r = a[j2] + a[j3];
 658 |             x2i = a[j2 + 1] + a[j3 + 1];
 659 |             x3r = a[j2] - a[j3];
 660 |             x3i = a[j2 + 1] - a[j3 + 1];
 661 |             a[j] = x0r + x2r;
 662 |             a[j + 1] = x0i + x2i;
 663 |             a[j2] = x0r - x2r;
 664 |             a[j2 + 1] = x0i - x2i;
 665 |             a[j1] = x1r - x3i;
 666 |             a[j1 + 1] = x1i + x3r;
 667 |             a[j3] = x1r + x3i;
 668 |             a[j3 + 1] = x1i - x3r;
 669 |         }
 670 |     } else {
 671 |         for (j = 0; j < l; j += 2) {
 672 |             j1 = j + l;
 673 |             x0r = a[j] - a[j1];
 674 |             x0i = a[j + 1] - a[j1 + 1];
 675 |             a[j] += a[j1];
 676 |             a[j + 1] += a[j1 + 1];
 677 |             a[j1] = x0r;
 678 |             a[j1 + 1] = x0i;
 679 |         }
 680 |     }
 681 | }
 682 | 
 683 |  void ooura_cftbsub(int n, double *a, double *w) {
 684 |     void ooura_cft1st(int n, double *a, double *w);
 685 |     void ooura_cftmdl(int n, int l, double *a, double *w);
 686 |     int j, j1, j2, j3, l;
 687 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
 688 | 
 689 |     l = 2;
 690 |     if (n > 8) {
 691 |         ooura_cft1st(n, a, w);
 692 |         l = 8;
 693 |         while ((l << 2) < n) {
 694 |             ooura_cftmdl(n, l, a, w);
 695 |             l <<= 2;
 696 |         }
 697 |     }
 698 |     if ((l << 2) == n) {
 699 |         for (j = 0; j < l; j += 2) {
 700 |             j1 = j + l;
 701 |             j2 = j1 + l;
 702 |             j3 = j2 + l;
 703 |             x0r = a[j] + a[j1];
 704 |             x0i = -a[j + 1] - a[j1 + 1];
 705 |             x1r = a[j] - a[j1];
 706 |             x1i = -a[j + 1] + a[j1 + 1];
 707 |             x2r = a[j2] + a[j3];
 708 |             x2i = a[j2 + 1] + a[j3 + 1];
 709 |             x3r = a[j2] - a[j3];
 710 |             x3i = a[j2 + 1] - a[j3 + 1];
 711 |             a[j] = x0r + x2r;
 712 |             a[j + 1] = x0i - x2i;
 713 |             a[j2] = x0r - x2r;
 714 |             a[j2 + 1] = x0i + x2i;
 715 |             a[j1] = x1r - x3i;
 716 |             a[j1 + 1] = x1i - x3r;
 717 |             a[j3] = x1r + x3i;
 718 |             a[j3 + 1] = x1i + x3r;
 719 |         }
 720 |     } else {
 721 |         for (j = 0; j < l; j += 2) {
 722 |             j1 = j + l;
 723 |             x0r = a[j] - a[j1];
 724 |             x0i = -a[j + 1] + a[j1 + 1];
 725 |             a[j] += a[j1];
 726 |             a[j + 1] = -a[j + 1] - a[j1 + 1];
 727 |             a[j1] = x0r;
 728 |             a[j1 + 1] = x0i;
 729 |         }
 730 |     }
 731 | }
 732 | 
 733 |  void ooura_cft1st(int n, double *a, double *w) {
 734 |     int j, k1, k2;
 735 |     double wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
 736 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
 737 | 
 738 |     x0r = a[0] + a[2];
 739 |     x0i = a[1] + a[3];
 740 |     x1r = a[0] - a[2];
 741 |     x1i = a[1] - a[3];
 742 |     x2r = a[4] + a[6];
 743 |     x2i = a[5] + a[7];
 744 |     x3r = a[4] - a[6];
 745 |     x3i = a[5] - a[7];
 746 |     a[0] = x0r + x2r;
 747 |     a[1] = x0i + x2i;
 748 |     a[4] = x0r - x2r;
 749 |     a[5] = x0i - x2i;
 750 |     a[2] = x1r - x3i;
 751 |     a[3] = x1i + x3r;
 752 |     a[6] = x1r + x3i;
 753 |     a[7] = x1i - x3r;
 754 |     wk1r = w[2];
 755 |     x0r = a[8] + a[10];
 756 |     x0i = a[9] + a[11];
 757 |     x1r = a[8] - a[10];
 758 |     x1i = a[9] - a[11];
 759 |     x2r = a[12] + a[14];
 760 |     x2i = a[13] + a[15];
 761 |     x3r = a[12] - a[14];
 762 |     x3i = a[13] - a[15];
 763 |     a[8] = x0r + x2r;
 764 |     a[9] = x0i + x2i;
 765 |     a[12] = x2i - x0i;
 766 |     a[13] = x0r - x2r;
 767 |     x0r = x1r - x3i;
 768 |     x0i = x1i + x3r;
 769 |     a[10] = wk1r * (x0r - x0i);
 770 |     a[11] = wk1r * (x0r + x0i);
 771 |     x0r = x3i + x1r;
 772 |     x0i = x3r - x1i;
 773 |     a[14] = wk1r * (x0i - x0r);
 774 |     a[15] = wk1r * (x0i + x0r);
 775 |     k1 = 0;
 776 |     for (j = 16; j < n; j += 16) {
 777 |         k1 += 2;
 778 |         k2 = 2 * k1;
 779 |         wk2r = w[k1];
 780 |         wk2i = w[k1 + 1];
 781 |         wk1r = w[k2];
 782 |         wk1i = w[k2 + 1];
 783 |         wk3r = wk1r - 2 * wk2i * wk1i;
 784 |         wk3i = 2 * wk2i * wk1r - wk1i;
 785 |         x0r = a[j] + a[j + 2];
 786 |         x0i = a[j + 1] + a[j + 3];
 787 |         x1r = a[j] - a[j + 2];
 788 |         x1i = a[j + 1] - a[j + 3];
 789 |         x2r = a[j + 4] + a[j + 6];
 790 |         x2i = a[j + 5] + a[j + 7];
 791 |         x3r = a[j + 4] - a[j + 6];
 792 |         x3i = a[j + 5] - a[j + 7];
 793 |         a[j] = x0r + x2r;
 794 |         a[j + 1] = x0i + x2i;
 795 |         x0r -= x2r;
 796 |         x0i -= x2i;
 797 |         a[j + 4] = wk2r * x0r - wk2i * x0i;
 798 |         a[j + 5] = wk2r * x0i + wk2i * x0r;
 799 |         x0r = x1r - x3i;
 800 |         x0i = x1i + x3r;
 801 |         a[j + 2] = wk1r * x0r - wk1i * x0i;
 802 |         a[j + 3] = wk1r * x0i + wk1i * x0r;
 803 |         x0r = x1r + x3i;
 804 |         x0i = x1i - x3r;
 805 |         a[j + 6] = wk3r * x0r - wk3i * x0i;
 806 |         a[j + 7] = wk3r * x0i + wk3i * x0r;
 807 |         wk1r = w[k2 + 2];
 808 |         wk1i = w[k2 + 3];
 809 |         wk3r = wk1r - 2 * wk2r * wk1i;
 810 |         wk3i = 2 * wk2r * wk1r - wk1i;
 811 |         x0r = a[j + 8] + a[j + 10];
 812 |         x0i = a[j + 9] + a[j + 11];
 813 |         x1r = a[j + 8] - a[j + 10];
 814 |         x1i = a[j + 9] - a[j + 11];
 815 |         x2r = a[j + 12] + a[j + 14];
 816 |         x2i = a[j + 13] + a[j + 15];
 817 |         x3r = a[j + 12] - a[j + 14];
 818 |         x3i = a[j + 13] - a[j + 15];
 819 |         a[j + 8] = x0r + x2r;
 820 |         a[j + 9] = x0i + x2i;
 821 |         x0r -= x2r;
 822 |         x0i -= x2i;
 823 |         a[j + 12] = -wk2i * x0r - wk2r * x0i;
 824 |         a[j + 13] = -wk2i * x0i + wk2r * x0r;
 825 |         x0r = x1r - x3i;
 826 |         x0i = x1i + x3r;
 827 |         a[j + 10] = wk1r * x0r - wk1i * x0i;
 828 |         a[j + 11] = wk1r * x0i + wk1i * x0r;
 829 |         x0r = x1r + x3i;
 830 |         x0i = x1i - x3r;
 831 |         a[j + 14] = wk3r * x0r - wk3i * x0i;
 832 |         a[j + 15] = wk3r * x0i + wk3i * x0r;
 833 |     }
 834 | }
 835 | 
 836 |  void ooura_cftmdl(int n, int l, double *a, double *w) {
 837 |     int j, j1, j2, j3, k, k1, k2, m, m2;
 838 |     double wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
 839 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
 840 | 
 841 |     m = l << 2;
 842 |     for (j = 0; j < l; j += 2) {
 843 |         j1 = j + l;
 844 |         j2 = j1 + l;
 845 |         j3 = j2 + l;
 846 |         x0r = a[j] + a[j1];
 847 |         x0i = a[j + 1] + a[j1 + 1];
 848 |         x1r = a[j] - a[j1];
 849 |         x1i = a[j + 1] - a[j1 + 1];
 850 |         x2r = a[j2] + a[j3];
 851 |         x2i = a[j2 + 1] + a[j3 + 1];
 852 |         x3r = a[j2] - a[j3];
 853 |         x3i = a[j2 + 1] - a[j3 + 1];
 854 |         a[j] = x0r + x2r;
 855 |         a[j + 1] = x0i + x2i;
 856 |         a[j2] = x0r - x2r;
 857 |         a[j2 + 1] = x0i - x2i;
 858 |         a[j1] = x1r - x3i;
 859 |         a[j1 + 1] = x1i + x3r;
 860 |         a[j3] = x1r + x3i;
 861 |         a[j3 + 1] = x1i - x3r;
 862 |     }
 863 |     wk1r = w[2];
 864 |     for (j = m; j < l + m; j += 2) {
 865 |         j1 = j + l;
 866 |         j2 = j1 + l;
 867 |         j3 = j2 + l;
 868 |         x0r = a[j] + a[j1];
 869 |         x0i = a[j + 1] + a[j1 + 1];
 870 |         x1r = a[j] - a[j1];
 871 |         x1i = a[j + 1] - a[j1 + 1];
 872 |         x2r = a[j2] + a[j3];
 873 |         x2i = a[j2 + 1] + a[j3 + 1];
 874 |         x3r = a[j2] - a[j3];
 875 |         x3i = a[j2 + 1] - a[j3 + 1];
 876 |         a[j] = x0r + x2r;
 877 |         a[j + 1] = x0i + x2i;
 878 |         a[j2] = x2i - x0i;
 879 |         a[j2 + 1] = x0r - x2r;
 880 |         x0r = x1r - x3i;
 881 |         x0i = x1i + x3r;
 882 |         a[j1] = wk1r * (x0r - x0i);
 883 |         a[j1 + 1] = wk1r * (x0r + x0i);
 884 |         x0r = x3i + x1r;
 885 |         x0i = x3r - x1i;
 886 |         a[j3] = wk1r * (x0i - x0r);
 887 |         a[j3 + 1] = wk1r * (x0i + x0r);
 888 |     }
 889 |     k1 = 0;
 890 |     m2 = 2 * m;
 891 |     for (k = m2; k < n; k += m2) {
 892 |         k1 += 2;
 893 |         k2 = 2 * k1;
 894 |         wk2r = w[k1];
 895 |         wk2i = w[k1 + 1];
 896 |         wk1r = w[k2];
 897 |         wk1i = w[k2 + 1];
 898 |         wk3r = wk1r - 2 * wk2i * wk1i;
 899 |         wk3i = 2 * wk2i * wk1r - wk1i;
 900 |         for (j = k; j < l + k; j += 2) {
 901 |             j1 = j + l;
 902 |             j2 = j1 + l;
 903 |             j3 = j2 + l;
 904 |             x0r = a[j] + a[j1];
 905 |             x0i = a[j + 1] + a[j1 + 1];
 906 |             x1r = a[j] - a[j1];
 907 |             x1i = a[j + 1] - a[j1 + 1];
 908 |             x2r = a[j2] + a[j3];
 909 |             x2i = a[j2 + 1] + a[j3 + 1];
 910 |             x3r = a[j2] - a[j3];
 911 |             x3i = a[j2 + 1] - a[j3 + 1];
 912 |             a[j] = x0r + x2r;
 913 |             a[j + 1] = x0i + x2i;
 914 |             x0r -= x2r;
 915 |             x0i -= x2i;
 916 |             a[j2] = wk2r * x0r - wk2i * x0i;
 917 |             a[j2 + 1] = wk2r * x0i + wk2i * x0r;
 918 |             x0r = x1r - x3i;
 919 |             x0i = x1i + x3r;
 920 |             a[j1] = wk1r * x0r - wk1i * x0i;
 921 |             a[j1 + 1] = wk1r * x0i + wk1i * x0r;
 922 |             x0r = x1r + x3i;
 923 |             x0i = x1i - x3r;
 924 |             a[j3] = wk3r * x0r - wk3i * x0i;
 925 |             a[j3 + 1] = wk3r * x0i + wk3i * x0r;
 926 |         }
 927 |         wk1r = w[k2 + 2];
 928 |         wk1i = w[k2 + 3];
 929 |         wk3r = wk1r - 2 * wk2r * wk1i;
 930 |         wk3i = 2 * wk2r * wk1r - wk1i;
 931 |         for (j = k + m; j < l + (k + m); j += 2) {
 932 |             j1 = j + l;
 933 |             j2 = j1 + l;
 934 |             j3 = j2 + l;
 935 |             x0r = a[j] + a[j1];
 936 |             x0i = a[j + 1] + a[j1 + 1];
 937 |             x1r = a[j] - a[j1];
 938 |             x1i = a[j + 1] - a[j1 + 1];
 939 |             x2r = a[j2] + a[j3];
 940 |             x2i = a[j2 + 1] + a[j3 + 1];
 941 |             x3r = a[j2] - a[j3];
 942 |             x3i = a[j2 + 1] - a[j3 + 1];
 943 |             a[j] = x0r + x2r;
 944 |             a[j + 1] = x0i + x2i;
 945 |             x0r -= x2r;
 946 |             x0i -= x2i;
 947 |             a[j2] = -wk2i * x0r - wk2r * x0i;
 948 |             a[j2 + 1] = -wk2i * x0i + wk2r * x0r;
 949 |             x0r = x1r - x3i;
 950 |             x0i = x1i + x3r;
 951 |             a[j1] = wk1r * x0r - wk1i * x0i;
 952 |             a[j1 + 1] = wk1r * x0i + wk1i * x0r;
 953 |             x0r = x1r + x3i;
 954 |             x0i = x1i - x3r;
 955 |             a[j3] = wk3r * x0r - wk3i * x0i;
 956 |             a[j3 + 1] = wk3r * x0i + wk3i * x0r;
 957 |         }
 958 |     }
 959 | }
 960 | 
 961 |  void ooura_rftfsub(int n, double *a, int nc, double *c) {
 962 |     int j, k, kk, ks, m;
 963 |     double wkr, wki, xr, xi, yr, yi;
 964 | 
 965 |     m = n >> 1;
 966 |     ks = 2 * nc / m;
 967 |     kk = 0;
 968 |     for (j = 2; j < m; j += 2) {
 969 |         k = n - j;
 970 |         kk += ks;
 971 |         wkr = 0.5 - c[nc - kk];
 972 |         wki = c[kk];
 973 |         xr = a[j] - a[k];
 974 |         xi = a[j + 1] + a[k + 1];
 975 |         yr = wkr * xr - wki * xi;
 976 |         yi = wkr * xi + wki * xr;
 977 |         a[j] -= yr;
 978 |         a[j + 1] -= yi;
 979 |         a[k] += yr;
 980 |         a[k + 1] -= yi;
 981 |     }
 982 | }
 983 | 
 984 |  void ooura_rftbsub(int n, double *a, int nc, double *c) {
 985 |     int j, k, kk, ks, m;
 986 |     double wkr, wki, xr, xi, yr, yi;
 987 | 
 988 |     a[1] = -a[1];
 989 |     m = n >> 1;
 990 |     ks = 2 * nc / m;
 991 |     kk = 0;
 992 |     for (j = 2; j < m; j += 2) {
 993 |         k = n - j;
 994 |         kk += ks;
 995 |         wkr = 0.5 - c[nc - kk];
 996 |         wki = c[kk];
 997 |         xr = a[j] - a[k];
 998 |         xi = a[j + 1] + a[k + 1];
 999 |         yr = wkr * xr + wki * xi;
1000 |         yi = wkr * xi - wki * xr;
1001 |         a[j] -= yr;
1002 |         a[j + 1] = yi - a[j + 1];
1003 |         a[k] += yr;
1004 |         a[k + 1] = yi - a[k + 1];
1005 |     }
1006 |     a[m + 1] = -a[m + 1];
1007 | }
1008 | 
1009 |  void ooura_dctsub(int n, double *a, int nc, double *c) {
1010 |     int j, k, kk, ks, m;
1011 |     double wkr, wki, xr;
1012 | 
1013 |     m = n >> 1;
1014 |     ks = nc / n;
1015 |     kk = 0;
1016 |     for (j = 1; j < m; j++) {
1017 |         k = n - j;
1018 |         kk += ks;
1019 |         wkr = c[kk] - c[nc - kk];
1020 |         wki = c[kk] + c[nc - kk];
1021 |         xr = wki * a[j] - wkr * a[k];
1022 |         a[j] = wkr * a[j] + wki * a[k];
1023 |         a[k] = xr;
1024 |     }
1025 |     a[m] *= c[0];
1026 | }
1027 | 
1028 |  void ooura_dstsub(int n, double *a, int nc, double *c) {
1029 |     int j, k, kk, ks, m;
1030 |     double wkr, wki, xr;
1031 | 
1032 |     m = n >> 1;
1033 |     ks = nc / n;
1034 |     kk = 0;
1035 |     for (j = 1; j < m; j++) {
1036 |         k = n - j;
1037 |         kk += ks;
1038 |         wkr = c[kk] - c[nc - kk];
1039 |         wki = c[kk] + c[nc - kk];
1040 |         xr = wki * a[k] - wkr * a[j];
1041 |         a[k] = wkr * a[k] + wki * a[j];
1042 |         a[j] = xr;
1043 |     }
1044 |     a[m] *= c[0];
1045 | }
1046 | 
1047 | 
1048 | 
1049 | 


--------------------------------------------------------------------------------
/WAV/WAV.cpp:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shleee47/mpWAV-Sound-Source-Localization/c3735d7b9235d359111f1284333501d2116e21a5/WAV/WAV.cpp


--------------------------------------------------------------------------------
/WAV/WAV.h:
--------------------------------------------------------------------------------
  1 | #ifndef _H_WAV_
  2 | #define _H_WAV_
  3 | 
  4 | #include <stdlib.h>
  5 | #include <stdio.h>
  6 | //for types
  7 | #include <stdint.h>
  8 | 
  9 | #include <cstdlib>
 10 | #include <string.h>
 11 | #include <vector>
 12 | 
 13 | // Data Type is fixed as short
 14 | 
 15 | class WAV {
 16 |   /*
 17 |    * http://www-mmsp.ece.mcgill.ca/Documents/AudioFormats/WAVE/WAVE.html
 18 |    * */
 19 | private:
 20 |   bool non_pcm;
 21 |   // 4bytes fixed size header infomation -> uint32_t
 22 |   char riff_id[4];    // riff string
 23 |   uint32_t riff_size; // overall size of file in bytes
 24 |   char wave_id[4];    // wave string
 25 |   char fmt_id[4];     // fmt string with trailing null char
 26 | 
 27 |   //          | pcm  | non-pcm
 28 |   // fmt_size |  16  |     18
 29 |   //
 30 |   uint32_t fmt_size;  // format chunk size 16,18,40
 31 |   short
 32 |       fmt_type; // format type 1-PCM 3-IEEE float 6- 8bit A law, 7- 8bit ,u law
 33 |   unsigned short channels;       // no of channel
 34 |   uint32_t sample_rate; // SampleRate(blocks per second)
 35 |   uint32_t byte_rate;   // ByteRate = SampleRate * NumChannels * BitsPerSample/8
 36 |   short block_align;    // NumChannels * BitsPerSample/8
 37 |   short bit_per_sample; // bits per sample, 8 - 8bits, 16-16bits etc
 38 | 
 39 |   /*   (if non -pcm )*/
 40 |   uint32_t cb_size;     //size of the extension
 41 |   char fact_id[4];
 42 |   uint32_t fact_size;
 43 |   uint32_t dwSampleLength; 
 44 | 
 45 |   char data_id[4];      // DATA string or FLLR string
 46 |   uint32_t data_size;   // NumSamples * NumChannels * BitsPerSample/8 - size of
 47 |                         // the nex chunk that will be read
 48 |   FILE *file;
 49 |   bool IsOpen;
 50 |   const char *file_name;
 51 |   // For Input usage only
 52 |   bool use_buf;
 53 |   int frame_size;
 54 |   int shift_size;
 55 | 
 56 |   int size_unit;
 57 | 
 58 |   void* buf; 
 59 | //  short* buf; 
 60 | 
 61 | public:
 62 |   WAV();
 63 |   WAV(short _ch, uint32_t _rate);
 64 |   WAV(short _ch, uint32_t _rate, int frame_size, int shift_size);
 65 |   ~WAV();
 66 |   int NewFile(const char *_file_name);
 67 |   int OpenFile(const char *_file_name);
 68 |   int Append(short *app_data, unsigned int app_size);
 69 |   int Append(float*app_data, unsigned int app_size);
 70 |   void WriteHeader();
 71 |   void Finish();
 72 | 
 73 |   void ReadHeader();
 74 | 
 75 |   /* There might be compile error for ReadUnit() in Visual Studio.
 76 |    * in this case, try to update your VS to most recent version. */
 77 |   size_t ReadUnit(short*dest,int unit);
 78 |   size_t ReadUnit(float*dest,int unit);
 79 |   int IsEOF() const;
 80 | 
 81 |   void Print() const;
 82 |   void Rewind();
 83 | 
 84 |   int Convert2ShiftedArray(double **raw);
 85 |   int Convert2ShiftedArray(double *raw);
 86 | 
 87 |   // Split 2 channel Wav into two 1 channel wav files.
 88 |   void SplitBy2(const char* f1,const char* f2);
 89 |   void SetSizes(int frame,int shift);
 90 | 
 91 |   int GetChannels();
 92 |   bool GetIsOpen();
 93 |   uint32_t GetSize(); 
 94 |   uint32_t GetSizeUnit(); 
 95 |   uint32_t GetSampleRate();
 96 |   short GetFmtType();
 97 |   void UseBuf(int frame_size,int shift_size);
 98 |   bool checkValidHeader();
 99 |   void* GetBuf();
100 | 
101 |   /*Split Wav file into each channel */
102 |   void Split(char* );
103 | };
104 | 
105 | 
106 | 
107 | #endif
108 | 


--------------------------------------------------------------------------------
/include/HannWindow.h:
--------------------------------------------------------------------------------
  1 | #ifndef _H_HANN_WINDOW_
  2 | #define _H_HANN_WINDOW_
  3 | 
  4 | #include <cmath>
  5 | #include <cstdio>
  6 | 
  7 | #define M_PI 3.14159265358979323846
  8 | 
  9 | class HannWindow {
 10 | private:
 11 |     double *hann;
 12 |     int shift_size;
 13 |     int frame_size;
 14 | 
 15 | 
 16 | public:
 17 |     inline HannWindow(int _frame_size, int _shift_size);
 18 |     inline ~HannWindow();
 19 |     // 2D
 20 |     inline void Process(double ** buf, int channels);
 21 |     // 1D - multi channel
 22 |     inline void Process(double * buf, int channels);
 23 |     // 1D - single channel 
 24 |     inline void Process(double * buf);
 25 |     // 2D
 26 |     inline void WindowWithScaling(double ** buf, int channels);
 27 |     // 1D - multi channel
 28 |     inline void WindowWithScaling(double * buf, int channels);
 29 |     // 1D - single channel 
 30 |     inline void WindowWithScaling(double * buf);
 31 | };
 32 | 
 33 | inline HannWindow::HannWindow(int _frame_size, int _shift_size) {
 34 |     int i;
 35 |     double temp;
 36 | 
 37 |     shift_size = _shift_size;
 38 |     frame_size = _frame_size;
 39 | 
 40 |     // From WPE_Online
 41 |     /*
 42 |      *
 43 |      Nfft = BufferSize * 4;
 44 |      Nfreq = Nfft / 2 + 1;
 45 |      Nwin = BufferSize * 4;
 46 |      *
 47 |      */
 48 |     hann = new double[frame_size];
 49 | 
 50 |     switch (frame_size / shift_size) {
 51 |     case 4:
 52 |         hann[0] = 0.0;
 53 |         for (i = 1; i < frame_size; ++i)
 54 |             hann[i] = 0.5 * (1.0 - cos(2.0 * M_PI * (double)i / (double)frame_size));
 55 | 
 56 |         temp = sqrt((double)2 / 3);
 57 |         for (i = 1; i < frame_size; i++)
 58 |             hann[i] *= temp;
 59 | 
 60 |         break;
 61 | 
 62 |     case 2:
 63 |         for (i = 0; i < frame_size; i++) {
 64 |             hann[i] = sin(M_PI * (i + 0.5) / frame_size);
 65 |         }
 66 | 
 67 |         break;
 68 |     default:
 69 |         printf("ERROR::frame_size/shift_size(%d) is Not Supported\n",
 70 |                frame_size / shift_size);
 71 |         exit(-1);
 72 |     }
 73 | #ifndef NDEBUG
 74 | /*
 75 |   printf("INFO::HannWindow\n");
 76 |   for(i=0;i<frame_size;i++)
 77 |   printf("%lf ",hann[i]);
 78 |   printf("\n");
 79 | */
 80 | #endif
 81 | }
 82 | 
 83 | inline HannWindow::~HannWindow() { delete[] hann; }
 84 | 
 85 | inline void HannWindow::Process(double **buffer,
 86 |                                    int channels) {
 87 |     int i, j;
 88 |     for (i = 0; i < channels; i++) {
 89 | #pragma omp parallel for
 90 |         for (j = 0; j < frame_size; j++) {
 91 |             buffer[i][j] *= hann[j];
 92 |         }
 93 |     }
 94 | }
 95 | inline void HannWindow::Process(double *buffer,
 96 |                                    int channels) {
 97 |     int i, j;
 98 |     for (i = 0; i < channels; i++) {
 99 | #pragma omp parallel for
100 |         for (j = 0; j < frame_size; j++) {
101 |             buffer[i*(frame_size+2) + j] *= hann[j];
102 |         }
103 |     }
104 | }
105 | 
106 | inline void HannWindow::Process(double *buffer){
107 |     int j;
108 |         for (j = 0; j < frame_size; j++) {
109 |            buffer[j] *= hann[j];
110 |         }
111 | }
112 | 
113 | inline void HannWindow::WindowWithScaling(double **buffer,
114 |                                    int channels) {
115 |     int i, j;
116 |     for (i = 0; i < channels; i++) {
117 | #pragma omp parallel for
118 |         for (j = 0; j < frame_size; j++) {
119 |             buffer[i][j] *= hann[j];
120 |             buffer[i][j] /= 32767.0;
121 |         }
122 |     }
123 | }
124 | inline void HannWindow::WindowWithScaling(double *buffer){
125 |     int j;
126 |         for (j = 0; j < frame_size; j++) {
127 |             buffer[j] *= hann[j];
128 |             buffer[j] /= 32767.0;
129 |         }
130 | }
131 | inline void HannWindow::WindowWithScaling(double *buffer,
132 |                                    int channels) {
133 |     int i, j;
134 |     for (i = 0; i < channels; i++) {
135 | #pragma omp parallel for
136 |         for (j = 0; j < frame_size; j++) {
137 |             buffer[ i*(frame_size+2) + j] *= hann[j];
138 |             buffer[ i*(frame_size+2) + j] /= 32767.0;
139 |         }
140 |     }
141 | }
142 | 
143 | #endif
144 | 


--------------------------------------------------------------------------------
/include/Ooura_FFT.h:
--------------------------------------------------------------------------------
   1 | #ifndef _H_OOURA_FFT_
   2 | #define _H_OOURA_FFT_
   3 | 
   4 | #include <cmath>
   5 | 
   6 | class Ooura_FFT{
   7 | private:
   8 |     int frame_size;
   9 |     int channels;
  10 |     double **a, **w;
  11 |     int **ip;
  12 | 
  13 | public:
  14 |     inline Ooura_FFT(int _frame_size, int _channels);
  15 |     inline ~Ooura_FFT();
  16 | 
  17 |     inline void FFT(double **);
  18 |     inline void iFFT(double **);
  19 |     inline void FFT(double *);
  20 |     inline void iFFT(double *);
  21 |     inline void SingleFFT(double *);
  22 |     inline void SingleiFFT(double *);
  23 | };
  24 | 
  25 | /*
  26 |   Copyright:
  27 |   Copyright(C) 1996-2001 Takuya OOURA
  28 |   email: ooura@mmm.t.u-tokyo.ac.jp
  29 |   download: http://momonga.t.u-tokyo.ac.jp/~ooura/fft.html
  30 |   You may use, copy, modify this code for any purpose and
  31 |   without fee. You may distribute this ORIGINAL package.
  32 | 
  33 |   Fast Fourier/Cosine/Sine Transform
  34 |   dimension   :one
  35 |   data length :power of 2
  36 |   decimation  :frequency
  37 |   radix       :4, 2
  38 |   data        :inplace
  39 |   table       :use
  40 |   functions
  41 |   cdft: Complex Discrete Fourier Transform
  42 |   rdft: Real Discrete Fourier Transform
  43 |   ddct: Discrete Cosine Transform
  44 |   ddst: Discrete Sine Transform
  45 |   dfct: Cosine Transform of RDFT (Real Symmetric DFT)
  46 |   dfst: Sine Transform of RDFT (Real Anti-symmetric DFT)
  47 |   function prototypes
  48 |   void cdft(int, int, double *, int *, double *);
  49 |   void rdft(int, int, double *, int *, double *);
  50 |   void ddct(int, int, double *, int *, double *);
  51 |   void ddst(int, int, double *, int *, double *);
  52 |   void dfct(int, double *, double *, int *, double *);
  53 |   void dfst(int, double *, double *, int *, double *);
  54 | 
  55 | 
  56 |   -------- Complex DFT (Discrete Fourier Transform) --------
  57 |   [definition]
  58 |   <case1>
  59 |   X[k] = sum_j=0^n-1 x[j]*exp(2*pi*i*j*k/n), 0<=k<n
  60 |   <case2>
  61 |   X[k] = sum_j=0^n-1 x[j]*exp(-2*pi*i*j*k/n), 0<=k<n
  62 |   (notes: sum_j=0^n-1 is a summation from j=0 to n-1)
  63 |   [usage]
  64 |   <case1>
  65 |   ip[0] = 0; // first time only
  66 |   cdft(2*n, 1, a, ip, w);
  67 |   <case2>
  68 |   ip[0] = 0; // first time only
  69 |   cdft(2*n, -1, a, ip, w);
  70 |   [parameters]
  71 |   2*n            :data length (int)
  72 |   n >= 1, n = power of 2
  73 |   a[0...2*n-1]   :input/output data (double *)
  74 |   input data
  75 |   a[2*j] = Re(x[j]),
  76 |   a[2*j+1] = Im(x[j]), 0<=j<n
  77 |   output data
  78 |   a[2*k] = Re(X[k]),
  79 |   a[2*k+1] = Im(X[k]), 0<=k<n
  80 |   ip[0...*]      :work area for bit reversal (int *)
  81 |   length of ip >= 2+sqrt(n)
  82 |   strictly,
  83 |   length of ip >=
  84 |   2+(1<<(int)(log(n+0.5)/log(2))/2).
  85 |   ip[0],ip[1] are pointers of the cos/sin table.
  86 |   w[0...n/2-1]   :cos/sin table (double *)
  87 |   w[],ip[] are initialized if ip[0] == 0.
  88 |   [remark]
  89 |   Inverse of
  90 |   cdft(2*n, -1, a, ip, w);
  91 |   is
  92 |   cdft(2*n, 1, a, ip, w);
  93 |   for (j = 0; j <= 2 * n - 1; j++) {
  94 |   a[j] *= 1.0 / n;
  95 |   }
  96 |   .
  97 | 
  98 | 
  99 |   -------- Real DFT / Inverse of Real DFT --------
 100 |   [definition]
 101 |   <case1> RDFT
 102 |   R[k] = sum_j=0^n-1 a[j]*cos(2*pi*j*k/n), 0<=k<=n/2
 103 |   I[k] = sum_j=0^n-1 a[j]*sin(2*pi*j*k/n), 0<k<n/2
 104 |   <case2> IRDFT (excluding scale)
 105 |   a[k] = (R[0] + R[n/2]*cos(pi*k))/2 +
 106 |   sum_j=1^n/2-1 R[j]*cos(2*pi*j*k/n) +
 107 |   sum_j=1^n/2-1 I[j]*sin(2*pi*j*k/n), 0<=k<n
 108 |   [usage]
 109 |   <case1>
 110 |   ip[0] = 0; // first time only
 111 |   rdft(n, 1, a, ip, w);
 112 |   <case2>
 113 |   ip[0] = 0; // first time only
 114 |   rdft(n, -1, a, ip, w);
 115 |   [parameters]
 116 |   n              :data length (int)
 117 |   n >= 2, n = power of 2
 118 |   a[0...n-1]     :input/output data (double *)
 119 |   <case1>
 120 |   output data
 121 |   a[2*k] = R[k], 0<=k<n/2
 122 |   a[2*k+1] = I[k], 0<k<n/2
 123 |   a[1] = R[n/2]
 124 |   <case2>
 125 |   input data
 126 |   a[2*j] = R[j], 0<=j<n/2
 127 |   a[2*j+1] = I[j], 0<j<n/2
 128 |   a[1] = R[n/2]
 129 |   ip[0...*]      :work area for bit reversal (int *)
 130 |   length of ip >= 2+sqrt(n/2)
 131 |   strictly,
 132 |   length of ip >=
 133 |   2+(1<<(int)(log(n/2+0.5)/log(2))/2).
 134 |   ip[0],ip[1] are pointers of the cos/sin table.
 135 |   w[0...n/2-1]   :cos/sin table (double *)
 136 |   w[],ip[] are initialized if ip[0] == 0.
 137 |   [remark]
 138 |   Inverse of
 139 |   rdft(n, 1, a, ip, w);
 140 |   is
 141 |   rdft(n, -1, a, ip, w);
 142 |   for (j = 0; j <= n - 1; j++) {
 143 |   a[j] *= 2.0 / n;
 144 |   }
 145 |   .
 146 | 
 147 | 
 148 |   -------- DCT (Discrete Cosine Transform) / Inverse of DCT --------
 149 |   [definition]
 150 |   <case1> IDCT (excluding scale)
 151 |   C[k] = sum_j=0^n-1 a[j]*cos(pi*j*(k+1/2)/n), 0<=k<n
 152 |   <case2> DCT
 153 |   C[k] = sum_j=0^n-1 a[j]*cos(pi*(j+1/2)*k/n), 0<=k<n
 154 |   [usage]
 155 |   <case1>
 156 |   ip[0] = 0; // first time only
 157 |   ddct(n, 1, a, ip, w);
 158 |   <case2>
 159 |   ip[0] = 0; // first time only
 160 |   ddct(n, -1, a, ip, w);
 161 |   [parameters]
 162 |   n              :data length (int)
 163 |   n >= 2, n = power of 2
 164 |   a[0...n-1]     :input/output data (double *)
 165 |   output data
 166 |   a[k] = C[k], 0<=k<n
 167 |   ip[0...*]      :work area for bit reversal (int *)
 168 |   length of ip >= 2+sqrt(n/2)
 169 |   strictly,
 170 |   length of ip >=
 171 |   2+(1<<(int)(log(n/2+0.5)/log(2))/2).
 172 |   ip[0],ip[1] are pointers of the cos/sin table.
 173 |   w[0...n*5/4-1] :cos/sin table (double *)
 174 |   w[],ip[] are initialized if ip[0] == 0.
 175 |   [remark]
 176 |   Inverse of
 177 |   ddct(n, -1, a, ip, w);
 178 |   is
 179 |   a[0] *= 0.5;
 180 |   ddct(n, 1, a, ip, w);
 181 |   for (j = 0; j <= n - 1; j++) {
 182 |   a[j] *= 2.0 / n;
 183 |   }
 184 |   .
 185 | 
 186 | 
 187 |   -------- DST (Discrete Sine Transform) / Inverse of DST --------
 188 |   [definition]
 189 |   <case1> IDST (excluding scale)
 190 |   S[k] = sum_j=1^n A[j]*sin(pi*j*(k+1/2)/n), 0<=k<n
 191 |   <case2> DST
 192 |   S[k] = sum_j=0^n-1 a[j]*sin(pi*(j+1/2)*k/n), 0<k<=n
 193 |   [usage]
 194 |   <case1>
 195 |   ip[0] = 0; // first time only
 196 |   ddst(n, 1, a, ip, w);
 197 |   <case2>
 198 |   ip[0] = 0; // first time only
 199 |   ddst(n, -1, a, ip, w);
 200 |   [parameters]
 201 |   n              :data length (int)
 202 |   n >= 2, n = power of 2
 203 |   a[0...n-1]     :input/output data (double *)
 204 |   <case1>
 205 |   input data
 206 |   a[j] = A[j], 0<j<n
 207 |   a[0] = A[n]
 208 |   output data
 209 |   a[k] = S[k], 0<=k<n
 210 |   <case2>
 211 |   output data
 212 |   a[k] = S[k], 0<k<n
 213 |   a[0] = S[n]
 214 |   ip[0...*]      :work area for bit reversal (int *)
 215 |   length of ip >= 2+sqrt(n/2)
 216 |   strictly,
 217 |   length of ip >=
 218 |   2+(1<<(int)(log(n/2+0.5)/log(2))/2).
 219 |   ip[0],ip[1] are pointers of the cos/sin table.
 220 |   w[0...n*5/4-1] :cos/sin table (double *)
 221 |   w[],ip[] are initialized if ip[0] == 0.
 222 |   [remark]
 223 |   Inverse of
 224 |   ddst(n, -1, a, ip, w);
 225 |   is
 226 |   a[0] *= 0.5;
 227 |   ddst(n, 1, a, ip, w);
 228 |   for (j = 0; j <= n - 1; j++) {
 229 |   a[j] *= 2.0 / n;
 230 |   }
 231 |   .
 232 | 
 233 | 
 234 |   -------- Cosine Transform of RDFT (Real Symmetric DFT) --------
 235 |   [definition]
 236 |   C[k] = sum_j=0^n a[j]*cos(pi*j*k/n), 0<=k<=n
 237 |   [usage]
 238 |   ip[0] = 0; // first time only
 239 |   dfct(n, a, t, ip, w);
 240 |   [parameters]
 241 |   n              :data length - 1 (int)
 242 |   n >= 2, n = power of 2
 243 |   a[0...n]       :input/output data (double *)
 244 |   output data
 245 |   a[k] = C[k], 0<=k<=n
 246 |   t[0...n/2]     :work area (double *)
 247 |   ip[0...*]      :work area for bit reversal (int *)
 248 |   length of ip >= 2+sqrt(n/4)
 249 |   strictly,
 250 |   length of ip >=
 251 |   2+(1<<(int)(log(n/4+0.5)/log(2))/2).
 252 |   ip[0],ip[1] are pointers of the cos/sin table.
 253 |   w[0...n*5/8-1] :cos/sin table (double *)
 254 |   w[],ip[] are initialized if ip[0] == 0.
 255 |   [remark]
 256 |   Inverse of
 257 |   a[0] *= 0.5;
 258 |   a[n] *= 0.5;
 259 |   dfct(n, a, t, ip, w);
 260 |   is
 261 |   a[0] *= 0.5;
 262 |   a[n] *= 0.5;
 263 |   dfct(n, a, t, ip, w);
 264 |   for (j = 0; j <= n; j++) {
 265 |   a[j] *= 2.0 / n;
 266 |   }
 267 |   .
 268 | 
 269 | 
 270 |   -------- Sine Transform of RDFT (Real Anti-symmetric DFT) --------
 271 |   [definition]
 272 |   S[k] = sum_j=1^n-1 a[j]*sin(pi*j*k/n), 0<k<n
 273 |   [usage]
 274 |   ip[0] = 0; // first time only
 275 |   dfst(n, a, t, ip, w);
 276 |   [parameters]
 277 |   n              :data length + 1 (int)
 278 |   n >= 2, n = power of 2
 279 |   a[0...n-1]     :input/output data (double *)
 280 |   output data
 281 |   a[k] = S[k], 0<k<n
 282 |   (a[0] is used for work area)
 283 |   t[0...n/2-1]   :work area (double *)
 284 |   ip[0...*]      :work area for bit reversal (int *)
 285 |   length of ip >= 2+sqrt(n/4)
 286 |   strictly,
 287 |   length of ip >=
 288 |   2+(1<<(int)(log(n/4+0.5)/log(2))/2).
 289 |   ip[0],ip[1] are pointers of the cos/sin table.
 290 |   w[0...n*5/8-1] :cos/sin table (double *)
 291 |   w[],ip[] are initialized if ip[0] == 0.
 292 |   [remark]
 293 |   Inverse of
 294 |   dfst(n, a, t, ip, w);
 295 |   is
 296 |   dfst(n, a, t, ip, w);
 297 |   for (j = 1; j <= n - 1; j++) {
 298 |   a[j] *= 2.0 / n;
 299 |   }
 300 |   .
 301 | 
 302 | 
 303 |   Appendix :
 304 |   The cos/sin table is recalculated when the larger table required.
 305 |   w[] and ip[] are compatible with all routines.
 306 | */
 307 | 
 308 | inline void cdft(int, int, double *, int *, double *);
 309 | inline void rdft(int, int, double *, int *, double *);
 310 | inline void ddct(int, int, double *, int *, double *);
 311 | inline void ddst(int, int, double *, int *, double *);
 312 | inline void dfct(int, double *, double *, int *, double *);
 313 | inline void dfst(int, double *, double *, int *, double *);
 314 | 
 315 | inline void makewt(int nw, int* ip, double* w);
 316 | inline void makect(int nc, int* ip, double* c);
 317 | inline void bitrv2(int n, int* ip, double* a);
 318 | inline void cftfsub(int n, double* a, double* w);
 319 | inline void cftbsub(int n, double* a, double* w);
 320 | inline void rftfsub(int n, double* a, int nc, double* c);
 321 | inline void rftbsub(int n, double* a, int nc, double* c);
 322 | 
 323 | inline void cft1st(int n, double* a, double* w);
 324 | inline void cftmdl(int n, int l, double* a, double* w);
 325 | 
 326 | inline Ooura_FFT::Ooura_FFT(int _frame_size, int _channels){
 327 |     frame_size = _frame_size;
 328 |     channels = _channels;
 329 | 
 330 |     a = new double *[channels];
 331 |     for (int i = 0; i < channels; i++)
 332 |         a[i] = new double[frame_size];
 333 | 
 334 |     w = new double *[channels];
 335 |     for (int i = 0; i < channels; i++)
 336 |         w[i] = new double[frame_size];
 337 | 
 338 |     ip = new int *[channels];
 339 |     for (int i = 0; i < channels; i++)
 340 |         ip[i] = new int[(int)(sqrt(frame_size / 2)) + 1];
 341 | }
 342 | 
 343 | inline Ooura_FFT::~Ooura_FFT() {
 344 |     for (int i = 0; i < channels; i++) {
 345 |         delete[] a[i];
 346 |         delete[] w[i];
 347 |         delete[] ip[i];
 348 |     }
 349 | 
 350 |     delete[] a;
 351 |     delete[] w;
 352 |     delete[] ip;
 353 | }
 354 | inline void Ooura_FFT::FFT(double **data) {
 355 |     int j;
 356 | #pragma omp parallel for
 357 |     for (j = 0; j < channels; j++) {
 358 |         double *t;
 359 |         t = data[j];
 360 |         ip[j][0] = 0;
 361 |         for (int i = 0; i < frame_size; i++)
 362 |             a[j][i] = t[i];
 363 | 
 364 |         rdft(frame_size, 1, a[j], ip[j], w[j]);
 365 | 
 366 |         for (int i = 0; i < frame_size; i += 2) {
 367 |             t[i] = a[j][i];
 368 |             t[i + 1] = -a[j][i + 1];
 369 |         }
 370 |         t[1] = 0;
 371 |         t[frame_size] = a[j][1];
 372 |         t[frame_size + 1] = 0;
 373 |     }
 374 | }
 375 | 
 376 | inline void Ooura_FFT::FFT(double *data) {
 377 |     int j;
 378 | #pragma omp parallel for
 379 |     for (j = 0; j < channels; j++) {
 380 |         double *t;
 381 |         t = &data[j*(frame_size+2)];
 382 |         ip[j][0] = 0;
 383 |         for (int i = 0; i < frame_size; i++)
 384 |             a[j][i] = t[i];
 385 | 
 386 |         rdft(frame_size, 1, a[j], ip[j], w[j]);
 387 | 
 388 |         for (int i = 0; i < frame_size; i += 2) {
 389 |             t[i] = a[j][i];
 390 |             t[i + 1] = -a[j][i + 1];
 391 |         }
 392 |         t[1] = 0;
 393 |         t[frame_size] = a[j][1];
 394 |         t[frame_size + 1] = 0;
 395 |     }
 396 | }
 397 | 
 398 | inline void Ooura_FFT::iFFT(double **data) {
 399 |     int j;
 400 | 
 401 | #pragma omp parallel for
 402 |     for (j = 0; j < channels; j++) {
 403 |         double *t;
 404 |         t = data[j];
 405 |         ip[j][0] = 0;
 406 |         for (int i = 0; i < frame_size; i += 2) {
 407 |             a[j][i] = t[i];
 408 |             a[j][i + 1] = -t[i + 1];
 409 |         }
 410 |         a[j][1] = t[frame_size];
 411 | 
 412 |         rdft(frame_size, -1, a[j], ip[j], w[j]);
 413 |         for (int i = 0; i < frame_size; i++) {
 414 |             a[j][i] *= 2.0;
 415 |             a[j][i] /= frame_size;
 416 |         }
 417 | 
 418 |         for (int i = 0; i < frame_size; i++) {
 419 |             t[i] = a[j][i];
 420 |         }
 421 |     }
 422 | }
 423 | 
 424 | inline void Ooura_FFT::iFFT(double *data) {
 425 |     int j;
 426 | 
 427 | #pragma omp parallel for
 428 |     for (j = 0; j < channels; j++) {
 429 |         double *t;
 430 |         t = &data[j*(frame_size+2)];
 431 |         ip[j][0] = 0;
 432 |         for (int i = 0; i < frame_size; i += 2) {
 433 |             a[j][i] = t[i];
 434 |             a[j][i + 1] = -t[i + 1];
 435 |         }
 436 |         a[j][1] = t[frame_size];
 437 | 
 438 |         rdft(frame_size, -1, a[j], ip[j], w[j]);
 439 |         for (int i = 0; i < frame_size; i++) {
 440 |             a[j][i] *= 2.0;
 441 |             a[j][i] /= frame_size;
 442 |         }
 443 | 
 444 |         for (int i = 0; i < frame_size; i++) {
 445 |             t[i] = a[j][i];
 446 |         }
 447 |     }
 448 | }
 449 | 
 450 | inline void Ooura_FFT::SingleFFT(double *data) {
 451 |     int i;
 452 | 
 453 |     ip[0][0] = 0;
 454 |     for (i = 0; i < frame_size; i++)
 455 |         a[0][i] = data[i];
 456 | 
 457 |     rdft(frame_size, 1, a[0], ip[0], w[0]);
 458 | 
 459 |     for (i = 0; i < frame_size; i += 2) {
 460 |         data[i] = a[0][i];
 461 |         data[i + 1] = -a[0][i + 1];
 462 |     }
 463 |     data[1] = 0;
 464 |     data[frame_size] = a[0][1];
 465 |     data[frame_size + 1] = 0;
 466 | }
 467 | inline void Ooura_FFT::SingleiFFT(double *data) {
 468 |     int i;
 469 |     ip[0][0] = 0;
 470 |     for (i = 0; i < frame_size; i += 2) {
 471 |         a[0][i] = data[i];
 472 |         a[0][i + 1] = -data[i + 1];
 473 |     }
 474 |     a[0][1] = data[frame_size];
 475 | 
 476 |     rdft(frame_size, -1, a[0], ip[0], w[0]);
 477 |     for (i = 0; i < frame_size; i++) {
 478 |         a[0][i] *= 2.0;
 479 |         a[0][i] /= frame_size;
 480 |     }
 481 | 
 482 |     for (i = 0; i < frame_size; i++) {
 483 |         data[i] = a[0][i];
 484 |     }
 485 | }
 486 | inline void cdft(int n, int isgn, double *a, int *ip, double *w) {
 487 |     void makewt(int nw, int *ip, double *w);
 488 |     void bitrv2(int n, int *ip, double *a);
 489 |     void bitrv2conj(int n, int *ip, double *a);
 490 |     void cftfsub(int n, double *a, double *w);
 491 |     void cftbsub(int n, double *a, double *w);
 492 | 
 493 |     if (n > (ip[0] << 2)) {
 494 |         makewt(n >> 2, ip, w);
 495 |     }
 496 |     if (n > 4) {
 497 |         if (isgn >= 0) {
 498 |             bitrv2(n, ip + 2, a);
 499 |             cftfsub(n, a, w);
 500 |         } else {
 501 |             bitrv2conj(n, ip + 2, a);
 502 |             cftbsub(n, a, w);
 503 |         }
 504 |     } else if (n == 4) {
 505 |         cftfsub(n, a, w);
 506 |     }
 507 | }
 508 | 
 509 | inline void rdft(int n, int isgn, double *a, int *ip, double *w) {
 510 |     void makewt(int nw, int *ip, double *w);
 511 |     void makect(int nc, int *ip, double *c);
 512 |     void bitrv2(int n, int *ip, double *a);
 513 |     void cftfsub(int n, double *a, double *w);
 514 |     void cftbsub(int n, double *a, double *w);
 515 |     void rftfsub(int n, double *a, int nc, double *c);
 516 |     void rftbsub(int n, double *a, int nc, double *c);
 517 |     int nw, nc;
 518 |     double xi;
 519 | 
 520 |     nw = ip[0];
 521 |     if (n > (nw << 2)) {
 522 |         nw = n >> 2;
 523 |         makewt(nw, ip, w);
 524 |     }
 525 |     nc = ip[1];
 526 |     if (n > (nc << 2)) {
 527 |         nc = n >> 2;
 528 |         makect(nc, ip, w + nw);
 529 |     }
 530 |     if (isgn >= 0) {
 531 |         if (n > 4) {
 532 |             bitrv2(n, ip + 2, a);
 533 |             cftfsub(n, a, w);
 534 |             rftfsub(n, a, nc, w + nw);
 535 |         } else if (n == 4) {
 536 |             cftfsub(n, a, w);
 537 |         }
 538 |         xi = a[0] - a[1];
 539 |         a[0] += a[1];
 540 |         a[1] = xi;
 541 |     } else {
 542 |         a[1] = 0.5 * (a[0] - a[1]);
 543 |         a[0] -= a[1];
 544 |         if (n > 4) {
 545 |             rftbsub(n, a, nc, w + nw);
 546 |             bitrv2(n, ip + 2, a);
 547 |             cftbsub(n, a, w);
 548 |         } else if (n == 4) {
 549 |             cftfsub(n, a, w);
 550 |         }
 551 |     }
 552 | }
 553 | 
 554 | inline void ddct(int n, int isgn, double *a, int *ip, double *w) {
 555 |     void makewt(int nw, int *ip, double *w);
 556 |     void makect(int nc, int *ip, double *c);
 557 |     void bitrv2(int n, int *ip, double *a);
 558 |     void cftfsub(int n, double *a, double *w);
 559 |     void cftbsub(int n, double *a, double *w);
 560 |     void rftfsub(int n, double *a, int nc, double *c);
 561 |     void rftbsub(int n, double *a, int nc, double *c);
 562 |     void dctsub(int n, double *a, int nc, double *c);
 563 |     int j, nw, nc;
 564 |     double xr;
 565 | 
 566 |     nw = ip[0];
 567 |     if (n > (nw << 2)) {
 568 |         nw = n >> 2;
 569 |         makewt(nw, ip, w);
 570 |     }
 571 |     nc = ip[1];
 572 |     if (n > nc) {
 573 |         nc = n;
 574 |         makect(nc, ip, w + nw);
 575 |     }
 576 |     if (isgn < 0) {
 577 |         xr = a[n - 1];
 578 |         for (j = n - 2; j >= 2; j -= 2) {
 579 |             a[j + 1] = a[j] - a[j - 1];
 580 |             a[j] += a[j - 1];
 581 |         }
 582 |         a[1] = a[0] - xr;
 583 |         a[0] += xr;
 584 |         if (n > 4) {
 585 |             rftbsub(n, a, nc, w + nw);
 586 |             bitrv2(n, ip + 2, a);
 587 |             cftbsub(n, a, w);
 588 |         } else if (n == 4) {
 589 |             cftfsub(n, a, w);
 590 |         }
 591 |     }
 592 |     dctsub(n, a, nc, w + nw);
 593 |     if (isgn >= 0) {
 594 |         if (n > 4) {
 595 |             bitrv2(n, ip + 2, a);
 596 |             cftfsub(n, a, w);
 597 |             rftfsub(n, a, nc, w + nw);
 598 |         } else if (n == 4) {
 599 |             cftfsub(n, a, w);
 600 |         }
 601 |         xr = a[0] - a[1];
 602 |         a[0] += a[1];
 603 |         for (j = 2; j < n; j += 2) {
 604 |             a[j - 1] = a[j] - a[j + 1];
 605 |             a[j] += a[j + 1];
 606 |         }
 607 |         a[n - 1] = xr;
 608 |     }
 609 | }
 610 | 
 611 | inline void ddst(int n, int isgn, double *a, int *ip, double *w) {
 612 |     void makewt(int nw, int *ip, double *w);
 613 |     void makect(int nc, int *ip, double *c);
 614 |     void bitrv2(int n, int *ip, double *a);
 615 |     void cftfsub(int n, double *a, double *w);
 616 |     void cftbsub(int n, double *a, double *w);
 617 |     void rftfsub(int n, double *a, int nc, double *c);
 618 |     void rftbsub(int n, double *a, int nc, double *c);
 619 |     void dstsub(int n, double *a, int nc, double *c);
 620 |     int j, nw, nc;
 621 |     double xr;
 622 | 
 623 |     nw = ip[0];
 624 |     if (n > (nw << 2)) {
 625 |         nw = n >> 2;
 626 |         makewt(nw, ip, w);
 627 |     }
 628 |     nc = ip[1];
 629 |     if (n > nc) {
 630 |         nc = n;
 631 |         makect(nc, ip, w + nw);
 632 |     }
 633 |     if (isgn < 0) {
 634 |         xr = a[n - 1];
 635 |         for (j = n - 2; j >= 2; j -= 2) {
 636 |             a[j + 1] = -a[j] - a[j - 1];
 637 |             a[j] -= a[j - 1];
 638 |         }
 639 |         a[1] = a[0] + xr;
 640 |         a[0] -= xr;
 641 |         if (n > 4) {
 642 |             rftbsub(n, a, nc, w + nw);
 643 |             bitrv2(n, ip + 2, a);
 644 |             cftbsub(n, a, w);
 645 |         } else if (n == 4) {
 646 |             cftfsub(n, a, w);
 647 |         }
 648 |     }
 649 |     dstsub(n, a, nc, w + nw);
 650 |     if (isgn >= 0) {
 651 |         if (n > 4) {
 652 |             bitrv2(n, ip + 2, a);
 653 |             cftfsub(n, a, w);
 654 |             rftfsub(n, a, nc, w + nw);
 655 |         } else if (n == 4) {
 656 |             cftfsub(n, a, w);
 657 |         }
 658 |         xr = a[0] - a[1];
 659 |         a[0] += a[1];
 660 |         for (j = 2; j < n; j += 2) {
 661 |             a[j - 1] = -a[j] - a[j + 1];
 662 |             a[j] -= a[j + 1];
 663 |         }
 664 |         a[n - 1] = -xr;
 665 |     }
 666 | }
 667 | 
 668 | inline void dfct(int n, double *a, double *t, int *ip, double *w) {
 669 |     void makewt(int nw, int *ip, double *w);
 670 |     void makect(int nc, int *ip, double *c);
 671 |     void bitrv2(int n, int *ip, double *a);
 672 |     void cftfsub(int n, double *a, double *w);
 673 |     void rftfsub(int n, double *a, int nc, double *c);
 674 |     void dctsub(int n, double *a, int nc, double *c);
 675 |     int j, k, l, m, mh, nw, nc;
 676 |     double xr, xi, yr, yi;
 677 | 
 678 |     nw = ip[0];
 679 |     if (n > (nw << 3)) {
 680 |         nw = n >> 3;
 681 |         makewt(nw, ip, w);
 682 |     }
 683 |     nc = ip[1];
 684 |     if (n > (nc << 1)) {
 685 |         nc = n >> 1;
 686 |         makect(nc, ip, w + nw);
 687 |     }
 688 |     m = n >> 1;
 689 |     yi = a[m];
 690 |     xi = a[0] + a[n];
 691 |     a[0] -= a[n];
 692 |     t[0] = xi - yi;
 693 |     t[m] = xi + yi;
 694 |     if (n > 2) {
 695 |         mh = m >> 1;
 696 |         for (j = 1; j < mh; j++) {
 697 |             k = m - j;
 698 |             xr = a[j] - a[n - j];
 699 |             xi = a[j] + a[n - j];
 700 |             yr = a[k] - a[n - k];
 701 |             yi = a[k] + a[n - k];
 702 |             a[j] = xr;
 703 |             a[k] = yr;
 704 |             t[j] = xi - yi;
 705 |             t[k] = xi + yi;
 706 |         }
 707 |         t[mh] = a[mh] + a[n - mh];
 708 |         a[mh] -= a[n - mh];
 709 |         dctsub(m, a, nc, w + nw);
 710 |         if (m > 4) {
 711 |             bitrv2(m, ip + 2, a);
 712 |             cftfsub(m, a, w);
 713 |             rftfsub(m, a, nc, w + nw);
 714 |         } else if (m == 4) {
 715 |             cftfsub(m, a, w);
 716 |         }
 717 |         a[n - 1] = a[0] - a[1];
 718 |         a[1] = a[0] + a[1];
 719 |         for (j = m - 2; j >= 2; j -= 2) {
 720 |             a[2 * j + 1] = a[j] + a[j + 1];
 721 |             a[2 * j - 1] = a[j] - a[j + 1];
 722 |         }
 723 |         l = 2;
 724 |         m = mh;
 725 |         while (m >= 2) {
 726 |             dctsub(m, t, nc, w + nw);
 727 |             if (m > 4) {
 728 |                 bitrv2(m, ip + 2, t);
 729 |                 cftfsub(m, t, w);
 730 |                 rftfsub(m, t, nc, w + nw);
 731 |             } else if (m == 4) {
 732 |                 cftfsub(m, t, w);
 733 |             }
 734 |             a[n - l] = t[0] - t[1];
 735 |             a[l] = t[0] + t[1];
 736 |             k = 0;
 737 |             for (j = 2; j < m; j += 2) {
 738 |                 k += l << 2;
 739 |                 a[k - l] = t[j] - t[j + 1];
 740 |                 a[k + l] = t[j] + t[j + 1];
 741 |             }
 742 |             l <<= 1;
 743 |             mh = m >> 1;
 744 |             for (j = 0; j < mh; j++) {
 745 |                 k = m - j;
 746 |                 t[j] = t[m + k] - t[m + j];
 747 |                 t[k] = t[m + k] + t[m + j];
 748 |             }
 749 |             t[mh] = t[m + mh];
 750 |             m = mh;
 751 |         }
 752 |         a[l] = t[0];
 753 |         a[n] = t[2] - t[1];
 754 |         a[0] = t[2] + t[1];
 755 |     } else {
 756 |         a[1] = a[0];
 757 |         a[2] = t[0];
 758 |         a[0] = t[1];
 759 |     }
 760 | }
 761 | 
 762 | inline void dfst(int n, double *a, double *t, int *ip, double *w) {
 763 |     void makewt(int nw, int *ip, double *w);
 764 |     void makect(int nc, int *ip, double *c);
 765 |     void bitrv2(int n, int *ip, double *a);
 766 |     void cftfsub(int n, double *a, double *w);
 767 |     void rftfsub(int n, double *a, int nc, double *c);
 768 |     void dstsub(int n, double *a, int nc, double *c);
 769 |     int j, k, l, m, mh, nw, nc;
 770 |     double xr, xi, yr, yi;
 771 | 
 772 |     nw = ip[0];
 773 |     if (n > (nw << 3)) {
 774 |         nw = n >> 3;
 775 |         makewt(nw, ip, w);
 776 |     }
 777 |     nc = ip[1];
 778 |     if (n > (nc << 1)) {
 779 |         nc = n >> 1;
 780 |         makect(nc, ip, w + nw);
 781 |     }
 782 |     if (n > 2) {
 783 |         m = n >> 1;
 784 |         mh = m >> 1;
 785 |         for (j = 1; j < mh; j++) {
 786 |             k = m - j;
 787 |             xr = a[j] + a[n - j];
 788 |             xi = a[j] - a[n - j];
 789 |             yr = a[k] + a[n - k];
 790 |             yi = a[k] - a[n - k];
 791 |             a[j] = xr;
 792 |             a[k] = yr;
 793 |             t[j] = xi + yi;
 794 |             t[k] = xi - yi;
 795 |         }
 796 |         t[0] = a[mh] - a[n - mh];
 797 |         a[mh] += a[n - mh];
 798 |         a[0] = a[m];
 799 |         dstsub(m, a, nc, w + nw);
 800 |         if (m > 4) {
 801 |             bitrv2(m, ip + 2, a);
 802 |             cftfsub(m, a, w);
 803 |             rftfsub(m, a, nc, w + nw);
 804 |         } else if (m == 4) {
 805 |             cftfsub(m, a, w);
 806 |         }
 807 |         a[n - 1] = a[1] - a[0];
 808 |         a[1] = a[0] + a[1];
 809 |         for (j = m - 2; j >= 2; j -= 2) {
 810 |             a[2 * j + 1] = a[j] - a[j + 1];
 811 |             a[2 * j - 1] = -a[j] - a[j + 1];
 812 |         }
 813 |         l = 2;
 814 |         m = mh;
 815 |         while (m >= 2) {
 816 |             dstsub(m, t, nc, w + nw);
 817 |             if (m > 4) {
 818 |                 bitrv2(m, ip + 2, t);
 819 |                 cftfsub(m, t, w);
 820 |                 rftfsub(m, t, nc, w + nw);
 821 |             } else if (m == 4) {
 822 |                 cftfsub(m, t, w);
 823 |             }
 824 |             a[n - l] = t[1] - t[0];
 825 |             a[l] = t[0] + t[1];
 826 |             k = 0;
 827 |             for (j = 2; j < m; j += 2) {
 828 |                 k += l << 2;
 829 |                 a[k - l] = -t[j] - t[j + 1];
 830 |                 a[k + l] = t[j] - t[j + 1];
 831 |             }
 832 |             l <<= 1;
 833 |             mh = m >> 1;
 834 |             for (j = 1; j < mh; j++) {
 835 |                 k = m - j;
 836 |                 t[j] = t[m + k] + t[m + j];
 837 |                 t[k] = t[m + k] - t[m + j];
 838 |             }
 839 |             t[0] = t[m + mh];
 840 |             m = mh;
 841 |         }
 842 |         a[l] = t[0];
 843 |     }
 844 |     a[0] = 0;
 845 | }
 846 | 
 847 | /* -------- initializing routines -------- */
 848 | 
 849 | inline void makewt(int nw, int *ip, double *w) {
 850 |     void bitrv2(int n, int *ip, double *a);
 851 |     int j, nwh;
 852 |     double delta, x, y;
 853 | 
 854 |     ip[0] = nw;
 855 |     ip[1] = 1;
 856 |     if (nw > 2) {
 857 |         nwh = nw >> 1;
 858 |         delta = atan(1.0) / nwh;
 859 |         w[0] = 1;
 860 |         w[1] = 0;
 861 |         w[nwh] = cos(delta * nwh);
 862 |         w[nwh + 1] = w[nwh];
 863 |         if (nwh > 2) {
 864 |             for (j = 2; j < nwh; j += 2) {
 865 |                 x = cos(delta * j);
 866 |                 y = sin(delta * j);
 867 |                 w[j] = x;
 868 |                 w[j + 1] = y;
 869 |                 w[nw - j] = y;
 870 |                 w[nw - j + 1] = x;
 871 |             }
 872 |             bitrv2(nw, ip + 2, w);
 873 |         }
 874 |     }
 875 | }
 876 | 
 877 | inline void makect(int nc, int *ip, double *c) {
 878 |     int j, nch;
 879 |     double delta;
 880 | 
 881 |     ip[1] = nc;
 882 |     if (nc > 1) {
 883 |         nch = nc >> 1;
 884 |         delta = atan(1.0) / nch;
 885 |         c[0] = cos(delta * nch);
 886 |         c[nch] = 0.5 * c[0];
 887 |         for (j = 1; j < nch; j++) {
 888 |             c[j] = 0.5 * cos(delta * j);
 889 |             c[nc - j] = 0.5 * sin(delta * j);
 890 |         }
 891 |     }
 892 | }
 893 | 
 894 | /* -------- child routines -------- */
 895 | 
 896 | inline void bitrv2(int n, int *ip, double *a) {
 897 |     int j, j1, k, k1, l, m, m2;
 898 |     double xr, xi, yr, yi;
 899 | 
 900 |     ip[0] = 0;
 901 |     l = n;
 902 |     m = 1;
 903 |     while ((m << 3) < l) {
 904 |         l >>= 1;
 905 |         for (j = 0; j < m; j++) {
 906 |             ip[m + j] = ip[j] + l;
 907 |         }
 908 |         m <<= 1;
 909 |     }
 910 |     m2 = 2 * m;
 911 |     if ((m << 3) == l) {
 912 |         for (k = 0; k < m; k++) {
 913 |             for (j = 0; j < k; j++) {
 914 |                 j1 = 2 * j + ip[k];
 915 |                 k1 = 2 * k + ip[j];
 916 |                 xr = a[j1];
 917 |                 xi = a[j1 + 1];
 918 |                 yr = a[k1];
 919 |                 yi = a[k1 + 1];
 920 |                 a[j1] = yr;
 921 |                 a[j1 + 1] = yi;
 922 |                 a[k1] = xr;
 923 |                 a[k1 + 1] = xi;
 924 |                 j1 += m2;
 925 |                 k1 += 2 * m2;
 926 |                 xr = a[j1];
 927 |                 xi = a[j1 + 1];
 928 |                 yr = a[k1];
 929 |                 yi = a[k1 + 1];
 930 |                 a[j1] = yr;
 931 |                 a[j1 + 1] = yi;
 932 |                 a[k1] = xr;
 933 |                 a[k1 + 1] = xi;
 934 |                 j1 += m2;
 935 |                 k1 -= m2;
 936 |                 xr = a[j1];
 937 |                 xi = a[j1 + 1];
 938 |                 yr = a[k1];
 939 |                 yi = a[k1 + 1];
 940 |                 a[j1] = yr;
 941 |                 a[j1 + 1] = yi;
 942 |                 a[k1] = xr;
 943 |                 a[k1 + 1] = xi;
 944 |                 j1 += m2;
 945 |                 k1 += 2 * m2;
 946 |                 xr = a[j1];
 947 |                 xi = a[j1 + 1];
 948 |                 yr = a[k1];
 949 |                 yi = a[k1 + 1];
 950 |                 a[j1] = yr;
 951 |                 a[j1 + 1] = yi;
 952 |                 a[k1] = xr;
 953 |                 a[k1 + 1] = xi;
 954 |             }
 955 |             j1 = 2 * k + m2 + ip[k];
 956 |             k1 = j1 + m2;
 957 |             xr = a[j1];
 958 |             xi = a[j1 + 1];
 959 |             yr = a[k1];
 960 |             yi = a[k1 + 1];
 961 |             a[j1] = yr;
 962 |             a[j1 + 1] = yi;
 963 |             a[k1] = xr;
 964 |             a[k1 + 1] = xi;
 965 |         }
 966 |     } else {
 967 |         for (k = 1; k < m; k++) {
 968 |             for (j = 0; j < k; j++) {
 969 |                 j1 = 2 * j + ip[k];
 970 |                 k1 = 2 * k + ip[j];
 971 |                 xr = a[j1];
 972 |                 xi = a[j1 + 1];
 973 |                 yr = a[k1];
 974 |                 yi = a[k1 + 1];
 975 |                 a[j1] = yr;
 976 |                 a[j1 + 1] = yi;
 977 |                 a[k1] = xr;
 978 |                 a[k1 + 1] = xi;
 979 |                 j1 += m2;
 980 |                 k1 += m2;
 981 |                 xr = a[j1];
 982 |                 xi = a[j1 + 1];
 983 |                 yr = a[k1];
 984 |                 yi = a[k1 + 1];
 985 |                 a[j1] = yr;
 986 |                 a[j1 + 1] = yi;
 987 |                 a[k1] = xr;
 988 |                 a[k1 + 1] = xi;
 989 |             }
 990 |         }
 991 |     }
 992 | }
 993 | 
 994 | inline void bitrv2conj(int n, int *ip, double *a) {
 995 |     int j, j1, k, k1, l, m, m2;
 996 |     double xr, xi, yr, yi;
 997 | 
 998 |     ip[0] = 0;
 999 |     l = n;
1000 |     m = 1;
1001 |     while ((m << 3) < l) {
1002 |         l >>= 1;
1003 |         for (j = 0; j < m; j++) {
1004 |             ip[m + j] = ip[j] + l;
1005 |         }
1006 |         m <<= 1;
1007 |     }
1008 |     m2 = 2 * m;
1009 |     if ((m << 3) == l) {
1010 |         for (k = 0; k < m; k++) {
1011 |             for (j = 0; j < k; j++) {
1012 |                 j1 = 2 * j + ip[k];
1013 |                 k1 = 2 * k + ip[j];
1014 |                 xr = a[j1];
1015 |                 xi = -a[j1 + 1];
1016 |                 yr = a[k1];
1017 |                 yi = -a[k1 + 1];
1018 |                 a[j1] = yr;
1019 |                 a[j1 + 1] = yi;
1020 |                 a[k1] = xr;
1021 |                 a[k1 + 1] = xi;
1022 |                 j1 += m2;
1023 |                 k1 += 2 * m2;
1024 |                 xr = a[j1];
1025 |                 xi = -a[j1 + 1];
1026 |                 yr = a[k1];
1027 |                 yi = -a[k1 + 1];
1028 |                 a[j1] = yr;
1029 |                 a[j1 + 1] = yi;
1030 |                 a[k1] = xr;
1031 |                 a[k1 + 1] = xi;
1032 |                 j1 += m2;
1033 |                 k1 -= m2;
1034 |                 xr = a[j1];
1035 |                 xi = -a[j1 + 1];
1036 |                 yr = a[k1];
1037 |                 yi = -a[k1 + 1];
1038 |                 a[j1] = yr;
1039 |                 a[j1 + 1] = yi;
1040 |                 a[k1] = xr;
1041 |                 a[k1 + 1] = xi;
1042 |                 j1 += m2;
1043 |                 k1 += 2 * m2;
1044 |                 xr = a[j1];
1045 |                 xi = -a[j1 + 1];
1046 |                 yr = a[k1];
1047 |                 yi = -a[k1 + 1];
1048 |                 a[j1] = yr;
1049 |                 a[j1 + 1] = yi;
1050 |                 a[k1] = xr;
1051 |                 a[k1 + 1] = xi;
1052 |             }
1053 |             k1 = 2 * k + ip[k];
1054 |             a[k1 + 1] = -a[k1 + 1];
1055 |             j1 = k1 + m2;
1056 |             k1 = j1 + m2;
1057 |             xr = a[j1];
1058 |             xi = -a[j1 + 1];
1059 |             yr = a[k1];
1060 |             yi = -a[k1 + 1];
1061 |             a[j1] = yr;
1062 |             a[j1 + 1] = yi;
1063 |             a[k1] = xr;
1064 |             a[k1 + 1] = xi;
1065 |             k1 += m2;
1066 |             a[k1 + 1] = -a[k1 + 1];
1067 |         }
1068 |     } else {
1069 |         a[1] = -a[1];
1070 |         a[m2 + 1] = -a[m2 + 1];
1071 |         for (k = 1; k < m; k++) {
1072 |             for (j = 0; j < k; j++) {
1073 |                 j1 = 2 * j + ip[k];
1074 |                 k1 = 2 * k + ip[j];
1075 |                 xr = a[j1];
1076 |                 xi = -a[j1 + 1];
1077 |                 yr = a[k1];
1078 |                 yi = -a[k1 + 1];
1079 |                 a[j1] = yr;
1080 |                 a[j1 + 1] = yi;
1081 |                 a[k1] = xr;
1082 |                 a[k1 + 1] = xi;
1083 |                 j1 += m2;
1084 |                 k1 += m2;
1085 |                 xr = a[j1];
1086 |                 xi = -a[j1 + 1];
1087 |                 yr = a[k1];
1088 |                 yi = -a[k1 + 1];
1089 |                 a[j1] = yr;
1090 |                 a[j1 + 1] = yi;
1091 |                 a[k1] = xr;
1092 |                 a[k1 + 1] = xi;
1093 |             }
1094 |             k1 = 2 * k + ip[k];
1095 |             a[k1 + 1] = -a[k1 + 1];
1096 |             a[k1 + m2 + 1] = -a[k1 + m2 + 1];
1097 |         }
1098 |     }
1099 | }
1100 | 
1101 | inline void cftfsub(int n, double *a, double *w) {
1102 |     void cft1st(int n, double *a, double *w);
1103 |     void cftmdl(int n, int l, double *a, double *w);
1104 |     int j, j1, j2, j3, l;
1105 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
1106 | 
1107 |     l = 2;
1108 |     if (n > 8) {
1109 |         cft1st(n, a, w);
1110 |         l = 8;
1111 |         while ((l << 2) < n) {
1112 |             cftmdl(n, l, a, w);
1113 |             l <<= 2;
1114 |         }
1115 |     }
1116 |     if ((l << 2) == n) {
1117 |         for (j = 0; j < l; j += 2) {
1118 |             j1 = j + l;
1119 |             j2 = j1 + l;
1120 |             j3 = j2 + l;
1121 |             x0r = a[j] + a[j1];
1122 |             x0i = a[j + 1] + a[j1 + 1];
1123 |             x1r = a[j] - a[j1];
1124 |             x1i = a[j + 1] - a[j1 + 1];
1125 |             x2r = a[j2] + a[j3];
1126 |             x2i = a[j2 + 1] + a[j3 + 1];
1127 |             x3r = a[j2] - a[j3];
1128 |             x3i = a[j2 + 1] - a[j3 + 1];
1129 |             a[j] = x0r + x2r;
1130 |             a[j + 1] = x0i + x2i;
1131 |             a[j2] = x0r - x2r;
1132 |             a[j2 + 1] = x0i - x2i;
1133 |             a[j1] = x1r - x3i;
1134 |             a[j1 + 1] = x1i + x3r;
1135 |             a[j3] = x1r + x3i;
1136 |             a[j3 + 1] = x1i - x3r;
1137 |         }
1138 |     } else {
1139 |         for (j = 0; j < l; j += 2) {
1140 |             j1 = j + l;
1141 |             x0r = a[j] - a[j1];
1142 |             x0i = a[j + 1] - a[j1 + 1];
1143 |             a[j] += a[j1];
1144 |             a[j + 1] += a[j1 + 1];
1145 |             a[j1] = x0r;
1146 |             a[j1 + 1] = x0i;
1147 |         }
1148 |     }
1149 | }
1150 | 
1151 | inline void cftbsub(int n, double *a, double *w) {
1152 |     void cft1st(int n, double *a, double *w);
1153 |     void cftmdl(int n, int l, double *a, double *w);
1154 |     int j, j1, j2, j3, l;
1155 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
1156 | 
1157 |     l = 2;
1158 |     if (n > 8) {
1159 |         cft1st(n, a, w);
1160 |         l = 8;
1161 |         while ((l << 2) < n) {
1162 |             cftmdl(n, l, a, w);
1163 |             l <<= 2;
1164 |         }
1165 |     }
1166 |     if ((l << 2) == n) {
1167 |         for (j = 0; j < l; j += 2) {
1168 |             j1 = j + l;
1169 |             j2 = j1 + l;
1170 |             j3 = j2 + l;
1171 |             x0r = a[j] + a[j1];
1172 |             x0i = -a[j + 1] - a[j1 + 1];
1173 |             x1r = a[j] - a[j1];
1174 |             x1i = -a[j + 1] + a[j1 + 1];
1175 |             x2r = a[j2] + a[j3];
1176 |             x2i = a[j2 + 1] + a[j3 + 1];
1177 |             x3r = a[j2] - a[j3];
1178 |             x3i = a[j2 + 1] - a[j3 + 1];
1179 |             a[j] = x0r + x2r;
1180 |             a[j + 1] = x0i - x2i;
1181 |             a[j2] = x0r - x2r;
1182 |             a[j2 + 1] = x0i + x2i;
1183 |             a[j1] = x1r - x3i;
1184 |             a[j1 + 1] = x1i - x3r;
1185 |             a[j3] = x1r + x3i;
1186 |             a[j3 + 1] = x1i + x3r;
1187 |         }
1188 |     } else {
1189 |         for (j = 0; j < l; j += 2) {
1190 |             j1 = j + l;
1191 |             x0r = a[j] - a[j1];
1192 |             x0i = -a[j + 1] + a[j1 + 1];
1193 |             a[j] += a[j1];
1194 |             a[j + 1] = -a[j + 1] - a[j1 + 1];
1195 |             a[j1] = x0r;
1196 |             a[j1 + 1] = x0i;
1197 |         }
1198 |     }
1199 | }
1200 | 
1201 | inline void cft1st(int n, double *a, double *w) {
1202 |     int j, k1, k2;
1203 |     double wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
1204 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
1205 | 
1206 |     x0r = a[0] + a[2];
1207 |     x0i = a[1] + a[3];
1208 |     x1r = a[0] - a[2];
1209 |     x1i = a[1] - a[3];
1210 |     x2r = a[4] + a[6];
1211 |     x2i = a[5] + a[7];
1212 |     x3r = a[4] - a[6];
1213 |     x3i = a[5] - a[7];
1214 |     a[0] = x0r + x2r;
1215 |     a[1] = x0i + x2i;
1216 |     a[4] = x0r - x2r;
1217 |     a[5] = x0i - x2i;
1218 |     a[2] = x1r - x3i;
1219 |     a[3] = x1i + x3r;
1220 |     a[6] = x1r + x3i;
1221 |     a[7] = x1i - x3r;
1222 |     wk1r = w[2];
1223 |     x0r = a[8] + a[10];
1224 |     x0i = a[9] + a[11];
1225 |     x1r = a[8] - a[10];
1226 |     x1i = a[9] - a[11];
1227 |     x2r = a[12] + a[14];
1228 |     x2i = a[13] + a[15];
1229 |     x3r = a[12] - a[14];
1230 |     x3i = a[13] - a[15];
1231 |     a[8] = x0r + x2r;
1232 |     a[9] = x0i + x2i;
1233 |     a[12] = x2i - x0i;
1234 |     a[13] = x0r - x2r;
1235 |     x0r = x1r - x3i;
1236 |     x0i = x1i + x3r;
1237 |     a[10] = wk1r * (x0r - x0i);
1238 |     a[11] = wk1r * (x0r + x0i);
1239 |     x0r = x3i + x1r;
1240 |     x0i = x3r - x1i;
1241 |     a[14] = wk1r * (x0i - x0r);
1242 |     a[15] = wk1r * (x0i + x0r);
1243 |     k1 = 0;
1244 |     for (j = 16; j < n; j += 16) {
1245 |         k1 += 2;
1246 |         k2 = 2 * k1;
1247 |         wk2r = w[k1];
1248 |         wk2i = w[k1 + 1];
1249 |         wk1r = w[k2];
1250 |         wk1i = w[k2 + 1];
1251 |         wk3r = wk1r - 2 * wk2i * wk1i;
1252 |         wk3i = 2 * wk2i * wk1r - wk1i;
1253 |         x0r = a[j] + a[j + 2];
1254 |         x0i = a[j + 1] + a[j + 3];
1255 |         x1r = a[j] - a[j + 2];
1256 |         x1i = a[j + 1] - a[j + 3];
1257 |         x2r = a[j + 4] + a[j + 6];
1258 |         x2i = a[j + 5] + a[j + 7];
1259 |         x3r = a[j + 4] - a[j + 6];
1260 |         x3i = a[j + 5] - a[j + 7];
1261 |         a[j] = x0r + x2r;
1262 |         a[j + 1] = x0i + x2i;
1263 |         x0r -= x2r;
1264 |         x0i -= x2i;
1265 |         a[j + 4] = wk2r * x0r - wk2i * x0i;
1266 |         a[j + 5] = wk2r * x0i + wk2i * x0r;
1267 |         x0r = x1r - x3i;
1268 |         x0i = x1i + x3r;
1269 |         a[j + 2] = wk1r * x0r - wk1i * x0i;
1270 |         a[j + 3] = wk1r * x0i + wk1i * x0r;
1271 |         x0r = x1r + x3i;
1272 |         x0i = x1i - x3r;
1273 |         a[j + 6] = wk3r * x0r - wk3i * x0i;
1274 |         a[j + 7] = wk3r * x0i + wk3i * x0r;
1275 |         wk1r = w[k2 + 2];
1276 |         wk1i = w[k2 + 3];
1277 |         wk3r = wk1r - 2 * wk2r * wk1i;
1278 |         wk3i = 2 * wk2r * wk1r - wk1i;
1279 |         x0r = a[j + 8] + a[j + 10];
1280 |         x0i = a[j + 9] + a[j + 11];
1281 |         x1r = a[j + 8] - a[j + 10];
1282 |         x1i = a[j + 9] - a[j + 11];
1283 |         x2r = a[j + 12] + a[j + 14];
1284 |         x2i = a[j + 13] + a[j + 15];
1285 |         x3r = a[j + 12] - a[j + 14];
1286 |         x3i = a[j + 13] - a[j + 15];
1287 |         a[j + 8] = x0r + x2r;
1288 |         a[j + 9] = x0i + x2i;
1289 |         x0r -= x2r;
1290 |         x0i -= x2i;
1291 |         a[j + 12] = -wk2i * x0r - wk2r * x0i;
1292 |         a[j + 13] = -wk2i * x0i + wk2r * x0r;
1293 |         x0r = x1r - x3i;
1294 |         x0i = x1i + x3r;
1295 |         a[j + 10] = wk1r * x0r - wk1i * x0i;
1296 |         a[j + 11] = wk1r * x0i + wk1i * x0r;
1297 |         x0r = x1r + x3i;
1298 |         x0i = x1i - x3r;
1299 |         a[j + 14] = wk3r * x0r - wk3i * x0i;
1300 |         a[j + 15] = wk3r * x0i + wk3i * x0r;
1301 |     }
1302 | }
1303 | 
1304 | inline void cftmdl(int n, int l, double *a, double *w) {
1305 |     int j, j1, j2, j3, k, k1, k2, m, m2;
1306 |     double wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
1307 |     double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
1308 | 
1309 |     m = l << 2;
1310 |     for (j = 0; j < l; j += 2) {
1311 |         j1 = j + l;
1312 |         j2 = j1 + l;
1313 |         j3 = j2 + l;
1314 |         x0r = a[j] + a[j1];
1315 |         x0i = a[j + 1] + a[j1 + 1];
1316 |         x1r = a[j] - a[j1];
1317 |         x1i = a[j + 1] - a[j1 + 1];
1318 |         x2r = a[j2] + a[j3];
1319 |         x2i = a[j2 + 1] + a[j3 + 1];
1320 |         x3r = a[j2] - a[j3];
1321 |         x3i = a[j2 + 1] - a[j3 + 1];
1322 |         a[j] = x0r + x2r;
1323 |         a[j + 1] = x0i + x2i;
1324 |         a[j2] = x0r - x2r;
1325 |         a[j2 + 1] = x0i - x2i;
1326 |         a[j1] = x1r - x3i;
1327 |         a[j1 + 1] = x1i + x3r;
1328 |         a[j3] = x1r + x3i;
1329 |         a[j3 + 1] = x1i - x3r;
1330 |     }
1331 |     wk1r = w[2];
1332 |     for (j = m; j < l + m; j += 2) {
1333 |         j1 = j + l;
1334 |         j2 = j1 + l;
1335 |         j3 = j2 + l;
1336 |         x0r = a[j] + a[j1];
1337 |         x0i = a[j + 1] + a[j1 + 1];
1338 |         x1r = a[j] - a[j1];
1339 |         x1i = a[j + 1] - a[j1 + 1];
1340 |         x2r = a[j2] + a[j3];
1341 |         x2i = a[j2 + 1] + a[j3 + 1];
1342 |         x3r = a[j2] - a[j3];
1343 |         x3i = a[j2 + 1] - a[j3 + 1];
1344 |         a[j] = x0r + x2r;
1345 |         a[j + 1] = x0i + x2i;
1346 |         a[j2] = x2i - x0i;
1347 |         a[j2 + 1] = x0r - x2r;
1348 |         x0r = x1r - x3i;
1349 |         x0i = x1i + x3r;
1350 |         a[j1] = wk1r * (x0r - x0i);
1351 |         a[j1 + 1] = wk1r * (x0r + x0i);
1352 |         x0r = x3i + x1r;
1353 |         x0i = x3r - x1i;
1354 |         a[j3] = wk1r * (x0i - x0r);
1355 |         a[j3 + 1] = wk1r * (x0i + x0r);
1356 |     }
1357 |     k1 = 0;
1358 |     m2 = 2 * m;
1359 |     for (k = m2; k < n; k += m2) {
1360 |         k1 += 2;
1361 |         k2 = 2 * k1;
1362 |         wk2r = w[k1];
1363 |         wk2i = w[k1 + 1];
1364 |         wk1r = w[k2];
1365 |         wk1i = w[k2 + 1];
1366 |         wk3r = wk1r - 2 * wk2i * wk1i;
1367 |         wk3i = 2 * wk2i * wk1r - wk1i;
1368 |         for (j = k; j < l + k; j += 2) {
1369 |             j1 = j + l;
1370 |             j2 = j1 + l;
1371 |             j3 = j2 + l;
1372 |             x0r = a[j] + a[j1];
1373 |             x0i = a[j + 1] + a[j1 + 1];
1374 |             x1r = a[j] - a[j1];
1375 |             x1i = a[j + 1] - a[j1 + 1];
1376 |             x2r = a[j2] + a[j3];
1377 |             x2i = a[j2 + 1] + a[j3 + 1];
1378 |             x3r = a[j2] - a[j3];
1379 |             x3i = a[j2 + 1] - a[j3 + 1];
1380 |             a[j] = x0r + x2r;
1381 |             a[j + 1] = x0i + x2i;
1382 |             x0r -= x2r;
1383 |             x0i -= x2i;
1384 |             a[j2] = wk2r * x0r - wk2i * x0i;
1385 |             a[j2 + 1] = wk2r * x0i + wk2i * x0r;
1386 |             x0r = x1r - x3i;
1387 |             x0i = x1i + x3r;
1388 |             a[j1] = wk1r * x0r - wk1i * x0i;
1389 |             a[j1 + 1] = wk1r * x0i + wk1i * x0r;
1390 |             x0r = x1r + x3i;
1391 |             x0i = x1i - x3r;
1392 |             a[j3] = wk3r * x0r - wk3i * x0i;
1393 |             a[j3 + 1] = wk3r * x0i + wk3i * x0r;
1394 |         }
1395 |         wk1r = w[k2 + 2];
1396 |         wk1i = w[k2 + 3];
1397 |         wk3r = wk1r - 2 * wk2r * wk1i;
1398 |         wk3i = 2 * wk2r * wk1r - wk1i;
1399 |         for (j = k + m; j < l + (k + m); j += 2) {
1400 |             j1 = j + l;
1401 |             j2 = j1 + l;
1402 |             j3 = j2 + l;
1403 |             x0r = a[j] + a[j1];
1404 |             x0i = a[j + 1] + a[j1 + 1];
1405 |             x1r = a[j] - a[j1];
1406 |             x1i = a[j + 1] - a[j1 + 1];
1407 |             x2r = a[j2] + a[j3];
1408 |             x2i = a[j2 + 1] + a[j3 + 1];
1409 |             x3r = a[j2] - a[j3];
1410 |             x3i = a[j2 + 1] - a[j3 + 1];
1411 |             a[j] = x0r + x2r;
1412 |             a[j + 1] = x0i + x2i;
1413 |             x0r -= x2r;
1414 |             x0i -= x2i;
1415 |             a[j2] = -wk2i * x0r - wk2r * x0i;
1416 |             a[j2 + 1] = -wk2i * x0i + wk2r * x0r;
1417 |             x0r = x1r - x3i;
1418 |             x0i = x1i + x3r;
1419 |             a[j1] = wk1r * x0r - wk1i * x0i;
1420 |             a[j1 + 1] = wk1r * x0i + wk1i * x0r;
1421 |             x0r = x1r + x3i;
1422 |             x0i = x1i - x3r;
1423 |             a[j3] = wk3r * x0r - wk3i * x0i;
1424 |             a[j3 + 1] = wk3r * x0i + wk3i * x0r;
1425 |         }
1426 |     }
1427 | }
1428 | 
1429 | inline void rftfsub(int n, double *a, int nc, double *c) {
1430 |     int j, k, kk, ks, m;
1431 |     double wkr, wki, xr, xi, yr, yi;
1432 | 
1433 |     m = n >> 1;
1434 |     ks = 2 * nc / m;
1435 |     kk = 0;
1436 |     for (j = 2; j < m; j += 2) {
1437 |         k = n - j;
1438 |         kk += ks;
1439 |         wkr = 0.5 - c[nc - kk];
1440 |         wki = c[kk];
1441 |         xr = a[j] - a[k];
1442 |         xi = a[j + 1] + a[k + 1];
1443 |         yr = wkr * xr - wki * xi;
1444 |         yi = wkr * xi + wki * xr;
1445 |         a[j] -= yr;
1446 |         a[j + 1] -= yi;
1447 |         a[k] += yr;
1448 |         a[k + 1] -= yi;
1449 |     }
1450 | }
1451 | 
1452 | inline void rftbsub(int n, double *a, int nc, double *c) {
1453 |     int j, k, kk, ks, m;
1454 |     double wkr, wki, xr, xi, yr, yi;
1455 | 
1456 |     a[1] = -a[1];
1457 |     m = n >> 1;
1458 |     ks = 2 * nc / m;
1459 |     kk = 0;
1460 |     for (j = 2; j < m; j += 2) {
1461 |         k = n - j;
1462 |         kk += ks;
1463 |         wkr = 0.5 - c[nc - kk];
1464 |         wki = c[kk];
1465 |         xr = a[j] - a[k];
1466 |         xi = a[j + 1] + a[k + 1];
1467 |         yr = wkr * xr + wki * xi;
1468 |         yi = wkr * xi - wki * xr;
1469 |         a[j] -= yr;
1470 |         a[j + 1] = yi - a[j + 1];
1471 |         a[k] += yr;
1472 |         a[k + 1] = yi - a[k + 1];
1473 |     }
1474 |     a[m + 1] = -a[m + 1];
1475 | }
1476 | 
1477 | inline void dctsub(int n, double *a, int nc, double *c) {
1478 |     int j, k, kk, ks, m;
1479 |     double wkr, wki, xr;
1480 | 
1481 |     m = n >> 1;
1482 |     ks = nc / n;
1483 |     kk = 0;
1484 |     for (j = 1; j < m; j++) {
1485 |         k = n - j;
1486 |         kk += ks;
1487 |         wkr = c[kk] - c[nc - kk];
1488 |         wki = c[kk] + c[nc - kk];
1489 |         xr = wki * a[j] - wkr * a[k];
1490 |         a[j] = wkr * a[j] + wki * a[k];
1491 |         a[k] = xr;
1492 |     }
1493 |     a[m] *= c[0];
1494 | }
1495 | 
1496 | inline void dstsub(int n, double *a, int nc, double *c) {
1497 |     int j, k, kk, ks, m;
1498 |     double wkr, wki, xr;
1499 | 
1500 |     m = n >> 1;
1501 |     ks = nc / n;
1502 |     kk = 0;
1503 |     for (j = 1; j < m; j++) {
1504 |         k = n - j;
1505 |         kk += ks;
1506 |         wkr = c[kk] - c[nc - kk];
1507 |         wki = c[kk] + c[nc - kk];
1508 |         xr = wki * a[k] - wkr * a[j];
1509 |         a[k] = wkr * a[k] + wki * a[j];
1510 |         a[j] = xr;
1511 |     }
1512 |     a[m] *= c[0];
1513 | }
1514 | 
1515 | #endif
1516 | 


--------------------------------------------------------------------------------
/include/PostProcessor.h:
--------------------------------------------------------------------------------
  1 | #ifndef _H_AFTER_PROCESSOR_
  2 | #define _H_AFTER_PROCESSOR_
  3 | 
  4 | #include <cstdint>
  5 | #include <cstdlib>
  6 | #include <cstring>
  7 | 
  8 | class PostProcessor {
  9 | private:
 10 |     uint32_t frame_size;
 11 |     uint32_t shift_size;
 12 |     uint32_t channels;
 13 |     // frame_size / shift_size;
 14 |     uint32_t num_block;
 15 |     short *output;
 16 |     double **buf;
 17 |     uint32_t buf_offset;
 18 | 
 19 | public:
 20 |     inline PostProcessor(uint32_t _frame_size,
 21 |                           uint32_t _shift_size,
 22 |                           uint32_t _channels);
 23 |     inline ~PostProcessor();
 24 | 
 25 |     inline short *Overlap(double **in);
 26 |     inline short *OverlapSingle(double **in);
 27 |     inline short *Overlap(double *in);
 28 |     inline short *Array2WavForm(double **in);
 29 | 
 30 |     inline short *Frame2Wav(double *in);
 31 | };
 32 | 
 33 | inline PostProcessor::PostProcessor(uint32_t _frame_size,
 34 |                                       uint32_t _shift_size,
 35 |                                       uint32_t _channels) {
 36 |     int i;
 37 |     channels = _channels;
 38 |     frame_size = _frame_size;
 39 |     shift_size = _shift_size;
 40 | 
 41 |     buf_offset = 0;
 42 | 
 43 |     num_block = frame_size / shift_size;
 44 | 
 45 |     output = new short[shift_size * channels];
 46 |     buf = new double *[channels];
 47 |     for (i = 0; i < static_cast<int>(channels); i++)
 48 |         // buf[i] = new double[ num_block * frame_size ];
 49 |         buf[i] = new double[frame_size];
 50 |     for (i = 0; i < static_cast<int>(channels); i++)
 51 |         // memset(buf[i],0,  num_block * frame_size*sizeof(double)  );
 52 |         memset(buf[i], 0, frame_size * sizeof(double));
 53 | }
 54 | 
 55 | inline PostProcessor::~PostProcessor() {
 56 |     for (int i = 0; i < static_cast<int>(channels); i++)
 57 |         delete[] buf[i];
 58 |     delete[] buf;
 59 |     delete[] output;
 60 | }
 61 | 
 62 | /*
 63 |  * buffer         output
 64 |  * 0  0  0  a1    0
 65 |  * 0  0  a2 b1    0
 66 |  * 0  a3 b2 c1    0
 67 |  * a4 b3 c2 d1    a4 + a3 + a2 + a1
 68 |  * b4 c3 d2 e1    b4 + b3 + b2 + b1
 69 |  *
 70 |  * */
 71 | inline short *PostProcessor::Overlap(double **in) {
 72 |     int i, j;
 73 |     for (j = 0; j < static_cast<int>(channels); j++) {
 74 | 
 75 |         // Shift
 76 |         for (i = 0; i < static_cast<int>(frame_size - shift_size); i++)
 77 |             buf[j][i] = buf[j][i + shift_size];
 78 | 
 79 |         // Emptying Last Block
 80 |         memset(buf[j] + shift_size * (num_block - 1), 0,
 81 |                sizeof(double) * shift_size);
 82 | 
 83 |         // Sum
 84 |         for (i = 0; i < static_cast<int>(frame_size); i++) {
 85 |             buf[j][i] += in[j][i];
 86 |         }
 87 |     }
 88 |     // Distribution for Wav format
 89 |     for (i = 0; i < static_cast<int>(shift_size); i++) {
 90 | #pragma ivdep
 91 |         for (j = 0; j < static_cast<int>(channels); j++) {
 92 |             //output[i * channels + j] = (short)(buf[j][i] * 32767);
 93 |             output[i * channels + j] = static_cast<short>(buf[j][i] * 32767);
 94 | #ifndef NDEBUG
 95 | // printf("Overlap::buf[%d][%d]= %lf\n",j,i,buf[j][i]);
 96 | #endif
 97 |         }
 98 |     }
 99 |     return output;
100 | }
101 | 
102 | 
103 | inline short *PostProcessor::OverlapSingle(double **in){
104 |     int i;
105 |         // Shift
106 |         for (i = 0; i < static_cast<int>(frame_size - shift_size); i++)
107 |             buf[0][i] = buf[0][i + shift_size];
108 |         // Emptying Last Block
109 |         memset(buf[0] + shift_size * (num_block - 1), 0,
110 |                sizeof(double) * shift_size);
111 |         // Sum
112 |         for (i = 0; i < static_cast<int>(frame_size); i++) {
113 |             buf[0][i] += in[0][i];
114 |         }
115 |     // Distribution for Wav format
116 |     for (i = 0; i < static_cast<int>(shift_size); i++) {
117 | #pragma ivdep
118 |             output[i] = static_cast<short>(buf[0][i] * 32767);
119 |         }
120 |     return output;
121 | }
122 | 
123 | inline short *PostProcessor::Overlap(double *in) {
124 |   int i,j;
125 |   for (j = 0; j < static_cast<int>(channels); j++) {
126 |     // Shift
127 |     for (i = 0; i < static_cast<int>(frame_size - shift_size); i++)
128 |       buf[j][i] = buf[j][i + shift_size];
129 | 
130 |     // Emptying Last Block
131 |     memset(buf[j] + shift_size * (num_block - 1), 0,
132 |         sizeof(double) * shift_size);
133 | 
134 |     // Sum
135 |     for (i = 0; i < static_cast<int>(frame_size); i++) {
136 |       buf[j][i] += in[j*(frame_size+2) +  i];
137 |     }
138 |   }
139 |   // Distribution for Wav format
140 |   for (i = 0; i < static_cast<int>(shift_size); i++) {
141 | #pragma ivdep
142 |     //output[i * channels + ch] = (short)(buf[ch][i] * 32767);
143 |     for (j = 0; j < static_cast<int>(channels); j++) {
144 |       output[i * channels + j] = static_cast<short>(buf[j][i] * 32767);
145 |     }
146 |   }
147 | 
148 |   return output;
149 | }
150 | 
151 | inline short *PostProcessor::Array2WavForm(double **in) {
152 |     int i, j;
153 |     for (i = 0; i < static_cast<int>(shift_size); i++) {
154 |         for (j = 0; j < static_cast<int>(channels); j++) {
155 |             output[i * channels + j] = (short)in[j][i];
156 |         }
157 |     }
158 |     return output;
159 | }
160 | 
161 | inline short *PostProcessor::Frame2Wav(double *in) {
162 |     int i, j;
163 |     for (i = 0; i < static_cast<int>(frame_size); i++) {
164 |         for (j = 0; j < static_cast<int>(channels); j++) {
165 |             output[i * channels + j] = (short)in[j*frame_size + i];
166 |         }
167 |     }
168 |     return output;
169 | }
170 | 
171 | #endif
172 | 


--------------------------------------------------------------------------------
/include/STFT.h:
--------------------------------------------------------------------------------
  1 | #ifndef _H_STFT_
  2 | #define _H_STFT_
  3 | 
  4 | #include "Ooura_FFT.h"
  5 | #include "HannWindow.h"
  6 | #include "PostProcessor.h"
  7 | 
  8 | class STFT{
  9 |   private : 
 10 |     HannWindow *hw;
 11 |     Ooura_FFT *fft;
 12 |     PostProcessor *ap;
 13 | 
 14 |     int channels;
 15 |     int frame_size;
 16 |     int shift_size;
 17 |     int ol;
 18 | 
 19 |     double**buf;
 20 | 
 21 |   public :
 22 |     inline STFT(int channels,int frame,int shift);
 23 |     inline ~STFT();
 24 |     inline void stft(short*in,int length,double**out);
 25 |     inline void istft(double**in,short*out);
 26 | };
 27 | 
 28 | 
 29 | STFT::STFT(int channels_,int frame_,int shift_){
 30 |   int i;
 31 |   channels = channels_;
 32 |   frame_size = frame_;
 33 |   shift_size = shift_;
 34 |   ol = frame_size - shift_size;
 35 | 
 36 |   hw = new HannWindow(frame_size, shift_size);
 37 |   fft= new Ooura_FFT(frame_size, channels);
 38 |   ap = new PostProcessor(frame_size, shift_size, channels);
 39 | 
 40 |   buf =  new double*[channels];
 41 |   for(i=0;i<channels;i++){
 42 |     buf[i] = new double[frame_size];
 43 |     memset(buf[i],0,sizeof(double)*frame_size);
 44 |   }
 45 | }
 46 | 
 47 | STFT::~STFT(){
 48 |   int i;
 49 |   delete hw;
 50 |   delete fft;
 51 |   delete ap;
 52 |   for(i=0;i<channels;i++)
 53 |     delete[] buf[i];
 54 |   delete[] buf;
 55 | 
 56 | }
 57 | 
 58 | void STFT::stft(short*in,int length,double**out){
 59 |   int i,j;
 60 |   /*** Shfit & Copy***/
 61 |   for (j = 0; j < channels; j++) {
 62 |     for (i = 0; i < ol; i++) {
 63 |       buf[j][i] = buf[j][i + shift_size];
 64 |     }
 65 |   }
 66 |   //// EOF
 67 |   if(length!=shift_size*channels){
 68 | 
 69 |     length = length/channels;
 70 |     for (i = 0; i < length; i++) {
 71 |       for (j = 0; j < channels; j++)
 72 |         buf[j][i + ol]
 73 |           =  (double)(in[i * channels+ j]);
 74 |     }
 75 |     for (i = length; i < shift_size; i++) {
 76 |       for (j = 0; j < channels; j++)
 77 |         buf[j][i + ol] = 0;
 78 |     }
 79 |     //// continue
 80 |   }else{
 81 |     for (i = 0; i < shift_size; i++) {
 82 |       for (j = 0; j < channels; j++){
 83 |         buf[j][i + ol] 
 84 |           = (double)(in[i * channels+ j]);
 85 |       }
 86 |     }
 87 |   }
 88 |   /*** Copy input -> hann_input buffer ***/
 89 |   for (i = 0; i < channels; i++)
 90 |     memcpy(out[i], buf[i], sizeof(double) * frame_size);
 91 | 
 92 |   // scaling for precision
 93 |   for (i = 0; i < channels; i++)
 94 |     for (j = 0; j < frame_size; j++)
 95 |       out[i][j] /= 32767.0;
 96 | 
 97 |   /*** Window ***/
 98 |   hw->Process(out, channels);
 99 | 
100 |   /*** FFT ***/
101 |   fft->FFT(out);
102 | }
103 | 
104 | 
105 | void STFT::istft(double**in,short*out){
106 |   /*** iFFT ***/
107 |   fft->iFFT(in);
108 | 
109 |   /*** Window ***/
110 |   hw->Process(in, channels);
111 | 
112 |   /*** Output ***/
113 |   memcpy(out,ap->Overlap(in),sizeof(short)*shift_size*channels);
114 | }
115 | 
116 | 
117 | #endif
118 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
  1 | #define _CRT_SECURE_NO_WARNINGS
  2 | #include "RT_Input.h"
  3 | #include "WAV/WAV.h"
  4 | #include <stdio.h>
  5 | #include <stdlib.h>
  6 | #include <string.h>
  7 | #include "STFT/cpp/STFT.h"
  8 | #include "SRP_DSB.h"
  9 | #include "GCC_PHAT.h"
 10 | #include <math.h>
 11 | 
 12 | #define PI 3.1415926535897
 13 | #define DEVICE 4
 14 | #define CHANNEL 2
 15 | #define SAMPLE_RATE 16000
 16 | #define FRAME_SIZE 512
 17 | #define SHIFT_SIZE 128
 18 | #define MIC_DISTANCE 0.05
 19 | 
 20 | double getRadian(int _num) { return _num * (PI / 180); }
 21 | 
 22 | int main() {
 23 | 
 24 |     /* Available Device Information */
 25 |     RtAudio audio;
 26 |     unsigned int devices = audio.getDeviceCount();
 27 |     RtAudio::DeviceInfo info;
 28 |     for (unsigned int i = 0; i < devices; i++) {
 29 |         info = audio.getDeviceInfo(i);
 30 |         std::cout << "device = " << i;
 31 |         std::cout << ": device name = " << info.name << "\n";
 32 |     }
 33 | 
 34 |     /* Declaration */
 35 |     const int device = DEVICE;
 36 |     const int ch = CHANNEL;
 37 |     const int fs = SAMPLE_RATE;
 38 |     const int frame = FRAME_SIZE;
 39 |     const int shift = SHIFT_SIZE;
 40 |     const double mic_dist = MIC_DISTANCE;
 41 | 
 42 |     /* Initialization */
 43 |     RT_Input input(device, 16, fs, shift, frame);
 44 |     GCC_PHAT* gcc = nullptr;
 45 |     STFT* stft = nullptr;
 46 |     WAV* wav_1 = nullptr;
 47 |     WAV* wav_2 = nullptr;
 48 | 
 49 |     double sample_delay;
 50 |     double** raw;
 51 |     double** raw_tmp;
 52 |     double** data;
 53 | 
 54 |     stft = new STFT(ch, frame, shift);
 55 |     gcc = new GCC_PHAT(ch, frame, fs, shift, mic_dist);
 56 | 
 57 |     raw_tmp = new double* [16];
 58 |     for (int i = 0; i < 16; i++)
 59 |         raw_tmp[i] = new double[shift];
 60 | 
 61 |     raw = new double* [ch];
 62 |     for (int i = 0; i < ch; i++)
 63 |         raw[i] = new double[shift];
 64 | 
 65 |     data = new double* [ch];
 66 |     for (int i = 0; i < ch; i++)
 67 |         data[i] = new double[frame + 2];
 68 | 
 69 |     /* process */
 70 |     int unit = shift * 1;
 71 |     size_t read_1, read_2;
 72 |     bool prev_sil = true;
 73 |     bool sil = true;
 74 |     int t = 0;
 75 | 
 76 |     input.Start();
 77 | 
 78 |     while (input.IsRunning()) {
 79 |         if (input.data.stock.load() >= shift) {
 80 |             input.Convert2Array(raw_tmp);
 81 |             for (int i = 0; i < shift; i++) {
 82 |                 raw[0][i] = raw_tmp[4][i];
 83 |                 raw[1][i] = raw_tmp[8][i];
 84 |             }
 85 | 
 86 |             stft->stft(raw, data);
 87 |             /* Estimation Result */
 88 |             if (!stft->sil) {
 89 |                 gcc->Process(data);
 90 |                 printf("t : %d |     Estimated Azimuth %f\n", t, (gcc->Estimated_azimuth.real() + 90));
 91 |             }
 92 |             if (!prev_sil && stft->sil)
 93 |                 printf("silence\n");
 94 |             prev_sil = stft->sil;
 95 |             t++;
 96 |         }
 97 |         else {
 98 |             SLEEP(10);
 99 |         }
100 |     }
101 |     printf("Streaming Finished!!!\n");
102 |     delete gcc;
103 |     delete stft;
104 | 
105 |     for (int i = 0; i < ch; i++) {
106 |         delete[] raw[i];
107 |         delete[] data[i];
108 |     }
109 |     delete[] raw;
110 |     delete[] data;
111 | 
112 |     input.Stop();
113 |     return 0;
114 | }


--------------------------------------------------------------------------------