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