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         int connections[n_speakers][n_speakers];
 119         float distance_table[((n_speakers * (n_speakers - 1)) / 2)];
 120         int distance_table_i[((n_speakers * (n_speakers - 1)) / 2)];
 121         int distance_table_j[((n_speakers * (n_speakers - 1)) / 2)];
 122         float distance;
 123         struct ls_triplet_chain *trip_ptr, *prev, *tmp_ptr;
 124
 125         if (n_speakers == 0) {
 126                 return;
 127         }
 128
 129         for (i = 0; i < n_speakers; i++) {
 130                 for (j = i+1; j < n_speakers; j++) {
 131                         for(k = j+1; k < n_speakers; k++) {
 132                                 if (vol_p_side_lgth(i, j, k, _speakers) > MIN_VOL_P_SIDE_LGTH) {
 133                                         connections[i][j]=1;
 134                                         connections[j][i]=1;
 135                                         connections[i][k]=1;
 136                                         connections[k][i]=1;
 137                                         connections[j][k]=1;
 138                                         connections[k][j]=1;
 139                                         add_ldsp_triplet(i,j,k,ls_triplets);
 140                                 }
 141                         }
 142                 }
 143         }
 144
 145         /*calculate distancies between all speakers and sorting them*/
 146         table_size =(((n_speakers - 1) * (n_speakers)) / 2);
 147         for (i = 0; i < table_size; i++) {
 148                 distance_table[i] = 100000.0;
 149         }
 150
 151         for (i = 0;i < n_speakers; i++) {
 152                 for (j = i+1; j < n_speakers; j++) {
 153                         if (connections[i][j] == 1) {
 154                                 distance = fabs(vec_angle(_speakers[i].coords(),_speakers[j].coords()));
 155                                 k=0;
 156                                 while(distance_table[k] < distance) {
 157                                         k++;
 158                                 }
 159                                 for (l = table_size - 1; l > k ; l--) {
 160                                         distance_table[l] = distance_table[l-1];
 161                                         distance_table_i[l] = distance_table_i[l-1];
 162                                         distance_table_j[l] = distance_table_j[l-1];
 163                                 }
 164                                 distance_table[k] = distance;
 165                                 distance_table_i[k] = i;
 166                                 distance_table_j[k] = j;
 167                         } else
 168                                 table_size--;
 169                 }
 170         }
 171
 172         /* disconnecting connections which are crossing shorter ones,
 173            starting from shortest one and removing all that cross it,
 174            and proceeding to next shortest */
 175         for (i = 0; i < table_size; i++) {
 176                 int fst_ls = distance_table_i[i];
 177                 int sec_ls = distance_table_j[i];
 178                 if (connections[fst_ls][sec_ls] == 1) {
 179                         for (j = 0; j < n_speakers; j++) {
 180                                 for (k = j+1; k < n_speakers; k++) {
 181                                         if ((j != fst_ls) && (k != sec_ls) && (k != fst_ls) && (j != sec_ls)) {
 182                                                 if (lines_intersect(fst_ls, sec_ls, j, k) == 1){
 183                                                         connections[j][k] = 0;
 184                                                         connections[k][j] = 0;
 185                                                 }
 186                                         }
 187                                 }
 188                         }
 189                 }
 190         }
 191
 192         /* remove triangles which had crossing sides
 193            with smaller triangles or include loudspeakers*/
 194         trip_ptr = *ls_triplets;
 195         prev = 0;
 196         while (trip_ptr != 0){
 197                 i = trip_ptr->ls_nos[0];
 198                 j = trip_ptr->ls_nos[1];
 199                 k = trip_ptr->ls_nos[2];
 200                 if (connections[i][j] == 0 ||
 201                     connections[i][k] == 0 ||
 202                     connections[j][k] == 0 ||
 203                     any_ls_inside_triplet(i,j,k) == 1 ){
 204                         if (prev != 0) {
 205                                 prev->next = trip_ptr->next;
 206                                 tmp_ptr = trip_ptr;
 207                                 trip_ptr = trip_ptr->next;
 208                                 free(tmp_ptr);
 209                         } else {
 210                                 *ls_triplets = trip_ptr->next;
 211                                 tmp_ptr = trip_ptr;
 212                                 trip_ptr = trip_ptr->next;
 213                                 free(tmp_ptr);
 214                         }
 215                 } else {
 216                         prev = trip_ptr;
 217                         trip_ptr = trip_ptr->next;
 218
 219                 }
 220         }
 221 }
 222
 223 int
 224 VBAPSpeakers::any_ls_inside_triplet(int a, int b, int c)
 225 {
 226         /* returns 1 if there is loudspeaker(s) inside given ls triplet */
 227         float invdet;
 228         const CartesianVector* lp1;
 229         const CartesianVector* lp2;
 230         const CartesianVector* lp3;
 231         float invmx[9];
 232         int i,j;
 233         float tmp;
 234         bool any_ls_inside;
 235         bool this_inside;
 236         int n_speakers = _speakers.size();
 237
 238         lp1 =  &(_speakers[a].coords());
 239         lp2 =  &(_speakers[b].coords());
 240         lp3 =  &(_speakers[c].coords());
 241
 242         /* matrix inversion */
 243         invdet = 1.0 / (  lp1->x * ((lp2->y * lp3->z) - (lp2->z * lp3->y))
 244                           - lp1->y * ((lp2->x * lp3->z) - (lp2->z * lp3->x))
 245                           + lp1->z * ((lp2->x * lp3->y) - (lp2->y * lp3->x)));
 246
 247         invmx[0] = ((lp2->y * lp3->z) - (lp2->z * lp3->y)) * invdet;
 248         invmx[3] = ((lp1->y * lp3->z) - (lp1->z * lp3->y)) * -invdet;
 249         invmx[6] = ((lp1->y * lp2->z) - (lp1->z * lp2->y)) * invdet;
 250         invmx[1] = ((lp2->x * lp3->z) - (lp2->z * lp3->x)) * -invdet;
 251         invmx[4] = ((lp1->x * lp3->z) - (lp1->z * lp3->x)) * invdet;
 252         invmx[7] = ((lp1->x * lp2->z) - (lp1->z * lp2->x)) * -invdet;
 253         invmx[2] = ((lp2->x * lp3->y) - (lp2->y * lp3->x)) * invdet;
 254         invmx[5] = ((lp1->x * lp3->y) - (lp1->y * lp3->x)) * -invdet;
 255         invmx[8] = ((lp1->x * lp2->y) - (lp1->y * lp2->x)) * invdet;
 256
 257         any_ls_inside = false;
 258         for (i = 0; i < n_speakers; i++) {
 259                 if (i != a && i!=b && i != c) {
 260                         this_inside = true;
 261                         for (j = 0; j < 3; j++) {
 262                                 tmp = _speakers[i].coords().x * invmx[0 + j*3];
 263                                 tmp += _speakers[i].coords().y * invmx[1 + j*3];
 264                                 tmp += _speakers[i].coords().z * invmx[2 + j*3];
 265                                 if (tmp < -0.001) {
 266                                         this_inside = false;
 267                                 }
 268                         }
 269                         if (this_inside) {
 270                                 any_ls_inside = true;
 271                         }
 272                 }
 273         }
 274
 275         return any_ls_inside;
 276 }
 277
 278
 279 void
 280 VBAPSpeakers::add_ldsp_triplet(int i, int j, int k, struct ls_triplet_chain **ls_triplets)
 281 {
 282         /* adds i,j,k triplet to triplet chain*/
 283
 284         struct ls_triplet_chain *trip_ptr, *prev;
 285         trip_ptr = *ls_triplets;
 286         prev = 0;
 287
 288         while (trip_ptr != 0){
 289                 prev = trip_ptr;
 290                 trip_ptr = trip_ptr->next;
 291         }
 292
 293         trip_ptr = (struct ls_triplet_chain*) malloc (sizeof (struct ls_triplet_chain));
 294
 295         if (prev == 0) {
 296                 *ls_triplets = trip_ptr;
 297         } else {
 298                 prev->next = trip_ptr;
 299         }
 300
 301         trip_ptr->next = 0;
 302         trip_ptr->ls_nos[0] = i;
 303         trip_ptr->ls_nos[1] = j;
 304         trip_ptr->ls_nos[2] = k;
 305 }
 306
 307 double
 308 VBAPSpeakers::vec_angle(CartesianVector v1, CartesianVector v2)
 309 {
 310         double inner= ((v1.x*v2.x + v1.y*v2.y + v1.z*v2.z)/
 311                       (vec_length(v1) * vec_length(v2)));
 312
 313         if (inner > 1.0) {
 314                 inner = 1.0;
 315         }
 316
 317         if (inner < -1.0) {
 318                 inner = -1.0;
 319         }
 320
 321         return fabs(acos(inner));
 322 }
 323
 324 double
 325 VBAPSpeakers::vec_length(CartesianVector v1)
 326 {
 327         double rv = sqrt(v1.x*v1.x + v1.y*v1.y + v1.z*v1.z);
 328         if (rv > 1e-14) return rv;
 329         return 0;
 330 }
 331
 332 double
 333 VBAPSpeakers::vec_prod(CartesianVector v1, CartesianVector v2)
 334 {
 335         return (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
 336 }
 337
 338 double
 339 VBAPSpeakers::vol_p_side_lgth(int i, int j, int k, const vector<Speaker>& speakers)
 340 {
 341         /* calculate volume of the parallelepiped defined by the loudspeaker
 342            direction vectors and divide it with total length of the triangle sides.
 343            This is used when removing too narrow triangles. */
 344
 345         double volper, lgth;
 346         CartesianVector xprod;
 347         cross_prod (speakers[i].coords(), speakers[j].coords(), &xprod);
 348         volper = fabs (vec_prod(xprod, speakers[k].coords()));
 349         lgth = (  fabs (vec_angle(speakers[i].coords(), speakers[j].coords()))
 350                 + fabs (vec_angle(speakers[i].coords(), speakers[k].coords()))
 351                 + fabs (vec_angle(speakers[j].coords(), speakers[k].coords())));
 352         if (lgth > 0.00001) {
 353                 return volper / lgth;
 354         } else {
 355                 return 0.0;
 356         }
 357 }
 358
 359 void
 360 VBAPSpeakers::cross_prod(CartesianVector v1,CartesianVector v2, CartesianVector *res)
 361 {
 362         double length;
 363
 364         res->x = (v1.y * v2.z) - (v1.z * v2.y);
 365         res->y = (v1.z * v2.x) - (v1.x * v2.z);
 366         res->z = (v1.x * v2.y) - (v1.y * v2.x);
 367
 368         length = vec_length(*res);
 369         if (length > 0) {
 370                 res->x /= length;
 371                 res->y /= length;
 372                 res->z /= length;
 373         } else {
 374                 res->x = 0;
 375                 res->y = 0;
 376                 res->z = 0;
 377         }
 378 }
 379
 380 int
 381 VBAPSpeakers::lines_intersect (int i, int j, int k, int l)
 382 {
 383         /* checks if two lines intersect on 3D sphere
 384            see theory in paper Pulkki, V. Lokki, T. "Creating Auditory Displays
 385            with Multiple Loudspeakers Using VBAP: A Case Study with
 386            DIVA Project" in International Conference on
 387            Auditory Displays -98. E-mail Ville.Pulkki@hut.fi
 388            if you want to have that paper.
 389         */
 390
 391         CartesianVector v1;
 392         CartesianVector v2;
 393         CartesianVector v3, neg_v3;
 394         float dist_ij,dist_kl,dist_iv3,dist_jv3,dist_inv3,dist_jnv3;
 395         float dist_kv3,dist_lv3,dist_knv3,dist_lnv3;
 396
 397         cross_prod(_speakers[i].coords(),_speakers[j].coords(),&v1);
 398         cross_prod(_speakers[k].coords(),_speakers[l].coords(),&v2);
 399         cross_prod(v1,v2,&v3);
 400
 401         neg_v3.x= 0.0 - v3.x;
 402         neg_v3.y= 0.0 - v3.y;
 403         neg_v3.z= 0.0 - v3.z;
 404
 405         dist_ij = (vec_angle(_speakers[i].coords(),_speakers[j].coords()));
 406         dist_kl = (vec_angle(_speakers[k].coords(),_speakers[l].coords()));
 407         dist_iv3 = (vec_angle(_speakers[i].coords(),v3));
 408         dist_jv3 = (vec_angle(v3,_speakers[j].coords()));
 409         dist_inv3 = (vec_angle(_speakers[i].coords(),neg_v3));
 410         dist_jnv3 = (vec_angle(neg_v3,_speakers[j].coords()));
 411         dist_kv3 = (vec_angle(_speakers[k].coords(),v3));
 412         dist_lv3 = (vec_angle(v3,_speakers[l].coords()));
 413         dist_knv3 = (vec_angle(_speakers[k].coords(),neg_v3));
 414         dist_lnv3 = (vec_angle(neg_v3,_speakers[l].coords()));
 415
 416         /* if one of loudspeakers is close to crossing point, don't do anything*/
 417         if(fabsf(dist_iv3) <= 0.01 || fabsf(dist_jv3) <= 0.01 ||
 418            fabsf(dist_kv3) <= 0.01 || fabsf(dist_lv3) <= 0.01 ||
 419            fabsf(dist_inv3) <= 0.01 || fabsf(dist_jnv3) <= 0.01 ||
 420            fabsf(dist_knv3) <= 0.01 || fabsf(dist_lnv3) <= 0.01 ) {
 421                 return(0);
 422         }
 423
 424         /* if crossing point is on line between both loudspeakers return 1 */
 425         if (((fabsf(dist_ij - (dist_iv3 + dist_jv3)) <= 0.01 ) &&
 426              (fabsf(dist_kl - (dist_kv3 + dist_lv3))  <= 0.01)) ||
 427             ((fabsf(dist_ij - (dist_inv3 + dist_jnv3)) <= 0.01)  &&
 428              (fabsf(dist_kl - (dist_knv3 + dist_lnv3)) <= 0.01 ))) {
 429                 return (1);
 430         } else {
 431                 return (0);
 432         }
 433 }
 434
 435 void
 436 VBAPSpeakers::calculate_3x3_matrixes(struct ls_triplet_chain *ls_triplets)
 437 {
 438         /* Calculates the inverse matrices for 3D */
 439         float invdet;
 440         const CartesianVector* lp1;
 441         const CartesianVector* lp2;
 442         const CartesianVector* lp3;
 443         float *invmx;
 444         struct ls_triplet_chain *tr_ptr = ls_triplets;
 445         int triplet_count = 0;
 446         int triplet;
 447
 448         assert (tr_ptr);
 449
 450         /* counting triplet amount */
 451
 452         while (tr_ptr != 0) {
 453                 triplet_count++;
 454                 tr_ptr = tr_ptr->next;
 455         }
 456
 457 #if 0 // DEVEL/DEBUG
 458         cerr << "@@@ VBAP triplets generate " << triplet_count << " of speaker tuples\n";
 459 #endif
 460
 461         triplet = 0;
 462
 463         _matrices.clear ();
 464         _speaker_tuples.clear ();
 465
 466         for (int n = 0; n < triplet_count; ++n) {
 467                 _matrices.push_back (threeDmatrix());
 468                 _speaker_tuples.push_back (tmatrix());
 469         }
 470
 471         tr_ptr = ls_triplets;
 472         while (tr_ptr != 0) {
 473                 lp1 =  &(_speakers[tr_ptr->ls_nos[0]].coords());
 474                 lp2 =  &(_speakers[tr_ptr->ls_nos[1]].coords());
 475                 lp3 =  &(_speakers[tr_ptr->ls_nos[2]].coords());
 476
 477                 /* matrix inversion */
 478                 invmx = tr_ptr->inv_mx;
 479                 invdet = 1.0 / (  lp1->x * ((lp2->y * lp3->z) - (lp2->z * lp3->y))
 480                                   - lp1->y * ((lp2->x * lp3->z) - (lp2->z * lp3->x))
 481                                   + lp1->z * ((lp2->x * lp3->y) - (lp2->y * lp3->x)));
 482
 483                 invmx[0] = ((lp2->y * lp3->z) - (lp2->z * lp3->y)) * invdet;
 484                 invmx[3] = ((lp1->y * lp3->z) - (lp1->z * lp3->y)) * -invdet;
 485                 invmx[6] = ((lp1->y * lp2->z) - (lp1->z * lp2->y)) * invdet;
 486                 invmx[1] = ((lp2->x * lp3->z) - (lp2->z * lp3->x)) * -invdet;
 487                 invmx[4] = ((lp1->x * lp3->z) - (lp1->z * lp3->x)) * invdet;
 488                 invmx[7] = ((lp1->x * lp2->z) - (lp1->z * lp2->x)) * -invdet;
 489                 invmx[2] = ((lp2->x * lp3->y) - (lp2->y * lp3->x)) * invdet;
 490                 invmx[5] = ((lp1->x * lp3->y) - (lp1->y * lp3->x)) * -invdet;
 491                 invmx[8] = ((lp1->x * lp2->y) - (lp1->y * lp2->x)) * invdet;
 492
 493                 /* copy the matrix */
 494
 495                 _matrices[triplet][0] = invmx[0];
 496                 _matrices[triplet][1] = invmx[1];
 497                 _matrices[triplet][2] = invmx[2];
 498                 _matrices[triplet][3] = invmx[3];
 499                 _matrices[triplet][4] = invmx[4];
 500                 _matrices[triplet][5] = invmx[5];
 501                 _matrices[triplet][6] = invmx[6];
 502                 _matrices[triplet][7] = invmx[7];
 503                 _matrices[triplet][8] = invmx[8];
 504
 505                 _speaker_tuples[triplet][0] = tr_ptr->ls_nos[0];
 506                 _speaker_tuples[triplet][1] = tr_ptr->ls_nos[1];
 507                 _speaker_tuples[triplet][2] = tr_ptr->ls_nos[2];
 508
 509 #if 0 // DEVEL/DEBUG
 510                 cerr << "Triplet[" << triplet << "] = "
 511                      << tr_ptr->ls_nos[0] << " + "
 512                      << tr_ptr->ls_nos[1] << " + "
 513                      << tr_ptr->ls_nos[2] << endl;
 514 #endif
 515
 516                 triplet++;
 517
 518                 tr_ptr = tr_ptr->next;
 519         }
 520 }
 521
 522 void
 523 VBAPSpeakers::choose_speaker_pairs (){
 524
 525         /* selects the loudspeaker pairs, calculates the inversion
 526            matrices and stores the data to a global array
 527         */
 528         const int n_speakers = _speakers.size();
 529         const double AZIMUTH_DELTA_THRESHOLD_DEGREES = (180.0/M_PI) * (M_PI - 0.175);
 530         int sorted_speakers[n_speakers];
 531         bool exists[n_speakers];
 532         double inverse_matrix[n_speakers][4];
 533         int expected_pairs = 0;
 534         int pair;
 535         int speaker;
 536
 537
 538         if (n_speakers == 0) {
 539                 return;
 540         }
 541
 542         for (speaker = 0; speaker < n_speakers; ++speaker) {
 543                 exists[speaker] = false;
 544         }
 545
 546         /* sort loudspeakers according their aximuth angle */
 547         sort_2D_lss (sorted_speakers);
 548
 549         /* adjacent loudspeakers are the loudspeaker pairs to be used.*/
 550         for (speaker = 0; speaker < n_speakers-1; speaker++) {
 551
 552                 if ((_speakers[sorted_speakers[speaker+1]].angles().azi -
 553                      _speakers[sorted_speakers[speaker]].angles().azi) <= AZIMUTH_DELTA_THRESHOLD_DEGREES) {
 554                         if (calc_2D_inv_tmatrix( _speakers[sorted_speakers[speaker]].angles().azi,
 555                                                  _speakers[sorted_speakers[speaker+1]].angles().azi,
 556                                                  inverse_matrix[speaker]) != 0){
 557                                 exists[speaker] = true;
 558                                 expected_pairs++;
 559                         }
 560                 }
 561         }
 562
 563         if (((6.283 - _speakers[sorted_speakers[n_speakers-1]].angles().azi)
 564              +_speakers[sorted_speakers[0]].angles().azi) <= AZIMUTH_DELTA_THRESHOLD_DEGREES) {
 565                 if (calc_2D_inv_tmatrix(_speakers[sorted_speakers[n_speakers-1]].angles().azi,
 566                                         _speakers[sorted_speakers[0]].angles().azi,
 567                                         inverse_matrix[n_speakers-1]) != 0) {
 568                         exists[n_speakers-1] = true;
 569                         expected_pairs++;
 570                 }
 571         }
 572
 573         pair = 0;
 574
 575         _matrices.clear ();
 576         _speaker_tuples.clear ();
 577
 578         for (int n = 0; n < expected_pairs; ++n) {
 579                 _matrices.push_back (twoDmatrix());
 580                 _speaker_tuples.push_back (tmatrix());
 581         }
 582
 583         for (speaker = 0; speaker < n_speakers - 1; speaker++) {
 584                 if (exists[speaker]) {
 585                         _matrices[pair][0] = inverse_matrix[speaker][0];
 586                         _matrices[pair][1] = inverse_matrix[speaker][1];
 587                         _matrices[pair][2] = inverse_matrix[speaker][2];
 588                         _matrices[pair][3] = inverse_matrix[speaker][3];
 589
 590                         _speaker_tuples[pair][0] = sorted_speakers[speaker];
 591                         _speaker_tuples[pair][1] = sorted_speakers[speaker+1];
 592
 593                         pair++;
 594                 }
 595         }
 596
 597         if (exists[n_speakers-1]) {
 598                 _matrices[pair][0] = inverse_matrix[speaker][0];
 599                 _matrices[pair][1] = inverse_matrix[speaker][1];
 600                 _matrices[pair][2] = inverse_matrix[speaker][2];
 601                 _matrices[pair][3] = inverse_matrix[speaker][3];
 602
 603                 _speaker_tuples[pair][0] = sorted_speakers[n_speakers-1];
 604                 _speaker_tuples[pair][1] = sorted_speakers[0];
 605         }
 606 }
 607
 608 void
 609 VBAPSpeakers::sort_2D_lss (int* sorted_speakers)
 610 {
 611         vector<Speaker> tmp = _speakers;
 612         vector<Speaker>::iterator s;
 613         azimuth_sorter sorter;
 614         int n;
 615
 616         sort (tmp.begin(), tmp.end(), sorter);
 617
 618         for (n = 0, s = tmp.begin(); s != tmp.end(); ++s, ++n) {
 619                 sorted_speakers[n] = (*s).id;
 620         }
 621 }
 622
 623 int
 624 VBAPSpeakers::calc_2D_inv_tmatrix (double azi1, double azi2, double* inverse_matrix)
 625 {
 626         double x1,x2,x3,x4;
 627         double det;
 628
 629         x1 = cos (azi1 * (M_PI/180.0));
 630         x2 = sin (azi1 * (M_PI/180.0));
 631         x3 = cos (azi2 * (M_PI/180.0));
 632         x4 = sin (azi2 * (M_PI/180.0));
 633         det = (x1 * x4) - ( x3 * x2 );
 634
 635         if (fabs(det) <= 0.001) {
 636
 637                 inverse_matrix[0] = 0.0;
 638                 inverse_matrix[1] = 0.0;
 639                 inverse_matrix[2] = 0.0;
 640                 inverse_matrix[3] = 0.0;
 641
 642                 return 0;
 643
 644         } else {
 645
 646                 inverse_matrix[0] = x4 / det;
 647                 inverse_matrix[1] = -x3 / det;
 648                 inverse_matrix[2] = -x2 / det;
 649                 inverse_matrix[3] = x1 / det;
 650
 651                 return 1;
 652         }
 653 }
 654
 655