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