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