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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:48