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