An audio player for macOS 10.8 and newer. https://kode54.net/cog
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

368 lines
8.6KB

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