]> 4ch.mooo.com Git - 16.git/blob - src/lib/dl/ext/vorbtool/resample.c
meh did some cleanings and i will work on mapread to mm thingy sometime soon! oops...
[16.git] / src / lib / dl / ext / vorbtool / resample.c
1 /* resample.c: see resample.h for interesting stuff */
2
3 #ifdef HAVE_CONFIG_H
4 #include "config.h"
5 #endif
6
7 #include <math.h>
8 #include <stdlib.h>
9 #include <string.h>
10 #include <stdarg.h>
11 #include <assert.h>
12
13 #include "resample.h"
14
15 /* Some systems don't define this */
16 #ifndef M_PI
17 #define M_PI       3.14159265358979323846 
18 #endif
19
20 static int hcf(int arg1, int arg2)
21 {
22     int mult = 1;
23
24     while (~(arg1 | arg2) & 1)
25         arg1 >>= 1, arg2 >>= 1, mult <<= 1;
26
27     while (arg1 > 0)
28     {
29         if (~(arg1 & arg2) & 1)
30         {
31             arg1 >>= (~arg1 & 1);
32             arg2 >>= (~arg2 & 1);
33         }
34         else if (arg1 < arg2)
35             arg2 = (arg2 - arg1) >> 1;
36         else
37             arg1 = (arg1 - arg2) >> 1;
38     }
39
40     return arg2 * mult;
41 }
42
43
44 static void filt_sinc(float *dest, int N, int step, double fc, double gain, int width)
45 {
46     double s = fc / step;
47     int mid, x;
48     float *endpoint = dest + N,
49         *base = dest,
50         *origdest = dest;
51
52     assert(width <= N);
53
54     if ((N & 1) == 0)
55     {
56         *dest = 0.0;
57         dest += width;
58         if (dest >= endpoint)
59             dest = ++base;
60         N--;
61     }
62
63     mid = N / 2;
64     x = -mid;
65
66     while (N--)
67     {
68         *dest = (x ? sin(x * M_PI * s) / (x * M_PI) * step : fc) * gain;
69         x++;
70         dest += width;
71         if (dest >= endpoint)
72             dest = ++base;
73     }
74     assert(dest == origdest + width);
75 }
76
77
78 static double I_zero(double x)
79 {
80     int n = 0;
81     double u = 1.0,
82         s = 1.0,
83         t;
84
85     do
86     {
87         n += 2;
88         t = x / n;
89         u *= t * t;
90         s += u;
91     } while (u > 1e-21 * s);
92
93     return s;
94 }
95
96
97 static void win_kaiser(float *dest, int N, double alpha, int width)
98 {
99     double I_alpha, midsq;
100     int x;
101     float *endpoint = dest + N,
102         *base = dest,
103         *origdest = dest;
104
105     assert(width <= N);
106
107     if ((N & 1) == 0)
108     {
109         *dest = 0.0;
110         dest += width;
111         if (dest >= endpoint)
112             dest = ++base;
113         N--;
114     }
115
116     x = -(N / 2);
117     midsq = (double)(x - 1) * (double)(x - 1);
118     I_alpha = I_zero(alpha);
119
120     while (N--)
121     {
122         *dest *= I_zero(alpha * sqrt(1.0 - ((double)x * (double)x) / midsq)) / I_alpha;
123         x++;
124         dest += width;
125         if (dest >= endpoint)
126             dest = ++base;
127     }
128     assert(dest == origdest + width);
129 }
130
131
132 int res_init(res_state *state, int channels, int outfreq, int infreq, res_parameter op1, ...)
133 {
134     double beta = 16.0,
135         cutoff = 0.80,
136         gain = 1.0;
137     int taps = 45;
138
139     int factor;
140
141     assert(state);
142     assert(channels > 0);
143     assert(outfreq > 0);
144     assert(infreq > 0);
145     assert(taps > 0);
146
147     if (state == NULL || channels <= 0 || outfreq <= 0 || infreq <= 0 || taps <= 0)
148         return -1;
149
150     if (op1 != RES_END)
151     {
152         va_list argp;
153         va_start(argp, op1);
154         do
155         {
156             switch (op1)
157             {
158             case RES_GAIN:
159                 gain = va_arg(argp, double);
160                 break;
161
162             case RES_CUTOFF:
163                 cutoff = va_arg(argp, double);
164                 assert(cutoff > 0.01 && cutoff <= 1.0);
165                 break;
166
167             case RES_TAPS:
168                 taps = va_arg(argp, int);
169                 assert(taps > 2 && taps < 1000);
170                 break;
171
172             case RES_BETA:
173                 beta = va_arg(argp, double);
174                 assert(beta > 2.0);
175                 break;
176             default:
177                 assert("arglist" == "valid");
178                 return -1;
179             }
180             op1 = va_arg(argp, res_parameter);
181         } while (op1 != RES_END);
182         va_end(argp);
183     }
184
185     factor = hcf(infreq, outfreq);
186     outfreq /= factor;
187     infreq /= factor;
188
189     /* adjust to rational values for downsampling */
190     if (outfreq < infreq)
191     {
192         /* push the cutoff frequency down to the output frequency */
193         cutoff = cutoff * outfreq / infreq; 
194
195         /* compensate for the sharper roll-off requirement
196          * by using a bigger hammer */
197         taps = taps * infreq/outfreq;
198     }
199
200     assert(taps >= (infreq + outfreq - 1) / outfreq);
201
202     if ((state->table = calloc(outfreq * taps, sizeof(float))) == NULL)
203         return -1;
204     if ((state->pool = calloc(channels * taps, sizeof(SAMPLE))) == NULL)
205     {
206         free(state->table);
207         state->table = NULL;
208         return -1;
209     }
210
211     state->poolfill = taps / 2 + 1;
212     state->channels = channels;
213     state->outfreq = outfreq;
214     state->infreq = infreq;
215     state->taps = taps;
216     state->offset = 0;
217
218     filt_sinc(state->table, outfreq * taps, outfreq, cutoff, gain, taps);
219     win_kaiser(state->table, outfreq * taps, beta, taps);
220
221     return 0;
222 }
223
224
225 static SAMPLE sum(float const *scale, int count, SAMPLE const *source, SAMPLE const *trigger, SAMPLE const *reset, int srcstep)
226 {
227     float total = 0.0;
228
229     while (count--)
230     {
231         total += *source * *scale;
232
233         if (source == trigger)
234             source = reset, srcstep = 1;
235         source -= srcstep;
236         scale++;
237     }
238
239     return total;
240 }
241
242
243 static int push(res_state const * const state, SAMPLE *pool, int * const poolfill, int * const offset, SAMPLE *dest, int dststep, SAMPLE const *source, int srcstep, size_t srclen)
244 {
245     SAMPLE    * const destbase = dest,
246         *poolhead = pool + *poolfill,
247         *poolend = pool + state->taps,
248         *newpool = pool;
249     SAMPLE const *refill, *base, *endpoint;
250     int    lencheck;
251
252
253     assert(state);
254     assert(pool);
255     assert(poolfill);
256     assert(dest);
257     assert(source);
258
259     assert(state->poolfill != -1);
260
261     lencheck = res_push_check(state, srclen);
262
263     /* fill the pool before diving in */
264     while (poolhead < poolend && srclen > 0)
265     {
266         *poolhead++ = *source;
267         source += srcstep;
268         srclen--;
269     }
270
271     if ((long)srclen <= 0)
272         return 0;
273
274     base = source;
275     endpoint = source + srclen * srcstep;
276
277     while (source < endpoint)
278     {
279         *dest = sum(state->table + *offset * state->taps, state->taps, source, base, poolend, srcstep);
280         dest += dststep;
281         *offset += state->infreq;
282         while (*offset >= state->outfreq)
283         {
284             *offset -= state->outfreq;
285             source += srcstep;
286         }
287     }
288
289     assert(dest == destbase + lencheck * dststep);
290
291     /* pretend that source has that underrun data we're not going to get */
292     srclen += (source - endpoint) / srcstep;
293
294     /* if we didn't get enough to completely replace the pool, then shift things about a bit */
295     if (srclen < state->taps)
296     {
297         refill = pool + srclen;
298         while (refill < poolend)
299             *newpool++ = *refill++;
300
301         refill = source - srclen * srcstep;
302     }
303     else
304         refill = source - state->taps * srcstep;
305
306     /* pull in fresh pool data */
307     while (refill < endpoint)
308     {
309         *newpool++ = *refill;
310         refill += srcstep;
311     }
312
313     assert(newpool > pool);
314     assert(newpool <= poolend);
315
316     *poolfill = newpool - pool;
317
318     return (dest - destbase) / dststep;
319 }
320
321
322 int res_push_max_input(res_state const * const state, size_t maxoutput)
323 {
324     return maxoutput * state->infreq / state->outfreq;
325 }
326
327
328 int res_push_check(res_state const * const state, size_t srclen)
329 {
330     if (state->poolfill < state->taps)
331         srclen -= state->taps - state->poolfill;
332
333     return (srclen * state->outfreq - state->offset + state->infreq - 1) / state->infreq;
334 }
335
336
337 int res_push(res_state *state, SAMPLE **dstlist, SAMPLE const **srclist, size_t srclen)
338 {
339     int result = -1, poolfill = -1, offset = -1, i;
340
341     assert(state);
342     assert(dstlist);
343     assert(srclist);
344     assert(state->poolfill >= 0);
345
346     for (i = 0; i < state->channels; i++)
347     {
348         poolfill = state->poolfill;
349         offset = state->offset;
350         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dstlist[i], 1, srclist[i], 1, srclen);
351     }
352     state->poolfill = poolfill;
353     state->offset = offset;
354
355     return result;
356 }
357
358
359 int res_push_interleaved(res_state *state, SAMPLE *dest, SAMPLE const *source, size_t srclen)
360 {
361     int result = -1, poolfill = -1, offset = -1, i;
362
363     assert(state);
364     assert(dest);
365     assert(source);
366     assert(state->poolfill >= 0);
367
368     for (i = 0; i < state->channels; i++)
369     {
370         poolfill = state->poolfill;
371         offset = state->offset;
372         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dest + i, state->channels, source + i, state->channels, srclen);
373     }
374     state->poolfill = poolfill;
375     state->offset = offset;
376
377     return result;
378 }
379
380
381 int res_drain(res_state *state, SAMPLE **dstlist)
382 {
383     SAMPLE *tail;
384     int result = -1, poolfill = -1, offset = -1, i;
385
386     assert(state);
387     assert(dstlist);
388     assert(state->poolfill >= 0);
389
390     if ((tail = calloc(state->taps, sizeof(SAMPLE))) == NULL)
391         return -1;
392
393     for (i = 0; i < state->channels; i++)
394     {
395         poolfill = state->poolfill;
396         offset = state->offset;
397         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dstlist[i], 1, tail, 1, state->taps / 2 - 1);
398     }
399
400     free(tail);
401
402     state->poolfill = -1;
403
404     return result;
405 }
406
407
408 int res_drain_interleaved(res_state *state, SAMPLE *dest)
409 {
410     SAMPLE *tail;
411     int result = -1, poolfill = -1, offset = -1, i;
412
413     assert(state);
414     assert(dest);
415     assert(state->poolfill >= 0);
416
417     if ((tail = calloc(state->taps, sizeof(SAMPLE))) == NULL)
418         return -1;
419
420     for (i = 0; i < state->channels; i++)
421     {
422         poolfill = state->poolfill;
423         offset = state->offset;
424         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dest + i, state->channels, tail, 1, state->taps / 2 - 1);
425     }
426
427     free(tail);
428
429     state->poolfill = -1;
430
431     return result;
432 }
433
434
435 void res_clear(res_state *state)
436 {
437     assert(state);
438     assert(state->table);
439     assert(state->pool);
440
441     free(state->table);
442     free(state->pool);
443     memset(state, 0, sizeof(*state));
444 }