1 /*
2 Copyright (C) 2004-2009 Fons Adriaensen <fons@kokkinizita.net>
3
4 This program is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with this program; if not, write to the Free Software
16 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
17 */
18
19
20 #include <string.h>
21 #include <math.h>
22 #include "ambisonic2.h"
23
24
25 #define DEG2RAD 0.01745329f
26 #define MIN3DB 0.707107f
27
28
setport(unsigned long port,LADSPA_Data * data)29 void Ladspa_Monopan21::setport (unsigned long port, LADSPA_Data *data)
30 {
31 _port [port] = data;
32 }
33
34
active(bool act)35 void Ladspa_Monopan21::active (bool act)
36 {
37 if (act) calcpar (0.0f, 0.0f);
38 }
39
40
calcpar(float az,float el)41 void Ladspa_Monopan21::calcpar (float az, float el)
42 {
43 float ce;
44
45 az *= DEG2RAD;
46 el *= DEG2RAD;
47 ce = cosf (el);
48 _xx = ce * cosf (-az);
49 _yy = ce * sinf (-az);
50 _zz = sinf (el);
51 _uu = _xx * _xx - _yy * _yy;
52 _vv = 2 * _xx * _yy;
53 }
54
55
runproc(unsigned long len,bool add)56 void Ladspa_Monopan21::runproc (unsigned long len, bool add)
57 {
58 float t, xx, yy, zz, uu, vv, dxx, dyy, dzz, duu, dvv;
59 float *in, *out_w, *out_x, *out_y, *out_z, *out_u, *out_v;
60
61 xx = _xx;
62 yy = _yy;
63 zz = _zz;
64 uu = _uu;
65 vv = _vv;
66 calcpar (_port [CTL_AZIM][0], _port [CTL_ELEV][0]);
67 dxx = (_xx - xx) / len;
68 dyy = (_yy - yy) / len;
69 dzz = (_zz - zz) / len;
70 duu = (_uu - uu) / len;
71 dvv = (_vv - vv) / len;
72
73 in = _port [INP];
74 out_w = _port [OUT_W];
75 out_x = _port [OUT_X];
76 out_y = _port [OUT_Y];
77 out_z = _port [OUT_Z];
78 out_u = _port [OUT_U];
79 out_v = _port [OUT_V];
80
81 while (len--)
82 {
83 xx += dxx;
84 yy += dyy;
85 zz += dzz;
86 uu += duu;
87 vv += dvv;
88 t = *in++;
89 *out_w++ = MIN3DB * t;
90 *out_x++ = xx * t;
91 *out_y++ = yy * t;
92 *out_z++ = zz * t;
93 *out_u++ = uu * t;
94 *out_v++ = vv * t;
95 }
96 }
97
98
99
setport(unsigned long port,LADSPA_Data * data)100 void Ladspa_Rotator21::setport (unsigned long port, LADSPA_Data *data)
101 {
102 _port [port] = data;
103 }
104
105
active(bool act)106 void Ladspa_Rotator21::active (bool act)
107 {
108 if (act) calcpar (0.0f);
109 }
110
111
calcpar(float az)112 void Ladspa_Rotator21::calcpar (float az)
113 {
114 az *= DEG2RAD;
115 _c1 = cosf (az);
116 _s1 = sinf (az);
117 _c2 = _c1 * _c1 - _s1 * _s1;
118 _s2 = 2 * _c1 * _s1;
119 }
120
121
runproc(unsigned long len,bool add)122 void Ladspa_Rotator21::runproc (unsigned long len, bool add)
123 {
124 unsigned long k;
125 float x, y;
126 float c1, s1;
127 float c2, s2;
128 float dc, ds;
129 float *inp1, *inp2;
130 float *out1, *out2;
131
132 memcpy (_port [OUT_W], _port [INP_W], len * sizeof (float));
133 memcpy (_port [OUT_Z], _port [INP_Z], len * sizeof (float));
134
135 c1 = _c1;
136 s1 = _s1;
137 c2 = _c2;
138 s2 = _s2;
139 calcpar (_port [CTL_AZIM][0]);
140
141 inp1 = _port [INP_X];
142 inp2 = _port [INP_Y];
143 out1 = _port [OUT_X];
144 out2 = _port [OUT_Y];
145 k = len;
146 dc = (_c1 - c1) / len;
147 ds = (_s1 - s1) / len;
148 while (k--)
149 {
150 c1 += dc;
151 s1 += ds;
152 x = *inp1++;
153 y = *inp2++;
154 *out1++ = c1 * x + s1 * y;
155 *out2++ = c1 * y - s1 * x;
156 }
157
158 inp1 = _port [INP_U];
159 inp2 = _port [INP_V];
160 out1 = _port [OUT_U];
161 out2 = _port [OUT_V];
162 k = len;
163 dc = (_c2 - c2) / len;
164 ds = (_s2 - s2) / len;
165 while (k--)
166 {
167 c2 += dc;
168 s2 += ds;
169 x = *inp1++;
170 y = *inp2++;
171 *out1++ = c2 * x + s2 * y;
172 *out2++ = c2 * y - s2 * x;
173 }
174 }
175
176
177
setport(unsigned long port,LADSPA_Data * data)178 void Ladspa_Monopan22::setport (unsigned long port, LADSPA_Data *data)
179 {
180 _port [port] = data;
181 }
182
183
active(bool act)184 void Ladspa_Monopan22::active (bool act)
185 {
186 if (act) calcpar (0.0f, 0.0f);
187 }
188
189
calcpar(float az,float el)190 void Ladspa_Monopan22::calcpar (float az, float el)
191 {
192 float ce;
193
194 az *= DEG2RAD;
195 el *= DEG2RAD;
196 ce = cosf (el);
197 _xx = ce * cosf (-az);
198 _yy = ce * sinf (-az);
199 _zz = sinf (el);
200 _rr = 1.5f * _zz * _zz - 0.5f;
201 _ss = 2 * _xx * _zz;
202 _tt = 2 * _yy * _zz;
203 _uu = _xx * _xx - _yy * _yy;
204 _vv = 2 * _xx * _yy;
205 }
206
207
runproc(unsigned long len,bool add)208 void Ladspa_Monopan22::runproc (unsigned long len, bool add)
209 {
210 float t, xx, yy, zz, rr, ss, tt, uu, vv, dxx, dyy, dzz, drr, dss, dtt, duu, dvv;
211 float *in, *out_w, *out_x, *out_y, *out_z, *out_r, *out_s, *out_t, *out_u, *out_v;
212
213 xx = _xx;
214 yy = _yy;
215 zz = _zz;
216 rr = _rr;
217 ss = _ss;
218 tt = _tt;
219 uu = _uu;
220 vv = _vv;
221 calcpar (_port [CTL_AZIM][0], _port [CTL_ELEV][0]);
222 dxx = (_xx - xx) / len;
223 dyy = (_yy - yy) / len;
224 dzz = (_zz - zz) / len;
225 drr = (_rr - rr) / len;
226 dss = (_ss - ss) / len;
227 dtt = (_tt - tt) / len;
228 duu = (_uu - uu) / len;
229 dvv = (_vv - vv) / len;
230
231 in = _port [INP];
232 out_w = _port [OUT_W];
233 out_x = _port [OUT_X];
234 out_y = _port [OUT_Y];
235 out_z = _port [OUT_Z];
236 out_r = _port [OUT_R];
237 out_s = _port [OUT_S];
238 out_t = _port [OUT_T];
239 out_u = _port [OUT_U];
240 out_v = _port [OUT_V];
241
242 while (len--)
243 {
244 xx += dxx;
245 yy += dyy;
246 zz += dzz;
247 rr += drr;
248 ss += dss;
249 tt += dtt;
250 uu += duu;
251 vv += dvv;
252 t = *in++;
253 *out_w++ = MIN3DB * t;
254 *out_x++ = xx * t;
255 *out_y++ = yy * t;
256 *out_z++ = zz * t;
257 *out_r++ = rr * t;
258 *out_s++ = ss * t;
259 *out_t++ = tt * t;
260 *out_u++ = uu * t;
261 *out_v++ = vv * t;
262 }
263 }
264
265
266
setport(unsigned long port,LADSPA_Data * data)267 void Ladspa_Rotator22::setport (unsigned long port, LADSPA_Data *data)
268 {
269 _port [port] = data;
270 }
271
272
active(bool act)273 void Ladspa_Rotator22::active (bool act)
274 {
275 if (act) calcpar (0.0f);
276 }
277
278
calcpar(float az)279 void Ladspa_Rotator22::calcpar (float az)
280 {
281 az *= DEG2RAD;
282 _c1 = cosf (az);
283 _s1 = sinf (az);
284 _c2 = _c1 * _c1 - _s1 * _s1;
285 _s2 = 2 * _c1 * _s1;
286 }
287
288
runproc(unsigned long len,bool add)289 void Ladspa_Rotator22::runproc (unsigned long len, bool add)
290 {
291 unsigned long k;
292 float x, y;
293 float c1, s1;
294 float c2, s2;
295 float dc, ds;
296 float *inp1, *inp2, *inp3, *inp4;
297 float *out1, *out2, *out3, *out4;
298
299 memcpy (_port [OUT_W], _port [INP_W], len * sizeof (float));
300 memcpy (_port [OUT_Z], _port [INP_Z], len * sizeof (float));
301 memcpy (_port [OUT_R], _port [INP_R], len * sizeof (float));
302
303 c1 = _c1;
304 s1 = _s1;
305 c2 = _c2;
306 s2 = _s2;
307 calcpar (_port [CTL_AZIM][0]);
308
309 inp1 = _port [INP_X];
310 inp2 = _port [INP_Y];
311 inp3 = _port [INP_S];
312 inp4 = _port [INP_T];
313 out1 = _port [OUT_X];
314 out2 = _port [OUT_Y];
315 out3 = _port [OUT_S];
316 out4 = _port [OUT_T];
317 k = len;
318 dc = (_c1 - c1) / len;
319 ds = (_s1 - s1) / len;
320 while (k--)
321 {
322 c1 += dc;
323 s1 += ds;
324 x = *inp1++;
325 y = *inp2++;
326 *out1++ = c1 * x + s1 * y;
327 *out2++ = c1 * y - s1 * x;
328 x = *inp3++;
329 y = *inp4++;
330 *out3++ = c1 * x + s1 * y;
331 *out4++ = c1 * y - s1 * x;
332 }
333
334 inp1 = _port [INP_U];
335 inp2 = _port [INP_V];
336 out1 = _port [OUT_U];
337 out2 = _port [OUT_V];
338 k = len;
339 dc = (_c2 - c2) / len;
340 ds = (_s2 - s2) / len;
341 while (k--)
342 {
343 c2 += dc;
344 s2 += ds;
345 x = *inp1++;
346 y = *inp2++;
347 *out1++ = c2 * x + s2 * y;
348 *out2++ = c2 * y - s2 * x;
349 }
350 }
351
352
353
354