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