Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : #include <stdint.h>
34 : #include "options.h"
35 : #include "prot.h"
36 : #include "ivas_prot.h"
37 : #include "ivas_stat_dec.h"
38 : #include "ivas_cnst.h"
39 : #include <math.h>
40 : #ifdef DEBUGGING
41 : #include "debug.h"
42 : #endif
43 : #include "wmc_auto.h"
44 :
45 :
46 : /*-----------------------------------------------------------------------*
47 : * Local constants
48 : *-----------------------------------------------------------------------*/
49 :
50 : /* The SVD is sensitive to changes to the following constants, so please be careful when trying to tune things */
51 : #define SVD_MINIMUM_VALUE 1e-32f /* minimum value */
52 : #define CONVERGENCE_FACTOR 1.0e-04f /* factor for SVD convergence */
53 : #define SVD_MAX_NUM_ITERATION 75 /* maximum number of interations before exiting the SVD */
54 : #define SVD_ZERO_FLUSH_THRESHOLD 1.0e-20f
55 :
56 :
57 : /*-----------------------------------------------------------------------*
58 : * Local function prototypes
59 : *-----------------------------------------------------------------------*/
60 :
61 : static float GivensRotation( const float x, const float z );
62 :
63 : static void biDiagonalReductionLeft( float singularVectors[][MAX_OUTPUT_CHANNELS], const int16_t nChannelsL, const int16_t nChannelsC, const int16_t currChannel, float *g );
64 :
65 : static void biDiagonalReductionRight( float singularVectors[][MAX_OUTPUT_CHANNELS], const int16_t nChannelsL, const int16_t nChannelsC, const int16_t currChannel, float *g );
66 :
67 : static void singularVectorsAccumulationLeft( float singularVectors_Left[][MAX_OUTPUT_CHANNELS], float singularValues[MAX_OUTPUT_CHANNELS], const int16_t nChannelsL, const int16_t nChannelsC );
68 :
69 : static void singularVectorsAccumulationRight( float singularVectors_Left[][MAX_OUTPUT_CHANNELS], float singularVectors_Right[][MAX_OUTPUT_CHANNELS], float secDiag[MAX_OUTPUT_CHANNELS], const int16_t nChannelsC );
70 :
71 : static void HouseholderReduction( float singularVectors_Left[][MAX_OUTPUT_CHANNELS], float singularValues[MAX_OUTPUT_CHANNELS], float singularVectors_Right[][MAX_OUTPUT_CHANNELS], float secDiag[MAX_OUTPUT_CHANNELS], const int16_t nChannelsL, const int16_t nChannelsC, float *eps_x );
72 :
73 : static int16_t BidagonalDiagonalisation( float singularVectors_Left[][MAX_OUTPUT_CHANNELS], float singularValues[MAX_OUTPUT_CHANNELS], float singularVectors_Right[][MAX_OUTPUT_CHANNELS], float secDiag[MAX_OUTPUT_CHANNELS], const int16_t nChannelsL, const int16_t nChannelsC, const float eps_x );
74 :
75 : static void ApplyQRTransform( float singularVectors_Left[][MAX_OUTPUT_CHANNELS], float singularValues[MAX_OUTPUT_CHANNELS], float singularVectors_Right[][MAX_OUTPUT_CHANNELS], float secDiag[MAX_OUTPUT_CHANNELS], const int16_t startIndex, const int16_t currentIndex, const int16_t nChannelsL, const int16_t nChannelsC );
76 :
77 : static void ApplyRotation( float singularVector[][MAX_OUTPUT_CHANNELS], const float c, const float s, float x11, float x12, float *f, float *g, const int16_t currentIndex1, const int16_t currentIndex2, const int16_t nChannels );
78 :
79 : static float maxWithSign( const float a );
80 :
81 : static void flushToZeroArray( float arr[MAX_OUTPUT_CHANNELS], const int16_t length );
82 :
83 : static void flushToZeroMat( float mat[][MAX_OUTPUT_CHANNELS], const int16_t m, const int16_t n );
84 :
85 :
86 : /*-------------------------------------------------------------------------
87 : * mat2svdMat()
88 : *
89 : * external matrix format to internal
90 : *-------------------------------------------------------------------------*/
91 :
92 3053268 : void mat2svdMat(
93 : const float *mat, /* i : matrix as column ordered vector */
94 : float svdMat[MAX_OUTPUT_CHANNELS][MAX_OUTPUT_CHANNELS], /* o : matrix as two-dimensional arry */
95 : const int16_t nRows, /* i : number of rows of the matrix */
96 : const int16_t mCols, /* i : number of columns of the matrix */
97 : const int16_t transpose /* i : flag indication transposition */
98 : )
99 : {
100 : int16_t i, j;
101 :
102 3053268 : if ( transpose )
103 : {
104 2492223 : for ( i = 0; i < mCols; i++ )
105 : {
106 6287691 : for ( j = 0; j < nRows; j++ )
107 : {
108 4204764 : svdMat[i][j] = mat[j + nRows * i];
109 : }
110 :
111 2082927 : set_zero( &svdMat[i][mCols], MAX_OUTPUT_CHANNELS - nRows );
112 : }
113 :
114 4875105 : for ( ; i < MAX_OUTPUT_CHANNELS; i++ )
115 : {
116 4465809 : set_zero( svdMat[i], MAX_OUTPUT_CHANNELS );
117 : }
118 : }
119 : else
120 : {
121 11180925 : for ( i = 0; i < nRows; i++ )
122 : {
123 42640728 : for ( j = 0; j < mCols; j++ )
124 : {
125 34103775 : svdMat[i][j] = mat[i + nRows * j];
126 : }
127 :
128 8536953 : set_zero( &svdMat[i][mCols], MAX_OUTPUT_CHANNELS - mCols );
129 : }
130 :
131 36410571 : for ( ; i < MAX_OUTPUT_CHANNELS; i++ )
132 : {
133 33766599 : set_zero( svdMat[i], MAX_OUTPUT_CHANNELS );
134 : }
135 : }
136 :
137 3053268 : return;
138 : }
139 :
140 :
141 : /*---------------------------------------------------------------------*
142 : * svdMat2mat()
143 : *
144 : * transfer a matrix from a two dimensional array to a column wise ordered vector
145 : *---------------------------------------------------------------------*/
146 :
147 3871212 : void svdMat2mat(
148 : float svdMat[MAX_OUTPUT_CHANNELS][MAX_OUTPUT_CHANNELS], /* i : matrix as two-dimensional arry */
149 : float *mat, /* o : matrix as column ordered vector */
150 : const int16_t nRows, /* i : number of rows of the matrix */
151 : const int16_t mCols /* i : number of columns of the matrix */
152 : )
153 : {
154 : int16_t i, j;
155 :
156 14884245 : for ( i = 0; i < nRows; i++ )
157 : {
158 :
159 :
160 43594035 : for ( j = 0; j < mCols; j++ )
161 : {
162 32581002 : mat[i + nRows * j] = svdMat[i][j];
163 : }
164 : }
165 :
166 3871212 : return;
167 : }
168 :
169 :
170 : /*-------------------------------------------------------------------------
171 : * svd()
172 : *
173 : * perform a singular value decomposition X=USV of a matrix X
174 : *-------------------------------------------------------------------------*/
175 :
176 : /*! r: error or success */
177 3053268 : int16_t svd(
178 : float InputMatrix[][MAX_OUTPUT_CHANNELS], /* i : matrix to be decomposed (M) */
179 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS], /* o : left singular vectors (U) */
180 : float singularValues[MAX_OUTPUT_CHANNELS], /* o : singular values vector (S) */
181 : float singularVectors_Right[][MAX_OUTPUT_CHANNELS], /* o : right singular vectors (V) */
182 : const int16_t nChannelsL, /* i : number of rows in the matrix to be decomposed */
183 : const int16_t nChannelsC /* i : number of columns in the matrix to be decomposed */
184 : )
185 : {
186 : int16_t iCh, jCh;
187 : int16_t lengthSingularValues;
188 : int16_t errorMessage, condition;
189 3053268 : int16_t max_length = ( ( nChannelsL > nChannelsC ) ? nChannelsL : nChannelsC );
190 : float secDiag[MAX_OUTPUT_CHANNELS];
191 3053268 : float eps_x = 0.0f, temp;
192 :
193 3053268 : push_wmops( "svd" );
194 :
195 3053268 : set_zero( secDiag, MAX_OUTPUT_CHANNELS );
196 :
197 : /* Collecting Values */
198 13673148 : for ( iCh = 0; iCh < nChannelsL; iCh++ )
199 : {
200 48928419 : for ( jCh = 0; jCh < nChannelsC; jCh++ )
201 : {
202 38308539 : singularVectors_Left[iCh][jCh] = InputMatrix[iCh][jCh];
203 : }
204 : }
205 :
206 : /* Householder reduction */
207 3053268 : HouseholderReduction( singularVectors_Left, singularValues, singularVectors_Right, secDiag, nChannelsL, nChannelsC, &eps_x );
208 :
209 : /* Set extremely small values to zero if needed */
210 3053268 : flushToZeroArray( singularValues, max_length );
211 3053268 : flushToZeroMat( singularVectors_Left, nChannelsL, nChannelsL );
212 3053268 : flushToZeroMat( singularVectors_Right, nChannelsC, nChannelsC );
213 :
214 : /* BidagonalDiagonalisation */
215 3053268 : errorMessage = BidagonalDiagonalisation( singularVectors_Left, singularValues, singularVectors_Right, secDiag, nChannelsL, nChannelsC, eps_x );
216 :
217 : /* Sort the singular values descending order */
218 3053268 : lengthSingularValues = min( nChannelsL, nChannelsC );
219 :
220 : do
221 : {
222 4157757 : condition = 0;
223 14083212 : for ( iCh = 0; iCh < lengthSingularValues - 1; iCh++ )
224 : {
225 9925455 : if ( singularValues[iCh] < singularValues[iCh + 1] )
226 : {
227 1387023 : condition = 1;
228 1387023 : temp = singularValues[iCh];
229 1387023 : singularValues[iCh] = singularValues[iCh + 1];
230 1387023 : singularValues[iCh + 1] = temp;
231 :
232 7674543 : for ( jCh = 0; jCh < nChannelsL; ++jCh )
233 : {
234 6287520 : temp = singularVectors_Left[jCh][iCh];
235 6287520 : singularVectors_Left[jCh][iCh] = singularVectors_Left[jCh][iCh + 1];
236 6287520 : singularVectors_Left[jCh][iCh + 1] = temp;
237 : }
238 :
239 7649118 : for ( jCh = 0; jCh < nChannelsC; ++jCh )
240 : {
241 6262095 : temp = singularVectors_Right[jCh][iCh];
242 6262095 : singularVectors_Right[jCh][iCh] = singularVectors_Right[jCh][iCh + 1];
243 6262095 : singularVectors_Right[jCh][iCh + 1] = temp;
244 : }
245 : }
246 : }
247 4157757 : } while ( condition == 1 );
248 :
249 3053268 : pop_wmops();
250 3053268 : return ( errorMessage );
251 : }
252 :
253 :
254 : /*-----------------------------------------------------------------------*
255 : * Local functions
256 : *-----------------------------------------------------------------------*/
257 :
258 : /*-------------------------------------------------------------------------
259 : * BidagonalDiagonalisation()
260 : *
261 : *
262 : *-------------------------------------------------------------------------*/
263 :
264 3053268 : static int16_t BidagonalDiagonalisation(
265 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS], /* i/o: left singular vectors (U) */
266 : float singularValues[MAX_OUTPUT_CHANNELS], /* i/o: singular values vector (S) */
267 : float singularVectors_Right[][MAX_OUTPUT_CHANNELS], /* i/o: right singular vectors (V) */
268 : float secDiag[MAX_OUTPUT_CHANNELS], /* i/o: */
269 : const int16_t nChannelsL, /* i : number of rows in the matrix to be decomposed */
270 : const int16_t nChannelsC, /* i : number of columns in the matrix to be decomposed */
271 : const float eps_x /* i : */
272 : )
273 : {
274 : int16_t kCh, nCh, iCh, jCh, split;
275 : float c, s, f1, f2;
276 3053268 : float g = 0.0f;
277 : int16_t convergence, iteration, found_split;
278 3053268 : int16_t error = 0;
279 :
280 12412893 : for ( iCh = nChannelsC - 1; iCh >= 0; iCh-- ) /* nChannelsC */
281 : {
282 9359625 : convergence = 0;
283 9359625 : iteration = 0;
284 9359625 : split = iCh - 1;
285 :
286 24660129 : while ( convergence == 0 )
287 : {
288 15300504 : iteration++;
289 15300504 : found_split = 1;
290 :
291 29163426 : for ( jCh = iCh; jCh >= 0; jCh-- )
292 : {
293 29163426 : split = jCh - 1;
294 29163426 : if ( fabsf( secDiag[jCh] ) <= CONVERGENCE_FACTOR * eps_x ) /* is secDiag[ch] vanishing compared to eps_x */
295 : {
296 15067242 : found_split = 0;
297 15067242 : break;
298 : }
299 14096184 : if ( fabsf( singularValues[split] ) <= CONVERGENCE_FACTOR * eps_x ) /* is singularValues[split] vanishing compared to eps_x */
300 : {
301 233262 : break;
302 : }
303 : }
304 :
305 15300504 : convergence = ( jCh == iCh ) ? 1 : 0;
306 :
307 15300504 : if ( found_split )
308 : {
309 233262 : s = 1.0f;
310 233262 : c = 0.0f;
311 :
312 466680 : for ( kCh = jCh; kCh <= iCh; kCh++ )
313 : {
314 233463 : g = s * secDiag[kCh];
315 233463 : secDiag[kCh] = c * secDiag[kCh];
316 233463 : if ( fabsf( g ) <= CONVERGENCE_FACTOR * eps_x )
317 : {
318 45 : break;
319 : }
320 :
321 233418 : c = singularValues[kCh];
322 233418 : singularValues[kCh] = GivensRotation( g, singularValues[kCh] );
323 233418 : c = c / maxWithSign( singularValues[kCh] );
324 233418 : s = -g / maxWithSign( singularValues[kCh] );
325 :
326 233418 : ApplyRotation( singularVectors_Left, c, s, 0, 0, &f1, &f2, kCh, split, nChannelsL ); /* nChannelsL */
327 : }
328 : }
329 :
330 15300504 : if ( convergence )
331 : {
332 9359625 : singularValues[iCh] = (float) singularValues[iCh];
333 9359625 : if ( singularValues[iCh] < 0.0f )
334 : {
335 1775583 : singularValues[iCh] = -singularValues[iCh];
336 6494673 : for ( nCh = 0; nCh < nChannelsC; nCh++ ) /* nChannelsC */
337 : {
338 4719090 : singularVectors_Right[nCh][iCh] = -singularVectors_Right[nCh][iCh];
339 : }
340 : }
341 : }
342 : else
343 : {
344 5940879 : if ( iteration >= SVD_MAX_NUM_ITERATION )
345 : {
346 0 : if ( singularValues[iCh] < 0.0f )
347 : {
348 0 : singularValues[iCh] = -singularValues[iCh];
349 :
350 0 : for ( nCh = 0; nCh < nChannelsC; nCh++ ) /* nChannelsC */
351 : {
352 0 : singularVectors_Right[nCh][iCh] = -singularVectors_Right[nCh][iCh];
353 : }
354 : }
355 0 : error = 1;
356 0 : convergence = 1;
357 : }
358 : else
359 : {
360 5940879 : ApplyQRTransform( singularVectors_Left, singularValues, singularVectors_Right, secDiag, jCh, iCh, nChannelsL, nChannelsC ); /* nChannelsC */
361 : }
362 : }
363 : }
364 : }
365 :
366 3053268 : return ( error );
367 : }
368 :
369 :
370 : /*-------------------------------------------------------------------------
371 : * ApplyQRTransform()
372 : *
373 : *
374 : *-------------------------------------------------------------------------*/
375 :
376 5940879 : static void ApplyQRTransform(
377 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS], /* i/o: left singular vectors (U) */
378 : float singularValues[MAX_OUTPUT_CHANNELS], /* i/o: singular values vector (S) */
379 : float singularVectors_Right[][MAX_OUTPUT_CHANNELS], /* i/o: right singular vectors (V) */
380 : float secDiag[MAX_OUTPUT_CHANNELS], /* i/o: */
381 : const int16_t startIndex, /* i : */
382 : const int16_t currentIndex, /* i : */
383 : const int16_t nChannelsL, /* i : number of rows in the matrix to be decomposed */
384 : const int16_t nChannelsC /* i : number of columns in the matrix to be decomposed */
385 : )
386 : {
387 : int16_t ch, split;
388 5940879 : float d = 0.0f, g = 0.0f, r = 0.0f, x_ii = 0.0f, x_split = 0.0f, x_kk = 0.0f, mu = 0.0f, aux = 0.0f;
389 5940879 : float c = 1.0f;
390 5940879 : float s = 1.0f;
391 :
392 5940879 : x_kk = singularValues[currentIndex];
393 5940879 : x_ii = singularValues[startIndex];
394 5940879 : split = currentIndex - 1;
395 :
396 5940879 : x_split = singularValues[split];
397 5940879 : g = secDiag[split];
398 5940879 : r = secDiag[currentIndex];
399 :
400 5940879 : d = ( x_split + x_kk ) * ( x_split - x_kk ) + ( g + r ) * ( g - r );
401 5940879 : d /= maxWithSign( ( r + r ) * x_split );
402 :
403 5940879 : g = GivensRotation( 1.0f, d );
404 5940879 : mu = x_split / maxWithSign( d + ( d >= 0.0f ? 1 : ( -1 ) ) * fabsf( g ) ) - r;
405 5940879 : d = ( ( x_ii + x_kk ) * ( x_ii - x_kk ) + r * mu ) / maxWithSign( x_ii );
406 :
407 : /*QR transformation*/
408 19803801 : for ( ch = startIndex; ch <= split; ch++ )
409 : {
410 13862922 : r = s * secDiag[ch + 1];
411 13862922 : g = c * secDiag[ch + 1];
412 :
413 13862922 : secDiag[ch] = GivensRotation( d, r );
414 13862922 : c = d / maxWithSign( secDiag[ch] );
415 13862922 : s = r / maxWithSign( secDiag[ch] );
416 :
417 13862922 : r = s * singularValues[ch + 1];
418 13862922 : x_split = c * singularValues[ch + 1];
419 13862922 : aux = g;
420 13862922 : ApplyRotation( singularVectors_Right, c, s, x_ii, aux, &d, &g, ch + 1, ch, nChannelsC );
421 :
422 13862922 : singularValues[ch] = GivensRotation( d, r );
423 13862922 : if ( fabsf( singularValues[ch] ) > CONVERGENCE_FACTOR * fabsf( singularValues[ch] ) )
424 : {
425 13857465 : aux = 1.0f / singularValues[ch];
426 13857465 : c = d * aux;
427 13857465 : s = r * aux;
428 : }
429 :
430 13862922 : ApplyRotation( singularVectors_Left, c, s, g, x_split, &d, &x_ii, ch + 1, ch, nChannelsL );
431 : }
432 :
433 5940879 : secDiag[startIndex] = 0.0f;
434 5940879 : secDiag[currentIndex] = d;
435 5940879 : singularValues[currentIndex] = x_ii;
436 :
437 5940879 : return;
438 : }
439 :
440 :
441 : /*-------------------------------------------------------------------------
442 : * ApplyRotation()
443 : *
444 : *
445 : *-------------------------------------------------------------------------*/
446 :
447 27959262 : static void ApplyRotation(
448 : float singularVector[][MAX_OUTPUT_CHANNELS],
449 : const float c,
450 : const float s,
451 : float x11,
452 : float x12,
453 : float *d,
454 : float *g,
455 : const int16_t currentIndex1,
456 : const int16_t currentIndex2,
457 : const int16_t nChannels )
458 : {
459 : int16_t ch;
460 :
461 27959262 : *d = c * x11 + s * x12;
462 27959262 : *g = c * x12 - s * x11;
463 :
464 166479558 : for ( ch = 0; ch < nChannels; ch++ )
465 : {
466 138520296 : x11 = singularVector[ch][currentIndex2];
467 138520296 : x12 = singularVector[ch][currentIndex1];
468 138520296 : singularVector[ch][currentIndex2] = ( c * x11 + s * x12 );
469 138520296 : singularVector[ch][currentIndex1] = ( c * x12 - s * x11 );
470 : }
471 :
472 27959262 : return;
473 : }
474 :
475 :
476 : /*-------------------------------------------------------------------------
477 : * HouseholderReduction()
478 : *
479 : *
480 : *-------------------------------------------------------------------------*/
481 :
482 3053268 : static void HouseholderReduction(
483 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS],
484 : float singularValues[MAX_OUTPUT_CHANNELS],
485 : float singularVectors_Right[][MAX_OUTPUT_CHANNELS],
486 : float secDiag[MAX_OUTPUT_CHANNELS],
487 : const int16_t nChannelsL,
488 : const int16_t nChannelsC,
489 : float *eps_x )
490 : {
491 : int16_t nCh;
492 3053268 : float g_left = 0.0f;
493 3053268 : float g_right = 0.0f;
494 :
495 : /* Bidiagonal Reduction for every channel */
496 12412893 : for ( nCh = 0; nCh < nChannelsC; nCh++ ) /* nChannelsC */
497 : {
498 9359625 : secDiag[nCh] = g_right; /* from the previous channel */
499 9359625 : biDiagonalReductionLeft( singularVectors_Left, nChannelsL, nChannelsC, nCh, &g_left );
500 9359625 : singularValues[nCh] = g_left;
501 9359625 : biDiagonalReductionRight( singularVectors_Left, nChannelsL, nChannelsC, nCh, &g_right );
502 :
503 9359625 : *eps_x = max( *eps_x, ( fabsf( singularValues[nCh] ) + fabsf( secDiag[nCh] ) ) );
504 : }
505 :
506 : /* SingularVecotr Accumulation */
507 3053268 : singularVectorsAccumulationRight( singularVectors_Left, singularVectors_Right, secDiag, nChannelsC );
508 3053268 : singularVectorsAccumulationLeft( singularVectors_Left, singularValues, nChannelsL, nChannelsC );
509 :
510 3053268 : return;
511 : }
512 :
513 :
514 : /*-------------------------------------------------------------------------
515 : * biDiagonalReductionLeft()
516 : *
517 : *
518 : *-------------------------------------------------------------------------*/
519 :
520 9359625 : static void biDiagonalReductionLeft(
521 : float singularVectors[][MAX_OUTPUT_CHANNELS],
522 : const int16_t nChannelsL,
523 : const int16_t nChannelsC,
524 : const int16_t currChannel,
525 : float *g )
526 : {
527 : int16_t iCh, jCh;
528 : float norm_x, f, r;
529 :
530 : /* Setting values to 0 */
531 9359625 : ( *g ) = 0.0f;
532 :
533 9359625 : if ( currChannel < nChannelsL ) /* i <= m */
534 : {
535 9359625 : norm_x = 0.0f;
536 :
537 34467297 : for ( jCh = currChannel; jCh < nChannelsL; jCh++ ) /* nChannelsL */
538 : {
539 25107672 : norm_x += ( singularVectors[jCh][currChannel] * singularVectors[jCh][currChannel] );
540 : }
541 :
542 9359625 : if ( ( norm_x ) ) /*(fabsf(*sig_x) > EPSILON * fabsf(*sig_x)) { */
543 : {
544 8841363 : ( *g ) = -( singularVectors[currChannel][currChannel] >= 0 ? 1 : ( -1 ) ) * sqrtf( norm_x );
545 8841363 : r = ( *g ) * singularVectors[currChannel][currChannel] - norm_x;
546 8841363 : singularVectors[currChannel][currChannel] = ( singularVectors[currChannel][currChannel] - ( *g ) );
547 :
548 22031997 : for ( iCh = currChannel + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
549 : {
550 13190634 : norm_x = 0.0f;
551 65941065 : for ( jCh = currChannel; jCh < nChannelsL; jCh++ ) /* nChannelsL */
552 : {
553 52750431 : norm_x += ( singularVectors[jCh][currChannel] * singularVectors[jCh][iCh] );
554 : }
555 :
556 13190634 : f = norm_x / maxWithSign( r );
557 :
558 :
559 65941065 : for ( jCh = currChannel; jCh < nChannelsL; jCh++ ) /* nChannelsL */
560 : {
561 52750431 : singularVectors[jCh][iCh] += ( f * singularVectors[jCh][currChannel] );
562 : }
563 : }
564 : }
565 : }
566 :
567 9359625 : return;
568 : }
569 :
570 :
571 : /*-------------------------------------------------------------------------
572 : * biDiagonalReductionRight()
573 : *
574 : *
575 : *-------------------------------------------------------------------------*/
576 :
577 9359625 : static void biDiagonalReductionRight(
578 : float singularVectors[][MAX_OUTPUT_CHANNELS],
579 : const int16_t nChannelsL,
580 : const int16_t nChannelsC,
581 : const int16_t currChannel,
582 : float *g )
583 : {
584 : int16_t iCh, jCh, idx;
585 : float norm_x, r;
586 :
587 : /* Setting values to 0 */
588 9359625 : ( *g ) = 0.0f;
589 :
590 9359625 : if ( currChannel < nChannelsL && currChannel != ( nChannelsC - 1 ) ) /* i <=m && i !=n */
591 : {
592 6306357 : idx = currChannel + 1;
593 :
594 6306357 : norm_x = 0.0f;
595 :
596 19507224 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /*nChannelsC */
597 : {
598 13200867 : norm_x += ( singularVectors[currChannel][jCh] * singularVectors[currChannel][jCh] );
599 : }
600 :
601 6306357 : if ( norm_x ) /*(fabsf(*sig_x) > EPSILON * fabsf(*sig_x)) { */
602 : {
603 5831334 : ( *g ) = -( singularVectors[currChannel][idx] >= 0 ? 1 : ( -1 ) ) * sqrtf( norm_x );
604 5831334 : r = ( *g ) * singularVectors[currChannel][idx] - norm_x;
605 5831334 : singularVectors[currChannel][idx] = ( singularVectors[currChannel][idx] - ( *g ) );
606 :
607 19842201 : for ( iCh = currChannel + 1; iCh < nChannelsL; iCh++ ) /* nChannelsL */
608 : {
609 14010867 : norm_x = 0.0f;
610 53103936 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /* nChannelsC */
611 : {
612 39093069 : norm_x += ( singularVectors[iCh][jCh] * singularVectors[currChannel][jCh] );
613 : }
614 14010867 : norm_x /= r;
615 53103936 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /* nChannelsC */
616 : {
617 39093069 : singularVectors[iCh][jCh] += ( norm_x * singularVectors[currChannel][jCh] );
618 : }
619 : }
620 : }
621 : }
622 :
623 9359625 : return;
624 : }
625 :
626 :
627 : /*-------------------------------------------------------------------------
628 : * singularVectorsAccumulationLeft()
629 : *
630 : *
631 : *-------------------------------------------------------------------------*/
632 :
633 3053268 : static void singularVectorsAccumulationLeft(
634 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS],
635 : float singularValues[MAX_OUTPUT_CHANNELS],
636 : const int16_t nChannelsL,
637 : const int16_t nChannelsC )
638 : {
639 : int16_t nCh, iCh, k;
640 : int16_t nChannels;
641 : float norm_y, t_jj, t_ii;
642 :
643 : /* Processing */
644 3053268 : nChannels = min( nChannelsL, nChannelsC ); /* min(nChannelsL,ChannelsC) */
645 :
646 12412893 : for ( nCh = nChannels - 1; nCh >= 0; nCh-- ) /* min(nChannelsL,ChannelsC) */
647 : {
648 9359625 : t_ii = singularValues[nCh];
649 :
650 22560492 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
651 : {
652 13200867 : singularVectors_Left[nCh][iCh] = 0.0f;
653 : }
654 :
655 9359625 : if ( t_ii ) /*if (fabsf(t_ii) > EPSILON *fabsf(t_ii)) {*/
656 : {
657 8841363 : t_ii = 1.0f / maxWithSign( t_ii );
658 :
659 :
660 22031997 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
661 : {
662 13190634 : norm_y = 0.0f;
663 52750431 : for ( k = nCh + 1; k < nChannelsL; k++ ) /* nChannelsL */
664 : {
665 39559797 : norm_y += ( singularVectors_Left[k][nCh] * singularVectors_Left[k][iCh] );
666 : }
667 13190634 : t_jj = t_ii * norm_y / maxWithSign( singularVectors_Left[nCh][nCh] );
668 :
669 65941065 : for ( k = nCh; k < nChannelsL; k++ ) /* nChannelsL */
670 : {
671 52750431 : singularVectors_Left[k][iCh] += ( t_jj * singularVectors_Left[k][nCh] );
672 : }
673 : }
674 :
675 33420270 : for ( iCh = nCh; iCh < nChannelsL; iCh++ ) /* nChannelsL */
676 : {
677 24578907 : singularVectors_Left[iCh][nCh] = ( singularVectors_Left[iCh][nCh] * t_ii );
678 : }
679 : }
680 : else
681 : {
682 1047027 : for ( iCh = nCh; iCh < nChannelsL; iCh++ ) /* nChannelsL */
683 : {
684 528765 : singularVectors_Left[iCh][nCh] = 0.0f;
685 : }
686 : }
687 :
688 9359625 : ++singularVectors_Left[nCh][nCh];
689 : }
690 :
691 3053268 : return;
692 : }
693 :
694 :
695 : /*-------------------------------------------------------------------------
696 : * singularVectorsAccumulationRight()
697 : *
698 : *
699 : *-------------------------------------------------------------------------*/
700 :
701 3053268 : static void singularVectorsAccumulationRight(
702 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS],
703 : float singularVectors_Right[][MAX_OUTPUT_CHANNELS],
704 : float secDiag[MAX_OUTPUT_CHANNELS],
705 : const int16_t nChannelsC )
706 : {
707 : int16_t nCh, iCh, k;
708 : int16_t nChannels;
709 : float norm_y, t_ii, ratio;
710 :
711 : /* Processing */
712 3053268 : nChannels = nChannelsC; /* nChannelsC */
713 :
714 : /* avoid compiler warning */
715 3053268 : t_ii = secDiag[nChannels - 1];
716 :
717 12412893 : for ( nCh = nChannels - 1; nCh >= 0; nCh-- ) /* nChannelsC, min(nChannelsLmnChannelsC) otherwise */
718 : {
719 :
720 9359625 : if ( nCh < nChannelsC - 1 ) /* nChannelsC */
721 : {
722 6306357 : if ( t_ii ) /*if (fabsf(t_ii) > EPSILON *fabsf(t_ii)) {*/
723 : {
724 :
725 18555546 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC*/
726 : {
727 12724212 : ratio = singularVectors_Left[nCh][iCh] / maxWithSign( singularVectors_Left[nCh][nCh + 1] );
728 12724212 : singularVectors_Right[iCh][nCh] = ratio / maxWithSign( t_ii );
729 : }
730 :
731 18555546 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
732 : {
733 12724212 : norm_y = 0.0f;
734 :
735 50503956 : for ( k = nCh + 1; k < nChannelsC; k++ ) /* nChannelsC */
736 : {
737 37779744 : norm_y += ( singularVectors_Left[nCh][k] * singularVectors_Right[k][iCh] );
738 : }
739 :
740 50503956 : for ( k = nCh + 1; k < nChannelsC; k++ ) /* nChannelsC */
741 : {
742 37779744 : singularVectors_Right[k][iCh] += ( norm_y * singularVectors_Right[k][nCh] );
743 : }
744 : }
745 : }
746 :
747 19507224 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
748 : {
749 13200867 : singularVectors_Right[nCh][iCh] = singularVectors_Right[iCh][nCh] = 0.0f;
750 : }
751 : }
752 9359625 : singularVectors_Right[nCh][nCh] = 1.0f;
753 9359625 : t_ii = secDiag[nCh];
754 : }
755 :
756 3053268 : return;
757 : }
758 :
759 :
760 : /*-------------------------------------------------------------------------
761 : * GivensRotation()
762 : *
763 : *
764 : *-------------------------------------------------------------------------*/
765 :
766 33900141 : static float GivensRotation(
767 : const float x,
768 : const float z )
769 : {
770 : float x_abs, z_abs;
771 : float cotan, tan, r;
772 33900141 : x_abs = fabsf( x );
773 33900141 : z_abs = fabsf( z );
774 33900141 : if ( x_abs <= CONVERGENCE_FACTOR * x_abs && z_abs <= CONVERGENCE_FACTOR * z_abs )
775 : {
776 5457 : r = 0.0f;
777 : }
778 33894684 : else if ( x_abs >= z_abs )
779 : {
780 25665279 : if ( x_abs <= SVD_MINIMUM_VALUE )
781 : {
782 0 : r = 0.0f;
783 : }
784 : else
785 : {
786 25665279 : cotan = z_abs / ( x_abs );
787 25665279 : r = x_abs * sqrtf( 1.0f + cotan * cotan );
788 : }
789 : }
790 : else
791 : {
792 8229405 : if ( z_abs <= SVD_MINIMUM_VALUE )
793 : {
794 0 : r = 0.0f;
795 : }
796 : else
797 : {
798 8229405 : tan = x_abs / ( z_abs );
799 8229405 : r = z_abs * sqrtf( 1.0f + tan * tan );
800 : }
801 : }
802 :
803 33900141 : return ( r );
804 : }
805 :
806 :
807 : /*-------------------------------------------------------------------------
808 : * maxWithSign()
809 : *
810 : *
811 : *-------------------------------------------------------------------------*/
812 :
813 106686372 : static float maxWithSign(
814 : const float a )
815 : {
816 106686372 : if ( fabsf( a ) > SVD_MINIMUM_VALUE )
817 : {
818 106686195 : return a;
819 : }
820 177 : else if ( a < 0.0f )
821 : {
822 177 : return -SVD_MINIMUM_VALUE;
823 : }
824 : else
825 : {
826 0 : return SVD_MINIMUM_VALUE;
827 : }
828 : }
829 :
830 :
831 : /*-------------------------------------------------------------------------
832 : * flushToZeroArray()
833 : *
834 : *
835 : *-------------------------------------------------------------------------*/
836 :
837 3053268 : static void flushToZeroArray(
838 : float arr[MAX_OUTPUT_CHANNELS],
839 : const int16_t length )
840 : {
841 : int16_t i;
842 :
843 13673148 : for ( i = 0; i < length; ++i )
844 : {
845 10619880 : if ( fabsf( arr[i] ) < SVD_ZERO_FLUSH_THRESHOLD )
846 : {
847 539922 : arr[i] = 0.0f;
848 : }
849 : }
850 :
851 3053268 : return;
852 : }
853 :
854 :
855 : /*-------------------------------------------------------------------------
856 : * flushToZeroMat()
857 : *
858 : *
859 : *-------------------------------------------------------------------------*/
860 :
861 6106536 : static void flushToZeroMat(
862 : float mat[][MAX_OUTPUT_CHANNELS],
863 : const int16_t m,
864 : const int16_t n )
865 : {
866 : int16_t i, j;
867 :
868 26086041 : for ( i = 0; i < m; ++i )
869 : {
870 100610736 : for ( j = 0; j < n; ++j )
871 : {
872 80631231 : if ( fabsf( mat[i][j] ) < SVD_ZERO_FLUSH_THRESHOLD )
873 : {
874 14080968 : mat[i][j] = 0.0f;
875 : }
876 : }
877 : }
878 :
879 6106536 : return;
880 : }
|
