/* Symmetric tridiagonal QL algorithm. */ static void tql2(double V[n][n], double d[n], double e[n]) { /* This is derived from the Algol procedures tql2, by */ /* Bowdler, Martin, Reinsch, and Wilkinson, Handbook for */ /* Auto. Comp., Vol.ii-Linear Algebra, and the corresponding */ /* Fortran subroutine in EISPACK. */ int i, j, k, l, m; double f; double tst1; double eps; int iter; double g, p, r; double dl1, h, c, c2, c3, el1, s, s2; for (i = 1; i < n; i++) { e[i-1] = e[i]; } e[n-1] = 0.0; f = 0.0; tst1 = 0.0; eps = pow(2.0, -52.0); for (l = 0; l < n; l++) { /* Find small subdiagonal element */ tst1 = MAX(tst1, fabs(d[l]) + fabs(e[l])); m = l; while (m < n) { if (fabs(e[m]) <= eps*tst1) { break; } m++; } /* If m == l, d[l] is an eigenvalue, */ /* otherwise, iterate. */ if (m > l) { iter = 0; do { iter = iter + 1; /* (Could check iteration count here.) */ /* Compute implicit shift */ g = d[l]; p = (d[l+1] - g) / (2.0 * e[l]); r = hypot2(p, 1.0); if (p < 0) { r = -r; } d[l] = e[l] / (p + r); d[l+1] = e[l] * (p + r); dl1 = d[l+1]; h = g - d[l]; for (i = l+2; i < n; i++) { d[i] -= h; } f = f + h; /* Implicit QL transformation. */ p = d[m]; c = 1.0; c2 = c; c3 = c; el1 = e[l+1]; s = 0.0; s2 = 0.0; for (i = m-1; i >= l; i--) { c3 = c2; c2 = c; s2 = s; g = c * e[i]; h = c * p; r = hypot2(p, e[i]); e[i+1] = s * r; s = e[i] / r; c = p / r; p = c * d[i] - s * g; d[i+1] = h + s * (c * g + s * d[i]); /* Accumulate transformation. */ for (k = 0; k < n; k++) { h = V[k][i+1]; V[k][i+1] = s * V[k][i] + c * h; V[k][i] = c * V[k][i] - s * h; } } p = -s * s2 * c3 * el1 * e[l] / dl1; e[l] = s * p; d[l] = c * p; /* Check for convergence. */ } while (fabs(e[l]) > eps*tst1); } d[l] = d[l] + f; e[l] = 0.0; } /* Sort eigenvalues and corresponding vectors. */ for (i = 0; i < n-1; i++) { k = i; p = d[i]; for (j = i+1; j < n; j++) { if (d[j] < p) { k = j; p = d[j]; } } if (k != i) { d[k] = d[i]; d[i] = p; for (j = 0; j < n; j++) { p = V[j][i]; V[j][i] = V[j][k]; V[j][k] = p; } } } }
/* Symmetric tridiagonal QL algorithm. */ static void tql2(int dim, double *V, double *d, double *e) { /* This is derived from the Algol procedures tql2, by Bowdler, Martin, Reinsch, and Wilkinson, Handbook for Auto. Comp., Vol.ii-Linear Algebra, and the corresponding Fortran subroutine in EISPACK. */ int i, m, l, k; double g, p, r, dl1, h, f, tst1, eps; double c, c2, c3, el1, s, s2; for (i = 1; i < dim; i++) { e[i - 1] = e[i]; } e[dim - 1] = 0.0; f = 0.0; tst1 = 0.0; eps = pow(2.0, -52.0); for (l = 0; l < dim; l++) { /* Find small subdiagonal element */ tst1 = fmax(tst1, fabs(d[l]) + fabs(e[l])); m = l; while (m < dim) { if (fabs(e[m]) <= eps * tst1) { break; } m++; } /* If m == l, d[l] is an eigenvalue, otherwise, iterate. */ if (m > l) { int iter = 0; do { iter = iter + 1; /* Compute implicit shift */ g = d[l]; p = (d[l + 1] - g) / (2.0 * e[l]); r = hypot2(p, 1.0); if (p < 0) { r = -r; } d[l] = e[l] / (p + r); d[l + 1] = e[l] * (p + r); dl1 = d[l + 1]; h = g - d[l]; for (i = l + 2; i < dim; i++) { d[i] -= h; } f = f + h; /* Implicit QL transformation. */ p = d[m]; c = 1.0; c2 = c; c3 = c; el1 = e[l + 1]; s = 0.0; s2 = 0.0; for (i = m - 1; i >= l; i--) { c3 = c2; c2 = c; s2 = s; g = c * e[i]; h = c * p; r = hypot2(p, e[i]); e[i + 1] = s * r; s = e[i] / r; c = p / r; p = c * d[i] - s * g; d[i + 1] = h + s * (c * g + s * d[i]); /* Accumulate transformation. */ for (k = 0; k < dim; k++) { h = V[k * dim + i + 1]; V[k * dim + i + 1] = s * V[k * dim + i] + c * h; V[k * dim + i] = c * V[k * dim + i] - s * h; } } p = -s * s2 * c3 * el1 * e[l] / dl1; e[l] = s * p; d[l] = c * p; /* Check for convergence. */ } while (fabs(e[l]) > eps * tst1 && iter < 20); /* (Check iteration count here.) */ } d[l] = d[l] + f; e[l] = 0.0; } }
static void tql2(double V[n][n], double d[n], double e[n]) { // This is derived from the Algol procedures tql2, by // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding // Fortran subroutine in EISPACK. for (int i = 1; i < n; i++) { e[i-1] = e[i]; } e[n-1] = 0.0; double f = 0.0; double tst1 = 0.0; double eps = pow(2.0,-52.0); for (int l = 0; l < n; l++) { // Find small subdiagonal element tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l])); int m = l; while (m < n) { if (fabs(e[m]) <= eps*tst1) { break; } m++; } // If m == l, d[l] is an eigenvalue, // otherwise, iterate. if (m > l) { int iter = 0; do { iter = iter + 1; // (Could check iteration count here.) // Compute implicit shift double g = d[l]; double p = (d[l+1] - g) / (2.0 * e[l]); double r = hypot2(p,1.0); if (p < 0) { r = -r; } d[l] = e[l] / (p + r); d[l+1] = e[l] * (p + r); double dl1 = d[l+1]; double h = g - d[l]; for (int i = l+2; i < n; i++) { d[i] -= h; } f = f + h; // Implicit QL transformation. p = d[m]; double c = 1.0; double c2 = c; double c3 = c; double el1 = e[l+1]; double s = 0.0; double s2 = 0.0; for (int i = m-1; i >= l; i--) { c3 = c2; c2 = c; s2 = s; g = c * e[i]; h = c * p; r = hypot2(p,e[i]); e[i+1] = s * r; s = e[i] / r; c = p / r; p = c * d[i] - s * g; d[i+1] = h + s * (c * g + s * d[i]); // Accumulate transformation. for (int k = 0; k < n; k++) { h = V[k][i+1]; V[k][i+1] = s * V[k][i] + c * h; V[k][i] = c * V[k][i] - s * h; } } p = -s * s2 * c3 * el1 * e[l] / dl1; e[l] = s * p; d[l] = c * p; // Check for convergence. } while (fabs(e[l]) > eps*tst1); } d[l] = d[l] + f; e[l] = 0.0; } // Sort eigenvalues and corresponding vectors. for (int i = 0; i < n-1; i++) { int k = i; double p = d[i]; for (int j = i+1; j < n; j++) { if (d[j] < p) { k = j; p = d[j]; } } if (k != i) { d[k] = d[i]; d[i] = p; for (int j = 0; j < n; j++) { p = V[j][i]; V[j][i] = V[j][k]; V[j][k] = p; } } } }
FLOAT length2( FLOAT p0[3], FLOAT p1[3] ) { return hypot2(p0[0]-p1[0],p0[1]-p1[1],p0[2]-p1[2]); }