Fawkes API
Fawkes Development Version
|
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 }