Fawkes API  Fawkes Development Version
eig3.c
00001 
00002 /***************************************************************************
00003  *  eig3.c: Eigen decomposition code for symmetric 3x3 matrices
00004  *
00005  *  Created: Thu May 24 18:38:12 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
00025    domain Java Matrix library JAMA. */
00026 
00027 #include <math.h>
00028 
00029 #ifndef MAX
00030 #define MAX(a, b) ((a)>(b)?(a):(b))
00031 #endif
00032 
00033 //#define n 3
00034 static int n = 3;
00035 
00036 static double hypot2(double x, double y) {
00037   return sqrt(x*x+y*y);
00038 }
00039 
00040 // Symmetric Householder reduction to tridiagonal form.
00041 
00042 static void tred2(double V[n][n], double d[n], double e[n]) {
00043 
00044 //  This is derived from the Algol procedures tred2 by
00045 //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
00046 //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
00047 //  Fortran subroutine in EISPACK.
00048 
00049   int i,j,k;
00050   double f,g,h,hh;
00051   for (j = 0; j < n; j++) {
00052     d[j] = V[n-1][j];
00053   }
00054 
00055   // Householder reduction to tridiagonal form.
00056 
00057   for (i = n-1; i > 0; i--) {
00058 
00059     // Scale to avoid under/overflow.
00060 
00061     double scale = 0.0;
00062     double h = 0.0;
00063     for (k = 0; k < i; k++) {
00064       scale = scale + fabs(d[k]);
00065     }
00066     if (scale == 0.0) {
00067       e[i] = d[i-1];
00068       for (j = 0; j < i; j++) {
00069         d[j] = V[i-1][j];
00070         V[i][j] = 0.0;
00071         V[j][i] = 0.0;
00072       }
00073     } else {
00074 
00075       // Generate Householder vector.
00076 
00077       for (k = 0; k < i; k++) {
00078         d[k] /= scale;
00079         h += d[k] * d[k];
00080       }
00081       f = d[i-1];
00082       g = sqrt(h);
00083       if (f > 0) {
00084         g = -g;
00085       }
00086       e[i] = scale * g;
00087       h = h - f * g;
00088       d[i-1] = f - g;
00089       for (j = 0; j < i; j++) {
00090         e[j] = 0.0;
00091       }
00092 
00093       // Apply similarity transformation to remaining columns.
00094 
00095       for (j = 0; j < i; j++) {
00096         f = d[j];
00097         V[j][i] = f;
00098         g = e[j] + V[j][j] * f;
00099         for (k = j+1; k <= i-1; k++) {
00100           g += V[k][j] * d[k];
00101           e[k] += V[k][j] * f;
00102         }
00103         e[j] = g;
00104       }
00105       f = 0.0;
00106       for (j = 0; j < i; j++) {
00107         e[j] /= h;
00108         f += e[j] * d[j];
00109       }
00110       hh = f / (h + h);
00111       for (j = 0; j < i; j++) {
00112         e[j] -= hh * d[j];
00113       }
00114       for (j = 0; j < i; j++) {
00115         f = d[j];
00116         g = e[j];
00117         for (k = j; k <= i-1; k++) {
00118           V[k][j] -= (f * e[k] + g * d[k]);
00119         }
00120         d[j] = V[i-1][j];
00121         V[i][j] = 0.0;
00122       }
00123     }
00124     d[i] = h;
00125   }
00126 
00127   // Accumulate transformations.
00128 
00129   for (i = 0; i < n-1; i++) {
00130     V[n-1][i] = V[i][i];
00131     V[i][i] = 1.0;
00132     h = d[i+1];
00133     if (h != 0.0) {
00134       for (k = 0; k <= i; k++) {
00135         d[k] = V[k][i+1] / h;
00136       }
00137       for (j = 0; j <= i; j++) {
00138         g = 0.0;
00139         for (k = 0; k <= i; k++) {
00140           g += V[k][i+1] * V[k][j];
00141         }
00142         for (k = 0; k <= i; k++) {
00143           V[k][j] -= g * d[k];
00144         }
00145       }
00146     }
00147     for (k = 0; k <= i; k++) {
00148       V[k][i+1] = 0.0;
00149     }
00150   }
00151   for (j = 0; j < n; j++) {
00152     d[j] = V[n-1][j];
00153     V[n-1][j] = 0.0;
00154   }
00155   V[n-1][n-1] = 1.0;
00156   e[0] = 0.0;
00157 } 
00158 
00159 // Symmetric tridiagonal QL algorithm.
00160 
00161 static void tql2(double V[n][n], double d[n], double e[n]) {
00162 
00163 //  This is derived from the Algol procedures tql2, by
00164 //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
00165 //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
00166 //  Fortran subroutine in EISPACK.
00167 
00168   int i,j,m,l,k;
00169   double g,p,r,dl1,h,f,tst1,eps;
00170   double c,c2,c3,el1,s,s2;
00171 
00172   for (i = 1; i < n; i++) {
00173     e[i-1] = e[i];
00174   }
00175   e[n-1] = 0.0;
00176 
00177   f = 0.0;
00178   tst1 = 0.0;
00179   eps = pow(2.0,-52.0);
00180   for (l = 0; l < n; l++) {
00181 
00182     // Find small subdiagonal element
00183 
00184     tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
00185     m = l;
00186     while (m < n) {
00187       if (fabs(e[m]) <= eps*tst1) {
00188         break;
00189       }
00190       m++;
00191     }
00192 
00193     // If m == l, d[l] is an eigenvalue,
00194     // otherwise, iterate.
00195 
00196     if (m > l) {
00197       int iter = 0;
00198       do {
00199         iter = iter + 1;  // (Could check iteration count here.)
00200 
00201         // Compute implicit shift
00202 
00203         g = d[l];
00204         p = (d[l+1] - g) / (2.0 * e[l]);
00205         r = hypot2(p,1.0);
00206         if (p < 0) {
00207           r = -r;
00208         }
00209         d[l] = e[l] / (p + r);
00210         d[l+1] = e[l] * (p + r);
00211         dl1 = d[l+1];
00212         h = g - d[l];
00213         for (i = l+2; i < n; i++) {
00214           d[i] -= h;
00215         }
00216         f = f + h;
00217 
00218         // Implicit QL transformation.
00219 
00220         p = d[m];
00221         c = 1.0;
00222         c2 = c;
00223         c3 = c;
00224         el1 = e[l+1];
00225         s = 0.0;
00226         s2 = 0.0;
00227         for (i = m-1; i >= l; i--) {
00228           c3 = c2;
00229           c2 = c;
00230           s2 = s;
00231           g = c * e[i];
00232           h = c * p;
00233           r = hypot2(p,e[i]);
00234           e[i+1] = s * r;
00235           s = e[i] / r;
00236           c = p / r;
00237           p = c * d[i] - s * g;
00238           d[i+1] = h + s * (c * g + s * d[i]);
00239 
00240           // Accumulate transformation.
00241 
00242           for (k = 0; k < n; k++) {
00243             h = V[k][i+1];
00244             V[k][i+1] = s * V[k][i] + c * h;
00245             V[k][i] = c * V[k][i] - s * h;
00246           }
00247         }
00248         p = -s * s2 * c3 * el1 * e[l] / dl1;
00249         e[l] = s * p;
00250         d[l] = c * p;
00251 
00252         // Check for convergence.
00253 
00254       } while (fabs(e[l]) > eps*tst1);
00255     }
00256     d[l] = d[l] + f;
00257     e[l] = 0.0;
00258   }
00259   
00260   // Sort eigenvalues and corresponding vectors.
00261 
00262   for (i = 0; i < n-1; i++) {
00263     k = i;
00264     p = d[i];
00265     for (j = i+1; j < n; j++) {
00266       if (d[j] < p) {
00267         k = j;
00268         p = d[j];
00269       }
00270     }
00271     if (k != i) {
00272       d[k] = d[i];
00273       d[i] = p;
00274       for (j = 0; j < n; j++) {
00275         p = V[j][i];
00276         V[j][i] = V[j][k];
00277         V[j][k] = p;
00278       }
00279     }
00280   }
00281 }
00282 
00283 void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
00284   int i,j;
00285   double e[n];
00286   for (i = 0; i < n; i++) {
00287     for (j = 0; j < n; j++) {
00288       V[i][j] = A[i][j];
00289     }
00290   }
00291   tred2(V, d, e);
00292   tql2(V, d, e);
00293 }