Mercurial > audlegacy
comparison Plugins/Input/mpg123/paranoia.c @ 1119:042f189bd225 trunk
[svn] - rename psycho.c to paranoia.c, as other stuff will be here (such as frequency locking code, to fix warble, etc.)
author | nenolod |
---|---|
date | Tue, 30 May 2006 22:52:11 -0700 |
parents | Plugins/Input/mpg123/psycho.c@9ebbf4d00d89 |
children | 8e6b7ed7dd88 |
comparison
equal
deleted
inserted
replaced
1118:9ebbf4d00d89 | 1119:042f189bd225 |
---|---|
1 /* libmpgdec: An advanced MPEG layer 1/2/3 decoder. | |
2 * psycho.c: Psychoaccoustic modeling. | |
3 * | |
4 * Copyright (C) 2005-2006 William Pitcock <nenolod@nenolod.net> | |
5 * Portions copyright (C) 2001 Rafal Bosak <gyver@fanthom.irc.pl> | |
6 * Portions copyright (C) 1999 Michael Hipp | |
7 * | |
8 * This program is free software; you can redistribute it and/or modify | |
9 * it under the terms of the GNU General Public License as published by | |
10 * the Free Software Foundation; either version 2 of the License, or | |
11 * (at your option) any later version. | |
12 * | |
13 * This program is distributed in the hope that it will be useful, | |
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
16 * GNU General Public License for more details. | |
17 * | |
18 * You should have received a copy of the GNU General Public License | |
19 * along with this program; if not, write to the Free Software | |
20 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA | |
21 */ | |
22 | |
23 #include <stdio.h> | |
24 #include <stdlib.h> | |
25 #include <math.h> | |
26 | |
27 #include "common.h" | |
28 | |
29 int bext_level; | |
30 int stereo_level; | |
31 int filter_level; | |
32 int harmonics_level; | |
33 int bext_sfactor; | |
34 int stereo_sfactor; | |
35 int harmonics_sfactor; | |
36 int enable_plugin = 1; | |
37 int lsine[65536]; | |
38 int rsine[65536]; | |
39 | |
40 /* | |
41 * Initialize and configure the psychoaccoustics engine. | |
42 */ | |
43 void psycho_init(void) | |
44 { | |
45 int i, x; | |
46 double lsum; | |
47 double rsum; | |
48 | |
49 bext_level = 28; | |
50 stereo_level = stereo_sfactor = 16; | |
51 filter_level = 3; | |
52 harmonics_level = harmonics_sfactor = 43; | |
53 | |
54 bext_sfactor = (float)(((float)16384 * 10) / (float)(bext_level + 1)) + (float)(102 - bext_level) * 128; | |
55 | |
56 #define COND 0 | |
57 /* calculate sinetables */ | |
58 for (i = 0; i < 32768; ++i) | |
59 { | |
60 lsum = rsum = 0; | |
61 if (COND || i < 32768 )lsum+= ((cos((double)i * 3.141592535 / 32768/2 )) + 0) / 2; | |
62 if (COND || i < 16384 )rsum-= ((cos((double)i * 3.141592535 / 16384/2 )) + 0) / 4; | |
63 rsum = lsum; | |
64 | |
65 if (COND || i < 8192 ) lsum += ((cos((double)i * 3.141592535 / 8192/2 )) + 0) / 8; | |
66 if (COND || i < 5641 ) rsum += ((cos((double)i * 3.141592535 / 5641.333333/2)) + 0) / 8; | |
67 | |
68 lsine[32768 + i] = (double)(lsum - 0.5) * 32768 * 1.45; | |
69 lsine[32768 - i] = lsine[32768 + i]; | |
70 rsine[32768 + i] = (double)(rsum - 0.5) * 32768 * 1.45; | |
71 rsine[32768 - i] = rsine[32768 + i]; | |
72 | |
73 x = i; | |
74 } | |
75 } | |
76 | |
77 /* | |
78 * This routine computes a reverb for the track. | |
79 */ | |
80 | |
81 #define DELAY2 21000 | |
82 #define DELAY1 35000 | |
83 #define DELAY3 14000 | |
84 #define DELAY4 5 | |
85 | |
86 #define BUF_SIZE DELAY1+DELAY2+DELAY3+DELAY4 | |
87 | |
88 void echo3d(gint16 *data, int datasize) | |
89 { | |
90 int x; | |
91 int left, right, dif, left0, right0, left1, right1, left2, right2, left3, right3, left4, right4, leftc, rightc, lsf, rsf; | |
92 static int left0p = 0, right0p = 0; | |
93 static int rangeErrorsUp = 0; | |
94 static int rangeErrorsDown = 0; | |
95 gint16 *dataptr; | |
96 static int l0, l1, l2, r0, r1, r2, ls, rs, ls1, rs1; | |
97 static int ll0, ll1, ll2, rr0, rr1, rr2; | |
98 static int lharmb = 0, rharmb = 0, lhfb = 0, rhfb = 0; | |
99 int lharm0, rharm0; | |
100 static gint16 buf[BUF_SIZE]; | |
101 static int bufPos1 = 1 + BUF_SIZE - DELAY1; | |
102 static int bufPos2 = 1 + BUF_SIZE - DELAY1 - DELAY2; | |
103 static int bufPos3 = 1 + BUF_SIZE - DELAY1 - DELAY2 - DELAY3; | |
104 static int bufPos4 = 1 + BUF_SIZE - DELAY1 - DELAY2 - DELAY3 - DELAY4; | |
105 dataptr = data; | |
106 | |
107 for (x = 0; x < datasize; x += 4) { | |
108 | |
109 // ************ load sample ********** | |
110 left0 = dataptr[0]; | |
111 right0 = dataptr[1]; | |
112 | |
113 // ************ slightly expand stereo for direct input ********** | |
114 ll0=left0;rr0=right0; | |
115 dif = (ll0+ll1+ll2 - rr0-rr1-rr2) * stereo_sfactor / 256; | |
116 left0 += dif; | |
117 right0 -= dif; | |
118 ll2= ll1; ll1= ll0; | |
119 rr2= rr1; rr1= rr0; | |
120 | |
121 // ************ echo from buffer - first echo ********** | |
122 // ************ ********** | |
123 left1 = buf[bufPos1++]; | |
124 if (bufPos1 == BUF_SIZE) | |
125 bufPos1 = 0; | |
126 right1 = buf[bufPos1++]; | |
127 if (bufPos1 == BUF_SIZE) | |
128 bufPos1 = 0; | |
129 | |
130 // ************ highly expand stereo for first echo ********** | |
131 dif = (left1 - right1); | |
132 left1 = left1 + dif; | |
133 right1 = right1 - dif; | |
134 | |
135 // ************ second echo ********** | |
136 left2 = buf[bufPos2++]; | |
137 if (bufPos2 == BUF_SIZE) | |
138 bufPos2 = 0; | |
139 right2 = buf[bufPos2++]; | |
140 if (bufPos2 == BUF_SIZE) | |
141 bufPos2 = 0; | |
142 | |
143 // ************ expand stereo for second echo ********** | |
144 dif = (left2 - right2); | |
145 left2 = left2 - dif; | |
146 right2 = right2 - dif; | |
147 | |
148 // ************ third echo ********** | |
149 left3 = buf[bufPos3++]; | |
150 if (bufPos3 == BUF_SIZE) | |
151 bufPos3 = 0; | |
152 right3 = buf[bufPos3++]; | |
153 if (bufPos3 == BUF_SIZE) | |
154 bufPos3 = 0; | |
155 | |
156 // ************ fourth echo ********** | |
157 left4 = buf[bufPos4++]; | |
158 if (bufPos4 == BUF_SIZE) | |
159 bufPos4 = 0; | |
160 right4 = buf[bufPos4++]; | |
161 if (bufPos4 == BUF_SIZE) | |
162 bufPos4 = 0; | |
163 | |
164 left3 = (left4+left3) / 2; | |
165 right3 = (right4+right3) / 2; | |
166 | |
167 // ************ expand stereo for second echo ********** | |
168 dif = (left4 - right4); | |
169 left3 = left3 - dif; | |
170 right3 = right3 - dif; | |
171 | |
172 // ************ a weighted sum taken from reverb buffer ********** | |
173 leftc = left1 / 9 + right2 /8 + left3 / 8; | |
174 rightc = right1 / 11 + left2 / 9 + right3 / 10; | |
175 | |
176 left = left0p; | |
177 right = right0p; | |
178 | |
179 l0 = leftc + left0 / 2; | |
180 r0 = rightc + right0 / 2; | |
181 | |
182 ls = l0 + l1 + l2; // do not reverb high frequencies (filter) | |
183 rs = r0 + r1 + r2; // | |
184 | |
185 // ************ add some extra even harmonics ********** | |
186 // ************ or rather specific nonlinearity | |
187 | |
188 lhfb = lhfb + (ls * 32768 - lhfb) / 32; | |
189 rhfb = rhfb + (rs * 32768 - rhfb) / 32; | |
190 | |
191 lsf = ls - lhfb / 32768; | |
192 rsf = rs - rhfb / 32768; | |
193 | |
194 lharm0 = 0 | |
195 + ((lsf + 10000) * ((((lsine[((lsf/4) + 32768 + 65536) % 65536] * harmonics_sfactor)) / 64))) / 32768 | |
196 - ((lsine[((lsf/4) + 32768 +65536) % 65536]) * harmonics_sfactor) / 128 | |
197 ; | |
198 | |
199 rharm0 = | |
200 + ((rsf + 10000) * ((((lsine[((rsf/4) + 32768 + 65536) % 65536] * harmonics_sfactor)) / 64))) / 32768 | |
201 - ((rsine[((rsf/4) + 32768 +65536) % 65536]) * harmonics_sfactor) / 128 | |
202 ; | |
203 | |
204 lharmb = lharmb + (lharm0 * 32768 - lharmb) / 16384; | |
205 rharmb = rharmb + (rharm0 * 32768 - rharmb) / 16384; | |
206 | |
207 // ************ for convolution filters ********** | |
208 l2= l1; r2= r1; | |
209 l1 = l0; r1 = r0; | |
210 ls1 = ls; rs1 = rs; | |
211 | |
212 left = 0 + lharm0 - lharmb / 32768 + left; | |
213 right = 0 + rharm0 - rharmb / 32768 + right; | |
214 | |
215 left0p = left0; | |
216 right0p = right0; | |
217 | |
218 | |
219 // ************ limiter ********** | |
220 if (left < -32768) { | |
221 left = -32768; // limit | |
222 rangeErrorsDown++; | |
223 } | |
224 else if (left > 32767) { | |
225 left = 32767; | |
226 rangeErrorsUp++; | |
227 } | |
228 if (right < -32768) { | |
229 right = -32768; | |
230 rangeErrorsDown++; | |
231 } | |
232 else if (right > 32767) { | |
233 right = 32767; | |
234 rangeErrorsUp++; | |
235 } | |
236 // ************ store sample ********** | |
237 dataptr[0] = left; | |
238 dataptr[1] = right; | |
239 dataptr += 2; | |
240 | |
241 } | |
242 } | |
243 | |
244 /* | |
245 * simple pith shifter, plays short fragments at 1.5 speed | |
246 */ | |
247 void pitchShifter(const int lin, const int rin, int *lout, int *rout) | |
248 { | |
249 #define SH_BUF_SIZE 100 * 3 | |
250 static gint16 shBuf[SH_BUF_SIZE]; | |
251 static int shBufPos = SH_BUF_SIZE - 6; | |
252 static int shBufPos1 = SH_BUF_SIZE - 6; | |
253 static int cond; | |
254 | |
255 shBuf[shBufPos++] = lin; | |
256 shBuf[shBufPos++] = rin; | |
257 | |
258 if (shBufPos == SH_BUF_SIZE) shBufPos = 0; | |
259 | |
260 switch (cond){ | |
261 case 1: | |
262 *lout = (shBuf[shBufPos1 + 0] * 2 + shBuf[shBufPos1 + 2])/4; | |
263 *rout = (shBuf[shBufPos1 + 1] * 2 + shBuf[shBufPos1 + 3])/4; | |
264 break; | |
265 case 0: | |
266 *lout = (shBuf[shBufPos1 + 4] * 2 + shBuf[shBufPos1 + 2])/4; | |
267 *rout = (shBuf[shBufPos1 + 5] * 2 + shBuf[shBufPos1 + 3])/4; | |
268 cond = 2; | |
269 shBufPos1 += 6; | |
270 if (shBufPos1 == SH_BUF_SIZE) { | |
271 shBufPos1 = 0; | |
272 } | |
273 break; | |
274 } | |
275 cond--; | |
276 } | |
277 | |
278 | |
279 struct Interpolation{ | |
280 int acount; // counter | |
281 int lval, rval; // value | |
282 int sal, sar; // sum | |
283 int al, ar; | |
284 int a1l, a1r; | |
285 }; | |
286 | |
287 /* | |
288 * interpolation routine for ampliude and "energy" | |
289 */ | |
290 static inline void interpolate(struct Interpolation *s, int l, int r) | |
291 { | |
292 #define AMPL_COUNT 64 | |
293 int a0l, a0r, dal = 0, dar = 0; | |
294 | |
295 if (l < 0) l = -l; | |
296 if (r < 0) r = -r; | |
297 | |
298 s->lval += l / 8; | |
299 s->rval += r / 8; | |
300 | |
301 s->lval = (s->lval * 120) / 128; | |
302 s->rval = (s->rval * 120) / 128; | |
303 | |
304 s->sal += s->lval; | |
305 s->sar += s->rval; | |
306 | |
307 s->acount++; | |
308 if (s->acount == AMPL_COUNT){ | |
309 s->acount = 0; | |
310 a0l = s->a1l; | |
311 a0r = s->a1r; | |
312 s->a1l = s->sal / AMPL_COUNT; | |
313 s->a1r = s->sar / AMPL_COUNT; | |
314 s->sal = 0; | |
315 s->sar = 0; | |
316 dal = s->a1l - a0l; | |
317 dar = s->a1r - a0r; | |
318 s->al = a0l * AMPL_COUNT; | |
319 s->ar = a0r * AMPL_COUNT; | |
320 } | |
321 | |
322 s->al += dal; | |
323 s->ar += dar; | |
324 } | |
325 | |
326 /* | |
327 * calculate scalefactor for mixer | |
328 */ | |
329 inline int calc_scalefactor(int a, int e) | |
330 { | |
331 int x; | |
332 | |
333 if (a > 8192) a = 8192; | |
334 else if (a < 0) a = 0; | |
335 | |
336 if (e > 8192) e = 8192; | |
337 else if (e < 0) e = 0; | |
338 | |
339 x = ((e+500) * 4096 )/ (a + 300) + e; | |
340 | |
341 if (x > 16384) x = 16384; | |
342 else if (x < 0) x = 0; | |
343 return x; | |
344 } | |
345 | |
346 static struct Interpolation bandext_energy; | |
347 static struct Interpolation bandext_amplitude; | |
348 | |
349 /* | |
350 * exact bandwidth extender ("exciter") routine | |
351 */ | |
352 static void bandext(gint16 *data, const int datasize) | |
353 { | |
354 | |
355 int x; | |
356 static int saw; // test stuff | |
357 int left, right; | |
358 gint16 *dataptr = data; | |
359 static int lprev0, rprev0, lprev1, rprev1, lprev2, rprev2; | |
360 int left0, right0, left1, right1, left2, right2, left3, right3; | |
361 static int lamplUp, lamplDown; | |
362 static int ramplUp, ramplDown; | |
363 int lampl, rampl; | |
364 int tmp; | |
365 | |
366 for (x = 0; x < datasize; x += 4) { | |
367 | |
368 // ************ load sample ********** | |
369 left0 = dataptr[0]; | |
370 right0 = dataptr[1]; | |
371 | |
372 // ************ highpass filter part 1 ********** | |
373 left1 = (left0 - lprev0) * 56880 / 65536; | |
374 right1 = (right0 - rprev0) * 56880 / 65536; | |
375 | |
376 left2 = (left1 - lprev1) * 56880 / 65536; | |
377 right2 = (right1 - rprev1) * 56880 / 65536; | |
378 | |
379 left3 = (left2 - lprev2) * 56880 / 65536; | |
380 right3 = (right2 - rprev2) * 56880 / 65536; | |
381 | |
382 switch (filter_level){ | |
383 case 1: | |
384 pitchShifter(left1, right1, &left, &right); | |
385 break; | |
386 case 2: | |
387 pitchShifter(left2, right2, &left, &right); | |
388 break; | |
389 case 3: | |
390 pitchShifter(left3, right3, &left, &right); | |
391 break; | |
392 } | |
393 | |
394 // ************ amplitude detector ************ | |
395 tmp = left1 + lprev1; | |
396 if (tmp * 16 > lamplUp ) lamplUp += (tmp - lamplUp ); | |
397 else if (tmp * 16 < lamplDown) lamplDown += (tmp - lamplDown); | |
398 lamplUp = (lamplUp * 1000) /1024; | |
399 lamplDown = (lamplDown * 1000) /1024; | |
400 lampl = lamplUp - lamplDown; | |
401 | |
402 tmp = right1 + rprev1; | |
403 if (tmp * 16 > ramplUp ) ramplUp += (tmp - ramplUp ); | |
404 else if (tmp * 16 < ramplDown) ramplDown += (tmp - ramplDown); | |
405 ramplUp = (ramplUp * 1000) /1024; | |
406 ramplDown = (ramplDown * 1000) /1024; | |
407 rampl = ramplUp - ramplDown; | |
408 | |
409 interpolate(&bandext_amplitude, lampl, rampl); | |
410 | |
411 // ************ "sound energy" detector (approx. spectrum complexity) *********** | |
412 interpolate(&bandext_energy, left0 - lprev0, right0 - rprev0); | |
413 | |
414 // ************ mixer *********** | |
415 left = left0 + left * calc_scalefactor(bandext_amplitude.lval, bandext_energy.lval) / bext_sfactor; | |
416 right = right0 + right * calc_scalefactor(bandext_amplitude.rval, bandext_energy.rval) / bext_sfactor; //16384 | |
417 | |
418 saw = (saw + 2048) & 0x7fff; | |
419 | |
420 lprev0 = left0; | |
421 rprev0 = right0; | |
422 lprev1 = left1; | |
423 rprev1 = right1; | |
424 lprev2 = left2; | |
425 rprev2 = right2; | |
426 | |
427 if (left < -32768) left = -32768; | |
428 else if (left > 32767) left = 32767; | |
429 if (right < -32768) right = -32768; | |
430 else if (right > 32767) right = 32767; | |
431 | |
432 dataptr[0] = left; | |
433 dataptr[1] = right; | |
434 dataptr += 2; | |
435 } | |
436 } | |
437 | |
438 int psycho_process(void *data, int length, int nch) | |
439 { | |
440 if (nch != 2) | |
441 return length; /* XXX: we cant process mono yet */ | |
442 | |
443 echo3d((gint16 *)data, length); | |
444 bandext((gint16 *)data, length); | |
445 return length; | |
446 } | |
447 |