1 /*
2     vbap1.c:
3 
4     Copyright (C) 2000 Ville Pulkki
5                   2012 John ffitch
6 
7     This file is part of Csound.
8 
9     The Csound Library is free software; you can redistribute it
10     and/or modify it under the terms of the GNU Lesser General Public
11     License as published by the Free Software Foundation; either
12     version 2.1 of the License, or (at your option) any later version.
13 
14     Csound is distributed in the hope that it will be useful,
15     but WITHOUT ANY WARRANTY; without even the implied warranty of
16     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17     GNU Lesser General Public License for more details.
18 
19     You should have received a copy of the GNU Lesser General Public
20     License along with Csound; if not, write to the Free Software
21     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
22     02110-1301 USA
23 */
24 
25 /* vbap1.c
26 
27 functions specific gains for loudspeaker VBAP
28 
29 Ville Pulkki heavily modified by John ffitch 2012
30 */
31 
32 
33 #include "csoundCore.h"
34 #include "vbap.h"
35 #include <math.h>
36 #include <stdio.h>
37 #include <stdlib.h>
38 
39 static int32_t vbap1_moving_control(CSOUND *, VBAP1_MOVE_DATA *, OPDS *, MYFLT,
40                                 MYFLT, MYFLT, MYFLT**);
41 static int32_t vbap1_control(CSOUND *, VBAP1_DATA *, MYFLT*, MYFLT*, MYFLT*);
42 
vbap1(CSOUND * csound,VBAP1 * p)43 int32_t vbap1(CSOUND *csound, VBAP1 *p) /* during note performance: */
44 {
45     int32_t j;
46     int32_t cnt = p->q.number;
47     vbap1_control(csound,&p->q, p->azi, p->ele, p->spread);
48 
49     /* write gains */
50     for (j=0; j<cnt; j++) {
51       *p->out_array[j] = p->q.gains[j];
52     }
53     return OK;
54 }
55 
vbap1_control(CSOUND * csound,VBAP1_DATA * p,MYFLT * azi,MYFLT * ele,MYFLT * spread)56 static int32_t vbap1_control(CSOUND *csound, VBAP1_DATA *p,
57                          MYFLT* azi, MYFLT* ele, MYFLT* spread)
58 {
59     CART_VEC spreaddir[16];
60     CART_VEC spreadbase[16];
61     ANG_VEC atmp;
62     int32 i,j, spreaddirnum;
63     int32_t cnt = p->number;
64     MYFLT *tmp_gains=malloc(sizeof(MYFLT)*cnt),sum=FL(0.0);
65     if (UNLIKELY(p->dim == 2 && fabs(*ele) > 0.0)) {
66       csound->Warning(csound,
67                       Str("Warning: truncating elevation to 2-D plane\n"));
68       *ele = FL(0.0);
69     }
70 
71     if (*spread <FL(0.0))
72       *spread = FL(0.0);
73     else if (*spread >FL(100.0))
74       *spread = FL(100.0);
75     /* Current panning angles */
76     p->ang_dir.azi = *azi;
77     p->ang_dir.ele = *ele;
78     p->ang_dir.length = FL(1.0);
79     angle_to_cart(p->ang_dir, &(p->cart_dir));
80     calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
81                   p->gains, cnt, p->cart_dir);
82 
83     /* Calculated gain factors of a spreaded virtual source*/
84     if (*spread > FL(0.0)) {
85       if (p->dim == 3) {
86         spreaddirnum = 16;
87         /* four orthogonal dirs*/
88         new_spread_dir(&spreaddir[0], p->cart_dir,
89                        p->spread_base, *azi, *spread);
90         new_spread_base(spreaddir[0], p->cart_dir,
91                         *spread, &p->spread_base);
92         cross_prod(p->spread_base, p->cart_dir, &spreadbase[1]);
93         cross_prod(spreadbase[1], p->cart_dir, &spreadbase[2]);
94         cross_prod(spreadbase[2], p->cart_dir, &spreadbase[3]);
95         /* four between them*/
96         vec_mean(p->spread_base, spreadbase[1], &spreadbase[4]);
97         vec_mean(spreadbase[1], spreadbase[2], &spreadbase[5]);
98         vec_mean(spreadbase[2], spreadbase[3], &spreadbase[6]);
99         vec_mean(spreadbase[3], p->spread_base, &spreadbase[7]);
100 
101         /* four at half spreadangle*/
102         vec_mean(p->cart_dir, p->spread_base, &spreadbase[8]);
103         vec_mean(p->cart_dir, spreadbase[1], &spreadbase[9]);
104         vec_mean(p->cart_dir, spreadbase[2], &spreadbase[10]);
105         vec_mean(p->cart_dir, spreadbase[3], &spreadbase[11]);
106 
107         /* four at quarter spreadangle*/
108         vec_mean(p->cart_dir, spreadbase[8], &spreadbase[12]);
109         vec_mean(p->cart_dir, spreadbase[9], &spreadbase[13]);
110         vec_mean(p->cart_dir, spreadbase[10], &spreadbase[14]);
111         vec_mean(p->cart_dir, spreadbase[11], &spreadbase[15]);
112 
113         for (i=1;i<spreaddirnum;i++) {
114           new_spread_dir(&spreaddir[i], p->cart_dir,
115                          spreadbase[i],*azi,*spread);
116           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
117                         tmp_gains, cnt, spreaddir[i]);
118           for (j=0;j<cnt;j++) {
119             p->gains[j] += tmp_gains[j];
120           }
121         }
122       }
123       else if (p->dim == 2) {
124         spreaddirnum = 6;
125         atmp.ele = FL(0.0);
126         atmp.azi = *azi - *spread;
127         angle_to_cart(atmp, &spreaddir[0]);
128         atmp.azi = *azi - *spread/2;
129         angle_to_cart(atmp, &spreaddir[1]);
130         atmp.azi = *azi - *spread/4;
131         angle_to_cart(atmp, &spreaddir[2]);
132         atmp.azi = *azi + *spread/4;
133         angle_to_cart(atmp, &spreaddir[3]);
134         atmp.azi = *azi + *spread/2;
135         angle_to_cart(atmp, &spreaddir[4]);
136         atmp.azi = *azi + *spread;
137         angle_to_cart(atmp, &spreaddir[5]);
138 
139         for (i=0;i<spreaddirnum;i++) {
140           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
141                         tmp_gains, cnt, spreaddir[i]);
142           for (j=0;j<cnt;j++) {
143             p->gains[j] += tmp_gains[j];
144           }
145         }
146       }
147     }
148     if (*spread > FL(70.0))
149       for (i=0;i<cnt ;i++) {
150         p->gains[i] +=(*spread - FL(70.0))/FL(30.0) *
151           (*spread - FL(70.0))/FL(30.0)*FL(20.0);
152       }
153 
154     /*normalization*/
155     for (i=0;i<cnt;i++) {
156       sum=sum+(p->gains[i]*p->gains[i]);
157     }
158 
159     sum=SQRT(sum);
160     for (i=0;i<cnt;i++) {
161       p->gains[i] /= sum;
162     }
163     free(tmp_gains);
164     return OK;
165 }
166 
vbap1_init(CSOUND * csound,VBAP1 * p)167 int32_t vbap1_init(CSOUND *csound, VBAP1 *p)
168 {                               /* Initializations before run time*/
169     int32_t     i, j;
170     MYFLT   *ls_table, *ptr;
171     LS_SET  *ls_set_ptr;
172     char name[24];
173     snprintf(name, 24, "vbap_ls_table_%d", (int32_t)*p->layout);
174     ls_table = (MYFLT*) (csound->QueryGlobalVariableNoCheck(csound, name));
175     if (ls_table==NULL)
176       return csound->InitError(csound,
177                                Str("could not find layout table no.%d"),
178                                (int32_t)*p->layout );
179     p->q.number = p->OUTOCOUNT;
180     p->q.dim       = (int32_t)ls_table[0];   /* reading in loudspeaker info */
181     p->q.ls_am     = (int32_t)ls_table[1];
182     p->q.ls_set_am = (int32_t)ls_table[2];
183     ptr = &(ls_table[3]);
184     if (!p->q.ls_set_am)
185       return csound->InitError(csound, Str("vbap system NOT configured.\nMissing"
186                                            " vbaplsinit opcode in orchestra?"));
187     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof (LS_SET), &p->q.aux);
188     if (UNLIKELY(p->q.aux.auxp == NULL)) {
189       return csound->InitError(csound, Str("could not allocate memory"));
190     }
191     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
192     ls_set_ptr = p->q.ls_sets;
193     for (i=0; i < p->q.ls_set_am; i++) {
194       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
195       for (j=0 ; j < p->q.dim ; j++) {
196         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
197       }
198       memset(ls_set_ptr[i].ls_mx, '\0', 9*sizeof(MYFLT)); // initial setting
199       /* for (j=0 ; j < 9; j++) */
200       /*   ls_set_ptr[i].ls_mx[j] = FL(0.0);  /\*initial setting*\/ */
201       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
202         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
203       }
204     }
205 
206     /* other initialization */
207     if (UNLIKELY(p->q.dim == 2 && fabs(*p->ele) > 0.0)) {
208       csound->Warning(csound,
209                       Str("Warning: truncating elevation to 2-D plane\n"));
210       *p->ele = FL(0.0);
211     }
212     p->q.ang_dir.azi    = (MYFLT)*p->azi;
213     p->q.ang_dir.ele    = (MYFLT)*p->ele;
214     p->q.ang_dir.length = FL(1.0);
215     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
216     p->q.spread_base.x  = p->q.cart_dir.y;
217     p->q.spread_base.y  = p->q.cart_dir.z;
218     p->q.spread_base.z  = -p->q.cart_dir.x;
219     vbap1_control(csound,&p->q, p->azi, p->ele, p->spread);
220     return OK;
221 }
222 
vbap1a(CSOUND * csound,VBAPA1 * p)223 int32_t vbap1a(CSOUND *csound, VBAPA1 *p) /* during note performance: */
224 {
225     int32_t j;
226     int32_t cnt = p->q.number;
227     vbap1_control(csound,&p->q, p->azi, p->ele, p->spread);
228 
229     /* write gains */
230     for (j=0; j<cnt; j++) {
231       p->tabout->data[j] = p->q.gains[j];
232     }
233     return OK;
234 }
235 
vbap1_init_a(CSOUND * csound,VBAPA1 * p)236 int32_t vbap1_init_a(CSOUND *csound, VBAPA1 *p)
237 {                               /* Initializations before run time*/
238     int32_t     i, j;
239     MYFLT   *ls_table, *ptr;
240     LS_SET  *ls_set_ptr;
241     char name[24];
242     snprintf(name, 24, "vbap_ls_table_%d", (int32_t)*p->layout);
243     ls_table = (MYFLT*) (csound->QueryGlobalVariableNoCheck(csound, name));
244     if (ls_table==NULL)
245       return csound->InitError(csound,
246                                Str("could not find layout table no.%d"),
247                                (int32_t)*p->layout );
248     p->q.number = p->tabout->sizes[0];
249     p->q.dim       = (int32_t)ls_table[0];   /* reading in loudspeaker info */
250     p->q.ls_am     = (int32_t)ls_table[1];
251     p->q.ls_set_am = (int32_t)ls_table[2];
252     ptr = &(ls_table[3]);
253     if (!p->q.ls_set_am)
254       return csound->InitError(csound, Str("vbap system NOT configured.\nMissing"
255                                            " vbaplsinit opcode in orchestra?"));
256     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof (LS_SET), &p->q.aux);
257     if (UNLIKELY(p->q.aux.auxp == NULL)) {
258       return csound->InitError(csound, Str("could not allocate memory"));
259     }
260     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
261     ls_set_ptr = p->q.ls_sets;
262     for (i=0; i < p->q.ls_set_am; i++) {
263       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
264       for (j=0 ; j < p->q.dim ; j++) {
265         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
266       }
267       memset(ls_set_ptr[i].ls_mx, '\0', 9*sizeof(MYFLT));  /*initial setting*/
268       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
269         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
270       }
271     }
272 
273     /* other initialization */
274     if (UNLIKELY(p->q.dim == 2 && fabs(*p->ele) > 0.0)) {
275       csound->Warning(csound,
276                       Str("Warning: truncating elevation to 2-D plane\n"));
277       *p->ele = FL(0.0);
278     }
279     p->q.ang_dir.azi    = (MYFLT)*p->azi;
280     p->q.ang_dir.ele    = (MYFLT)*p->ele;
281     p->q.ang_dir.length = FL(1.0);
282     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
283     p->q.spread_base.x  = p->q.cart_dir.y;
284     p->q.spread_base.y  = p->q.cart_dir.z;
285     p->q.spread_base.z  = -p->q.cart_dir.x;
286     vbap1_control(csound,&p->q, p->azi, p->ele, p->spread);
287     return OK;
288 }
289 
vbap1_moving(CSOUND * csound,VBAP1_MOVING * p)290 int32_t vbap1_moving(CSOUND *csound, VBAP1_MOVING *p)
291 {                               /* during note performance:   */
292     int32_t j;
293     int32_t cnt = p->q.number;
294 
295     vbap1_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
296                          *p->spread, *p->field_am, p->fld);
297 
298     /* write audio to resulting audio streams weighted
299        with gain factors*/
300     for (j=0; j<cnt ;j++) {
301       *p->out_array[j] = p->q.gains[j];
302     }
303     return OK;
304 }
305 
vbap1_moving_a(CSOUND * csound,VBAPA1_MOVING * p)306 int32_t vbap1_moving_a(CSOUND *csound, VBAPA1_MOVING *p)
307 {                               /* during note performance:   */
308     //    int32_t j;
309     int32_t cnt = p->q.number;
310 
311     vbap1_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
312                          *p->spread, *p->field_am, p->fld);
313 
314     /* write audio to resulting audio streams weighted
315        with gain factors*/
316     memcpy(p->tabout->data, p->q.gains, cnt*sizeof(MYFLT));
317     /* for (j=0; j<cnt ;j++) { */
318     /*   p->tabout->data[j] = p->q.gains[j]; */
319     /* } */
320     return OK;
321 }
322 
vbap1_moving_control(CSOUND * csound,VBAP1_MOVE_DATA * p,OPDS * h,MYFLT ONEDKR,MYFLT spread,MYFLT field_am,MYFLT ** fld)323 static int32_t vbap1_moving_control(CSOUND *csound, VBAP1_MOVE_DATA *p,
324                                 OPDS *h, MYFLT ONEDKR,
325                                 MYFLT spread, MYFLT field_am, MYFLT **fld)
326 {
327     CART_VEC spreaddir[16];
328     CART_VEC spreadbase[16];
329     ANG_VEC atmp;
330     int32 i,j, spreaddirnum;
331     CART_VEC tmp1, tmp2, tmp3;
332     MYFLT coeff, angle;
333     int32_t cnt = p->number;
334     MYFLT *tmp_gains=malloc(sizeof(MYFLT)*cnt),sum=FL(0.0);
335 #ifdef JPFF
336     printf("cnt=%d dim=%d\n", cnt, p->dim);
337 #endif
338     if (UNLIKELY(p->dim == 2 && fabs(p->ang_dir.ele) > 0.0)) {
339       csound->Warning(csound,
340                       Str("Warning: truncating elevation to 2-D plane\n"));
341       p->ang_dir.ele = FL(0.0);
342     }
343     if (spread <FL(0.0))
344       spread = FL(0.0);
345     else if (spread >FL(100.0))
346       spread = FL(100.0);
347     if (p->point_change_counter++ >= p->point_change_interval) {
348       p->point_change_counter = 0;
349       p->curr_fld = p->next_fld;
350       if (++p->next_fld >= (int32_t) fabs(field_am)) {
351         if (field_am >= FL(0.0)) /* point-to-point */
352           p->next_fld = 0;
353         else
354           p->next_fld = 1;
355       }
356       if (p->dim == 3) { /*jumping over second field */
357         p->curr_fld = p->next_fld;
358         if (++p->next_fld >= ((int32_t) fabs(field_am))) {
359           if (field_am >= FL(0.0)) /* point-to-point */
360             p->next_fld = 0;
361           else
362             p->next_fld = 1;
363         }
364       }
365       if (UNLIKELY((fld[abs(p->next_fld)]==NULL)))
366         return csound->PerfError(csound, h,
367                                  Str("Missing fields in vbapmove\n"));
368       if (field_am >= FL(0.0) && p->dim == 2) /* point-to-point */
369         if (UNLIKELY(fabs(fabs(*fld[p->next_fld] -
370                                *fld[p->curr_fld]) - 180.0) < 1.0))
371           csound->Warning(csound,
372                           Str("Warning: Ambiguous transition 180 degrees.\n"));
373     }
374     if (field_am >= FL(0.0)) { /* point-to-point */
375       if (p->dim == 3) { /* 3-D*/
376         p->prev_ang_dir.azi =  *fld[p->curr_fld-1];
377         p->next_ang_dir.azi =  *fld[p->next_fld];
378         p->prev_ang_dir.ele = *fld[p->curr_fld];
379         p->next_ang_dir.ele = *fld[p->next_fld+1];
380         coeff = ((MYFLT) p->point_change_counter) /
381           ((MYFLT) p->point_change_interval);
382         angle_to_cart( p->prev_ang_dir,&tmp1);
383         angle_to_cart( p->next_ang_dir,&tmp2);
384         tmp3.x = (FL(1.0)-coeff) * tmp1.x + coeff * tmp2.x;
385         tmp3.y = (FL(1.0)-coeff) * tmp1.y + coeff * tmp2.y;
386         tmp3.z = (FL(1.0)-coeff) * tmp1.z + coeff * tmp2.z;
387         coeff = (MYFLT)sqrt((double)(tmp3.x * tmp3.x +
388                                      tmp3.y * tmp3.y +
389                                      tmp3.z * tmp3.z));
390         tmp3.x /= coeff; tmp3.y /= coeff; tmp3.z /= coeff;
391         cart_to_angle(tmp3,&(p->ang_dir));
392       }
393       else if (p->dim == 2) { /* 2-D */
394         p->prev_ang_dir.azi =  *fld[p->curr_fld];
395         p->next_ang_dir.azi =  *fld[p->next_fld ];
396         p->prev_ang_dir.ele = p->next_ang_dir.ele =  FL(0.0);
397         scale_angles(&(p->prev_ang_dir));
398         scale_angles(&(p->next_ang_dir));
399         angle = (p->prev_ang_dir.azi - p->next_ang_dir.azi);
400         while (angle > FL(180.0))
401           angle -= FL(360.0);
402         while (angle < -FL(180.0))
403           angle += FL(360.0);
404         coeff = ((MYFLT) p->point_change_counter) /
405           ((MYFLT) p->point_change_interval);
406         angle  *=  (coeff);
407         p->ang_dir.azi = p->prev_ang_dir.azi -  angle;
408         p->ang_dir.ele = FL(0.0);
409       }
410       else {
411         free(tmp_gains);
412         return csound->PerfError(csound, h,
413                                  Str("Missing fields in vbapmove\n"));
414       }
415     }
416     else { /* angular velocities */
417       if (p->dim == 2) {
418         p->ang_dir.azi =  p->ang_dir.azi +
419           (*fld[p->next_fld] * ONEDKR);
420         scale_angles(&(p->ang_dir));
421       }
422       else { /* 3D angular*/
423         p->ang_dir.azi =  p->ang_dir.azi +
424           (*fld[p->next_fld] * ONEDKR);
425         p->ang_dir.ele =  p->ang_dir.ele +
426           p->ele_vel * (*fld[p->next_fld+1] * ONEDKR);
427         if (p->ang_dir.ele > FL(90.0)) {
428           p->ang_dir.ele = FL(90.0);
429           p->ele_vel = -p->ele_vel;
430         }
431         if (p->ang_dir.ele < FL(0.0)) {
432           p->ang_dir.ele = FL(0.0);
433           p->ele_vel =  -p->ele_vel;
434         }
435         scale_angles(&(p->ang_dir));
436       }
437     }
438     angle_to_cart(p->ang_dir, &(p->cart_dir));
439     calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
440                   p->gains, cnt, p->cart_dir);
441     if (spread > FL(0.0)) {
442       if (p->dim == 3) {
443         spreaddirnum=16;
444         /* four orthogonal dirs*/
445         new_spread_dir(&spreaddir[0], p->cart_dir,
446                        p->spread_base, p->ang_dir.azi, spread);
447 
448         new_spread_base(spreaddir[0], p->cart_dir, spread, &p->spread_base);
449         cross_prod(p->spread_base, p->cart_dir, &spreadbase[1]);
450         cross_prod(spreadbase[1], p->cart_dir, &spreadbase[2]);
451         cross_prod(spreadbase[2], p->cart_dir, &spreadbase[3]);
452         /* four between them*/
453         vec_mean(p->spread_base, spreadbase[1], &spreadbase[4]);
454         vec_mean(spreadbase[1], spreadbase[2], &spreadbase[5]);
455         vec_mean(spreadbase[2], spreadbase[3], &spreadbase[6]);
456         vec_mean(spreadbase[3], p->spread_base, &spreadbase[7]);
457 
458         /* four at half spreadangle*/
459         vec_mean(p->cart_dir, p->spread_base, &spreadbase[8]);
460         vec_mean(p->cart_dir, spreadbase[1], &spreadbase[9]);
461         vec_mean(p->cart_dir, spreadbase[2], &spreadbase[10]);
462         vec_mean(p->cart_dir, spreadbase[3], &spreadbase[11]);
463 
464         /* four at quarter spreadangle*/
465         vec_mean(p->cart_dir, spreadbase[8], &spreadbase[12]);
466         vec_mean(p->cart_dir, spreadbase[9], &spreadbase[13]);
467         vec_mean(p->cart_dir, spreadbase[10], &spreadbase[14]);
468         vec_mean(p->cart_dir, spreadbase[11], &spreadbase[15]);
469 
470         for (i=1;i<spreaddirnum;i++) {
471           new_spread_dir(&spreaddir[i], p->cart_dir,
472                          spreadbase[i],p->ang_dir.azi,spread);
473           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
474                         tmp_gains, cnt, spreaddir[i]);
475           for (j=0;j<cnt;j++) {
476             p->gains[j] += tmp_gains[j];
477           }
478         }
479       }
480       else if (p->dim == 2) {
481         spreaddirnum=6;
482         atmp.ele = FL(0.0);
483         atmp.azi = p->ang_dir.azi - spread;
484         angle_to_cart(atmp, &spreaddir[0]);
485         atmp.azi = p->ang_dir.azi - spread/2;
486         angle_to_cart(atmp, &spreaddir[1]);
487         atmp.azi = p->ang_dir.azi - spread/4;
488         angle_to_cart(atmp, &spreaddir[2]);
489         atmp.azi = p->ang_dir.azi + spread/4;
490         angle_to_cart(atmp, &spreaddir[3]);
491         atmp.azi = p->ang_dir.azi + spread/2;
492         angle_to_cart(atmp, &spreaddir[4]);
493         atmp.azi = p->ang_dir.azi + spread;
494         angle_to_cart(atmp, &spreaddir[5]);
495 
496         for (i=0;i<spreaddirnum;i++) {
497           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
498                         tmp_gains, cnt, spreaddir[i]);
499           for (j=0;j<cnt;j++) {
500             p->gains[j] += tmp_gains[j];
501           }
502         }
503       }
504     }
505     if (spread > FL(70.0))
506       for (i=0;i<cnt ;i++) {
507         p->gains[i] +=(spread - FL(70.0))/FL(30.0) *
508           (spread - FL(70.0))/FL(30.0)*FL(10.0);
509       }
510     /*normalization*/
511     for (i=0;i<cnt;i++) {
512       sum=sum+(p->gains[i]*p->gains[i]);
513     }
514 
515     sum=SQRT(sum);
516     for (i=0;i<cnt;i++) {
517       p->gains[i] /= sum;
518     }
519     free(tmp_gains);
520     return OK;
521 }
522 
vbap1_moving_init(CSOUND * csound,VBAP1_MOVING * p)523 int32_t vbap1_moving_init(CSOUND *csound, VBAP1_MOVING *p)
524 {
525     int32_t     i, j;
526     MYFLT   *ls_table, *ptr;
527     LS_SET  *ls_set_ptr;
528 
529     p->q.number = p->OUTCOUNT;
530     ls_table =
531       (MYFLT*) (csound->QueryGlobalVariableNoCheck(csound, "vbap_ls_table_0"));
532     /* reading in loudspeaker info */
533     p->q.dim       = (int32_t)ls_table[0];
534     p->q.ls_am     = (int32_t)ls_table[1];
535     p->q.ls_set_am = (int32_t)ls_table[2];
536     ptr = &(ls_table[3]);
537     if (!p->q.ls_set_am)
538       return csound->InitError(csound, Str("vbap system NOT configured.\n"
539                                            "Missing vbaplsinit opcode"
540                                            " in orchestra?"));
541     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof(LS_SET), &p->q.aux);
542     if (UNLIKELY(p->q.aux.auxp == NULL)) {
543       return csound->InitError(csound, Str("could not allocate memory"));
544     }
545     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
546     ls_set_ptr = p->q.ls_sets;
547     for (i=0 ; i < p->q.ls_set_am ; i++) {
548       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
549       for (j=0 ; j < p->q.dim ; j++) {
550         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
551       }
552       for (j=0 ; j < 9; j++)
553         ls_set_ptr[i].ls_mx[j] = FL(0.0);  /*initial setting*/
554       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
555         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
556       }
557     }
558 
559     /* other initialization */
560     p->q.ele_vel = FL(1.0);    /* functions specific to movement */
561     if (UNLIKELY(fabs(*p->field_am) < (2+ (p->q.dim - 2)*2))) {
562       return csound->InitError(csound,
563                   Str("Have to have at least %d directions in vbapmove"),
564                   2 + (p->q.dim - 2) * 2);
565     }
566     if (p->q.dim == 2)
567       p->q.point_change_interval =
568         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am) - 1.0));
569     else if (LIKELY(p->q.dim == 3))
570       p->q.point_change_interval =
571         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am)*0.5 - 1.0));
572     else
573       return csound->InitError(csound, Str("Wrong dimension"));
574     p->q.point_change_counter = 0;
575     p->q.curr_fld = 0;
576     p->q.next_fld = 1;
577     p->q.ang_dir.azi = *p->fld[0];
578     if (p->q.dim == 3) {
579       p->q.ang_dir.ele = *p->fld[1];
580     } else {
581       p->q.ang_dir.ele = FL(0.0);
582     }
583     if (p->q.dim == 3) {
584       p->q.curr_fld = 1;
585       p->q.next_fld = 2;
586     }
587     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
588     p->q.spread_base.x  = p->q.cart_dir.y;
589     p->q.spread_base.y  = p->q.cart_dir.z;
590     p->q.spread_base.z  = -p->q.cart_dir.x;
591     vbap1_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
592                          *p->spread, *p->field_am, p->fld);
593     return OK;
594 }
595 
vbap1_moving_init_a(CSOUND * csound,VBAPA1_MOVING * p)596 int32_t vbap1_moving_init_a(CSOUND *csound, VBAPA1_MOVING *p)
597 {
598     int32_t     i, j;
599     MYFLT   *ls_table, *ptr;
600     LS_SET  *ls_set_ptr;
601 
602     if (UNLIKELY(p->tabout->data == NULL || p->tabout->dimensions!=1))
603       return csound->InitError(csound, Str("Output array not initialised"));
604     p->q.number = p->tabout->sizes[0];
605     ls_table =
606       (MYFLT*) (csound->QueryGlobalVariableNoCheck(csound, "vbap_ls_table_0"));
607     /* reading in loudspeaker info */
608     p->q.dim       = (int32_t)ls_table[0];
609     p->q.ls_am     = (int32_t)ls_table[1];
610     p->q.ls_set_am = (int32_t)ls_table[2];
611     ptr = &(ls_table[3]);
612     if (UNLIKELY(!p->q.ls_set_am))
613       return csound->InitError(csound, Str("vbap system NOT configured.\n"
614                                            "Missing vbaplsinit opcode"
615                                            " in orchestra?"));
616     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof(LS_SET), &p->q.aux);
617     if (UNLIKELY(p->q.aux.auxp == NULL)) {
618       return csound->InitError(csound, Str("could not allocate memory"));
619     }
620     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
621     ls_set_ptr = p->q.ls_sets;
622     for (i=0 ; i < p->q.ls_set_am ; i++) {
623       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
624       for (j=0 ; j < p->q.dim ; j++) {
625         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
626       }
627       for (j=0 ; j < 9; j++)
628         ls_set_ptr[i].ls_mx[j] = FL(0.0);  /*initial setting*/
629       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
630         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
631       }
632     }
633 
634     /* other initialization */
635     p->q.ele_vel = FL(1.0);    /* functions specific to movement */
636     if (UNLIKELY(fabs(*p->field_am) < (2+ (p->q.dim - 2)*2))) {
637       return csound->InitError(csound,
638                   Str("Have to have at least %d directions in vbapmove"),
639                   2 + (p->q.dim - 2) * 2);
640     }
641     if (p->q.dim == 2)
642       p->q.point_change_interval =
643         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am) - 1.0));
644     else if (LIKELY(p->q.dim == 3))
645       p->q.point_change_interval =
646         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am)*0.5 - 1.0));
647     else
648       return csound->InitError(csound, Str("Wrong dimension"));
649     p->q.point_change_counter = 0;
650     p->q.curr_fld = 0;
651     p->q.next_fld = 1;
652     p->q.ang_dir.azi = *p->fld[0];
653     if (p->q.dim == 3) {
654       p->q.ang_dir.ele = *p->fld[1];
655     } else {
656       p->q.ang_dir.ele = FL(0.0);
657     }
658     if (p->q.dim == 3) {
659       p->q.curr_fld = 1;
660       p->q.next_fld = 2;
661     }
662     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
663     p->q.spread_base.x  = p->q.cart_dir.y;
664     p->q.spread_base.y  = p->q.cart_dir.z;
665     p->q.spread_base.z  = -p->q.cart_dir.x;
666     vbap1_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
667                          *p->spread, *p->field_am, p->fld);
668     return OK;
669 }
670 
671