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