├── .gitignore ├── Makefile ├── ciglet.c ├── ciglet.h ├── ciglet.sln ├── ciglet.vcxproj ├── external ├── fast_median.c ├── fastapprox-all.h ├── fftsg_h.c └── wavfile.c ├── readme.md └── test ├── test-fft.c ├── test.c ├── test.vcxproj └── test.vcxproj.filters /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.a 3 | test/*.wav 4 | single-file/* 5 | 6 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | PREFIX = /usr 2 | CC = $(CROSS)gcc 3 | LINK = $(CROSS)gcc 4 | AR = $(CROSS)ar 5 | 6 | FP_TYPE = float 7 | CONFIG = Debug 8 | 9 | ifeq 'Darwin' '$(shell uname)' 10 | CFLAGS_PLAT = 11 | else 12 | CFLAGS_PLAT = -fopenmp 13 | endif 14 | 15 | CFLAGS_COMMON = -Iexternal -D_POSIX_C_SOURCE=2 -DFP_TYPE=$(FP_TYPE) -std=c99 -Wall -fPIC $(CFLAGSEXT) $(CFLAGS_PLAT) 16 | CFLAGS_DBG = $(CFLAGS_COMMON) -Og -g 17 | CFLAGS_REL = $(CFLAGS_COMMON) -Ofast 18 | ifeq ($(CONFIG), Debug) 19 | CFLAGS = $(CFLAGS_DBG) 20 | else 21 | CFLAGS = $(CFLAGS_REL) 22 | endif 23 | ARFLAGS = -rv 24 | OBJS = ciglet.o fftsg.o fastmedian.o wavfile.o 25 | 26 | default: libciglet.a 27 | 28 | test: ciglet-test 29 | ./ciglet-test noplot 30 | 31 | test-fft: ciglet-test-fft 32 | ./ciglet-test-fft 33 | 34 | single-file: single-file/ciglet.c single-file/ciglet.h 35 | $(CC) $(CFLAGS) -o single-file/ciglet.o -c single-file/ciglet.c -Wno-unused-result 36 | 37 | ciglet-test: libciglet.a test/test.c 38 | $(CC) $(CFLAGS) test/test.c libciglet.a -o ciglet-test -lm 39 | 40 | ciglet-test-fft: libciglet.a test/test-fft.c 41 | $(CC) $(CFLAGS) test/test-fft.c libciglet.a -o ciglet-test-fft -lm 42 | 43 | libciglet.a: $(OBJS) 44 | $(AR) $(ARFLAGS) libciglet.a $(OBJS) 45 | @echo Done. 46 | 47 | install: libciglet.a ciglet.h 48 | mkdir -p $(PREFIX)/lib/ $(PREFIX)/include/ciglet/external 49 | cp libciglet.a $(PREFIX)/lib 50 | cp ciglet.h $(PREFIX)/include/ciglet 51 | cp external/fastapprox-all.h $(PREFIX)/include/ciglet/external 52 | 53 | ciglet.o: ciglet.c ciglet.h 54 | fftsg.o: external/fftsg_h.c 55 | $(CC) $(CFLAGS) -o fftsg.o -c external/fftsg_h.c 56 | fastmedian.o: external/fast_median.c 57 | $(CC) $(CFLAGS) -o fastmedian.o -c external/fast_median.c 58 | wavfile.o: external/wavfile.c 59 | $(CC) $(CFLAGS) -o wavfile.o -c external/wavfile.c -Wno-unused-result 60 | 61 | %.o: %.c 62 | $(CC) $(CFLAGS) -o $*.o -c $*.c 63 | 64 | single-file/ciglet.c: ciglet.c 65 | mkdir -p single-file 66 | cat external/fftsg_h.c > single-file/ciglet.c 67 | cat external/fast_median.c >> single-file/ciglet.c 68 | cat external/wavfile.c >> single-file/ciglet.c 69 | cat ciglet.c >> single-file/ciglet.c 70 | 71 | single-file/ciglet.h: ciglet.h 72 | mkdir -p single-file 73 | cat external/fastapprox-all.h > single-file/ciglet.h 74 | echo "" >> single-file/ciglet.h 75 | echo "#define CIGLET_SINGLE_FILE" >> single-file/ciglet.h 76 | echo "" >> single-file/ciglet.h 77 | cat ciglet.h >> single-file/ciglet.h 78 | 79 | clean: 80 | @echo 'Removing all temporary binaries... ' 81 | @rm -f *.o *.a 82 | @echo Done. 83 | 84 | clear: 85 | @echo 'Removing all temporary binaries... ' 86 | @rm -f *.o *.a 87 | @echo Done. 88 | -------------------------------------------------------------------------------- /ciglet.c: -------------------------------------------------------------------------------- 1 | /* 2 | ciglet 3 | === 4 | 5 | Copyright (c) 2016-2019, Kanru Hua 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without modification, 9 | are permitted provided that the following conditions are met: 10 | 11 | 1. Redistributions of source code must retain the above copyright notice, this 12 | list of conditions and the following disclaimer. 13 | 14 | 2. Redistributions in binary form must reproduce the above copyright notice, 15 | this list of conditions and the following disclaimer in the documentation and/or 16 | other materials provided with the distribution. 17 | 18 | 3. Neither the name of the copyright holder nor the names of its contributors 19 | may be used to endorse or promote products derived from this software without 20 | specific prior written permission. 21 | 22 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 23 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 24 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 26 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 29 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "ciglet.h" 35 | 36 | // fftsg 37 | void cdft(int n, int isgn, FP_TYPE* a); 38 | void rdft(int n, int isgn, FP_TYPE* a); 39 | 40 | // fast median 41 | FP_TYPE opt_med3(FP_TYPE* x); 42 | FP_TYPE opt_med5(FP_TYPE* x); 43 | FP_TYPE opt_med7(FP_TYPE* x); 44 | FP_TYPE opt_med9(FP_TYPE* x); 45 | FP_TYPE opt_med25(FP_TYPE* x); 46 | 47 | #define def_transpose(nbit, type) \ 48 | static type** cig_transpose_##nbit(type** ptr, size_t m, size_t n) { \ 49 | type** ret = malloc(n * sizeof(type*)); \ 50 | for(size_t i = 0; i < n; i ++) { \ 51 | ret[i] = malloc(m * sizeof(type)); \ 52 | for(size_t j = 0; j < m; j ++) \ 53 | ret[i][j] = ptr[j][i]; \ 54 | } \ 55 | return ret; \ 56 | } 57 | 58 | def_transpose(8, unsigned char); 59 | def_transpose(16, uint16_t); 60 | def_transpose(32, uint32_t); 61 | def_transpose(64, uint64_t); 62 | 63 | void** cig_transpose(void** ptr, size_t m, size_t n, size_t size) { 64 | switch(size * 8) { 65 | case 8: 66 | return (void**)cig_transpose_8((unsigned char**)ptr, m, n); 67 | break; 68 | case 16: 69 | return (void**)cig_transpose_16((uint16_t**)ptr, m, n); 70 | break; 71 | case 32: 72 | return (void**)cig_transpose_32((uint32_t**)ptr, m, n); 73 | break; 74 | case 64: 75 | return (void**)cig_transpose_64((uint64_t**)ptr, m, n); 76 | break; 77 | } 78 | fprintf(stderr, "Error: invalid element size '%d'.\n", (int)size); 79 | return NULL; 80 | } 81 | 82 | // https://wikicoding.org/wiki/c/Quickselect/ 83 | FP_TYPE cig_qselect(FP_TYPE* v, int len, int k) { 84 | # define swap(a, b) { \ 85 | tmp = v[a]; v[a] = v[b]; v[b] = tmp; \ 86 | } 87 | int i, st; 88 | FP_TYPE tmp; 89 | 90 | for(st = i = 0; i < len - 1; i ++) { 91 | if(v[i] > v[len - 1]) continue; 92 | swap(i, st); 93 | st ++; 94 | } 95 | swap(len - 1, st); 96 | 97 | return k == st ? v[st] 98 | :st > k ? cig_qselect(v, st, k) 99 | : cig_qselect(v + st, len - st, k - st); 100 | # undef swap 101 | } 102 | 103 | typedef struct { 104 | FP_TYPE val; 105 | int idx; 106 | } sort_struct; 107 | 108 | int compare_sort_struct(const void* a, const void* b) { 109 | return ((sort_struct*)a) -> val > ((sort_struct*)b) -> val ? 1 : -1; 110 | } 111 | 112 | FP_TYPE* cig_sort(FP_TYPE* x, int nx, int* outidx) { 113 | sort_struct* s = malloc(nx * sizeof(sort_struct)); 114 | for(int i = 0; i < nx; i ++) { 115 | s[i].val = x[i]; 116 | s[i].idx = i; 117 | } 118 | qsort(s, nx, sizeof(sort_struct), compare_sort_struct); 119 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 120 | for(int i = 0; i < nx; i ++) 121 | y[i] = s[i].val; 122 | if(outidx != NULL) 123 | for(int i = 0; i < nx; i ++) 124 | outidx[i] = s[i].idx; 125 | free(s); 126 | return y; 127 | } 128 | 129 | FP_TYPE cig_medianfp(FP_TYPE* x, int nx) { 130 | FP_TYPE* x_ = malloc(nx * sizeof(FP_TYPE)); 131 | memcpy(x_, x, nx * sizeof(FP_TYPE)); 132 | FP_TYPE ret = nx % 2 == 1 ? cig_qselect(x_, nx, nx / 2) : 133 | (cig_qselect(x_, nx, nx / 2 - 1) + cig_qselect(x_, nx, nx / 2)) * 0.5; 134 | free(x_); 135 | return ret; 136 | } 137 | 138 | FP_TYPE* cig_xcorr(FP_TYPE* x, FP_TYPE* y, int nx, int maxlag) { 139 | FP_TYPE* R = calloc(maxlag, sizeof(FP_TYPE)); 140 | for(int m = 0; m < maxlag; m ++) { 141 | for(int i = 0; i < nx - m; i ++) 142 | R[m] += x[m + i] * y[i]; 143 | } 144 | return R; 145 | } 146 | 147 | // Brent's method (inverse quad interp + secant + bisection) 148 | // https://en.wikipedia.org/wiki/Brent%27s_method 149 | FP_TYPE cig_fzero(fpe_one_to_one func, FP_TYPE xmin, FP_TYPE xmax, void* env) { 150 | const FP_TYPE eps = 1e-8; 151 | FP_TYPE a, b, c, s, d; 152 | FP_TYPE fa, fb, fc, fs; 153 | a = xmin; b = xmax; 154 | fa = func(a, env); fb = func(b, env); 155 | if(fa * fb >= 0) return (a + b) / 2.0; 156 | if(fabs(fa) < fabs(fb)) { 157 | c = b; b = a; a = c; 158 | fc = fb; fb = fa; fa = fc; 159 | } else { 160 | c = a; 161 | fc = fa; 162 | } 163 | int mflag = 1; 164 | int niter = 0; 165 | while(fabs(fb) > eps && fabs(a - b) > eps) { 166 | if(fa != fc && fb != fc) { 167 | s = a * fb * fc / (fa - fb) / (fa - fc) 168 | + b * fa * fc / (fb - fa) / (fb - fc) 169 | + c * fa * fb / (fc - fa) / (fc - fb); // Inv. Quad Interpolation 170 | } else { 171 | s = b - fb * (b - a) / (fb - fa); // Secant 172 | } 173 | if((s < (3.0 * a + b) / 4.0 || s > b) || 174 | (mflag && fabs(s - b) >= fabs(b - c) * 0.5) || 175 | (! mflag && fabs(s - b) >= fabs(c - d) * 0.5) || 176 | (mflag && fabs(b - c) < eps) || 177 | (! mflag && fabs(c - d) < eps)) { 178 | s = (a + b) / 2.0; 179 | if(s == b || s == a) break; // maximum precision reached 180 | mflag = 1; 181 | } else { 182 | mflag = 0; 183 | } 184 | fs = func(s, env); 185 | d = c; c = b; 186 | fc = fb; 187 | if(fa * fs < 0) { 188 | b = s; 189 | fb = fs; 190 | } else { 191 | a = s; 192 | fa = fs; 193 | } 194 | if(fabs(fa) < fabs(fb)) { 195 | FP_TYPE x = b; b = a; a = x; 196 | x = fb; fb = fa; fa = x; 197 | } 198 | niter ++; 199 | } 200 | return b; 201 | } 202 | 203 | cplx cig_polyval(cplx* poly, int np, cplx x) { 204 | cplx y = poly[np - 1]; // constant term 205 | cplx xpow = x; 206 | for(int i = np - 2; i >= 0; i --) { 207 | y = c_add(y, c_mul(poly[i], xpow)); 208 | xpow = c_mul(xpow, x); 209 | } 210 | return y; 211 | } 212 | 213 | // Durand-Kerner method for finding polynomial roots 214 | cplx* cig_roots(cplx* poly, int np) { 215 | if(np < 2) return NULL; 216 | cplx* r0 = calloc(np - 1, sizeof(cplx)); 217 | cplx* r1 = calloc(np - 1, sizeof(cplx)); 218 | 219 | // normalize polynomial coefficients 220 | cplx* a = calloc(np, sizeof(cplx)); 221 | for(int i = 0; i < np; i ++) 222 | a[i] = c_div(poly[i], poly[0]); 223 | 224 | // max radius of roots 225 | FP_TYPE rmax = 0; 226 | for(int i = 0; i < np; i ++) 227 | rmax = max(rmax, c_abs(a[i])); 228 | rmax += 1; 229 | 230 | r0[0] = c_cplx(1, 0); 231 | if(np > 3) { 232 | // start from np-1 numbers uniformly distributed on the unit circle 233 | // if np <= 3, all initial values would be real, which hardly converge. 234 | cplx rotation = c_cplx(cos_1(2 * M_PI / (np - 1)), sin_1(2 * M_PI / (np - 1))); 235 | for(int i = 1; i < np - 1; i ++) { 236 | r0[i] = c_mul(r0[i - 1], rotation); 237 | r1[i] = r0[i]; 238 | } 239 | } else { 240 | for(int i = 1; i < np - 1; i ++) 241 | r0[i] = r1[i] = c_cplx(0.4, 0.6); 242 | } 243 | 244 | const int niter = 50; 245 | for(int n = 0; n < niter; n ++) { 246 | FP_TYPE maxerr = 0; 247 | for(int i = 0; i < np - 1; i ++) { 248 | cplx fval = cig_polyval(a, np, r0[i]); 249 | FP_TYPE err = c_abs(fval); 250 | if(err > maxerr) maxerr = err; 251 | cplx denom = c_cplx(1, 0); 252 | for(int j = 0; j < np - 1; j ++) { // calculate denominator 253 | if(j == i) continue; 254 | denom = c_mul(denom, c_sub(r0[i], r0[j])); 255 | } 256 | cplx delta = c_div(fval, denom); 257 | r1[i] = c_sub(r0[i], delta); 258 | } 259 | if(maxerr < 1e-6) break; 260 | for(int i = 0; i < np - 1; i ++) { 261 | // take off-center points back into possible range of roots 262 | FP_TYPE rr = c_abs(r1[i]); 263 | if(rr > rmax) { 264 | r1[i].real *= rmax / rr; 265 | r1[i].imag *= rmax / rr; 266 | } 267 | r0[i] = r1[i]; 268 | } 269 | } 270 | 271 | free(a); 272 | free(r0); 273 | return r1; 274 | } 275 | 276 | int* cig_ppivot(FP_TYPE* A, int n) { 277 | int* permidx = malloc(n * sizeof(int)); 278 | for(int i = 0; i < n; i ++) { 279 | // compare diagonal entry with the rest in the same column 280 | int maxrow = i; FP_TYPE maxval = 0; 281 | for(int j = i; j < n; j ++) 282 | if(fabs(A[j + i * n]) > maxval) { 283 | maxval = fabs(A[j + i * n]); 284 | maxrow = j; 285 | } 286 | permidx[i] = maxrow; 287 | if(maxrow != i) { // exchange row i with row maxrow 288 | for(int j = 0; j < n; j ++) { 289 | FP_TYPE tmp = A[i + j * n]; 290 | A[i + j * n] = A[maxrow + j * n]; 291 | A[maxrow + j * n] = tmp; 292 | } 293 | } 294 | } 295 | return permidx; 296 | } 297 | 298 | void cig_permm(FP_TYPE* A, int* perm, int m, int n) { 299 | for(int i = 0; i < m; i ++) { 300 | int permrow = perm[i]; 301 | if(permrow != i) { // exchange row i with row perm[i] 302 | for(int j = 0; j < n; j ++) { 303 | FP_TYPE tmp = A[i + j * n]; 304 | A[i + j * n] = A[permrow + j * n]; 305 | A[permrow + j * n] = tmp; 306 | } 307 | } 308 | } 309 | } 310 | 311 | void cig_lu(FP_TYPE* A, int n) { 312 | for(int i = 0; i < n; i ++) { 313 | for(int j = 0; j < n; j ++) { 314 | if(j >= i) { // U 315 | FP_TYPE udotl = 0; 316 | for(int k = 0; k < i; k ++) 317 | udotl += A[k + j * n] * A[i + k * n]; 318 | A[i + j * n] -= udotl; 319 | } else { // L 320 | FP_TYPE udotl = 0; 321 | for(int k = 0; k < j; k ++) 322 | udotl += A[k + j * n] * A[i + k * n]; 323 | A[i + j * n] = (A[i + j * n] - udotl) / A[j + j * n]; 324 | } 325 | } 326 | } 327 | } 328 | 329 | void cig_lusolve(FP_TYPE* LU, FP_TYPE* b, int n) { 330 | // Solve Ly = b 331 | for(int j = 0; j < n; j ++) { 332 | for(int i = j + 1; i < n; i ++) 333 | b[i] -= LU[i + j * n] * b[j]; 334 | } 335 | // Solve Ux = y 336 | for(int j = n - 1; j >= 0; j --) { 337 | b[j] /= LU[j + j * n]; 338 | for(int i = 0; i < j; i ++) 339 | b[i] -= LU[i + j * n] * b[j]; 340 | } 341 | } 342 | 343 | // m x n, n * l -> m * l 344 | // i j j k i k 345 | void cig_matmul(FP_TYPE* A, FP_TYPE* B, FP_TYPE* C, int m, int n, int l) { 346 | for(int k = 0; k < l; k ++) { 347 | // k-th column in C is the sum of columns in A, weighted by k-th column in B 348 | for(int i = 0; i < m; i ++) C[i + k * m] = 0; 349 | for(int j = 0; j < n; j ++) { 350 | FP_TYPE b = B[j + k * n]; 351 | for(int i = 0; i < m; i ++) 352 | C[i + k * m] += b * A[i + j * m]; 353 | } 354 | } 355 | } 356 | 357 | void cig_mvecmul(FP_TYPE* A, FP_TYPE* x, FP_TYPE* b, int m, int n) { 358 | for(int i = 0; i < m; i ++) b[i] = 0; 359 | for(int j = 0; j < n; j ++) { 360 | FP_TYPE xj = x[j]; 361 | for(int i = 0; i < m; i ++) 362 | b[i] += xj * A[i + j * m]; 363 | } 364 | } 365 | 366 | // orient: 1 (maximum) or -1 (minimum) 367 | int cig_find_peak(FP_TYPE* x, int lidx, int uidx, int orient) { 368 | FP_TYPE max = x[lidx] * orient; 369 | FP_TYPE max_idx = lidx; 370 | for(int i = lidx; i <= uidx; i ++) 371 | if(x[i] * orient > x[i - 1] * orient && x[i] * orient > x[i + 1] * orient) 372 | if(x[i] * orient > max) { 373 | max = x[i] * orient; 374 | max_idx = i; 375 | } 376 | return max_idx; 377 | } 378 | 379 | // orient: 1 (maximum) or -1 (minimum) 380 | int cig_find_extrema(FP_TYPE* x, int lidx, int uidx, int orient) { 381 | FP_TYPE max = x[lidx] * orient; 382 | FP_TYPE max_idx = lidx; 383 | for(int i = lidx; i <= uidx; i ++) 384 | if(x[i] * orient > max) { 385 | max = x[i] * orient; 386 | max_idx = i; 387 | } 388 | return max_idx; 389 | } 390 | 391 | FP_TYPE* cig_gensins(FP_TYPE* freq, FP_TYPE* ampl, FP_TYPE* phse, 392 | int nsin, int fs, int n) { 393 | FP_TYPE* x = calloc(n, sizeof(FP_TYPE)); 394 | FP_TYPE* s = calloc(n, sizeof(FP_TYPE)); 395 | for(int i = 0; i < nsin; i ++) { 396 | FP_TYPE i_f = freq[i]; 397 | FP_TYPE i_a = ampl[i]; 398 | FP_TYPE i_p = phse[i]; 399 | 400 | FP_TYPE tpffs = 2.0 * M_PI / fs * i_f; 401 | FP_TYPE c = 2.0 * cos_3(tpffs); 402 | s[0] = cos_3(tpffs * (- n / 2) + i_p) * i_a; 403 | s[1] = cos_3(tpffs * (- n / 2 + 1) + i_p) * i_a; 404 | x[0] += s[0]; 405 | x[1] += s[1]; 406 | for(int t = 2; t < n; t ++) { 407 | s[t] = c * s[t - 1] - s[t - 2]; 408 | x[t] += s[t]; 409 | } 410 | } 411 | free(s); 412 | return x; 413 | } 414 | 415 | static FP_TYPE* get_window(char* name, int nw, int optlv) { 416 | if(optlv >= 3) { 417 | if(! strcmp(name, "hanning")) 418 | return hanning(nw); 419 | else if(! strcmp(name, "hamming")) 420 | return hamming(nw); 421 | else if(! strcmp(name, "blackman")) 422 | return blackman(nw); 423 | else if(! strcmp(name, "blackman_harris")) 424 | return blackman_harris(nw); 425 | return boxcar(nw); 426 | } else { 427 | if(! strcmp(name, "hanning")) 428 | return hanning_2(nw); 429 | else if(! strcmp(name, "hamming")) 430 | return hamming_2(nw); 431 | else if(! strcmp(name, "blackman")) 432 | return blackman_2(nw); 433 | else if(! strcmp(name, "blackman_harris")) 434 | return blackman_harris_2(nw); 435 | return boxcar(nw); 436 | } 437 | } 438 | 439 | FP_TYPE* cig_dct(FP_TYPE* x, int nx) { 440 | FP_TYPE* ret = calloc(nx, sizeof(FP_TYPE)); 441 | for(int k = 0; k < nx; k ++) 442 | for(int n = 0; n < nx; n ++) 443 | ret[k] += x[n] * cos_2(M_PI / nx * (n + 0.5) * k); 444 | ret[0] /= sqrt(nx); 445 | for(int k = 1; k < nx; k ++) 446 | ret[k] /= sqrt(nx * 0.5); 447 | return ret; 448 | } 449 | 450 | void cig_fft(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, 451 | int n, FP_TYPE* buffer, FP_TYPE mode) { 452 | if(xr != NULL) { 453 | for(int i = 0; i < n; i ++) 454 | buffer[i * 2] = xr[i]; 455 | } else { 456 | for(int i = 0; i < n; i ++) 457 | buffer[i * 2] = 0.0; 458 | } 459 | if(xi != NULL) { 460 | for(int i = 0; i < n; i ++) 461 | buffer[i * 2 + 1] = xi[i]; 462 | } else { 463 | for(int i = 0; i < n; i ++) 464 | buffer[i * 2 + 1] = 0.0; 465 | } 466 | cdft(2 * n, mode, buffer); 467 | if(mode < 0) { 468 | if(yr != NULL) { 469 | for(int i = 0; i < n; i ++) 470 | yr[i] = buffer[i * 2]; 471 | } 472 | if(yi != NULL) { 473 | for(int i = 0; i < n; i ++) 474 | yi[i] = buffer[i * 2 + 1]; 475 | } 476 | } else { 477 | if(yr != NULL) { 478 | for(int i = 0; i < n; i ++) 479 | yr[i] = buffer[i * 2] / n; 480 | } 481 | if(yi != NULL) { 482 | for(int i = 0; i < n; i ++) 483 | yi[i] = buffer[i * 2 + 1] / n; 484 | } 485 | } 486 | } 487 | 488 | void cig_czt(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, 489 | FP_TYPE omega0, int n) { 490 | int m = pow(2, ceil(log2(n)) + 1); 491 | FP_TYPE* buffer = calloc(m * 6, sizeof(FP_TYPE)); 492 | FP_TYPE* xq = buffer + 0; 493 | FP_TYPE* wq = buffer + 2 * m; 494 | FP_TYPE* Wq = buffer + 4 * m; 495 | Wq[0] = wq[0] = 1.0; Wq[1] = wq[1] = 0; 496 | for(int i = 1; i < n; i ++) { 497 | FP_TYPE phi = -0.5 * i * i * omega0; 498 | FP_TYPE Wr = cos_3(phi); 499 | FP_TYPE Wi = sin_3(phi); 500 | Wq[i * 2 + 0] = wq[i * 2 + 0] = Wr; 501 | Wq[i * 2 + 1] = wq[i * 2 + 1] = -Wi; 502 | Wq[(m - i) * 2 + 0] = wq[(m - i) * 2 + 0] = Wr; 503 | Wq[(m - i) * 2 + 1] = wq[(m - i) * 2 + 1] = -Wi; 504 | } 505 | if(xi != NULL) { 506 | xq[1] = xi[0]; 507 | for(int i = 1; i < n; i ++) { 508 | FP_TYPE Wr = wq[i * 2 + 0]; 509 | FP_TYPE Wi = - wq[i * 2 + 1]; 510 | xq[i * 2 + 0] = - xi[i] * Wi; 511 | xq[i * 2 + 1] = xi[i] * Wr; 512 | } 513 | } 514 | if(xr != NULL) { 515 | xq[0] = xr[0]; 516 | for(int i = 1; i < n; i ++) { 517 | FP_TYPE Wr = wq[i * 2 + 0]; 518 | FP_TYPE Wi = - wq[i * 2 + 1]; 519 | xq[i * 2 + 0] += xr[i] * Wr; 520 | xq[i * 2 + 1] += xr[i] * Wi; 521 | } 522 | } 523 | cdft(2 * m, -1, xq); 524 | cdft(2 * m, -1, Wq); 525 | for(int i = 0; i < m; i ++) { 526 | FP_TYPE yqr = xq[i * 2 + 0] * Wq[i * 2 + 0] - xq[i * 2 + 1] * Wq[i * 2 + 1]; 527 | FP_TYPE yqi = xq[i * 2 + 0] * Wq[i * 2 + 1] + xq[i * 2 + 1] * Wq[i * 2 + 0]; 528 | xq[i * 2 + 0] = yqr; 529 | xq[i * 2 + 1] = yqi; 530 | } 531 | cdft(2 * m, 1, xq); 532 | if(yr != NULL) { 533 | for(int i = 0; i < n; i ++) 534 | yr[i] = ( xq[i * 2 + 0] * wq[i * 2 + 0] + xq[i * 2 + 1] * wq[i * 2 + 1]) / m; 535 | } 536 | if(yi != NULL) { 537 | for(int i = 0; i < n; i ++) 538 | yi[i] = (- xq[i * 2 + 0] * wq[i * 2 + 1] + xq[i * 2 + 1] * wq[i * 2 + 0]) / m; 539 | } 540 | free(buffer); 541 | } 542 | 543 | void cig_idft(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, int n) { 544 | FP_TYPE tpon = 2.0 * M_PI / n; 545 | for(int k = 0; k < n; k ++) { 546 | FP_TYPE sumr = 0; 547 | FP_TYPE sumi = 0; 548 | FP_TYPE tponk = tpon * k; 549 | if(xr != NULL && xi != NULL) 550 | for(int i = 0; i < n; i ++) { 551 | FP_TYPE re = cos_3(tponk * i); 552 | FP_TYPE im = sin_3(tponk * i); 553 | sumr += xr[i] * re - xi[i] * im; 554 | sumi += xr[i] * im + xi[i] * re; 555 | } 556 | else if(xr != NULL && xi == NULL) 557 | for(int i = 0; i < n; i ++) { 558 | FP_TYPE re = cos_3(tponk * i); 559 | FP_TYPE im = sin_3(tponk * i); 560 | sumr += xr[i] * re; 561 | sumi += xr[i] * im; 562 | } 563 | else if(xr == NULL && xi != NULL) 564 | for(int i = 0; i < n; i ++) { 565 | FP_TYPE re = cos_3(tponk * i); 566 | FP_TYPE im = sin_3(tponk * i); 567 | sumr -= xi[i] * im; 568 | sumi += xi[i] * re; 569 | } 570 | if(yr != NULL) yr[k] = sumr / n; 571 | if(yi != NULL) yi[k] = sumi / n; 572 | } 573 | } 574 | 575 | FP_TYPE* cig_levinson(FP_TYPE* R, int n) { 576 | FP_TYPE* a = calloc(n, sizeof(FP_TYPE)); 577 | FP_TYPE* a_new = calloc(n, sizeof(FP_TYPE)); 578 | FP_TYPE k = -R[1] / (R[0] * 1.000001); 579 | a[0] = 1.0; 580 | a[1] = k; 581 | FP_TYPE gain = R[0] * 1.000001 * (1 - k * k); 582 | for(int i = 2; i < n; i ++) { 583 | FP_TYPE s = R[i]; 584 | for(int j = 1; j < i; j ++) 585 | s += R[j] * a[i - j]; 586 | k = -s / gain; 587 | for(int j = 1; j < i; j ++) 588 | a_new[j] = a[j] + k * a[i - j]; 589 | for(int j = 1; j < i; j ++) 590 | a[j] = a_new[j]; 591 | a[i] = k; 592 | gain *= 1.0 - k * k; 593 | } 594 | free(a_new); 595 | return a; 596 | } 597 | 598 | FP_TYPE* cig_winfir(int order, FP_TYPE cutoff, FP_TYPE cutoff2, char* type, char* window) { 599 | FP_TYPE cutk = cutoff * order; 600 | FP_TYPE cutk2 = cutoff2 * order; 601 | FP_TYPE* freqrsp = calloc(order, sizeof(FP_TYPE)); 602 | FP_TYPE* timersp = calloc(order, sizeof(FP_TYPE)); 603 | 604 | FP_TYPE* w = get_window(window, order, 3); 605 | 606 | if(! strcmp(type, "lowpass")) { 607 | for(int i = 0; i <= floor(cutk); i ++) 608 | freqrsp[i] = 1; 609 | freqrsp[(int)ceil(cutk)] = fmod(cutk, 1.0); 610 | } else 611 | if(! strcmp(type, "highpass")) { 612 | for(int i = order / 2; i > floor(cutk); i --) 613 | freqrsp[i] = 1; 614 | freqrsp[(int)floor(cutk)] = 1.0 - fmod(cutk, 1.0); 615 | } else 616 | if(! strcmp(type, "bandpass")) { 617 | for(int i = floor(cutk2); i > floor(cutk); i --) 618 | freqrsp[i] = 1; 619 | freqrsp[(int)floor(cutk)] = 1.0 - fmod(cutk, 1.0); 620 | freqrsp[(int)ceil(cutk2)] = fmod(cutk2, 1.0); 621 | } else 622 | if(! strcmp(type, "bandstop")) { 623 | for(int i = 0; i < order; i ++) freqrsp[i] = 1; 624 | for(int i = floor(cutk2); i > floor(cutk); i --) 625 | freqrsp[i] = 0; 626 | freqrsp[(int)floor(cutk)] = fmod(cutk, 1.0); 627 | freqrsp[(int)ceil(cutk2)] = 1.0 - fmod(cutk2, 1.0); 628 | } 629 | 630 | complete_symm(freqrsp, order); 631 | idft(freqrsp, NULL, timersp, NULL, order); 632 | FP_TYPE* h = fftshift(timersp, order); 633 | for(int i = 0; i < order; i ++) 634 | h[i] *= w[i]; 635 | 636 | free(w); 637 | free(freqrsp); 638 | free(timersp); 639 | return h; 640 | } 641 | 642 | FP_TYPE* cig_convolution(FP_TYPE* x, FP_TYPE* h, int nx, int nh) { 643 | FP_TYPE* xpad = calloc(nx + nh * 2 - 1, sizeof(FP_TYPE)); 644 | FP_TYPE* y = calloc(nx + nh - 1, sizeof(FP_TYPE)); 645 | for(int i = 0; i < nx; i ++) 646 | xpad[i + nh - 1] = x[i]; 647 | for(int i = 0; i < nx + nh - 1; i ++) 648 | for(int k = 0; k < nh; k ++) 649 | y[i] += h[k] * xpad[i + nh - 1 - k]; 650 | free(xpad); 651 | return y; 652 | } 653 | 654 | // unrolled filters 655 | 656 | static FP_TYPE* cig_filter_order6(FP_TYPE* b, FP_TYPE* a, FP_TYPE* x, int nx) { 657 | FP_TYPE* y = calloc(nx + 5, sizeof(FP_TYPE)); 658 | for(int i = 5; i < nx; i ++) { 659 | y[i] -= a[1] * y[i - 1] + a[2] * y[i - 2] + a[3] * y[i - 3] + a[4] * y[i - 4] + a[5] * y[i - 5]; 660 | y[i] += b[0] * x[i - 0] + b[1] * x[i - 1] + b[2] * x[i - 2] + b[3] * x[i - 3] + b[4] * x[i - 4] + b[5] * x[i - 5]; 661 | } 662 | return y; 663 | } 664 | 665 | static FP_TYPE* cig_filter_order5(FP_TYPE* b, FP_TYPE* a, FP_TYPE* x, int nx) { 666 | FP_TYPE* y = calloc(nx + 4, sizeof(FP_TYPE)); 667 | for(int i = 4; i < nx; i ++) { 668 | y[i] -= a[1] * y[i - 1] + a[2] * y[i - 2] + a[3] * y[i - 3] + a[4] * y[i - 4]; 669 | y[i] += b[0] * x[i - 0] + b[1] * x[i - 1] + b[2] * x[i - 2] + b[3] * x[i - 3] + b[4] * x[i - 4]; 670 | } 671 | return y; 672 | } 673 | 674 | static FP_TYPE* cig_filter_order4(FP_TYPE* b, FP_TYPE* a, FP_TYPE* x, int nx) { 675 | FP_TYPE* y = calloc(nx + 3, sizeof(FP_TYPE)); 676 | for(int i = 3; i < nx; i ++) { 677 | y[i] -= a[1] * y[i - 1] + a[2] * y[i - 2] + a[3] * y[i - 3]; 678 | y[i] += b[0] * x[i - 0] + b[1] * x[i - 1] + b[2] * x[i - 2] + b[3] * x[i - 3]; 679 | } 680 | return y; 681 | } 682 | 683 | static FP_TYPE* cig_filter_order3(FP_TYPE* b, FP_TYPE* a, FP_TYPE* x, int nx) { 684 | FP_TYPE* y = calloc(nx + 2, sizeof(FP_TYPE)); 685 | for(int i = 2; i < nx; i ++) { 686 | y[i] -= a[1] * y[i - 1] + a[2] * y[i - 2]; 687 | y[i] += b[0] * x[i - 0] + b[1] * x[i - 1] + b[2] * x[i - 2]; 688 | } 689 | return y; 690 | } 691 | 692 | static FP_TYPE* cig_filter_order2(FP_TYPE* b, FP_TYPE* a, FP_TYPE* x, int nx) { 693 | FP_TYPE* y = calloc(nx + 1, sizeof(FP_TYPE)); 694 | for(int i = 1; i < nx; i ++) { 695 | y[i] -= a[1] * y[i - 1]; 696 | y[i] += b[0] * x[i - 0] + b[1] * x[i - 1]; 697 | } 698 | return y; 699 | } 700 | 701 | FP_TYPE* cig_filter(FP_TYPE* b, int nb, FP_TYPE* a, int na, FP_TYPE* x, int nx) { 702 | if(na == nb && na < 7) { 703 | if(na == 6) 704 | return cig_filter_order6(b, a, x, nx); 705 | if(na == 5) 706 | return cig_filter_order5(b, a, x, nx); 707 | if(na == 4) 708 | return cig_filter_order4(b, a, x, nx); 709 | if(na == 3) 710 | return cig_filter_order3(b, a, x, nx); 711 | if(na == 2) 712 | return cig_filter_order2(b, a, x, nx); 713 | } 714 | 715 | int nh = max(na, nb); 716 | FP_TYPE* y = calloc(nx + nh - 1, sizeof(FP_TYPE)); 717 | for(int i = 0; i < nh; i ++) { 718 | for(int k = 1; k < na; k ++) 719 | if(i - k >= 0) 720 | y[i] -= a[k] * y[i - k]; 721 | for(int k = 0; k < nb; k ++) 722 | if(i - k >= 0) 723 | y[i] += b[k] * x[i - k]; 724 | } 725 | for(int i = nh; i < nx; i ++) { 726 | for(int k = 1; k < na; k ++) 727 | y[i] -= a[k] * y[i - k]; 728 | for(int k = 0; k < nb; k ++) 729 | y[i] += b[k] * x[i - k]; 730 | } 731 | return y; 732 | } 733 | 734 | FP_TYPE* cig_kalmanf1d(FP_TYPE* z, FP_TYPE* Q, FP_TYPE* R, int nz, FP_TYPE x0, 735 | FP_TYPE* P, FP_TYPE* L_) { 736 | FP_TYPE* x = calloc(nz, sizeof(FP_TYPE)); 737 | FP_TYPE xpred = x0; 738 | FP_TYPE Ppred = Q[0]; 739 | FP_TYPE L = 0; 740 | for(int t = 0; t < nz; t ++) { 741 | if(t != 0) { 742 | Ppred = P[t - 1] + Q[t]; 743 | xpred = x[t - 1]; 744 | } 745 | 746 | FP_TYPE e_t = z[t] - xpred; 747 | FP_TYPE P_e_t = Ppred + R[t]; 748 | FP_TYPE K_t = Ppred / P_e_t; 749 | 750 | x[t] = xpred + K_t * e_t; 751 | P[t] = Ppred - K_t * Ppred; 752 | L += - 0.5 * log_2(2.0 * M_PI * P_e_t) - 0.5 * e_t * e_t / P_e_t; 753 | } 754 | if(L_ != NULL) *L_ = L; 755 | return x; 756 | } 757 | 758 | FP_TYPE* cig_kalmans1d(FP_TYPE* y, FP_TYPE* P, FP_TYPE* Q, int ny) { 759 | FP_TYPE* x = calloc(ny, sizeof(FP_TYPE)); 760 | FP_TYPE xbackward = y[ny - 1]; 761 | for(int t = ny - 1; t > 0; t --) { 762 | x[t] = xbackward; 763 | FP_TYPE J_t = P[t - 1] / (P[t - 1] + Q[t]); 764 | xbackward = y[t - 1] + J_t * (xbackward - y[t - 1]); 765 | } 766 | x[0] = xbackward; 767 | return x; 768 | } 769 | 770 | static FP_TYPE interp_kernel(FP_TYPE x, FP_TYPE a) { 771 | if(x == 0) return 1; 772 | if(x > a || x < -a) return 0; 773 | return a * sin_2(M_PI * x) * sin_2(M_PI * x / a) / M_PI / M_PI / x / x; 774 | } 775 | 776 | // xi should be ascending while the choice of x can be arbitrary 777 | FP_TYPE* cig_interp(FP_TYPE* xi, FP_TYPE* yi, int ni, FP_TYPE* x, int nx) { 778 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 779 | int srcidx = 0; 780 | for(int i = 0; i < nx; i ++) { 781 | FP_TYPE dstx = x[i]; 782 | while(srcidx + 1 < ni && xi[srcidx + 1] < dstx) srcidx ++; 783 | int i0 = srcidx == 0 ? 0 : srcidx; 784 | int i1 = srcidx == ni - 1 ? srcidx : srcidx + 1; 785 | if(i0 != i1 && dstx > xi[0]) 786 | y[i] = (yi[i1] - yi[i0]) * (dstx - xi[i0]) / (xi[i1] - xi[i0]) + yi[i0]; 787 | else 788 | y[i] = yi[i0]; 789 | } 790 | return y; 791 | } 792 | 793 | // interpolation on a uniformly sampled signal, assuming x is ascending 794 | FP_TYPE* cig_interpu(FP_TYPE xi0, FP_TYPE xi1, FP_TYPE* yi, int ni, FP_TYPE* x, int nx) { 795 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 796 | int begin = 0, end = nx - 1; 797 | while(x[begin] < xi0 && begin < nx) { 798 | y[begin] = yi[0]; 799 | begin ++; 800 | } 801 | for(end = nx - 1; end > begin; end --) { 802 | FP_TYPE srcidx = (x[end] - xi0) / (xi1 - xi0) * ni; 803 | if(srcidx + 1.01 < ni) break; 804 | y[end] = yi[ni - 1]; 805 | } 806 | for(int i = begin; i <= end; i ++) { 807 | FP_TYPE srcidx = (x[i] - xi0) / (xi1 - xi0) * ni; 808 | int base = srcidx; 809 | FP_TYPE r = srcidx - base; 810 | y[i] = yi[base] + (yi[base + 1] - yi[base]) * r; 811 | } 812 | return y; 813 | } 814 | 815 | FP_TYPE* cig_sincinterpu(FP_TYPE xi0, FP_TYPE xi1, FP_TYPE* yi, int ni, FP_TYPE* x, int nx) { 816 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 817 | for(int i = 0; i < nx; i ++) { 818 | FP_TYPE srcidx = (x[i] - xi0) / (xi1 - xi0) * ni; 819 | int ix_base = floor(srcidx); 820 | FP_TYPE a = srcidx - ix_base; 821 | for(int j = -3; j <= 4; j ++) 822 | if(ix_base + j > 0 && ix_base + j < ni) 823 | y[i] += interp_kernel(a - j, 4) * yi[ix_base + j]; 824 | } 825 | return y; 826 | } 827 | 828 | typedef FP_TYPE (*fp_n_to_one)(FP_TYPE* x); 829 | 830 | FP_TYPE* cig_medfilt(FP_TYPE* x, int nx, int order) { 831 | fp_n_to_one ffilt = NULL; 832 | if(order == 3) 833 | ffilt = opt_med3; 834 | else if(order == 5) 835 | ffilt = opt_med5; 836 | else if(order == 7) 837 | ffilt = opt_med7; 838 | else if(order == 9) 839 | ffilt = opt_med9; 840 | else if(order == 25) 841 | ffilt = opt_med25; 842 | 843 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 844 | FP_TYPE* tmp = malloc(order * sizeof(FP_TYPE)); 845 | int halford = order / 2; 846 | for(int i = 0; i < halford; i ++) 847 | y[i] = medianfp(x, i + 1); 848 | for(int i = 0; i < order; i ++) 849 | tmp[i] = x[i]; 850 | for(int i = halford; i < nx - halford; i ++) { 851 | for(int k = 0; k < order; k ++) 852 | tmp[k] = x[i + k - halford]; 853 | if(ffilt != NULL) 854 | y[i] = ffilt(tmp); 855 | else 856 | y[i] = medianfp(tmp, order); 857 | } 858 | for(int i = nx - halford; i < nx; i ++) 859 | y[i] = medianfp(x + i, nx - i); 860 | free(tmp); 861 | return y; 862 | } 863 | 864 | FP_TYPE* cig_moving_avg(FP_TYPE* x, int nx, FP_TYPE halford) { 865 | int ihalford = halford; 866 | FP_TYPE* acc = malloc((nx + ihalford * 2) * sizeof(FP_TYPE)); 867 | 868 | acc[0] = x[0]; 869 | for(int i = 1; i <= ihalford; i ++) 870 | acc[i] = acc[i - 1] + x[0]; 871 | for(int i = 1; i < nx; i ++) 872 | acc[i + ihalford] = acc[i + ihalford - 1] + x[i]; 873 | for(int i = 0; i < ihalford; i ++) 874 | acc[nx + ihalford + i] = acc[nx + ihalford + i - 1] + x[nx - 1]; 875 | 876 | FP_TYPE* interp_idx = malloc(nx * sizeof(FP_TYPE)); 877 | for(int i = 0; i < nx; i ++) interp_idx[i] = i + halford; 878 | FP_TYPE* interp_upper = interp1u(-halford, nx + ihalford, acc, nx + ihalford * 2, 879 | interp_idx, nx); 880 | for(int i = 0; i < nx; i ++) interp_idx[i] = i - halford; 881 | FP_TYPE* interp_lower = interp1u(-halford, nx + ihalford, acc, nx + ihalford * 2, 882 | interp_idx, nx); 883 | 884 | for(int i = 0; i < nx; i ++) { 885 | interp_upper[i] = (interp_upper[i] - interp_lower[i]) / halford * 0.5; 886 | } 887 | 888 | free(acc); 889 | free(interp_idx); 890 | free(interp_lower); 891 | 892 | return interp_upper; 893 | } 894 | 895 | FP_TYPE* cig_rresample(FP_TYPE* x, int nx, FP_TYPE ratio, int* ny) { 896 | *ny = ratio == 1.0 ? nx : round(nx * ratio); 897 | FP_TYPE* xlp = x; 898 | FP_TYPE* y = calloc(*ny, sizeof(FP_TYPE)); 899 | if(ratio == 1.0) { 900 | memcpy(y, x, sizeof(FP_TYPE) * nx); 901 | return y; 902 | } 903 | 904 | if(ratio < 1.0) { // low pass the signal first 905 | FP_TYPE* h = fir1(32, ratio, "lowpass", "hamming"); 906 | xlp = conv(x, h, nx, 32) + 16; 907 | free(h); 908 | } 909 | 910 | for(int i = 0; i < *ny; i ++) { 911 | FP_TYPE ix = (FP_TYPE)i / ratio; 912 | int ix_base = floor(ix); 913 | FP_TYPE a = ix - ix_base; 914 | for(int j = -3; j <= 4; j ++) 915 | if(ix_base + j > 0 && ix_base + j < nx) 916 | y[i] += interp_kernel(a - j, 4) * xlp[ix_base + j]; 917 | } 918 | 919 | if(ratio < 1.0) { 920 | xlp -= 16; 921 | free(xlp); 922 | } 923 | 924 | return y; 925 | } 926 | 927 | ifdetector* cig_create_ifdetector(FP_TYPE fc, FP_TYPE fres) { 928 | const FP_TYPE a[4] = {0.338946, 0.481973, 0.161054, 0.018027}; 929 | 930 | ifdetector* ret = malloc(sizeof(ifdetector)); 931 | int nh = 4 / fres; 932 | ret -> fc = fc; 933 | ret -> nh = nh; 934 | ret -> hr = calloc(ret -> nh, sizeof(FP_TYPE)); 935 | ret -> hi = calloc(ret -> nh, sizeof(FP_TYPE)); 936 | ret -> hdr = calloc(ret -> nh, sizeof(FP_TYPE)); 937 | ret -> hdi = calloc(ret -> nh, sizeof(FP_TYPE)); 938 | FP_TYPE omega = 2.0 * M_PI * fc; 939 | FP_TYPE omegaw = 2.0 * M_PI / nh; 940 | 941 | for(int i = 0; i < ret -> nh; i ++) 942 | for(int k = 0; k < 4; k ++) { 943 | ret -> hr[i] += a[k] * cos_3(k * omegaw * (i - nh / 2)); 944 | ret -> hdr[i] += -omegaw * k * a[k] * sin_3(k * omegaw * (i - nh / 2)); 945 | } 946 | 947 | for(int i = 0; i < ret -> nh; i ++) { 948 | FP_TYPE sini = sin_3(omega * (i - nh / 2)); 949 | FP_TYPE cosi = cos_3(omega * (i - nh / 2)); 950 | FP_TYPE w = ret -> hr[i]; 951 | FP_TYPE wd = ret -> hdr[i]; 952 | ret -> hr[i] = w * cosi; 953 | ret -> hi[i] = w * sini; 954 | ret -> hdi[i] = omega * w * cosi + wd * sini; 955 | ret -> hdr[i] = wd * cosi - omega * w * sini; 956 | } 957 | 958 | return ret; 959 | } 960 | 961 | void cig_delete_ifdetector(ifdetector* dst) { 962 | if(dst == NULL) return; 963 | free(dst -> hr); free(dst -> hi); 964 | free(dst -> hdr); free(dst -> hdi); 965 | free(dst); 966 | } 967 | 968 | FP_TYPE cig_ifdetector_estimate(ifdetector* ifd, FP_TYPE* x, int nx) { 969 | if(nx < ifd -> nh) return 0; 970 | int nh = ifd -> nh; 971 | int n0 = nx / 2 - nh / 2; 972 | 973 | // single-point convolution 974 | FP_TYPE yr = 0; FP_TYPE yi = 0; 975 | FP_TYPE ydr = 0; FP_TYPE ydi = 0; 976 | for(int i = 0; i < nh; i ++) { 977 | yr += ifd -> hr[i] * x[n0 + i]; 978 | yi += ifd -> hi[i] * x[n0 + i]; 979 | ydr += ifd -> hdr[i] * x[n0 + i]; 980 | ydi += ifd -> hdi[i] * x[n0 + i]; 981 | } 982 | 983 | // apply Flanagan's equation 984 | return (yr * ydi - yi * ydr) / (yr * yr + yi * yi) / 2.0 / M_PI; 985 | } 986 | 987 | // Pearson Correlation Coefficient 988 | static FP_TYPE corr_kernel_acf( 989 | FP_TYPE* x, FP_TYPE* xx, double* x1, double* x2, int w, int d) { 990 | FP_TYPE a = 0; 991 | FP_TYPE b = 0; 992 | FP_TYPE sumx = x1[w] - x1[0]; 993 | FP_TYPE sumxd = x1[w + d] - x1[d]; 994 | FP_TYPE sumx2 = x2[w] - x2[0]; 995 | FP_TYPE sumx2d = x2[w + d] - x2[d]; 996 | for(int i = 0; i < w; i ++) 997 | a += x[i] * x[i + d]; 998 | a *= w; 999 | a -= sumx * sumxd; 1000 | b = (w * sumx2 - sumx * sumx) * (w * sumx2d - sumxd * sumxd); 1001 | FP_TYPE r = a / sqrt(b); 1002 | r = max(r, 0); r = min(r, 1); 1003 | return r; 1004 | } 1005 | 1006 | static FP_TYPE corr_kernel_amdf( 1007 | FP_TYPE* x, FP_TYPE* xx, double* x1, double* x2, int w, int d) { 1008 | FP_TYPE a = 0; 1009 | for(int i = 0; i < w; i ++) 1010 | a += fabs(x[i] - x[i + d]); 1011 | return a; 1012 | } 1013 | 1014 | static FP_TYPE corr_kernel_sqrdiff( 1015 | FP_TYPE* x, FP_TYPE* xx, double* x1, double* x2, int w, int d) { 1016 | FP_TYPE a = 0; 1017 | for(int i = 0; i < w; i ++) 1018 | a += (x[i] - x[i + d]) * (x[i] - x[i + d]); 1019 | return a; 1020 | } 1021 | 1022 | void cig_correlogram(FP_TYPE* x, int nx, int* center, int* nwin, int nfrm, 1023 | int max_period, int method, FP_TYPE** R) { 1024 | double* x1 = calloc(nx + 1, sizeof(double)); // integrate x[i] 1025 | double* x2 = calloc(nx + 1, sizeof(double)); // integrate x^2[i] 1026 | FP_TYPE* xx = calloc(nx, sizeof(FP_TYPE)); // x^2[i] 1027 | for(int i = 0; i < nx; i ++) 1028 | x1[i + 1] = x1[i] + (double)x[i]; 1029 | for(int i = 0; i < nx; i ++) 1030 | x2[i + 1] = x2[i] + (double)x[i] * (double)x[i]; 1031 | for(int i = 0; i < nx; i ++) 1032 | xx[i] = x[i] * x[i]; 1033 | for(int i = 0; i < nfrm; i ++) { 1034 | int w = nwin[i]; 1035 | int t = center[i] - w / 2; 1036 | t = max(0, t); 1037 | t = min(nx - 1, t); 1038 | if(method == CIG_CORR_YIN) { 1039 | R[i][0] = 1.0; 1040 | FP_TYPE cumm = 0; 1041 | for(int d = 1; d < max_period; d ++) { 1042 | if(t + w + d < nx) { 1043 | FP_TYPE ac = corr_kernel_sqrdiff(x + t, xx + t, x1 + t, x2 + t, w, d); 1044 | ac = max(ac, 1e-10); 1045 | cumm += ac; 1046 | R[i][d] = ac * d / cumm; 1047 | } else 1048 | R[i][d] = 1.0; 1049 | } 1050 | } else 1051 | for(int d = 0; d < max_period; d ++) { 1052 | if(t + w + d < nx) { 1053 | if(method == CIG_CORR_ACF) 1054 | R[i][d] = corr_kernel_acf(x + t, xx + t, x1 + t, x2 + t, w, d); 1055 | else if(method == CIG_CORR_AMDF) 1056 | R[i][d] = corr_kernel_amdf(x + t, xx + t, x1 + t, x2 + t, w, d); 1057 | else if(method == CIG_CORR_SQRDIFF) 1058 | R[i][d] = corr_kernel_sqrdiff(x + t, xx + t, x1 + t, x2 + t, w, d); 1059 | } else 1060 | R[i][d] = 0; 1061 | } 1062 | } 1063 | free(x1); 1064 | free(x2); 1065 | free(xx); 1066 | } 1067 | 1068 | FP_TYPE** cig_invcrgm(FP_TYPE** R, int nfrm, int max_period, int fs, FP_TYPE* faxis, int nf) { 1069 | FP_TYPE** Ri = malloc2d(nfrm, nf, sizeof(FP_TYPE*)); 1070 | FP_TYPE* Raxis = linspace(0, max_period - 1, max_period); 1071 | FP_TYPE* invmap = calloc(nf, sizeof(FP_TYPE)); 1072 | for(int i = 0; i < nf; i ++) 1073 | invmap[i] = fs / faxis[nf - i - 1]; 1074 | for(int i = 0; i < nfrm; i ++) { 1075 | FP_TYPE* rflip = interp1(Raxis, R[i], max_period, invmap, nf); 1076 | for(int j = 0; j < nf; j ++) 1077 | Ri[i][j] = rflip[nf - j - 1]; 1078 | free(rflip); 1079 | } 1080 | free(invmap); 1081 | free(Raxis); 1082 | return Ri; 1083 | } 1084 | 1085 | void cig_stft_forward(FP_TYPE* x, int nx, int* center, int* nwin, int nfrm, 1086 | int nfft, char* window, int subt_mean, int optlv, 1087 | FP_TYPE* norm_factor, FP_TYPE* weight_factor, FP_TYPE** Xmagn, FP_TYPE** Xphse) { 1088 | 1089 | # ifndef _OPENMP 1090 | FP_TYPE* buff = calloc(nfft * 5, sizeof(FP_TYPE)); 1091 | FP_TYPE* fftbuff = buff; 1092 | FP_TYPE* xbuff = buff + nfft * 2; 1093 | FP_TYPE* ybuffr = buff + nfft * 3; 1094 | FP_TYPE* ybuffi = buff + nfft * 4; 1095 | # endif 1096 | 1097 | FP_TYPE* w = NULL; 1098 | if(norm_factor != NULL || weight_factor != NULL) 1099 | w = get_window(window, nwin[0], optlv); 1100 | 1101 | if(weight_factor != NULL) { 1102 | *weight_factor = 0; 1103 | for(int i = 0; i < nwin[0]; i ++) 1104 | *weight_factor += w[i]; 1105 | } 1106 | if(norm_factor != NULL) { 1107 | *norm_factor = 0; 1108 | for(int i = 0; i < nwin[0]; i += center[1] - center[0]) 1109 | *norm_factor += w[i]; 1110 | } 1111 | 1112 | # ifdef _OPENMP 1113 | # pragma omp parallel for 1114 | # endif 1115 | for(int t = 0; t < nfrm; t ++) { 1116 | # ifdef _OPENMP 1117 | FP_TYPE* buff = calloc(nfft * 5, sizeof(FP_TYPE)); 1118 | FP_TYPE* fftbuff = buff; 1119 | FP_TYPE* xbuff = buff + nfft * 2; 1120 | FP_TYPE* ybuffr = buff + nfft * 3; 1121 | FP_TYPE* ybuffi = buff + nfft * 4; 1122 | # endif 1123 | int tn = center[t]; 1124 | 1125 | FP_TYPE* xfrm; 1126 | FP_TYPE* spec_magn, *spec_phse; 1127 | xfrm = spec_magn = spec_phse = NULL; 1128 | 1129 | FP_TYPE* wlocal = w; 1130 | if(norm_factor == NULL && weight_factor == NULL) { 1131 | wlocal = get_window(window, nwin[t], optlv); 1132 | } 1133 | 1134 | xfrm = fetch_frame(x, nx, tn, nwin[t]); 1135 | if(subt_mean) { 1136 | FP_TYPE mean_xfrm = sumfp(xfrm, nwin[t]) / nwin[t]; 1137 | for(int i = 0; i < nwin[t]; i ++) 1138 | xfrm[i] = (xfrm[i] - mean_xfrm) * wlocal[i]; 1139 | } else { 1140 | for(int i = 0; i < nwin[t]; i ++) 1141 | xfrm[i] *= wlocal[i]; 1142 | } 1143 | 1144 | memset(xbuff, 0, nfft * sizeof(FP_TYPE)); 1145 | for(int i = 0; i < nwin[t] / 2; i ++) { 1146 | xbuff[i] = xfrm[i + nwin[t] / 2]; 1147 | xbuff[nfft - nwin[t] / 2 + i] = xfrm[i]; 1148 | } 1149 | fft(xbuff, NULL, ybuffr, ybuffi, nfft, fftbuff); 1150 | 1151 | if(Xmagn != NULL) { 1152 | spec_magn = abscplx(ybuffr, ybuffi, nfft / 2 + 1); 1153 | for(int i = 0; i < nfft / 2 + 1; i ++) 1154 | Xmagn[t][i] = spec_magn[i]; 1155 | free(spec_magn); 1156 | } 1157 | 1158 | if(Xphse != NULL) { 1159 | spec_phse = argcplx(ybuffr, ybuffi, nfft / 2 + 1); 1160 | for(int i = 0; i < nfft / 2 + 1; i ++) 1161 | Xphse[t][i] = spec_phse[i]; 1162 | free(spec_phse); 1163 | } 1164 | 1165 | free(xfrm); 1166 | if(norm_factor == NULL && weight_factor == NULL) 1167 | free(wlocal); 1168 | 1169 | # ifdef _OPENMP 1170 | free(buff); 1171 | # endif 1172 | } 1173 | 1174 | if(norm_factor != NULL || weight_factor != NULL) 1175 | free(w); 1176 | # ifndef _OPENMP 1177 | free(buff); 1178 | # endif 1179 | } 1180 | 1181 | FP_TYPE* cig_stft_backward(FP_TYPE** Xmagn, FP_TYPE** Xphse, int nhop, int nfrm, 1182 | int offset, int hop_factor, int zp_factor, int nfade, FP_TYPE norm_factor, int* ny) { 1183 | int nwin = nhop * hop_factor; 1184 | int nfft = nwin * zp_factor; 1185 | FP_TYPE* wfade = hanning(nfade * 2); 1186 | 1187 | *ny = nhop * nfrm + offset; 1188 | FP_TYPE* y = calloc(*ny, sizeof(FP_TYPE)); 1189 | 1190 | # ifndef _OPENMP 1191 | FP_TYPE* buff = calloc(nfft * 5, sizeof(FP_TYPE)); 1192 | FP_TYPE* fftbuff = buff; 1193 | FP_TYPE* ybuff = buff + nfft * 2; 1194 | FP_TYPE* xbuffr = buff + nfft * 3; 1195 | FP_TYPE* xbuffi = buff + nfft * 4; 1196 | # endif 1197 | 1198 | # ifdef _OPENMP 1199 | # pragma omp parallel for 1200 | # endif 1201 | for(int t = 0; t < nfrm; t ++) { 1202 | # ifdef _OPENMP 1203 | FP_TYPE* buff = calloc(nfft * 5, sizeof(FP_TYPE)); 1204 | FP_TYPE* fftbuff = buff; 1205 | FP_TYPE* ybuff = buff + nfft * 2; 1206 | FP_TYPE* xbuffr = buff + nfft * 3; 1207 | FP_TYPE* xbuffi = buff + nfft * 4; 1208 | # endif 1209 | 1210 | int tn = t * nhop + offset; 1211 | 1212 | for(int i = 0; i < nfft / 2 + 1; i ++) { 1213 | xbuffr[i] = Xmagn[t][i] * cos_2(Xphse[t][i]); 1214 | xbuffi[i] = Xmagn[t][i] * sin_2(Xphse[t][i]); 1215 | } 1216 | complete_symm (xbuffr, nfft); 1217 | complete_asymm(xbuffi, nfft); 1218 | ifft(xbuffr, xbuffi, ybuff, NULL, nfft, fftbuff); 1219 | 1220 | FP_TYPE* yfrm = fftshift(ybuff, nfft); 1221 | for(int i = 0; i < nfade; i ++) { 1222 | yfrm[i] *= wfade[i]; 1223 | yfrm[nfft - i - 1] *= wfade[i]; 1224 | } 1225 | 1226 | # ifdef _OPENMP 1227 | # pragma omp critical 1228 | # endif 1229 | for(int i = 0; i < nfft; i ++) { 1230 | int idx = tn + i - nfft / 2; 1231 | if(idx >= 0 && idx < *ny) { 1232 | y[idx] += yfrm[i] / norm_factor; 1233 | } 1234 | } 1235 | 1236 | free(yfrm); 1237 | # ifdef _OPENMP 1238 | free(buff); 1239 | # endif 1240 | } 1241 | 1242 | free(wfade); 1243 | # ifndef _OPENMP 1244 | free(buff); 1245 | # endif 1246 | return y; 1247 | } 1248 | 1249 | FP_TYPE cig_qifft(FP_TYPE* magn, int k, FP_TYPE* dst_freq) { 1250 | FP_TYPE a, b, c, a1, a2, x; 1251 | a = magn[k - 1]; 1252 | b = magn[k + 0]; 1253 | c = magn[k + 1]; 1254 | a1 = (a + c) / 2.0 - b; 1255 | a2 = c - b - a1; 1256 | x = - a2 / a1 * 0.5; 1257 | 1258 | x = (fabs(x) < 1.0) ? x : 0; // in case we get some x outside of [k-1, k+1] 1259 | 1260 | *dst_freq = (FP_TYPE)k + x; 1261 | FP_TYPE ret = a1 * x * x + a2 * x + b; 1262 | return ret > b + 0.2 ? b + 0.2 : ret; // in case it pops 1263 | } 1264 | 1265 | static inline FP_TYPE barkmask(FP_TYPE z) { 1266 | if(z < -1.3 || z > 2.5) return 0; 1267 | if(z < -0.5) return fastpow(10.0, 2.5 * (z + 0.5)); 1268 | if(z <= 0.5) return 1.0; 1269 | return fastpow(10.0, (0.5 - z)); 1270 | } 1271 | 1272 | filterbank* cig_create_empty_filterbank(int nf, FP_TYPE fnyq, int nchannel) { 1273 | filterbank* ret = malloc(sizeof(filterbank)); 1274 | ret -> nchannel = nchannel; 1275 | ret -> nf = nf; 1276 | ret -> fnyq = fnyq; 1277 | ret -> fresp = calloc(nchannel, sizeof(FP_TYPE*)); 1278 | ret -> lower_idx = calloc(nchannel, sizeof(int)); 1279 | ret -> upper_idx = calloc(nchannel, sizeof(int)); 1280 | for(int i = 0; i < nchannel; i ++) { 1281 | ret -> fresp[i] = calloc(nf, sizeof(FP_TYPE)); 1282 | ret -> upper_idx[i] = nf; 1283 | } 1284 | return ret; 1285 | } 1286 | 1287 | filterbank* cig_create_plp_filterbank(int nf, FP_TYPE fnyq, int nchannel) { 1288 | filterbank* ret = cig_create_empty_filterbank(nf, fnyq, nchannel); 1289 | for(int i = 0; i < nchannel; i ++) { 1290 | FP_TYPE weight = eqloud(bark2freq(i)); 1291 | for(int k = 0; k < nf; k ++) 1292 | ret -> fresp[i][k] = barkmask(i - freq2bark((FP_TYPE)k / nf * fnyq)) * weight; 1293 | } 1294 | return ret; 1295 | } 1296 | 1297 | filterbank* cig_create_melfreq_filterbank(int nf, FP_TYPE fnyq, int nchannel, 1298 | FP_TYPE min_freq, FP_TYPE max_freq, FP_TYPE scale, FP_TYPE min_width) { 1299 | filterbank* ret = cig_create_empty_filterbank(nf, fnyq, nchannel); 1300 | 1301 | FP_TYPE* freqs = melspace(min_freq, max_freq, nchannel); 1302 | for(int j = 0; j < nchannel; j ++) { 1303 | FP_TYPE f_0 = j == 0 ? 0 : freqs[j - 1]; 1304 | FP_TYPE f_1 = freqs[j]; 1305 | FP_TYPE f_2 = freqs[j + 1]; 1306 | if(f_0 > f_1 - min_width) 1307 | f_0 = max(0, f_1 - min_width); 1308 | if(f_2 < f_1 + min_width) 1309 | f_2 = f_1 + min_width; 1310 | int lower_idx = j == 0 ? 0 : floor(f_0 * nf / fnyq * scale); 1311 | int upper_idx = ceil(f_2 * nf / fnyq * scale); 1312 | int centr_idx = round(f_1 * nf / fnyq * scale); 1313 | upper_idx = min(upper_idx, nf); 1314 | centr_idx = min(centr_idx, upper_idx - 1); 1315 | lower_idx = min(lower_idx, centr_idx - 1); 1316 | 1317 | ret -> lower_idx[j] = lower_idx; 1318 | ret -> upper_idx[j] = upper_idx; 1319 | for(int k = lower_idx; k < centr_idx; k ++) 1320 | ret -> fresp[j][k] = (FP_TYPE)(k - lower_idx + 1) / (centr_idx - lower_idx); 1321 | for(int k = centr_idx; k < upper_idx; k ++) 1322 | ret -> fresp[j][k] = (1.0 - (FP_TYPE)(k - centr_idx) / (upper_idx - centr_idx)); 1323 | } 1324 | free(freqs); 1325 | return ret; 1326 | } 1327 | 1328 | void cig_delete_filterbank(filterbank* dst) { 1329 | if(dst == NULL) return; 1330 | for(int i = 0; i < dst -> nchannel; i ++) 1331 | free(dst -> fresp[i]); 1332 | free(dst -> lower_idx); 1333 | free(dst -> upper_idx); 1334 | free(dst -> fresp); 1335 | free(dst); 1336 | } 1337 | 1338 | FP_TYPE** cig_filterbank_spectrogram(filterbank* fbank, FP_TYPE** S, int nfrm, 1339 | int nfft, int fs, int crtenergy) { 1340 | FP_TYPE** X = malloc2d(nfrm, fbank -> nchannel, sizeof(FP_TYPE)); 1341 | # ifdef _OPENMP 1342 | # pragma omp parallel for 1343 | # endif 1344 | for(int i = 0; i < nfrm; i ++) { 1345 | for(int j = 0; j < fbank -> nchannel; j ++) { 1346 | X[i][j] = 0; 1347 | for(int k = fbank -> lower_idx[j]; k < fbank -> upper_idx[j]; k ++) 1348 | X[i][j] += fbank -> fresp[j][k] * S[i][k]; 1349 | X[i][j] = crtenergy ? pow(X[i][j], 0.33) : log_2(X[i][j] + M_EPS); 1350 | } 1351 | } 1352 | return X; 1353 | } 1354 | 1355 | FP_TYPE* cig_filterbank_spectrum(filterbank* fbank, FP_TYPE* S, int nfft, int fs, 1356 | int crtenergy) { 1357 | FP_TYPE* X = calloc(fbank -> nchannel, sizeof(FP_TYPE)); 1358 | for(int j = 0; j < fbank -> nchannel; j ++) { 1359 | for(int k = fbank -> lower_idx[j]; k < fbank -> upper_idx[j]; k ++) 1360 | X[j] += fbank -> fresp[j][k] * S[k]; 1361 | X[j] = crtenergy ? pow(X[j], 0.33) : log_2(X[j] + M_EPS); 1362 | } 1363 | return X; 1364 | } 1365 | 1366 | // Morise, Masanori. "Cheaptrick, a spectral envelope estimator for high-quality 1367 | // speech synthesis." Speech Communication 67 (2015): 1-7. 1368 | FP_TYPE* cig_spec2env(FP_TYPE* S, int nfft, FP_TYPE f0, int nhar, FP_TYPE* Cout) { 1369 | FP_TYPE* buff = malloc(nfft * 4 * sizeof(FP_TYPE)); 1370 | FP_TYPE* V = buff; 1371 | FP_TYPE* C = buff + nfft; 1372 | FP_TYPE* fftbuff = buff + nfft * 2; 1373 | FP_TYPE smoothord = (FP_TYPE)nfft / (3.0 / f0); 1374 | 1375 | for(int i = 0; i < nfft / 2 + 1; i ++) V[i] = S[i]; 1376 | int kf0 = ceil(f0 * nfft); 1377 | for(int i = 0; i < kf0 / 2; i ++) V[i] = V[kf0 - i]; 1378 | FP_TYPE* smoothed = moving_avg(V, nfft / 2 + 1, smoothord); 1379 | int top = floor(f0 * (nhar - 2) * nfft); 1380 | for(int i = top; i < nfft / 2 + 1; i ++) 1381 | smoothed[i] = smoothed[i - 1]; 1382 | 1383 | for(int i = 0; i < nfft / 2 + 1; i ++) 1384 | V[i] = log_2(smoothed[i] + M_EPS); 1385 | complete_symm(V, nfft); 1386 | ifft(V, NULL, C, NULL, nfft, fftbuff); 1387 | for(int i = 1; i < nfft / 2 + 1; i ++) 1388 | C[i] *= sin_2(i * f0 * M_PI) / (i * f0 * M_PI) * 1389 | (1.18 - 2.0 * 0.09 * cos_2(2.0 * M_PI * i * f0)); 1390 | complete_symm(C, nfft); 1391 | fft(C, NULL, V, NULL, nfft, fftbuff); 1392 | free(smoothed); 1393 | if(Cout != NULL) 1394 | for(int i = 0; i < nfft / 2 + 1; i ++) 1395 | Cout[i] = C[i]; 1396 | return realloc(V, nfft * sizeof(FP_TYPE)); 1397 | } 1398 | 1399 | // Huber, Stefan, and Axel Roebel. "On the use of voice descriptors for glottal 1400 | // source shape parameter estimation." Computer Speech & Language 28.5 (2014): 1401 | // 1170-1194. 1402 | // Fant, Gunnar. "The LF-model revisited. Transformations and frequency domain 1403 | // analysis." Speech Trans. Lab. Q. Rep., Royal Inst. of Tech. Stockholm 2.3 1404 | // (1995): 40. 1405 | lfmodel cig_lfmodel_from_rd(FP_TYPE rd, FP_TYPE T0, FP_TYPE Ee) { 1406 | lfmodel ret; 1407 | FP_TYPE Rap = rd < 0.21 ? 1e-6 : (rd < 2.7 ? (-1.0 + 4.8 * rd) / 100.0 : 0.323 / rd); 1408 | FP_TYPE OQupp = 1.0 - 1.0 / (2.17 * rd); 1409 | FP_TYPE Rkp, Rgp; 1410 | if(rd < 2.7) { 1411 | Rkp = (22.4 + 11.8 * rd) / 100.0; 1412 | Rgp = 0.25 * Rkp / ((0.11 * rd) / (0.5 + 1.2 * Rkp) - Rap); 1413 | } else { 1414 | Rgp = 9.3552e-3 + 596e-2 / (7.96 - 2.0 * OQupp); 1415 | Rkp = 2.0 * Rgp * OQupp - 1.0428; 1416 | } 1417 | ret.tp = 1.0 / (2.0 * Rgp); 1418 | ret.te = ret.tp * (Rkp + 1.0); 1419 | ret.ta = Rap; 1420 | ret.T0 = T0; 1421 | ret.Ee = Ee; 1422 | return ret; 1423 | }; 1424 | 1425 | typedef struct { 1426 | FP_TYPE T0; 1427 | FP_TYPE Te; 1428 | FP_TYPE Tp; 1429 | FP_TYPE Ta; 1430 | FP_TYPE wg; 1431 | FP_TYPE sin_wgTe; 1432 | FP_TYPE cos_wgTe; 1433 | FP_TYPE e; 1434 | FP_TYPE A; 1435 | FP_TYPE a; 1436 | FP_TYPE E0; 1437 | FP_TYPE scale; 1438 | } lfparam; 1439 | 1440 | static FP_TYPE efunc(FP_TYPE x, void* env) { 1441 | lfparam* tmpparam = (lfparam*)env; 1442 | return 1.0 - exp_3((tmpparam -> Te - tmpparam -> T0) * x) - tmpparam -> Ta * x; 1443 | } 1444 | 1445 | static FP_TYPE afunc(FP_TYPE x, lfparam* tmpparam) { 1446 | FP_TYPE C = tmpparam -> wg * tmpparam -> wg * 1447 | tmpparam -> sin_wgTe * tmpparam -> A - 1448 | tmpparam -> wg * tmpparam -> cos_wgTe; 1449 | FP_TYPE f = tmpparam -> sin_wgTe * tmpparam -> A * x * x + 1450 | tmpparam -> sin_wgTe * x + 1451 | tmpparam -> wg * exp_3(- x * tmpparam -> Te) + C; 1452 | return f; 1453 | } 1454 | 1455 | static FP_TYPE aderiv(FP_TYPE x, lfparam* tmpparam) { 1456 | FP_TYPE d = 2 * tmpparam -> sin_wgTe * tmpparam -> A * x + 1457 | tmpparam -> sin_wgTe - 1458 | tmpparam -> wg * tmpparam -> Te * exp_3(- x * tmpparam -> Te); 1459 | return d; 1460 | } 1461 | 1462 | static FP_TYPE newton_search(lfparam paramset) { 1463 | FP_TYPE a = 0; 1464 | for(int i = 0; i < 8; i ++) 1465 | a -= afunc(a, & paramset) / aderiv(a, & paramset); 1466 | return a; 1467 | } 1468 | 1469 | // Fant, Gunnar, Johan Liljencrants, and Qi-guang Lin. "A four-parameter model 1470 | // of glottal flow." STL-QPSR 4.1985 (1985): 1-13. 1471 | static lfparam lfparam_from_lfmodel(lfmodel model) { 1472 | FP_TYPE scale = 1.0; 1473 | const FP_TYPE max_hz = 800.0f; 1474 | if(model.T0 < 1.0 / max_hz) { 1475 | scale = 1.0 / model.T0 / max_hz; 1476 | model.T0 = 1.0 / max_hz; 1477 | } 1478 | lfparam ret = { 1479 | .T0 = model.T0, 1480 | .Te = model.T0 * model.te, 1481 | .Tp = model.T0 * model.tp, 1482 | .Ta = model.T0 * model.ta, 1483 | .a = 0, 1484 | .scale = scale 1485 | }; 1486 | ret.wg = M_PI / ret.Tp; 1487 | ret.sin_wgTe = sin_3(ret.wg * ret.Te); 1488 | ret.cos_wgTe = cos_3(ret.wg * ret.Te); 1489 | FP_TYPE e = fzero(efunc, 1.0, 2.0 / (ret.Ta + 1e-9), & ret); 1490 | FP_TYPE e_Te_T0 = exp_3(e * (ret.Te - ret.T0)); 1491 | ret.A = (1.0 - e_Te_T0) / (e * e * ret.Ta) + 1492 | (ret.Te - ret.T0) * e_Te_T0 / (e * ret.Ta); 1493 | ret.e = e; 1494 | ret.a = newton_search(ret); 1495 | ret.E0 = -model.Ee / (exp_3(ret.a * ret.Te) * ret.sin_wgTe); 1496 | return ret; 1497 | } 1498 | 1499 | // B. Doval, C. d'Alessandro, "Spectral Correlates of Glottal Waveform Models: 1500 | // an Analytic Study." ICASSP, Munich, 1997. 1501 | FP_TYPE* cig_lfmodel_spectrum(lfmodel model, FP_TYPE* freq, int nf, FP_TYPE* dst_phase) { 1502 | lfparam tmpparam = lfparam_from_lfmodel(model); 1503 | FP_TYPE e = tmpparam.e; 1504 | FP_TYPE a = tmpparam.a; 1505 | FP_TYPE wg = tmpparam.wg; 1506 | FP_TYPE E0 = tmpparam.E0; 1507 | FP_TYPE sin_wgTe = tmpparam.sin_wgTe; 1508 | FP_TYPE cos_wgTe = tmpparam.cos_wgTe; 1509 | FP_TYPE Te = tmpparam.Te; 1510 | FP_TYPE Ta = tmpparam.Ta; 1511 | FP_TYPE T0 = tmpparam.T0; 1512 | FP_TYPE e1eTa = e * (1.0 - e * Ta); 1513 | 1514 | FP_TYPE* dst_magn = calloc(nf, sizeof(FP_TYPE)); 1515 | for(int i = 0; i < nf; i ++) { 1516 | FP_TYPE Omega = 2.0 * M_PI * freq[i] / tmpparam.scale; 1517 | cplx asubipif = c_cplx(a, - Omega); 1518 | cplx P1 = c_div(c_cplx(E0, 0), c_add(c_mul(asubipif, asubipif), c_cplx(wg * wg, 0))); 1519 | cplx P2 = c_add(c_cplx(wg, 0), 1520 | c_mul(c_exp(c_mul(asubipif, c_cplx(Te, 0))), 1521 | c_sub(c_mul(asubipif, c_cplx(sin_wgTe, 0)), c_cplx(wg * cos_wgTe, 0)) 1522 | )); 1523 | cplx P3 = c_mul(c_cplx(model.Ee, 0), c_div( 1524 | c_exp(c_cplx(0, - Omega * Te)), 1525 | c_mul(c_cplx(0, e * Ta * Omega), c_cplx(e, Omega)) 1526 | )); 1527 | cplx P4; 1528 | if(e1eTa < 1.0) { // use approximated version for numerical stability 1529 | P4 = c_cplx(0, -e * Ta * Omega); 1530 | } else { 1531 | P4 = c_sub( 1532 | c_mul(c_cplx(e1eTa, 0), 1533 | c_sub(c_cplx(1.0, 0), c_exp(c_cplx(0, -Omega * (T0 - Te))))), 1534 | c_cplx(0, e * Ta * Omega)); 1535 | } 1536 | cplx G = c_add(c_mul(P1, P2), c_mul(P3, P4)); 1537 | dst_magn[i] = c_abs(G); 1538 | if(isnan(dst_magn[i])) { 1539 | dst_magn[i] = 0; 1540 | G = c_cplx(0, 0); 1541 | } 1542 | if(dst_phase) dst_phase[i] = c_arg(G); 1543 | } 1544 | return dst_magn; 1545 | } 1546 | 1547 | FP_TYPE* cig_lfmodel_period(lfmodel model, int fs, int n) { 1548 | lfparam tmpparam = lfparam_from_lfmodel(model); 1549 | FP_TYPE e = tmpparam.e; 1550 | FP_TYPE a = tmpparam.a; 1551 | FP_TYPE wg = tmpparam.wg; 1552 | FP_TYPE E0 = tmpparam.E0; 1553 | FP_TYPE Te = tmpparam.Te; 1554 | FP_TYPE Ta = tmpparam.Ta; 1555 | FP_TYPE T0 = tmpparam.T0; 1556 | 1557 | int i; 1558 | int ne = round(Te * fs / tmpparam.scale); 1559 | int nT = round(T0 * fs / tmpparam.scale); 1560 | FP_TYPE* y = calloc(n, sizeof(FP_TYPE)); 1561 | for(i = 0; i < min(ne + 1, n); i ++) { 1562 | FP_TYPE t = (FP_TYPE)i / fs * tmpparam.scale; 1563 | y[i] = E0 * exp_2(a * t) * sin_2(wg * t); 1564 | } 1565 | for(; i < min(nT, n); i ++) { 1566 | FP_TYPE t = (FP_TYPE)i / fs * tmpparam.scale; 1567 | y[i] = - model.Ee / e / Ta * (exp_2(- e * (t - Te)) - exp_2(- e * (T0 - Te))); 1568 | } 1569 | return y; 1570 | } 1571 | -------------------------------------------------------------------------------- /ciglet.h: -------------------------------------------------------------------------------- 1 | /* 2 | ciglet 3 | === 4 | 5 | Copyright (c) 2016-2019, Kanru Hua 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without modification, 9 | are permitted provided that the following conditions are met: 10 | 11 | 1. Redistributions of source code must retain the above copyright notice, this 12 | list of conditions and the following disclaimer. 13 | 14 | 2. Redistributions in binary form must reproduce the above copyright notice, 15 | this list of conditions and the following disclaimer in the documentation and/or 16 | other materials provided with the distribution. 17 | 18 | 3. Neither the name of the copyright holder nor the names of its contributors 19 | may be used to endorse or promote products derived from this software without 20 | specific prior written permission. 21 | 22 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 23 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 24 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 26 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 29 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef CIGLET_H 35 | #define CIGLET_H 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #ifndef CIGLET_SINGLE_FILE 43 | #include "external/fastapprox-all.h" 44 | #endif 45 | 46 | // === Scalar operations === 47 | 48 | #define max(a, b) ((a) > (b) ? (a) : (b)) 49 | #define min(a, b) ((a) < (b) ? (a) : (b)) 50 | 51 | static inline FP_TYPE linterp(FP_TYPE a, FP_TYPE b, FP_TYPE ratio) { 52 | return a + (b - a) * ratio; 53 | } 54 | 55 | /* 56 | For higher efficiency we would like to replace some std math functions by their 57 | approximated versions. 58 | However we also need to be careful with approximation error. 59 | The numbers after function names listed below indicate the level of precision, 60 | the higher the better (but slower). 61 | */ 62 | #define sin_3 sin 63 | #define sin_2 fastsinfull 64 | #define sin_1 fastersinfull 65 | #define cos_3 cos 66 | #define cos_2 fastcosfull 67 | #define cos_1 fastercosfull 68 | #define exp_3 exp 69 | #define exp_2 fastexp 70 | #define exp_1 fasterexp 71 | #define log_3 log 72 | #define log_2 fastlog 73 | #define log_1 fasterlog 74 | #define atan2_3 atan2 75 | #define atan2_2 atan2 76 | #define atan2_1 fastatan2 77 | 78 | #ifndef M_PI 79 | #define M_PI 3.14159265358979323846264 80 | #endif 81 | #ifndef M_PI_2 82 | #define M_PI_2 1.57079632679489661923132 83 | #endif 84 | #define M_EPS 1e-15 85 | #define M_INF 1e15 86 | 87 | // https://gist.github.com/volkansalma/2972237 88 | static inline FP_TYPE fastatan2(FP_TYPE y, FP_TYPE x) { 89 | if(x == 0.0f) { 90 | if(y > 0.0f) return M_PI_2; 91 | if(y == 0.0f) return 0.0f; 92 | return -M_PI_2; 93 | } 94 | FP_TYPE atan; 95 | FP_TYPE z = y / x; 96 | if(fabs(z) < 1.0f) { 97 | atan = z / (1.0f + 0.28f * z * z); 98 | if(x < 0.0f) { 99 | if(y < 0.0f) return atan - M_PI; 100 | return atan + M_PI; 101 | } 102 | } else { 103 | atan = M_PI_2 - z / (z * z + 0.28f); 104 | if(y < 0.0f) return atan - M_PI; 105 | } 106 | return atan; 107 | } 108 | 109 | static inline FP_TYPE randu() { 110 | return ((FP_TYPE)rand() / RAND_MAX - 0.5) * 2.0; 111 | } 112 | 113 | static inline FP_TYPE randn(FP_TYPE u, FP_TYPE v) { 114 | FP_TYPE u1 = (FP_TYPE)rand() / RAND_MAX; 115 | FP_TYPE u2 = (FP_TYPE)rand() / RAND_MAX; 116 | return sqrt(-2.0 * log_2(u1) * v) * cos_2(2.0 * M_PI * u2) + u; 117 | } 118 | 119 | // === Complex scalar arithmetics === 120 | 121 | typedef struct { 122 | FP_TYPE real; 123 | FP_TYPE imag; 124 | } cplx; 125 | 126 | static inline cplx c_cplx(FP_TYPE real, FP_TYPE imag) { 127 | cplx ret; 128 | ret.real = real; 129 | ret.imag = imag; 130 | return ret; 131 | } 132 | 133 | static inline cplx c_conj(cplx a) { 134 | a.imag = -a.imag; 135 | return a; 136 | } 137 | 138 | static inline cplx c_add(cplx a, cplx b) { 139 | cplx ret; 140 | ret.real = a.real + b.real; 141 | ret.imag = a.imag + b.imag; 142 | return ret; 143 | } 144 | 145 | static inline cplx c_sub(cplx a, cplx b) { 146 | cplx ret; 147 | ret.real = a.real - b.real; 148 | ret.imag = a.imag - b.imag; 149 | return ret; 150 | } 151 | 152 | static inline cplx c_mul(cplx a, cplx b) { 153 | cplx ret; 154 | ret.real = a.real * b.real - a.imag * b.imag; 155 | ret.imag = a.real * b.imag + a.imag * b.real; 156 | return ret; 157 | } 158 | 159 | static inline cplx c_div(cplx a, cplx b) { 160 | cplx ret; 161 | FP_TYPE denom = b.real * b.real + b.imag * b.imag; 162 | ret.real = (a.real * b.real + a.imag * b.imag) / denom; 163 | ret.imag = (a.imag * b.real - a.real * b.imag) / denom; 164 | return ret; 165 | } 166 | 167 | #define c_exp c_exp_2 168 | 169 | static inline cplx c_exp_3(cplx a) { 170 | cplx ret; 171 | FP_TYPE mag = exp_3(a.real); 172 | ret.real = mag * cos_3(a.imag); 173 | ret.imag = mag * sin_3(a.imag); 174 | return ret; 175 | } 176 | 177 | static inline cplx c_exp_2(cplx a) { 178 | cplx ret; 179 | FP_TYPE mag = exp_2(a.real); 180 | ret.real = mag * cos_2(a.imag); 181 | ret.imag = mag * sin_2(a.imag); 182 | return ret; 183 | } 184 | 185 | static inline cplx c_exp_1(cplx a) { 186 | cplx ret; 187 | FP_TYPE mag = exp_1(a.real); 188 | ret.real = mag * cos_1(a.imag); 189 | ret.imag = mag * sin_1(a.imag); 190 | return ret; 191 | } 192 | 193 | static inline FP_TYPE c_abs(cplx a) { 194 | return sqrt(a.real * a.real + a.imag * a.imag); 195 | } 196 | 197 | #define c_arg_3 c_arg_2 198 | #define c_arg c_arg_2 199 | 200 | static inline FP_TYPE c_arg_2(cplx a) { 201 | return atan2_2(a.imag, a.real); 202 | } 203 | 204 | static inline FP_TYPE c_arg_1(cplx a) { 205 | return atan2_1(a.imag, a.real); 206 | } 207 | 208 | // === Vector operations & statistics === 209 | 210 | #define CIG_DEF_N_TO_ONE(name, op, init) \ 211 | static inline FP_TYPE name(FP_TYPE* src, int n) { \ 212 | FP_TYPE ret = init; \ 213 | for(int i = 0; i < n; i ++) \ 214 | ret = op(ret, src[i]); \ 215 | return ret; \ 216 | } 217 | 218 | #define CIG_DEF_ADD(a, b) ((a) + (b)) 219 | #define CIG_DEF_ADDSQR(a, b) ((a) + (b) * (b)) 220 | #define CIG_DEF_MAX(a, b) ((a) > (b) ? (a) : (b)) 221 | #define CIG_DEF_MIN(a, b) ((a) < (b) ? (a) : (b)) 222 | 223 | CIG_DEF_N_TO_ONE(sumfp, CIG_DEF_ADD, 0); 224 | CIG_DEF_N_TO_ONE(sumsqrfp, CIG_DEF_ADDSQR, 0); 225 | CIG_DEF_N_TO_ONE(maxfp, CIG_DEF_MAX, src[0]); 226 | CIG_DEF_N_TO_ONE(minfp, CIG_DEF_MIN, src[0]); 227 | 228 | #define meanfp(src, n) (sumfp(src, n) / (n)) 229 | 230 | static inline FP_TYPE varfp(FP_TYPE* src, int n) { 231 | FP_TYPE mean = meanfp(src, n); 232 | FP_TYPE sumsqr = sumsqrfp(src, n) / n; 233 | return sumsqr - mean * mean; 234 | } 235 | 236 | FP_TYPE cig_qselect(FP_TYPE* x, int nx, int n); 237 | FP_TYPE cig_medianfp(FP_TYPE* x, int nx); 238 | 239 | // cross correlation; maxlag is size of returned array 240 | FP_TYPE* cig_xcorr(FP_TYPE* x, FP_TYPE* y, int nx, int maxlag); 241 | 242 | static inline FP_TYPE* xcorr(FP_TYPE* x, FP_TYPE* y, int nx) { 243 | return cig_xcorr(x, y, nx, nx); 244 | } 245 | 246 | static inline FP_TYPE selectnth(FP_TYPE* x, int nx, int n) { 247 | return cig_qselect(x, nx, n); 248 | } 249 | 250 | static inline FP_TYPE medianfp(FP_TYPE* x, int nx) { 251 | return cig_medianfp(x, nx); 252 | } 253 | 254 | FP_TYPE* cig_sort(FP_TYPE* x, int nx, int* outidx); 255 | 256 | static inline FP_TYPE* sort(FP_TYPE* x, int nx, int* outidx) { 257 | return cig_sort(x, nx, outidx); 258 | } 259 | 260 | static inline FP_TYPE cov(FP_TYPE* x, FP_TYPE* y, int nx) { 261 | FP_TYPE ux = meanfp(x, nx); 262 | FP_TYPE uy = meanfp(y, nx); 263 | FP_TYPE cov = 0; 264 | for(int i = 0; i < nx; i ++) 265 | cov += (x[i] - ux) * (y[i] - uy); 266 | return cov / nx; 267 | } 268 | 269 | static inline FP_TYPE corr(FP_TYPE* x, FP_TYPE* y, int nx) { 270 | return cov(x, y, nx) / sqrt(varfp(x, nx) * varfp(y, nx)); 271 | } 272 | 273 | int cig_find_peak(FP_TYPE* x, int lidx, int uidx, int orient); 274 | int cig_find_extrema(FP_TYPE* x, int lidx, int uidx, int orient); 275 | 276 | static inline int find_peak(FP_TYPE* x, int lidx, int uidx) { 277 | return cig_find_peak(x, lidx, uidx, 1); 278 | } 279 | 280 | static inline int find_valley(FP_TYPE* x, int lidx, int uidx) { 281 | return cig_find_peak(x, lidx, uidx, -1); 282 | } 283 | 284 | static inline int find_maxima(FP_TYPE* x, int lidx, int uidx) { 285 | return cig_find_extrema(x, lidx, uidx, 1); 286 | } 287 | 288 | static inline int find_minima(FP_TYPE* x, int lidx, int uidx) { 289 | return cig_find_extrema(x, lidx, uidx, -1); 290 | } 291 | 292 | // === Numerical routines === 293 | 294 | // function pointer of one-to-one float point operation with environment variable 295 | typedef FP_TYPE (*fpe_one_to_one)(FP_TYPE x, void* env); 296 | 297 | FP_TYPE cig_fzero(fpe_one_to_one func, FP_TYPE xmin, FP_TYPE xmax, void* env); 298 | 299 | static inline FP_TYPE fzero(fpe_one_to_one func, FP_TYPE xmin, FP_TYPE xmax, void* env) { 300 | return cig_fzero(func, xmin, xmax, env); 301 | } 302 | 303 | // evaluate polynomial of order np-1 at x 304 | cplx cig_polyval(cplx* poly, int np, cplx x); 305 | 306 | static inline cplx polyval(cplx* poly, int np, cplx x) { 307 | return cig_polyval(poly, np, x); 308 | } 309 | 310 | // real coefficient version of polyval 311 | static inline cplx polyvalr(FP_TYPE* poly, int np, cplx x) { 312 | cplx* cpoly = malloc(np * sizeof(cplx)); 313 | for(int i = 0; i < np; i ++) cpoly[i] = c_cplx(poly[i], 0); 314 | cplx ret = cig_polyval(cpoly, np, x); 315 | free(cpoly); 316 | return ret; 317 | } 318 | 319 | // polynomial root finding; order = np-1 320 | // note: may not work for polynomials whose degree > 20 321 | cplx* cig_roots(cplx* poly, int np); 322 | 323 | static inline cplx* roots(cplx* poly, int np) { 324 | return cig_roots(poly, np); 325 | } 326 | 327 | // real coefficient version of roots 328 | static inline cplx* rootsr(FP_TYPE* poly, int np) { 329 | cplx* cpoly = malloc(np * sizeof(cplx)); 330 | for(int i = 0; i < np; i ++) cpoly[i] = c_cplx(poly[i], 0); 331 | cplx* ret = cig_roots(cpoly, np); 332 | free(cpoly); 333 | return ret; 334 | } 335 | 336 | // === Memory (de)allocation === 337 | 338 | static inline FP_TYPE* linspace(FP_TYPE x0, FP_TYPE x1, int nx) { 339 | FP_TYPE* x = malloc(nx * sizeof(FP_TYPE)); 340 | for(int i = 0; i < nx; i ++) 341 | x[i] = x0 + (x1 - x0) * i / (nx - 1); 342 | return x; 343 | } 344 | 345 | static inline int* iota(int x0, int step, int nx) { 346 | int* x = malloc(nx * sizeof(int)); 347 | for(int i = 0; i < nx; i ++) 348 | x[i] = x0 + step * i; 349 | return x; 350 | } 351 | 352 | #define malloc2d(m, n, size) (void*)malloc2d_(m, n, size) 353 | static inline void** malloc2d_(size_t m, size_t n, size_t size) { 354 | void** ret = calloc(m, sizeof(void*)); 355 | for(size_t i = 0; i < m; i++) 356 | ret[i] = calloc(n, size); 357 | return ret; 358 | } 359 | 360 | #define copy2d(src, m, n, size) (void*)copy2d_((void**)src, m, n, size) 361 | static inline void** copy2d_(void** src, size_t m, size_t n, size_t size) { 362 | void** ret = malloc2d(m, n, size); 363 | for(size_t i = 0; i < m; i ++) 364 | memcpy(ret[i], src[i], n * size); 365 | return ret; 366 | } 367 | 368 | #define free2d(ptr, m) free2d_((void**)(ptr), m) 369 | static inline void free2d_(void** ptr, size_t m) { 370 | for(size_t i = 0; i < m; i ++) 371 | free(ptr[i]); 372 | free(ptr); 373 | } 374 | 375 | #define flatten(ptr, m, n, size) (void*)flatten_((void**)(ptr), m, n, size) 376 | static inline void* flatten_(void** ptr, size_t m, size_t n, size_t size) { 377 | void* ret = malloc(size * m * n); 378 | for(size_t i = 0; i < m; i ++) 379 | memcpy((char*)ret + i * n * size, ptr[i], n * size); 380 | return ret; 381 | } 382 | 383 | #define reshape(ptr, m, n, size) (void*)reshape_((void**)(ptr), m, n, size) 384 | static inline void** reshape_(void* ptr, size_t m, size_t n, size_t size) { 385 | void** ret = malloc(m * sizeof(void*)); 386 | for(size_t i = 0; i < m; i ++) { 387 | ret[i] = malloc(n * size); 388 | memcpy(ret[i], (char*)ptr + i * n * size, n * size); 389 | } 390 | return ret; 391 | } 392 | 393 | void** cig_transpose(void** ptr, size_t m, size_t n, size_t size); 394 | 395 | #define transpose(ptr, m, n, size) (void*)cig_transpose((void**)(ptr), m, n, size) 396 | 397 | // === Basic Linear Algebra === 398 | 399 | /* 400 | Matrices are column-major, to be consistent with Matlab/Octave. 401 | 402 | indexing for MxN matrix A 403 | A(i, j) = A[i + j * M] 404 | */ 405 | 406 | static inline FP_TYPE* zeros(int m, int n) { 407 | return calloc(m * n, sizeof(FP_TYPE)); 408 | } 409 | 410 | static inline FP_TYPE* eye(int n) { 411 | FP_TYPE* A = zeros(n, n); 412 | for(int i = 0; i < n; i ++) 413 | A[i + n * i] = 1.0; 414 | return A; 415 | } 416 | 417 | // In-place partial pivoting, allocates & returns permutation vector. 418 | int* cig_ppivot(FP_TYPE* A, int n); 419 | 420 | // In-place row permutation. 421 | void cig_permm(FP_TYPE* A, int* perm, int m, int n); 422 | 423 | // In-place LU decomposition, need pivoting first. 424 | void cig_lu(FP_TYPE* A, int n); 425 | 426 | // In-place LU substitution, modifies b into x. 427 | void cig_lusolve(FP_TYPE* LU, FP_TYPE* b, int n); 428 | 429 | static inline int* ppivot(FP_TYPE* A, int n) { 430 | return cig_ppivot(A, n); 431 | } 432 | 433 | static inline void permm(FP_TYPE* A, int* perm, int m, int n) { 434 | cig_permm(A, perm, m, n); 435 | } 436 | 437 | // In-place vector permutation 438 | static inline void permv(FP_TYPE* x, int* perm, int n) { 439 | for(int i = 0; i < n; i ++) { 440 | int permidx = perm[i]; 441 | FP_TYPE tmp = x[i]; 442 | x[i] = x[permidx]; 443 | x[permidx] = tmp; 444 | } 445 | } 446 | 447 | static inline void lu(FP_TYPE* A, int n) { 448 | cig_lu(A, n); 449 | } 450 | 451 | static inline void lusolve(FP_TYPE* LU, FP_TYPE* b, int n) { 452 | cig_lusolve(LU, b, n); 453 | } 454 | 455 | void cig_matmul(FP_TYPE* A, FP_TYPE* B, FP_TYPE* C, int m, int n, int l); 456 | void cig_mvecmul(FP_TYPE* A, FP_TYPE* x, FP_TYPE* b, int m, int n); 457 | 458 | // A (m x n) times B (n * l) -> C (m * l) 459 | static inline void matmul(FP_TYPE* A, FP_TYPE* B, FP_TYPE* C, int m, int n, int l) { 460 | cig_matmul(A, B, C, m, n, l); 461 | } 462 | 463 | // A (m x n) times x (n * 1) -> b (m * 1) 464 | static inline void mvecmul(FP_TYPE* A, FP_TYPE* x, FP_TYPE* b, int m, int n) { 465 | cig_mvecmul(A, x, b, n, n); 466 | } 467 | 468 | static inline FP_TYPE dot(FP_TYPE* x, FP_TYPE* y, int n) { 469 | FP_TYPE sum = 0; 470 | for(int i = 0; i < n; i ++) 471 | sum += x[i] * y[i]; 472 | return sum; 473 | } 474 | 475 | // === Audio I/O === 476 | 477 | FP_TYPE* wavread(char* filename, int* fs, int* nbit, int* nx); 478 | void wavwrite(FP_TYPE* y, int ny, int fs, int nbit, char* filename); 479 | FP_TYPE* wavread_fp(FILE* fin, int* fs, int* nbit, int* nx); 480 | void wavwrite_fp(FP_TYPE* y, int ny, int fs, int nbit, FILE* fout); 481 | 482 | // === General DSP routines === 483 | 484 | static inline FP_TYPE* fetch_frame(FP_TYPE* x, int nx, int center, int nf) { 485 | FP_TYPE* y = malloc(nf * sizeof(FP_TYPE)); 486 | for(int i = 0; i < nf; i ++) { 487 | int isrc = center + i - nf / 2; 488 | y[i] = (isrc >= 0 && isrc < nx) ? x[isrc] : 0; 489 | } 490 | return y; 491 | } 492 | 493 | // generate a sinusoid given frequency, amplitude and its phase at n/2 position 494 | static inline FP_TYPE* gensin(FP_TYPE freq, FP_TYPE ampl, FP_TYPE phse, int n, int fs) { 495 | FP_TYPE tpffs = 2.0 * M_PI / fs * freq; 496 | FP_TYPE c = 2.0 * cos_3(tpffs); 497 | FP_TYPE* s = calloc(n, sizeof(FP_TYPE)); 498 | s[0] = cos_3(tpffs * (- n / 2) + phse); 499 | s[1] = cos_3(tpffs * (- n / 2 + 1) + phse); 500 | for(int t = 2; t < n; t ++) { 501 | s[t] = c * s[t - 1] - s[t - 2]; 502 | } 503 | return s; 504 | } 505 | 506 | FP_TYPE* cig_gensins(FP_TYPE* freq, FP_TYPE* ampl, FP_TYPE* phse, 507 | int nsin, int fs, int n); 508 | 509 | static inline FP_TYPE* gensins(FP_TYPE* freq, FP_TYPE* ampl, FP_TYPE* phse, 510 | int nsin, int fs, int n) { 511 | return cig_gensins(freq, ampl, phse, nsin, fs, n); 512 | } 513 | 514 | static inline FP_TYPE* boxcar(int n) { 515 | FP_TYPE* ret = malloc(n * sizeof(FP_TYPE)); 516 | for(int i = 0; i < n; i ++) 517 | ret[i] = 1.0; 518 | return ret; 519 | } 520 | 521 | #define CIG_DEF_HANNING(fname, cosfunc) \ 522 | static inline FP_TYPE* fname(int n) { \ 523 | FP_TYPE* ret = malloc(n * sizeof(FP_TYPE)); \ 524 | for(int i = 0; i < n; i ++) \ 525 | ret[i] = 0.5 * (1 - cosfunc(2 * M_PI * i / (n - 1))); \ 526 | return ret; \ 527 | } 528 | 529 | CIG_DEF_HANNING(hanning, cos_3); 530 | CIG_DEF_HANNING(hanning_2, cos_2); 531 | 532 | #define CIG_DEF_HAMMING(fname, cosfunc) \ 533 | static inline FP_TYPE* fname(int n) { \ 534 | FP_TYPE* ret = malloc(n * sizeof(FP_TYPE)); \ 535 | for(int i = 0; i < n; i ++) \ 536 | ret[i] = 0.54 - 0.46 * cosfunc(2 * M_PI * i / (n - 1)); \ 537 | return ret; \ 538 | } 539 | 540 | CIG_DEF_HAMMING(hamming, cos_3); 541 | CIG_DEF_HAMMING(hamming_2, cos_2); 542 | 543 | #define CIG_DEF_MLTSINE(fname, sinfunc) \ 544 | static inline FP_TYPE* fname(int n) { \ 545 | FP_TYPE* ret = malloc(n * sizeof(FP_TYPE)); \ 546 | for(int i = 0; i < n; i ++) \ 547 | ret[i] = sinfunc(M_PI / n * (i + 0.5)); \ 548 | return ret; \ 549 | } 550 | 551 | CIG_DEF_MLTSINE(mltsine, sin_3); 552 | CIG_DEF_MLTSINE(mltsine_2, sin_2); 553 | 554 | // 92dB side lobe 555 | #define CIG_DEF_BLACKMAN_HARRIS(fname, cosfunc) \ 556 | static inline FP_TYPE* fname(int n) { \ 557 | FP_TYPE* ret = malloc(n * sizeof(FP_TYPE)); \ 558 | const FP_TYPE a0 = 0.35875; \ 559 | const FP_TYPE a1 = 0.48829; \ 560 | const FP_TYPE a2 = 0.14128; \ 561 | const FP_TYPE a3 = 0.01168; \ 562 | for(int i = 0; i < n; i ++) \ 563 | ret[i] = a0 - a1 * cosfunc(2.0 * M_PI * i / n) + \ 564 | a2 * cosfunc(4.0 * M_PI * i / n) - \ 565 | a3 * cosfunc(6.0 * M_PI * i / n); \ 566 | return ret; \ 567 | } 568 | 569 | CIG_DEF_BLACKMAN_HARRIS(blackman_harris, cos_3); 570 | CIG_DEF_BLACKMAN_HARRIS(blackman_harris_2, cos_2); 571 | 572 | // 98dB side lobe, Prof. Kawahara's favorite 573 | #define CIG_DEF_NUTTALL98(fname, cosfunc) \ 574 | static inline FP_TYPE* fname(int n) { \ 575 | FP_TYPE* ret = malloc(n * sizeof(FP_TYPE)); \ 576 | const FP_TYPE a0 = 0.3635819; \ 577 | const FP_TYPE a1 = 0.4891775; \ 578 | const FP_TYPE a2 = 0.1365995; \ 579 | const FP_TYPE a3 = 0.0106411; \ 580 | for(int i = 0; i < n; i ++) \ 581 | ret[i] = a0 - a1 * cosfunc(2.0 * M_PI * i / n) + \ 582 | a2 * cosfunc(4.0 * M_PI * i / n) - \ 583 | a3 * cosfunc(6.0 * M_PI * i / n); \ 584 | return ret; \ 585 | } 586 | 587 | CIG_DEF_NUTTALL98(nuttall98, cos_3); 588 | CIG_DEF_NUTTALL98(nuttall98_2, cos_2); 589 | 590 | #define CIG_DEF_BLACKMAN(fname, cosfunc) \ 591 | static inline FP_TYPE* fname(int n) { \ 592 | FP_TYPE* ret = malloc(n * sizeof(FP_TYPE)); \ 593 | const FP_TYPE a0 = 0.42; \ 594 | const FP_TYPE a1 = 0.5; \ 595 | const FP_TYPE a2 = 0.08; \ 596 | for(int i = 0; i < n; i ++) \ 597 | ret[i] = a0 - a1 * cos_3(2.0 * M_PI * i / n) + \ 598 | a2 * cos_3(4.0 * M_PI * i / n); \ 599 | return ret; \ 600 | } 601 | 602 | CIG_DEF_BLACKMAN(blackman, cos_3); 603 | CIG_DEF_BLACKMAN(blackman_2, cos_2); 604 | 605 | void cig_fft(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, 606 | int n, FP_TYPE* buffer, FP_TYPE mode); 607 | 608 | static inline void fft(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, 609 | int n, FP_TYPE* buffer) { 610 | cig_fft(xr, xi, yr, yi, n, buffer, -1.0); 611 | } 612 | 613 | static inline void ifft(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, 614 | int n, FP_TYPE* buffer) { 615 | cig_fft(xr, xi, yr, yi, n, buffer, 1.0); 616 | } 617 | 618 | void cig_czt(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, FP_TYPE omega0, int n); 619 | 620 | static inline void czt(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, 621 | FP_TYPE omega0, int n) { 622 | cig_czt(xr, xi, yr, yi, omega0, n); 623 | } 624 | 625 | static inline void iczt(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, 626 | FP_TYPE omega0, int n) { 627 | cig_czt(xr, xi, yr, yi, -omega0, n); 628 | if(yr != NULL) 629 | for(int i = 0; i < n; i ++) 630 | yr[i] /= n; 631 | if(yi != NULL) 632 | for(int i = 0; i < n; i ++) 633 | yi[i] /= n; 634 | } 635 | 636 | void cig_idft(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, int n); 637 | 638 | static inline void idft(FP_TYPE* xr, FP_TYPE* xi, FP_TYPE* yr, FP_TYPE* yi, int n) { 639 | cig_idft(xr, xi, yr, yi, n); 640 | } 641 | 642 | FP_TYPE* cig_dct(FP_TYPE* x, int nx); 643 | 644 | static inline FP_TYPE* dct(FP_TYPE* x, int nx) { 645 | return cig_dct(x, nx); 646 | } 647 | 648 | static inline FP_TYPE* fftshift(FP_TYPE* x, int n) { 649 | FP_TYPE* y = malloc(n * sizeof(FP_TYPE)); 650 | int halfs = n / 2; 651 | int halfl = (n + 1) / 2; 652 | for(int i = 0; i < halfs; i ++) 653 | y[i] = x[i + halfl]; 654 | for(int i = 0; i < halfl; i ++) 655 | y[i + halfs] = x[i]; 656 | return y; 657 | } 658 | 659 | static inline FP_TYPE* unwrap(FP_TYPE* x, int n) { 660 | FP_TYPE* y = malloc(n * sizeof(FP_TYPE)); 661 | y[0] = x[0]; 662 | for(int i = 1; i < n; i ++) { 663 | if(fabs(x[i] - x[i - 1]) > M_PI) 664 | y[i] = y[i - 1] + x[i] - (x[i - 1] + 2.0 * M_PI * (x[i] > x[i - 1] ? 1.0 : -1.0)); 665 | else 666 | y[i] = y[i - 1] + x[i] - x[i - 1]; 667 | } 668 | return y; 669 | } 670 | 671 | static inline FP_TYPE wrap(FP_TYPE x) { 672 | return x - round(x / 2.0 / M_PI) * 2.0 * M_PI; 673 | } 674 | 675 | static inline FP_TYPE* diff(FP_TYPE* x, int nx) { 676 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 677 | y[0] = x[0]; 678 | for(int i = 1; i < nx; i ++) 679 | y[i] = x[i] - x[i - 1]; 680 | return y; 681 | } 682 | 683 | static inline FP_TYPE* cumsum(FP_TYPE* x, int nx) { 684 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 685 | y[0] = x[0]; 686 | for(int i = 1; i < nx; i ++) 687 | y[i] = y[i - 1] + x[i]; 688 | return y; 689 | } 690 | 691 | static inline FP_TYPE* flip(FP_TYPE* x, int nx) { 692 | FP_TYPE* y = malloc(nx * sizeof(FP_TYPE)); 693 | for(int i = 0; i < nx; i ++) 694 | y[i] = x[nx - i - 1]; 695 | return y; 696 | } 697 | 698 | static inline FP_TYPE* abscplx(FP_TYPE* xr, FP_TYPE* xi, int n) { 699 | FP_TYPE* y = malloc(n * sizeof(FP_TYPE)); 700 | for(int i = 0; i < n; i ++) 701 | y[i] = sqrt(xr[i] * xr[i] + xi[i] * xi[i]); 702 | return y; 703 | } 704 | 705 | static inline FP_TYPE* argcplx(FP_TYPE* xr, FP_TYPE* xi, int n) { 706 | FP_TYPE* y = malloc(n * sizeof(FP_TYPE)); 707 | for(int i = 0; i < n; i ++) 708 | y[i] = atan2_3(xi[i], xr[i]); 709 | return y; 710 | } 711 | 712 | static inline FP_TYPE* polar2real(FP_TYPE* xabs, FP_TYPE* xarg, int n) { 713 | FP_TYPE* y = malloc(n * sizeof(FP_TYPE)); 714 | for(int i = 0; i < n; i ++) 715 | y[i] = xabs[i] * cos_2(xarg[i]); 716 | return y; 717 | } 718 | 719 | static inline FP_TYPE* polar2imag(FP_TYPE* xabs, FP_TYPE* xarg, int n) { 720 | FP_TYPE* y = malloc(n * sizeof(FP_TYPE)); 721 | for(int i = 0; i < n; i ++) 722 | y[i] = xabs[i] * sin_2(xarg[i]); 723 | return y; 724 | } 725 | 726 | static inline FP_TYPE phase_diff(FP_TYPE p1, FP_TYPE p2) { 727 | if(p2 > p1) p1 += (int)((p2 - p1) / 2.0 / M_PI + 1) * 2.0 * M_PI; 728 | return fmod(p1 - p2 + M_PI, M_PI * 2.0) - M_PI; 729 | } 730 | 731 | static inline void complete_symm(FP_TYPE* x, int n) { 732 | if(n / 2 == (n + 1) / 2) // even 733 | x[n / 2] = x[n / 2 - 1]; 734 | for(int i = n / 2 + 1; i < n; i ++) 735 | x[i] = x[n - i]; 736 | } 737 | 738 | static inline void complete_asymm(FP_TYPE* x, int n) { 739 | if(n / 2 == (n + 1) / 2) // even 740 | x[n / 2] = x[n / 2 - 1]; 741 | for(int i = n / 2 + 1; i < n; i ++) 742 | x[i] = -x[n - i]; 743 | } 744 | 745 | static inline FP_TYPE* rceps(FP_TYPE* S, int nfft) { 746 | FP_TYPE* buff = malloc(nfft * 4 * sizeof(FP_TYPE)); 747 | FP_TYPE* C = buff; 748 | FP_TYPE* S_symm = buff + nfft; 749 | FP_TYPE* fftbuff = buff + nfft * 2; 750 | for(int i = 0; i < nfft / 2 + 1; i ++) 751 | S_symm[i] = S[i]; 752 | complete_symm(S_symm, nfft); 753 | ifft(S_symm, NULL, C, NULL, nfft, fftbuff); 754 | return realloc(C, nfft * sizeof(FP_TYPE)); 755 | } 756 | 757 | static inline FP_TYPE* irceps(FP_TYPE* C, int n, int nfft) { 758 | FP_TYPE* buff = malloc(nfft * 4 * sizeof(FP_TYPE)); 759 | FP_TYPE* S = buff; 760 | FP_TYPE* C_symm = buff + nfft; 761 | FP_TYPE* fftbuff = buff + nfft * 2; 762 | for(int i = 0; i < n; i ++) 763 | C_symm[i] = C[i]; 764 | for(int i = n; i < nfft / 2 + 1; i ++) 765 | C_symm[i] = 0; 766 | complete_symm(C_symm, nfft); 767 | fft(C_symm, NULL, S, NULL, nfft, fftbuff); 768 | return realloc(S, nfft * sizeof(FP_TYPE)); 769 | } 770 | 771 | static inline FP_TYPE* minphase(FP_TYPE* S, int nfft) { 772 | FP_TYPE* buff = malloc(nfft * 4 * sizeof(FP_TYPE)); 773 | FP_TYPE* S_symm = buff; 774 | FP_TYPE* C = buff + nfft; 775 | FP_TYPE* fftbuff = buff + nfft * 2; 776 | for(int i = 0; i < nfft / 2 + 1; i ++) 777 | S_symm[i] = S[i]; 778 | complete_symm(S_symm, nfft); 779 | ifft(S_symm, NULL, C, NULL, nfft, fftbuff); 780 | for(int i = 1; i < nfft / 2 + 1; i ++) 781 | C[i] *= 2.0; 782 | for(int i = nfft / 2 + 2; i < nfft; i ++) 783 | C[i] = 0.0; 784 | fft(C, NULL, NULL, S_symm, nfft, fftbuff); 785 | return realloc(S_symm, nfft * sizeof(FP_TYPE)); 786 | } 787 | 788 | FP_TYPE* cig_winfir(int order, FP_TYPE cutoff, FP_TYPE cutoff2, 789 | char* type, char* window); 790 | 791 | static inline FP_TYPE* fir1(int order, FP_TYPE cutoff, char* type, char* window) { 792 | return cig_winfir(order, cutoff / 2.0, 0, type, window); 793 | } 794 | 795 | static inline FP_TYPE* fir1bp(int order, FP_TYPE cutoff_low, FP_TYPE cutoff_high, 796 | char* window) { 797 | return cig_winfir(order, cutoff_low / 2.0, cutoff_high / 2.0, "bandpass", window); 798 | } 799 | 800 | FP_TYPE* cig_convolution(FP_TYPE* x, FP_TYPE* h, int nx, int nh); 801 | 802 | static inline FP_TYPE* conv(FP_TYPE* x, FP_TYPE* h, int nx, int nh) { 803 | return cig_convolution(x, h, nx, nh); 804 | } 805 | 806 | FP_TYPE* cig_filter(FP_TYPE* b, int nb, FP_TYPE* a, int na, FP_TYPE* x, int nx); 807 | 808 | static inline FP_TYPE* filter(FP_TYPE* b, int nb, FP_TYPE* a, int na, 809 | FP_TYPE* x, int nx) { 810 | return cig_filter(b, nb, a, na, x, nx); 811 | } 812 | 813 | static inline FP_TYPE* filtfilt(FP_TYPE* b, int nb, FP_TYPE* a, int na, 814 | FP_TYPE* x, int nx) { 815 | FP_TYPE* y = cig_filter(b, nb, a, na, x, nx); 816 | FP_TYPE* z = malloc(nx * sizeof(FP_TYPE)); 817 | for(int i = 0; i < nx; i ++) z[i] = y[nx - i - 1]; // flip the signal 818 | FP_TYPE* y2 = cig_filter(b, nb, a, na, z, nx); // fliter again 819 | for(int i = 0; i < nx; i ++) y[i] = y2[nx - i - 1]; // flip back the signal 820 | free(z); 821 | free(y2); 822 | return y; 823 | } 824 | 825 | // 1d time-varying Kalman filtering, returns E[x_t | z_1, ..., z_t]; 826 | // writes Var[x_t | z_1, ..., z_t] into P; 827 | // writes total log likelihood into L if L != NULL 828 | FP_TYPE* cig_kalmanf1d(FP_TYPE* z, FP_TYPE* Q, FP_TYPE* R, int nz, FP_TYPE x0, 829 | FP_TYPE* P, FP_TYPE* L); 830 | 831 | static inline FP_TYPE* kalmanf1d(FP_TYPE* z, FP_TYPE* Q, FP_TYPE* R, int nz, 832 | FP_TYPE* P, FP_TYPE* L) { 833 | return cig_kalmanf1d(z, Q, R, nz, z[0], P, L); 834 | } 835 | 836 | // 1d time-varying Kalman smoothing, returns E[x_t | z_1, ..., z_T] 837 | FP_TYPE* cig_kalmans1d(FP_TYPE* y, FP_TYPE* P, FP_TYPE* Q, int ny); 838 | 839 | static inline FP_TYPE* kalmans1d(FP_TYPE* y, FP_TYPE* P, FP_TYPE* Q, int ny) { 840 | return cig_kalmans1d(y, P, Q, ny); 841 | } 842 | 843 | // The following lists different forms of an all-pole filter, which might be helpful 844 | // when using LPC-related functions. 845 | // a[0] x[n] = u[n] - a[1] x[n - 1] - a[2] x[n - 2] - a[3] x[n - 3] - ... recurrent form 846 | // a[0] X(z) = U(z) - (a[1] z^-1 + a[2] z^-2 + a[3] z^-3 + ...) X(z) z-transform 847 | // H(z) = X(z) / U(z) = 1 / (a[0] + a[1] z^-1 + a[2] z^-2 + a[3] z^-3 + ...) transfer function 848 | // = z^N / (a[0] z^p + a[1] z^p-1 + a[2] z^p-2 + ... + a[p - 1] z + a[p]) anti-causal 849 | // = z^N / [a[0] (z - p[1]) (z - p[2]) (z - p[3]) ... (z - p[p])] factorization 850 | // = 1 / [a[0] (1 - p[1] z^-1) (1 - p[2] z^-1) ... (1 - p[p] z^-1)] poles 851 | 852 | // Matlab functions tf2zp/tf2zpk are equivalent to respectively calling roots() on 853 | // numerator and denominator coefficients (you might want to first normalize the 854 | // zero-order term to get the gain). For the sake of simplicity they are not going 855 | // to be implemented. 856 | 857 | // R: autocorrelation; returns n sized array of AR coefficients 858 | FP_TYPE* cig_levinson(FP_TYPE* R, int n); 859 | 860 | static inline FP_TYPE* levinson(FP_TYPE* R, int n) { 861 | return cig_levinson(R, n); 862 | } 863 | 864 | // Note: length of returned array is p+1 (since polynomial of order p has p+1 terms) 865 | static inline FP_TYPE* lpc(FP_TYPE* x, int nx, int p, FP_TYPE** R_out) { 866 | if(p + 1 > nx) return NULL; 867 | FP_TYPE* R = cig_xcorr(x, x, nx, p + 1); 868 | FP_TYPE* a = cig_levinson(R, p + 1); 869 | if(R_out != NULL) 870 | *R_out = R; 871 | else 872 | free(R); 873 | return a; 874 | } 875 | 876 | // LPC from magnitude spectrum 877 | static inline FP_TYPE* flpc(FP_TYPE* S, int ns, int p, FP_TYPE** R_out) { 878 | int nfft = pow(2, ceil(log2(ns - 1) + 1)); 879 | FP_TYPE* tmp = calloc(nfft * 4, sizeof(FP_TYPE)); 880 | FP_TYPE* R = tmp; 881 | FP_TYPE* Xsqr = tmp + nfft; 882 | FP_TYPE* fftbuff = tmp + nfft * 2; 883 | for(int j = 0; j < ns; j ++) 884 | Xsqr[j] = S[j] * S[j]; 885 | complete_symm(Xsqr, nfft); 886 | ifft(Xsqr, NULL, R, NULL, nfft, fftbuff); 887 | FP_TYPE* a = cig_levinson(R, p + 1); 888 | if(R_out != NULL) 889 | *R_out = realloc(tmp, (p + 1) * sizeof(FP_TYPE)); 890 | else 891 | free(tmp); 892 | return a; 893 | } 894 | 895 | static inline FP_TYPE lpgain(FP_TYPE* a, FP_TYPE* R, int n) { 896 | FP_TYPE g = 0; 897 | for(int i = 0; i < n; i ++) 898 | g += a[i] * R[i]; 899 | return sqrt(g); 900 | } 901 | 902 | static inline FP_TYPE* lpspec(FP_TYPE* a, FP_TYPE gain, int n, int nfft) { 903 | FP_TYPE* buff = malloc(nfft * 5 * sizeof(FP_TYPE)); 904 | FP_TYPE* Sr = buff; 905 | FP_TYPE* Si = buff + nfft; 906 | FP_TYPE* A = buff + nfft * 2; 907 | FP_TYPE* fftbuff = buff + nfft * 3; 908 | for(int i = 0; i < n; i ++) 909 | A[i] = a[i]; 910 | for(int i = n; i < nfft; i ++) 911 | A[i] = 0; 912 | fft(A, NULL, Sr, Si, nfft, fftbuff); 913 | for(int i = 0; i < nfft; i ++) 914 | Sr[i] = gain / sqrt(Sr[i] * Sr[i] + Si[i] * Si[i]); 915 | Sr = realloc(Sr, nfft * sizeof(FP_TYPE)); 916 | return Sr; 917 | } 918 | 919 | // get resonance frequencies (relative to nyquist) from LP coefficients 920 | // returns array of frequencies in ascending order; np is number of poles; 921 | // pole locations are written into poles if not NULL. 922 | static inline FP_TYPE* lpresf(FP_TYPE* a, int n, cplx* poles, int* np) { 923 | cplx* r = rootsr(a, n); 924 | FP_TYPE* freqs = malloc((n - 1) * sizeof(FP_TYPE)); 925 | *np = 0; 926 | for(int i = 0; i < n - 1; i ++) { 927 | FP_TYPE f = atan2_2(r[i].imag, r[i].real) / M_PI; 928 | if(f > 0.001 && f < 0.999) { // ignore real poles and negative frequencies 929 | r[*np] = r[i]; 930 | freqs[*np] = f; 931 | (*np) ++; 932 | } 933 | } 934 | int* freqidx = malloc((*np) * sizeof(int)); 935 | FP_TYPE* freqsort = cig_sort(freqs, *np, freqidx); 936 | free(freqs); 937 | 938 | if(poles != NULL) { 939 | for(int i = 0; i < *np; i ++) 940 | poles[i] = r[freqidx[i]]; 941 | } 942 | free(r); 943 | free(freqidx); 944 | return freqsort; 945 | } 946 | 947 | FP_TYPE* cig_interp(FP_TYPE* xi, FP_TYPE* yi, int ni, FP_TYPE* x, int nx); 948 | 949 | static inline FP_TYPE* interp1(FP_TYPE* xi, FP_TYPE* yi, int ni, FP_TYPE* x, int nx) { 950 | return cig_interp(xi, yi, ni, x, nx); 951 | } 952 | 953 | FP_TYPE* cig_interpu(FP_TYPE xi0, FP_TYPE xi1, FP_TYPE* yi, int ni, FP_TYPE* x, int nx); 954 | 955 | static inline FP_TYPE* interp1u(FP_TYPE xi0, FP_TYPE xi1, FP_TYPE* yi, int ni, 956 | FP_TYPE* x, int nx) { 957 | return cig_interpu(xi0, xi1, yi, ni, x, nx); 958 | } 959 | 960 | FP_TYPE* cig_sincinterpu(FP_TYPE xi0, FP_TYPE xi1, FP_TYPE* yi, int ni, FP_TYPE* x, int nx); 961 | static inline FP_TYPE* sincinterp1u(FP_TYPE xi0, FP_TYPE xi1, FP_TYPE* yi, int ni, 962 | FP_TYPE* x, int nx) { 963 | return cig_sincinterpu(xi0, xi1, yi, ni, x, nx); 964 | } 965 | 966 | // replace discontinuties in a series with interpolated values 967 | static inline FP_TYPE* interp_in_blank(FP_TYPE* x, int nx, FP_TYPE blank) { 968 | FP_TYPE* x_sample = calloc(nx + 2, sizeof(FP_TYPE)); 969 | FP_TYPE* i_sample = calloc(nx + 2, sizeof(FP_TYPE)); 970 | int n = 1; 971 | for(int i = 0; i < nx; i ++) 972 | if(x[i] != blank || (isnan(blank) && (! isnan(x[i])))) { 973 | x_sample[n] = x[i]; 974 | i_sample[n] = i; 975 | n ++; 976 | } 977 | x_sample[0] = x_sample[1]; 978 | i_sample[0] = 0; 979 | x_sample[n] = x_sample[n - 1]; 980 | i_sample[n] = nx; 981 | FP_TYPE* yi = linspace(0, nx - 1, nx); 982 | FP_TYPE* y = interp1(i_sample, x_sample, n + 2, yi, nx); 983 | free(yi); 984 | free(x_sample); 985 | free(i_sample); 986 | return y; 987 | } 988 | 989 | FP_TYPE* cig_medfilt(FP_TYPE* x, int nx, int order); 990 | 991 | static inline FP_TYPE* medfilt1(FP_TYPE* x, int nx, int order) { 992 | return cig_medfilt(x, nx, order); 993 | } 994 | 995 | static inline FP_TYPE* white_noise(FP_TYPE amplitude, int n) { 996 | FP_TYPE* y = malloc(n * sizeof(FP_TYPE)); 997 | for(int i = 0; i < n; i ++) 998 | y[i] = ((FP_TYPE)rand() / RAND_MAX - 0.5) * amplitude * 2.0; 999 | return y; 1000 | } 1001 | 1002 | FP_TYPE* cig_moving_avg(FP_TYPE* x, int nx, FP_TYPE halford); 1003 | static inline FP_TYPE* moving_avg(FP_TYPE* x, int nx, FP_TYPE halford) { 1004 | return cig_moving_avg(x, nx, halford); 1005 | } 1006 | 1007 | static inline FP_TYPE* moving_rms(FP_TYPE* x, int nx, int order) { 1008 | FP_TYPE* xsqr = malloc(nx * sizeof(FP_TYPE)); 1009 | for(int i = 0; i < nx; i ++) 1010 | xsqr[i] = x[i] * x[i]; 1011 | FP_TYPE* y = moving_avg(xsqr, nx, order); 1012 | for(int i = 0; i < nx; i ++) 1013 | y[i] = y[i] <= 0 ? 0 : sqrt(y[i]); 1014 | free(xsqr); 1015 | return y; 1016 | } 1017 | 1018 | static inline FP_TYPE itakura_saito(FP_TYPE* S, FP_TYPE* S0, int nS) { 1019 | FP_TYPE* d = malloc(nS * sizeof(FP_TYPE)); 1020 | for(int i = 0; i < nS; i ++) 1021 | d[i] = S[i] / S0[i] - log_2(S[i] / S0[i]) - 1.0; 1022 | FP_TYPE ret = log_2(sumfp(d, nS) / nS); 1023 | free(d); 1024 | return ret; 1025 | } 1026 | 1027 | // DTFT sinc function 1028 | static inline FP_TYPE safe_aliased_sinc(FP_TYPE M, FP_TYPE omega) { 1029 | FP_TYPE denom = sin_3(omega * 0.5); 1030 | if(fabs(denom) < 10e-5) return M; 1031 | return sin_3(M * omega * 0.5) / denom; 1032 | } 1033 | 1034 | // derivative of DTFT sinc function 1035 | static inline FP_TYPE safe_aliased_dsinc(FP_TYPE M, FP_TYPE omega) { 1036 | FP_TYPE cosn = cos_3(M * omega / 2); 1037 | FP_TYPE cosw = cos_3(omega / 2); 1038 | FP_TYPE sinn = sin_3(M * omega / 2); 1039 | FP_TYPE sinw = sin_3(omega / 2); 1040 | if(fabs(sinw) < 1e-5) return 0; 1041 | return 0.5 * (M * cosn - cosw / sinw * sinn) / sinw; 1042 | } 1043 | 1044 | FP_TYPE* cig_rresample(FP_TYPE* x, int nx, FP_TYPE ratio, int* ny); 1045 | 1046 | static inline FP_TYPE* rresample(FP_TYPE* x, int nx, FP_TYPE ratio, int* ny) { 1047 | return cig_rresample(x, nx, ratio, ny); 1048 | } 1049 | 1050 | 1051 | // === Audio/speech processing routines === 1052 | 1053 | // instantaneous frequency detector 1054 | typedef struct { 1055 | FP_TYPE fc; // central frequency over sampling rate 1056 | int nh; // length of impulse responses 1057 | FP_TYPE* hr; // impulse response (real) 1058 | FP_TYPE* hi; // impulse response (imag) 1059 | FP_TYPE* hdr; // impulse response derivative (real) 1060 | FP_TYPE* hdi; // impulse response derivative (imag) 1061 | } ifdetector; 1062 | 1063 | // fc: central frequency, fres: frequency resolution 1064 | // both are expressed as ratio of sampling rate 1065 | ifdetector* cig_create_ifdetector(FP_TYPE fc, FP_TYPE fres); 1066 | void cig_delete_ifdetector(ifdetector* dst); 1067 | 1068 | static inline ifdetector* create_ifdetector(FP_TYPE fc, FP_TYPE fres) { 1069 | return cig_create_ifdetector(fc, fres); 1070 | } 1071 | 1072 | static inline void delete_ifdetector(ifdetector* dst) { 1073 | cig_delete_ifdetector(dst); 1074 | } 1075 | 1076 | // estimate instantaneous frequency (as a ratio of sampling rate) at the center of x 1077 | // returns 0 if x is not long enough 1078 | FP_TYPE cig_ifdetector_estimate(ifdetector* ifd, FP_TYPE* x, int nx); 1079 | 1080 | static inline FP_TYPE ifdetector_estimate(ifdetector* ifd, FP_TYPE* x, int nx) { 1081 | return cig_ifdetector_estimate(ifd, x, nx); 1082 | } 1083 | 1084 | typedef struct { 1085 | int nchannel; // number of channels/bands 1086 | int nf; // size of frequency response 1087 | FP_TYPE fnyq; // upperbound of frequency response 1088 | FP_TYPE** fresp; // array of frequency response of each channel 1089 | 1090 | int* lower_idx; // index where the freq. resp. in each channel rises from 0 1091 | int* upper_idx; // index where the freq. resp. in each channel fades to 0 1092 | } filterbank; 1093 | 1094 | static inline FP_TYPE mel2freq(FP_TYPE mel) { 1095 | return 700.0 * (exp_2(mel / 1125.0) - 1.0); 1096 | } 1097 | 1098 | static inline FP_TYPE freq2mel(FP_TYPE f) { 1099 | return 1125.0 * log_2(1.0 + f / 700.0); 1100 | } 1101 | 1102 | static inline FP_TYPE freq2bark(FP_TYPE f) { 1103 | return 6.0 * asinh(f / 600.0); 1104 | } 1105 | 1106 | static inline FP_TYPE bark2freq(FP_TYPE z) { 1107 | return 600.0 * sinh(z / 6.0); 1108 | } 1109 | 1110 | static inline FP_TYPE eqloud(FP_TYPE f) { 1111 | FP_TYPE f2 = f * f; 1112 | FP_TYPE f4 = f2 * f2; 1113 | return f4 / (f2 + 1.6e5) / (f2 + 1.6e5) * (f2 + 1.44e6) / (f2 + 9.61e6); 1114 | } 1115 | 1116 | static inline FP_TYPE* melspace(FP_TYPE fmin, FP_TYPE fmax, int n) { 1117 | FP_TYPE* freq = malloc((n + 1) * sizeof(FP_TYPE)); 1118 | FP_TYPE mmin = freq2mel(fmin); 1119 | FP_TYPE mmax = freq2mel(fmax); 1120 | for(int i = 0; i <= n; i ++) 1121 | freq[i] = mel2freq((FP_TYPE)i / n * (mmax - mmin) + mmin); 1122 | return freq; 1123 | } 1124 | 1125 | #define CIG_CORR_ACF 0 1126 | #define CIG_CORR_AMDF 1 1127 | #define CIG_CORR_SQRDIFF 2 1128 | #define CIG_CORR_YIN 3 1129 | 1130 | // size of R: nfrm x max_period 1131 | void cig_correlogram(FP_TYPE* x, int nx, int* center, int* nwin, int nfrm, 1132 | int max_period, int method, FP_TYPE** R); 1133 | 1134 | // convert y axis of correlogram from period to frequency 1135 | FP_TYPE** cig_invcrgm(FP_TYPE** R, int nfrm, int max_period, int fs, FP_TYPE* faxis, int nf); 1136 | 1137 | void cig_stft_forward(FP_TYPE* x, int nx, int* center, int* nwin, int nfrm, 1138 | int nfft, char* window, int subt_mean, int optlv, 1139 | FP_TYPE* norm_factor, FP_TYPE* weight_factor, FP_TYPE** Xmagn, FP_TYPE** Xphse); 1140 | 1141 | FP_TYPE* cig_stft_backward(FP_TYPE** Xmagn, FP_TYPE** Xphse, int nhop, int nfrm, 1142 | int offset, int hop_factor, int zp_factor, int nfade, FP_TYPE norm_factor, int* ny); 1143 | 1144 | #define CIG_DEF_STFT(fname, optlv) \ 1145 | static inline void fname(FP_TYPE* x, int nx, int nhop, int nfrm, int hopfc, int zpfc, \ 1146 | FP_TYPE* normfc, FP_TYPE* weightfc, FP_TYPE** Xmagn, FP_TYPE** Xphse) { \ 1147 | int* center = malloc(nfrm * sizeof(int)); \ 1148 | int* nwin = malloc(nfrm * sizeof(int)); \ 1149 | for(int i = 0; i < nfrm; i ++) { \ 1150 | center[i] = nhop * i; \ 1151 | nwin[i] = nhop * hopfc; \ 1152 | } \ 1153 | cig_stft_forward(x, nx, center, nwin, nfrm, nhop * hopfc * zpfc, "blackman", 0, \ 1154 | optlv, normfc, weightfc, Xmagn, Xphse); \ 1155 | free(center); \ 1156 | free(nwin); \ 1157 | } 1158 | 1159 | CIG_DEF_STFT(stft, 3); 1160 | CIG_DEF_STFT(stft_2, 2); 1161 | 1162 | static inline FP_TYPE* istft(FP_TYPE** Xmagn, FP_TYPE** Xphse, int nhop, int nfrm, 1163 | int hopfc, int zpfc, FP_TYPE normfc, int* ny) { 1164 | return cig_stft_backward(Xmagn, Xphse, nhop, nfrm, 0, hopfc, zpfc, 32, normfc, ny); 1165 | } 1166 | 1167 | FP_TYPE cig_qifft(FP_TYPE* magn, int k, FP_TYPE* dst_freq); 1168 | 1169 | static inline FP_TYPE qifft(FP_TYPE* magn, int k, FP_TYPE* dst_freq) { 1170 | return cig_qifft(magn, k, dst_freq); 1171 | } 1172 | 1173 | static inline FP_TYPE** spgm2cegm(FP_TYPE** S, int nfrm, int nfft, int ncep) { 1174 | FP_TYPE** C = (FP_TYPE**)malloc2d(nfrm, ncep, sizeof(FP_TYPE)); 1175 | FP_TYPE* xbuff = calloc(nfft, sizeof(FP_TYPE)); 1176 | FP_TYPE* cbuff = calloc(nfft, sizeof(FP_TYPE)); 1177 | FP_TYPE* fftbuff = calloc(nfft * 2, sizeof(FP_TYPE)); 1178 | 1179 | for(int i = 0; i < nfrm; i ++) { 1180 | for(int j = 0; j < nfft / 2 + 1; j ++) 1181 | xbuff[j] = log_2(S[i][j] + M_EPS); 1182 | complete_symm(xbuff, nfft); 1183 | ifft(xbuff, NULL, cbuff, NULL, nfft, fftbuff); 1184 | for(int j = 0; j < ncep; j ++) { 1185 | C[i][j] = cbuff[j]; 1186 | } 1187 | } 1188 | 1189 | free(fftbuff); 1190 | free(cbuff); 1191 | free(xbuff); 1192 | return C; 1193 | } 1194 | 1195 | static inline FP_TYPE** cegm2spgm(FP_TYPE** C, int nfrm, int nfft, int ncep) { 1196 | FP_TYPE** S = (FP_TYPE**)malloc2d(nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 1197 | FP_TYPE* xbuff = calloc(nfft, sizeof(FP_TYPE)); 1198 | FP_TYPE* cbuff = calloc(nfft, sizeof(FP_TYPE)); 1199 | FP_TYPE* fftbuff = calloc(nfft * 2, sizeof(FP_TYPE)); 1200 | 1201 | for(int i = 0; i < nfrm; i ++) { 1202 | for(int j = 0; j < ncep; j ++) 1203 | cbuff[j] = C[i][j]; 1204 | for(int j = ncep; j < nfft / 2 + 1; j ++) 1205 | cbuff[j] = 0; 1206 | complete_symm(cbuff, nfft); 1207 | fft(cbuff, NULL, xbuff, NULL, nfft, fftbuff); 1208 | for(int j = 0; j < nfft / 2 + 1; j ++) 1209 | S[i][j] = exp_2(xbuff[j]); 1210 | } 1211 | 1212 | free(fftbuff); 1213 | free(cbuff); 1214 | free(xbuff); 1215 | return S; 1216 | } 1217 | 1218 | filterbank* cig_create_empty_filterbank(int nf, FP_TYPE fnyq, int nchannel); 1219 | filterbank* cig_create_plp_filterbank(int nf, FP_TYPE fnyq, int nchannel); 1220 | filterbank* cig_create_melfreq_filterbank(int nf, FP_TYPE fnyq, int nchannel, 1221 | FP_TYPE min_freq, FP_TYPE max_freq, FP_TYPE scale, FP_TYPE min_width); 1222 | void cig_delete_filterbank(filterbank* dst); 1223 | 1224 | static inline filterbank* create_filterbank(int nf, FP_TYPE fnyq, int nchannel) { 1225 | return cig_create_empty_filterbank(nf, fnyq, nchannel); 1226 | } 1227 | 1228 | static inline filterbank* create_plpfilterbank(int nf, FP_TYPE fnyq, int nchannel) { 1229 | return cig_create_plp_filterbank(nf, fnyq, nchannel); 1230 | } 1231 | 1232 | static inline filterbank* create_melfilterbank(int nf, FP_TYPE fnyq, int nchannel, 1233 | FP_TYPE min_freq, FP_TYPE max_freq) { 1234 | return cig_create_melfreq_filterbank( 1235 | nf, fnyq, nchannel, min_freq, max_freq, 1.0, 400.0); 1236 | } 1237 | 1238 | static inline void delete_filterbank(filterbank* dst) { 1239 | cig_delete_filterbank(dst); 1240 | } 1241 | 1242 | FP_TYPE** cig_filterbank_spectrogram(filterbank* fbank, FP_TYPE** S, int nfrm, 1243 | int nfft, int fs, int crtenergy); 1244 | 1245 | static inline FP_TYPE** filterbank_spgm(filterbank* fbank, FP_TYPE** S, int nfrm, 1246 | int nfft, int fs, int crtenergy) { 1247 | return cig_filterbank_spectrogram(fbank, S, nfrm, nfft, fs, crtenergy); 1248 | } 1249 | 1250 | FP_TYPE* cig_filterbank_spectrum(filterbank* fbank, FP_TYPE* S, int nfft, int fs, 1251 | int crtenergy); 1252 | 1253 | static inline FP_TYPE* filterbank_spec(filterbank* fbank, FP_TYPE* S, int nfft, 1254 | int fs, int crtenergy) { 1255 | return cig_filterbank_spectrum(fbank, S, nfft, fs, crtenergy); 1256 | } 1257 | 1258 | static inline FP_TYPE* be2cc(FP_TYPE* band_energy, int nbe, int ncc, int with_energy) { 1259 | FP_TYPE* dctcoef = dct(band_energy, nbe); 1260 | FP_TYPE energy = dctcoef[0]; 1261 | ncc = min(nbe - 1, ncc); 1262 | for(int i = 0; i < ncc; i ++) 1263 | dctcoef[i] = dctcoef[i + 1]; 1264 | if(with_energy) 1265 | dctcoef[ncc] = energy; 1266 | return dctcoef; 1267 | } 1268 | 1269 | static inline FP_TYPE** be2ccgm(FP_TYPE** E, int nfrm, int nbe, int ncc, int with_energy) { 1270 | FP_TYPE** C = malloc(nfrm * sizeof(FP_TYPE*)); 1271 | for(int i = 0; i < nfrm; i ++) 1272 | C[i] = be2cc(E[i], nbe, ncc, with_energy); 1273 | return C; 1274 | } 1275 | 1276 | // estimate speech spectral envelope on a spectrum S of size nfft / 2 + 1 1277 | // f0: ratio of fundamental frequency to sampling rate 1278 | FP_TYPE* cig_spec2env(FP_TYPE* S, int nfft, FP_TYPE f0, int nhar, FP_TYPE* Cout); 1279 | 1280 | static inline FP_TYPE* spec2env(FP_TYPE* S, int nfft, FP_TYPE f0, FP_TYPE* Cout) { 1281 | return cig_spec2env(S, nfft, f0, floor(nfft / 2 / f0), Cout); 1282 | } 1283 | 1284 | typedef struct { 1285 | FP_TYPE T0; 1286 | FP_TYPE te; 1287 | FP_TYPE tp; 1288 | FP_TYPE ta; 1289 | FP_TYPE Ee; 1290 | } lfmodel; 1291 | 1292 | lfmodel cig_lfmodel_from_rd(FP_TYPE rd, FP_TYPE T0, FP_TYPE Ee); 1293 | 1294 | static inline lfmodel lfmodel_from_rd(FP_TYPE rd, FP_TYPE T0, FP_TYPE Ee) { 1295 | return cig_lfmodel_from_rd(rd, T0, Ee); 1296 | }; 1297 | 1298 | FP_TYPE* cig_lfmodel_spectrum(lfmodel model, FP_TYPE* freq, int nf, FP_TYPE* dst_phase); 1299 | 1300 | static inline FP_TYPE* lfmodel_spectrum(lfmodel model, FP_TYPE* freq, int nf, FP_TYPE* dst_phase) { 1301 | return cig_lfmodel_spectrum(model, freq, nf, dst_phase); 1302 | } 1303 | 1304 | FP_TYPE* cig_lfmodel_period(lfmodel model, int fs, int n); 1305 | 1306 | static inline FP_TYPE* lfmodel_period(lfmodel model, int fs, int n) { 1307 | return cig_lfmodel_period(model, fs, n); 1308 | } 1309 | 1310 | // === Plotting utilities (Gnuplot interface) === 1311 | // Notice: not supported on Windows 1312 | #if _POSIX_C_SOURCE >= 2 1313 | #include 1314 | #include 1315 | #include "sys/types.h" 1316 | 1317 | typedef struct { 1318 | FILE* handle; 1319 | } figure; 1320 | 1321 | static inline figure* plotopenv(const char* name) { 1322 | figure* fg = malloc(sizeof(figure)); 1323 | int childio[2]; 1324 | pipe(childio); 1325 | 1326 | pid_t child = fork(); 1327 | if(child == 0) { 1328 | dup2(childio[0], STDIN_FILENO); 1329 | close(childio[1]); 1330 | execlp(name, name, "-p", NULL); 1331 | fprintf(stderr, "Error: Gnuplot not available.\n"); 1332 | exit(1); 1333 | } else { 1334 | close(childio[0]); 1335 | fg -> handle = fdopen(childio[1], "w"); 1336 | } 1337 | return fg; 1338 | } 1339 | 1340 | static inline figure* plotopen() { 1341 | return plotopenv("gnuplot-qt"); 1342 | } 1343 | 1344 | static inline void plot(figure* fg, FP_TYPE* x, FP_TYPE* y, int nx, char ccolor) { 1345 | char* color = "blue"; 1346 | switch(ccolor) { 1347 | case 'b': 1348 | color = "blue"; 1349 | break; 1350 | case 'r': 1351 | color = "red"; 1352 | break; 1353 | case 'g': 1354 | color = "green"; 1355 | break; 1356 | case 'k': 1357 | color = "black"; 1358 | break; 1359 | case 'c': 1360 | color = "cyan"; 1361 | break; 1362 | case 'y': 1363 | color = "yellow"; 1364 | break; 1365 | } 1366 | fprintf(fg -> handle, "plot '-' with lines lc rgb \"%s\"\n", color); 1367 | for(int i = 0; i < nx; i ++) { 1368 | fprintf(fg -> handle, "%f %f\n", x == NULL ? (FP_TYPE)i : x[i], y[i]); 1369 | } 1370 | fprintf(fg -> handle, "e\n"); 1371 | } 1372 | 1373 | static inline void imagesc(figure* fg, FP_TYPE** X, int m, int n) { 1374 | fprintf(fg -> handle, "set xrange [%f:%f]\n", 0.0, n - 1.0); 1375 | fprintf(fg -> handle, "set yrange [%f:%f]\n", 0.0, m - 1.0); 1376 | fprintf(fg -> handle, "set view map\n"); 1377 | fprintf(fg -> handle, "splot '-' matrix with image\n"); 1378 | for(int i = 0; i < m; i ++) { 1379 | for(int j = 0; j < n; j ++) 1380 | fprintf(fg -> handle, "%5.3e ", X[i][j]); 1381 | fprintf(fg -> handle, "\n"); 1382 | } 1383 | fprintf(fg -> handle, "e\n"); 1384 | } 1385 | 1386 | static inline void plotclose(figure* fg) { 1387 | fclose(fg -> handle); 1388 | free(fg); 1389 | } 1390 | 1391 | #endif 1392 | 1393 | #endif 1394 | -------------------------------------------------------------------------------- /ciglet.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 14 4 | VisualStudioVersion = 14.0.25420.1 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "ciglet", "ciglet.vcxproj", "{6FD573BB-3F2E-4EA9-8A29-752448730E11}" 7 | EndProject 8 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "test", "test\test.vcxproj", "{C947E0D4-E63A-47A5-B40F-94D65541389A}" 9 | EndProject 10 | Global 11 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 12 | Debug|x64 = Debug|x64 13 | Debug|x86 = Debug|x86 14 | Release|x64 = Release|x64 15 | Release|x86 = Release|x86 16 | EndGlobalSection 17 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 18 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Debug|x64.ActiveCfg = Debug|x64 19 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Debug|x64.Build.0 = Debug|x64 20 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Debug|x86.ActiveCfg = Debug|Win32 21 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Debug|x86.Build.0 = Debug|Win32 22 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Release|x64.ActiveCfg = Release|x64 23 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Release|x64.Build.0 = Release|x64 24 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Release|x86.ActiveCfg = Release|Win32 25 | {6FD573BB-3F2E-4EA9-8A29-752448730E11}.Release|x86.Build.0 = Release|Win32 26 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Debug|x64.ActiveCfg = Debug|x64 27 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Debug|x64.Build.0 = Debug|x64 28 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Debug|x86.ActiveCfg = Debug|Win32 29 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Debug|x86.Build.0 = Debug|Win32 30 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Release|x64.ActiveCfg = Release|x64 31 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Release|x64.Build.0 = Release|x64 32 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Release|x86.ActiveCfg = Release|Win32 33 | {C947E0D4-E63A-47A5-B40F-94D65541389A}.Release|x86.Build.0 = Release|Win32 34 | EndGlobalSection 35 | GlobalSection(SolutionProperties) = preSolution 36 | HideSolutionNode = FALSE 37 | EndGlobalSection 38 | EndGlobal 39 | -------------------------------------------------------------------------------- /ciglet.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | {6FD573BB-3F2E-4EA9-8A29-752448730E11} 23 | Win32Proj 24 | ciglet 25 | 10.0 26 | 27 | 28 | 29 | StaticLibrary 30 | true 31 | v142 32 | Unicode 33 | 34 | 35 | StaticLibrary 36 | false 37 | Intel C++ Compiler 19.0 38 | true 39 | Unicode 40 | 41 | 42 | StaticLibrary 43 | true 44 | v142 45 | Unicode 46 | 47 | 48 | StaticLibrary 49 | false 50 | Intel C++ Compiler 19.0 51 | true 52 | Unicode 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | $(ProjectDir)/external;$(IncludePath) 74 | 75 | 76 | $(ProjectDir)/external;$(IncludePath) 77 | 78 | 79 | $(ProjectDir)/external;$(IncludePath) 80 | 81 | 82 | $(ProjectDir)/external;$(IncludePath) 83 | 84 | 85 | 86 | 87 | 88 | Level3 89 | Disabled 90 | FP_TYPE=float; WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) 91 | CompileAsC 92 | 4244;4305;4996 93 | 94 | 95 | Windows 96 | 97 | 98 | 99 | 100 | 101 | 102 | Level3 103 | Disabled 104 | FP_TYPE=float; _DEBUG;_LIB;%(PreprocessorDefinitions) 105 | CompileAsC 106 | 4244;4305;4996 107 | 108 | 109 | Windows 110 | 111 | 112 | 113 | 114 | Level3 115 | 116 | 117 | MaxSpeed 118 | true 119 | true 120 | FP_TYPE=float; WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) 121 | CompileAsC 122 | 4244;4305;4996 123 | stdcpp14 124 | MultiThreaded 125 | 126 | 127 | Windows 128 | true 129 | true 130 | 131 | 132 | 133 | 134 | Level3 135 | 136 | 137 | MaxSpeed 138 | true 139 | true 140 | FP_TYPE=float; NDEBUG;_LIB;%(PreprocessorDefinitions) 141 | CompileAsC 142 | 4244;4305;4996 143 | stdcpp14 144 | MultiThreaded 145 | 146 | 147 | Windows 148 | true 149 | true 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | -------------------------------------------------------------------------------- /external/fast_median.c: -------------------------------------------------------------------------------- 1 | /* 2 | * The following routines have been built from knowledge gathered 3 | * around the Web. I am not aware of any copyright problem with 4 | * them, so use it as you want. 5 | * N. Devillard - 1998 6 | */ 7 | 8 | #define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); } 9 | #define PIX_SWAP(a,b) { FP_TYPE temp=(a);(a)=(b);(b)=temp; } 10 | 11 | /*---------------------------------------------------------------------------- 12 | Function : opt_med3() 13 | In : pointer to array of 3 pixel values 14 | Out : a FP_TYPE 15 | Job : optimized search of the median of 3 pixel values 16 | Notice : found on sci.image.processing 17 | cannot go faster unless assumptions are made 18 | on the nature of the input signal. 19 | ---------------------------------------------------------------------------*/ 20 | 21 | FP_TYPE opt_med3(FP_TYPE * p) 22 | { 23 | PIX_SORT(p[0],p[1]) ; PIX_SORT(p[1],p[2]) ; PIX_SORT(p[0],p[1]) ; 24 | return(p[1]) ; 25 | } 26 | 27 | /*---------------------------------------------------------------------------- 28 | Function : opt_med5() 29 | In : pointer to array of 5 pixel values 30 | Out : a FP_TYPE 31 | Job : optimized search of the median of 5 pixel values 32 | Notice : found on sci.image.processing 33 | cannot go faster unless assumptions are made 34 | on the nature of the input signal. 35 | ---------------------------------------------------------------------------*/ 36 | 37 | FP_TYPE opt_med5(FP_TYPE * p) 38 | { 39 | PIX_SORT(p[0],p[1]) ; PIX_SORT(p[3],p[4]) ; PIX_SORT(p[0],p[3]) ; 40 | PIX_SORT(p[1],p[4]) ; PIX_SORT(p[1],p[2]) ; PIX_SORT(p[2],p[3]) ; 41 | PIX_SORT(p[1],p[2]) ; return(p[2]) ; 42 | } 43 | 44 | /*---------------------------------------------------------------------------- 45 | Function : opt_med7() 46 | In : pointer to array of 7 pixel values 47 | Out : a FP_TYPE 48 | Job : optimized search of the median of 7 pixel values 49 | Notice : found on sci.image.processing 50 | cannot go faster unless assumptions are made 51 | on the nature of the input signal. 52 | ---------------------------------------------------------------------------*/ 53 | 54 | FP_TYPE opt_med7(FP_TYPE * p) 55 | { 56 | PIX_SORT(p[0], p[5]) ; PIX_SORT(p[0], p[3]) ; PIX_SORT(p[1], p[6]) ; 57 | PIX_SORT(p[2], p[4]) ; PIX_SORT(p[0], p[1]) ; PIX_SORT(p[3], p[5]) ; 58 | PIX_SORT(p[2], p[6]) ; PIX_SORT(p[2], p[3]) ; PIX_SORT(p[3], p[6]) ; 59 | PIX_SORT(p[4], p[5]) ; PIX_SORT(p[1], p[4]) ; PIX_SORT(p[1], p[3]) ; 60 | PIX_SORT(p[3], p[4]) ; return (p[3]) ; 61 | } 62 | 63 | /*---------------------------------------------------------------------------- 64 | Function : opt_med9() 65 | In : pointer to an array of 9 FP_TYPEs 66 | Out : a FP_TYPE 67 | Job : optimized search of the median of 9 FP_TYPEs 68 | Notice : in theory, cannot go faster without assumptions on the 69 | signal. 70 | Formula from: 71 | XILINX XCELL magazine, vol. 23 by John L. Smith 72 | 73 | The input array is modified in the process 74 | The result array is guaranteed to contain the median 75 | value 76 | in middle position, but other elements are NOT sorted. 77 | ---------------------------------------------------------------------------*/ 78 | 79 | FP_TYPE opt_med9(FP_TYPE * p) 80 | { 81 | PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; 82 | PIX_SORT(p[0], p[1]) ; PIX_SORT(p[3], p[4]) ; PIX_SORT(p[6], p[7]) ; 83 | PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; 84 | PIX_SORT(p[0], p[3]) ; PIX_SORT(p[5], p[8]) ; PIX_SORT(p[4], p[7]) ; 85 | PIX_SORT(p[3], p[6]) ; PIX_SORT(p[1], p[4]) ; PIX_SORT(p[2], p[5]) ; 86 | PIX_SORT(p[4], p[7]) ; PIX_SORT(p[4], p[2]) ; PIX_SORT(p[6], p[4]) ; 87 | PIX_SORT(p[4], p[2]) ; return(p[4]) ; 88 | } 89 | 90 | 91 | /*---------------------------------------------------------------------------- 92 | Function : opt_med25() 93 | In : pointer to an array of 25 FP_TYPEs 94 | Out : a FP_TYPE 95 | Job : optimized search of the median of 25 FP_TYPEs 96 | Notice : in theory, cannot go faster without assumptions on the 97 | signal. 98 | Code taken from Graphic Gems. 99 | ---------------------------------------------------------------------------*/ 100 | 101 | FP_TYPE opt_med25(FP_TYPE * p) 102 | { 103 | 104 | 105 | PIX_SORT(p[0], p[1]) ; PIX_SORT(p[3], p[4]) ; PIX_SORT(p[2], p[4]) ; 106 | PIX_SORT(p[2], p[3]) ; PIX_SORT(p[6], p[7]) ; PIX_SORT(p[5], p[7]) ; 107 | PIX_SORT(p[5], p[6]) ; PIX_SORT(p[9], p[10]) ; PIX_SORT(p[8], p[10]) ; 108 | PIX_SORT(p[8], p[9]) ; PIX_SORT(p[12], p[13]) ; PIX_SORT(p[11], p[13]) ; 109 | PIX_SORT(p[11], p[12]) ; PIX_SORT(p[15], p[16]) ; PIX_SORT(p[14], p[16]) ; 110 | PIX_SORT(p[14], p[15]) ; PIX_SORT(p[18], p[19]) ; PIX_SORT(p[17], p[19]) ; 111 | PIX_SORT(p[17], p[18]) ; PIX_SORT(p[21], p[22]) ; PIX_SORT(p[20], p[22]) ; 112 | PIX_SORT(p[20], p[21]) ; PIX_SORT(p[23], p[24]) ; PIX_SORT(p[2], p[5]) ; 113 | PIX_SORT(p[3], p[6]) ; PIX_SORT(p[0], p[6]) ; PIX_SORT(p[0], p[3]) ; 114 | PIX_SORT(p[4], p[7]) ; PIX_SORT(p[1], p[7]) ; PIX_SORT(p[1], p[4]) ; 115 | PIX_SORT(p[11], p[14]) ; PIX_SORT(p[8], p[14]) ; PIX_SORT(p[8], p[11]) ; 116 | PIX_SORT(p[12], p[15]) ; PIX_SORT(p[9], p[15]) ; PIX_SORT(p[9], p[12]) ; 117 | PIX_SORT(p[13], p[16]) ; PIX_SORT(p[10], p[16]) ; PIX_SORT(p[10], p[13]) ; 118 | PIX_SORT(p[20], p[23]) ; PIX_SORT(p[17], p[23]) ; PIX_SORT(p[17], p[20]) ; 119 | PIX_SORT(p[21], p[24]) ; PIX_SORT(p[18], p[24]) ; PIX_SORT(p[18], p[21]) ; 120 | PIX_SORT(p[19], p[22]) ; PIX_SORT(p[8], p[17]) ; PIX_SORT(p[9], p[18]) ; 121 | PIX_SORT(p[0], p[18]) ; PIX_SORT(p[0], p[9]) ; PIX_SORT(p[10], p[19]) ; 122 | PIX_SORT(p[1], p[19]) ; PIX_SORT(p[1], p[10]) ; PIX_SORT(p[11], p[20]) ; 123 | PIX_SORT(p[2], p[20]) ; PIX_SORT(p[2], p[11]) ; PIX_SORT(p[12], p[21]) ; 124 | PIX_SORT(p[3], p[21]) ; PIX_SORT(p[3], p[12]) ; PIX_SORT(p[13], p[22]) ; 125 | PIX_SORT(p[4], p[22]) ; PIX_SORT(p[4], p[13]) ; PIX_SORT(p[14], p[23]) ; 126 | PIX_SORT(p[5], p[23]) ; PIX_SORT(p[5], p[14]) ; PIX_SORT(p[15], p[24]) ; 127 | PIX_SORT(p[6], p[24]) ; PIX_SORT(p[6], p[15]) ; PIX_SORT(p[7], p[16]) ; 128 | PIX_SORT(p[7], p[19]) ; PIX_SORT(p[13], p[21]) ; PIX_SORT(p[15], p[23]) ; 129 | PIX_SORT(p[7], p[13]) ; PIX_SORT(p[7], p[15]) ; PIX_SORT(p[1], p[9]) ; 130 | PIX_SORT(p[3], p[11]) ; PIX_SORT(p[5], p[17]) ; PIX_SORT(p[11], p[17]) ; 131 | PIX_SORT(p[9], p[17]) ; PIX_SORT(p[4], p[10]) ; PIX_SORT(p[6], p[12]) ; 132 | PIX_SORT(p[7], p[14]) ; PIX_SORT(p[4], p[6]) ; PIX_SORT(p[4], p[7]) ; 133 | PIX_SORT(p[12], p[14]) ; PIX_SORT(p[10], p[14]) ; PIX_SORT(p[6], p[7]) ; 134 | PIX_SORT(p[10], p[12]) ; PIX_SORT(p[6], p[10]) ; PIX_SORT(p[6], p[17]) ; 135 | PIX_SORT(p[12], p[17]) ; PIX_SORT(p[7], p[17]) ; PIX_SORT(p[7], p[10]) ; 136 | PIX_SORT(p[12], p[18]) ; PIX_SORT(p[7], p[12]) ; PIX_SORT(p[10], p[18]) ; 137 | PIX_SORT(p[12], p[20]) ; PIX_SORT(p[10], p[20]) ; PIX_SORT(p[10], p[12]) ; 138 | 139 | return (p[12]); 140 | } 141 | 142 | 143 | 144 | #undef PIX_SORT 145 | #undef PIX_SWAP 146 | 147 | -------------------------------------------------------------------------------- /external/fastapprox-all.h: -------------------------------------------------------------------------------- 1 | /*=====================================================================* 2 | * Copyright (C) 2012 Paul Mineiro * 3 | * All rights reserved. * 4 | * * 5 | * Redistribution and use in source and binary forms, with * 6 | * or without modification, are permitted provided that the * 7 | * following conditions are met: * 8 | * * 9 | * * Redistributions of source code must retain the * 10 | * above copyright notice, this list of conditions and * 11 | * the following disclaimer. * 12 | * * 13 | * * Redistributions in binary form must reproduce the * 14 | * above copyright notice, this list of conditions and * 15 | * the following disclaimer in the documentation and/or * 16 | * other materials provided with the distribution. * 17 | * * 18 | * * Neither the name of Paul Mineiro nor the names * 19 | * of other contributors may be used to endorse or promote * 20 | * products derived from this software without specific * 21 | * prior written permission. * 22 | * * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 24 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 25 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 26 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 27 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 28 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 30 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 31 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 32 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 33 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 34 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 35 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 36 | * POSSIBILITY OF SUCH DAMAGE. * 37 | * * 38 | * Contact: Paul Mineiro * 39 | *=====================================================================*/ 40 | 41 | #ifndef __CAST_H_ 42 | 43 | #ifdef __cplusplus 44 | #define cast_uint32_t static_cast 45 | #else 46 | #define cast_uint32_t (uint32_t) 47 | #endif 48 | 49 | #endif // __CAST_H_ 50 | /*=====================================================================* 51 | * Copyright (C) 2011 Paul Mineiro * 52 | * All rights reserved. * 53 | * * 54 | * Redistribution and use in source and binary forms, with * 55 | * or without modification, are permitted provided that the * 56 | * following conditions are met: * 57 | * * 58 | * * Redistributions of source code must retain the * 59 | * above copyright notice, this list of conditions and * 60 | * the following disclaimer. * 61 | * * 62 | * * Redistributions in binary form must reproduce the * 63 | * above copyright notice, this list of conditions and * 64 | * the following disclaimer in the documentation and/or * 65 | * other materials provided with the distribution. * 66 | * * 67 | * * Neither the name of Paul Mineiro nor the names * 68 | * of other contributors may be used to endorse or promote * 69 | * products derived from this software without specific * 70 | * prior written permission. * 71 | * * 72 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 73 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 74 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 75 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 76 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 77 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 78 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 79 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 80 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 81 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 82 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 83 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 84 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 85 | * POSSIBILITY OF SUCH DAMAGE. * 86 | * * 87 | * Contact: Paul Mineiro * 88 | *=====================================================================*/ 89 | 90 | #ifndef __SSE_H_ 91 | #define __SSE_H_ 92 | 93 | #ifdef __SSE2__ 94 | 95 | #include 96 | 97 | #ifdef __cplusplus 98 | namespace { 99 | #endif // __cplusplus 100 | 101 | typedef __m128 v4sf; 102 | typedef __m128i v4si; 103 | 104 | #define v4si_to_v4sf _mm_cvtepi32_ps 105 | #define v4sf_to_v4si _mm_cvttps_epi32 106 | 107 | #define v4sfl(x) ((const v4sf) { (x), (x), (x), (x) }) 108 | #define v2dil(x) ((const v4si) { (x), (x) }) 109 | #define v4sil(x) v2dil((((unsigned long long) (x)) << 32) | (x)) 110 | 111 | typedef union { v4sf f; float array[4]; } v4sfindexer; 112 | #define v4sf_index(_findx, _findi) \ 113 | ({ \ 114 | v4sfindexer _findvx = { _findx } ; \ 115 | _findvx.array[_findi]; \ 116 | }) 117 | typedef union { v4si i; int array[4]; } v4siindexer; 118 | #define v4si_index(_iindx, _iindi) \ 119 | ({ \ 120 | v4siindexer _iindvx = { _iindx } ; \ 121 | _iindvx.array[_iindi]; \ 122 | }) 123 | 124 | typedef union { v4sf f; v4si i; } v4sfv4sipun; 125 | #define v4sf_fabs(x) \ 126 | ({ \ 127 | v4sfv4sipun vx; \ 128 | vx.f = x; \ 129 | vx.i &= v4sil (0x7FFFFFFF); \ 130 | vx.f; \ 131 | }) 132 | 133 | #ifdef __cplusplus 134 | } // end namespace 135 | #endif // __cplusplus 136 | 137 | #endif // __SSE2__ 138 | 139 | #endif // __SSE_H_ 140 | /*=====================================================================* 141 | * Copyright (C) 2011 Paul Mineiro * 142 | * All rights reserved. * 143 | * * 144 | * Redistribution and use in source and binary forms, with * 145 | * or without modification, are permitted provided that the * 146 | * following conditions are met: * 147 | * * 148 | * * Redistributions of source code must retain the * 149 | * above copyright notice, this list of conditions and * 150 | * the following disclaimer. * 151 | * * 152 | * * Redistributions in binary form must reproduce the * 153 | * above copyright notice, this list of conditions and * 154 | * the following disclaimer in the documentation and/or * 155 | * other materials provided with the distribution. * 156 | * * 157 | * * Neither the name of Paul Mineiro nor the names * 158 | * of other contributors may be used to endorse or promote * 159 | * products derived from this software without specific * 160 | * prior written permission. * 161 | * * 162 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 163 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 164 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 165 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 166 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 167 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 168 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 169 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 170 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 171 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 172 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 173 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 174 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 175 | * POSSIBILITY OF SUCH DAMAGE. * 176 | * * 177 | * Contact: Paul Mineiro * 178 | *=====================================================================*/ 179 | 180 | #ifndef __FAST_EXP_H_ 181 | #define __FAST_EXP_H_ 182 | 183 | #include 184 | 185 | // Underflow of exponential is common practice in numerical routines, 186 | // so handle it here. 187 | 188 | static inline float 189 | fastpow2 (float p) 190 | { 191 | float offset = (p < 0) ? 1.0f : 0.0f; 192 | float clipp = (p < -126) ? -126.0f : p; 193 | int w = clipp; 194 | float z = clipp - w + offset; 195 | union { uint32_t i; float f; } v = { cast_uint32_t ( (1 << 23) * (clipp + 121.2740575f + 27.7280233f / (4.84252568f - z) - 1.49012907f * z) ) }; 196 | 197 | return v.f; 198 | } 199 | 200 | static inline float 201 | fastexp (float p) 202 | { 203 | return fastpow2 (1.442695040f * p); 204 | } 205 | 206 | static inline float 207 | fasterpow2 (float p) 208 | { 209 | float clipp = (p < -126) ? -126.0f : p; 210 | union { uint32_t i; float f; } v = { cast_uint32_t ( (1 << 23) * (clipp + 126.94269504f) ) }; 211 | return v.f; 212 | } 213 | 214 | static inline float 215 | fasterexp (float p) 216 | { 217 | return fasterpow2 (1.442695040f * p); 218 | } 219 | 220 | #if defined(__SSE2__) && ! defined(__ICL) 221 | static inline v4sf 222 | vfastpow2 (const v4sf p) 223 | { 224 | v4sf ltzero = _mm_cmplt_ps (p, v4sfl (0.0f)); 225 | v4sf offset = _mm_and_ps (ltzero, v4sfl (1.0f)); 226 | v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f)); 227 | v4sf clipp = _mm_or_ps (_mm_andnot_ps (lt126, p), _mm_and_ps (lt126, v4sfl (-126.0f))); 228 | v4si w = v4sf_to_v4si (clipp); 229 | v4sf z = clipp - v4si_to_v4sf (w) + offset; 230 | 231 | const v4sf c_121_2740838 = v4sfl (121.2740575f); 232 | const v4sf c_27_7280233 = v4sfl (27.7280233f); 233 | const v4sf c_4_84252568 = v4sfl (4.84252568f); 234 | const v4sf c_1_49012907 = v4sfl (1.49012907f); 235 | union { v4si i; v4sf f; } v = { 236 | v4sf_to_v4si ( 237 | v4sfl (1 << 23) * 238 | (clipp + c_121_2740838 + c_27_7280233 / (c_4_84252568 - z) - c_1_49012907 * z) 239 | ) 240 | }; 241 | 242 | return v.f; 243 | } 244 | 245 | static inline v4sf 246 | vfastexp (const v4sf p) 247 | { 248 | const v4sf c_invlog_2 = v4sfl (1.442695040f); 249 | 250 | return vfastpow2 (c_invlog_2 * p); 251 | } 252 | 253 | static inline v4sf 254 | vfasterpow2 (const v4sf p) 255 | { 256 | const v4sf c_126_94269504 = v4sfl (126.94269504f); 257 | v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f)); 258 | v4sf clipp = _mm_or_ps (_mm_andnot_ps (lt126, p), _mm_and_ps (lt126, v4sfl (-126.0f))); 259 | union { v4si i; v4sf f; } v = { v4sf_to_v4si (v4sfl (1 << 23) * (clipp + c_126_94269504)) }; 260 | return v.f; 261 | } 262 | 263 | static inline v4sf 264 | vfasterexp (const v4sf p) 265 | { 266 | const v4sf c_invlog_2 = v4sfl (1.442695040f); 267 | 268 | return vfasterpow2 (c_invlog_2 * p); 269 | } 270 | 271 | #endif //__SSE2__ 272 | 273 | #endif // __FAST_EXP_H_ 274 | /*=====================================================================* 275 | * Copyright (C) 2011 Paul Mineiro * 276 | * All rights reserved. * 277 | * * 278 | * Redistribution and use in source and binary forms, with * 279 | * or without modification, are permitted provided that the * 280 | * following conditions are met: * 281 | * * 282 | * * Redistributions of source code must retain the * 283 | * above copyright notice, this list of conditions and * 284 | * the following disclaimer. * 285 | * * 286 | * * Redistributions in binary form must reproduce the * 287 | * above copyright notice, this list of conditions and * 288 | * the following disclaimer in the documentation and/or * 289 | * other materials provided with the distribution. * 290 | * * 291 | * * Neither the name of Paul Mineiro nor the names * 292 | * of other contributors may be used to endorse or promote * 293 | * products derived from this software without specific * 294 | * prior written permission. * 295 | * * 296 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 297 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 298 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 299 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 300 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 301 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 302 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 303 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 304 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 305 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 306 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 307 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 308 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 309 | * POSSIBILITY OF SUCH DAMAGE. * 310 | * * 311 | * Contact: Paul Mineiro * 312 | *=====================================================================*/ 313 | 314 | #ifndef __FAST_LOG_H_ 315 | #define __FAST_LOG_H_ 316 | 317 | #include 318 | 319 | static inline float 320 | fastlog2 (float x) 321 | { 322 | union { float f; uint32_t i; } vx = { x }; 323 | union { uint32_t i; float f; } mx = { (vx.i & 0x007FFFFF) | 0x3f000000 }; 324 | float y = vx.i; 325 | y *= 1.1920928955078125e-7f; 326 | 327 | return y - 124.22551499f 328 | - 1.498030302f * mx.f 329 | - 1.72587999f / (0.3520887068f + mx.f); 330 | } 331 | 332 | static inline float 333 | fastlog (float x) 334 | { 335 | return 0.69314718f * fastlog2 (x); 336 | } 337 | 338 | static inline float 339 | fasterlog2 (float x) 340 | { 341 | union { float f; uint32_t i; } vx = { x }; 342 | float y = vx.i; 343 | y *= 1.1920928955078125e-7f; 344 | return y - 126.94269504f; 345 | } 346 | 347 | static inline float 348 | fasterlog (float x) 349 | { 350 | // return 0.69314718f * fasterlog2 (x); 351 | 352 | union { float f; uint32_t i; } vx = { x }; 353 | float y = vx.i; 354 | y *= 8.2629582881927490e-8f; 355 | return y - 87.989971088f; 356 | } 357 | 358 | #if defined(__SSE2__) && ! defined(__ICL) 359 | 360 | static inline v4sf 361 | vfastlog2 (v4sf x) 362 | { 363 | union { v4sf f; v4si i; } vx = { x }; 364 | union { v4si i; v4sf f; } mx; mx.i = (vx.i & v4sil (0x007FFFFF)) | v4sil (0x3f000000); 365 | v4sf y = v4si_to_v4sf (vx.i); 366 | y *= v4sfl (1.1920928955078125e-7f); 367 | 368 | const v4sf c_124_22551499 = v4sfl (124.22551499f); 369 | const v4sf c_1_498030302 = v4sfl (1.498030302f); 370 | const v4sf c_1_725877999 = v4sfl (1.72587999f); 371 | const v4sf c_0_3520087068 = v4sfl (0.3520887068f); 372 | 373 | return y - c_124_22551499 374 | - c_1_498030302 * mx.f 375 | - c_1_725877999 / (c_0_3520087068 + mx.f); 376 | } 377 | 378 | static inline v4sf 379 | vfastlog (v4sf x) 380 | { 381 | const v4sf c_0_69314718 = v4sfl (0.69314718f); 382 | 383 | return c_0_69314718 * vfastlog2 (x); 384 | } 385 | 386 | static inline v4sf 387 | vfasterlog2 (v4sf x) 388 | { 389 | union { v4sf f; v4si i; } vx = { x }; 390 | v4sf y = v4si_to_v4sf (vx.i); 391 | y *= v4sfl (1.1920928955078125e-7f); 392 | 393 | const v4sf c_126_94269504 = v4sfl (126.94269504f); 394 | 395 | return y - c_126_94269504; 396 | } 397 | 398 | static inline v4sf 399 | vfasterlog (v4sf x) 400 | { 401 | // const v4sf c_0_69314718 = v4sfl (0.69314718f); 402 | // 403 | // return c_0_69314718 * vfasterlog2 (x); 404 | 405 | union { v4sf f; v4si i; } vx = { x }; 406 | v4sf y = v4si_to_v4sf (vx.i); 407 | y *= v4sfl (8.2629582881927490e-8f); 408 | 409 | const v4sf c_87_989971088 = v4sfl (87.989971088f); 410 | 411 | return y - c_87_989971088; 412 | } 413 | 414 | #endif // __SSE2__ 415 | 416 | #endif // __FAST_LOG_H_ 417 | /*=====================================================================* 418 | * Copyright (C) 2011 Paul Mineiro * 419 | * All rights reserved. * 420 | * * 421 | * Redistribution and use in source and binary forms, with * 422 | * or without modification, are permitted provided that the * 423 | * following conditions are met: * 424 | * * 425 | * * Redistributions of source code must retain the * 426 | * above copyright notice, this list of conditions and * 427 | * the following disclaimer. * 428 | * * 429 | * * Redistributions in binary form must reproduce the * 430 | * above copyright notice, this list of conditions and * 431 | * the following disclaimer in the documentation and/or * 432 | * other materials provided with the distribution. * 433 | * * 434 | * * Neither the name of Paul Mineiro nor the names * 435 | * of other contributors may be used to endorse or promote * 436 | * products derived from this software without specific * 437 | * prior written permission. * 438 | * * 439 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 440 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 441 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 442 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 443 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 444 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 445 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 446 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 447 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 448 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 449 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 450 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 451 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 452 | * POSSIBILITY OF SUCH DAMAGE. * 453 | * * 454 | * Contact: Paul Mineiro * 455 | *=====================================================================*/ 456 | 457 | #ifndef __FAST_ERF_H_ 458 | #define __FAST_ERF_H_ 459 | 460 | #include 461 | #include 462 | 463 | // fasterfc: not actually faster than erfcf(3) on newer machines! 464 | // ... although vectorized version is interesting 465 | // and fastererfc is very fast 466 | 467 | static inline float 468 | fasterfc (float x) 469 | { 470 | static const float k = 3.3509633149424609f; 471 | static const float a = 0.07219054755431126f; 472 | static const float b = 15.418191568719577f; 473 | static const float c = 5.609846028328545f; 474 | 475 | union { float f; uint32_t i; } vc = { c * x }; 476 | float xsq = x * x; 477 | float xquad = xsq * xsq; 478 | 479 | vc.i |= 0x80000000; 480 | 481 | return 2.0f / (1.0f + fastpow2 (k * x)) - a * x * (b * xquad - 1.0f) * fasterpow2 (vc.f); 482 | } 483 | 484 | static inline float 485 | fastererfc (float x) 486 | { 487 | static const float k = 3.3509633149424609f; 488 | 489 | return 2.0f / (1.0f + fasterpow2 (k * x)); 490 | } 491 | 492 | // fasterf: not actually faster than erff(3) on newer machines! 493 | // ... although vectorized version is interesting 494 | // and fastererf is very fast 495 | 496 | static inline float 497 | fasterf (float x) 498 | { 499 | return 1.0f - fasterfc (x); 500 | } 501 | 502 | static inline float 503 | fastererf (float x) 504 | { 505 | return 1.0f - fastererfc (x); 506 | } 507 | 508 | static inline float 509 | fastinverseerf (float x) 510 | { 511 | static const float invk = 0.30004578719350504f; 512 | static const float a = 0.020287853348211326f; 513 | static const float b = 0.07236892874789555f; 514 | static const float c = 0.9913030456864257f; 515 | static const float d = 0.8059775923760193f; 516 | 517 | float xsq = x * x; 518 | 519 | return invk * fastlog2 ((1.0f + x) / (1.0f - x)) 520 | + x * (a - b * xsq) / (c - d * xsq); 521 | } 522 | 523 | static inline float 524 | fasterinverseerf (float x) 525 | { 526 | static const float invk = 0.30004578719350504f; 527 | 528 | return invk * fasterlog2 ((1.0f + x) / (1.0f - x)); 529 | } 530 | 531 | #if defined(__SSE2__) && ! defined(__ICL) 532 | 533 | static inline v4sf 534 | vfasterfc (v4sf x) 535 | { 536 | const v4sf k = v4sfl (3.3509633149424609f); 537 | const v4sf a = v4sfl (0.07219054755431126f); 538 | const v4sf b = v4sfl (15.418191568719577f); 539 | const v4sf c = v4sfl (5.609846028328545f); 540 | 541 | union { v4sf f; v4si i; } vc; vc.f = c * x; 542 | vc.i |= v4sil (0x80000000); 543 | 544 | v4sf xsq = x * x; 545 | v4sf xquad = xsq * xsq; 546 | 547 | return v4sfl (2.0f) / (v4sfl (1.0f) + vfastpow2 (k * x)) - a * x * (b * xquad - v4sfl (1.0f)) * vfasterpow2 (vc.f); 548 | } 549 | 550 | static inline v4sf 551 | vfastererfc (const v4sf x) 552 | { 553 | const v4sf k = v4sfl (3.3509633149424609f); 554 | 555 | return v4sfl (2.0f) / (v4sfl (1.0f) + vfasterpow2 (k * x)); 556 | } 557 | 558 | static inline v4sf 559 | vfasterf (v4sf x) 560 | { 561 | return v4sfl (1.0f) - vfasterfc (x); 562 | } 563 | 564 | static inline v4sf 565 | vfastererf (const v4sf x) 566 | { 567 | return v4sfl (1.0f) - vfastererfc (x); 568 | } 569 | 570 | static inline v4sf 571 | vfastinverseerf (v4sf x) 572 | { 573 | const v4sf invk = v4sfl (0.30004578719350504f); 574 | const v4sf a = v4sfl (0.020287853348211326f); 575 | const v4sf b = v4sfl (0.07236892874789555f); 576 | const v4sf c = v4sfl (0.9913030456864257f); 577 | const v4sf d = v4sfl (0.8059775923760193f); 578 | 579 | v4sf xsq = x * x; 580 | 581 | return invk * vfastlog2 ((v4sfl (1.0f) + x) / (v4sfl (1.0f) - x)) 582 | + x * (a - b * xsq) / (c - d * xsq); 583 | } 584 | 585 | static inline v4sf 586 | vfasterinverseerf (v4sf x) 587 | { 588 | const v4sf invk = v4sfl (0.30004578719350504f); 589 | 590 | return invk * vfasterlog2 ((v4sfl (1.0f) + x) / (v4sfl (1.0f) - x)); 591 | } 592 | 593 | #endif //__SSE2__ 594 | 595 | #endif // __FAST_ERF_H_ 596 | /*=====================================================================* 597 | * Copyright (C) 2011 Paul Mineiro * 598 | * All rights reserved. * 599 | * * 600 | * Redistribution and use in source and binary forms, with * 601 | * or without modification, are permitted provided that the * 602 | * following conditions are met: * 603 | * * 604 | * * Redistributions of source code must retain the * 605 | * above copyright notice, this list of conditions and * 606 | * the following disclaimer. * 607 | * * 608 | * * Redistributions in binary form must reproduce the * 609 | * above copyright notice, this list of conditions and * 610 | * the following disclaimer in the documentation and/or * 611 | * other materials provided with the distribution. * 612 | * * 613 | * * Neither the name of Paul Mineiro nor the names * 614 | * of other contributors may be used to endorse or promote * 615 | * products derived from this software without specific * 616 | * prior written permission. * 617 | * * 618 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 619 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 620 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 621 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 622 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 623 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 624 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 625 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 626 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 627 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 628 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 629 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 630 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 631 | * POSSIBILITY OF SUCH DAMAGE. * 632 | * * 633 | * Contact: Paul Mineiro * 634 | *=====================================================================*/ 635 | 636 | #ifndef __FAST_GAMMA_H_ 637 | #define __FAST_GAMMA_H_ 638 | 639 | #include 640 | 641 | /* gamma/digamma functions only work for positive inputs */ 642 | 643 | static inline float 644 | fastlgamma (float x) 645 | { 646 | float logterm = fastlog (x * (1.0f + x) * (2.0f + x)); 647 | float xp3 = 3.0f + x; 648 | 649 | return - 2.081061466f 650 | - x 651 | + 0.0833333f / xp3 652 | - logterm 653 | + (2.5f + x) * fastlog (xp3); 654 | } 655 | 656 | static inline float 657 | fasterlgamma (float x) 658 | { 659 | return - 0.0810614667f 660 | - x 661 | - fasterlog (x) 662 | + (0.5f + x) * fasterlog (1.0f + x); 663 | } 664 | 665 | static inline float 666 | fastdigamma (float x) 667 | { 668 | float twopx = 2.0f + x; 669 | float logterm = fastlog (twopx); 670 | 671 | return (-48.0f + x * (-157.0f + x * (-127.0f - 30.0f * x))) / 672 | (12.0f * x * (1.0f + x) * twopx * twopx) 673 | + logterm; 674 | } 675 | 676 | static inline float 677 | fasterdigamma (float x) 678 | { 679 | float onepx = 1.0f + x; 680 | 681 | return -1.0f / x - 1.0f / (2 * onepx) + fasterlog (onepx); 682 | } 683 | 684 | #if defined(__SSE2__) && ! defined(__ICL) 685 | 686 | static inline v4sf 687 | vfastlgamma (v4sf x) 688 | { 689 | const v4sf c_1_0 = v4sfl (1.0f); 690 | const v4sf c_2_0 = v4sfl (2.0f); 691 | const v4sf c_3_0 = v4sfl (3.0f); 692 | const v4sf c_2_081061466 = v4sfl (2.081061466f); 693 | const v4sf c_0_0833333 = v4sfl (0.0833333f); 694 | const v4sf c_2_5 = v4sfl (2.5f); 695 | 696 | v4sf logterm = vfastlog (x * (c_1_0 + x) * (c_2_0 + x)); 697 | v4sf xp3 = c_3_0 + x; 698 | 699 | return - c_2_081061466 700 | - x 701 | + c_0_0833333 / xp3 702 | - logterm 703 | + (c_2_5 + x) * vfastlog (xp3); 704 | } 705 | 706 | static inline v4sf 707 | vfasterlgamma (v4sf x) 708 | { 709 | const v4sf c_0_0810614667 = v4sfl (0.0810614667f); 710 | const v4sf c_0_5 = v4sfl (0.5f); 711 | const v4sf c_1 = v4sfl (1.0f); 712 | 713 | return - c_0_0810614667 714 | - x 715 | - vfasterlog (x) 716 | + (c_0_5 + x) * vfasterlog (c_1 + x); 717 | } 718 | 719 | static inline v4sf 720 | vfastdigamma (v4sf x) 721 | { 722 | v4sf twopx = v4sfl (2.0f) + x; 723 | v4sf logterm = vfastlog (twopx); 724 | 725 | return (v4sfl (-48.0f) + x * (v4sfl (-157.0f) + x * (v4sfl (-127.0f) - v4sfl (30.0f) * x))) / 726 | (v4sfl (12.0f) * x * (v4sfl (1.0f) + x) * twopx * twopx) 727 | + logterm; 728 | } 729 | 730 | static inline v4sf 731 | vfasterdigamma (v4sf x) 732 | { 733 | const v4sf c_1_0 = v4sfl (1.0f); 734 | const v4sf c_2_0 = v4sfl (2.0f); 735 | v4sf onepx = c_1_0 + x; 736 | 737 | return -c_1_0 / x - c_1_0 / (c_2_0 * onepx) + vfasterlog (onepx); 738 | } 739 | 740 | #endif //__SSE2__ 741 | 742 | #endif // __FAST_GAMMA_H_ 743 | /*=====================================================================* 744 | * Copyright (C) 2011 Paul Mineiro * 745 | * All rights reserved. * 746 | * * 747 | * Redistribution and use in source and binary forms, with * 748 | * or without modification, are permitted provided that the * 749 | * following conditions are met: * 750 | * * 751 | * * Redistributions of source code must retain the * 752 | * above copyright notice, this list of conditions and * 753 | * the following disclaimer. * 754 | * * 755 | * * Redistributions in binary form must reproduce the * 756 | * above copyright notice, this list of conditions and * 757 | * the following disclaimer in the documentation and/or * 758 | * other materials provided with the distribution. * 759 | * * 760 | * * Neither the name of Paul Mineiro nor the names * 761 | * of other contributors may be used to endorse or promote * 762 | * products derived from this software without specific * 763 | * prior written permission. * 764 | * * 765 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 766 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 767 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 768 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 769 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 770 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 771 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 772 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 773 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 774 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 775 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 776 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 777 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 778 | * POSSIBILITY OF SUCH DAMAGE. * 779 | * * 780 | * Contact: Paul Mineiro * 781 | *=====================================================================*/ 782 | 783 | #ifndef __FAST_HYPERBOLIC_H_ 784 | #define __FAST_HYPERBOLIC_H_ 785 | 786 | #include 787 | 788 | static inline float 789 | fastsinh (float p) 790 | { 791 | return 0.5f * (fastexp (p) - fastexp (-p)); 792 | } 793 | 794 | static inline float 795 | fastersinh (float p) 796 | { 797 | return 0.5f * (fasterexp (p) - fasterexp (-p)); 798 | } 799 | 800 | static inline float 801 | fastcosh (float p) 802 | { 803 | return 0.5f * (fastexp (p) + fastexp (-p)); 804 | } 805 | 806 | static inline float 807 | fastercosh (float p) 808 | { 809 | return 0.5f * (fasterexp (p) + fasterexp (-p)); 810 | } 811 | 812 | static inline float 813 | fasttanh (float p) 814 | { 815 | return -1.0f + 2.0f / (1.0f + fastexp (-2.0f * p)); 816 | } 817 | 818 | static inline float 819 | fastertanh (float p) 820 | { 821 | return -1.0f + 2.0f / (1.0f + fasterexp (-2.0f * p)); 822 | } 823 | 824 | #if defined(__SSE2__) && ! defined(__ICL) 825 | 826 | static inline v4sf 827 | vfastsinh (const v4sf p) 828 | { 829 | const v4sf c_0_5 = v4sfl (0.5f); 830 | 831 | return c_0_5 * (vfastexp (p) - vfastexp (-p)); 832 | } 833 | 834 | static inline v4sf 835 | vfastersinh (const v4sf p) 836 | { 837 | const v4sf c_0_5 = v4sfl (0.5f); 838 | 839 | return c_0_5 * (vfasterexp (p) - vfasterexp (-p)); 840 | } 841 | 842 | static inline v4sf 843 | vfastcosh (const v4sf p) 844 | { 845 | const v4sf c_0_5 = v4sfl (0.5f); 846 | 847 | return c_0_5 * (vfastexp (p) + vfastexp (-p)); 848 | } 849 | 850 | static inline v4sf 851 | vfastercosh (const v4sf p) 852 | { 853 | const v4sf c_0_5 = v4sfl (0.5f); 854 | 855 | return c_0_5 * (vfasterexp (p) + vfasterexp (-p)); 856 | } 857 | 858 | static inline v4sf 859 | vfasttanh (const v4sf p) 860 | { 861 | const v4sf c_1 = v4sfl (1.0f); 862 | const v4sf c_2 = v4sfl (2.0f); 863 | 864 | return -c_1 + c_2 / (c_1 + vfastexp (-c_2 * p)); 865 | } 866 | 867 | static inline v4sf 868 | vfastertanh (const v4sf p) 869 | { 870 | const v4sf c_1 = v4sfl (1.0f); 871 | const v4sf c_2 = v4sfl (2.0f); 872 | 873 | return -c_1 + c_2 / (c_1 + vfasterexp (-c_2 * p)); 874 | } 875 | 876 | #endif //__SSE2__ 877 | 878 | #endif // __FAST_HYPERBOLIC_H_ 879 | /*=====================================================================* 880 | * Copyright (C) 2011 Paul Mineiro * 881 | * All rights reserved. * 882 | * * 883 | * Redistribution and use in source and binary forms, with * 884 | * or without modification, are permitted provided that the * 885 | * following conditions are met: * 886 | * * 887 | * * Redistributions of source code must retain the * 888 | * above copyright notice, this list of conditions and * 889 | * the following disclaimer. * 890 | * * 891 | * * Redistributions in binary form must reproduce the * 892 | * above copyright notice, this list of conditions and * 893 | * the following disclaimer in the documentation and/or * 894 | * other materials provided with the distribution. * 895 | * * 896 | * * Neither the name of Paul Mineiro nor the names * 897 | * of other contributors may be used to endorse or promote * 898 | * products derived from this software without specific * 899 | * prior written permission. * 900 | * * 901 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 902 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 903 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 904 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 905 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 906 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 907 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 908 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 909 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 910 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 911 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 912 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 913 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 914 | * POSSIBILITY OF SUCH DAMAGE. * 915 | * * 916 | * Contact: Paul Mineiro * 917 | *=====================================================================*/ 918 | 919 | #ifndef __FAST_LAMBERT_W_H_ 920 | #define __FAST_LAMBERT_W_H_ 921 | 922 | #include 923 | 924 | // these functions compute the upper branch aka W_0 925 | 926 | static inline float 927 | fastlambertw (float x) 928 | { 929 | static const float threshold = 2.26445f; 930 | 931 | float c = (x < threshold) ? 1.546865557f : 1.0f; 932 | float d = (x < threshold) ? 2.250366841f : 0.0f; 933 | float a = (x < threshold) ? -0.737769969f : 0.0f; 934 | 935 | float logterm = fastlog (c * x + d); 936 | float loglogterm = fastlog (logterm); 937 | 938 | float minusw = -a - logterm + loglogterm - loglogterm / logterm; 939 | float expminusw = fastexp (minusw); 940 | float xexpminusw = x * expminusw; 941 | float pexpminusw = xexpminusw - minusw; 942 | 943 | return (2.0f * xexpminusw - minusw * (4.0f * xexpminusw - minusw * pexpminusw)) / 944 | (2.0f + pexpminusw * (2.0f - minusw)); 945 | } 946 | 947 | static inline float 948 | fasterlambertw (float x) 949 | { 950 | static const float threshold = 2.26445f; 951 | 952 | float c = (x < threshold) ? 1.546865557f : 1.0f; 953 | float d = (x < threshold) ? 2.250366841f : 0.0f; 954 | float a = (x < threshold) ? -0.737769969f : 0.0f; 955 | 956 | float logterm = fasterlog (c * x + d); 957 | float loglogterm = fasterlog (logterm); 958 | 959 | float w = a + logterm - loglogterm + loglogterm / logterm; 960 | float expw = fasterexp (-w); 961 | 962 | return (w * w + expw * x) / (1.0f + w); 963 | } 964 | 965 | static inline float 966 | fastlambertwexpx (float x) 967 | { 968 | static const float k = 1.1765631309f; 969 | static const float a = 0.94537622168f; 970 | 971 | float logarg = fmaxf (x, k); 972 | float powarg = (x < k) ? a * (x - k) : 0; 973 | 974 | float logterm = fastlog (logarg); 975 | float powterm = fasterpow2 (powarg); // don't need accuracy here 976 | 977 | float w = powterm * (logarg - logterm + logterm / logarg); 978 | float logw = fastlog (w); 979 | float p = x - logw; 980 | 981 | return w * (2.0f + p + w * (3.0f + 2.0f * p)) / 982 | (2.0f - p + w * (5.0f + 2.0f * w)); 983 | } 984 | 985 | static inline float 986 | fasterlambertwexpx (float x) 987 | { 988 | static const float k = 1.1765631309f; 989 | static const float a = 0.94537622168f; 990 | 991 | float logarg = fmaxf (x, k); 992 | float powarg = (x < k) ? a * (x - k) : 0; 993 | 994 | float logterm = fasterlog (logarg); 995 | float powterm = fasterpow2 (powarg); 996 | 997 | float w = powterm * (logarg - logterm + logterm / logarg); 998 | float logw = fasterlog (w); 999 | 1000 | return w * (1.0f + x - logw) / (1.0f + w); 1001 | } 1002 | 1003 | #if defined(__SSE2__) && ! defined(__ICL) 1004 | 1005 | static inline v4sf 1006 | vfastlambertw (v4sf x) 1007 | { 1008 | const v4sf threshold = v4sfl (2.26445f); 1009 | 1010 | v4sf under = _mm_cmplt_ps (x, threshold); 1011 | v4sf c = _mm_or_ps (_mm_and_ps (under, v4sfl (1.546865557f)), 1012 | _mm_andnot_ps (under, v4sfl (1.0f))); 1013 | v4sf d = _mm_and_ps (under, v4sfl (2.250366841f)); 1014 | v4sf a = _mm_and_ps (under, v4sfl (-0.737769969f)); 1015 | 1016 | v4sf logterm = vfastlog (c * x + d); 1017 | v4sf loglogterm = vfastlog (logterm); 1018 | 1019 | v4sf minusw = -a - logterm + loglogterm - loglogterm / logterm; 1020 | v4sf expminusw = vfastexp (minusw); 1021 | v4sf xexpminusw = x * expminusw; 1022 | v4sf pexpminusw = xexpminusw - minusw; 1023 | 1024 | return (v4sfl (2.0f) * xexpminusw - minusw * (v4sfl (4.0f) * xexpminusw - minusw * pexpminusw)) / 1025 | (v4sfl (2.0f) + pexpminusw * (v4sfl (2.0f) - minusw)); 1026 | } 1027 | 1028 | static inline v4sf 1029 | vfasterlambertw (v4sf x) 1030 | { 1031 | const v4sf threshold = v4sfl (2.26445f); 1032 | 1033 | v4sf under = _mm_cmplt_ps (x, threshold); 1034 | v4sf c = _mm_or_ps (_mm_and_ps (under, v4sfl (1.546865557f)), 1035 | _mm_andnot_ps (under, v4sfl (1.0f))); 1036 | v4sf d = _mm_and_ps (under, v4sfl (2.250366841f)); 1037 | v4sf a = _mm_and_ps (under, v4sfl (-0.737769969f)); 1038 | 1039 | v4sf logterm = vfasterlog (c * x + d); 1040 | v4sf loglogterm = vfasterlog (logterm); 1041 | 1042 | v4sf w = a + logterm - loglogterm + loglogterm / logterm; 1043 | v4sf expw = vfasterexp (-w); 1044 | 1045 | return (w * w + expw * x) / (v4sfl (1.0f) + w); 1046 | } 1047 | 1048 | static inline v4sf 1049 | vfastlambertwexpx (v4sf x) 1050 | { 1051 | const v4sf k = v4sfl (1.1765631309f); 1052 | const v4sf a = v4sfl (0.94537622168f); 1053 | const v4sf two = v4sfl (2.0f); 1054 | const v4sf three = v4sfl (3.0f); 1055 | const v4sf five = v4sfl (5.0f); 1056 | 1057 | v4sf logarg = _mm_max_ps (x, k); 1058 | v4sf powarg = _mm_and_ps (_mm_cmplt_ps (x, k), a * (x - k)); 1059 | 1060 | v4sf logterm = vfastlog (logarg); 1061 | v4sf powterm = vfasterpow2 (powarg); // don't need accuracy here 1062 | 1063 | v4sf w = powterm * (logarg - logterm + logterm / logarg); 1064 | v4sf logw = vfastlog (w); 1065 | v4sf p = x - logw; 1066 | 1067 | return w * (two + p + w * (three + two * p)) / 1068 | (two - p + w * (five + two * w)); 1069 | } 1070 | 1071 | static inline v4sf 1072 | vfasterlambertwexpx (v4sf x) 1073 | { 1074 | const v4sf k = v4sfl (1.1765631309f); 1075 | const v4sf a = v4sfl (0.94537622168f); 1076 | 1077 | v4sf logarg = _mm_max_ps (x, k); 1078 | v4sf powarg = _mm_and_ps (_mm_cmplt_ps (x, k), a * (x - k)); 1079 | 1080 | v4sf logterm = vfasterlog (logarg); 1081 | v4sf powterm = vfasterpow2 (powarg); 1082 | 1083 | v4sf w = powterm * (logarg - logterm + logterm / logarg); 1084 | v4sf logw = vfasterlog (w); 1085 | 1086 | return w * (v4sfl (1.0f) + x - logw) / (v4sfl (1.0f) + w); 1087 | } 1088 | 1089 | #endif // __SSE2__ 1090 | 1091 | #endif // __FAST_LAMBERT_W_H_ 1092 | 1093 | /*=====================================================================* 1094 | * Copyright (C) 2011 Paul Mineiro * 1095 | * All rights reserved. * 1096 | * * 1097 | * Redistribution and use in source and binary forms, with * 1098 | * or without modification, are permitted provided that the * 1099 | * following conditions are met: * 1100 | * * 1101 | * * Redistributions of source code must retain the * 1102 | * above copyright notice, this list of conditions and * 1103 | * the following disclaimer. * 1104 | * * 1105 | * * Redistributions in binary form must reproduce the * 1106 | * above copyright notice, this list of conditions and * 1107 | * the following disclaimer in the documentation and/or * 1108 | * other materials provided with the distribution. * 1109 | * * 1110 | * * Neither the name of Paul Mineiro nor the names * 1111 | * of other contributors may be used to endorse or promote * 1112 | * products derived from this software without specific * 1113 | * prior written permission. * 1114 | * * 1115 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 1116 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 1117 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 1118 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 1119 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 1120 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 1121 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 1122 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 1123 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 1124 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 1125 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 1126 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 1127 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 1128 | * POSSIBILITY OF SUCH DAMAGE. * 1129 | * * 1130 | * Contact: Paul Mineiro * 1131 | *=====================================================================*/ 1132 | 1133 | #ifndef __FAST_POW_H_ 1134 | #define __FAST_POW_H_ 1135 | 1136 | #include 1137 | 1138 | static inline float 1139 | fastpow (float x, 1140 | float p) 1141 | { 1142 | return fastpow2 (p * fastlog2 (x)); 1143 | } 1144 | 1145 | static inline float 1146 | fasterpow (float x, 1147 | float p) 1148 | { 1149 | return fasterpow2 (p * fasterlog2 (x)); 1150 | } 1151 | 1152 | #if defined(__SSE2__) && ! defined(__ICL) 1153 | 1154 | static inline v4sf 1155 | vfastpow (const v4sf x, 1156 | const v4sf p) 1157 | { 1158 | return vfastpow2 (p * vfastlog2 (x)); 1159 | } 1160 | 1161 | static inline v4sf 1162 | vfasterpow (const v4sf x, 1163 | const v4sf p) 1164 | { 1165 | return vfasterpow2 (p * vfasterlog2 (x)); 1166 | } 1167 | 1168 | #endif //__SSE2__ 1169 | 1170 | #endif // __FAST_POW_H_ 1171 | /*=====================================================================* 1172 | * Copyright (C) 2011 Paul Mineiro * 1173 | * All rights reserved. * 1174 | * * 1175 | * Redistribution and use in source and binary forms, with * 1176 | * or without modification, are permitted provided that the * 1177 | * following conditions are met: * 1178 | * * 1179 | * * Redistributions of source code must retain the * 1180 | * above copyright notice, this list of conditions and * 1181 | * the following disclaimer. * 1182 | * * 1183 | * * Redistributions in binary form must reproduce the * 1184 | * above copyright notice, this list of conditions and * 1185 | * the following disclaimer in the documentation and/or * 1186 | * other materials provided with the distribution. * 1187 | * * 1188 | * * Neither the name of Paul Mineiro nor the names * 1189 | * of other contributors may be used to endorse or promote * 1190 | * products derived from this software without specific * 1191 | * prior written permission. * 1192 | * * 1193 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 1194 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 1195 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 1196 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 1197 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 1198 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 1199 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 1200 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 1201 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 1202 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 1203 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 1204 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 1205 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 1206 | * POSSIBILITY OF SUCH DAMAGE. * 1207 | * * 1208 | * Contact: Paul Mineiro * 1209 | *=====================================================================*/ 1210 | 1211 | #ifndef __FAST_SIGMOID_H_ 1212 | #define __FAST_SIGMOID_H_ 1213 | 1214 | #include 1215 | 1216 | static inline float 1217 | fastsigmoid (float x) 1218 | { 1219 | return 1.0f / (1.0f + fastexp (-x)); 1220 | } 1221 | 1222 | static inline float 1223 | fastersigmoid (float x) 1224 | { 1225 | return 1.0f / (1.0f + fasterexp (-x)); 1226 | } 1227 | 1228 | #if defined(__SSE2__) && ! defined(__ICL) 1229 | 1230 | static inline v4sf 1231 | vfastsigmoid (const v4sf x) 1232 | { 1233 | const v4sf c_1 = v4sfl (1.0f); 1234 | 1235 | return c_1 / (c_1 + vfastexp (-x)); 1236 | } 1237 | 1238 | static inline v4sf 1239 | vfastersigmoid (const v4sf x) 1240 | { 1241 | const v4sf c_1 = v4sfl (1.0f); 1242 | 1243 | return c_1 / (c_1 + vfasterexp (-x)); 1244 | } 1245 | 1246 | #endif //__SSE2__ 1247 | 1248 | #endif // __FAST_SIGMOID_H_ 1249 | /*=====================================================================* 1250 | * Copyright (C) 2011 Paul Mineiro * 1251 | * All rights reserved. * 1252 | * * 1253 | * Redistribution and use in source and binary forms, with * 1254 | * or without modification, are permitted provided that the * 1255 | * following conditions are met: * 1256 | * * 1257 | * * Redistributions of source code must retain the * 1258 | * above copyright notice, this list of conditions and * 1259 | * the following disclaimer. * 1260 | * * 1261 | * * Redistributions in binary form must reproduce the * 1262 | * above copyright notice, this list of conditions and * 1263 | * the following disclaimer in the documentation and/or * 1264 | * other materials provided with the distribution. * 1265 | * * 1266 | * * Neither the name of Paul Mineiro nor the names * 1267 | * of other contributors may be used to endorse or promote * 1268 | * products derived from this software without specific * 1269 | * prior written permission. * 1270 | * * 1271 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 1272 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 1273 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * 1274 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 1275 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER * 1276 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * 1277 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 1278 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * 1279 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * 1280 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 1281 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 1282 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 1283 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 1284 | * POSSIBILITY OF SUCH DAMAGE. * 1285 | * * 1286 | * Contact: Paul Mineiro * 1287 | *=====================================================================*/ 1288 | 1289 | #ifndef __FAST_TRIG_H_ 1290 | #define __FAST_TRIG_H_ 1291 | 1292 | #include 1293 | 1294 | // http://www.devmaster.net/forums/showthread.php?t=5784 1295 | // fast sine variants are for x \in [ -\pi, pi ] 1296 | // fast cosine variants are for x \in [ -\pi, pi ] 1297 | // fast tangent variants are for x \in [ -\pi / 2, pi / 2 ] 1298 | // "full" versions of functions handle the entire range of inputs 1299 | // although the range reduction technique used here will be hopelessly 1300 | // inaccurate for |x| >> 1000 1301 | // 1302 | // WARNING: fastsinfull, fastcosfull, and fasttanfull can be slower than 1303 | // libc calls on older machines (!) and on newer machines are only 1304 | // slighly faster. however: 1305 | // * vectorized versions are competitive 1306 | // * faster full versions are competitive 1307 | 1308 | static inline float 1309 | fastsin (float x) 1310 | { 1311 | static const float fouroverpi = 1.2732395447351627f; 1312 | static const float fouroverpisq = 0.40528473456935109f; 1313 | static const float q = 0.78444488374548933f; 1314 | union { float f; uint32_t i; } p = { 0.20363937680730309f }; 1315 | union { float f; uint32_t i; } r = { 0.015124940802184233f }; 1316 | union { float f; uint32_t i; } s = { -0.0032225901625579573f }; 1317 | 1318 | union { float f; uint32_t i; } vx = { x }; 1319 | uint32_t sign = vx.i & 0x80000000; 1320 | vx.i = vx.i & 0x7FFFFFFF; 1321 | 1322 | float qpprox = fouroverpi * x - fouroverpisq * x * vx.f; 1323 | float qpproxsq = qpprox * qpprox; 1324 | 1325 | p.i |= sign; 1326 | r.i |= sign; 1327 | s.i ^= sign; 1328 | 1329 | return q * qpprox + qpproxsq * (p.f + qpproxsq * (r.f + qpproxsq * s.f)); 1330 | } 1331 | 1332 | static inline float 1333 | fastersin (float x) 1334 | { 1335 | static const float fouroverpi = 1.2732395447351627f; 1336 | static const float fouroverpisq = 0.40528473456935109f; 1337 | static const float q = 0.77633023248007499f; 1338 | union { float f; uint32_t i; } p = { 0.22308510060189463f }; 1339 | 1340 | union { float f; uint32_t i; } vx = { x }; 1341 | uint32_t sign = vx.i & 0x80000000; 1342 | vx.i &= 0x7FFFFFFF; 1343 | 1344 | float qpprox = fouroverpi * x - fouroverpisq * x * vx.f; 1345 | 1346 | p.i |= sign; 1347 | 1348 | return qpprox * (q + p.f * qpprox); 1349 | } 1350 | 1351 | static inline float 1352 | fastsinfull (float x) 1353 | { 1354 | static const float twopi = 6.2831853071795865f; 1355 | static const float invtwopi = 0.15915494309189534f; 1356 | 1357 | int k = x * invtwopi; 1358 | float half = (x < 0) ? -0.5f : 0.5f; 1359 | return fastsin ((half + k) * twopi - x); 1360 | } 1361 | 1362 | static inline float 1363 | fastersinfull (float x) 1364 | { 1365 | static const float twopi = 6.2831853071795865f; 1366 | static const float invtwopi = 0.15915494309189534f; 1367 | 1368 | int k = x * invtwopi; 1369 | float half = (x < 0) ? -0.5f : 0.5f; 1370 | return fastersin ((half + k) * twopi - x); 1371 | } 1372 | 1373 | static inline float 1374 | fastcos (float x) 1375 | { 1376 | static const float halfpi = 1.5707963267948966f; 1377 | static const float halfpiminustwopi = -4.7123889803846899f; 1378 | float offset = (x > halfpi) ? halfpiminustwopi : halfpi; 1379 | return fastsin (x + offset); 1380 | } 1381 | 1382 | static inline float 1383 | fastercos (float x) 1384 | { 1385 | static const float twooverpi = 0.63661977236758134f; 1386 | static const float p = 0.54641335845679634f; 1387 | 1388 | union { float f; uint32_t i; } vx = { x }; 1389 | vx.i &= 0x7FFFFFFF; 1390 | 1391 | float qpprox = 1.0f - twooverpi * vx.f; 1392 | 1393 | return qpprox + p * qpprox * (1.0f - qpprox * qpprox); 1394 | } 1395 | 1396 | static inline float 1397 | fastcosfull (float x) 1398 | { 1399 | static const float halfpi = 1.5707963267948966f; 1400 | return fastsinfull (x + halfpi); 1401 | } 1402 | 1403 | static inline float 1404 | fastercosfull (float x) 1405 | { 1406 | static const float halfpi = 1.5707963267948966f; 1407 | return fastersinfull (x + halfpi); 1408 | } 1409 | 1410 | static inline float 1411 | fasttan (float x) 1412 | { 1413 | static const float halfpi = 1.5707963267948966f; 1414 | return fastsin (x) / fastsin (x + halfpi); 1415 | } 1416 | 1417 | static inline float 1418 | fastertan (float x) 1419 | { 1420 | return fastersin (x) / fastercos (x); 1421 | } 1422 | 1423 | static inline float 1424 | fasttanfull (float x) 1425 | { 1426 | static const float twopi = 6.2831853071795865f; 1427 | static const float invtwopi = 0.15915494309189534f; 1428 | 1429 | int k = x * invtwopi; 1430 | float half = (x < 0) ? -0.5f : 0.5f; 1431 | float xnew = x - (half + k) * twopi; 1432 | 1433 | return fastsin (xnew) / fastcos (xnew); 1434 | } 1435 | 1436 | static inline float 1437 | fastertanfull (float x) 1438 | { 1439 | static const float twopi = 6.2831853071795865f; 1440 | static const float invtwopi = 0.15915494309189534f; 1441 | 1442 | int k = x * invtwopi; 1443 | float half = (x < 0) ? -0.5f : 0.5f; 1444 | float xnew = x - (half + k) * twopi; 1445 | 1446 | return fastersin (xnew) / fastercos (xnew); 1447 | } 1448 | 1449 | #if defined(__SSE2__) && ! defined(__ICL) 1450 | 1451 | static inline v4sf 1452 | vfastsin (const v4sf x) 1453 | { 1454 | const v4sf fouroverpi = v4sfl (1.2732395447351627f); 1455 | const v4sf fouroverpisq = v4sfl (0.40528473456935109f); 1456 | const v4sf q = v4sfl (0.78444488374548933f); 1457 | const v4sf p = v4sfl (0.20363937680730309f); 1458 | const v4sf r = v4sfl (0.015124940802184233f); 1459 | const v4sf s = v4sfl (-0.0032225901625579573f); 1460 | 1461 | union { v4sf f; v4si i; } vx = { x }; 1462 | v4si sign = vx.i & v4sil (0x80000000); 1463 | vx.i &= v4sil (0x7FFFFFFF); 1464 | 1465 | v4sf qpprox = fouroverpi * x - fouroverpisq * x * vx.f; 1466 | v4sf qpproxsq = qpprox * qpprox; 1467 | union { v4sf f; v4si i; } vy; vy.f = qpproxsq * (p + qpproxsq * (r + qpproxsq * s)); 1468 | vy.i ^= sign; 1469 | 1470 | return q * qpprox + vy.f; 1471 | } 1472 | 1473 | static inline v4sf 1474 | vfastersin (const v4sf x) 1475 | { 1476 | const v4sf fouroverpi = v4sfl (1.2732395447351627f); 1477 | const v4sf fouroverpisq = v4sfl (0.40528473456935109f); 1478 | const v4sf q = v4sfl (0.77633023248007499f); 1479 | const v4sf plit = v4sfl (0.22308510060189463f); 1480 | union { v4sf f; v4si i; } p = { plit }; 1481 | 1482 | union { v4sf f; v4si i; } vx = { x }; 1483 | v4si sign = vx.i & v4sil (0x80000000); 1484 | vx.i &= v4sil (0x7FFFFFFF); 1485 | 1486 | v4sf qpprox = fouroverpi * x - fouroverpisq * x * vx.f; 1487 | 1488 | p.i |= sign; 1489 | 1490 | return qpprox * (q + p.f * qpprox); 1491 | } 1492 | 1493 | static inline v4sf 1494 | vfastsinfull (const v4sf x) 1495 | { 1496 | const v4sf twopi = v4sfl (6.2831853071795865f); 1497 | const v4sf invtwopi = v4sfl (0.15915494309189534f); 1498 | 1499 | v4si k = v4sf_to_v4si (x * invtwopi); 1500 | 1501 | v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); 1502 | v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), 1503 | _mm_andnot_ps (ltzero, v4sfl (0.5f))); 1504 | 1505 | return vfastsin ((half + v4si_to_v4sf (k)) * twopi - x); 1506 | } 1507 | 1508 | static inline v4sf 1509 | vfastersinfull (const v4sf x) 1510 | { 1511 | const v4sf twopi = v4sfl (6.2831853071795865f); 1512 | const v4sf invtwopi = v4sfl (0.15915494309189534f); 1513 | 1514 | v4si k = v4sf_to_v4si (x * invtwopi); 1515 | 1516 | v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); 1517 | v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), 1518 | _mm_andnot_ps (ltzero, v4sfl (0.5f))); 1519 | 1520 | return vfastersin ((half + v4si_to_v4sf (k)) * twopi - x); 1521 | } 1522 | 1523 | static inline v4sf 1524 | vfastcos (const v4sf x) 1525 | { 1526 | const v4sf halfpi = v4sfl (1.5707963267948966f); 1527 | const v4sf halfpiminustwopi = v4sfl (-4.7123889803846899f); 1528 | v4sf lthalfpi = _mm_cmpnlt_ps (x, halfpi); 1529 | v4sf offset = _mm_or_ps (_mm_and_ps (lthalfpi, halfpiminustwopi), 1530 | _mm_andnot_ps (lthalfpi, halfpi)); 1531 | return vfastsin (x + offset); 1532 | } 1533 | 1534 | static inline v4sf 1535 | vfastercos (v4sf x) 1536 | { 1537 | const v4sf twooverpi = v4sfl (0.63661977236758134f); 1538 | const v4sf p = v4sfl (0.54641335845679634); 1539 | 1540 | v4sf vx = v4sf_fabs (x); 1541 | v4sf qpprox = v4sfl (1.0f) - twooverpi * vx; 1542 | 1543 | return qpprox + p * qpprox * (v4sfl (1.0f) - qpprox * qpprox); 1544 | } 1545 | 1546 | static inline v4sf 1547 | vfastcosfull (const v4sf x) 1548 | { 1549 | const v4sf halfpi = v4sfl (1.5707963267948966f); 1550 | return vfastsinfull (x + halfpi); 1551 | } 1552 | 1553 | static inline v4sf 1554 | vfastercosfull (const v4sf x) 1555 | { 1556 | const v4sf halfpi = v4sfl (1.5707963267948966f); 1557 | return vfastersinfull (x + halfpi); 1558 | } 1559 | 1560 | static inline v4sf 1561 | vfasttan (const v4sf x) 1562 | { 1563 | const v4sf halfpi = v4sfl (1.5707963267948966f); 1564 | return vfastsin (x) / vfastsin (x + halfpi); 1565 | } 1566 | 1567 | static inline v4sf 1568 | vfastertan (const v4sf x) 1569 | { 1570 | return vfastersin (x) / vfastercos (x); 1571 | } 1572 | 1573 | static inline v4sf 1574 | vfasttanfull (const v4sf x) 1575 | { 1576 | const v4sf twopi = v4sfl (6.2831853071795865f); 1577 | const v4sf invtwopi = v4sfl (0.15915494309189534f); 1578 | 1579 | v4si k = v4sf_to_v4si (x * invtwopi); 1580 | 1581 | v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); 1582 | v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), 1583 | _mm_andnot_ps (ltzero, v4sfl (0.5f))); 1584 | v4sf xnew = x - (half + v4si_to_v4sf (k)) * twopi; 1585 | 1586 | return vfastsin (xnew) / vfastcos (xnew); 1587 | } 1588 | 1589 | static inline v4sf 1590 | vfastertanfull (const v4sf x) 1591 | { 1592 | const v4sf twopi = v4sfl (6.2831853071795865f); 1593 | const v4sf invtwopi = v4sfl (0.15915494309189534f); 1594 | 1595 | v4si k = v4sf_to_v4si (x * invtwopi); 1596 | 1597 | v4sf ltzero = _mm_cmplt_ps (x, v4sfl (0.0f)); 1598 | v4sf half = _mm_or_ps (_mm_and_ps (ltzero, v4sfl (-0.5f)), 1599 | _mm_andnot_ps (ltzero, v4sfl (0.5f))); 1600 | v4sf xnew = x - (half + v4si_to_v4sf (k)) * twopi; 1601 | 1602 | return vfastersin (xnew) / vfastercos (xnew); 1603 | } 1604 | 1605 | #endif //__SSE2__ 1606 | 1607 | #endif // __FAST_TRIG_H_ 1608 | -------------------------------------------------------------------------------- /external/wavfile.c: -------------------------------------------------------------------------------- 1 | //----------------------------------------------------------------------------- 2 | // Copyright 2012-2015 Masanori Morise. All Rights Reserved. 3 | // Author: mmorise [at] yamanashi.ac.jp (Masanori Morise) 4 | // 5 | // Matlab functions implemented for WORLD 6 | // Since these functions are implemented as the same function of Matlab, 7 | // the source code does not follow the style guide (Names of variables 8 | // and functions). 9 | // Please see the reference of Matlab to show the usage of functions. 10 | // Caution: 11 | // Since these functions (wavread() and wavwrite()) are roughly implemented, 12 | // we recommend more suitable functions provided by other organizations. 13 | // 14 | // Modifications by k.hua.kanru [at] ieee.org (Kanru Hua) 15 | // Sept 17, 2015 16 | // Add IEEE Float and fmt extension support for wavread. 17 | // Add 8bit/24bit/32bit PCM support for wavwrite. 18 | // Sept 25, 2015 19 | // Add MinGW support. 20 | // Oct 31, 2016 21 | // Fix bug in detecting float point format. 22 | // Nov 1, 2016 23 | // Expose API operating directly on file pointers. 24 | //----------------------------------------------------------------------------- 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #define false 0 33 | #define true 1 34 | typedef int bool; 35 | 36 | static inline int MyMax(int x, int y) { 37 | return x > y ? x : y; 38 | } 39 | 40 | static inline int MyMin(int x, int y) { 41 | return x < y ? x : y; 42 | } 43 | 44 | #define FORMAT_PCM 1 45 | #define FORMAT_IEEE_FLOAT 3 46 | #define FORMAT_EXT 254 47 | 48 | //----------------------------------------------------------------------------- 49 | // GetParameters() extracts fp, nbit, wav_length from the .wav file 50 | // This function is only used in wavread(). 51 | //----------------------------------------------------------------------------- 52 | static bool GetParameters(FILE *fp, int *fs, int *nbit, int *wav_length, int *format_tag, int *nchannel) { 53 | unsigned char data_check[5] = {0}; 54 | char str_check[5] = {0}; 55 | str_check[4] = '\0'; 56 | int fmt_size = 16; 57 | 58 | fread(str_check, 1, 4, fp); // "RIFF" 59 | str_check[4] = '\0'; 60 | if (0 != strcmp(str_check, "RIFF")) { 61 | printf("RIFF error.\n"); 62 | return false; 63 | } 64 | fseek(fp, 4, SEEK_CUR); 65 | fread(str_check, 1, 4, fp); // "WAVE" 66 | if (0 != strcmp(str_check, "WAVE")) { 67 | printf("WAVE error.\n"); 68 | return false; 69 | } 70 | fread(str_check, 1, 4, fp); // "fmt " 71 | while(strcmp(str_check, "fmt ")) 72 | if(! fread(str_check, 1, 4, fp)) { 73 | printf("fmt error.\n"); 74 | return false; 75 | } 76 | fread(data_check, 1, 4, fp); // cksize: 16/18/40 0 0 0 77 | if (!((16 == data_check[0] || 18 == data_check[0] || 40 == data_check[0]) 78 | && 0 == data_check[1] && 0 == data_check[2] && 0 == data_check[3])) { 79 | printf("fmt (2) error.\n"); 80 | return false; 81 | } 82 | fmt_size = data_check[0]; 83 | fread(data_check, 1, 2, fp); // wFormatTag: 1(PCM)/3(IEEE Float) 0 | FE FF(extended format) 84 | if (!(((1 == data_check[0] || 3 == data_check[0]) && 0 == data_check[1]) || 85 | (254 == data_check[0] && 255 == data_check[1]))) { 86 | printf("Format ID error.\n"); 87 | return false; 88 | } 89 | if (1 == data_check[0]) 90 | *format_tag = FORMAT_PCM; 91 | else if (3 == data_check[0]) 92 | *format_tag = FORMAT_IEEE_FLOAT; 93 | else 94 | *format_tag = FORMAT_EXT; 95 | fread(data_check, 1, 2, fp); // nChannels: 1 0 96 | 97 | if (1 == data_check[0] && 0 == data_check[1]) 98 | *nchannel = 1; 99 | else 100 | if (2 == data_check[0] && 0 == data_check[1]) 101 | *nchannel = 2; 102 | else { 103 | printf("Unsupported channel number %d %d.\n", data_check[0], data_check[1]); 104 | return false; 105 | } 106 | 107 | fread(data_check, 1, 4, fp); // fs 108 | *fs = 0; 109 | for (int i = 3; i >= 0; --i) *fs = *fs * 256 + data_check[i]; 110 | fread(data_check, 1, 4, fp); // nAvgBytesPerSec 111 | fread(data_check, 1, 2, fp); // nBlockAlign 112 | int block_align = data_check[0] / (*nchannel); 113 | fread(data_check, 1, 2, fp); // nBitsPerSample 114 | *nbit = data_check[0]; 115 | 116 | if ((*nbit) / 8 != block_align) { 117 | printf("Unsupported block align.\n"); 118 | return false; 119 | } 120 | 121 | short int extsize = 0; 122 | if (fmt_size > 16) { 123 | fread(&extsize, 1, 2, fp); // cbSize 124 | if (0 != extsize && 22 != extsize) { 125 | printf("Unsupported extension.\n"); 126 | return false; 127 | } 128 | } 129 | if(extsize > 0) { 130 | char extension[22]; 131 | fread(extension, 1, extsize, fp); 132 | if (extension[6] == 1 && extension[7] == 0) 133 | *format_tag = FORMAT_EXT; 134 | else if (extension[6] == 3 && extension[7] == 0) 135 | *format_tag = FORMAT_IEEE_FLOAT; 136 | else { 137 | printf("Unsupported format.\n"); 138 | return false; 139 | } 140 | } 141 | 142 | // Skip until "data" is found. 2011/03/28 143 | while (0 != fread(str_check, 1, 1, fp)) { 144 | if (str_check[0] == 'd') { 145 | fread(&str_check[1], 1, 3, fp); 146 | if (0 != strcmp(str_check, "data")) { 147 | fseek(fp, -3, SEEK_CUR); 148 | } else { 149 | break; 150 | } 151 | } 152 | } 153 | if (0 != strcmp(str_check, "data")) { 154 | printf("data error.\n"); 155 | return false; 156 | } 157 | 158 | fread(data_check, 1, 4, fp); // "data" 159 | *wav_length = 0; 160 | for (int i = 3; i >= 0; --i) 161 | *wav_length = *wav_length * 256 + data_check[i]; 162 | *wav_length /= (*nbit / 8) * (*nchannel); 163 | return true; 164 | } 165 | 166 | void wavwrite_fp(FP_TYPE* x, int x_length, int fs, int nbit, FILE* fp) { 167 | if (nbit % 8 != 0 || nbit > 32) { 168 | printf("Unsupported bit per sample.\n"); 169 | return; 170 | } 171 | 172 | int nbyte = nbit / 8; 173 | 174 | char text[4] = {'R', 'I', 'F', 'F'}; 175 | uint32_t long_number = 36 + x_length * nbyte; 176 | fwrite(text, 1, 4, fp); 177 | fwrite(&long_number, 4, 1, fp); 178 | 179 | text[0] = 'W'; 180 | text[1] = 'A'; 181 | text[2] = 'V'; 182 | text[3] = 'E'; 183 | fwrite(text, 1, 4, fp); 184 | text[0] = 'f'; 185 | text[1] = 'm'; 186 | text[2] = 't'; 187 | text[3] = ' '; 188 | fwrite(text, 1, 4, fp); 189 | 190 | long_number = 16; 191 | fwrite(&long_number, 4, 1, fp); 192 | int16_t short_number = FORMAT_PCM; 193 | fwrite(&short_number, 2, 1, fp); 194 | short_number = 1; // nChannels 195 | fwrite(&short_number, 2, 1, fp); 196 | long_number = fs; // nSamplesPerSec 197 | fwrite(&long_number, 4, 1, fp); 198 | long_number = fs * nbyte; // nAvgBytesPerSec 199 | fwrite(&long_number, 4, 1, fp); 200 | short_number = nbyte; // nBlockAlign 201 | fwrite(&short_number, 2, 1, fp); 202 | short_number = nbit; // nBitsPerSample 203 | fwrite(&short_number, 2, 1, fp); 204 | 205 | text[0] = 'd'; 206 | text[1] = 'a'; 207 | text[2] = 't'; 208 | text[3] = 'a'; 209 | fwrite(text, 1, 4, fp); 210 | long_number = x_length * nbyte; 211 | fwrite(&long_number, 4, 1, fp); 212 | 213 | if (nbyte == 1) 214 | for (int i = 0; i < x_length; ++i) { 215 | uint8_t tmp_signal; 216 | tmp_signal = (uint8_t)(MyMax(0, MyMin(255, (int)((x[i] + 1.0) * 127)))); 217 | fwrite(&tmp_signal, 1, 1, fp); 218 | } 219 | else if (nbyte == 2) 220 | for (int i = 0; i < x_length; ++i) { 221 | int16_t tmp_signal; 222 | tmp_signal = (int16_t)(MyMax(-32768, MyMin(32767, (int)(x[i] * 32767)))); 223 | fwrite(&tmp_signal, 2, 1, fp); 224 | } 225 | else if (nbyte == 3) 226 | for (int i = 0; i < x_length; ++i) { 227 | int32_t tmp_signal; 228 | tmp_signal = (int32_t)(MyMax(-8388608, MyMin(8388607, (int)(x[i] * 8388607)))); 229 | fwrite(&tmp_signal, 3, 1, fp); 230 | } 231 | else if (nbyte == 4) 232 | for (int i = 0; i < x_length; ++i) { 233 | int32_t tmp_signal; 234 | tmp_signal = (int32_t)(MyMax(-2147483648, MyMin(2147483647, (int)(x[i] * 2147483647)))); 235 | fwrite(&tmp_signal, 4, 1, fp); 236 | } 237 | } 238 | 239 | void wavwrite(FP_TYPE* x, int x_length, int fs, int nbit, char* filename) { 240 | FILE* fp = fopen(filename, "wb"); 241 | if (fp == NULL) { 242 | printf("File cannot be opened.\n"); 243 | return; 244 | } 245 | wavwrite_fp(x, x_length, fs, nbit, fp); 246 | fclose(fp); 247 | } 248 | 249 | typedef union { 250 | unsigned char data[16]; 251 | float num_float; 252 | double num_double; 253 | } char_float_double; 254 | 255 | FP_TYPE* wavread_fp(FILE* fp, int* fs, int* nbit, int* wav_length) { 256 | int format_tag = 0; 257 | int nchannel = 1; 258 | if (GetParameters(fp, fs, nbit, wav_length, & format_tag, & nchannel) == false) { 259 | fclose(fp); 260 | return NULL; 261 | } 262 | 263 | FP_TYPE *waveform = calloc(*wav_length, sizeof(FP_TYPE)); 264 | if (waveform == NULL) return NULL; 265 | 266 | int quantization_byte = *nbit / 8; 267 | FP_TYPE zero_line = pow(2.0, *nbit - 1); 268 | FP_TYPE tmp, sign_bias, nbitpow; 269 | char_float_double for_int_number; 270 | nbitpow = pow(2.0, *nbit - 1); 271 | for (int i = 0; i < *wav_length; ++i) { 272 | sign_bias = tmp = 0.0; 273 | fread(for_int_number.data, 1, quantization_byte, fp); // "data" 274 | if(nchannel == 2) 275 | fread(for_int_number.data + 8, 1, quantization_byte, fp); // right channel 276 | if (format_tag == FORMAT_PCM) { 277 | if (quantization_byte == 1) { 278 | waveform[i] = for_int_number.data[0] / 128.0 - 0.5; 279 | continue; 280 | } 281 | if (for_int_number.data[quantization_byte-1] >= 128) { 282 | sign_bias = nbitpow; 283 | for_int_number.data[quantization_byte - 1] = 284 | for_int_number.data[quantization_byte - 1] & 0x7F; 285 | } 286 | for (int j = quantization_byte - 1; j >= 0; --j) 287 | tmp = tmp * 256.0 + for_int_number.data[j]; 288 | waveform[i] = (tmp - sign_bias) / zero_line; 289 | } else 290 | if (format_tag == FORMAT_IEEE_FLOAT) { 291 | if (quantization_byte == 4) { 292 | waveform[i] = for_int_number.num_float; 293 | } else if (quantization_byte == 8) { 294 | waveform[i] = for_int_number.num_double; 295 | } 296 | } 297 | } 298 | return waveform; 299 | } 300 | 301 | FP_TYPE* wavread(char* filename, int* fs, int* nbit, int* wav_length) { 302 | FILE* fp = fopen(filename, "rb"); 303 | if (NULL == fp) { 304 | printf("File not found.\n"); 305 | return NULL; 306 | } 307 | FP_TYPE* ret = wavread_fp(fp, fs, nbit, wav_length); 308 | fclose(fp); 309 | return ret; 310 | } 311 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | ciglet 2 | === 3 | 4 | Ciglet is a lightweight C library for digital signal processing, in particular audio and speech processing. 5 | 6 | Language: C99 7 | 8 | License: BSD 9 | 10 | Background 11 | --- 12 | 13 | In the past few years I've been writing and maintaining quite a few C-written speech processing projects (e.g. *libllsm*, *libpyin*, *moresampler*), some of which were partially translated from Matlab/Octave-based prototypes. Gradually I ended up having lots of frequent rewrites of C version of Matlab routines such as `sum`, `fir1`, `conv`, `interp1`, etc. In August 2016 I finally made the move to extract all these repeating patterns and group them into one independent library. Since it would be referred by quite a few projects, the new library is meant to be lightweight, easy to link, and fast, which is why it's named *ciglet*, an acronym of "C-written sIGnal codeLETs". 14 | 15 | Function index 16 | --- 17 | 18 | ### Scalar operations 19 | 20 | * random number generation: `randu`, `randn` 21 | * miscellaneous: `max`, `min`, `linterp`, `fastatan2` 22 | * complex arithmetics: `c_cplx`, `c_conj`, `c_add`, `c_sub`, `c_mul`, `c_div`, `c_exp`, `c_abs`, `c_arg` 23 | 24 | ### Vector operations and statistics 25 | 26 | * vectorized arithmetics: `sumfp`, `sumsqrfp`, `maxfp`, `minfp` 27 | * descriptive statistics: `meanfp`, `varfp`, `medianfp`, `xcorr`, `corr`, `cov` 28 | * sorting: `selectnth`, `sort` 29 | * peak picking: `find_peak`, `find_valley`, `find_maxima`, `find_minima` 30 | 31 | ### Numerical routines 32 | 33 | `fzero`, `polyval`, `roots` 34 | 35 | ### Basic linear algebra 36 | 37 | * products: `matmul`, `mvecmul`, `dot` 38 | * solving a linear system: `lu`, `lusolve` 39 | * pivoting: `ppivot`, `permm`, `permv` 40 | 41 | ### Memory (de)allocation 42 | 43 | * enumeration: `linspace`, `iota` 44 | * 2d array operations: `malloc2d`, `free2d`, `copy2d`, `flatten`, `reshape`, `transpose` 45 | 46 | ### Audio I/O 47 | 48 | `wavread`, `wavwrite` 49 | 50 | ### General DSP routines 51 | 52 | * windows: `boxcar`, `hanning`, `hamming`, `mltsine`, `blackman_harris`, `nuttall98`, `blackman` 53 | * Fourier transform: `fft`, `ifft`, `czt`, `iczt`, `idft`, `dct`, `fftshift` 54 | * phase manipulation: `wrap`, `unwrap`, `phase_diff` 55 | * complex number conversion: `abscplx`, `argcplx`, `polar2real`, `polar2imag`, `complete_symm`, `complete_asymm` 56 | * cepstral analysis: `rceps`, `irceps`, `minphase` 57 | * filtering: `fir1`, `conv`, `filter`, `filtfilt`, `moving_avg`, `moving_rms`, `medfilt1`, `kalmanf1d`, `kalmans1d` 58 | * linear prediction: `levinson`, `lpc`, `flpc`, `lpgain`, `lpspec`, `lpresf` 59 | * interpolation: `interp1`, `interp1u`, `sincinterp1u`, `interp_in_blank`, `rresample` 60 | * operations on sinusoids: `gensin`, `gensins`, `safe_aliased_sinc`, `safe_aliased_dsinc` 61 | * miscellaneous: `fetch_frame`, `diff`, `cumsum`, `flip` ,`white_noise`, `itakura_saito` 62 | 63 | ### Audio/speech processing routines 64 | 65 | * psychoacoustics: `mel2freq`, `freq2mel`, `freq2bark`, `bark2freq`, `eqloud`, `melspace` 66 | * frequency estimation: `ifdetector_estimate`, `correlogram`, `invcrgm` 67 | * spectrogram and STFT: `stft`, `istft`, `qifft`, `spgm2cegm`, `cegm2spgm` 68 | * filterbank analysis: `filterbank_spgm`, `filterbank_spec`, `be2cc`, `be2ccgm` 69 | * spectral envelope estimation: `spec2env` 70 | * glottal model: `lfmodel_from_rd`, `lfmodel_spectrum`, `lfmodel_period` 71 | 72 | ### Plotting utilities (Gnuplot interface, unavailable on Windows) 73 | 74 | `plotopen`, `plot`, `imagesc`, `plotclose` 75 | -------------------------------------------------------------------------------- /test/test-fft.c: -------------------------------------------------------------------------------- 1 | #include "../ciglet.h" 2 | #include 3 | 4 | static double gettime() { 5 | struct timeval t; 6 | gettimeofday(&t, NULL); 7 | return (t.tv_sec + (t.tv_usec / 1000000.0)) * 1000.0; 8 | } 9 | 10 | int main(void) { 11 | int N = 512; 12 | FP_TYPE* in_re = calloc(N, sizeof(FP_TYPE)); 13 | FP_TYPE* in_im = calloc(N, sizeof(FP_TYPE)); 14 | FP_TYPE* out_re = calloc(N, sizeof(FP_TYPE)); 15 | FP_TYPE* out_im = calloc(N, sizeof(FP_TYPE)); 16 | FP_TYPE* buffer = calloc(N * 2, sizeof(FP_TYPE)); 17 | 18 | double T = 0; 19 | for(int i = 0; i < 1000; i ++) { 20 | for(int j = 0; j < N; j ++) { 21 | #if 0 22 | in_re[j] = randu(); 23 | in_im[j] = randu(); 24 | #else 25 | in_re[j] = 1e-44; // subnormal for single precision floats 26 | in_im[j] = -1e-44; 27 | #endif 28 | } 29 | double t0 = gettime(); 30 | ifft(in_re, in_im, out_re, NULL, N, buffer); 31 | double t1 = gettime(); 32 | T += t1 - t0; 33 | } 34 | printf("Time taken: %fms\n", T); 35 | 36 | free(in_re); free(in_im); free(out_re); free(out_im); 37 | } 38 | -------------------------------------------------------------------------------- /test/test.c: -------------------------------------------------------------------------------- 1 | #include "../ciglet.h" 2 | 3 | int noplot = 0; 4 | 5 | static void test_statistics() { 6 | FP_TYPE* rand1 = white_noise(1, 10000); 7 | FP_TYPE* rand2 = white_noise(1, 10000); 8 | printf("corr = %f, mean = %f, median = %f\n", corr(rand1, rand2, 10000), 9 | meanfp(rand1, 10000), medianfp(rand1, 10000)); 10 | free(rand1); 11 | free(rand2); 12 | } 13 | 14 | static void print_mat(FP_TYPE* A, int m, int n) { 15 | for(int i = 0; i < m; i ++) { 16 | for(int j = 0; j < n; j ++) 17 | printf("%6.3f, ", A[i + j * n]); 18 | printf("\n"); 19 | } 20 | } 21 | 22 | static void test_la() { 23 | int n = 5; 24 | FP_TYPE A[25] = { // magic(5) 25 | 17, 23, 4, 10, 11, 26 | 24, 5, 6, 12, 18, 27 | 1, 7, 13, 19, 25, 28 | 8, 14, 20, 21, 2, 29 | 15, 16, 22, 3, 9 30 | }; 31 | FP_TYPE b[5] = {-63, 84, 21, 73, 15}; 32 | print_mat(A, n, n); 33 | FP_TYPE* Acpy = zeros(n, n); 34 | FP_TYPE* Acpy2 = zeros(n, n); 35 | FP_TYPE* I = eye(n); 36 | for(int i = 0; i < n * n; i ++) Acpy2[i] = Acpy[i] = A[i]; 37 | 38 | int* permidx = ppivot(A, n); 39 | printf("Permutation: "); 40 | for(int i = 0; i < n; i ++) 41 | printf("%d ", permidx[i]); 42 | printf("\n"); 43 | print_mat(A, n, n); 44 | printf("permm:\n"); 45 | 46 | permm(Acpy, permidx, n, n); 47 | print_mat(Acpy, n, n); 48 | 49 | lu(A, n); 50 | printf("LU:\n"); 51 | print_mat(A, n, n); 52 | 53 | permv(b, permidx, n); 54 | lusolve(A, b, n); 55 | printf("Solution: "); 56 | for(int i = 0; i < n; i ++) 57 | printf("%6.3f ", b[i]); 58 | printf("\n"); 59 | 60 | FP_TYPE c[5]; 61 | mvecmul(Acpy2, b, c, n, n); 62 | printf("mvecmul: "); 63 | for(int i = 0; i < n; i ++) 64 | printf("%6.3f ", c[i]); 65 | printf("\n"); 66 | 67 | FP_TYPE* B = zeros(n, n); 68 | matmul(Acpy2, I, B, n, n, n); 69 | printf("matmul:\n"); 70 | print_mat(B, n, n); 71 | 72 | free(I); free(B); 73 | free(Acpy); 74 | free(Acpy2); 75 | free(permidx); 76 | } 77 | 78 | static void test_czt() { 79 | int n = 16; 80 | FP_TYPE* xr = calloc(n, sizeof(FP_TYPE)); 81 | FP_TYPE* xi = calloc(n, sizeof(FP_TYPE)); 82 | FP_TYPE* yr = calloc(n, sizeof(FP_TYPE)); 83 | FP_TYPE* yi = calloc(n, sizeof(FP_TYPE)); 84 | FP_TYPE* yrfft = calloc(n, sizeof(FP_TYPE)); 85 | FP_TYPE* yifft = calloc(n, sizeof(FP_TYPE)); 86 | FP_TYPE* buff = calloc(n * 2, sizeof(FP_TYPE)); 87 | for(int i = 0; i < n; i ++) { 88 | xr[i] = randn(0, 1); 89 | xi[i] = randn(0, 1); 90 | } 91 | printf("Forward transform (CZT vs FFT):\n"); 92 | czt(xr, xi, yr, yi, 2 * M_PI / n, n); 93 | fft(xr, xi, yrfft, yifft, n, buff); 94 | for(int i = 0; i < n; i ++) 95 | printf("%f\t%f\t%f\t%f\n", yr[i], yrfft[i], yi[i], yifft[i]); 96 | 97 | printf("Inverse transform (ICZT vs original signal):\n"); 98 | FP_TYPE* ixr = calloc(n, sizeof(FP_TYPE)); 99 | FP_TYPE* ixi = calloc(n, sizeof(FP_TYPE)); 100 | iczt(yr, yi, ixr, ixi, 2 * M_PI / n, n); 101 | for(int i = 0; i < n; i ++) 102 | printf("%f\t%f\t%f\t%f\n", ixr[i], xr[i], ixi[i], xi[i]); 103 | 104 | free(xr); free(xi); free(yr); free(yi); 105 | free(yrfft); free(yifft); free(buff); 106 | free(ixr); free(ixi); 107 | } 108 | 109 | static void test_numerical() { 110 | int order = 20; 111 | FP_TYPE* a = calloc(order + 1, sizeof(FP_TYPE)); 112 | for(int i = 0; i < order + 1; i ++) 113 | a[i] = randn(0, 5); 114 | 115 | cplx* r = rootsr(a, order + 1); 116 | for(int i = 0; i < order; i ++) { 117 | cplx y = polyvalr(a, order + 1, r[i]); 118 | printf("roots[%d] = %f + %fi\n", i, r[i].real, r[i].imag); 119 | printf("f(roots[%d]) = %f + %fi\n", i, y.real, y.imag); 120 | } 121 | free(r); 122 | free(a); 123 | } 124 | 125 | static void test_lf() { 126 | lfmodel testlf = lfmodel_from_rd(1.0, 0.008, 0.3); 127 | int nfft = 1024; 128 | int fs = 44100; 129 | FP_TYPE* freq = calloc(nfft, sizeof(FP_TYPE)); 130 | FP_TYPE* time_axis = linspace(0, nfft - 1, nfft); 131 | for(int i = 0; i < nfft; i ++) 132 | freq[i] = (i <= nfft / 2 ? i : i - nfft) * fs / nfft; 133 | freq[0] = 1e-5; 134 | FP_TYPE* lfphseresp = calloc(nfft, sizeof(FP_TYPE)); 135 | FP_TYPE* lfmagnresp = lfmodel_spectrum(testlf, freq, nfft, lfphseresp); 136 | FP_TYPE* fftbuffer = calloc(nfft * 2, sizeof(FP_TYPE)); 137 | for(int i = 0; i < nfft; i ++) lfmagnresp[i] *= fs; 138 | FP_TYPE* lfrealresp = polar2real(lfmagnresp, lfphseresp, nfft); 139 | FP_TYPE* lfimagresp = polar2imag(lfmagnresp, lfphseresp, nfft); 140 | lfrealresp[0] = 0; 141 | lfimagresp[0] = 0; 142 | for(int i = 0; i < nfft; i ++) 143 | lfmagnresp[i] = log(lfmagnresp[i]); 144 | FP_TYPE* lfperiod = lfmodel_period(testlf, fs, nfft / 2); 145 | 146 | ifft(lfrealresp, lfimagresp, lfrealresp, lfimagresp, nfft, fftbuffer); 147 | 148 | FP_TYPE* shifted_resp = fftshift(lfrealresp, nfft); 149 | FP_TYPE max_error = 0; 150 | for(int i = 0; i < nfft / 2; i ++) { 151 | FP_TYPE err = fabs(lfperiod[i] - shifted_resp[i + nfft / 2]); 152 | max_error = max(max_error, err); 153 | } 154 | free(shifted_resp); 155 | printf("LF model (IFFT error): %f%%\n", max_error / testlf.Ee * 100); 156 | printf("Note: some 1%% error is expected here due to the IFFT " 157 | "version being band-limited while the time-domain version being subjected " 158 | "to aliasing.\n"); 159 | 160 | # if _POSIX_C_SOURCE >= 2 161 | if(! noplot) { 162 | figure* lffg = plotopen(); 163 | plot(lffg, time_axis, lfrealresp, nfft / 2, 'b'); 164 | plotclose(lffg); 165 | lffg = plotopen(); 166 | plot(lffg, NULL, lfperiod, nfft / 2, 'b'); 167 | plotclose(lffg); 168 | } 169 | # endif 170 | free(freq); 171 | free(time_axis); 172 | free(lfmagnresp); 173 | free(lfphseresp); 174 | free(lfrealresp); 175 | free(lfimagresp); 176 | free(fftbuffer); 177 | free(lfperiod); 178 | } 179 | 180 | static FP_TYPE* test_wav(int* fs, int* nx, int* nbit) { 181 | int ny; 182 | FP_TYPE* x = wavread("test/in.wav", fs, nbit, nx); 183 | FP_TYPE* y = rresample(x, *nx, 1.5, & ny); 184 | FP_TYPE* y2 = moving_avg(y, ny, 10); 185 | FP_TYPE* y2d = diff(y2, ny); 186 | FP_TYPE* y2c = cumsum(y2d, ny); 187 | free(y); 188 | free(y2); 189 | free(y2d); 190 | 191 | for(int i = 0; i < ny; i ++) 192 | y2c[i] += randn(0, 0.01 * 0.01); 193 | wavwrite(y2c, ny, *fs * 1.5, *nbit, "test/out-resample-mavg.wav"); 194 | free(y2c); 195 | return x; 196 | } 197 | 198 | static void test_if(FP_TYPE* x, int nx, int fs) { 199 | ifdetector* ifd = cig_create_ifdetector(220.0 / fs, 220.0 / fs); 200 | int nhop = 256; 201 | int nfrm = nx / nhop; 202 | 203 | FP_TYPE* x_if = calloc(nfrm, sizeof(FP_TYPE)); 204 | for(int i = 0; i < nfrm; i ++) { 205 | FP_TYPE* xi = fetch_frame(x, nx, i * nhop, ifd -> nh); 206 | x_if[i] = cig_ifdetector_estimate(ifd, xi, ifd -> nh) * fs; 207 | //printf("%f %f\n", (FP_TYPE)i * nhop / fs, x_if[i]); 208 | free(xi); 209 | } 210 | 211 | # if _POSIX_C_SOURCE >= 2 212 | if(! noplot) { 213 | figure* fg = plotopen(); 214 | plot(fg, NULL, x_if, nfrm, 'b'); 215 | plotclose(fg); 216 | } 217 | # endif 218 | free(x_if); 219 | cig_delete_ifdetector(ifd); 220 | } 221 | 222 | static void test_lpc(FP_TYPE* x, int nx, int fs) { 223 | int nhop = 256; 224 | int nfft = 1024; 225 | int nfrm = round(nx / nhop); 226 | int order = 12; 227 | FP_TYPE maxfreq = 5500; 228 | FP_TYPE normfc = 0; 229 | FP_TYPE** Xm = malloc2d(nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 230 | stft(x, nx, nhop, nfrm, 4, 1, & normfc, NULL, Xm, NULL); 231 | 232 | FP_TYPE* faxis = linspace(0, fs / 2, nfft / 2 + 1); 233 | FP_TYPE* faxis_warp = linspace(0, maxfreq, nfft / 2 + 1); 234 | FP_TYPE* buffer = malloc(nfft * 2 * sizeof(FP_TYPE)); 235 | for(int i = 0; i < nfrm; i ++) { 236 | FP_TYPE* Xwarp = interp1(faxis, Xm[i], nfft / 2 + 1, faxis_warp, nfft / 2 + 1); 237 | FP_TYPE* R = NULL; 238 | FP_TYPE* a = flpc(Xwarp, nfft / 2 + 1, order, & R); 239 | free(Xwarp); 240 | 241 | FP_TYPE g = lpgain(a, R, order + 1); 242 | FP_TYPE* S = lpspec(a, g, order + 1, nfft); 243 | for(int j = 0; j < nfft / 2 + 1; j ++) 244 | Xm[i][j] = log(S[j]); 245 | 246 | int npole = 0; 247 | cplx* poles = calloc(order, sizeof(cplx)); 248 | FP_TYPE* formants = lpresf(a, order + 1, poles, & npole); 249 | if(i % (nfrm / 10) == 0) 250 | for(int j = 0; j < npole; j ++) { 251 | formants[j] *= maxfreq; 252 | printf("%d %f (%f, %f)\n", j, formants[j], poles[j].real, poles[j].imag); 253 | } 254 | 255 | free(poles); 256 | free(formants); 257 | 258 | free(S); 259 | free(a); 260 | free(R); 261 | } 262 | # if _POSIX_C_SOURCE >= 2 263 | if(! noplot) { 264 | figure* fig = plotopen(); 265 | imagesc(fig, Xm, nfrm, nfft / 2 + 1); 266 | plotclose(fig); 267 | } 268 | # endif 269 | free(faxis); 270 | free(faxis_warp); 271 | free2d(Xm, nfrm); 272 | free(buffer); 273 | } 274 | 275 | static void test_lpcwave(FP_TYPE* x, int nx, int fs) { 276 | int nhop = 256; 277 | int nwin = 1024; 278 | int nfft = 1024; 279 | int nfrm = round(nx / nhop); 280 | int p = 32; 281 | FP_TYPE** Xm = malloc2d(nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 282 | for(int i = 0; i < nfrm; i ++) { 283 | FP_TYPE* xi = fetch_frame(x, nx, i * nhop, nwin); 284 | FP_TYPE* R = NULL; 285 | FP_TYPE* a = lpc(xi, nwin, p, & R); 286 | FP_TYPE g = lpgain(a, R, p + 1); 287 | FP_TYPE* S = lpspec(a, g, p + 1, nfft); 288 | for(int j = 0; j < nfft / 2 + 1; j ++) 289 | Xm[i][j] = log(S[j]); 290 | free(R); 291 | free(S); 292 | free(a); 293 | free(xi); 294 | } 295 | # if _POSIX_C_SOURCE >= 2 296 | if(! noplot) { 297 | figure* fig = plotopen(); 298 | imagesc(fig, Xm, nfrm, nfft / 2 + 1); 299 | plotclose(fig); 300 | } 301 | # endif 302 | free2d(Xm, nfrm); 303 | } 304 | 305 | static void test_correlogram(FP_TYPE* x, int nx, int fs) { 306 | int nhop = 128; 307 | int nfrm = nx / nhop; 308 | int max_period = 1024; 309 | int* center = calloc(nfrm, sizeof(int)); 310 | int* nwin = calloc(nfrm, sizeof(int)); 311 | for(int i = 0; i < nfrm; i ++) { 312 | center[i] = i * nhop; 313 | nwin[i] = 300; 314 | } 315 | FP_TYPE** R = malloc2d(nfrm, 1024, sizeof(FP_TYPE)); 316 | cig_correlogram(x, nx, center, nwin, nfrm, max_period, CIG_CORR_ACF, R); 317 | FP_TYPE* faxis = linspace(0, 999, 1000); 318 | FP_TYPE** Ri = cig_invcrgm(R, nfrm, max_period, fs, faxis, 1000); 319 | # if _POSIX_C_SOURCE >= 2 320 | if(! noplot) { 321 | figure* fig = plotopen(); 322 | imagesc(fig, Ri, nfrm, 1000); 323 | plotclose(fig); 324 | } 325 | # endif 326 | free2d(Ri, nfrm); 327 | free2d(R, nfrm); 328 | free(faxis); 329 | free(center); free(nwin); 330 | } 331 | 332 | static void test_spectral(FP_TYPE* x, int nx, int fs, int nbit) { 333 | int nhop = 256; 334 | int nfft = 2048; 335 | int nfrm = round(nx / nhop); 336 | FP_TYPE normfc = 0; 337 | FP_TYPE** Xm = malloc2d(nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 338 | FP_TYPE** Xp = malloc2d(nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 339 | stft(x, nx, nhop, nfrm, 8, 1, & normfc, NULL, Xm, Xp); 340 | 341 | filterbank* mfbank = create_melfilterbank(nfft / 2 + 1, fs / 2, 36, 50, 8000); 342 | FP_TYPE** Xmfb = filterbank_spgm(mfbank, Xm, nfrm, nfft, fs, 0); 343 | FP_TYPE** Xmfcc = calloc(nfrm, sizeof(FP_TYPE*)); 344 | for(int i = 0; i < nfrm; i ++) 345 | Xmfcc[i] = be2cc(Xmfb[i], 36, 12, 0); 346 | 347 | # if _POSIX_C_SOURCE >= 2 348 | figure* fg = NULL; 349 | if(! noplot) { 350 | fg = plotopen(); 351 | imagesc(fg, Xmfcc, nfrm, 12); 352 | plotclose(fg); 353 | } 354 | # endif 355 | 356 | printf("Selected content of Xmfcc:\n"); 357 | for(int i = 0; i < nfrm; i += nfrm / 5) { 358 | for(int j = 0; j < 12; j += 3) 359 | printf("%10f ", Xmfcc[i][j]); 360 | puts(""); 361 | } 362 | 363 | FP_TYPE** Xmmf = calloc(nfft / 2 + 1, sizeof(FP_TYPE*)); 364 | FP_TYPE** Xmtr = transpose(Xm, nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 365 | for(int i = 0; i < nfft / 2 + 1; i ++) { 366 | Xmmf[i] = medfilt1(Xmtr[i], nfrm, 25); 367 | for(int j = 0; j < nfrm; j ++) 368 | Xmmf[i][j] = log_2(Xmmf[i][j]); 369 | } 370 | free2d(Xmtr, nfft / 2 + 1); 371 | Xmtr = transpose(Xmmf, nfft / 2 + 1, nfrm, sizeof(FP_TYPE)); 372 | free2d(Xmmf, nfft / 2 + 1); 373 | for(int i = 0; i < nfrm; i ++) { 374 | FP_TYPE* tmp = medfilt1(Xmtr[i], nfft / 2 + 1, 25); 375 | free(Xmtr[i]); 376 | Xmtr[i] = tmp; 377 | } 378 | free2d(Xmtr, nfrm); 379 | 380 | free2d(Xmfb, nfrm); 381 | free2d(Xmfcc, nfrm); 382 | delete_filterbank(mfbank); 383 | 384 | FP_TYPE** Xm2 = copy2d(Xm, nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 385 | for(int i = 0; i < nfrm; i ++) { 386 | for(int j = round(30 + pow(sin(i * 0.05), 2) * 200); j < nfft / 2 + 1; j ++) 387 | Xm2[i][j] = 0; 388 | } 389 | 390 | int ny; 391 | FP_TYPE* y = istft(Xm2, Xp, nhop, nfrm, 8, 1, normfc, & ny); 392 | wavwrite(y, ny, fs, nbit, "test/out-stft-wow.wav"); 393 | for(int i = 0; i < ny; i += ny / 20) 394 | printf("%10f ", y[i]); 395 | puts(""); 396 | free(y); 397 | free2d(Xm2, nfrm); 398 | printf("Selected content of y:\n"); 399 | 400 | FP_TYPE** C = malloc2d(nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 401 | for(int i = 0; i < nfrm; i ++) { 402 | free(spec2env(Xm[i], nfft, 250.0 / fs, C[i])); 403 | } 404 | FP_TYPE** Xm3 = cegm2spgm(C, nfrm, nfft, nfft / 2 + 1); 405 | # if _POSIX_C_SOURCE >= 2 406 | if(! noplot) { 407 | fg = plotopen(); 408 | imagesc(fg, Xm3, nfrm, nfft / 2 + 1); 409 | plotclose(fg); 410 | } 411 | # endif 412 | 413 | FP_TYPE* Xm3_flattened = flatten(Xm3, nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 414 | FP_TYPE Xm3median = medianfp(Xm3_flattened, nfrm * (nfft / 2 + 1)); 415 | FP_TYPE Xm3mean = meanfp(Xm3_flattened, nfrm * (nfft / 2 + 1)); 416 | printf("Median of spectrogram: %f\n", Xm3median); 417 | printf("Mean of spectrogram: %f (stdvar: %f)\n", Xm3mean, 418 | sqrt(varfp(Xm3_flattened, nfrm * (nfft / 2 + 1)))); 419 | //for(int i = 0; i < nfrm * (nfft / 2 + 1); i ++) 420 | // Xm3_flattened[i] = exp_2(Xm3_flattened[i]); 421 | FP_TYPE** Xm4 = reshape(Xm3_flattened, nfrm, nfft / 2 + 1, sizeof(FP_TYPE)); 422 | free(Xm3_flattened); 423 | free2d(Xm3, nfrm); free2d(Xm4, nfrm); free2d(C, nfrm); 424 | 425 | free2d(Xm, nfrm); free2d(Xp, nfrm); 426 | } 427 | 428 | int main(int argc, char* argv[]) { 429 | int stat_on = 0; 430 | int la_on = 0; 431 | int lf_on = 0; 432 | int if_on = 0; 433 | int czt_on = 0; 434 | int numerical_on = 0; 435 | int lpc_on = 0; 436 | int lpcwave_on = 0; 437 | int corr_on = 0; 438 | int spec_on = 0; 439 | 440 | if(argc >= 2 && ! strcmp(argv[1], "noplot")) 441 | noplot = 1; 442 | if(argc >= 3 && (strcmp(argv[2], "all"))) { 443 | if(! strcmp(argv[2], "statistics")) 444 | stat_on = 1; 445 | else 446 | if(! strcmp(argv[2], "la")) 447 | la_on = 1; 448 | else 449 | if(! strcmp(argv[2], "lf")) 450 | lf_on = 1; 451 | else 452 | if(! strcmp(argv[2], "if")) 453 | if_on = 1; 454 | else 455 | if(! strcmp(argv[2], "czt")) 456 | czt_on = 1; 457 | else 458 | if(! strcmp(argv[2], "numerical")) 459 | numerical_on = 1; 460 | else 461 | if(! strcmp(argv[2], "lpc")) 462 | lpc_on = 1; 463 | else 464 | if(! strcmp(argv[2], "lpcwave")) 465 | lpcwave_on = 1; 466 | else 467 | if(! strcmp(argv[2], "corr")) 468 | corr_on = 1; 469 | else 470 | if(! strcmp(argv[2], "spec")) 471 | spec_on = 1; 472 | } else { 473 | stat_on = la_on = lf_on = if_on = czt_on = numerical_on = lpc_on = 474 | lpcwave_on = corr_on = spec_on = 1; 475 | } 476 | 477 | if(stat_on) 478 | test_statistics(); 479 | if(la_on) 480 | test_la(); 481 | if(lf_on) 482 | test_lf(); 483 | if(czt_on) 484 | test_czt(); 485 | if(numerical_on) 486 | test_numerical(); 487 | 488 | int fs, nx, nbit; 489 | FP_TYPE* x = NULL; 490 | if(if_on + lpc_on + lpcwave_on + corr_on + spec_on > 0) 491 | x = test_wav(& fs, & nx, & nbit); 492 | 493 | if(if_on) 494 | test_if(x, nx, fs); 495 | if(lpc_on) 496 | test_lpc(x, nx, fs); 497 | if(lpcwave_on) 498 | test_lpcwave(x, nx, fs); 499 | if(corr_on) 500 | test_correlogram(x, nx, fs); 501 | if(spec_on) 502 | test_spectral(x, nx, fs, nbit); 503 | 504 | free(x); 505 | return 0; 506 | } 507 | -------------------------------------------------------------------------------- /test/test.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | {C947E0D4-E63A-47A5-B40F-94D65541389A} 23 | Win32Proj 24 | test 25 | 8.1 26 | 27 | 28 | 29 | Application 30 | true 31 | v140 32 | Unicode 33 | 34 | 35 | Application 36 | false 37 | v140 38 | true 39 | Unicode 40 | 41 | 42 | Application 43 | true 44 | v140 45 | Unicode 46 | 47 | 48 | Application 49 | false 50 | v140 51 | true 52 | Unicode 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | true 74 | 75 | 76 | true 77 | 78 | 79 | false 80 | 81 | 82 | false 83 | 84 | 85 | 86 | 87 | 88 | Level3 89 | Disabled 90 | FP_TYPE=float;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 91 | CompileAsC 92 | 4244;4305;4996 93 | 94 | 95 | Console 96 | true 97 | kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) 98 | 99 | 100 | 101 | 102 | 103 | 104 | Level3 105 | Disabled 106 | FP_TYPE=float;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 107 | CompileAsC 108 | 4244;4305;4996 109 | 110 | 111 | Console 112 | true 113 | 114 | 115 | 116 | 117 | Level3 118 | 119 | 120 | MaxSpeed 121 | true 122 | true 123 | FP_TYPE=float;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 124 | CompileAsC 125 | 4244;4305;4996 126 | 127 | 128 | Console 129 | true 130 | true 131 | true 132 | 133 | 134 | 135 | 136 | Level3 137 | 138 | 139 | MaxSpeed 140 | true 141 | true 142 | FP_TYPE=float;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 143 | CompileAsC 144 | 4244;4305;4996 145 | 146 | 147 | Console 148 | true 149 | true 150 | true 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | {6fd573bb-3f2e-4ea9-8a29-752448730e11} 159 | 160 | 161 | 162 | 163 | 164 | -------------------------------------------------------------------------------- /test/test.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | --------------------------------------------------------------------------------