Getting blender to compile for IRIX, in particular:
[blender.git] / intern / iksolver / intern / TNT / svd.h
1 /**
2  * $Id$
3  */
4
5 #ifndef SVD_H
6
7 #define SVD_H
8
9 // Compute the Single Value Decomposition of an arbitrary matrix A
10 // That is compute the 3 matrices U,W,V with U column orthogonal (m,n) 
11 // ,W a diagonal matrix and V an orthogonal square matrix s.t. 
12 // A = U.W.Vt. From this decomposition it is trivial to compute the 
13 // inverse of A as Ainv = V.Winv.tranpose(U).
14 //
15 // s = diagonal elements of W
16 // work1 = workspace, length must be A.num_rows
17 // work2 = workspace, length must be A.num_cols
18
19 #include "tntmath.h"
20
21 namespace TNT
22 {
23
24 template <class MaTRiX, class VecToR >
25 void SVD(MaTRiX &A, MaTRiX &U, VecToR &s, MaTRiX &V, VecToR &work1, VecToR &work2) {
26
27         int m = A.num_rows();
28         int n = A.num_cols();
29         int nu = min(m,n);
30
31         VecToR& work = work1;
32         VecToR& e = work2;
33
34         U = 0;
35         s = 0;
36
37         int i=0, j=0, k=0;
38
39         // Reduce A to bidiagonal form, storing the diagonal elements
40         // in s and the super-diagonal elements in e.
41
42         int nct = min(m-1,n);
43         int nrt = max(0,min(n-2,m));
44         for (k = 0; k < max(nct,nrt); k++) {
45                 if (k < nct) {
46
47                         // Compute the transformation for the k-th column and
48                         // place the k-th diagonal in s[k].
49                         // Compute 2-norm of k-th column without under/overflow.
50                         s[k] = 0;
51                         for (i = k; i < m; i++) {
52                                 s[k] = hypot(s[k],A[i][k]);
53                         }
54                         if (s[k] != 0.0) {
55                                 if (A[k][k] < 0.0) {
56                                         s[k] = -s[k];
57                                 }
58                                 for (i = k; i < m; i++) {
59                                         A[i][k] /= s[k];
60                                 }
61                                 A[k][k] += 1.0;
62                         }
63                         s[k] = -s[k];
64                 }
65                 for (j = k+1; j < n; j++) {
66                         if ((k < nct) && (s[k] != 0.0))  {
67
68                         // Apply the transformation.
69
70                                 typename MaTRiX::value_type t = 0;
71                                 for (i = k; i < m; i++) {
72                                         t += A[i][k]*A[i][j];
73                                 }
74                                 t = -t/A[k][k];
75                                 for (i = k; i < m; i++) {
76                                         A[i][j] += t*A[i][k];
77                                 }
78                         }
79
80                         // Place the k-th row of A into e for the
81                         // subsequent calculation of the row transformation.
82
83                         e[j] = A[k][j];
84                 }
85                 if (k < nct) {
86
87                         // Place the transformation in U for subsequent back
88                         // multiplication.
89
90                         for (i = k; i < m; i++)
91                                 U[i][k] = A[i][k];
92                 }
93                 if (k < nrt) {
94
95                         // Compute the k-th row transformation and place the
96                         // k-th super-diagonal in e[k].
97                         // Compute 2-norm without under/overflow.
98                         e[k] = 0;
99                         for (i = k+1; i < n; i++) {
100                                 e[k] = hypot(e[k],e[i]);
101                         }
102                         if (e[k] != 0.0) {
103                                 if (e[k+1] < 0.0) {
104                                         e[k] = -e[k];
105                                 }
106                                 for (i = k+1; i < n; i++) {
107                                         e[i] /= e[k];
108                                 }
109                                 e[k+1] += 1.0;
110                         }
111                         e[k] = -e[k];
112                         if ((k+1 < m) & (e[k] != 0.0)) {
113
114                         // Apply the transformation.
115
116                                 for (i = k+1; i < m; i++) {
117                                         work[i] = 0.0;
118                                 }
119                                 for (j = k+1; j < n; j++) {
120                                         for (i = k+1; i < m; i++) {
121                                                 work[i] += e[j]*A[i][j];
122                                         }
123                                 }
124                                 for (j = k+1; j < n; j++) {
125                                         typename MaTRiX::value_type t = -e[j]/e[k+1];
126                                         for (i = k+1; i < m; i++) {
127                                                 A[i][j] += t*work[i];
128                                         }
129                                 }
130                         }
131
132                         // Place the transformation in V for subsequent
133                         // back multiplication.
134
135                         for (i = k+1; i < n; i++)
136                                 V[i][k] = e[i];
137                 }
138         }
139
140         // Set up the final bidiagonal matrix or order p.
141
142         int p = min(n,m+1);
143         if (nct < n) {
144                 s[nct] = A[nct][nct];
145         }
146         if (m < p) {
147                 s[p-1] = 0.0;
148         }
149         if (nrt+1 < p) {
150                 e[nrt] = A[nrt][p-1];
151         }
152         e[p-1] = 0.0;
153
154         // If required, generate U.
155
156         for (j = nct; j < nu; j++) {
157                 for (i = 0; i < m; i++) {
158                         U[i][j] = 0.0;
159                 }
160                 U[j][j] = 1.0;
161         }
162         for (k = nct-1; k >= 0; k--) {
163                 if (s[k] != 0.0) {
164                         for (j = k+1; j < nu; j++) {
165                                 typename MaTRiX::value_type t = 0;
166                                 for (i = k; i < m; i++) {
167                                         t += U[i][k]*U[i][j];
168                                 }
169                                 t = -t/U[k][k];
170                                 for (i = k; i < m; i++) {
171                                         U[i][j] += t*U[i][k];
172                                 }
173                         }
174                         for (i = k; i < m; i++ ) {
175                                 U[i][k] = -U[i][k];
176                         }
177                         U[k][k] = 1.0 + U[k][k];
178                         for (i = 0; i < k-1; i++) {
179                                 U[i][k] = 0.0;
180                         }
181                 } else {
182                         for (i = 0; i < m; i++) {
183                                 U[i][k] = 0.0;
184                         }
185                         U[k][k] = 1.0;
186                 }
187         }
188
189         // If required, generate V.
190
191         for (k = n-1; k >= 0; k--) {
192                 if ((k < nrt) & (e[k] != 0.0)) {
193                         for (j = k+1; j < nu; j++) {
194                                 typename MaTRiX::value_type t = 0;
195                                 for (i = k+1; i < n; i++) {
196                                         t += V[i][k]*V[i][j];
197                                 }
198                                 t = -t/V[k+1][k];
199                                 for (i = k+1; i < n; i++) {
200                                         V[i][j] += t*V[i][k];
201                                 }
202                         }
203                 }
204                 for (i = 0; i < n; i++) {
205                         V[i][k] = 0.0;
206                 }
207                 V[k][k] = 1.0;
208         }
209
210         // Main iteration loop for the singular values.
211
212         int pp = p-1;
213         int iter = 0;
214         typename MaTRiX::value_type eps = pow(2.0,-52.0);
215         while (p > 0) {
216                 int kase=0;
217                 k=0;
218
219                 // Here is where a test for too many iterations would go.
220
221                 // This section of the program inspects for
222                 // negligible elements in the s and e arrays.  On
223                 // completion the variables kase and k are set as follows.
224
225                 // kase = 1       if s(p) and e[k-1] are negligible and k<p
226                 // kase = 2       if s(k) is negligible and k<p
227                 // kase = 3       if e[k-1] is negligible, k<p, and
228                 //                                s(k), ..., s(p) are not negligible (qr step).
229                 // kase = 4       if e(p-1) is negligible (convergence).
230
231                 for (k = p-2; k >= -1; k--) {
232                         if (k == -1) {
233                                 break;
234                         }
235                         if (TNT::abs(e[k]) <= eps*(TNT::abs(s[k]) + TNT::abs(s[k+1]))) {
236                                 e[k] = 0.0;
237                                 break;
238                         }
239                 }
240                 if (k == p-2) {
241                         kase = 4;
242                 } else {
243                         int ks;
244                         for (ks = p-1; ks >= k; ks--) {
245                                 if (ks == k) {
246                                         break;
247                                 }
248                                 typename MaTRiX::value_type t = (ks != p ? TNT::abs(e[ks]) : 0.) + 
249                                                           (ks != k+1 ? TNT::abs(e[ks-1]) : 0.);
250                                 if (TNT::abs(s[ks]) <= eps*t)  {
251                                         s[ks] = 0.0;
252                                         break;
253                                 }
254                         }
255                         if (ks == k) {
256                                 kase = 3;
257                         } else if (ks == p-1) {
258                                 kase = 1;
259                         } else {
260                                 kase = 2;
261                                 k = ks;
262                         }
263                 }
264                 k++;
265
266                 // Perform the task indicated by kase.
267
268                 switch (kase) {
269
270                         // Deflate negligible s(p).
271
272                         case 1: {
273                                 typename MaTRiX::value_type f = e[p-2];
274                                 e[p-2] = 0.0;
275                                 for (j = p-2; j >= k; j--) {
276                                         typename MaTRiX::value_type t = hypot(s[j],f);
277                                         typename MaTRiX::value_type cs = s[j]/t;
278                                         typename MaTRiX::value_type sn = f/t;
279                                         s[j] = t;
280                                         if (j != k) {
281                                                 f = -sn*e[j-1];
282                                                 e[j-1] = cs*e[j-1];
283                                         }
284
285                                         for (i = 0; i < n; i++) {
286                                                 t = cs*V[i][j] + sn*V[i][p-1];
287                                                 V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1];
288                                                 V[i][j] = t;
289                                         }
290                                 }
291                         }
292                         break;
293
294                         // Split at negligible s(k).
295
296                         case 2: {
297                                 typename MaTRiX::value_type f = e[k-1];
298                                 e[k-1] = 0.0;
299                                 for (j = k; j < p; j++) {
300                                         typename MaTRiX::value_type t = hypot(s[j],f);
301                                         typename MaTRiX::value_type cs = s[j]/t;
302                                         typename MaTRiX::value_type sn = f/t;
303                                         s[j] = t;
304                                         f = -sn*e[j];
305                                         e[j] = cs*e[j];
306
307                                         for (i = 0; i < m; i++) {
308                                                 t = cs*U[i][j] + sn*U[i][k-1];
309                                                 U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1];
310                                                 U[i][j] = t;
311                                         }
312                                 }
313                         }
314                         break;
315
316                         // Perform one qr step.
317
318                         case 3: {
319
320                                 // Calculate the shift.
321
322                                 typename MaTRiX::value_type scale = max(max(max(max(
323                                                   TNT::abs(s[p-1]),TNT::abs(s[p-2])),TNT::abs(e[p-2])), 
324                                                   TNT::abs(s[k])),TNT::abs(e[k]));
325                                 typename MaTRiX::value_type sp = s[p-1]/scale;
326                                 typename MaTRiX::value_type spm1 = s[p-2]/scale;
327                                 typename MaTRiX::value_type epm1 = e[p-2]/scale;
328                                 typename MaTRiX::value_type sk = s[k]/scale;
329                                 typename MaTRiX::value_type ek = e[k]/scale;
330                                 typename MaTRiX::value_type b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0;
331                                 typename MaTRiX::value_type c = (sp*epm1)*(sp*epm1);
332                                 typename MaTRiX::value_type shift = 0.0;
333                                 if ((b != 0.0) || (c != 0.0)) {
334                                         shift = sqrt(b*b + c);
335                                         if (b < 0.0) {
336                                                 shift = -shift;
337                                         }
338                                         shift = c/(b + shift);
339                                 }
340                                 typename MaTRiX::value_type f = (sk + sp)*(sk - sp) + shift;
341                                 typename MaTRiX::value_type g = sk*ek;
342
343                                 // Chase zeros.
344
345                                 for (j = k; j < p-1; j++) {
346                                         typename MaTRiX::value_type t = hypot(f,g);
347                                         typename MaTRiX::value_type cs = f/t;
348                                         typename MaTRiX::value_type sn = g/t;
349                                         if (j != k) {
350                                                 e[j-1] = t;
351                                         }
352                                         f = cs*s[j] + sn*e[j];
353                                         e[j] = cs*e[j] - sn*s[j];
354                                         g = sn*s[j+1];
355                                         s[j+1] = cs*s[j+1];
356
357                                         for (i = 0; i < n; i++) {
358                                                 t = cs*V[i][j] + sn*V[i][j+1];
359                                                 V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1];
360                                                 V[i][j] = t;
361                                         }
362
363                                         t = hypot(f,g);
364                                         cs = f/t;
365                                         sn = g/t;
366                                         s[j] = t;
367                                         f = cs*e[j] + sn*s[j+1];
368                                         s[j+1] = -sn*e[j] + cs*s[j+1];
369                                         g = sn*e[j+1];
370                                         e[j+1] = cs*e[j+1];
371                                         if (j < m-1) {
372                                                 for (i = 0; i < m; i++) {
373                                                         t = cs*U[i][j] + sn*U[i][j+1];
374                                                         U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1];
375                                                         U[i][j] = t;
376                                                 }
377                                         }
378                                 }
379                                 e[p-2] = f;
380                                 iter = iter + 1;
381                         }
382                         break;
383
384                         // Convergence.
385
386                         case 4: {
387
388                                 // Make the singular values positive.
389
390                                 if (s[k] <= 0.0) {
391                                         s[k] = (s[k] < 0.0 ? -s[k] : 0.0);
392
393                                         for (i = 0; i <= pp; i++)
394                                                 V[i][k] = -V[i][k];
395                                 }
396
397                                 // Order the singular values.
398
399                                 while (k < pp) {
400                                         if (s[k] >= s[k+1]) {
401                                                 break;
402                                         }
403                                         typename MaTRiX::value_type t = s[k];
404                                         s[k] = s[k+1];
405                                         s[k+1] = t;
406                                         if (k < n-1) {
407                                                 for (i = 0; i < n; i++) {
408                                                         t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t;
409                                                 }
410                                         }
411                                         if (k < m-1) {
412                                                 for (i = 0; i < m; i++) {
413                                                         t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t;
414                                                 }
415                                         }
416                                         k++;
417                                 }
418                                 iter = 0;
419                                 p--;
420                         }
421                         break;
422                 }
423         }
424 }
425
426 }
427
428 #endif
429