Files: f8137a6ba7c36d2d53855c2c3c4fdd0961c4d9b6 / src / syntrax / resampler.c
8838 bytesRaw
1 | |
2 | |
3 | |
4 | |
5 | |
6 | |
7 | /* Copyright (C) 2004-2008 Shay Green. |
8 | Copyright (C) 2015 Christopher Snowhill. This module is free software; you |
9 | can redistribute it and/or modify it under the terms of the GNU Lesser |
10 | General Public License as published by the Free Software Foundation; either |
11 | version 2.1 of the License, or (at your option) any later version. This |
12 | module is distributed in the hope that it will be useful, but WITHOUT ANY |
13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS |
14 | FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more |
15 | details. You should have received a copy of the GNU Lesser General Public |
16 | License along with this module; if not, write to the Free Software Foundation, |
17 | Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */ |
18 | |
19 | |
20 | |
21 | |
22 | enum { imp_scale = 0x7FFF }; |
23 | typedef int16_t imp_t; |
24 | typedef int32_t imp_off_t; /* for max_res of 512 and impulse width of 32, end offsets must be 32 bits */ |
25 | |
26 | |
27 | typedef int32_t intermediate_t; |
28 | |
29 | typedef int64_t intermediate_t; |
30 | |
31 | |
32 | static void gen_sinc( double rolloff, int width, double offset, double spacing, double scale, |
33 | int count, imp_t* out ) |
34 | { |
35 | double angle; |
36 | |
37 | double const maxh = 256; |
38 | double const step = PI / maxh * spacing; |
39 | double const to_w = maxh * 2 / width; |
40 | double const pow_a_n = pow( rolloff, maxh ); |
41 | scale /= maxh * 2; |
42 | angle = (count / 2 - 1 + offset) * -step; |
43 | |
44 | while ( count-- ) |
45 | { |
46 | double w; |
47 | *out++ = 0; |
48 | w = angle * to_w; |
49 | if ( fabs( w ) < PI ) |
50 | { |
51 | double rolloff_cos_a = rolloff * cos( angle ); |
52 | double num = 1 - rolloff_cos_a - |
53 | pow_a_n * cos( maxh * angle ) + |
54 | pow_a_n * rolloff * cos( (maxh - 1) * angle ); |
55 | double den = 1 - rolloff_cos_a - rolloff_cos_a + rolloff * rolloff; |
56 | double sinc = scale * num / den - scale; |
57 | |
58 | out [-1] = (imp_t) (cos( w ) * sinc + sinc); |
59 | } |
60 | angle += step; |
61 | } |
62 | } |
63 | |
64 | enum { width = 32 }; |
65 | enum { max_res = 512 }; |
66 | enum { min_width = (width < 4 ? 4 : width) }; |
67 | enum { adj_width = min_width / 4 * 4 + 2 }; |
68 | enum { write_offset = adj_width }; |
69 | |
70 | enum { buffer_size = 128 }; |
71 | |
72 | typedef struct _resampler |
73 | { |
74 | int width_; |
75 | int rate_; |
76 | int inptr; |
77 | int infilled; |
78 | int outptr; |
79 | int outfilled; |
80 | |
81 | int latency; |
82 | |
83 | imp_t const* imp; |
84 | imp_t impulses [max_res * (adj_width + 2 * (sizeof(imp_off_t) / sizeof(imp_t)))]; |
85 | sample_t buffer_in[buffer_size * 2]; |
86 | sample_t buffer_out[buffer_size]; |
87 | } resampler; |
88 | |
89 | void * resampler_create() |
90 | { |
91 | resampler *r = (resampler *) malloc(sizeof(resampler)); |
92 | if (r) resampler_clear(r); |
93 | return r; |
94 | } |
95 | |
96 | void * resampler_dup(const void *_r) |
97 | { |
98 | void *_t = (resampler *) malloc(sizeof(resampler)); |
99 | if (_t) resampler_dup_inplace(_t, _r); |
100 | return _t; |
101 | } |
102 | |
103 | void resampler_dup_inplace(void *_t, const void *_r) |
104 | { |
105 | const resampler *r = (const resampler *)_r; |
106 | resampler *t = (resampler *)_t; |
107 | if (r && t) |
108 | { |
109 | memcpy(t, r, sizeof(resampler)); |
110 | t->imp = t->impulses + (r->imp - r->impulses); |
111 | } |
112 | else if (t) |
113 | { |
114 | resampler_clear(t); |
115 | } |
116 | } |
117 | |
118 | void resampler_destroy(void *r) |
119 | { |
120 | free(r); |
121 | } |
122 | |
123 | void resampler_clear(void *_r) |
124 | { |
125 | resampler * r = (resampler *)_r; |
126 | r->width_ = adj_width; |
127 | r->inptr = 0; |
128 | r->infilled = 0; |
129 | r->outptr = 0; |
130 | r->outfilled = 0; |
131 | r->latency = 0; |
132 | r->imp = r->impulses; |
133 | |
134 | resampler_set_rate(r, 1.0); |
135 | } |
136 | |
137 | void resampler_set_rate( void *_r, double new_factor ) |
138 | { |
139 | int step; //const |
140 | double filter; //const |
141 | double fraction, pos; |
142 | int n; |
143 | |
144 | resampler *rs = (resampler *)_r; |
145 | imp_t* out; |
146 | |
147 | double const rolloff = 0.999; |
148 | double const gain = 1.0; |
149 | |
150 | /* determine number of sub-phases that yield lowest error */ |
151 | double ratio_ = 0.0; |
152 | int res = -1; |
153 | { |
154 | double least_error = 2; |
155 | double pos = 0; |
156 | int r; |
157 | for ( r = 1; r <= max_res; r++ ) |
158 | { |
159 | double nearest, error; |
160 | pos += new_factor; |
161 | nearest = floor( pos + 0.5 ); |
162 | error = fabs( pos - nearest ); |
163 | if ( error < least_error ) |
164 | { |
165 | res = r; |
166 | ratio_ = nearest / res; |
167 | least_error = error; |
168 | } |
169 | } |
170 | } |
171 | rs->rate_ = ratio_; |
172 | |
173 | /* how much of input is used for each output sample */ |
174 | step = (int) floor( ratio_ ); |
175 | fraction = fmod( ratio_, 1.0 ); |
176 | |
177 | filter = (ratio_ < 1.0) ? 1.0 : 1.0 / ratio_; |
178 | pos = 0.0; |
179 | /*int input_per_cycle = 0;*/ |
180 | out = rs->impulses; |
181 | |
182 | for ( n = res; --n >= 0; ) |
183 | { |
184 | int cur_step; |
185 | |
186 | gen_sinc( rolloff, (int) (rs->width_ * filter + 1) & ~1, pos, filter, |
187 | (double)(imp_scale * gain * filter), (int) rs->width_, out ); |
188 | out += rs->width_; |
189 | |
190 | cur_step = step; |
191 | pos += fraction; |
192 | if ( pos >= 0.9999999 ) |
193 | { |
194 | pos -= 1.0; |
195 | cur_step += 1; |
196 | } |
197 | |
198 | ((imp_off_t*)out)[0] = (cur_step - rs->width_ + 2) * sizeof (sample_t); |
199 | ((imp_off_t*)out)[1] = 2 * sizeof (imp_t) + 2 * sizeof (imp_off_t); |
200 | out += 2 * (sizeof(imp_off_t) / sizeof(imp_t)); |
201 | /*input_per_cycle += cur_step;*/ |
202 | } |
203 | /* last offset moves back to beginning of impulses*/ |
204 | ((imp_off_t*)out) [-1] -= (char*) out - (char*) rs->impulses; |
205 | |
206 | rs->imp = rs->impulses; |
207 | } |
208 | |
209 | int resampler_get_free(void *_r) |
210 | { |
211 | resampler *r = (resampler *)_r; |
212 | return buffer_size - r->infilled; |
213 | } |
214 | |
215 | int resampler_get_min_fill(void *_r) |
216 | { |
217 | resampler *r = (resampler *)_r; |
218 | const int min_needed = write_offset + 1; |
219 | const int latency = r->latency ? 0 : adj_width; |
220 | int min_free = min_needed - r->infilled - latency; |
221 | return min_free < 0 ? 0 : min_free; |
222 | } |
223 | |
224 | void resampler_write_sample(void *_r, sample_t s) |
225 | { |
226 | resampler *r = (resampler *)_r; |
227 | |
228 | if (!r->latency) |
229 | { |
230 | int i; |
231 | for ( i = 0; i < adj_width / 2; ++i) |
232 | { |
233 | r->buffer_in[r->inptr] = 0; |
234 | r->buffer_in[buffer_size + r->inptr] = 0; |
235 | r->inptr = (r->inptr + 1) % (buffer_size); |
236 | r->infilled += 1; |
237 | } |
238 | r->latency = 1; |
239 | } |
240 | |
241 | if (r->infilled < buffer_size) |
242 | { |
243 | r->buffer_in[r->inptr] = s; |
244 | r->buffer_in[buffer_size + r->inptr + 0] = s; |
245 | r->inptr = (r->inptr + 1) % (buffer_size); |
246 | r->infilled += 1; |
247 | } |
248 | } |
249 | |
250 | |
251 | |
252 | |
253 | |
254 | static const sample_t * resampler_inner_loop( resampler *r, sample_t** out_, |
255 | sample_t const* out_end, sample_t const in [], int in_size ) |
256 | { |
257 | in_size -= write_offset; |
258 | if ( in_size > 0 ) |
259 | { |
260 | sample_t* restrict out = *out_; |
261 | sample_t const* const in_end = in + in_size; |
262 | imp_t const* imp = r->imp; |
263 | |
264 | do |
265 | { |
266 | int n; |
267 | /* accumulate in extended precision*/ |
268 | int pt = imp [0]; |
269 | intermediate_t s = (intermediate_t)pt * (intermediate_t)(in [0]); |
270 | if ( out >= out_end ) |
271 | break; |
272 | for ( n = (adj_width - 2) / 2; n; --n ) |
273 | { |
274 | pt = imp [1]; |
275 | s += (intermediate_t)pt * (intermediate_t)(in [1]); |
276 | |
277 | /* pre-increment more efficient on some RISC processors*/ |
278 | imp += 2; |
279 | pt = imp [0]; |
280 | s += (intermediate_t)pt * (intermediate_t)(in [2]); |
281 | in += 2; |
282 | } |
283 | pt = imp [1]; |
284 | s += (intermediate_t)pt * (intermediate_t)(in [1]); |
285 | |
286 | /* these two "samples" after the end of the impulse give the |
287 | * proper offsets to the next input sample and next impulse */ |
288 | in = (sample_t const*) ((char const*) in + ((imp_off_t*)(&imp [2]))[0]); /* some negative value */ |
289 | imp = (imp_t const*) ((char const*) imp + ((imp_off_t*)(&imp [2]))[1]); /* small positive or large negative */ |
290 | |
291 | out [0] = (sample_t) (s >> 15); |
292 | out += 1; |
293 | } |
294 | while ( in < in_end ); |
295 | |
296 | r->imp = imp; |
297 | *out_ = out; |
298 | } |
299 | return in; |
300 | } |
301 | |
302 | |
303 | |
304 | static int resampler_wrapper( resampler *r, sample_t out [], int* out_size, |
305 | sample_t const in [], int in_size ) |
306 | { |
307 | sample_t* out_ = out; |
308 | int result = resampler_inner_loop( r, &out_, out + *out_size, in, in_size ) - in; |
309 | |
310 | *out_size = out_ - out; |
311 | return result; |
312 | } |
313 | |
314 | static void resampler_fill( resampler *r ) |
315 | { |
316 | while (!r->outfilled && r->infilled) |
317 | { |
318 | int inread; |
319 | |
320 | int writepos = ( r->outptr + r->outfilled ) % (buffer_size); |
321 | int writesize = (buffer_size) - writepos; |
322 | if ( writesize > ( buffer_size - r->outfilled ) ) |
323 | writesize = buffer_size - r->outfilled; |
324 | inread = resampler_wrapper(r, &r->buffer_out[writepos], &writesize, &r->buffer_in[buffer_size + r->inptr - r->infilled], r->infilled); |
325 | r->infilled -= inread; |
326 | r->outfilled += writesize; |
327 | if (!inread) |
328 | break; |
329 | } |
330 | } |
331 | |
332 | int resampler_get_avail(void *_r) |
333 | { |
334 | resampler *r = (resampler *)_r; |
335 | if (r->outfilled < 1 && r->infilled >= r->width_) |
336 | resampler_fill( r ); |
337 | return r->outfilled; |
338 | } |
339 | |
340 | static void resampler_read_sample_internal( resampler *r, sample_t *s, int advance ) |
341 | { |
342 | if (r->outfilled < 1) |
343 | resampler_fill( r ); |
344 | if (r->outfilled < 1) |
345 | { |
346 | *s = 0; |
347 | return; |
348 | } |
349 | *s = r->buffer_out[r->outptr]; |
350 | if (advance) |
351 | { |
352 | r->outptr = (r->outptr + 1) % (buffer_size); |
353 | r->outfilled -= 1; |
354 | } |
355 | } |
356 | |
357 | void resampler_read_sample( void *_r, sample_t *s ) |
358 | { |
359 | resampler *r = (resampler *)_r; |
360 | resampler_read_sample_internal(r, s, 1); |
361 | } |
362 | |
363 | void resampler_peek_sample( void *_r, sample_t *s ) |
364 | { |
365 | resampler *r = (resampler *)_r; |
366 | resampler_read_sample_internal(r, s, 0); |
367 | } |
368 |
Built with git-ssb-web