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