Function Tql2

roborts_localization/localization_math.cpp:165–286

}

inline void Tql2(Mat3d &matrix_v, Vec3d &vector_d, Vec3d &vector_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, j, m, l, k;
  double g, p, r, dl1, h, f, tst1, eps;
  double c, c2, c3, el1, s, s2;

  for (i = 1; i < 3; i++) {
    vector_e[i - 1] = vector_e[i];
  }
  vector_e[3 - 1] = 0.0;

  f = 0.0;
  tst1 = 0.0;
  eps = pow(2.0, -52.0);
  for (l = 0; l < 3; l++) {

    // Find small subdiagonal element

    tst1 = std::max(tst1, fabs(vector_d[l]) + fabs(vector_e[l]));
    m = l;
    while (m < 3) {
      if (fabs(vector_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

        g = vector_d[l];
        p = (vector_d[l + 1] - g) / (2.0 * vector_e[l]);
        r = std::sqrt(p * p + 1.0 * 1.0);
//        r = hypot2(p,1.0);
        if (p < 0) {
          r = -r;
        }
        vector_d[l] = vector_e[l] / (p + r);
        vector_d[l + 1] = vector_e[l] * (p + r);
        dl1 = vector_d[l + 1];
        h = g - vector_d[l];
        for (i = l + 2; i < 3; i++) {
          vector_d[i] -= h;
        }
        f = f + h;