1 /*
2 Copyright (C) 2009 Fons Adriaensen <fons@kokkinizita.net>
3 and Jörn Nettingsmeier <nettings@stackingdwarves.net>
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program; if not, write to the Free Software
17 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18 */
19
20
21 #include <string.h>
22 #include <math.h>
23 #include "ambisonic3.h"
24
25
26 #define DEG2RAD 0.01745329f
27 #define MIN3DB 0.707107f
28
29
setport(unsigned long port,LADSPA_Data * data)30 void Ladspa_Monopan31::setport (unsigned long port, LADSPA_Data *data)
31 {
32 _port [port] = data;
33 }
34
35
active(bool act)36 void Ladspa_Monopan31::active (bool act)
37 {
38 if (act) calcpar (0.0f, 0.0f);
39 }
40
41
calcpar(float az,float el)42 void Ladspa_Monopan31::calcpar (float az, float el)
43 {
44 float t, x2, y2;
45
46 az *= DEG2RAD;
47 el *= DEG2RAD;
48 t = cosf (el);
49 _xx = t * cosf (-az);
50 _yy = t * sinf (-az);
51 _zz = sinf (el);
52 x2 = _xx * _xx;
53 y2 = _yy * _yy;
54 _uu = x2 - y2;
55 _vv = 2 * _xx * _yy;
56 _pp = (x2 - 3 * y2) * _xx;
57 _qq = (3 * x2 - y2) * _yy;
58 }
59
60
runproc(unsigned long len,bool add)61 void Ladspa_Monopan31::runproc (unsigned long len, bool add)
62 {
63 float t, xx, yy, zz, uu, vv, pp, qq;
64 float dxx, dyy, dzz, duu, dvv, dpp, dqq;
65
66 float *in, *out_w, *out_x, *out_y, *out_z, *out_u, *out_v, *out_p, *out_q;
67
68 xx = _xx;
69 yy = _yy;
70 zz = _zz;
71 uu = _uu;
72 vv = _vv;
73 pp = _pp;
74 qq = _qq;
75
76 calcpar (_port [CTL_AZIM][0], _port [CTL_ELEV][0]);
77
78 dxx = (_xx - xx) / len;
79 dyy = (_yy - yy) / len;
80 dzz = (_zz - zz) / len;
81 duu = (_uu - uu) / len;
82 dvv = (_vv - vv) / len;
83 dpp = (_pp - pp) / len;
84 dqq = (_qq - qq) / len;
85
86 in = _port [INP];
87
88 out_w = _port [OUT_W];
89 out_x = _port [OUT_X];
90 out_y = _port [OUT_Y];
91 out_z = _port [OUT_Z];
92 out_u = _port [OUT_U];
93 out_v = _port [OUT_V];
94 out_p = _port [OUT_P];
95 out_q = _port [OUT_Q];
96
97 while (len--)
98 {
99 xx += dxx;
100 yy += dyy;
101 zz += dzz;
102 uu += duu;
103 vv += dvv;
104 pp += dpp;
105 qq += dqq;
106
107 t = *in++;
108 *out_w++ = MIN3DB * t;
109 *out_x++ = xx * t;
110 *out_y++ = yy * t;
111 *out_z++ = zz * t;
112 *out_u++ = uu * t;
113 *out_v++ = vv * t;
114 *out_p++ = pp * t;
115 *out_q++ = qq * t;
116 }
117 }
118
119
120
setport(unsigned long port,LADSPA_Data * data)121 void Ladspa_Rotator31::setport (unsigned long port, LADSPA_Data *data)
122 {
123 _port [port] = data;
124 }
125
126
active(bool act)127 void Ladspa_Rotator31::active (bool act)
128 {
129 if (act) calcpar (0.0f);
130 }
131
132
calcpar(float az)133 void Ladspa_Rotator31::calcpar (float az)
134 {
135 az *= DEG2RAD;
136 _c1 = cosf (az);
137 _s1 = sinf (az);
138 _c2 = _c1 * _c1 - _s1 * _s1;
139 _s2 = 2 * _c1 * _s1;
140 _c3 = _c2 * _c1 - _s2 * _s1;
141 _s3 = _c2 * _s1 + _c1 * _s2;
142 }
143
144
runproc(unsigned long len,bool add)145 void Ladspa_Rotator31::runproc (unsigned long len, bool add)
146 {
147 int k;
148 float c1, s1, c2, s2, c3, s3;
149 float x, y, dc, ds;
150 float *inp1, *inp2, *out1, *out2;
151
152 // W and Z are invariant under rotation around the z axis.
153
154 memcpy (_port [OUT_W], _port [INP_W], len * sizeof (float));
155 memcpy (_port [OUT_Z], _port [INP_Z], len * sizeof (float));
156
157 c1 = _c1;
158 s1 = _s1;
159 c2 = _c2;
160 s2 = _s2;
161 c3 = _c3;
162 s3 = _s3;
163
164 calcpar (_port [CTL_AZIM][0]);
165
166 inp1 = _port [INP_X];
167 inp2 = _port [INP_Y];
168 out1 = _port [OUT_X];
169 out2 = _port [OUT_Y];
170 k = len;
171 dc = (_c1 - c1) / len;
172 ds = (_s1 - s1) / len;
173 while (k--)
174 {
175 c1 += dc;
176 s1 += ds;
177 x = *inp1++;
178 y = *inp2++;
179 *out1++ = c1 * x + s1 * y;
180 *out2++ = c1 * y - s1 * x;
181 }
182
183 inp1 = _port [INP_U];
184 inp2 = _port [INP_V];
185 out1 = _port [OUT_U];
186 out2 = _port [OUT_V];
187 k = len;
188 dc = (_c2 - c2) / len;
189 ds = (_s2 - s2) / len;
190 while (k--)
191 {
192 c2 += dc;
193 s2 += ds;
194 x = *inp1++;
195 y = *inp2++;
196 *out1++ = c2 * x + s2 * y;
197 *out2++ = c2 * y - s2 * x;
198 }
199
200 inp1 = _port [INP_P];
201 inp2 = _port [INP_Q];
202 out1 = _port [OUT_P];
203 out2 = _port [OUT_Q];
204 k = len;
205 dc = (_c3 - c3) / len;
206 ds = (_s3 - s3) / len;
207 while (k--)
208 {
209 c3 += dc;
210 s3 += ds;
211 x = *inp1++;
212 y = *inp2++;
213 *out1++ = c3 * x + s3 * y;
214 *out2++ = c3 * y - s3 * x;
215 }
216 }
217
218
219
220
setport(unsigned long port,LADSPA_Data * data)221 void Ladspa_Monopan33::setport (unsigned long port, LADSPA_Data *data)
222 {
223 _port [port] = data;
224 }
225
226
active(bool act)227 void Ladspa_Monopan33::active (bool act)
228 {
229 if (act) calcpar (0.0f, 0.0f);
230 }
231
232
calcpar(float az,float el)233 void Ladspa_Monopan33::calcpar (float az, float el)
234 {
235 float t, x2, y2, z2;
236
237 az *= DEG2RAD;
238 el *= DEG2RAD;
239 t = cosf (el);
240 _xx = t * cosf (-az);
241 _yy = t * sinf (-az);
242 _zz = sinf (el);
243 x2 = _xx * _xx;
244 y2 = _yy * _yy;
245 z2 = _zz * _zz;
246 _uu = x2 - y2;
247 _vv = 2 * _xx * _yy;
248 _ss = 2 * _zz * _xx;
249 _tt = 2 * _zz * _yy;
250 _rr = (3 * z2 - 1) / 2;
251 _pp = (x2 - 3 * y2) * _xx;
252 _qq = (3 * x2 - y2) * _yy;
253 t = 2.598076f * _zz;
254 _nn = t * _uu;
255 _oo = t * _vv;
256 t = 0.726184f * (5 * z2 - 1);
257 _ll = t * _xx;
258 _mm = t * _yy;
259 _kk = _zz * (5 * z2 - 3) / 2;
260 }
261
262
runproc(unsigned long len,bool add)263 void Ladspa_Monopan33::runproc (unsigned long len, bool add)
264 {
265 float t, xx, yy, zz, rr, ss, tt, uu, vv, kk, ll, mm, nn, oo, pp, qq;
266 float dxx, dyy, dzz, drr, dss, dtt, duu, dvv, dkk, dll, dmm, dnn, doo, dpp, dqq;
267 float *in, *out_w, *out_x, *out_y, *out_z, *out_r, *out_s, *out_t, *out_u, *out_v,
268 *out_k, *out_l, *out_m, *out_n, *out_o, *out_p, *out_q;
269
270 xx = _xx;
271 yy = _yy;
272 zz = _zz;
273 rr = _rr;
274 ss = _ss;
275 tt = _tt;
276 uu = _uu;
277 vv = _vv;
278 kk = _kk;
279 ll = _ll;
280 mm = _mm;
281 nn = _nn;
282 oo = _oo;
283 pp = _pp;
284 qq = _qq;
285
286 calcpar (_port [CTL_AZIM][0], _port [CTL_ELEV][0]);
287
288 dxx = (_xx - xx) / len;
289 dyy = (_yy - yy) / len;
290 dzz = (_zz - zz) / len;
291 drr = (_rr - rr) / len;
292 dss = (_ss - ss) / len;
293 dtt = (_tt - tt) / len;
294 duu = (_uu - uu) / len;
295 dvv = (_vv - vv) / len;
296 dkk = (_kk - kk) / len;
297 dll = (_ll - ll) / len;
298 dmm = (_mm - mm) / len;
299 dnn = (_nn - nn) / len;
300 doo = (_oo - oo) / len;
301 dpp = (_pp - pp) / len;
302 dqq = (_qq - qq) / len;
303
304 in = _port [INP];
305 out_w = _port [OUT_W];
306 out_x = _port [OUT_X];
307 out_y = _port [OUT_Y];
308 out_z = _port [OUT_Z];
309 out_r = _port [OUT_R];
310 out_s = _port [OUT_S];
311 out_t = _port [OUT_T];
312 out_u = _port [OUT_U];
313 out_v = _port [OUT_V];
314 out_k = _port [OUT_K];
315 out_l = _port [OUT_L];
316 out_m = _port [OUT_M];
317 out_n = _port [OUT_N];
318 out_o = _port [OUT_O];
319 out_p = _port [OUT_P];
320 out_q = _port [OUT_Q];
321
322 while (len--)
323 {
324 xx += dxx;
325 yy += dyy;
326 zz += dzz;
327 rr += drr;
328 ss += dss;
329 tt += dtt;
330 uu += duu;
331 vv += dvv;
332 kk += dkk;
333 ll += dll;
334 mm += dmm;
335 nn += dnn;
336 oo += doo;
337 pp += dpp;
338 qq += dqq;
339
340 t = *in++;
341 *out_w++ = MIN3DB * t;
342 *out_x++ = xx * t;
343 *out_y++ = yy * t;
344 *out_z++ = zz * t;
345 *out_r++ = rr * t;
346 *out_s++ = ss * t;
347 *out_t++ = tt * t;
348 *out_u++ = uu * t;
349 *out_v++ = vv * t;
350 *out_k++ = kk * t;
351 *out_l++ = ll * t;
352 *out_m++ = mm * t;
353 *out_n++ = nn * t;
354 *out_o++ = oo * t;
355 *out_p++ = pp * t;
356 *out_q++ = qq * t;
357 }
358 }
359
360
setport(unsigned long port,LADSPA_Data * data)361 void Ladspa_Rotator33::setport (unsigned long port, LADSPA_Data *data)
362 {
363 _port [port] = data;
364 }
365
366
active(bool act)367 void Ladspa_Rotator33::active (bool act)
368 {
369 if (act) calcpar (0.0f);
370 }
371
372
calcpar(float az)373 void Ladspa_Rotator33::calcpar (float az)
374 {
375 az *= DEG2RAD;
376 _c1 = cosf (az);
377 _s1 = sinf (az);
378 _c2 = _c1 * _c1 - _s1 * _s1;
379 _s2 = 2 * _c1 * _s1;
380 _c3 = _c2 * _c1 - _s2 * _s1;
381 _s3 = _c2 * _s1 + _c1 * _s2;
382 }
383
384
runproc(unsigned long len,bool add)385 void Ladspa_Rotator33::runproc (unsigned long len, bool add)
386 {
387 int k;
388 float c1, s1, c2, s2, c3, s3;
389 float x, y, dc, ds;
390 float *inp1, *inp2, *inp3, *inp4, *inp5, *inp6;
391 float *out1, *out2, *out3, *out4, *out5, *out6;
392
393 // W, Z, R and K are invariant under rotation around the z axis.
394
395 memcpy (_port [OUT_W], _port [INP_W], len * sizeof (LADSPA_Data));
396 memcpy (_port [OUT_Z], _port [INP_Z], len * sizeof (LADSPA_Data));
397 memcpy (_port [OUT_R], _port [INP_R], len * sizeof (LADSPA_Data));
398 memcpy (_port [OUT_K], _port [INP_K], len * sizeof (LADSPA_Data));
399
400 c1 = _c1;
401 s1 = _s1;
402 c2 = _c2;
403 s2 = _s2;
404 c3 = _c3;
405 s3 = _s3;
406
407 calcpar (_port [CTL_AZIM][0]);
408
409 inp1 = _port [INP_X];
410 inp2 = _port [INP_Y];
411 inp3 = _port [INP_S];
412 inp4 = _port [INP_T];
413 inp5 = _port [INP_L];
414 inp6 = _port [INP_M];
415 out1 = _port [OUT_X];
416 out2 = _port [OUT_Y];
417 out3 = _port [OUT_S];
418 out4 = _port [OUT_T];
419 out5 = _port [OUT_L];
420 out6 = _port [OUT_M];
421 k = len;
422 dc = (_c1 - c1) / len;
423 ds = (_s1 - s1) / len;
424 while (k--)
425 {
426 c1 += dc;
427 s1 += ds;
428 x = *inp1++;
429 y = *inp2++;
430 *out1++ = c1 * x + s1 * y;
431 *out2++ = c1 * y - s1 * x;
432 x = *inp3++;
433 y = *inp4++;
434 *out3++ = c1 * x + s1 * y;
435 *out4++ = c1 * y - s1 * x;
436 x = *inp5++;
437 y = *inp6++;
438 *out5++ = c1 * x + s1 * y;
439 *out6++ = c1 * y - s1 * x;
440 }
441
442 inp1 = _port [INP_U];
443 inp2 = _port [INP_V];
444 inp3 = _port [INP_N];
445 inp4 = _port [INP_O];
446 out1 = _port [OUT_U];
447 out2 = _port [OUT_V];
448 out3 = _port [OUT_N];
449 out4 = _port [OUT_O];
450 k = len;
451 dc = (_c2 - c2) / len;
452 ds = (_s2 - s2) / len;
453 while (k--)
454 {
455 c2 += dc;
456 s2 += ds;
457 x = *inp1++;
458 y = *inp2++;
459 *out1++ = c2 * x + s2 * y;
460 *out2++ = c2 * y - s2 * x;
461 x = *inp3++;
462 y = *inp4++;
463 *out3++ = c2 * x + s2 * y;
464 *out4++ = c2 * y - s2 * x;
465 }
466
467 inp1 = _port [INP_P];
468 inp2 = _port [INP_Q];
469 out1 = _port [OUT_P];
470 out2 = _port [OUT_Q];
471 k = len;
472 dc = (_c3 - c3) / len;
473 ds = (_s3 - s3) / len;
474 while (k--)
475 {
476 c3 += dc;
477 s3 += ds;
478 x = *inp1++;
479 y = *inp2++;
480 *out1++ = c3 * x + s3 * y;
481 *out2++ = c3 * y - s3 * x;
482 }
483 }
484
485
486