62eae28f2974682f4cad6e85e7cab7b6ef3f2fb1
[ep-processor-libraries/dsplib.git] / ti / dsplib / src / DSPF_sp_svd_cmplx / c66 / DSPF_sp_svd_cmplx.c
1 /* ======================================================================= */
2 /* DSPF_sp_svd_cmplx.c -- singular value decomposition                     */
3 /*                 Optimized C Implementation                              */
4 /*                                                                         */
5 /* Rev 1.0.0                                                               */
6 /*                                                                         */
7 /* Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/  */
8 /*                                                                         */
9 /*                                                                         */
10 /*  Redistribution and use in source and binary forms, with or without     */
11 /*  modification, are permitted provided that the following conditions     */
12 /*  are met:                                                               */
13 /*                                                                         */
14 /*    Redistributions of source code must retain the above copyright       */
15 /*    notice, this list of conditions and the following disclaimer.        */
16 /*                                                                         */
17 /*    Redistributions in binary form must reproduce the above copyright    */
18 /*    notice, this list of conditions and the following disclaimer in the  */
19 /*    documentation and/or other materials provided with the               */
20 /*    distribution.                                                        */
21 /*                                                                         */
22 /*    Neither the name of Texas Instruments Incorporated nor the names of  */
23 /*    its contributors may be used to endorse or promote products derived  */
24 /*    from this software without specific prior written permission.        */
25 /*                                                                         */
26 /*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
27 /*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
28 /*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  */
29 /*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT   */
30 /*  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,  */
31 /*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT       */
32 /*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,  */
33 /*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY  */
34 /*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT    */
35 /*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE  */
36 /*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   */
37 /*                                                                         */
38 /* ======================================================================= */\
39 #include <math.h>
40 #include <float.h>
41 #include <string.h>
42 #include <c6x.h>
44 #pragma CODE_SECTION(DSPF_sp_svd_cmplx,".text:optimized");
45 #pragma CODE_SECTION(DSPF_sp_convert_to_bidiag_cmplx,".text:optimized");
46 #pragma CODE_SECTION(DSPF_sp_bidiag_to_diag_cmplx,".text:optimized");
47 #pragma CODE_SECTION(DSPF_sp_sort_singular_values_cmplx,".text:optimized");
49 #include "DSPF_sp_svd_cmplx.h"
51 #define ENABLE_NR   1
52 #define ENABLE_MATHLIB 1
54 #define MAX_ITERATION_COUNT 30
55 #define ZERO_TOLERANCE 1.0e-6
56 //#define ENABLE_REDUCED_FORM 1
58 #ifdef ENABLE_MATHLIB
59 #include <ti/mathlib/src/atan2sp/c66/atan2sp.h>
60 #include <ti/mathlib/src/cossp/c66/cossp.h>
61 #include <ti/mathlib/src/sinsp/c66/sinsp.h>
62 #endif
64 #define zero          _ftof2(0,0)
66 #ifndef _LITTLE_ENDIAN
67 #define cmpy(a,b)     _complex_mpysp(a,b)
68 #define ccmpy(a,b)    _complex_conjugate_mpysp(a,b)
69 #define real(a)       _hif2(a)
70 #define imag(a)       _lof2(a)
71 #define one           _ftof2(1,0)
72 #define real_to_f2(a) _ftof2(a,0)
73 #else
74 #define cmpy(a,b)     _ftof2(_lof2(_complex_mpysp(a,b)),-_hif2(_complex_mpysp(a,b)))
75 #define ccmpy(a,b)    _ftof2(-_lof2(_complex_conjugate_mpysp(a,b)),_hif2(_complex_conjugate_mpysp(a,b)))
76 #define real(a)       _lof2(a)
77 #define imag(a)       _hif2(a)
78 #define one           _ftof2(0,1)
79 #define real_to_f2(a) _ftof2(0,a)
80 #endif
82 static int DSPF_sp_convert_to_bidiag_cmplx(const int Nrows,const int Ncols,float *restrict U,float *restrict V,float *restrict diag,float *restrict superdiag);
83 static int DSPF_sp_bidiag_to_diag_cmplx(const int Nrows,const int Ncols,float *restrict U,float *restrict V,float *restrict diag,float *restrict superdiag);
84 static int DSPF_sp_sort_singular_values_cmplx(const int Nrows,const int Ncols,float *restrict U,float *restrict V,float *restrict singular_values);
86 static void complex_sp_sqrt(__float2_t cx,__float2_t *cz);
87 static void complex_sp_inv(__float2_t cx,__float2_t *cz);
88 static void complex_sp_div(__float2_t cx,__float2_t cy,__float2_t *cz);
90 int DSPF_sp_svd_cmplx(const int Nrows,const int Ncols,float *restrict A,float *restrict U,float *restrict V,float *restrict U1,float *restrict diag,float *restrict superdiag)
91 {
92   int row,col,Nrows1,Ncols1,status;
94   _nassert(Nrows>0);
95   _nassert(Ncols>0);
96   _nassert((int)A % 8 == 0);
97   _nassert((int)U % 8 == 0);
98   _nassert((int)V % 8 == 0);
99   _nassert((int)U1 % 8 == 0);
100   _nassert((int)diag % 8 == 0);
101   _nassert((int)superdiag % 8 == 0);
103   /* ------------------------------------------------------------------- */
104   /* copy A matrix to D                                                  */
105   /* ------------------------------------------------------------------- */
106   if (Nrows>=Ncols) {
107         Nrows1=Nrows;
108         Ncols1=Ncols;
109     memcpy(U,A,sizeof(float)*Nrows*2*Ncols);
110   } else {
111         /* transpose matrix */
112 #pragma MUST_ITERATE(1,,)
113         for (row=0;row<Nrows;row++) {
114 #pragma MUST_ITERATE(1,,)
115       for (col=0;col<Ncols;col++) {
116         U[2*row  +col*2*Nrows]= A[2*col  +row*2*Ncols];
117         U[2*row+1+col*2*Nrows]=-A[2*col+1+row*2*Ncols];
118       }
119         }
120         Nrows1=Ncols;
121         Ncols1=Nrows;
122   }
124   /* ------------------------------------------------------------------- */
125   /* convert A to bidiagonal matrix using Householder reflections        */
126   /* ------------------------------------------------------------------- */
127   DSPF_sp_convert_to_bidiag_cmplx(Nrows1,Ncols1,U,V,diag,superdiag);
129   /* ------------------------------------------------------------------- */
130   /* convert bidiagonal to diagonal using Givens rotations               */
131   /* ------------------------------------------------------------------- */
132   status=DSPF_sp_bidiag_to_diag_cmplx(Nrows1,Ncols1,U,V,diag,superdiag);
134   /* ------------------------------------------------------------------- */
135   /* sort singular values in descending order                            */
136   /* ------------------------------------------------------------------- */
137   DSPF_sp_sort_singular_values_cmplx(Nrows1,Ncols1,U,V,diag);
139   /* ------------------------------------------------------------------- */
140   /* switch U and V                                                      */
141   /* ------------------------------------------------------------------- */
142   if (Ncols>Nrows) {
143 #ifndef ENABLE_REDUCED_FORM
144         memcpy(U1,V,sizeof(float)*Nrows*2*Nrows);
145         memcpy(V,U,sizeof(float)*Ncols*2*Ncols);
146         memcpy(U,U1,sizeof(float)*Nrows*2*Nrows);
147 #else
148         memcpy(U1,V,sizeof(float)*Ncols*2*Ncols);
149         memcpy(V,U,sizeof(float)*Nrows*2*Ncols);
150         memcpy(U,U1,sizeof(float)*Nrows*2*Ncols);
151 #endif
152   }
153   /* get Hermitian of diagonal entries */
154   for (row=0;row<Nrows;row++) {
155     diag[2*row+1]=-diag[2*row+1];
156   }
158   return status;
161 static int DSPF_sp_convert_to_bidiag_cmplx(const int Nrows,const int Ncols,float *restrict U,float *restrict V,float *restrict diag,float *restrict superdiag)
163   int i,j,k,addr;
164   float scale;
165   float x,y;
166   float s,inv_s,s2;
167   __float2_t *UU,*VV,*ddiag,*ssuperdiag;
168   __float2_t s_f2,s2_f2,si,half_norm_squared,inv_half_norm_squared,inv_scale,inv_UU_x_s,temp;
170   _nassert(Nrows>0);
171   _nassert(Ncols>0);
172   _nassert((int)U % 8 == 0);
173   _nassert((int)V % 8 == 0);
174   _nassert((int)diag % 8 == 0);
175   _nassert((int)superdiag % 8 == 0);
178   /* initialize __float2_t arrays to overlay input float arrays */
179   addr=(int)(&U[0]);
180   UU=(__float2_t *)(addr);
181   addr=(int)(&V[0]);
182   VV=(__float2_t *)(addr);
183   addr=(int)(&diag[0]);
184   ddiag=(__float2_t *)(addr);
185   addr=(int)(&superdiag[0]);
186   ssuperdiag=(__float2_t *)(addr);
188   /* Householder processing */
189   s=0;
190   scale=0;
191   for (i=0;i<Ncols;i++) {
192         /*superdiag[i]=scale*s; */
193         ssuperdiag[i]=real_to_f2(scale*s);
195         /* process columns */
196         scale=0;
197 #pragma MUST_ITERATE(1,,)
198         for (j=i;j<Nrows;j++) {
199           /* scale+=fabs(U[i+j*Ncols]; */
200           temp=_dmpysp(UU[i+j*Ncols],UU[i+j*Ncols]);
201           scale+=sqrt(_lof2(temp)+_hif2(temp));
202         }
203 if (scale>0) {
204           s2_f2=0;
205 #pragma MUST_ITERATE(1,,)
206           for (j=i;j<Nrows;j++) {
207                 /* U[i+j*Ncols]=U[i+j*Ncols]/scale; */
208 #ifndef ENABLE_NR
209                 inv_scale=1/scale;
210 #else
211             x=scale;
212         y=_rcpsp(x);
213         y=y*(2.0-x*y);
214         y=y*(2.0-x*y);
215         y=y*(2.0-x*y);
216         inv_scale=y*(2.0-x*y);
217 #endif
218                 UU[i+j*Ncols]=_dmpysp(UU[i+j*Ncols],_ftof2(inv_scale,inv_scale));
219                 /* s2+=U[i+j*Ncols]*conj(U[i+j*Ncols]); */
220                 s2_f2=_daddsp(s2_f2,ccmpy(UU[i+j*Ncols],UU[i+j*Ncols]));
221           }
222           s2=real(s2_f2)+imag(s2_f2);
223           if (real(UU[i+i*Ncols])<0) {
224                 s=sqrt(s2);
225           } else {
226         s=-sqrt(s2);
227           }
228           /* half_norm_squared=U[i+i*Ncols]*s-s2; */
229           half_norm_squared=_dsubsp(_dmpysp(UU[i+i*Ncols],_ftof2(s,s)),real_to_f2(s2));
231           /* inv_half_norm_squared=1/half_norm_squared */
232       complex_sp_inv(half_norm_squared,&inv_half_norm_squared);
234       /* U[i+i*Ncols]-=s; */
235           UU[i+i*Ncols]=_dsubsp(UU[i+i*Ncols],real_to_f2(s));
237           for (j=i+1;j<Ncols;j++) {
238                 si=zero;
239 #pragma MUST_ITERATE(1,,)
240                 for (k=i;k<Nrows;k++) {
241                   /* si+=conj(U[i+k*Ncols])*U[j+k*Ncols]; */
242                   si=_daddsp(si,ccmpy(UU[i+k*Ncols],UU[j+k*Ncols]));
243                 }
244         /* si=si/half_norm_squared; */
245                 si=cmpy(si,inv_half_norm_squared);
246 #pragma MUST_ITERATE(1,,)
247                 for (k=i;k<Nrows;k++) {
248                   /* U[j+k*Ncols]+=si*U[i+k*Ncols]; */
249                   UU[j+k*Ncols]=_daddsp(UU[j+k*Ncols],cmpy(si,UU[i+k*Ncols]));
250                 }
251           }
252         } /* if (scale>0) */
253 #pragma MUST_ITERATE(1,,)
254         for (j=i;j<Nrows;j++) {
255           /* U[i+j*Ncols]*=scale; */
256           UU[i+j*Ncols]=_dmpysp(UU[i+j*Ncols],_ftof2(scale,scale));
257         }
258         /* diag[i]=s*scale; */
259         ddiag[i]=real_to_f2(s*scale);
260         /* process rows */
261     s=0;
262     scale=0;
263     if ((i<Nrows)&&(i!=Ncols-1)) {
264       for (j=i+1;j<Ncols;j++) {
265         /* scale+=fabs(U[j+i*Ncols]); */
266         temp=_dmpysp(UU[j+i*Ncols],UU[j+i*Ncols]);
267         scale+=sqrt(_lof2(temp)+_hif2(temp));
268       }
269       if (scale>0) {
270         s2_f2=zero;
271 #ifndef ENABLE_NR
272         inv_scale=1/scale;
273 #else
274         x=scale;
275         y=_rcpsp(x);
276         y=y*(2.0-x*y);
277         y=y*(2.0-x*y);
278         y=y*(2.0-x*y);
279         inv_scale=y*(2.0-x*y);
280 #endif
281         for (j=i+1;j<Ncols;j++) {
282           /* U[j+i*Ncols]=U[j+i*Ncols]/scale; */
283           UU[j+i*Ncols]=_dmpysp(UU[j+i*Ncols],_ftof2(inv_scale,inv_scale));
284           /* s2+=U[j+i*Ncols]*conj(U[j+i*Ncols]); */
285           s2_f2=_daddsp(s2_f2,ccmpy(UU[j+i*Ncols],UU[j+i*Ncols]));
286         }
287             s2=real(s2_f2)+imag(s2_f2);
288         j--;
289         if (real(UU[j+i*Ncols])<0) {
290           s= sqrt(s2);
291         } else {
292           s=-sqrt(s2);
293         }
294         /* half_norm_squared=conj(U[i+1+i*Ncols])*s-s2; */
295             half_norm_squared=_dsubsp(_dmpysp(UU[i+1+i*Ncols],_ftof2(s,s)),real_to_f2(s2));
297             /* inv_half_norm_squared=1/half_norm_squared; */
298         complex_sp_inv(half_norm_squared,&inv_half_norm_squared);
300 #pragma MUST_ITERATE(1,,)
301         for (k=i+1;k<Ncols;k++) {
302           /* U[k+i*Ncols]=conj([U[k+i*Ncols]) */
303           U[2*k+1+i*2*Ncols]=-U[2*k+1+i*2*Ncols];
305           if (k==i+1) {
306                 UU[k+i*Ncols]=_dsubsp(UU[k+i*Ncols],real_to_f2(s));
307           }
309           /* superdiag[k]=conj(U[k+i*Ncols])/conj(half_norm_squared); */
310           ssuperdiag[k]=ccmpy(UU[k+i*Ncols],inv_half_norm_squared);
311         }
312         if (i<Nrows-1) {
313           for (j=i+1;j<Nrows;j++) {
314                 si=zero;
315 #pragma MUST_ITERATE(1,,)
316                 for (k=i+1;k<Ncols;k++) {
317                   /* si+=U[k+i*Ncols]*U[k+j*Ncols]; */
318                   si=_daddsp(si,cmpy(UU[k+i*Ncols],UU[k+j*Ncols]));
319                 }
320 #pragma MUST_ITERATE(1,,)
321                 for (k=i+1;k<Ncols;k++) {
322                   /* U[k+j*Ncols]+=si*superdiag[k]; */
323                   UU[k+j*Ncols]=_daddsp(UU[k+j*Ncols],cmpy(si,ssuperdiag[k]));
324                 }
325           }
326         }
327       } /* if (scale>0) */
328 #pragma MUST_ITERATE(1,,)
329       for (k=i+1;k<Ncols;k++) {
330         /* U[k+i*Ncols]*=scale; */
331         UU[k+i*Ncols]=_dmpysp(UU[k+i*Ncols],_ftof2(scale,scale));
332       }
333     } /* if ((i<Nrows)&&(i!=Ncols-1)) */
334   } /* for (i=0;i<Ncols;i++) */
336   /* update V */
337   /* V[Ncols*Ncols-1]=1; */
338   VV[Ncols*Ncols-1]=one;
339   /* s=superdiag[Ncols-1]; */
340   s_f2=ssuperdiag[Ncols-1];
341   for (i=Ncols-2;i>=0;i--) {
342         /* if (s!=0) { */
343           if(real(s_f2)!=0) {
345           /* inv=1/(conj(U[i+1+i*Ncols])*s); */
346           complex_sp_inv(ccmpy(UU[i+1+i*Ncols],s_f2),&temp);
348 #pragma MUST_ITERATE(1,,)
349           for (j=i+1;j<Ncols;j++) {
350                 /* V[i+j*Ncols]=U[j+i*Ncols]/(conj(U[i+1+i*Ncols])*s); */
351                 VV[i+j*Ncols]=cmpy(UU[j+i*Ncols],temp);
352           }
353           for (j=i+1;j<Ncols;j++) {
354             si=zero;
355 #pragma MUST_ITERATE(1,,)
356             for (k=i+1;k<Ncols;k++) {
357               /* si+=conj(U[k+i*Ncols])*V[j+k*Ncols]; */
358                   si=_daddsp(si,ccmpy(UU[k+i*Ncols],VV[j+k*Ncols]));
359             }
360 #pragma MUST_ITERATE(1,,)
361             for (k=i+1;k<Ncols;k++) {
362               /* V[j+k*Ncols]+=si*V[i+k*Ncols]; */
363                   VV[j+k*Ncols]=_daddsp(VV[j+k*Ncols],cmpy(si,VV[i+k*Ncols]));
364             }
365           }
366         } /* if (s!=0) */
367 #pragma MUST_ITERATE(1,,)
368         for (j=i+1;j<Ncols;j++) {
369           /* V[j+i*Ncols]=0; */
370           VV[j+i*Ncols]=zero;
371           /* V[i+j*Ncols]=0; */
372           VV[i+j*Ncols]=zero;
373         }
374         /* V[i+i*Ncols]=1; */
375         VV[i+i*Ncols]=one;
376         /* s=superdiag[i]; */
377         s_f2=ssuperdiag[i];
378   } /* for (i=Ncols-2;i>=0;i--) */
380 #ifndef ENABLE_REDUCED_FORM
381   /* expand U to from Nrows x Ncols to */
382   /*                  Nrows x Nrows    */
383   if (Nrows>Ncols) {
384         for (i=Nrows-1;i>=0;i--) {
385           for (j=Nrows-1;j>=0;j--) {
386                 if (j<=Ncols-1) {
387                 /* U[j+i*Nrows]=U[j+i*Ncols]; */
388                   UU[j+i*Nrows]=UU[j+i*Ncols];
389                 } else {
390                   /* U[j+i*Nrows]=0; */
391                   UU[j+i*Nrows]=zero;
392                 }
393           }
394         }
395   }
397   /* update U */
398   for (i=Ncols-1;i>=0;i--) {
400         /* s=diag[i]; */
401         s=real(ddiag[i]);
403 #ifndef ENABLE_NR
404     inv_s=1/s;
405 #else
406     inv_s=_rcpsp(s);
407     inv_s=inv_s*(2.0-s*inv_s);
408     inv_s=inv_s*(2.0-s*inv_s);
409     inv_s=inv_s*(2.0-s*inv_s);
410 #endif
412     /* inv_UU=1/(conj(UU[i+i*Nrows])*s); */
413     complex_sp_inv(_dmpysp(UU[i+i*Nrows],_ftof2(s,s)),&inv_UU_x_s);
415 #pragma MUST_ITERATE(1,,)
416         for (j=i+1;j<Ncols;j++) {
417           /* U[j+i*Nrows]=0; */
418           UU[j+i*Nrows]=zero;
419         }
420     /* if (s!=0) { */
421         if (fabs(s)>ZERO_TOLERANCE) {
423       for (j=i+1;j<Nrows;j++) {
424         /* si=0; */
425         si=zero;
426 #pragma MUST_ITERATE(1,,)
427         for (k=i+1;k<Nrows;k++) {
428           /* si+=conj(U[i+k*Nrows])*U[j+k*Nrows]; */
429           si=_daddsp(si,ccmpy(UU[i+k*Nrows],UU[j+k*Nrows]));
430         }
431         /* si=si/(conj(U[i+i*Nrows])*s); */
432         si=ccmpy(inv_UU_x_s,si);
434         #pragma MUST_ITERATE(1,,)
435         for (k=i;k<Nrows;k++) {
436           /* U[j+k*Nrows]+=si*U[i+k*Nrows]; */
437           UU[j+k*Nrows]=_daddsp(UU[j+k*Nrows],cmpy(si,UU[i+k*Nrows]));
438         }
439       }
440       /* initial U1 */
441       if (i==Ncols-1) {
442         for (j=i;j<Nrows;j++) {
443           for (k=Nrows-1;k>=i+1;k--) {
444             /* U[k+j*Nrows]=U[i+j*Nrows]*conj(U[i+k*Nrows])/(conj(U[i+i*Nrows])*s); */
445                 UU[k+j*Nrows]=ccmpy(inv_UU_x_s,ccmpy(UU[i+k*Nrows],UU[i+j*Nrows]));
446             if (j==k) {
447               /* U[k+j*Nrows]+=1; */
448               UU[k+j*Nrows]=_daddsp(UU[k+j*Nrows],one);
449             }
450           }
451         }
452       }
453 #pragma MUST_ITERATE(1,,)
454       for (j=i;j<Nrows;j++) {
455         /* U[i+j*Nrows]=U[i+j*Nrows]/s; */
456         UU[i+j*Nrows]=_dmpysp(UU[i+j*Nrows],_ftof2(inv_s,inv_s));
457       }
458     } else { /* if (s!=0) */
459       if (i==Ncols-1) {
460 #pragma MUST_ITERATE(1,,)
461         for (k=1;k<=Nrows-Ncols;k++) {
462           /* U[i+k+(i+k)*Nrows]=1; */
463                 UU[i+k+(i+k)*Nrows]=one;
464         }
465       }
466 #pragma MUST_ITERATE(1,,)
467       for (j=i;j<Nrows;j++) {
468         /* U[i+j*Nrows]=0; */
469         UU[i+j*Nrows]=zero;
470       }
471     } /* if (s!=0) */
472     /* U[i+i*Nrows]+=1; */
473         UU[i+i*Nrows]=_daddsp(UU[i+i*Nrows],one);
474   } /* for (i=Ncols-1;i>=0;i--) */
475 #else /* #ifndef ENABLE_REDUCED_FORM */
476   /* update U */
477   for (i=Ncols-1;i>=0;i--) {
479         /* s=diag[i]; */
480         s=real(ddiag[i]);
481 #ifndef ENABLE_NR
482     inv_s=1/s;
483 #else
484     inv_s=_rcpsp(s);
485     inv_s=inv_s*(2.0-s*inv_s);
486     inv_s=inv_s*(2.0-s*inv_s);
487     inv_s=inv_s*(2.0-s*inv_s);
488 #endif
490     /* inv_UU=1/(conj(UU[i+i*Ncols])*s); */
491     complex_sp_inv(_dmpysp(UU[i+i*Ncols],_ftof2(s,s)),&inv_UU_x_s);
493 #pragma MUST_ITERATE(1,,)
494         for (j=i+1;j<Ncols;j++) {
495           /* U[j+i*Nrows]=0; */
496           UU[j+i*Ncols]=zero;
497         }
498     /* if (s!=0) { */
499         if (fabs(s)>ZERO_TOLERANCE) {
501       for (j=i+1;j<Ncols;j++) {
502         /* si=0; */
503         si=zero;
504 #pragma MUST_ITERATE(1,,)
505         for (k=i+1;k<Nrows;k++) {
506           /* si+=conj(U[i+k*Ncols])*U[j+k*Ncols]; */
507           si=_daddsp(si,ccmpy(UU[i+k*Ncols],UU[j+k*Ncols]));
508         }
509         /* si=si/(conj(U[i+i*Ncols])*s); */
510         si=ccmpy(inv_UU_x_s,si);
512         #pragma MUST_ITERATE(1,,)
513         for (k=i;k<Nrows;k++) {
514           /* U[j+k*Ncols]+=si*U[i+k*Ncols]; */
515           UU[j+k*Ncols]=_daddsp(UU[j+k*Ncols],cmpy(si,UU[i+k*Ncols]));
516         }
517       }
518 #pragma MUST_ITERATE(1,,)
519        for (j=i;j<Nrows;j++) {
520         /* U[i+j*Ncols]=U[i+j*Ncols]/s; */
521         UU[i+j*Ncols]=_dmpysp(UU[i+j*Ncols],_ftof2(inv_s,inv_s));
522       }
523     } else { /* if (s!=0) */
524 #pragma MUST_ITERATE(1,,)
525       for (j=i;j<Nrows;j++) {
526         /* U[i+j*Ncols]=0; */
527         UU[i+j*Ncols]=zero;
528       }
529     } /* if (s!=0) */
530     /* U[i+i*Ncols]+=1; */
531         UU[i+i*Ncols]=_daddsp(UU[i+i*Ncols],one);
532   } /* for (i=Ncols-1;i>=0;i--)
533   */
534 #endif
536   return 0;
539 static int DSPF_sp_bidiag_to_diag_cmplx(const int Nrows,const int Ncols,float *restrict U,float *restrict V,float *restrict diag,float *restrict superdiag)
542   int row,i,k,m,rotation_test,iter,total_iter,addr;
543   float xx,yy,epsilon;
544   __float2_t *UU,*VV,*ddiag,*ssuperdiag;
545   __float2_t temp,c,s,f,g,h,x,y,z,num,den;
547   _nassert(Nrows>0);
548   _nassert(Ncols>0);
549   _nassert((int)U % 8 == 0);
550   _nassert((int)V % 8 == 0);
551   _nassert((int)diag % 8 == 0);
552   _nassert((int)superdiag % 8 == 0);
554   /* initialize __float2_t arrays to overlay input float arrays */
555   addr=(int)(&U[0]);
556   UU=(__float2_t *)(addr);
557   addr=(int)(&V[0]);
558   VV=(__float2_t *)(addr);
559   addr=(int)(&diag[0]);
560   ddiag=(__float2_t *)(addr);
561   addr=(int)(&superdiag[0]);
562   ssuperdiag=(__float2_t *)(addr);
564   iter=0;
565   total_iter=0;
566   /* ------------------------------------------------------------------- */
567   /* find max in col                                                     */
568   /* ------------------------------------------------------------------- */
569   xx=0;
570 #pragma MUST_ITERATE(1,,)
571   for (i=0;i<Ncols;i++) {
572         /* y=fabs(diag[i])+fabs(superdiag[i]); */
573         temp=_dmpysp(ddiag[i],ddiag[i]);
574         yy=sqrt(_lof2(temp)+_hif2(temp));
575         temp=_dmpysp(ssuperdiag[i],ssuperdiag[i]);
576         yy+=sqrt(_lof2(temp)+_hif2(temp));
577         if (xx<yy) {
578           xx=yy;
579         }
580   }
581   epsilon=FLT_EPSILON*xx;
582 #pragma MUST_ITERATE(1,,)
583   for (k=Ncols-1;k>=0;k--) {
584         total_iter+=iter;
585         iter=0;
586 #pragma MUST_ITERATE(1,,)
587     while (1==1) {
588       rotation_test=1;
589 #pragma MUST_ITERATE(1,,)
590       for (m=k;m>=0;m--) {
591         /* if (fabs(superdiag[m])<=epsilon) { */
592         temp=_dmpysp(ssuperdiag[m],ssuperdiag[m]);
593         if (sqrt(_lof2(temp)+_hif2(temp))<=epsilon) {
594           rotation_test=0;
595           break;
596         }
597         /* if (fabs(diag[m-1])<=epsilon) { */
598         temp=_dmpysp(ddiag[m-1],ddiag[m-1]);
599         if (sqrt(_lof2(temp)+_hif2(temp))<=epsilon) {
600           break;
601         }
602       } /* for (m=k;m>=0;m--) */
603       if (rotation_test) {
604         /* c=0; */
605         /* s=1; */
606         c=zero;
607         s=one;
608 #pragma MUST_ITERATE(1,,)
609         for (i=m;i<=k;i++) {
610           /* f=s*superdiag[i]; */
611           f=cmpy(s,ssuperdiag[i]);
612           /* superdiag[i]=c*superdiag[i]; */
613           ssuperdiag[i]=cmpy(c,ssuperdiag[i]);
615           /* if (fabs(f)<=epsilon) { */
616           temp=_dmpysp(f,f);
617           if (sqrt(_lof2(temp)+_hif2(temp))<=epsilon) {
618                 break;
619           }
621           /* g=diag[i]; */
622           g=ddiag[i];
624           /* h=sqrt(f*f+g*g); */
625           temp=_daddsp(cmpy(f,f),cmpy(g,g));
626           complex_sp_sqrt(temp,&h);
628           /* diag[i]=h; */
629           ddiag[i]=h;
631           /* c=g/h; */
632           complex_sp_div(g,h,&c);
633           /* s=-f/h; */
634           complex_sp_div(f,h,&s);
635           s=_dsubsp(zero,s);
637 #ifndef ENABLE_REDUCED_FORM
638 #pragma MUST_ITERATE(1,,)
639           for (row=0;row<Nrows;row++) {
640                 /* y=U[m-1+row*Nrows]; */
641                 y=UU[m-1+row*Nrows];
642                 /* z=U[i+row*Nrows]; */
643                 z=UU[i+row*Nrows];
644                 /* U[m-1+row*Nrows]= y*c+z*s; */
645                 UU[m-1+row*Nrows] = _daddsp(cmpy(y,c),cmpy(z,s));
646                 /* U[i+row*Nrows]  =-y*s+z*c; */
647                 UU[i+row*Nrows] =  _dsubsp(cmpy(z,c),cmpy(y,s));
648           }
649 #else
650 #pragma MUST_ITERATE(1,,)
651           for (row=0;row<Nrows;row++) {
652                 /* y=U[m-1+row*Ncols]; */
653                 y=UU[m-1+row*Ncols];
654                 /* z=U[i+row*Ncols]; */
655                 z=UU[i+row*Ncols];
656                 /* U[m-1+row*Ncols]= y*c+z*s; */
657                 UU[m-1+row*Ncols]=_daddsp(cmpy(y,c),cmpy(z,s));
658                 /* U[i+row*Ncols]  =-y*s+z*c; */
659                 UU[i+row*Ncols]=_dsubsp(cmpy(z,c),cmpy(y,s));
660           }
661 #endif
662         } /* for (i=m;i<=k;i++) */
663       } /* if (rotation_test) */
664       /* z=diag[k]; */
665       z=ddiag[k];
667       if (m==k) {
668         /* if (z<0) { */
669         if (real(z)<0) {
670           /* diag[k]=-z; */
671           ddiag[k]=_dsubsp(zero,z);
672 #pragma MUST_ITERATE(1,,)
673           for (row=0;row<Ncols;row++) {
674                 /* V[k+row*Ncols]=-V[k+row*Ncols]; */
675                 VV[k+row*Ncols]=_dsubsp(zero,VV[k+row*Ncols]);
676           }
677         } /* if (z>0) */
678         break;
679       } else { /* if (m==k) */
680         if (iter>=MAX_ITERATION_COUNT) {
681           return -1;
682         }
683         iter++;
684         /* x=diag[m]; */
685         x=ddiag[m];
686         /* y=diag[k-1]; */
687         y=ddiag[k-1];
688         /* g=superdiag[k-1]; */
689         g=ssuperdiag[k-1];
690         /* h=superdiag[k]; */
691         h=ssuperdiag[k];
692         /* f=((y-z)*(y+z)+(g-h)*(g+h))/(2*h*y); */
693         num=_daddsp(cmpy(_dsubsp(y,z),_daddsp(y,z)),cmpy(_dsubsp(g,h),_daddsp(g,h)));
694         den=_dmpysp(_ftof2(2,2),cmpy(h,y));
695         complex_sp_div(num,den,&f);
697         /* g=sqrt(f*f+1); */
698         temp=_daddsp(cmpy(f,f),_ftof2(1,0));
699         complex_sp_sqrt(temp,&g);
701         /* if (f<0) { */
702         if (real(f)<0) {
703           /* g=-g; */
704           g=_dsubsp(zero,g);
705         }
707         /* f=((x-z)*(x+z)+h*(y/(f+g)-h))/x; */
708         num=y;
709         den=_daddsp(f,g);
710         complex_sp_div(num,den,&temp);
711         temp=_dsubsp(temp,h);
712         temp=cmpy(h,temp);
713         num=_daddsp(cmpy(_dsubsp(x,z),_daddsp(x,z)),temp);
714         complex_sp_div(num,x,&f);
716         /* next QR transformation */
717         /* c=1; */
718         /* s=1; */
719         c=one;
720         s=one;
721 #pragma MUST_ITERATE(1,,)
722         for (i=m+1;i<=k;i++) {
723           /* g=superdiag[i]; */
724           g=ssuperdiag[i];
726           /* y=diag[i]; */
727           y=ddiag[i];
729           /* h=s*g; */
730           h=cmpy(s,g);
732           /* g=g*c; */
733           g=cmpy(g,c);
735           /* z=sqrt(f*f+h*h); */
736           temp=_daddsp(cmpy(f,f),cmpy(h,h));
737           complex_sp_sqrt(temp,&z);
739           /* superdiag[i-1]=z; */
740           ssuperdiag[i-1]=z;
742           /* c=f/z; */
743           complex_sp_div(f,z,&c);
745           /* s=h/z; */
746           complex_sp_div(h,z,&s);
748           /* f= x*c+g*s; */
749           f=_daddsp(cmpy(x,c),cmpy(g,s));
751           /* g=-x*s+g*c; */
752           g=_dsubsp(cmpy(g,c),cmpy(x,s));
754           /* h=y*s; */
755           h=cmpy(y,s);
757           /* y=c*y; */
758           y=cmpy(c,y);
760           #pragma MUST_ITERATE(1,,)
761           for (row=0;row<Ncols;row++) {
762                 /* x=V[i-1+row*Ncols]; */
763                 x=VV[i-1+row*Ncols];
765                 /* z=V[i+row*Ncols]; */
766                 z=VV[i+row*Ncols];
768                 /* V[i-1+row*Ncols]= x*c+z*s; */
769                 VV[i-1+row*Ncols]=_daddsp(cmpy(x,c),cmpy(z,s));
771                 /* V[i+row*Ncols]  =-x*s+z*c; */
772                 VV[i+row*Ncols]=_dsubsp(cmpy(z,c),cmpy(x,s));
773           }
775           /* z=sqrt(f*f+h*h); */
776           temp=_daddsp(cmpy(f,f),cmpy(h,h));
777           complex_sp_sqrt(temp,&z);
779           /* diag[i-1]=z; */
780           ddiag[i-1]=z;
782           /* if (z!=0) { */
783           temp=_dmpysp(z,z);
784           if (sqrt(_lof2(temp)+_hif2(temp))>ZERO_TOLERANCE) {
786                 /* c=f/z; */
787                 complex_sp_div(f,z,&c);
789                 /* s=h/z; */
790                 complex_sp_div(h,z,&s);
791           }
793           /* f= c*g+s*y; */
794           f=_daddsp(cmpy(c,g),cmpy(s,y));
796           /* x=-s*g+c*y; */
797           x=_dsubsp(cmpy(c,y),cmpy(s,g));
798 #ifndef ENABLE_REDUCED_FORM
799 #pragma MUST_ITERATE(1,,)
800           for (row=0;row<Nrows;row++) {
801                 /* y=U[i-1+row*Nrows]; */
802             y=UU[i-1+row*Nrows];
804             /* z=U[i+row*Nrows]; */
805             z=UU[i+row*Nrows];
807             /* U[i-1+row*Nrows]= c*y+s*z; */
808             UU[i-1+row*Nrows]=_daddsp(cmpy(c,y),cmpy(s,z));
810             /* U[i+row*Nrows]  =-s*y+c*z; */
811             UU[i+row*Nrows]=_dsubsp(cmpy(c,z),cmpy(s,y));
812           }
813 #else
814 #pragma MUST_ITERATE(1,,)
815           for (row=0;row<Nrows;row++) {
816                 /* y=U[i-1+row*Ncols]; */
817                 y=UU[i-1+row*Ncols];
818                 /* z=U[i+row*Ncols]; */
819                 z=UU[i+row*Ncols];
820                 /* U[i-1+row*Ncols]= c*y+s*z; */
821                 UU[i-1+row*Ncols]=_daddsp(cmpy(c,y),cmpy(s,z));
822                 /* U[i+row*Ncols]  =-s*y+c*z; */
823                 UU[i+row*Ncols]=_dsubsp(cmpy(c,z),cmpy(s,y));
824           }
825 #endif
826         } /* for (i=m+1;i<=k;i++) */
828         /* superdiag[m]=0; */
829         ssuperdiag[m]=zero;
831         /* superdiag[k]=f; */
832         ssuperdiag[k]=f;
834         /* diag[k]=x; */
835         ddiag[k]=x;
837       } /* if (m==k) */
838     } /* while (1==1) */
839   } /* for (k=Ncols-1:k>=0;k--) */
841   return total_iter;
844 static int DSPF_sp_sort_singular_values_cmplx(const int Nrows,const int Ncols,float *restrict U,float *restrict V,float *restrict singular_values)
846   int i,j,row,max_index,addr;
847   float temp1,temp2;
848   __float2_t *UU,*VV,*ssingular_values;
849   __float2_t temp;
851   _nassert(Nrows>0);
852   _nassert(Ncols>0);
853   _nassert((int)U % 8 == 0);
854   _nassert((int)V % 8 == 0);
855   _nassert((int)singular_values % 8 == 0);
857   /* initialize __float2_t arrays to overlay input float arrays */
858   addr=(int)(&U[0]);
859   UU=(__float2_t *)(addr);
860   addr=(int)(&V[0]);
861   VV=(__float2_t *)(addr);
862   addr=(int)(&singular_values[0]);
863   ssingular_values=(__float2_t *)(addr);
865   #pragma MUST_ITERATE(1,,)
866   for (i=0;i<Ncols-1;i++) {
867         max_index=i;
868 #pragma MUST_ITERATE(1,,)
869         for (j=i+1;j<Ncols;j++) {
870           /* (if (singular_values[j]>singular_values[max_index]) { */
871           temp=_dmpysp(ssingular_values[j],ssingular_values[j]);
872           temp1=sqrt(_lof2(temp)+_hif2(temp));
873           temp=_dmpysp(ssingular_values[max_index],ssingular_values[max_index]);
874           temp2=sqrt(_lof2(temp)+_hif2(temp));
875           if (temp1>temp2) {
876                 max_index=j;
877           }
878         }
879         if (max_index!=i) {
880           temp=ssingular_values[i];
881           ssingular_values[i]=ssingular_values[max_index];
882           ssingular_values[max_index]=temp;
883 #ifndef ENABLE_REDUCED_FORM
884 #pragma MUST_ITERATE(1,,)
885           for (row=0;row<Nrows;row++) {
886                 temp=UU[max_index+row*Nrows];
887                 UU[max_index+row*Nrows]=UU[i+row*Nrows];
888                 UU[i+row*Nrows]=temp;
889           }
890 #else
891 #pragma MUST_ITERATE(1,,)
892           for (row=0;row<Nrows;row++) {
893                 temp=UU[max_index+row*Ncols];
894                 UU[max_index+row*Ncols]=UU[i+row*Ncols];
895                 UU[i+row*Ncols]=temp;
896           }
897 #endif
898 #pragma MUST_ITERATE(1,,)
899           for (row=0;row<Ncols;row++) {
900                 temp=VV[max_index+row*Ncols];
901                 VV[max_index+row*Ncols]=VV[i+row*Ncols];
902                 VV[i+row*Ncols]=temp;
903           }
904         }
905   }
907   return 0;
910 static void complex_sp_sqrt(__float2_t cx,__float2_t *cz) {
912   float xmag,zmag,xreal,ximag,zreal,zimag;
913   float x_angle,z_angle;
914   __float2_t ctemp;
916   /* magnitude */
917   ctemp=_dmpysp(cx,cx);
918   xmag=sqrt(_lof2(ctemp)+_hif2(ctemp));
919   zmag=sqrt(xmag);
921   /* angle */
922 #ifndef _LITTLE_ENDIAN
923   xreal=_hif2(cx);
924   ximag=_lof2(cx);
925 #else
926   xreal=_lof2(cx);
927   ximag=_hif2(cx);
928 #endif
929 #ifndef ENABLE_MATHLIB
930   x_angle=atan2(ximag,xreal);
931 #else
932   x_angle=atan2sp_c(ximag,xreal);
933 #endif
934   z_angle=x_angle/2;
936   /* results */
937 #ifndef ENABLE_MATHLIB
938   zreal=cos(z_angle)*zmag;
939   zimag=sin(z_angle)*zmag;
940 #else
941   zreal=cossp(z_angle)*zmag;
942   zimag=sinsp(z_angle)*zmag;
943 #endif
944 #ifndef _LITTLE_ENDIAN
945   *cz=_ftof2(zreal,zimag);
946 #else
947   *cz=_ftof2(zimag,zreal);
948 #endif
951 static void complex_sp_inv(__float2_t cy,__float2_t *cz) {
953   float inv_mag_sq,yreal,yimag,zreal,zimag;
954 #ifdef ENABLE_NR
955   float x,y;
956 #endif
958 #ifndef _LITTLE_ENDIAN
959   yreal=_hif2(cy);
960   yimag=_lof2(cy);
961 #else
962   yreal=_lof2(cy);
963   yimag=_hif2(cy);
964 #endif
966 #ifndef ENABLE_NR
967   inv_mag_sq=1/(yreal*yreal+yimag*yimag);
968 #else
969   x=yreal*yreal+yimag*yimag;
970   y=_rcpsp(x);
971   y=y*(2.0-x*y);
972   y=y*(2.0-x*y);
973   inv_mag_sq=y;
974 #endif
976   /* results */
977   zreal= yreal*inv_mag_sq;
978   zimag=-yimag*inv_mag_sq;
979 #ifndef _LITTLE_ENDIAN
980   *cz=_ftof2(zreal,zimag);
981 #else
982   *cz=_ftof2(zimag,zreal);
983 #endif
987 static void complex_sp_div(__float2_t cx,__float2_t cy,__float2_t *cz) {
989   float xreal,ximag,yreal,yimag,zreal,zimag;
990   float inv_mag_sq;
991 #ifdef ENABLE_NR
992   float x,y;
993 #endif
995 #ifndef _LITTLE_ENDIAN
996   xreal=_hif2(cx);
997   ximag=_lof2(cx);
998   yreal=_hif2(cy);
999   yimag=_lof2(cy);
1000 #else
1001   xreal=_lof2(cx);
1002   ximag=_hif2(cx);
1003   yreal=_lof2(cy);
1004   yimag=_hif2(cy);
1005 #endif
1006 #ifndef ENABLE_NR
1007   inv_mag_sq=1/(yreal*yreal+yimag*yimag);
1008 #else
1009   x=yreal*yreal+yimag*yimag;
1010   y=_rcpsp(x);
1011   y=y*(2.0-x*y);
1012   y=y*(2.0-x*y);
1013   inv_mag_sq=y;
1014 #endif
1016   /* results */
1017   zreal=(xreal*yreal+ximag*yimag)*inv_mag_sq;
1018   zimag=(ximag*yreal-xreal*yimag)*inv_mag_sq;
1019 #ifndef _LITTLE_ENDIAN
1020   *cz=_ftof2(zreal,zimag);
1021 #else
1022   *cz=_ftof2(zimag,zreal);
1023 #endif
1026 /* ======================================================================= */
1027 /*  End of file:  DSPF_sp_svd_cmplx.c                                      */
1028 /* ----------------------------------------------------------------------- */
1029 /*            Copyright (c) 2013 Texas Instruments, Incorporated.          */
1030 /*                           All Rights Reserved.                          */
1031 /* ======================================================================= */