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