doxygen: intern/smoke tagged.
[blender.git] / intern / smoke / intern / EIGENVALUE_HELPER.cpp
1 /** \file smoke/intern/EIGENVALUE_HELPER.cpp
2  *  \ingroup smoke
3  */
4
5 #include "EIGENVALUE_HELPER.h"
6
7
8 void Eigentred2(sEigenvalue& eval) {
9
10    //  This is derived from the Algol procedures tred2 by
11    //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
12    //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
13    //  Fortran subroutine in EISPACK.
14
15           int n=eval.n;
16
17       for (int j = 0; j < n; j++) {
18          eval.d[j] = eval.V[n-1][j];
19       }
20
21       // Householder reduction to tridiagonal form.
22    
23       for (int i = n-1; i > 0; i--) {
24    
25          // Scale to avoid under/overflow.
26    
27          float scale = 0.0;
28          float h = 0.0;
29          for (int k = 0; k < i; k++) {
30             scale = scale + fabs(eval.d[k]);
31          }
32          if (scale == 0.0) {
33             eval.e[i] = eval.d[i-1];
34             for (int j = 0; j < i; j++) {
35                eval.d[j] = eval.V[i-1][j];
36                eval.V[i][j] = 0.0;
37                eval.V[j][i] = 0.0;
38             }
39          } else {
40    
41             // Generate Householder vector.
42    
43             for (int k = 0; k < i; k++) {
44                eval.d[k] /= scale;
45                h += eval.d[k] * eval.d[k];
46             }
47             float f = eval.d[i-1];
48             float g = sqrt(h);
49             if (f > 0) {
50                g = -g;
51             }
52             eval.e[i] = scale * g;
53             h = h - f * g;
54             eval.d[i-1] = f - g;
55             for (int j = 0; j < i; j++) {
56                eval.e[j] = 0.0;
57             }
58    
59             // Apply similarity transformation to remaining columns.
60    
61             for (int j = 0; j < i; j++) {
62                f = eval.d[j];
63                eval.V[j][i] = f;
64                g = eval.e[j] + eval.V[j][j] * f;
65                for (int k = j+1; k <= i-1; k++) {
66                   g += eval.V[k][j] * eval.d[k];
67                   eval.e[k] += eval.V[k][j] * f;
68                }
69                eval.e[j] = g;
70             }
71             f = 0.0;
72             for (int j = 0; j < i; j++) {
73                eval.e[j] /= h;
74                f += eval.e[j] * eval.d[j];
75             }
76             float hh = f / (h + h);
77             for (int j = 0; j < i; j++) {
78                eval.e[j] -= hh * eval.d[j];
79             }
80             for (int j = 0; j < i; j++) {
81                f = eval.d[j];
82                g = eval.e[j];
83                for (int k = j; k <= i-1; k++) {
84                   eval.V[k][j] -= (f * eval.e[k] + g * eval.d[k]);
85                }
86                eval.d[j] = eval.V[i-1][j];
87                eval.V[i][j] = 0.0;
88             }
89          }
90          eval.d[i] = h;
91       }
92    
93       // Accumulate transformations.
94    
95       for (int i = 0; i < n-1; i++) {
96          eval.V[n-1][i] = eval.V[i][i];
97          eval.V[i][i] = 1.0;
98          float h = eval.d[i+1];
99          if (h != 0.0) {
100             for (int k = 0; k <= i; k++) {
101                eval.d[k] = eval.V[k][i+1] / h;
102             }
103             for (int j = 0; j <= i; j++) {
104                float g = 0.0;
105                for (int k = 0; k <= i; k++) {
106                   g += eval.V[k][i+1] * eval.V[k][j];
107                }
108                for (int k = 0; k <= i; k++) {
109                   eval.V[k][j] -= g * eval.d[k];
110                }
111             }
112          }
113          for (int k = 0; k <= i; k++) {
114             eval.V[k][i+1] = 0.0;
115          }
116       }
117       for (int j = 0; j < n; j++) {
118          eval.d[j] = eval.V[n-1][j];
119          eval.V[n-1][j] = 0.0;
120       }
121       eval.V[n-1][n-1] = 1.0;
122       eval.e[0] = 0.0;
123 }
124
125 void Eigencdiv(sEigenvalue& eval, float xr, float xi, float yr, float yi) {
126       float r,d;
127       if (fabs(yr) > fabs(yi)) {
128          r = yi/yr;
129          d = yr + r*yi;
130          eval.cdivr = (xr + r*xi)/d;
131          eval.cdivi = (xi - r*xr)/d;
132       } else {
133          r = yr/yi;
134          d = yi + r*yr;
135          eval.cdivr = (r*xr + xi)/d;
136          eval.cdivi = (r*xi - xr)/d;
137       }
138    }
139
140 void Eigentql2 (sEigenvalue& eval) {
141
142    //  This is derived from the Algol procedures tql2, by
143    //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
144    //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
145    //  Fortran subroutine in EISPACK.
146    
147           int n=eval.n;
148
149       for (int i = 1; i < n; i++) {
150          eval.e[i-1] = eval.e[i];
151       }
152       eval.e[n-1] = 0.0;
153    
154       float f = 0.0;
155       float tst1 = 0.0;
156       float eps = pow(2.0,-52.0);
157       for (int l = 0; l < n; l++) {
158
159          // Find small subdiagonal element
160    
161          tst1 = max(tst1,fabs(eval.d[l]) + fabs(eval.e[l]));
162          int m = l;
163
164         // Original while-loop from Java code
165          while (m < n) {
166             if (fabs(eval.e[m]) <= eps*tst1) {
167                break;
168             }
169             m++;
170          }
171
172    
173          // If m == l, d[l] is an eigenvalue,
174          // otherwise, iterate.
175    
176          if (m > l) {
177             int iter = 0;
178             do {
179                iter = iter + 1;  // (Could check iteration count here.)
180    
181                // Compute implicit shift
182
183                float g = eval.d[l];
184                float p = (eval.d[l+1] - g) / (2.0 * eval.e[l]);
185                float r = hypot(p,1.0);
186                if (p < 0) {
187                   r = -r;
188                }
189                eval.d[l] = eval.e[l] / (p + r);
190                eval.d[l+1] = eval.e[l] * (p + r);
191                float dl1 = eval.d[l+1];
192                float h = g - eval.d[l];
193                for (int i = l+2; i < n; i++) {
194                   eval.d[i] -= h;
195                }
196                f = f + h;
197    
198                // Implicit QL transformation.
199    
200                p = eval.d[m];
201                float c = 1.0;
202                float c2 = c;
203                float c3 = c;
204                float el1 = eval.e[l+1];
205                float s = 0.0;
206                float s2 = 0.0;
207                for (int i = m-1; i >= l; i--) {
208                   c3 = c2;
209                   c2 = c;
210                   s2 = s;
211                   g = c * eval.e[i];
212                   h = c * p;
213                   r = hypot(p,eval.e[i]);
214                   eval.e[i+1] = s * r;
215                   s = eval.e[i] / r;
216                   c = p / r;
217                   p = c * eval.d[i] - s * g;
218                   eval.d[i+1] = h + s * (c * g + s * eval.d[i]);
219    
220                   // Accumulate transformation.
221    
222                   for (int k = 0; k < n; k++) {
223                      h = eval.V[k][i+1];
224                      eval.V[k][i+1] = s * eval.V[k][i] + c * h;
225                      eval.V[k][i] = c * eval.V[k][i] - s * h;
226                   }
227                }
228                p = -s * s2 * c3 * el1 * eval.e[l] / dl1;
229                eval.e[l] = s * p;
230                eval.d[l] = c * p;
231    
232                // Check for convergence.
233    
234             } while (fabs(eval.e[l]) > eps*tst1);
235          }
236          eval.d[l] = eval.d[l] + f;
237          eval.e[l] = 0.0;
238       }
239      
240       // Sort eigenvalues and corresponding vectors.
241    
242       for (int i = 0; i < n-1; i++) {
243          int k = i;
244          float p = eval.d[i];
245          for (int j = i+1; j < n; j++) {
246             if (eval.d[j] < p) {
247                k = j;
248                p = eval.d[j];
249             }
250          }
251          if (k != i) {
252             eval.d[k] = eval.d[i];
253             eval.d[i] = p;
254             for (int j = 0; j < n; j++) {
255                p = eval.V[j][i];
256                eval.V[j][i] = eval.V[j][k];
257                eval.V[j][k] = p;
258             }
259          }
260       }
261 }
262
263 void Eigenorthes (sEigenvalue& eval) {
264    
265       //  This is derived from the Algol procedures orthes and ortran,
266       //  by Martin and Wilkinson, Handbook for Auto. Comp.,
267       //  Vol.ii-Linear Algebra, and the corresponding
268       //  Fortran subroutines in EISPACK.
269    
270           int n=eval.n;
271
272       int low = 0;
273       int high = n-1;
274    
275       for (int m = low+1; m <= high-1; m++) {
276    
277          // Scale column.
278    
279          float scale = 0.0;
280          for (int i = m; i <= high; i++) {
281             scale = scale + fabs(eval.H[i][m-1]);
282          }
283          if (scale != 0.0) {
284    
285             // Compute Householder transformation.
286    
287             float h = 0.0;
288             for (int i = high; i >= m; i--) {
289                eval.ort[i] = eval.H[i][m-1]/scale;
290                h += eval.ort[i] * eval.ort[i];
291             }
292             float g = sqrt(h);
293             if (eval.ort[m] > 0) {
294                g = -g;
295             }
296             h = h - eval.ort[m] * g;
297             eval.ort[m] = eval.ort[m] - g;
298    
299             // Apply Householder similarity transformation
300             // H = (I-u*u'/h)*H*(I-u*u')/h)
301    
302             for (int j = m; j < n; j++) {
303                float f = 0.0;
304                for (int i = high; i >= m; i--) {
305                   f += eval.ort[i]*eval.H[i][j];
306                }
307                f = f/h;
308                for (int i = m; i <= high; i++) {
309                   eval.H[i][j] -= f*eval.ort[i];
310                }
311            }
312    
313            for (int i = 0; i <= high; i++) {
314                float f = 0.0;
315                for (int j = high; j >= m; j--) {
316                   f += eval.ort[j]*eval.H[i][j];
317                }
318                f = f/h;
319                for (int j = m; j <= high; j++) {
320                   eval.H[i][j] -= f*eval.ort[j];
321                }
322             }
323             eval.ort[m] = scale*eval.ort[m];
324             eval.H[m][m-1] = scale*g;
325          }
326       }
327    
328       // Accumulate transformations (Algol's ortran).
329
330       for (int i = 0; i < n; i++) {
331          for (int j = 0; j < n; j++) {
332             eval.V[i][j] = (i == j ? 1.0 : 0.0);
333          }
334       }
335
336       for (int m = high-1; m >= low+1; m--) {
337          if (eval.H[m][m-1] != 0.0) {
338             for (int i = m+1; i <= high; i++) {
339                eval.ort[i] = eval.H[i][m-1];
340             }
341             for (int j = m; j <= high; j++) {
342                float g = 0.0;
343                for (int i = m; i <= high; i++) {
344                   g += eval.ort[i] * eval.V[i][j];
345                }
346                // Double division avoids possible underflow
347                g = (g / eval.ort[m]) / eval.H[m][m-1];
348                for (int i = m; i <= high; i++) {
349                   eval.V[i][j] += g * eval.ort[i];
350                }
351             }
352          }
353       }
354    }
355
356 void Eigenhqr2 (sEigenvalue& eval) {
357    
358       //  This is derived from the Algol procedure hqr2,
359       //  by Martin and Wilkinson, Handbook for Auto. Comp.,
360       //  Vol.ii-Linear Algebra, and the corresponding
361       //  Fortran subroutine in EISPACK.
362    
363       // Initialize
364    
365       int nn = eval.n;
366       int n = nn-1;
367       int low = 0;
368       int high = nn-1;
369       float eps = pow(2.0,-52.0);
370       float exshift = 0.0;
371       float p=0,q=0,r=0,s=0,z=0,t,w,x,y;
372    
373       // Store roots isolated by balanc and compute matrix norm
374    
375       float norm = 0.0;
376       for (int i = 0; i < nn; i++) {
377          if ((i < low) || (i > high)) {
378             eval.d[i] = eval.H[i][i];
379             eval.e[i] = 0.0;
380          }
381          for (int j = max(i-1,0); j < nn; j++) {
382             norm = norm + fabs(eval.H[i][j]);
383          }
384       }
385    
386       // Outer loop over eigenvalue index
387    
388       int iter = 0;
389                 int totIter = 0;
390       while (n >= low) {
391
392                         // NT limit no. of iterations
393                         totIter++;
394                         if(totIter>100) {
395                                 //if(totIter>15) std::cout<<"!!!!iter ABORT !!!!!!! "<<totIter<<"\n"; 
396                                 // NT hack/fix, return large eigenvalues
397                                 for (int i = 0; i < nn; i++) {
398                                         eval.d[i] = 10000.;
399                                         eval.e[i] = 10000.;
400                                 }
401                                 return;
402                         }
403    
404          // Look for single small sub-diagonal element
405    
406          int l = n;
407          while (l > low) {
408             s = fabs(eval.H[l-1][l-1]) + fabs(eval.H[l][l]);
409             if (s == 0.0) {
410                s = norm;
411             }
412             if (fabs(eval.H[l][l-1]) < eps * s) {
413                break;
414             }
415             l--;
416          }
417        
418          // Check for convergence
419          // One root found
420    
421          if (l == n) {
422             eval.H[n][n] = eval.H[n][n] + exshift;
423             eval.d[n] = eval.H[n][n];
424             eval.e[n] = 0.0;
425             n--;
426             iter = 0;
427    
428          // Two roots found
429    
430          } else if (l == n-1) {
431             w = eval.H[n][n-1] * eval.H[n-1][n];
432             p = (eval.H[n-1][n-1] - eval.H[n][n]) / 2.0;
433             q = p * p + w;
434             z = sqrt(fabs(q));
435             eval.H[n][n] = eval.H[n][n] + exshift;
436             eval.H[n-1][n-1] = eval.H[n-1][n-1] + exshift;
437             x = eval.H[n][n];
438    
439             // float pair
440    
441             if (q >= 0) {
442                if (p >= 0) {
443                   z = p + z;
444                } else {
445                   z = p - z;
446                }
447                eval.d[n-1] = x + z;
448                eval.d[n] = eval.d[n-1];
449                if (z != 0.0) {
450                   eval.d[n] = x - w / z;
451                }
452                eval.e[n-1] = 0.0;
453                eval.e[n] = 0.0;
454                x = eval.H[n][n-1];
455                s = fabs(x) + fabs(z);
456                p = x / s;
457                q = z / s;
458                r = sqrt(p * p+q * q);
459                p = p / r;
460                q = q / r;
461    
462                // Row modification
463    
464                for (int j = n-1; j < nn; j++) {
465                   z = eval.H[n-1][j];
466                   eval.H[n-1][j] = q * z + p * eval.H[n][j];
467                   eval.H[n][j] = q * eval.H[n][j] - p * z;
468                }
469    
470                // Column modification
471    
472                for (int i = 0; i <= n; i++) {
473                   z = eval.H[i][n-1];
474                   eval.H[i][n-1] = q * z + p * eval.H[i][n];
475                   eval.H[i][n] = q * eval.H[i][n] - p * z;
476                }
477    
478                // Accumulate transformations
479    
480                for (int i = low; i <= high; i++) {
481                   z = eval.V[i][n-1];
482                   eval.V[i][n-1] = q * z + p * eval.V[i][n];
483                   eval.V[i][n] = q * eval.V[i][n] - p * z;
484                }
485    
486             // Complex pair
487    
488             } else {
489                eval.d[n-1] = x + p;
490                eval.d[n] = x + p;
491                eval.e[n-1] = z;
492                eval.e[n] = -z;
493             }
494             n = n - 2;
495             iter = 0;
496    
497          // No convergence yet
498    
499          } else {
500    
501             // Form shift
502    
503             x = eval.H[n][n];
504             y = 0.0;
505             w = 0.0;
506             if (l < n) {
507                y = eval.H[n-1][n-1];
508                w = eval.H[n][n-1] * eval.H[n-1][n];
509             }
510    
511             // Wilkinson's original ad hoc shift
512    
513             if (iter == 10) {
514                exshift += x;
515                for (int i = low; i <= n; i++) {
516                   eval.H[i][i] -= x;
517                }
518                s = fabs(eval.H[n][n-1]) + fabs(eval.H[n-1][n-2]);
519                x = y = 0.75 * s;
520                w = -0.4375 * s * s;
521             }
522
523             // MATLAB's new ad hoc shift
524
525             if (iter == 30) {
526                 s = (y - x) / 2.0;
527                 s = s * s + w;
528                 if (s > 0) {
529                     s = sqrt(s);
530                     if (y < x) {
531                        s = -s;
532                     }
533                     s = x - w / ((y - x) / 2.0 + s);
534                     for (int i = low; i <= n; i++) {
535                        eval.H[i][i] -= s;
536                     }
537                     exshift += s;
538                     x = y = w = 0.964;
539                 }
540             }
541    
542             iter = iter + 1;   // (Could check iteration count here.)
543    
544             // Look for two consecutive small sub-diagonal elements
545    
546             int m = n-2;
547             while (m >= l) {
548                z = eval.H[m][m];
549                r = x - z;
550                s = y - z;
551                p = (r * s - w) / eval.H[m+1][m] + eval.H[m][m+1];
552                q = eval.H[m+1][m+1] - z - r - s;
553                r = eval.H[m+2][m+1];
554                s = fabs(p) + fabs(q) + fabs(r);
555                p = p / s;
556                q = q / s;
557                r = r / s;
558                if (m == l) {
559                   break;
560                }
561                if (fabs(eval.H[m][m-1]) * (fabs(q) + fabs(r)) <
562                   eps * (fabs(p) * (fabs(eval.H[m-1][m-1]) + fabs(z) +
563                   fabs(eval.H[m+1][m+1])))) {
564                      break;
565                }
566                m--;
567             }
568    
569             for (int i = m+2; i <= n; i++) {
570                eval.H[i][i-2] = 0.0;
571                if (i > m+2) {
572                   eval.H[i][i-3] = 0.0;
573                }
574             }
575    
576             // Double QR step involving rows l:n and columns m:n
577    
578             for (int k = m; k <= n-1; k++) {
579                int notlast = (k != n-1);
580                if (k != m) {
581                   p = eval.H[k][k-1];
582                   q = eval.H[k+1][k-1];
583                   r = (notlast ? eval.H[k+2][k-1] : 0.0);
584                   x = fabs(p) + fabs(q) + fabs(r);
585                   if (x != 0.0) {
586                      p = p / x;
587                      q = q / x;
588                      r = r / x;
589                   }
590                }
591                if (x == 0.0) {
592                   break;
593                }
594                s = sqrt(p * p + q * q + r * r);
595                if (p < 0) {
596                   s = -s;
597                }
598                if (s != 0) {
599                   if (k != m) {
600                      eval.H[k][k-1] = -s * x;
601                   } else if (l != m) {
602                      eval.H[k][k-1] = -eval.H[k][k-1];
603                   }
604                   p = p + s;
605                   x = p / s;
606                   y = q / s;
607                   z = r / s;
608                   q = q / p;
609                   r = r / p;
610    
611                   // Row modification
612    
613                   for (int j = k; j < nn; j++) {
614                      p = eval.H[k][j] + q * eval.H[k+1][j];
615                      if (notlast) {
616                         p = p + r * eval.H[k+2][j];
617                         eval.H[k+2][j] = eval.H[k+2][j] - p * z;
618                      }
619                      eval.H[k][j] = eval.H[k][j] - p * x;
620                      eval.H[k+1][j] = eval.H[k+1][j] - p * y;
621                   }
622    
623                   // Column modification
624    
625                   for (int i = 0; i <= min(n,k+3); i++) {
626                      p = x * eval.H[i][k] + y * eval.H[i][k+1];
627                      if (notlast) {
628                         p = p + z * eval.H[i][k+2];
629                         eval.H[i][k+2] = eval.H[i][k+2] - p * r;
630                      }
631                      eval.H[i][k] = eval.H[i][k] - p;
632                      eval.H[i][k+1] = eval.H[i][k+1] - p * q;
633                   }
634    
635                   // Accumulate transformations
636    
637                   for (int i = low; i <= high; i++) {
638                      p = x * eval.V[i][k] + y * eval.V[i][k+1];
639                      if (notlast) {
640                         p = p + z * eval.V[i][k+2];
641                         eval.V[i][k+2] = eval.V[i][k+2] - p * r;
642                      }
643                      eval.V[i][k] = eval.V[i][k] - p;
644                      eval.V[i][k+1] = eval.V[i][k+1] - p * q;
645                   }
646                }  // (s != 0)
647             }  // k loop
648          }  // check convergence
649       }  // while (n >= low)
650                 //if(totIter>15) std::cout<<"!!!!iter "<<totIter<<"\n";
651       
652       // Backsubstitute to find vectors of upper triangular form
653
654       if (norm == 0.0) {
655          return;
656       }
657    
658       for (n = nn-1; n >= 0; n--) {
659          p = eval.d[n];
660          q = eval.e[n];
661    
662          // float vector
663    
664          if (q == 0) {
665             int l = n;
666             eval.H[n][n] = 1.0;
667             for (int i = n-1; i >= 0; i--) {
668                w = eval.H[i][i] - p;
669                r = 0.0;
670                for (int j = l; j <= n; j++) {
671                   r = r + eval.H[i][j] * eval.H[j][n];
672                }
673                if (eval.e[i] < 0.0) {
674                   z = w;
675                   s = r;
676                } else {
677                   l = i;
678                   if (eval.e[i] == 0.0) {
679                      if (w != 0.0) {
680                         eval.H[i][n] = -r / w;
681                      } else {
682                         eval.H[i][n] = -r / (eps * norm);
683                      }
684    
685                   // Solve real equations
686    
687                   } else {
688                      x = eval.H[i][i+1];
689                      y = eval.H[i+1][i];
690                      q = (eval.d[i] - p) * (eval.d[i] - p) + eval.e[i] * eval.e[i];
691                      t = (x * s - z * r) / q;
692                      eval.H[i][n] = t;
693                      if (fabs(x) > fabs(z)) {
694                         eval.H[i+1][n] = (-r - w * t) / x;
695                      } else {
696                         eval.H[i+1][n] = (-s - y * t) / z;
697                      }
698                   }
699    
700                   // Overflow control
701    
702                   t = fabs(eval.H[i][n]);
703                   if ((eps * t) * t > 1) {
704                      for (int j = i; j <= n; j++) {
705                         eval.H[j][n] = eval.H[j][n] / t;
706                      }
707                   }
708                }
709             }
710    
711          // Complex vector
712    
713          } else if (q < 0) {
714             int l = n-1;
715
716             // Last vector component imaginary so matrix is triangular
717    
718             if (fabs(eval.H[n][n-1]) > fabs(eval.H[n-1][n])) {
719                eval.H[n-1][n-1] = q / eval.H[n][n-1];
720                eval.H[n-1][n] = -(eval.H[n][n] - p) / eval.H[n][n-1];
721             } else {
722                Eigencdiv(eval, 0.0,-eval.H[n-1][n],eval.H[n-1][n-1]-p,q);
723                eval.H[n-1][n-1] = eval.cdivr;
724                eval.H[n-1][n] = eval.cdivi;
725             }
726             eval.H[n][n-1] = 0.0;
727             eval.H[n][n] = 1.0;
728             for (int i = n-2; i >= 0; i--) {
729                float ra,sa,vr,vi;
730                ra = 0.0;
731                sa = 0.0;
732                for (int j = l; j <= n; j++) {
733                   ra = ra + eval.H[i][j] * eval.H[j][n-1];
734                   sa = sa + eval.H[i][j] * eval.H[j][n];
735                }
736                w = eval.H[i][i] - p;
737    
738                if (eval.e[i] < 0.0) {
739                   z = w;
740                   r = ra;
741                   s = sa;
742                } else {
743                   l = i;
744                   if (eval.e[i] == 0) {
745                      Eigencdiv(eval,-ra,-sa,w,q);
746                      eval.H[i][n-1] = eval.cdivr;
747                      eval.H[i][n] = eval.cdivi;
748                   } else {
749    
750                      // Solve complex equations
751    
752                      x = eval.H[i][i+1];
753                      y = eval.H[i+1][i];
754                      vr = (eval.d[i] - p) * (eval.d[i] - p) + eval.e[i] * eval.e[i] - q * q;
755                      vi = (eval.d[i] - p) * 2.0 * q;
756                      if ((vr == 0.0) && (vi == 0.0)) {
757                         vr = eps * norm * (fabs(w) + fabs(q) +
758                         fabs(x) + fabs(y) + fabs(z));
759                      }
760                      Eigencdiv(eval, x*r-z*ra+q*sa,x*s-z*sa-q*ra,vr,vi);
761                      eval.H[i][n-1] = eval.cdivr;
762                      eval.H[i][n] = eval.cdivi;
763                      if (fabs(x) > (fabs(z) + fabs(q))) {
764                         eval.H[i+1][n-1] = (-ra - w * eval.H[i][n-1] + q * eval.H[i][n]) / x;
765                         eval.H[i+1][n] = (-sa - w * eval.H[i][n] - q * eval.H[i][n-1]) / x;
766                      } else {
767                         Eigencdiv(eval, -r-y*eval.H[i][n-1],-s-y*eval.H[i][n],z,q);
768                         eval.H[i+1][n-1] = eval.cdivr;
769                         eval.H[i+1][n] = eval.cdivi;
770                      }
771                   }
772    
773                   // Overflow control
774
775                   t = max(fabs(eval.H[i][n-1]),fabs(eval.H[i][n]));
776                   if ((eps * t) * t > 1) {
777                      for (int j = i; j <= n; j++) {
778                         eval.H[j][n-1] = eval.H[j][n-1] / t;
779                         eval.H[j][n] = eval.H[j][n] / t;
780                      }
781                   }
782                }
783             }
784          }
785       }
786    
787       // Vectors of isolated roots
788    
789       for (int i = 0; i < nn; i++) {
790          if (i < low || i > high) {
791             for (int j = i; j < nn; j++) {
792                eval.V[i][j] = eval.H[i][j];
793             }
794          }
795       }
796    
797       // Back transformation to get eigenvectors of original matrix
798    
799       for (int j = nn-1; j >= low; j--) {
800          for (int i = low; i <= high; i++) {
801             z = 0.0;
802             for (int k = low; k <= min(j,high); k++) {
803                z = z + eval.V[i][k] * eval.H[k][j];
804             }
805             eval.V[i][j] = z;
806          }
807       }
808 }
809
810
811
812 int computeEigenvalues3x3(
813                 float dout[3], 
814                 float a[3][3])
815 {
816   /*TNT::Array2D<float> A = TNT::Array2D<float>(3,3, &a[0][0]);
817   TNT::Array1D<float> eig = TNT::Array1D<float>(3);
818   TNT::Array1D<float> eigImag = TNT::Array1D<float>(3);
819   JAMA::Eigenvalue<float> jeig = JAMA::Eigenvalue<float>(A);*/
820
821         sEigenvalue jeig;
822
823         // Compute the values
824         {
825                 jeig.n = 3;
826                 int n=3;
827       //V = Array2D<float>(n,n);
828       //d = Array1D<float>(n);
829       //e = Array1D<float>(n);
830                 for (int y=0; y<3; y++)
831                  {
832                          jeig.d[y]=0.0f;
833                          jeig.e[y]=0.0f;
834                          for (int t=0; t<3; t++) jeig.V[y][t]=0.0f;
835                  }
836
837       jeig.issymmetric = 1;
838       for (int j = 0; (j < 3) && jeig.issymmetric; j++) {
839          for (int i = 0; (i < 3) && jeig.issymmetric; i++) {
840             jeig.issymmetric = (a[i][j] == a[j][i]);
841          }
842       }
843
844       if (jeig.issymmetric) {
845          for (int i = 0; i < 3; i++) {
846             for (int j = 0; j < 3; j++) {
847                jeig.V[i][j] = a[i][j];
848             }
849          }
850    
851          // Tridiagonalize.
852          Eigentred2(jeig);
853    
854          // Diagonalize.
855          Eigentql2(jeig);
856
857       } else {
858          //H = TNT::Array2D<float>(n,n);
859              for (int y=0; y<3; y++)
860                  {
861                          jeig.ort[y]=0.0f;
862                          for (int t=0; t<3; t++) jeig.H[y][t]=0.0f;
863                  }
864          //ort = TNT::Array1D<float>(n);
865          
866          for (int j = 0; j < n; j++) {
867             for (int i = 0; i < n; i++) {
868                jeig.H[i][j] = a[i][j];
869             }
870          }
871    
872          // Reduce to Hessenberg form.
873          Eigenorthes(jeig);
874    
875          // Reduce Hessenberg to real Schur form.
876          Eigenhqr2(jeig);
877       }
878    }
879
880   //jeig.getfloatEigenvalues(eig);
881
882   // complex ones
883   //jeig.getImagEigenvalues(eigImag);
884   dout[0]  = sqrt(jeig.d[0]*jeig.d[0] + jeig.e[0]*jeig.e[0]);
885   dout[1]  = sqrt(jeig.d[1]*jeig.d[1] + jeig.e[1]*jeig.e[1]);
886   dout[2]  = sqrt(jeig.d[2]*jeig.d[2] + jeig.e[2]*jeig.e[2]);
887   return 0;
888 }