diff --git a/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h --- a/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -586,138 +586,157 @@ template struct dir { numext::swap(roots(1),roots(2)); if (roots(0) >= roots(1)) numext::swap(roots(0),roots(1)); } } EIGEN_DEVICE_FUNC + static inline bool extract_kernel(MatrixType& mat, Ref res, Ref representative) + { + using std::abs; + // Compute the kernel of 'tmp' using a LU factorization of tmp with symmetric pivoting. + // Note that we only need to compute the factor U, which is of the form: + // x ? ? + // 0 x ? + // 0 0 0 + // With 'x' != 0. + Index i0, i1, i2; + Scalar p0, p1, p2, f; + // Find the first pivot of index i0 (by construction, there must exist a non zero coefficient on the diagonal): + mat.diagonal().cwiseAbs().maxCoeff(&i0); + // mat.col(i0) is a good candidate for an orthogonal vector to the current eigenvector, + // so let's save it: + representative = mat.col(i0); + p0 = Scalar(1)/mat.coeff(i0,i0); + // The other two rows/columns are: + i1 = (i0+1)%3; + i2 = (i0+2)%3; + // Update the first row of U: + mat.coeffRef(i0,i1) *= p0; + mat.coeffRef(i0,i2) *= p0; + + // Thanks to symmetry, the remaining 2x2 block is of the form: + // p1 f + // f p2 + // with: + p1 = mat.coeffRef(i1,i1) - mat.coeff(i1,i0) * mat.coeff(i0,i1); + p2 = mat.coeffRef(i2,i2) - mat.coeff(i2,i0) * mat.coeff(i0,i2); + f = mat.coeffRef(i1,i2) - mat.coeff(i1,i0) * mat.coeff(i0,i2); + + // Second pivoting: + if(abs(p1) < abs(p2)) + { + std::swap(i1,i2); + p1 = p2; + } + // Make sure we have a meaningful pivot: + if(abs(p1)<=abs(p0)*Eigen::NumTraits::epsilon()) + { + // `mat` appears to be rank 1 only + return false; + } + + // Back-substitutions to form the 1D kernel of 'mat': + res.coeffRef(i2) = 1; + res.coeffRef(i1) = - f / p1; + res.coeffRef(i0) = - (mat.coeff(i0,i2) + mat.coeff(i0,i1) * res.coeff(i1) ); + res.normalize(); + + return true; + } + + EIGEN_DEVICE_FUNC static inline void run(SolverType& solver, const MatrixType& mat, int options) { - using std::sqrt; eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows()); eigen_assert((options&~(EigVecMask|GenEigMask))==0 && (options&EigVecMask)!=EigVecMask && "invalid option parameter"); bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; MatrixType& eivecs = solver.m_eivec; VectorType& eivals = solver.m_eivalues; - // map the matrix coefficients to [-1:1] to avoid over- and underflow. - Scalar scale = mat.cwiseAbs().maxCoeff(); - MatrixType scaledMat = mat / scale; + // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow. + Scalar shift = mat.trace() / Scalar(3); + MatrixType scaledMat = mat; + scaledMat.diagonal().array() -= shift; + Scalar scale = scaledMat.cwiseAbs().maxCoeff(); + if(scale > 0) scaledMat /= scale; // compute the eigenvalues computeRoots(scaledMat,eivals); - // compute the eigen vectors + // compute the eigenvectors if(computeEigenvectors) { - Scalar safeNorm2 = Eigen::NumTraits::epsilon(); if((eivals(2)-eivals(0))<=Eigen::NumTraits::epsilon()) { + // All three eigenvalues are numerically the same eivecs.setIdentity(); } else { scaledMat = scaledMat.template selfadjointView(); MatrixType tmp; tmp = scaledMat; + // Compute the eigenvector of the most distinct eigenvalue Scalar d0 = eivals(2) - eivals(1); Scalar d1 = eivals(1) - eivals(0); - int k = d0 > d1 ? 2 : 0; - d0 = d0 > d1 ? d0 : d1; - - tmp.diagonal().array () -= eivals(k); - VectorType cross; - Scalar n; - n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm(); - - if(n>safeNorm2) + Index k(0), l(2); + if(d0 > d1) { - eivecs.col(k) = cross / sqrt(n); + std::swap(k,l); + d0 = d1; + } + + // Compute the eigenvector of index k + { + tmp.diagonal().array () -= eivals(k); + // By construction, 'tmp' is of rank 2, and its kernel corresponds to the respective eigenvector. + if(!extract_kernel(tmp, eivecs.col(k), eivecs.col(l))) + { + // Such a case should be impossible, the only reason would be that the three eigenvalues + // are very close to each other but not close enough to be detected earlier. + eivecs.setIdentity(); + } + } + + // Compute eigenvector of index l + if(d0<=2*Eigen::NumTraits::epsilon()*(eivals(2)-eivals(0))) + { + // If d0 is too small, then the two other eigenvalues are numerically the same, + // and thus we only have to normalize the orthogonal vector we saved above. + eivecs.col(l).normalize(); } else { - n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm(); - - if(n>safeNorm2) + tmp = scaledMat; + tmp.diagonal().array () -= eivals(l); + + VectorType dummy; + if(!extract_kernel(tmp, eivecs.col(l), dummy)) { - eivecs.col(k) = cross / sqrt(n); - } - else - { - n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm(); - - if(n>safeNorm2) - { - eivecs.col(k) = cross / sqrt(n); - } - else - { - // the input matrix and/or the eigenvaues probably contains some inf/NaN, - // => exit - // scale back to the original size. - eivals *= scale; - - solver.m_info = NumericalIssue; - solver.m_isInitialized = true; - solver.m_eigenvectorsOk = computeEigenvectors; - return; - } + // Such a case should have been detected earlier. + // 'tmp' appears to be rank one, so let's peak an arbitrary orthogonal unit vector. + eivecs.col(l).normalize(); } } - - tmp = scaledMat; - tmp.diagonal().array() -= eivals(1); - - if(d0<=Eigen::NumTraits::epsilon()) - { - eivecs.col(1) = eivecs.col(k).unitOrthogonal(); - } - else - { - n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm(); - if(n>safeNorm2) - { - eivecs.col(1) = cross / sqrt(n); - } - else - { - n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm(); - if(n>safeNorm2) - eivecs.col(1) = cross / sqrt(n); - else - { - n = (cross = eivecs.col(k).cross(tmp.row(2))).squaredNorm(); - if(n>safeNorm2) - eivecs.col(1) = cross / sqrt(n); - else - { - // we should never reach this point, - // if so the last two eigenvalues are likely to be very close to each other - eivecs.col(1) = eivecs.col(k).unitOrthogonal(); - } - } - } - - // make sure that eivecs[1] is orthogonal to eivecs[2] - // FIXME: this step should not be needed - Scalar d = eivecs.col(1).dot(eivecs.col(k)); - eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized(); - } - - eivecs.col(k==2 ? 0 : 2) = eivecs.col(k).cross(eivecs.col(1)).normalized(); + + // Compute last eigenvector from the other two + eivecs.col(1) = eivecs.col(2).cross(eivecs.col(0)).normalized(); } } + // Rescale back to the original size. eivals *= scale; + eivals.array() += shift; solver.m_info = Success; solver.m_isInitialized = true; solver.m_eigenvectorsOk = computeEigenvectors; } }; // 2x2 direct eigenvalues decomposition, code from Hauke Heibel