1 /*
2     vbap_n.c:
3 
4     Copyright (C) 2000 Ville Pulkki
5 
6     This file is part of Csound.
7 
8     The Csound Library is free software; you can redistribute it
9     and/or modify it under the terms of the GNU Lesser General Public
10     License as published by the Free Software Foundation; either
11     version 2.1 of the License, or (at your option) any later version.
12 
13     Csound 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 Lesser General Public License for more details.
17 
18     You should have received a copy of the GNU Lesser General Public
19     License along with Csound; if not, write to the Free Software
20     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
21     02110-1301 USA
22 */
23 
24 /* vbap_n.c
25 
26 functions specific to four loudspeaker VBAP
27 
28 Ville Pulkki heavily modified by John ffitch 2012
29 */
30 
31 
32 #include "csoundCore.h"
33 #include "vbap.h"
34 #include <math.h>
35 #include <stdio.h>
36 #include <stdlib.h>
37 #include "arrays.h"
38 
39 int32_t vbap_moving_control(CSOUND *, VBAP_MOVE_DATA *, OPDS*, MYFLT,
40                         MYFLT *, MYFLT*,MYFLT**);
41 
vbap(CSOUND * csound,VBAP * p)42 int32_t vbap(CSOUND *csound, VBAP *p) /* during note performance: */
43 {
44     MYFLT *outptr, *inptr;
45     MYFLT ogain, ngain, gainsubstr;
46     MYFLT invfloatn;
47     int32_t j;
48     uint32_t offset = p->h.insdshead->ksmps_offset;
49     uint32_t early  = p->h.insdshead->ksmps_no_end;
50     uint32_t i, nsmps = CS_KSMPS;
51     int32_t cnt = p->q.number;
52     vbap_control(csound,&p->q, p->azi, p->ele, p->spread);
53     for (j=0; j<cnt; j++) {
54       p->q.beg_gains[j] = p->q.end_gains[j];
55       p->q.end_gains[j] = p->q.updated_gains[j];
56     }
57 
58     /* write audio to result audio streams weighted
59        with gain factors*/
60     if (UNLIKELY(early)) nsmps -= early;
61     invfloatn =  FL(1.0)/(nsmps-offset);
62     for (j=0; j<cnt; j++) {
63       inptr      = p->audio;
64       outptr     = p->out_array[j];
65       ogain      = p->q.beg_gains[j];
66       ngain      = p->q.end_gains[j];
67       gainsubstr = ngain - ogain;
68       if (UNLIKELY(offset)) memset(outptr, '\0', offset*sizeof(MYFLT));
69       if (UNLIKELY(early)) memset(&outptr[nsmps], '\0', early*sizeof(MYFLT));
70       //printf("cnt%d: ngain=%lf ogain=%f\n", j, ngain, ogain);
71       if (ngain != FL(0.0) || ogain != FL(0.0)) {
72         if (ngain != ogain) {
73           for (i = offset; i < nsmps; i++) {
74             outptr[i] = inptr[i] *
75               (ogain + (MYFLT)(i+1) * invfloatn * gainsubstr);
76           }
77           p->q.curr_gains[j]= ogain +
78            (MYFLT)(i) * invfloatn * gainsubstr;
79         }
80         else {
81           for (i=offset; i<nsmps; ++i) {
82             outptr[i] = inptr[i] * ogain;
83           }
84         }
85       }
86       else {
87         memset(outptr, 0, nsmps*sizeof(MYFLT));
88       }
89     }
90     return OK;
91 }
92 
vbap_a(CSOUND * csound,VBAPA * p)93 int32_t vbap_a(CSOUND *csound, VBAPA *p) /* during note performance: */
94 {
95     MYFLT *outptr, *inptr;
96     MYFLT ogain, ngain, gainsubstr;
97     MYFLT invfloatn;
98     int32_t j;
99     uint32_t offset = p->h.insdshead->ksmps_offset;
100     uint32_t early  = p->h.insdshead->ksmps_no_end;
101     uint32_t i, nsmps = CS_KSMPS;
102     uint32_t ksmps = nsmps;
103     int32_t cnt = p->q.number;
104 
105     vbap_control(csound,&p->q, p->azi, p->ele, p->spread);
106     for (j=0; j<cnt; j++) {
107       p->q.beg_gains[j] = p->q.end_gains[j];
108       p->q.end_gains[j] = p->q.updated_gains[j];
109     }
110 
111     /* write audio to result audio streams weighted
112        with gain factors*/
113     if (UNLIKELY(early)) nsmps -= early;
114     invfloatn =  FL(1.0)/(nsmps-offset);
115     for (j=0; j<cnt; j++) {
116       outptr     = &p->tabout->data[j*ksmps];
117       inptr      = p->audio;
118       ogain      = p->q.beg_gains[j];
119       ngain      = p->q.end_gains[j];
120       gainsubstr = ngain - ogain;
121       if (UNLIKELY(offset)) memset(outptr, '\0', offset*sizeof(MYFLT));
122       if (UNLIKELY(early)) memset(&outptr[nsmps], '\0', early*sizeof(MYFLT));
123       if (ngain != FL(0.0) || ogain != FL(0.0)) {
124         if (ngain != ogain) {
125           for (i = offset; i < nsmps; i++) {
126             outptr[i] = inptr[i] *
127               (ogain + (MYFLT)(i+1) * invfloatn * gainsubstr);
128           }
129           p->q.curr_gains[j]= ogain +
130             (MYFLT)(i) * invfloatn * gainsubstr;
131         }
132         else {
133           for (i=offset; i<nsmps; ++i)
134             outptr[i] = inptr[i] * ogain;
135         }
136       }
137       else {
138         memset(outptr, 0, nsmps*sizeof(MYFLT));
139       }
140     }
141     return OK;
142 }
143 
vbap_control(CSOUND * csound,VBAP_DATA * p,MYFLT * azi,MYFLT * ele,MYFLT * spread)144 int32_t vbap_control(CSOUND *csound, VBAP_DATA *p,
145                  MYFLT *azi, MYFLT *ele, MYFLT *spread)
146 {
147     CART_VEC spreaddir[16];
148     CART_VEC spreadbase[16];
149     ANG_VEC atmp;
150     int32 i,j, spreaddirnum;
151     int32_t cnt = p->number;
152     MYFLT *tmp_gains = malloc(sizeof(MYFLT)*cnt),sum=FL(0.0);
153     if (UNLIKELY(p->dim == 2 && fabs(*ele) > 0.0)) {
154       csound->Warning(csound,
155                       Str("Warning: truncating elevation to 2-D plane\n"));
156       *ele = FL(0.0);
157     }
158 
159     if (*spread <FL(0.0))
160       *spread = FL(0.0);
161     else if (*spread >FL(100.0))
162       *spread = FL(100.0);
163     /* Current panning angles */
164     p->ang_dir.azi = (MYFLT) *azi;
165     p->ang_dir.ele = (MYFLT) *ele;
166     p->ang_dir.length = FL(1.0);
167     angle_to_cart(p->ang_dir, &(p->cart_dir));
168     calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
169                   p->updated_gains, cnt, p->cart_dir);
170 
171     /* Calculated gain factors of a spreaded virtual source*/
172     if (*spread > FL(0.0)) {
173       if (p->dim == 3) {
174         spreaddirnum = 16;
175         /* four orthogonal dirs*/
176         new_spread_dir(&spreaddir[0], p->cart_dir,
177                        p->spread_base, *azi, *spread);
178         new_spread_base(spreaddir[0], p->cart_dir,
179                         *spread, &p->spread_base);
180         cross_prod(p->spread_base, p->cart_dir, &spreadbase[1]);
181         cross_prod(spreadbase[1], p->cart_dir, &spreadbase[2]);
182         cross_prod(spreadbase[2], p->cart_dir, &spreadbase[3]);
183         /* four between them*/
184         vec_mean(p->spread_base, spreadbase[1], &spreadbase[4]);
185         vec_mean(spreadbase[1], spreadbase[2], &spreadbase[5]);
186         vec_mean(spreadbase[2], spreadbase[3], &spreadbase[6]);
187         vec_mean(spreadbase[3], p->spread_base, &spreadbase[7]);
188 
189         /* four at half spreadangle*/
190         vec_mean(p->cart_dir, p->spread_base, &spreadbase[8]);
191         vec_mean(p->cart_dir, spreadbase[1], &spreadbase[9]);
192         vec_mean(p->cart_dir, spreadbase[2], &spreadbase[10]);
193         vec_mean(p->cart_dir, spreadbase[3], &spreadbase[11]);
194 
195         /* four at quarter spreadangle*/
196         vec_mean(p->cart_dir, spreadbase[8], &spreadbase[12]);
197         vec_mean(p->cart_dir, spreadbase[9], &spreadbase[13]);
198         vec_mean(p->cart_dir, spreadbase[10], &spreadbase[14]);
199         vec_mean(p->cart_dir, spreadbase[11], &spreadbase[15]);
200 
201         for (i=1;i<spreaddirnum;i++) {
202           new_spread_dir(&spreaddir[i], p->cart_dir,
203                          spreadbase[i],*azi,*spread);
204           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
205                         tmp_gains, cnt, spreaddir[i]);
206           for (j=0;j<cnt;j++) {
207             p->updated_gains[j] += tmp_gains[j];
208           }
209         }
210       }
211       else if (p->dim == 2) {
212         spreaddirnum = 6;
213         atmp.ele = FL(0.0);
214         atmp.azi = *azi - *spread;
215         angle_to_cart(atmp, &spreaddir[0]);
216         atmp.azi = *azi - *spread/2;
217         angle_to_cart(atmp, &spreaddir[1]);
218         atmp.azi = *azi - *spread/4;
219         angle_to_cart(atmp, &spreaddir[2]);
220         atmp.azi = *azi + *spread/4;
221         angle_to_cart(atmp, &spreaddir[3]);
222         atmp.azi = *azi + *spread/2;
223         angle_to_cart(atmp, &spreaddir[4]);
224         atmp.azi = *azi + *spread;
225         angle_to_cart(atmp, &spreaddir[5]);
226 
227         for (i=0;i<spreaddirnum;i++) {
228           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
229                         tmp_gains, cnt, spreaddir[i]);
230           for (j=0;j<cnt;j++) {
231             p->updated_gains[j] += tmp_gains[j];
232           }
233         }
234       }
235     }
236     if (*spread > FL(70.0))
237       for (i=0;i<cnt ;i++) {
238         p->updated_gains[i] +=(*spread - FL(70.0))/FL(30.0) *
239           (*spread - FL(70.0))/FL(30.0)*FL(20.0);
240       }
241 
242     /*normalization*/
243     for (i=0;i<cnt;i++) {
244       sum += p->updated_gains[i]*p->updated_gains[i];
245     }
246 
247     sum=SQRT(sum);
248     for (i=0;i<cnt;i++) {
249       p->updated_gains[i] /= sum;
250     }
251     free(tmp_gains);
252     return OK;
253 }
254 
vbap_init(CSOUND * csound,VBAP * p)255 int32_t vbap_init(CSOUND *csound, VBAP *p)
256 {                               /* Initializations before run time*/
257     int32_t     i, j;
258     MYFLT   *ls_table, *ptr;
259     LS_SET  *ls_set_ptr;
260     int32_t cnt = p->q.number = (int32_t)(p->OUTOCOUNT);
261     char name[24];
262 
263     snprintf(name, 24, "vbap_ls_table_%d", (p->layout==NULL?0:(int32_t)*p->layout));
264     ls_table = (MYFLT*) (csound->QueryGlobalVariable(csound, name));
265 
266     if (UNLIKELY(ls_table==NULL))
267       return csound->InitError(csound,
268                                Str("could not find layout table no.%d"),
269                                (int32_t)*p->layout );
270 
271     p->q.dim       = (int32_t)ls_table[0];   /* reading in loudspeaker info */
272     p->q.ls_am     = (int32_t)ls_table[1];
273     p->q.ls_set_am = (int32_t)ls_table[2];
274     ptr = &(ls_table[3]);
275     if (UNLIKELY(!p->q.ls_set_am))
276       return csound->InitError(csound,
277                                Str("vbap system NOT configured.\nMissing"
278                                    " vbaplsinit opcode in orchestra?"));
279     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof (LS_SET), &p->q.aux);
280     if (UNLIKELY(p->q.aux.auxp == NULL)) {
281       return csound->InitError(csound, Str("could not allocate memory"));
282     }
283     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
284     ls_set_ptr = p->q.ls_sets;
285     for (i=0; i < p->q.ls_set_am; i++) {
286       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
287       for (j=0 ; j < p->q.dim ; j++) {
288         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
289       }
290       memset(ls_set_ptr[i].ls_mx, '\0', 9*sizeof(MYFLT)); // initial setting
291       /* for (j=0 ; j < 9; j++) */
292       /*   ls_set_ptr[i].ls_mx[j] = FL(0.0);  /\*initial setting*\/ */
293       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
294         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
295       }
296     }
297 
298     /* other initialization */
299     if (UNLIKELY(p->q.dim == 2 && fabs(p->ele==NULL?0:*p->ele) > 0.0)) {
300       csound->Warning(csound,
301                       Str("Warning: truncating elevation to 2-D plane\n"));
302       *p->ele = FL(0.0);
303     }
304     p->q.ang_dir.azi    = (MYFLT)*p->azi;
305     p->q.ang_dir.ele    = (MYFLT)*p->ele;
306     p->q.ang_dir.length = FL(1.0);
307     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
308     p->q.spread_base.x  = p->q.cart_dir.y;
309     p->q.spread_base.y  = p->q.cart_dir.z;
310     p->q.spread_base.z  = -p->q.cart_dir.x;
311     vbap_control(csound,&(p->q), p->azi, p->ele, p->spread);
312     for (i=0;i<cnt;i++) {
313       p->q.beg_gains[i] = p->q.updated_gains[i];
314       p->q.end_gains[i] = p->q.updated_gains[i];
315     }
316     return OK;
317 }
318 
vbap_init_a(CSOUND * csound,VBAPA * p)319 int32_t vbap_init_a(CSOUND *csound, VBAPA *p)
320 {                               /* Initializations before run time*/
321     int32_t     i, j;
322     MYFLT   *ls_table, *ptr;
323     LS_SET  *ls_set_ptr;
324     int32_t cnt;
325     char name[24];
326 
327     snprintf(name, 24, "vbap_ls_table_%d", (int32_t)*p->layout);
328     ls_table = (MYFLT*) (csound->QueryGlobalVariable(csound, name));
329 
330     if (UNLIKELY(ls_table==NULL))
331       return csound->InitError(csound,
332                                Str("could not find layout table no.%d"),
333                                (int32_t)*p->layout );
334 
335     p->q.dim       = (int32_t)ls_table[0];   /* reading in loudspeaker info */
336     p->q.ls_am     = (int32_t)ls_table[1];
337     p->q.ls_set_am = (int32_t)ls_table[2];
338     ptr = &(ls_table[3]);
339     if (UNLIKELY(!p->q.ls_set_am))
340       return csound->InitError(csound,
341                                Str("vbap system NOT configured.\nMissing"
342                                    " vbaplsinit opcode in orchestra?"));
343     //printf("**** size = %d\n", p->q.ls_set_am);
344     tabinit(csound,  p->tabout, p->q.ls_set_am);
345     cnt = p->q.number = p->tabout->sizes[0];
346     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof(LS_SET), &p->q.aux);
347     if (UNLIKELY(p->q.aux.auxp == NULL)) {
348       return csound->InitError(csound, Str("could not allocate memory"));
349     }
350     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
351     ls_set_ptr = p->q.ls_sets;
352     for (i=0; i < p->q.ls_set_am; i++) {
353       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
354       for (j=0 ; j < p->q.dim ; j++) {
355         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
356       }
357       memset(ls_set_ptr[i].ls_mx, '\0', 9*sizeof(MYFLT));
358       /* for (j=0 ; j < 9; j++) */
359       /*   ls_set_ptr[i].ls_mx[j] = FL(0.0);  /\*initial setting*\/ */
360       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
361         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
362       }
363     }
364 
365     /* other initialization */
366     if (UNLIKELY(p->q.dim == 2 && fabs(*p->ele) > 0.0)) {
367       csound->Warning(csound,
368                       Str("Warning: truncating elevation to 2-D plane\n"));
369       *p->ele = FL(0.0);
370     }
371     p->q.ang_dir.azi    = *p->azi;
372     p->q.ang_dir.ele    = *p->ele;
373     p->q.ang_dir.length = FL(1.0);
374     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
375     p->q.spread_base.x  = p->q.cart_dir.y;
376     p->q.spread_base.y  = p->q.cart_dir.z;
377     p->q.spread_base.z  = -p->q.cart_dir.x;
378     vbap_control(csound,&(p->q), p->azi, p->ele, p->spread);
379     for (i=0;i<cnt;i++) {
380       p->q.beg_gains[i] = p->q.updated_gains[i];
381       p->q.end_gains[i] = p->q.updated_gains[i];
382     }
383     return OK;
384 }
385 
vbap_moving(CSOUND * csound,VBAP_MOVING * p)386 int32_t vbap_moving(CSOUND *csound, VBAP_MOVING *p)
387 {                               /* during note performance:   */
388     MYFLT *outptr, *inptr;
389     MYFLT ogain, ngain, gainsubstr;
390     MYFLT invfloatn;
391     int32_t j;
392     uint32_t offset = p->h.insdshead->ksmps_offset;
393     uint32_t early  = p->h.insdshead->ksmps_no_end;
394     uint32_t i, nsmps = CS_KSMPS;
395     int32_t cnt = p->q.number;
396 
397     vbap_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
398                         p->spread, p->field_am, p->fld);
399     //    vbap_moving_control(csound,p);
400     for (j=0;j<cnt; j++) {
401       p->q.beg_gains[j] = p->q.end_gains[j];
402       p->q.end_gains[j] = p->q.updated_gains[j];
403     }
404 
405     /* write audio to resulting audio streams weighted
406        with gain factors*/
407     if (UNLIKELY(early)) nsmps -= early;
408     invfloatn = FL(1.0)/(nsmps-offset);
409     for (j=0; j<cnt ;j++) {
410       inptr = p->audio;
411       outptr = p->out_array[j];
412       if (UNLIKELY(offset)) memset(outptr, '\0', offset*sizeof(MYFLT));
413       if (UNLIKELY(early)) memset(&outptr[nsmps], '\0', early*sizeof(MYFLT));
414       ogain  = p->q.beg_gains[j];
415       ngain  = p->q.end_gains[j];
416       gainsubstr = ngain - ogain;
417       if (ngain != FL(0.0) || ogain != FL(0.0))
418         if (ngain != ogain) {
419           for (i = offset; i < nsmps; i++) {
420             outptr[i] = inptr[i] *
421               (ogain + (MYFLT)(i+1) * invfloatn * gainsubstr);
422           }
423           p->q.curr_gains[j]= ogain +
424             (MYFLT)(i) * invfloatn * gainsubstr;
425         }
426         else
427           for (i=offset; i<nsmps; ++i)
428             outptr[i] = inptr[i] * ogain;
429       else
430         memset(outptr, 0, nsmps*sizeof(MYFLT));
431     }
432     return OK;
433 }
434 
vbap_moving_control(CSOUND * csound,VBAP_MOVE_DATA * p,OPDS * h,MYFLT ONEDKR,MYFLT * spread,MYFLT * field_am,MYFLT * fld[])435 int32_t vbap_moving_control(CSOUND *csound, VBAP_MOVE_DATA *p, OPDS *h,
436                         MYFLT ONEDKR, MYFLT* spread, MYFLT* field_am, MYFLT *fld[])
437 {
438     CART_VEC spreaddir[16];
439     CART_VEC spreadbase[16];
440     ANG_VEC atmp;
441     int32 i,j, spreaddirnum;
442     CART_VEC tmp1, tmp2, tmp3;
443     MYFLT coeff, angle;
444     int32_t cnt = p->number;
445     MYFLT *tmp_gains=malloc(sizeof(MYFLT)*cnt),sum=FL(0.0);
446 
447     if (UNLIKELY(p->dim == 2 && fabs(p->ang_dir.ele) > 0.0)) {
448       csound->Warning(csound,
449                       Str("Warning: truncating elevation to 2-D plane\n"));
450       p->ang_dir.ele = FL(0.0);
451     }
452     if (*spread <FL(0.0))
453       *spread = FL(0.0);
454     else if (*spread >FL(100.0))
455       *spread = FL(100.0);
456     if (p->point_change_counter++ >= p->point_change_interval) {
457       p->point_change_counter = 0;
458       p->curr_fld = p->next_fld;
459       if (++p->next_fld >= (int32_t) fabs(*field_am)) {
460         if (*field_am >= FL(0.0)) /* point-to-point */
461           p->next_fld = 0;
462         else
463           p->next_fld = 1;
464       }
465       if (p->dim == 3) { /*jumping over second field */
466         p->curr_fld = p->next_fld;
467         if (++p->next_fld >= ((int32_t) fabs(*field_am))) {
468           if (*field_am >= FL(0.0)) /* point-to-point */
469             p->next_fld = 0;
470           else
471             p->next_fld = 1;
472         }
473       }
474       if (UNLIKELY((fld[abs(p->next_fld)]==NULL)))
475         return csound->PerfError(csound, h,
476                                  Str("Missing fields in vbapmove\n"));
477       if (*field_am >= FL(0.0) && p->dim == 2) /* point-to-point */
478         if (UNLIKELY(fabs(fabs(*fld[p->next_fld] -
479                                *fld[p->curr_fld]) - 180.0) < 1.0))
480           csound->Warning(csound,
481                           Str("Warning: Ambiguous transition 180 degrees.\n"));
482     }
483     if (*field_am >= FL(0.0)) { /* point-to-point */
484       if (p->dim == 3) { /* 3-D*/
485         p->prev_ang_dir.azi =  *fld[p->curr_fld-1];
486         p->next_ang_dir.azi =  *fld[p->next_fld];
487         p->prev_ang_dir.ele = *fld[p->curr_fld];
488         p->next_ang_dir.ele = *fld[p->next_fld+1];
489         coeff = ((MYFLT) p->point_change_counter) /
490           ((MYFLT) p->point_change_interval);
491         angle_to_cart( p->prev_ang_dir,&tmp1);
492         angle_to_cart( p->next_ang_dir,&tmp2);
493         tmp3.x = (FL(1.0)-coeff) * tmp1.x + coeff * tmp2.x;
494         tmp3.y = (FL(1.0)-coeff) * tmp1.y + coeff * tmp2.y;
495         tmp3.z = (FL(1.0)-coeff) * tmp1.z + coeff * tmp2.z;
496         coeff = (MYFLT)sqrt((double)(tmp3.x * tmp3.x +
497                                      tmp3.y * tmp3.y +
498                                      tmp3.z * tmp3.z));
499         tmp3.x /= coeff; tmp3.y /= coeff; tmp3.z /= coeff;
500         cart_to_angle(tmp3,&(p->ang_dir));
501       }
502       else if (LIKELY(p->dim == 2)) { /* 2-D */
503         p->prev_ang_dir.azi =  *fld[p->curr_fld];
504         p->next_ang_dir.azi =  *fld[p->next_fld ];
505         p->prev_ang_dir.ele = p->next_ang_dir.ele =  FL(0.0);
506         scale_angles(&(p->prev_ang_dir));
507         scale_angles(&(p->next_ang_dir));
508         angle = (p->prev_ang_dir.azi - p->next_ang_dir.azi);
509         while (angle > FL(180.0))
510           angle -= FL(360.0);
511         while (angle < -FL(180.0))
512           angle += FL(360.0);
513         coeff = ((MYFLT) p->point_change_counter) /
514           ((MYFLT) p->point_change_interval);
515         angle  *=  (coeff);
516         p->ang_dir.azi = p->prev_ang_dir.azi -  angle;
517         p->ang_dir.ele = FL(0.0);
518       }
519       else {
520         return csound->PerfError(csound, h,
521                                  Str("Missing fields in vbapmove\n"));
522       }
523     }
524     else { /* angular velocities */
525       if (p->dim == 2) {
526         p->ang_dir.azi =  p->ang_dir.azi +
527           (*fld[p->next_fld] * ONEDKR);
528         scale_angles(&(p->ang_dir));
529       }
530       else { /* 3D angular*/
531         p->ang_dir.azi =  p->ang_dir.azi +
532           (*fld[p->next_fld] * ONEDKR);
533         p->ang_dir.ele =  p->ang_dir.ele +
534           p->ele_vel * (*fld[p->next_fld+1] * ONEDKR);
535         if (p->ang_dir.ele > FL(90.0)) {
536           p->ang_dir.ele = FL(90.0);
537           p->ele_vel = -p->ele_vel;
538         }
539         if (p->ang_dir.ele < FL(0.0)) {
540           p->ang_dir.ele = FL(0.0);
541           p->ele_vel =  -p->ele_vel;
542         }
543         scale_angles(&(p->ang_dir));
544       }
545     }
546     angle_to_cart(p->ang_dir, &(p->cart_dir));
547     calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
548                   p->updated_gains, cnt, p->cart_dir);
549     if (*spread > FL(0.0)) {
550       if (p->dim == 3) {
551         spreaddirnum=16;
552         /* four orthogonal dirs*/
553         new_spread_dir(&spreaddir[0], p->cart_dir,
554                        p->spread_base, p->ang_dir.azi, *spread);
555 
556         new_spread_base(spreaddir[0], p->cart_dir,*spread, &p->spread_base);
557         cross_prod(p->spread_base, p->cart_dir, &spreadbase[1]);
558         cross_prod(spreadbase[1], p->cart_dir, &spreadbase[2]);
559         cross_prod(spreadbase[2], p->cart_dir, &spreadbase[3]);
560         /* four between them*/
561         vec_mean(p->spread_base, spreadbase[1], &spreadbase[4]);
562         vec_mean(spreadbase[1], spreadbase[2], &spreadbase[5]);
563         vec_mean(spreadbase[2], spreadbase[3], &spreadbase[6]);
564         vec_mean(spreadbase[3], p->spread_base, &spreadbase[7]);
565 
566         /* four at half spreadangle*/
567         vec_mean(p->cart_dir, p->spread_base, &spreadbase[8]);
568         vec_mean(p->cart_dir, spreadbase[1], &spreadbase[9]);
569         vec_mean(p->cart_dir, spreadbase[2], &spreadbase[10]);
570         vec_mean(p->cart_dir, spreadbase[3], &spreadbase[11]);
571 
572         /* four at quarter spreadangle*/
573         vec_mean(p->cart_dir, spreadbase[8], &spreadbase[12]);
574         vec_mean(p->cart_dir, spreadbase[9], &spreadbase[13]);
575         vec_mean(p->cart_dir, spreadbase[10], &spreadbase[14]);
576         vec_mean(p->cart_dir, spreadbase[11], &spreadbase[15]);
577 
578         for (i=1;i<spreaddirnum;i++) {
579           new_spread_dir(&spreaddir[i], p->cart_dir,
580                          spreadbase[i],p->ang_dir.azi,*spread);
581           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
582                         tmp_gains, cnt, spreaddir[i]);
583           for (j=0;j<cnt;j++) {
584             p->updated_gains[j] += tmp_gains[j];
585           }
586         }
587       }
588       else if (p->dim == 2) {
589         spreaddirnum=6;
590         atmp.ele = FL(0.0);
591         atmp.azi = p->ang_dir.azi - *spread;
592         angle_to_cart(atmp, &spreaddir[0]);
593         atmp.azi = p->ang_dir.azi - *spread/2;
594         angle_to_cart(atmp, &spreaddir[1]);
595         atmp.azi = p->ang_dir.azi - *spread/4;
596         angle_to_cart(atmp, &spreaddir[2]);
597         atmp.azi = p->ang_dir.azi + *spread/4;
598         angle_to_cart(atmp, &spreaddir[3]);
599         atmp.azi = p->ang_dir.azi + *spread/2;
600         angle_to_cart(atmp, &spreaddir[4]);
601         atmp.azi = p->ang_dir.azi + *spread;
602         angle_to_cart(atmp, &spreaddir[5]);
603 
604         for (i=0;i<spreaddirnum;i++) {
605           calc_vbap_gns(p->ls_set_am, p->dim,  p->ls_sets,
606                         tmp_gains, cnt, spreaddir[i]);
607           for (j=0;j<cnt;j++) {
608             p->updated_gains[j] += tmp_gains[j];
609           }
610         }
611       }
612     }
613     if (*spread > FL(70.0))
614       for (i=0;i<cnt ;i++) {
615         p->updated_gains[i] +=(*spread - FL(70.0))/FL(30.0) *
616           (*spread - FL(70.0))/FL(30.0)*FL(10.0);
617       }
618     /*normalization*/
619     for (i=0;i<cnt;i++) {
620       sum=sum+(p->updated_gains[i]*p->updated_gains[i]);
621     }
622 
623     sum=SQRT(sum);
624     for (i=0;i<cnt;i++) {
625       p->updated_gains[i] /= sum;
626     }
627     free(tmp_gains);
628     return OK;
629 }
630 
vbap_moving_init(CSOUND * csound,VBAP_MOVING * p)631 int32_t vbap_moving_init(CSOUND *csound, VBAP_MOVING *p)
632 {
633     int32_t     i, j;
634     MYFLT   *ls_table, *ptr;
635     LS_SET  *ls_set_ptr;
636     int32_t cnt = (int32_t)p->h.optext->t.outArgCount;
637     if ((!strncmp(p->h.optext->t.opcod, "vbapmove", 8)) == 0) {
638       p->audio = p->out_array[cnt];
639       p->dur = p->out_array[cnt+1];
640       p->spread = p->out_array[cnt+2];
641       p->field_am = p->out_array[cnt+3];
642       memcpy(p->fld, &(p->out_array[cnt+4]),
643              sizeof(MYFLT *)*(p->h.optext->t.inArgCount-4));
644     }
645 
646     ls_table = (MYFLT*) (csound->QueryGlobalVariableNoCheck(csound,
647                                                         "vbap_ls_table_0"));
648     if (UNLIKELY(ls_table==NULL))
649       return csound->InitError(csound, Str("could not find layout table no.0"));
650     p->q.number = cnt;
651     /* reading in loudspeaker info */
652     p->q.dim       = (int32_t)ls_table[0];
653     p->q.ls_am     = (int32_t)ls_table[1];
654     p->q.ls_set_am = (int32_t)ls_table[2];
655     ptr = &(ls_table[3]);
656     if (UNLIKELY(!p->q.ls_set_am))
657       return csound->InitError(csound, Str("vbap system NOT configured.\nMissing"
658                                            " vbaplsinit opcode in orchestra?"));
659     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof(LS_SET), &p->q.aux);
660     if (UNLIKELY(p->q.aux.auxp == NULL)) {
661       return csound->InitError(csound, Str("could not allocate memory"));
662     }
663     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
664     ls_set_ptr = p->q.ls_sets;
665     for (i=0 ; i < p->q.ls_set_am ; i++) {
666       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
667       for (j=0 ; j < p->q.dim ; j++) {
668         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
669       }
670       for (j=0 ; j < 9; j++)
671         ls_set_ptr[i].ls_mx[j] = FL(0.0);  /*initial setting*/
672       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
673         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
674       }
675     }
676 
677     /* other initialization */
678     p->q.ele_vel = FL(1.0);    /* functions specific to movement */
679     if (UNLIKELY(fabs(*p->field_am) < (2+ (p->q.dim - 2)*2))) {
680       return csound->InitError(csound,
681                   Str("Have to have at least %d directions in vbapmove"),
682                   2 + (p->q.dim - 2) * 2);
683     }
684     if (p->q.dim == 2)
685       p->q.point_change_interval =
686         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am) - 1.0));
687     else if (LIKELY(p->q.dim == 3))
688       p->q.point_change_interval =
689         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am)*0.5 - 1.0));
690     else
691       return csound->InitError(csound, Str("Wrong dimension"));
692     p->q.point_change_counter = 0;
693     p->q.curr_fld = 0;
694     p->q.next_fld = 1;
695     p->q.ang_dir.azi = *p->fld[0];
696     if (p->q.dim == 3) {
697       p->q.ang_dir.ele = *p->fld[1];
698     } else {
699       p->q.ang_dir.ele = FL(0.0);
700     }
701     if (p->q.dim == 3) {
702       p->q.curr_fld = 1;
703       p->q.next_fld = 2;
704     }
705     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
706     p->q.spread_base.x  = p->q.cart_dir.y;
707     p->q.spread_base.y  = p->q.cart_dir.z;
708     p->q.spread_base.z  = -p->q.cart_dir.x;
709     vbap_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
710                         p->spread, p->field_am, p->fld);
711     for (i = 0; i<cnt; i++) {
712       p->q.beg_gains[i] = p->q.updated_gains[i];
713       p->q.end_gains[i] = p->q.updated_gains[i];
714     }
715     return OK;
716 }
717 
vbap_moving_a(CSOUND * csound,VBAPA_MOVING * p)718 int32_t vbap_moving_a(CSOUND *csound, VBAPA_MOVING *p)
719 {                               /* during note performance:   */
720     MYFLT *outptr, *inptr;
721     MYFLT ogain, ngain, gainsubstr;
722     MYFLT invfloatn;
723     int32_t j;
724     uint32_t offset = p->h.insdshead->ksmps_offset;
725     uint32_t early  = p->h.insdshead->ksmps_no_end;
726     uint32_t i, nsmps = CS_KSMPS;
727     uint32_t ksmps = nsmps;
728     int32_t cnt = p->q.number;
729 
730     vbap_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
731                         p->spread, p->field_am, p->fld);
732     //    vbap_moving_control(csound,p);
733     for (j=0;j<cnt; j++) {
734       p->q.beg_gains[j] = p->q.end_gains[j];
735       p->q.end_gains[j] = p->q.updated_gains[j];
736     }
737 
738     /* write audio to resulting audio streams weighted
739        with gain factors*/
740     if (UNLIKELY(early)) nsmps -= early;
741     invfloatn = FL(1.0)/(nsmps-offset);
742     outptr = p->tabout->data;
743     for (j=0; j<cnt ;j++) {
744       inptr = p->audio;
745       outptr = &p->tabout->data[j*ksmps];
746       if (UNLIKELY(offset)) memset(outptr, '\0', offset*sizeof(MYFLT));
747       if (UNLIKELY(early)) memset(&outptr[nsmps], '\0', early*sizeof(MYFLT));
748       ogain  = p->q.beg_gains[j];
749       ngain  = p->q.end_gains[j];
750       gainsubstr = ngain - ogain;
751       if (ngain != FL(0.0) || ogain != FL(0.0))
752         if (ngain != ogain) {
753           for (i = offset; i < nsmps; i++) {
754             outptr[i] = inptr[i] *
755               (ogain + (MYFLT)(i+1) * invfloatn * gainsubstr);
756           }
757           p->q.curr_gains[j]= ogain +
758             (MYFLT)(i) * invfloatn * gainsubstr;
759         }
760         else
761           for (i=offset; i<nsmps; ++i)
762             outptr[i] = inptr[i] * ogain;
763       else
764         memset(outptr, 0, nsmps*sizeof(MYFLT));
765     }
766     return OK;
767 }
768 
vbap_moving_init_a(CSOUND * csound,VBAPA_MOVING * p)769 int32_t vbap_moving_init_a(CSOUND *csound, VBAPA_MOVING *p)
770 {
771     int32_t     i, j;
772     MYFLT   *ls_table, *ptr;
773     LS_SET  *ls_set_ptr;
774     int32_t cnt;
775 
776     if (UNLIKELY(p->tabout->data==NULL)) {
777       return csound->InitError(csound,
778                                Str("Output array in vpabmove not initalised"));
779     }
780     cnt = p->tabout->sizes[0];
781 
782     ls_table = (MYFLT*) (csound->QueryGlobalVariableNoCheck(csound,
783                                                         "vbap_ls_table_0"));
784     if (UNLIKELY(ls_table==NULL))
785       return csound->InitError(csound, Str("could not find layout table no.0"));
786     p->q.number = cnt;
787     /* reading in loudspeaker info */
788     p->q.dim       = (int32_t)ls_table[0];
789     p->q.ls_am     = (int32_t)ls_table[1];
790     p->q.ls_set_am = (int32_t)ls_table[2];
791     ptr = &(ls_table[3]);
792     if (UNLIKELY(!p->q.ls_set_am))
793       return csound->InitError(csound,
794                                Str("vbap system NOT configured.\nMissing"
795                                    " vbaplsinit opcode in orchestra?"));
796     csound->AuxAlloc(csound, p->q.ls_set_am * sizeof(LS_SET), &p->q.aux);
797     if (UNLIKELY(p->q.aux.auxp == NULL)) {
798       return csound->InitError(csound, Str("could not allocate memory"));
799     }
800     p->q.ls_sets = (LS_SET*) p->q.aux.auxp;
801     ls_set_ptr = p->q.ls_sets;
802     for (i=0 ; i < p->q.ls_set_am ; i++) {
803       ls_set_ptr[i].ls_nos[2] = 0;     /* initial setting */
804       for (j=0 ; j < p->q.dim ; j++) {
805         ls_set_ptr[i].ls_nos[j] = (int32_t)*(ptr++);
806       }
807       memset(ls_set_ptr[i].ls_mx, '\0', 9*sizeof(MYFLT));
808       /* for (j=0 ; j < 9; j++) */
809       /*   ls_set_ptr[i].ls_mx[j] = FL(0.0);  /\*initial setting*\/ */
810       for (j=0 ; j < (p->q.dim) * (p->q.dim); j++) {
811         ls_set_ptr[i].ls_mx[j] = (MYFLT)*(ptr++);
812       }
813     }
814 
815     /* other initialization */
816     p->q.ele_vel = FL(1.0);    /* functions specific to movement */
817     if (UNLIKELY(fabs(*p->field_am) < (2+ (p->q.dim - 2)*2))) {
818       return csound->InitError(csound,
819                   Str("Have to have at least %d directions in vbapmove"),
820                   2 + (p->q.dim - 2) * 2);
821     }
822     if (p->q.dim == 2)
823       p->q.point_change_interval =
824         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am) - 1.0));
825     else if (LIKELY(p->q.dim == 3))
826       p->q.point_change_interval =
827         (int32_t)(CS_EKR * *p->dur /(fabs(*p->field_am)*0.5 - 1.0));
828     else
829       return csound->InitError(csound, Str("Wrong dimension"));
830     p->q.point_change_counter = 0;
831     p->q.curr_fld = 0;
832     p->q.next_fld = 1;
833     p->q.ang_dir.azi = *p->fld[0];
834     if (p->q.dim == 3) {
835       p->q.ang_dir.ele = *p->fld[1];
836     } else {
837       p->q.ang_dir.ele = FL(0.0);
838     }
839     if (p->q.dim == 3) {
840       p->q.curr_fld = 1;
841       p->q.next_fld = 2;
842     }
843     angle_to_cart(p->q.ang_dir, &(p->q.cart_dir));
844     p->q.spread_base.x  = p->q.cart_dir.y;
845     p->q.spread_base.y  = p->q.cart_dir.z;
846     p->q.spread_base.z  = -p->q.cart_dir.x;
847     vbap_moving_control(csound,&p->q, &(p->h), CS_ONEDKR,
848                         p->spread, p->field_am, p->fld);
849     for (i = 0; i<cnt; i++) {
850       p->q.beg_gains[i] = p->q.updated_gains[i];
851       p->q.end_gains[i] = p->q.updated_gains[i];
852     }
853     return OK;
854 }
855 
856 
857