libs/panners/vbap/vbap_speakers.cc

   1 /*
   2    This software is being provided to you, the licensee, by Ville Pulkki,
   3    under the following license. By obtaining, using and/or copying this
   4    software, you agree that you have read, understood, and will comply
   5    with these terms and conditions: Permission to use, copy, modify and
   6    distribute, including the right to grant others rights to distribute
   7    at any tier, this software and its documentation for any purpose and
   8    without fee or royalty is hereby granted, provided that you agree to
   9    comply with the following copyright notice and statements, including
  10    the disclaimer, and that the same appear on ALL copies of the software
  11    and documentation, including modifications that you make for internal
  12    use or for distribution:
  13
  14    Copyright 1998 by Ville Pulkki, Helsinki University of Technology.  All
  15    rights reserved.
  16
  17    The software may be used, distributed, and included to commercial
  18    products without any charges. When included to a commercial product,
  19    the method "Vector Base Amplitude Panning" and its developer Ville
  20    Pulkki must be referred to in documentation.
  21
  22    This software is provided "as is", and Ville Pulkki or Helsinki
  23    University of Technology make no representations or warranties,
  24    expressed or implied. By way of example, but not limitation, Helsinki
  25    University of Technology or Ville Pulkki make no representations or
  26    warranties of merchantability or fitness for any particular purpose or
  27    that the use of the licensed software or documentation will not
  28    infringe any third party patents, copyrights, trademarks or other
  29    rights. The name of Ville Pulkki or Helsinki University of Technology
  30    may not be used in advertising or publicity pertaining to distribution
  31    of the software.
  32 */
  33
  34 #include <cmath>
  35 #include <algorithm>
  36 #include <stdlib.h>
  37
  38 #include "pbd/cartesian.h"
  39
  40 #include "vbap_speakers.h"
  41
  42 using namespace ARDOUR;
  43 using namespace PBD;
  44 using namespace std;
  45
  46 const double VBAPSpeakers::MIN_VOL_P_SIDE_LGTH = 0.01;
  47
  48 VBAPSpeakers::VBAPSpeakers (boost::shared_ptr<Speakers> s)
  49         : _dimension (2)
  50         , _parent (s)
  51 {
  52         _parent->Changed.connect_same_thread (speaker_connection, boost::bind (&VBAPSpeakers::update, this));
  53         update ();
  54 }
  55
  56 VBAPSpeakers::~VBAPSpeakers ()
  57 {
  58 }
  59
  60 void
  61 VBAPSpeakers::update ()
  62 {
  63         int dim = 2;
  64
  65         _speakers = _parent->speakers();
  66
  67         for (vector<Speaker>::const_iterator i = _speakers.begin(); i != _speakers.end(); ++i) {
  68                 if ((*i).angles().ele != 0.0) {
  69                         dim = 3;
  70                         break;
  71                 }
  72         }
  73
  74         _dimension = dim;
  75
  76         if (_speakers.size() < 2) {
  77                 /* nothing to be done with less than two speakers */
  78                 return;
  79         }
  80
  81         if (_dimension == 3)  {
  82                 ls_triplet_chain *ls_triplets = 0;
  83                 choose_speaker_triplets (&ls_triplets);
  84                 if (ls_triplets) {
  85                         calculate_3x3_matrixes (ls_triplets);
  86                         free (ls_triplets);
  87                 }
  88         } else {
  89                 choose_speaker_pairs ();
  90         }
  91 }
  92
  93 void
  94 VBAPSpeakers::choose_speaker_triplets(struct ls_triplet_chain **ls_triplets)
  95 {
  96         /* Selects the loudspeaker triplets, and
  97            calculates the inversion matrices for each selected triplet.
  98            A line (connection) is drawn between each loudspeaker. The lines
  99            denote the sides of the triangles. The triangles should not be
 100            intersecting. All crossing connections are searched and the
 101            longer connection is erased. This yields non-intesecting triangles,
 102            which can be used in panning.
 103         */
 104
 105 #if 0 // DEVEL/DEBUG
 106         for (vector<Speaker>::iterator i = _speakers.begin(); i != _speakers.end(); ++i) {
 107                 cout << "Speaker " << (*i).id << " @ "
 108                   << (*i).coords().x << ", " << (*i).coords().y << ", " << (*i).coords().z
 109                   << " azimuth " << (*i).angles().azi
 110                   << " elevation " << (*i).angles().ele
 111                   << " distance " << (*i).angles().length
 112                   << endl;
 113         }
 114 #endif
 115
 116         int i,j,k,l,table_size;
 117         int n_speakers = _speakers.size ();
 118
 119         if (n_speakers < 3) {
 120                 fprintf(stderr, "VBAP: at least 3 speakers need to be defined.");
 121                 return;
 122         }
 123
 124         /* variable length arrays arrived in C99, became optional in C11, and
 125            are only planned for C++14. Use alloca which is functionally
 126            identical (but uglier to read).
 127         */
 128         int* connections = (int*) alloca (sizeof (int) * n_speakers * n_speakers);
 129         float* distance_table = (float *) alloca (sizeof (float) * ((n_speakers * (n_speakers - 1)) / 2));
 130         int* distance_table_i = (int *) alloca (sizeof (int) * ((n_speakers * (n_speakers - 1)) / 2));
 131         int* distance_table_j = (int *) alloca (sizeof (int) * ((n_speakers * (n_speakers - 1)) / 2));
 132         float distance;
 133         struct ls_triplet_chain *trip_ptr, *prev, *tmp_ptr;
 134
 135         for (i = 0; i < n_speakers * n_speakers; i++) {
 136                 connections[i] = 0;
 137         }
 138
 139         for (i = 0; i < n_speakers; i++) {
 140                 for (j = i+1; j < n_speakers; j++) {
 141                         for(k = j+1; k < n_speakers; k++) {
 142                                 if (vol_p_side_lgth(i, j, k, _speakers) > MIN_VOL_P_SIDE_LGTH) {
 143                                         connections[(i*n_speakers)+j]=1;
 144                                         connections[(j*n_speakers)+i]=1;
 145                                         connections[(i*n_speakers)+k]=1;
 146                                         connections[(k*n_speakers)+i]=1;
 147                                         connections[(j*n_speakers)+k]=1;
 148                                         connections[(k*n_speakers)+j]=1;
 149                                         add_ldsp_triplet(i,j,k,ls_triplets);
 150                                 }
 151                         }
 152                 }
 153         }
 154
 155         /*calculate distancies between all speakers and sorting them*/
 156         table_size =(((n_speakers - 1) * (n_speakers)) / 2);
 157         for (i = 0; i < table_size; i++) {
 158                 distance_table[i] = 100000.0;
 159         }
 160
 161         for (i = 0;i < n_speakers; i++) {
 162                 for (j = i+1; j < n_speakers; j++) {
 163                         if (connections[(i*n_speakers)+j] == 1) {
 164                                 distance = fabs(vec_angle(_speakers[i].coords(),_speakers[j].coords()));
 165                                 k=0;
 166                                 while(distance_table[k] < distance) {
 167                                         k++;
 168                                 }
 169                                 for (l = table_size - 1; l > k ; l--) {
 170                                         distance_table[l] = distance_table[l-1];
 171                                         distance_table_i[l] = distance_table_i[l-1];
 172                                         distance_table_j[l] = distance_table_j[l-1];
 173                                 }
 174                                 distance_table[k] = distance;
 175                                 distance_table_i[k] = i;
 176                                 distance_table_j[k] = j;
 177                         } else
 178                                 table_size--;
 179                 }
 180         }
 181
 182         /* disconnecting connections which are crossing shorter ones,
 183            starting from shortest one and removing all that cross it,
 184            and proceeding to next shortest */
 185         for (i = 0; i < table_size; i++) {
 186                 int fst_ls = distance_table_i[i];
 187                 int sec_ls = distance_table_j[i];
 188                 if (connections[(fst_ls*n_speakers)+sec_ls] == 1) {
 189                         for (j = 0; j < n_speakers; j++) {
 190                                 for (k = j+1; k < n_speakers; k++) {
 191                                         if ((j != fst_ls) && (k != sec_ls) && (k != fst_ls) && (j != sec_ls)) {
 192                                                 if (lines_intersect(fst_ls, sec_ls, j, k) == 1){
 193                                                         connections[(j*n_speakers)+k] = 0;
 194                                                         connections[(k*n_speakers)+j] = 0;
 195                                                 }
 196                                         }
 197                                 }
 198                         }
 199                 }
 200         }
 201
 202         /* remove triangles which had crossing sides
 203            with smaller triangles or include loudspeakers*/
 204         trip_ptr = *ls_triplets;
 205         prev = 0;
 206         while (trip_ptr != 0){
 207                 i = trip_ptr->ls_nos[0];
 208                 j = trip_ptr->ls_nos[1];
 209                 k = trip_ptr->ls_nos[2];
 210                 if (connections[(i*n_speakers)+j] == 0 ||
 211                     connections[(i*n_speakers)+k] == 0 ||
 212                     connections[(j*n_speakers)+k] == 0 ||
 213                     any_ls_inside_triplet(i,j,k) == 1 ){
 214                         if (prev != 0) {
 215                                 prev->next = trip_ptr->next;
 216                                 tmp_ptr = trip_ptr;
 217                                 trip_ptr = trip_ptr->next;
 218                                 free(tmp_ptr);
 219                         } else {
 220                                 *ls_triplets = trip_ptr->next;
 221                                 tmp_ptr = trip_ptr;
 222                                 trip_ptr = trip_ptr->next;
 223                                 free(tmp_ptr);
 224                         }
 225                 } else {
 226                         prev = trip_ptr;
 227                         trip_ptr = trip_ptr->next;
 228
 229                 }
 230         }
 231 }
 232
 233 int
 234 VBAPSpeakers::any_ls_inside_triplet(int a, int b, int c)
 235 {
 236         /* returns 1 if there is loudspeaker(s) inside given ls triplet */
 237         float invdet;
 238         const CartesianVector* lp1;
 239         const CartesianVector* lp2;
 240         const CartesianVector* lp3;
 241         float invmx[9];
 242         int i,j;
 243         float tmp;
 244         bool any_ls_inside;
 245         bool this_inside;
 246         int n_speakers = _speakers.size();
 247
 248         lp1 =  &(_speakers[a].coords());
 249         lp2 =  &(_speakers[b].coords());
 250         lp3 =  &(_speakers[c].coords());
 251
 252         /* matrix inversion */
 253         invdet = 1.0 / (  lp1->x * ((lp2->y * lp3->z) - (lp2->z * lp3->y))
 254                           - lp1->y * ((lp2->x * lp3->z) - (lp2->z * lp3->x))
 255                           + lp1->z * ((lp2->x * lp3->y) - (lp2->y * lp3->x)));
 256
 257         invmx[0] = ((lp2->y * lp3->z) - (lp2->z * lp3->y)) * invdet;
 258         invmx[3] = ((lp1->y * lp3->z) - (lp1->z * lp3->y)) * -invdet;
 259         invmx[6] = ((lp1->y * lp2->z) - (lp1->z * lp2->y)) * invdet;
 260         invmx[1] = ((lp2->x * lp3->z) - (lp2->z * lp3->x)) * -invdet;
 261         invmx[4] = ((lp1->x * lp3->z) - (lp1->z * lp3->x)) * invdet;
 262         invmx[7] = ((lp1->x * lp2->z) - (lp1->z * lp2->x)) * -invdet;
 263         invmx[2] = ((lp2->x * lp3->y) - (lp2->y * lp3->x)) * invdet;
 264         invmx[5] = ((lp1->x * lp3->y) - (lp1->y * lp3->x)) * -invdet;
 265         invmx[8] = ((lp1->x * lp2->y) - (lp1->y * lp2->x)) * invdet;
 266
 267         any_ls_inside = false;
 268         for (i = 0; i < n_speakers; i++) {
 269                 if (i != a && i!=b && i != c) {
 270                         this_inside = true;
 271                         for (j = 0; j < 3; j++) {
 272                                 tmp = _speakers[i].coords().x * invmx[0 + j*3];
 273                                 tmp += _speakers[i].coords().y * invmx[1 + j*3];
 274                                 tmp += _speakers[i].coords().z * invmx[2 + j*3];
 275                                 if (tmp < -0.001) {
 276                                         this_inside = false;
 277                                 }
 278                         }
 279                         if (this_inside) {
 280                                 any_ls_inside = true;
 281                         }
 282                 }
 283         }
 284
 285         return any_ls_inside;
 286 }
 287
 288
 289 void
 290 VBAPSpeakers::add_ldsp_triplet(int i, int j, int k, struct ls_triplet_chain **ls_triplets)
 291 {
 292         /* adds i,j,k triplet to triplet chain*/
 293
 294         struct ls_triplet_chain *trip_ptr, *prev;
 295         trip_ptr = *ls_triplets;
 296         prev = 0;
 297
 298         while (trip_ptr != 0){
 299                 prev = trip_ptr;
 300                 trip_ptr = trip_ptr->next;
 301         }
 302
 303         trip_ptr = (struct ls_triplet_chain*) malloc (sizeof (struct ls_triplet_chain));
 304
 305         if (prev == 0) {
 306                 *ls_triplets = trip_ptr;
 307         } else {
 308                 prev->next = trip_ptr;
 309         }
 310
 311         trip_ptr->next = 0;
 312         trip_ptr->ls_nos[0] = i;
 313         trip_ptr->ls_nos[1] = j;
 314         trip_ptr->ls_nos[2] = k;
 315 }
 316
 317 double
 318 VBAPSpeakers::vec_angle(CartesianVector v1, CartesianVector v2)
 319 {
 320         double inner= ((v1.x*v2.x + v1.y*v2.y + v1.z*v2.z)/
 321                       (vec_length(v1) * vec_length(v2)));
 322
 323         if (inner > 1.0) {
 324                 inner = 1.0;
 325         }
 326
 327         if (inner < -1.0) {
 328                 inner = -1.0;
 329         }
 330
 331         return fabs(acos(inner));
 332 }
 333
 334 double
 335 VBAPSpeakers::vec_length(CartesianVector v1)
 336 {
 337         double rv = sqrt(v1.x*v1.x + v1.y*v1.y + v1.z*v1.z);
 338         if (rv > 1e-14) return rv;
 339         return 0;
 340 }
 341
 342 double
 343 VBAPSpeakers::vec_prod(CartesianVector v1, CartesianVector v2)
 344 {
 345         return (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
 346 }
 347
 348 double
 349 VBAPSpeakers::vol_p_side_lgth(int i, int j, int k, const vector<Speaker>& speakers)
 350 {
 351         /* calculate volume of the parallelepiped defined by the loudspeaker
 352            direction vectors and divide it with total length of the triangle sides.
 353            This is used when removing too narrow triangles. */
 354
 355         double volper, lgth;
 356         CartesianVector xprod;
 357         cross_prod (speakers[i].coords(), speakers[j].coords(), &xprod);
 358         volper = fabs (vec_prod(xprod, speakers[k].coords()));
 359         lgth = (  fabs (vec_angle(speakers[i].coords(), speakers[j].coords()))
 360                 + fabs (vec_angle(speakers[i].coords(), speakers[k].coords()))
 361                 + fabs (vec_angle(speakers[j].coords(), speakers[k].coords())));
 362         if (lgth > 0.00001) {
 363                 return volper / lgth;
 364         } else {
 365                 return 0.0;
 366         }
 367 }
 368
 369 void
 370 VBAPSpeakers::cross_prod(CartesianVector v1,CartesianVector v2, CartesianVector *res)
 371 {
 372         double length;
 373
 374         res->x = (v1.y * v2.z) - (v1.z * v2.y);
 375         res->y = (v1.z * v2.x) - (v1.x * v2.z);
 376         res->z = (v1.x * v2.y) - (v1.y * v2.x);
 377
 378         length = vec_length(*res);
 379         if (length > 0) {
 380                 res->x /= length;
 381                 res->y /= length;
 382                 res->z /= length;
 383         } else {
 384                 res->x = 0;
 385                 res->y = 0;
 386                 res->z = 0;
 387         }
 388 }
 389
 390 int
 391 VBAPSpeakers::lines_intersect (int i, int j, int k, int l)
 392 {
 393         /* checks if two lines intersect on 3D sphere
 394            see theory in paper Pulkki, V. Lokki, T. "Creating Auditory Displays
 395            with Multiple Loudspeakers Using VBAP: A Case Study with
 396            DIVA Project" in International Conference on
 397            Auditory Displays -98. E-mail Ville.Pulkki@hut.fi
 398            if you want to have that paper.
 399         */
 400
 401         CartesianVector v1;
 402         CartesianVector v2;
 403         CartesianVector v3, neg_v3;
 404         float dist_ij,dist_kl,dist_iv3,dist_jv3,dist_inv3,dist_jnv3;
 405         float dist_kv3,dist_lv3,dist_knv3,dist_lnv3;
 406
 407         cross_prod(_speakers[i].coords(),_speakers[j].coords(),&v1);
 408         cross_prod(_speakers[k].coords(),_speakers[l].coords(),&v2);
 409         cross_prod(v1,v2,&v3);
 410
 411         neg_v3.x= 0.0 - v3.x;
 412         neg_v3.y= 0.0 - v3.y;
 413         neg_v3.z= 0.0 - v3.z;
 414
 415         dist_ij = (vec_angle(_speakers[i].coords(),_speakers[j].coords()));
 416         dist_kl = (vec_angle(_speakers[k].coords(),_speakers[l].coords()));
 417         dist_iv3 = (vec_angle(_speakers[i].coords(),v3));
 418         dist_jv3 = (vec_angle(v3,_speakers[j].coords()));
 419         dist_inv3 = (vec_angle(_speakers[i].coords(),neg_v3));
 420         dist_jnv3 = (vec_angle(neg_v3,_speakers[j].coords()));
 421         dist_kv3 = (vec_angle(_speakers[k].coords(),v3));
 422         dist_lv3 = (vec_angle(v3,_speakers[l].coords()));
 423         dist_knv3 = (vec_angle(_speakers[k].coords(),neg_v3));
 424         dist_lnv3 = (vec_angle(neg_v3,_speakers[l].coords()));
 425
 426         /* if one of loudspeakers is close to crossing point, don't do anything*/
 427         if(fabsf(dist_iv3) <= 0.01 || fabsf(dist_jv3) <= 0.01 ||
 428            fabsf(dist_kv3) <= 0.01 || fabsf(dist_lv3) <= 0.01 ||
 429            fabsf(dist_inv3) <= 0.01 || fabsf(dist_jnv3) <= 0.01 ||
 430            fabsf(dist_knv3) <= 0.01 || fabsf(dist_lnv3) <= 0.01 ) {
 431                 return(0);
 432         }
 433
 434         /* if crossing point is on line between both loudspeakers return 1 */
 435         if (((fabsf(dist_ij - (dist_iv3 + dist_jv3)) <= 0.01 ) &&
 436              (fabsf(dist_kl - (dist_kv3 + dist_lv3))  <= 0.01)) ||
 437             ((fabsf(dist_ij - (dist_inv3 + dist_jnv3)) <= 0.01)  &&
 438              (fabsf(dist_kl - (dist_knv3 + dist_lnv3)) <= 0.01 ))) {
 439                 return (1);
 440         } else {
 441                 return (0);
 442         }
 443 }
 444
 445 void
 446 VBAPSpeakers::calculate_3x3_matrixes(struct ls_triplet_chain *ls_triplets)
 447 {
 448         /* Calculates the inverse matrices for 3D */
 449         float invdet;
 450         const CartesianVector* lp1;
 451         const CartesianVector* lp2;
 452         const CartesianVector* lp3;
 453         float *invmx;
 454         struct ls_triplet_chain *tr_ptr = ls_triplets;
 455         int triplet_count = 0;
 456         int triplet;
 457
 458         assert (tr_ptr);
 459
 460         /* counting triplet amount */
 461
 462         while (tr_ptr != 0) {
 463                 triplet_count++;
 464                 tr_ptr = tr_ptr->next;
 465         }
 466
 467 #if 0 // DEVEL/DEBUG
 468         cerr << "@@@ VBAP triplets generate " << triplet_count << " of speaker tuples\n";
 469 #endif
 470
 471         triplet = 0;
 472
 473         _matrices.clear ();
 474         _speaker_tuples.clear ();
 475
 476         for (int n = 0; n < triplet_count; ++n) {
 477                 _matrices.push_back (threeDmatrix());
 478                 _speaker_tuples.push_back (tmatrix());
 479         }
 480
 481         tr_ptr = ls_triplets;
 482         while (tr_ptr != 0) {
 483                 lp1 =  &(_speakers[tr_ptr->ls_nos[0]].coords());
 484                 lp2 =  &(_speakers[tr_ptr->ls_nos[1]].coords());
 485                 lp3 =  &(_speakers[tr_ptr->ls_nos[2]].coords());
 486
 487                 /* matrix inversion */
 488                 invmx = tr_ptr->inv_mx;
 489                 invdet = 1.0 / (  lp1->x * ((lp2->y * lp3->z) - (lp2->z * lp3->y))
 490                                   - lp1->y * ((lp2->x * lp3->z) - (lp2->z * lp3->x))
 491                                   + lp1->z * ((lp2->x * lp3->y) - (lp2->y * lp3->x)));
 492
 493                 invmx[0] = ((lp2->y * lp3->z) - (lp2->z * lp3->y)) * invdet;
 494                 invmx[3] = ((lp1->y * lp3->z) - (lp1->z * lp3->y)) * -invdet;
 495                 invmx[6] = ((lp1->y * lp2->z) - (lp1->z * lp2->y)) * invdet;
 496                 invmx[1] = ((lp2->x * lp3->z) - (lp2->z * lp3->x)) * -invdet;
 497                 invmx[4] = ((lp1->x * lp3->z) - (lp1->z * lp3->x)) * invdet;
 498                 invmx[7] = ((lp1->x * lp2->z) - (lp1->z * lp2->x)) * -invdet;
 499                 invmx[2] = ((lp2->x * lp3->y) - (lp2->y * lp3->x)) * invdet;
 500                 invmx[5] = ((lp1->x * lp3->y) - (lp1->y * lp3->x)) * -invdet;
 501                 invmx[8] = ((lp1->x * lp2->y) - (lp1->y * lp2->x)) * invdet;
 502
 503                 /* copy the matrix */
 504
 505                 _matrices[triplet][0] = invmx[0];
 506                 _matrices[triplet][1] = invmx[1];
 507                 _matrices[triplet][2] = invmx[2];
 508                 _matrices[triplet][3] = invmx[3];
 509                 _matrices[triplet][4] = invmx[4];
 510                 _matrices[triplet][5] = invmx[5];
 511                 _matrices[triplet][6] = invmx[6];
 512                 _matrices[triplet][7] = invmx[7];
 513                 _matrices[triplet][8] = invmx[8];
 514
 515                 _speaker_tuples[triplet][0] = tr_ptr->ls_nos[0];
 516                 _speaker_tuples[triplet][1] = tr_ptr->ls_nos[1];
 517                 _speaker_tuples[triplet][2] = tr_ptr->ls_nos[2];
 518
 519 #if 0 // DEVEL/DEBUG
 520                 cerr << "Triplet[" << triplet << "] = "
 521                      << tr_ptr->ls_nos[0] << " + "
 522                      << tr_ptr->ls_nos[1] << " + "
 523                      << tr_ptr->ls_nos[2] << endl;
 524 #endif
 525
 526                 triplet++;
 527
 528                 tr_ptr = tr_ptr->next;
 529         }
 530 }
 531
 532 void
 533 VBAPSpeakers::choose_speaker_pairs (){
 534
 535         /* selects the loudspeaker pairs, calculates the inversion
 536            matrices and stores the data to a global array
 537         */
 538         const int n_speakers = _speakers.size();
 539
 540         if (n_speakers < 2) {
 541                 fprintf(stderr, "VBAP: at least 2 speakers need to be defined.");
 542                 return;
 543         }
 544
 545         const double AZIMUTH_DELTA_THRESHOLD_DEGREES = (180.0/M_PI) * (M_PI - 0.175);
 546         /* variable length arrays arrived in C99, became optional in C11, and
 547            are only planned for C++14. Use alloca which is functionally
 548            identical (but uglier to read).
 549         */
 550         int* sorted_speakers = (int*) alloca (sizeof (int) * n_speakers);
 551         bool* exists = (bool*) alloca (sizeof(bool) * n_speakers);
 552         double* inverse_matrix = (double*) alloca (sizeof (double) * n_speakers * 4);
 553         int expected_pairs = 0;
 554         int pair;
 555         int speaker;
 556
 557         for (speaker = 0; speaker < n_speakers; ++speaker) {
 558                 exists[speaker] = false;
 559         }
 560
 561         /* sort loudspeakers according their aximuth angle */
 562 #ifdef __clang_analyzer__
 563         // sort_2D_lss() assigns values to all of sorted_speakers
 564         // "uninitialized value"
 565         memset(sorted_speakers, 0, sizeof(*sorted_speakers));
 566 #endif
 567         sort_2D_lss (sorted_speakers);
 568
 569         /* adjacent loudspeakers are the loudspeaker pairs to be used.*/
 570         for (speaker = 0; speaker < n_speakers-1; speaker++) {
 571
 572                 if ((_speakers[sorted_speakers[speaker+1]].angles().azi -
 573                      _speakers[sorted_speakers[speaker]].angles().azi) <= AZIMUTH_DELTA_THRESHOLD_DEGREES) {
 574                         if (calc_2D_inv_tmatrix( _speakers[sorted_speakers[speaker]].angles().azi,
 575                                                  _speakers[sorted_speakers[speaker+1]].angles().azi,
 576                                                  &inverse_matrix[4 * speaker]) != 0){
 577                                 exists[speaker] = true;
 578                                 expected_pairs++;
 579                         }
 580                 }
 581         }
 582
 583         if (((6.283 - _speakers[sorted_speakers[n_speakers-1]].angles().azi)
 584              +_speakers[sorted_speakers[0]].angles().azi) <= AZIMUTH_DELTA_THRESHOLD_DEGREES) {
 585                 if (calc_2D_inv_tmatrix(_speakers[sorted_speakers[n_speakers-1]].angles().azi,
 586                                         _speakers[sorted_speakers[0]].angles().azi,
 587                                         &inverse_matrix[4*(n_speakers-1)]) != 0) {
 588                         exists[n_speakers-1] = true;
 589                         expected_pairs++;
 590                 }
 591         }
 592
 593         pair = 0;
 594
 595         _matrices.clear ();
 596         _speaker_tuples.clear ();
 597
 598         for (int n = 0; n < expected_pairs; ++n) {
 599                 _matrices.push_back (twoDmatrix());
 600                 _speaker_tuples.push_back (tmatrix());
 601         }
 602
 603         for (speaker = 0; speaker < n_speakers - 1; speaker++) {
 604                 if (exists[speaker]) {
 605                         _matrices[pair][0] = inverse_matrix[(speaker*4)+0];
 606                         _matrices[pair][1] = inverse_matrix[(speaker*4)+1];
 607                         _matrices[pair][2] = inverse_matrix[(speaker*4)+2];
 608                         _matrices[pair][3] = inverse_matrix[(speaker*4)+3];
 609
 610                         _speaker_tuples[pair][0] = sorted_speakers[speaker];
 611                         _speaker_tuples[pair][1] = sorted_speakers[speaker+1];
 612
 613                         pair++;
 614                 }
 615         }
 616
 617         if (exists[n_speakers-1]) {
 618                 _matrices[pair][0] = inverse_matrix[(speaker*4)+0];
 619                 _matrices[pair][1] = inverse_matrix[(speaker*4)+1];
 620                 _matrices[pair][2] = inverse_matrix[(speaker*4)+2];
 621                 _matrices[pair][3] = inverse_matrix[(speaker*4)+3];
 622
 623                 _speaker_tuples[pair][0] = sorted_speakers[n_speakers-1];
 624                 _speaker_tuples[pair][1] = sorted_speakers[0];
 625         }
 626 }
 627
 628 void
 629 VBAPSpeakers::sort_2D_lss (int* sorted_speakers)
 630 {
 631         vector<Speaker> tmp = _speakers;
 632         vector<Speaker>::iterator s;
 633         azimuth_sorter sorter;
 634         unsigned int n;
 635
 636         sort (tmp.begin(), tmp.end(), sorter);
 637
 638         for (n = 0, s = tmp.begin(); s != tmp.end(); ++s, ++n) {
 639                 sorted_speakers[n] = (*s).id;
 640         }
 641         assert(n == _speakers.size ());
 642 }
 643
 644 int
 645 VBAPSpeakers::calc_2D_inv_tmatrix (double azi1, double azi2, double* inverse_matrix)
 646 {
 647         double x1,x2,x3,x4;
 648         double det;
 649
 650         x1 = cos (azi1 * (M_PI/180.0));
 651         x2 = sin (azi1 * (M_PI/180.0));
 652         x3 = cos (azi2 * (M_PI/180.0));
 653         x4 = sin (azi2 * (M_PI/180.0));
 654         det = (x1 * x4) - ( x3 * x2 );
 655
 656         if (fabs(det) <= 0.001) {
 657
 658                 inverse_matrix[0] = 0.0;
 659                 inverse_matrix[1] = 0.0;
 660                 inverse_matrix[2] = 0.0;
 661                 inverse_matrix[3] = 0.0;
 662
 663                 return 0;
 664
 665         } else {
 666
 667                 inverse_matrix[0] = x4 / det;
 668                 inverse_matrix[1] = -x3 / det;
 669                 inverse_matrix[2] = -x2 / det;
 670                 inverse_matrix[3] = x1 / det;
 671
 672                 return 1;
 673         }
 674 }
 675
 676