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], float singularValues[MAX_OUTPUT_CHANNELS], float secDiag[MAX_OUTPUT_CHANNELS], const int16_t nChannelsL, const int16_t nChannelsC, const int16_t currChannel, float *sig_x, float *g );
64 :
65 : static void biDiagonalReductionRight( float singularVectors[][MAX_OUTPUT_CHANNELS], float secDiag[MAX_OUTPUT_CHANNELS], const int16_t nChannelsL, const int16_t nChannelsC, const int16_t currChannel, float *sig_x, 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 4157337 : condition = 0;
223 14081925 : for ( iCh = 0; iCh < lengthSingularValues - 1; iCh++ )
224 : {
225 9924588 : if ( singularValues[iCh] < singularValues[iCh + 1] )
226 : {
227 1386972 : condition = 1;
228 1386972 : temp = singularValues[iCh];
229 1386972 : singularValues[iCh] = singularValues[iCh + 1];
230 1386972 : singularValues[iCh + 1] = temp;
231 :
232 7674792 : for ( jCh = 0; jCh < nChannelsL; ++jCh )
233 : {
234 6287820 : temp = singularVectors_Left[jCh][iCh];
235 6287820 : singularVectors_Left[jCh][iCh] = singularVectors_Left[jCh][iCh + 1];
236 6287820 : singularVectors_Left[jCh][iCh + 1] = temp;
237 : }
238 :
239 7649367 : for ( jCh = 0; jCh < nChannelsC; ++jCh )
240 : {
241 6262395 : temp = singularVectors_Right[jCh][iCh];
242 6262395 : singularVectors_Right[jCh][iCh] = singularVectors_Right[jCh][iCh + 1];
243 6262395 : singularVectors_Right[jCh][iCh + 1] = temp;
244 : }
245 : }
246 : }
247 4157337 : } 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 24660354 : while ( convergence == 0 )
287 : {
288 15300729 : iteration++;
289 15300729 : found_split = 1;
290 :
291 29165064 : for ( jCh = iCh; jCh >= 0; jCh-- )
292 : {
293 29165064 : split = jCh - 1;
294 29165064 : if ( fabsf( secDiag[jCh] ) <= CONVERGENCE_FACTOR * eps_x ) /* is secDiag[ch] vanishing compared to eps_x */
295 : {
296 15068259 : found_split = 0;
297 15068259 : break;
298 : }
299 14096805 : if ( fabsf( singularValues[split] ) <= CONVERGENCE_FACTOR * eps_x ) /* is singularValues[split] vanishing compared to eps_x */
300 : {
301 232470 : break;
302 : }
303 : }
304 :
305 15300729 : convergence = ( jCh == iCh ) ? 1 : 0;
306 :
307 15300729 : if ( found_split )
308 : {
309 232470 : s = 1.0f;
310 232470 : c = 0.0f;
311 :
312 465072 : for ( kCh = jCh; kCh <= iCh; kCh++ )
313 : {
314 232644 : g = s * secDiag[kCh];
315 232644 : secDiag[kCh] = c * secDiag[kCh];
316 232644 : if ( fabsf( g ) <= CONVERGENCE_FACTOR * eps_x )
317 : {
318 42 : break;
319 : }
320 :
321 232602 : c = singularValues[kCh];
322 232602 : singularValues[kCh] = GivensRotation( g, singularValues[kCh] );
323 232602 : c = c / maxWithSign( singularValues[kCh] );
324 232602 : s = -g / maxWithSign( singularValues[kCh] );
325 :
326 232602 : ApplyRotation( singularVectors_Left, c, s, 0, 0, &f1, &f2, kCh, split, nChannelsL ); /* nChannelsL */
327 : }
328 : }
329 :
330 15300729 : if ( convergence )
331 : {
332 9359625 : singularValues[iCh] = (float) singularValues[iCh];
333 9359625 : if ( singularValues[iCh] < 0.0f )
334 : {
335 1758519 : singularValues[iCh] = -singularValues[iCh];
336 6426483 : for ( nCh = 0; nCh < nChannelsC; nCh++ ) /* nChannelsC */
337 : {
338 4667964 : singularVectors_Right[nCh][iCh] = -singularVectors_Right[nCh][iCh];
339 : }
340 : }
341 : }
342 : else
343 : {
344 5941104 : 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 5941104 : 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 5941104 : 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 5941104 : 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 5941104 : float c = 1.0f;
390 5941104 : float s = 1.0f;
391 :
392 5941104 : x_kk = singularValues[currentIndex];
393 5941104 : x_ii = singularValues[startIndex];
394 5941104 : split = currentIndex - 1;
395 :
396 5941104 : x_split = singularValues[split];
397 5941104 : g = secDiag[split];
398 5941104 : r = secDiag[currentIndex];
399 :
400 5941104 : d = ( x_split + x_kk ) * ( x_split - x_kk ) + ( g + r ) * ( g - r );
401 5941104 : d /= maxWithSign( ( r + r ) * x_split );
402 :
403 5941104 : g = GivensRotation( 1.0f, d );
404 5941104 : mu = x_split / maxWithSign( d + ( d >= 0.0f ? 1 : ( -1 ) ) * fabsf( g ) ) - r;
405 5941104 : d = ( ( x_ii + x_kk ) * ( x_ii - x_kk ) + r * mu ) / maxWithSign( x_ii );
406 :
407 : /*QR transformation*/
408 19805439 : for ( ch = startIndex; ch <= split; ch++ )
409 : {
410 13864335 : r = s * secDiag[ch + 1];
411 13864335 : g = c * secDiag[ch + 1];
412 :
413 13864335 : secDiag[ch] = GivensRotation( d, r );
414 13864335 : c = d / maxWithSign( secDiag[ch] );
415 13864335 : s = r / maxWithSign( secDiag[ch] );
416 :
417 13864335 : r = s * singularValues[ch + 1];
418 13864335 : x_split = c * singularValues[ch + 1];
419 13864335 : aux = g;
420 13864335 : ApplyRotation( singularVectors_Right, c, s, x_ii, aux, &d, &g, ch + 1, ch, nChannelsC );
421 :
422 13864335 : singularValues[ch] = GivensRotation( d, r );
423 13864335 : if ( fabsf( singularValues[ch] ) > CONVERGENCE_FACTOR * fabsf( singularValues[ch] ) )
424 : {
425 13859523 : aux = 1.0f / singularValues[ch];
426 13859523 : c = d * aux;
427 13859523 : s = r * aux;
428 : }
429 :
430 13864335 : ApplyRotation( singularVectors_Left, c, s, g, x_split, &d, &x_ii, ch + 1, ch, nChannelsL );
431 : }
432 :
433 5941104 : secDiag[startIndex] = 0.0f;
434 5941104 : secDiag[currentIndex] = d;
435 5941104 : singularValues[currentIndex] = x_ii;
436 :
437 5941104 : return;
438 : }
439 :
440 :
441 : /*-------------------------------------------------------------------------
442 : * ApplyRotation()
443 : *
444 : *
445 : *-------------------------------------------------------------------------*/
446 :
447 27961272 : 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 27961272 : *d = c * x11 + s * x12;
462 27961272 : *g = c * x12 - s * x11;
463 :
464 166497225 : for ( ch = 0; ch < nChannels; ch++ )
465 : {
466 138535953 : x11 = singularVector[ch][currentIndex2];
467 138535953 : x12 = singularVector[ch][currentIndex1];
468 138535953 : singularVector[ch][currentIndex2] = ( c * x11 + s * x12 );
469 138535953 : singularVector[ch][currentIndex1] = ( c * x12 - s * x11 );
470 : }
471 :
472 27961272 : 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 = 0.0f, sig_x = 0.0f;
493 :
494 : /* Bidiagonal Reduction for every channel */
495 12412893 : for ( nCh = 0; nCh < nChannelsC; nCh++ ) /* nChannelsC */
496 : {
497 9359625 : biDiagonalReductionLeft( singularVectors_Left, singularValues, secDiag, nChannelsL, nChannelsC, nCh, &sig_x, &g );
498 9359625 : biDiagonalReductionRight( singularVectors_Left, secDiag, nChannelsL, nChannelsC, nCh, &sig_x, &g );
499 9359625 : *eps_x = max( *eps_x, ( fabsf( singularValues[nCh] ) + fabsf( secDiag[nCh] ) ) );
500 : }
501 :
502 : /* SingularVecotr Accumulation */
503 3053268 : singularVectorsAccumulationRight( singularVectors_Left, singularVectors_Right, secDiag, nChannelsC );
504 3053268 : singularVectorsAccumulationLeft( singularVectors_Left, singularValues, nChannelsL, nChannelsC );
505 :
506 3053268 : return;
507 : }
508 :
509 :
510 : /*-------------------------------------------------------------------------
511 : * biDiagonalReductionLeft()
512 : *
513 : *
514 : *-------------------------------------------------------------------------*/
515 :
516 9359625 : static void biDiagonalReductionLeft(
517 : float singularVectors[][MAX_OUTPUT_CHANNELS],
518 : float singularValues[MAX_OUTPUT_CHANNELS],
519 : float secDiag[MAX_OUTPUT_CHANNELS],
520 : const int16_t nChannelsL,
521 : const int16_t nChannelsC,
522 : const int16_t currChannel,
523 : float *sig_x,
524 : float *g )
525 : {
526 : int16_t iCh, jCh, idx;
527 : float norm_x, f, r;
528 :
529 9359625 : secDiag[currChannel] = ( *sig_x ) * ( *g );
530 :
531 : /* Setting values to 0 */
532 9359625 : ( *sig_x ) = 0.0f;
533 9359625 : ( *g ) = 0.0f;
534 :
535 9359625 : if ( currChannel < nChannelsL ) /* i <= m */
536 : {
537 9359625 : idx = currChannel;
538 :
539 34467297 : for ( jCh = idx; jCh < nChannelsL; jCh++ ) /* nChannelsL */
540 : {
541 25107672 : ( *sig_x ) += fabsf( singularVectors[jCh][currChannel] );
542 : }
543 :
544 9359625 : if ( ( *sig_x ) ) /*(fabsf(*sig_x) > EPSILON * fabsf(*sig_x)) { */
545 : {
546 8826147 : norm_x = 0.0f;
547 :
548 33381693 : for ( jCh = idx; jCh < nChannelsL; jCh++ ) /* nChannelsL */
549 : {
550 24555546 : singularVectors[jCh][currChannel] = ( singularVectors[jCh][currChannel] / maxWithSign( ( *sig_x ) ) );
551 24555546 : norm_x += ( singularVectors[jCh][currChannel] * singularVectors[jCh][currChannel] );
552 : }
553 8826147 : ( *g ) = -( singularVectors[currChannel][idx] >= 0 ? 1 : ( -1 ) ) * sqrtf( norm_x );
554 8826147 : r = ( *g ) * singularVectors[currChannel][idx] - norm_x;
555 8826147 : singularVectors[currChannel][idx] = ( singularVectors[currChannel][idx] - ( *g ) );
556 :
557 22008528 : for ( iCh = currChannel + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
558 : {
559 13182381 : norm_x = 0.0f;
560 65913204 : for ( jCh = idx; jCh < nChannelsL; jCh++ ) /* nChannelsL */
561 : {
562 52730823 : norm_x += ( singularVectors[jCh][currChannel] * singularVectors[jCh][iCh] );
563 : }
564 :
565 13182381 : f = norm_x / maxWithSign( r );
566 :
567 :
568 65913204 : for ( jCh = idx; jCh < nChannelsL; jCh++ ) /* nChannelsL */
569 : {
570 52730823 : singularVectors[jCh][iCh] += ( f * singularVectors[jCh][currChannel] );
571 : }
572 : }
573 :
574 :
575 33381693 : for ( jCh = idx; jCh < nChannelsL; jCh++ ) /* nChannelsL */
576 : {
577 24555546 : singularVectors[jCh][currChannel] = ( singularVectors[jCh][currChannel] * ( *sig_x ) );
578 : }
579 : }
580 :
581 9359625 : singularValues[currChannel] = ( ( *sig_x ) * ( *g ) );
582 : }
583 :
584 9359625 : return;
585 : }
586 :
587 :
588 : /*-------------------------------------------------------------------------
589 : * biDiagonalReductionRight()
590 : *
591 : *
592 : *-------------------------------------------------------------------------*/
593 :
594 9359625 : static void biDiagonalReductionRight(
595 : float singularVectors[][MAX_OUTPUT_CHANNELS],
596 : float secDiag[MAX_OUTPUT_CHANNELS],
597 : const int16_t nChannelsL,
598 : const int16_t nChannelsC,
599 : const int16_t currChannel,
600 : float *sig_x,
601 : float *g )
602 : {
603 : int16_t iCh, jCh, idx;
604 : float norm_x, r;
605 :
606 : /* Setting values to 0 */
607 9359625 : ( *sig_x ) = 0.0f;
608 9359625 : ( *g ) = 0.0f;
609 :
610 9359625 : if ( currChannel < nChannelsL && currChannel != ( nChannelsC - 1 ) ) /* i <=m && i !=n */
611 : {
612 6306357 : idx = currChannel + 1;
613 :
614 19507224 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /* nChannelsC */
615 : {
616 13200867 : ( *sig_x ) += fabsf( singularVectors[currChannel][jCh] );
617 : }
618 :
619 6306357 : if ( ( *sig_x ) ) /*(fabsf(*sig_x) > EPSILON * fabsf(*sig_x)) { */
620 : {
621 5813127 : norm_x = 0.0f;
622 :
623 18517782 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /*nChannelsC */
624 : {
625 12704655 : singularVectors[currChannel][jCh] = ( singularVectors[currChannel][jCh] / maxWithSign( ( *sig_x ) ) );
626 12704655 : norm_x += ( singularVectors[currChannel][jCh] * singularVectors[currChannel][jCh] );
627 : }
628 5813127 : ( *g ) = -( singularVectors[currChannel][idx] >= 0 ? 1 : ( -1 ) ) * sqrtf( norm_x );
629 5813127 : r = ( *g ) * singularVectors[currChannel][idx] - norm_x;
630 5813127 : singularVectors[currChannel][idx] = ( singularVectors[currChannel][idx] - ( *g ) );
631 :
632 18517782 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /* nChannelsC */
633 : {
634 12704655 : secDiag[jCh] = singularVectors[currChannel][jCh] / maxWithSign( r );
635 : }
636 :
637 19804545 : for ( iCh = currChannel + 1; iCh < nChannelsL; iCh++ ) /* nChannelsL */
638 : {
639 13991418 : norm_x = 0.0f;
640 53061972 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /* nChannelsC */
641 : {
642 39070554 : norm_x += ( singularVectors[iCh][jCh] * singularVectors[currChannel][jCh] );
643 : }
644 :
645 53061972 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /* nChannelsC */
646 : {
647 39070554 : singularVectors[iCh][jCh] += ( norm_x * secDiag[jCh] );
648 : }
649 : }
650 :
651 18517782 : for ( jCh = idx; jCh < nChannelsC; jCh++ ) /* nChannelsC */
652 : {
653 12704655 : singularVectors[currChannel][jCh] = ( singularVectors[currChannel][jCh] * ( *sig_x ) );
654 : }
655 : }
656 : }
657 :
658 9359625 : return;
659 : }
660 :
661 :
662 : /*-------------------------------------------------------------------------
663 : * singularVectorsAccumulationLeft()
664 : *
665 : *
666 : *-------------------------------------------------------------------------*/
667 :
668 3053268 : static void singularVectorsAccumulationLeft(
669 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS],
670 : float singularValues[MAX_OUTPUT_CHANNELS],
671 : const int16_t nChannelsL,
672 : const int16_t nChannelsC )
673 : {
674 : int16_t nCh, iCh, k;
675 : int16_t nChannels;
676 : float norm_y, t_jj, t_ii;
677 :
678 : /* Processing */
679 3053268 : nChannels = min( nChannelsL, nChannelsC ); /* min(nChannelsL,ChannelsC) */
680 :
681 12412893 : for ( nCh = nChannels - 1; nCh >= 0; nCh-- ) /* min(nChannelsL,ChannelsC) */
682 : {
683 9359625 : t_ii = singularValues[nCh];
684 :
685 22560492 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
686 : {
687 13200867 : singularVectors_Left[nCh][iCh] = 0.0f;
688 : }
689 :
690 9359625 : if ( t_ii ) /*if (fabsf(t_ii) > EPSILON *fabsf(t_ii)) {*/
691 : {
692 8826147 : t_ii = 1.0f / maxWithSign( t_ii );
693 :
694 :
695 22008528 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
696 : {
697 13182381 : norm_y = 0.0f;
698 52730823 : for ( k = nCh + 1; k < nChannelsL; k++ ) /* nChannelsL */
699 : {
700 39548442 : norm_y += ( singularVectors_Left[k][nCh] * singularVectors_Left[k][iCh] );
701 : }
702 13182381 : t_jj = t_ii * norm_y / maxWithSign( singularVectors_Left[nCh][nCh] );
703 :
704 65913204 : for ( k = nCh; k < nChannelsL; k++ ) /* nChannelsL */
705 : {
706 52730823 : singularVectors_Left[k][iCh] += ( t_jj * singularVectors_Left[k][nCh] );
707 : }
708 : }
709 :
710 33381693 : for ( iCh = nCh; iCh < nChannelsL; iCh++ ) /* nChannelsL */
711 : {
712 24555546 : singularVectors_Left[iCh][nCh] = ( singularVectors_Left[iCh][nCh] * t_ii );
713 : }
714 : }
715 : else
716 : {
717 1085604 : for ( iCh = nCh; iCh < nChannelsL; iCh++ ) /* nChannelsL */
718 : {
719 552126 : singularVectors_Left[iCh][nCh] = 0.0f;
720 : }
721 : }
722 :
723 9359625 : ++singularVectors_Left[nCh][nCh];
724 : }
725 :
726 3053268 : return;
727 : }
728 :
729 :
730 : /*-------------------------------------------------------------------------
731 : * singularVectorsAccumulationRight()
732 : *
733 : *
734 : *-------------------------------------------------------------------------*/
735 :
736 3053268 : static void singularVectorsAccumulationRight(
737 : float singularVectors_Left[][MAX_OUTPUT_CHANNELS],
738 : float singularVectors_Right[][MAX_OUTPUT_CHANNELS],
739 : float secDiag[MAX_OUTPUT_CHANNELS],
740 : const int16_t nChannelsC )
741 : {
742 : int16_t nCh, iCh, k;
743 : int16_t nChannels;
744 : float norm_y, t_ii, ratio;
745 :
746 : /* Processing */
747 3053268 : nChannels = nChannelsC; /* nChannelsC */
748 :
749 : /* avoid compiler warning */
750 3053268 : t_ii = secDiag[nChannels - 1];
751 :
752 12412893 : for ( nCh = nChannels - 1; nCh >= 0; nCh-- ) /* nChannelsC, min(nChannelsLmnChannelsC) otherwise */
753 : {
754 :
755 9359625 : if ( nCh < nChannelsC - 1 ) /* nChannelsC */
756 : {
757 6306357 : if ( t_ii ) /*if (fabsf(t_ii) > EPSILON *fabsf(t_ii)) {*/
758 : {
759 :
760 18517782 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC*/
761 : {
762 12704655 : ratio = singularVectors_Left[nCh][iCh] / maxWithSign( singularVectors_Left[nCh][nCh + 1] );
763 12704655 : singularVectors_Right[iCh][nCh] = ratio / maxWithSign( t_ii );
764 : }
765 :
766 18517782 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
767 : {
768 12704655 : norm_y = 0.0f;
769 :
770 50461776 : for ( k = nCh + 1; k < nChannelsC; k++ ) /* nChannelsC */
771 : {
772 37757121 : norm_y += ( singularVectors_Left[nCh][k] * singularVectors_Right[k][iCh] );
773 : }
774 :
775 50461776 : for ( k = nCh + 1; k < nChannelsC; k++ ) /* nChannelsC */
776 : {
777 37757121 : singularVectors_Right[k][iCh] += ( norm_y * singularVectors_Right[k][nCh] );
778 : }
779 : }
780 : }
781 :
782 19507224 : for ( iCh = nCh + 1; iCh < nChannelsC; iCh++ ) /* nChannelsC */
783 : {
784 13200867 : singularVectors_Right[nCh][iCh] = singularVectors_Right[iCh][nCh] = 0.0f;
785 : }
786 : }
787 9359625 : singularVectors_Right[nCh][nCh] = 1.0f;
788 9359625 : t_ii = secDiag[nCh];
789 : }
790 :
791 3053268 : return;
792 : }
793 :
794 :
795 : /*-------------------------------------------------------------------------
796 : * GivensRotation()
797 : *
798 : *
799 : *-------------------------------------------------------------------------*/
800 :
801 33902376 : static float GivensRotation(
802 : const float x,
803 : const float z )
804 : {
805 : float x_abs, z_abs;
806 : float cotan, tan, r;
807 33902376 : x_abs = fabsf( x );
808 33902376 : z_abs = fabsf( z );
809 33902376 : if ( x_abs <= CONVERGENCE_FACTOR * x_abs && z_abs <= CONVERGENCE_FACTOR * z_abs )
810 : {
811 4812 : r = 0.0f;
812 : }
813 33897564 : else if ( x_abs >= z_abs )
814 : {
815 25667235 : if ( x_abs <= SVD_MINIMUM_VALUE )
816 : {
817 0 : r = 0.0f;
818 : }
819 : else
820 : {
821 25667235 : cotan = z_abs / ( x_abs );
822 25667235 : r = x_abs * sqrtf( 1.0f + cotan * cotan );
823 : }
824 : }
825 : else
826 : {
827 8230329 : if ( z_abs <= SVD_MINIMUM_VALUE )
828 : {
829 0 : r = 0.0f;
830 : }
831 : else
832 : {
833 8230329 : tan = x_abs / ( z_abs );
834 8230329 : r = z_abs * sqrtf( 1.0f + tan * tan );
835 : }
836 : }
837 :
838 33902376 : return ( r );
839 : }
840 :
841 :
842 : /*-------------------------------------------------------------------------
843 : * maxWithSign()
844 : *
845 : *
846 : *-------------------------------------------------------------------------*/
847 :
848 156582261 : static float maxWithSign(
849 : const float a )
850 : {
851 156582261 : if ( fabsf( a ) > SVD_MINIMUM_VALUE )
852 : {
853 156582261 : return a;
854 : }
855 0 : else if ( a < 0.0f )
856 : {
857 0 : return -SVD_MINIMUM_VALUE;
858 : }
859 : else
860 : {
861 0 : return SVD_MINIMUM_VALUE;
862 : }
863 : }
864 :
865 :
866 : /*-------------------------------------------------------------------------
867 : * flushToZeroArray()
868 : *
869 : *
870 : *-------------------------------------------------------------------------*/
871 :
872 3053268 : static void flushToZeroArray(
873 : float arr[MAX_OUTPUT_CHANNELS],
874 : const int16_t length )
875 : {
876 : int16_t i;
877 :
878 13673148 : for ( i = 0; i < length; ++i )
879 : {
880 10619880 : if ( fabsf( arr[i] ) < SVD_ZERO_FLUSH_THRESHOLD )
881 : {
882 555084 : arr[i] = 0.0f;
883 : }
884 : }
885 :
886 3053268 : return;
887 : }
888 :
889 :
890 : /*-------------------------------------------------------------------------
891 : * flushToZeroMat()
892 : *
893 : *
894 : *-------------------------------------------------------------------------*/
895 :
896 6106536 : static void flushToZeroMat(
897 : float mat[][MAX_OUTPUT_CHANNELS],
898 : const int16_t m,
899 : const int16_t n )
900 : {
901 : int16_t i, j;
902 :
903 26086041 : for ( i = 0; i < m; ++i )
904 : {
905 100610736 : for ( j = 0; j < n; ++j )
906 : {
907 80631231 : if ( fabsf( mat[i][j] ) < SVD_ZERO_FLUSH_THRESHOLD )
908 : {
909 14080884 : mat[i][j] = 0.0f;
910 : }
911 : }
912 : }
913 :
914 6106536 : return;
915 : }
|
