diff --git a/.github/workflows/pip.yml b/.github/workflows/pip.yml index f53ed3c..2381b1e 100644 --- a/.github/workflows/pip.yml +++ b/.github/workflows/pip.yml @@ -9,28 +9,28 @@ on: jobs: build: + name: Build with Pip + runs-on: ${{ matrix.platform }} strategy: fail-fast: false matrix: - platform: [ubuntu-20.04, windows-2019, macos-11] - python-version: ["3.8", "3.9", "3.10"] - - runs-on: ${{ matrix.platform }} + platform: [windows-latest, macos-latest, ubuntu-latest] + python-version: ["3.7", "3.11"] steps: - - uses: actions/checkout@v3 - with: - submodules: true + - uses: actions/checkout@v4 - uses: actions/setup-python@v4 with: python-version: ${{ matrix.python-version }} - - name: Add requirements - run: python -m pip install --upgrade wheel setuptools + - name: Set min macOS version + if: runner.os == 'macOS' + run: | + echo "MACOS_DEPLOYMENT_TARGET=10.14" >> $GITHUB_ENV - name: Build and install - run: pip install --verbose .[test] + run: pip install --verbose .[test] && make data_pull - name: Test run: python -m pytest diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml index 7d22a51..02d6e5d 100644 --- a/.github/workflows/wheels.yml +++ b/.github/workflows/wheels.yml @@ -10,12 +10,19 @@ on: types: - published +env: + FORCE_COLOR: 3 + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: build_sdist: name: Build SDist runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: true @@ -36,29 +43,22 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-latest, windows-latest, macos-latest] + os: [ubuntu-latest, macos-latest, windows-latest] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: true - # - name: Set up QEMU - # if: runner.os == 'Linux' - # uses: docker/setup-qemu-action@v2 - # with: - # platforms: all - - - uses: pypa/cibuildwheel@v2.12.0 + - uses: pypa/cibuildwheel@v2.16 env: - # CIBW_ARCHS: auto64 - CIBW_ARCHS_LINUX: x86_64 # aarch64 - CIBW_ARCHS_WINDOWS: AMD64 # ARM64 - CIBW_ARCHS_MACOS: x86_64 arm64 - CIBW_BEFORE_BUILD: pip install numpy fire --prefer-binary + CIBW_ARCHS_MACOS: universal2 + CIBW_ARCHS_WINDOWS: auto ARM64 + CIBW_BEFORE_BUILD: pip install fire loguru numpy --prefer-binary && make data_pull # https://cibuildwheel.readthedocs.io/en/stable/options/#build-skip CIBW_SKIP: pp* *i686 *musllinux* CIBW_TEST_SKIP: "*macosx* *-win_arm64" + CIBW_ENVIRONMENT: MACOSX_DEPLOYMENT_TARGET=10.14 - name: Verify clean directory run: git diff --exit-code @@ -86,6 +86,6 @@ jobs: name: artifact path: dist - - uses: pypa/gh-action-pypi-publish@v1.6.4 + - uses: pypa/gh-action-pypi-publish@release/v1 with: password: ${{ secrets.PYPI_API_TOKEN }} diff --git a/.gitignore b/.gitignore index e601402..3d31049 100644 --- a/.gitignore +++ b/.gitignore @@ -5,7 +5,6 @@ _generate/ *.so *.py[cod] *.egg-info -*env* wheelhouse site data/* diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index ced1d11..0000000 --- a/.gitmodules +++ /dev/null @@ -1,4 +0,0 @@ -[submodule "pybind11"] - path = pybind11 - url = https://github.com/pybind/pybind11.git - branch = master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 10ff4e8..03e3ead 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -21,36 +21,31 @@ exclude: '(?x)^3rdparty/(Eigen/.*|.*\..*|rapidjson/.*|spdlog/.*|tl/.*)$' repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-case-conflict - id: check-merge-conflict - id: check-symlinks - id: check-yaml - exclude: ^conda\.recipe/meta\.yaml$ - id: debug-statements - id: end-of-file-fixer - id: mixed-line-ending - id: requirements-txt-fixer - id: trailing-whitespace -# Black, the code formatter, natively supports pre-commit -- repo: https://github.com/psf/black - rev: 23.1.0 - hooks: - - id: black - exclude: ^(docs) - -- repo: https://github.com/charliermarsh/ruff-pre-commit - rev: "v0.0.254" +# Check linting and style issues +- repo: https://github.com/astral-sh/ruff-pre-commit + rev: "v0.1.4" hooks: - id: ruff args: ["--fix", "--show-fixes"] + - id: ruff-format + exclude: ^(docs) # Changes tabs to spaces - repo: https://github.com/Lucas-C/pre-commit-hooks - rev: v1.4.2 + rev: v1.5.4 hooks: - id: remove-tabs exclude: ^(docs|Makefile|data/Makefile) @@ -64,7 +59,6 @@ repos: types: [file] files: (\.cmake|CMakeLists.txt)(.in)?$ -# Suggested hook if you add a .clang-format file - repo: https://github.com/pre-commit/mirrors-clang-format rev: v13.0.0 hooks: diff --git a/.vscode/settings.json b/.vscode/settings.json index 310e3fd..a16083a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -98,7 +98,8 @@ "concepts": "cpp", "memory_resource": "cpp", "ranges": "cpp", - "stop_token": "cpp" + "stop_token": "cpp", + "core": "cpp" }, "workbench.colorCustomizations": { // "activityBar.background": "#b80ae3", diff --git a/3rdparty/Eigen/Eigenvalues b/3rdparty/Eigen/Eigenvalues new file mode 100644 index 0000000..5467a2e --- /dev/null +++ b/3rdparty/Eigen/Eigenvalues @@ -0,0 +1,60 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_EIGENVALUES_MODULE_H +#define EIGEN_EIGENVALUES_MODULE_H + +#include "Core" + +#include "Cholesky" +#include "Jacobi" +#include "Householder" +#include "LU" +#include "Geometry" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup Eigenvalues_Module Eigenvalues module + * + * + * + * This module mainly provides various eigenvalue solvers. + * This module also provides some MatrixBase methods, including: + * - MatrixBase::eigenvalues(), + * - MatrixBase::operatorNorm() + * + * \code + * #include + * \endcode + */ + +#include "src/misc/RealSvd2x2.h" +#include "src/Eigenvalues/Tridiagonalization.h" +#include "src/Eigenvalues/RealSchur.h" +#include "src/Eigenvalues/EigenSolver.h" +#include "src/Eigenvalues/SelfAdjointEigenSolver.h" +#include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" +#include "src/Eigenvalues/HessenbergDecomposition.h" +#include "src/Eigenvalues/ComplexSchur.h" +#include "src/Eigenvalues/ComplexEigenSolver.h" +#include "src/Eigenvalues/RealQZ.h" +#include "src/Eigenvalues/GeneralizedEigenSolver.h" +#include "src/Eigenvalues/MatrixBaseEigenvalues.h" +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +#include "mkl_lapacke.h" +#else +#include "src/misc/lapacke.h" +#endif +#include "src/Eigenvalues/RealSchur_LAPACKE.h" +#include "src/Eigenvalues/ComplexSchur_LAPACKE.h" +#include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_EIGENVALUES_MODULE_H diff --git a/3rdparty/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/3rdparty/Eigen/src/Eigenvalues/ComplexEigenSolver.h new file mode 100644 index 0000000..081e918 --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/ComplexEigenSolver.h @@ -0,0 +1,346 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Claire Maurice +// Copyright (C) 2009 Gael Guennebaud +// Copyright (C) 2010,2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_COMPLEX_EIGEN_SOLVER_H +#define EIGEN_COMPLEX_EIGEN_SOLVER_H + +#include "./ComplexSchur.h" + +namespace Eigen { + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class ComplexEigenSolver + * + * \brief Computes eigenvalues and eigenvectors of general complex matrices + * + * \tparam _MatrixType the type of the matrix of which we are + * computing the eigendecomposition; this is expected to be an + * instantiation of the Matrix class template. + * + * The eigenvalues and eigenvectors of a matrix \f$ A \f$ are scalars + * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda v + * \f$. If \f$ D \f$ is a diagonal matrix with the eigenvalues on + * the diagonal, and \f$ V \f$ is a matrix with the eigenvectors as + * its columns, then \f$ A V = V D \f$. The matrix \f$ V \f$ is + * almost always invertible, in which case we have \f$ A = V D V^{-1} + * \f$. This is called the eigendecomposition. + * + * The main function in this class is compute(), which computes the + * eigenvalues and eigenvectors of a given function. The + * documentation for that function contains an example showing the + * main features of the class. + * + * \sa class EigenSolver, class SelfAdjointEigenSolver + */ +template class ComplexEigenSolver +{ + public: + + /** \brief Synonym for the template parameter \p _MatrixType. */ + typedef _MatrixType MatrixType; + + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + Options = MatrixType::Options, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + /** \brief Scalar type for matrices of type #MatrixType. */ + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + /** \brief Complex scalar type for #MatrixType. + * + * This is \c std::complex if #Scalar is real (e.g., + * \c float or \c double) and just \c Scalar if #Scalar is + * complex. + */ + typedef std::complex ComplexScalar; + + /** \brief Type for vector of eigenvalues as returned by eigenvalues(). + * + * This is a column vector with entries of type #ComplexScalar. + * The length of the vector is the size of #MatrixType. + */ + typedef Matrix EigenvalueType; + + /** \brief Type for matrix of eigenvectors as returned by eigenvectors(). + * + * This is a square matrix with entries of type #ComplexScalar. + * The size is the same as the size of #MatrixType. + */ + typedef Matrix EigenvectorType; + + /** \brief Default constructor. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via compute(). + */ + ComplexEigenSolver() + : m_eivec(), + m_eivalues(), + m_schur(), + m_isInitialized(false), + m_eigenvectorsOk(false), + m_matX() + {} + + /** \brief Default Constructor with memory preallocation + * + * Like the default constructor but with preallocation of the internal data + * according to the specified problem \a size. + * \sa ComplexEigenSolver() + */ + explicit ComplexEigenSolver(Index size) + : m_eivec(size, size), + m_eivalues(size), + m_schur(size), + m_isInitialized(false), + m_eigenvectorsOk(false), + m_matX(size, size) + {} + + /** \brief Constructor; computes eigendecomposition of given matrix. + * + * \param[in] matrix Square matrix whose eigendecomposition is to be computed. + * \param[in] computeEigenvectors If true, both the eigenvectors and the + * eigenvalues are computed; if false, only the eigenvalues are + * computed. + * + * This constructor calls compute() to compute the eigendecomposition. + */ + template + explicit ComplexEigenSolver(const EigenBase& matrix, bool computeEigenvectors = true) + : m_eivec(matrix.rows(),matrix.cols()), + m_eivalues(matrix.cols()), + m_schur(matrix.rows()), + m_isInitialized(false), + m_eigenvectorsOk(false), + m_matX(matrix.rows(),matrix.cols()) + { + compute(matrix.derived(), computeEigenvectors); + } + + /** \brief Returns the eigenvectors of given matrix. + * + * \returns A const reference to the matrix whose columns are the eigenvectors. + * + * \pre Either the constructor + * ComplexEigenSolver(const MatrixType& matrix, bool) or the member + * function compute(const MatrixType& matrix, bool) has been called before + * to compute the eigendecomposition of a matrix, and + * \p computeEigenvectors was set to true (the default). + * + * This function returns a matrix whose columns are the eigenvectors. Column + * \f$ k \f$ is an eigenvector corresponding to eigenvalue number \f$ k + * \f$ as returned by eigenvalues(). The eigenvectors are normalized to + * have (Euclidean) norm equal to one. The matrix returned by this + * function is the matrix \f$ V \f$ in the eigendecomposition \f$ A = V D + * V^{-1} \f$, if it exists. + * + * Example: \include ComplexEigenSolver_eigenvectors.cpp + * Output: \verbinclude ComplexEigenSolver_eigenvectors.out + */ + const EigenvectorType& eigenvectors() const + { + eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized."); + eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); + return m_eivec; + } + + /** \brief Returns the eigenvalues of given matrix. + * + * \returns A const reference to the column vector containing the eigenvalues. + * + * \pre Either the constructor + * ComplexEigenSolver(const MatrixType& matrix, bool) or the member + * function compute(const MatrixType& matrix, bool) has been called before + * to compute the eigendecomposition of a matrix. + * + * This function returns a column vector containing the + * eigenvalues. Eigenvalues are repeated according to their + * algebraic multiplicity, so there are as many eigenvalues as + * rows in the matrix. The eigenvalues are not sorted in any particular + * order. + * + * Example: \include ComplexEigenSolver_eigenvalues.cpp + * Output: \verbinclude ComplexEigenSolver_eigenvalues.out + */ + const EigenvalueType& eigenvalues() const + { + eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized."); + return m_eivalues; + } + + /** \brief Computes eigendecomposition of given matrix. + * + * \param[in] matrix Square matrix whose eigendecomposition is to be computed. + * \param[in] computeEigenvectors If true, both the eigenvectors and the + * eigenvalues are computed; if false, only the eigenvalues are + * computed. + * \returns Reference to \c *this + * + * This function computes the eigenvalues of the complex matrix \p matrix. + * The eigenvalues() function can be used to retrieve them. If + * \p computeEigenvectors is true, then the eigenvectors are also computed + * and can be retrieved by calling eigenvectors(). + * + * The matrix is first reduced to Schur form using the + * ComplexSchur class. The Schur decomposition is then used to + * compute the eigenvalues and eigenvectors. + * + * The cost of the computation is dominated by the cost of the + * Schur decomposition, which is \f$ O(n^3) \f$ where \f$ n \f$ + * is the size of the matrix. + * + * Example: \include ComplexEigenSolver_compute.cpp + * Output: \verbinclude ComplexEigenSolver_compute.out + */ + template + ComplexEigenSolver& compute(const EigenBase& matrix, bool computeEigenvectors = true); + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, \c NoConvergence otherwise. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "ComplexEigenSolver is not initialized."); + return m_schur.info(); + } + + /** \brief Sets the maximum number of iterations allowed. */ + ComplexEigenSolver& setMaxIterations(Index maxIters) + { + m_schur.setMaxIterations(maxIters); + return *this; + } + + /** \brief Returns the maximum number of iterations. */ + Index getMaxIterations() + { + return m_schur.getMaxIterations(); + } + + protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + + EigenvectorType m_eivec; + EigenvalueType m_eivalues; + ComplexSchur m_schur; + bool m_isInitialized; + bool m_eigenvectorsOk; + EigenvectorType m_matX; + + private: + void doComputeEigenvectors(RealScalar matrixnorm); + void sortEigenvalues(bool computeEigenvectors); +}; + + +template +template +ComplexEigenSolver& +ComplexEigenSolver::compute(const EigenBase& matrix, bool computeEigenvectors) +{ + check_template_parameters(); + + // this code is inspired from Jampack + eigen_assert(matrix.cols() == matrix.rows()); + + // Do a complex Schur decomposition, A = U T U^* + // The eigenvalues are on the diagonal of T. + m_schur.compute(matrix.derived(), computeEigenvectors); + + if(m_schur.info() == Success) + { + m_eivalues = m_schur.matrixT().diagonal(); + if(computeEigenvectors) + doComputeEigenvectors(m_schur.matrixT().norm()); + sortEigenvalues(computeEigenvectors); + } + + m_isInitialized = true; + m_eigenvectorsOk = computeEigenvectors; + return *this; +} + + +template +void ComplexEigenSolver::doComputeEigenvectors(RealScalar matrixnorm) +{ + const Index n = m_eivalues.size(); + + matrixnorm = numext::maxi(matrixnorm,(std::numeric_limits::min)()); + + // Compute X such that T = X D X^(-1), where D is the diagonal of T. + // The matrix X is unit triangular. + m_matX = EigenvectorType::Zero(n, n); + for(Index k=n-1 ; k>=0 ; k--) + { + m_matX.coeffRef(k,k) = ComplexScalar(1.0,0.0); + // Compute X(i,k) using the (i,k) entry of the equation X T = D X + for(Index i=k-1 ; i>=0 ; i--) + { + m_matX.coeffRef(i,k) = -m_schur.matrixT().coeff(i,k); + if(k-i-1>0) + m_matX.coeffRef(i,k) -= (m_schur.matrixT().row(i).segment(i+1,k-i-1) * m_matX.col(k).segment(i+1,k-i-1)).value(); + ComplexScalar z = m_schur.matrixT().coeff(i,i) - m_schur.matrixT().coeff(k,k); + if(z==ComplexScalar(0)) + { + // If the i-th and k-th eigenvalue are equal, then z equals 0. + // Use a small value instead, to prevent division by zero. + numext::real_ref(z) = NumTraits::epsilon() * matrixnorm; + } + m_matX.coeffRef(i,k) = m_matX.coeff(i,k) / z; + } + } + + // Compute V as V = U X; now A = U T U^* = U X D X^(-1) U^* = V D V^(-1) + m_eivec.noalias() = m_schur.matrixU() * m_matX; + // .. and normalize the eigenvectors + for(Index k=0 ; k +void ComplexEigenSolver::sortEigenvalues(bool computeEigenvectors) +{ + const Index n = m_eivalues.size(); + for (Index i=0; i +// Copyright (C) 2010,2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_COMPLEX_SCHUR_H +#define EIGEN_COMPLEX_SCHUR_H + +#include "./HessenbergDecomposition.h" + +namespace Eigen { + +namespace internal { +template struct complex_schur_reduce_to_hessenberg; +} + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class ComplexSchur + * + * \brief Performs a complex Schur decomposition of a real or complex square matrix + * + * \tparam _MatrixType the type of the matrix of which we are + * computing the Schur decomposition; this is expected to be an + * instantiation of the Matrix class template. + * + * Given a real or complex square matrix A, this class computes the + * Schur decomposition: \f$ A = U T U^*\f$ where U is a unitary + * complex matrix, and T is a complex upper triangular matrix. The + * diagonal of the matrix T corresponds to the eigenvalues of the + * matrix A. + * + * Call the function compute() to compute the Schur decomposition of + * a given matrix. Alternatively, you can use the + * ComplexSchur(const MatrixType&, bool) constructor which computes + * the Schur decomposition at construction time. Once the + * decomposition is computed, you can use the matrixU() and matrixT() + * functions to retrieve the matrices U and V in the decomposition. + * + * \note This code is inspired from Jampack + * + * \sa class RealSchur, class EigenSolver, class ComplexEigenSolver + */ +template class ComplexSchur +{ + public: + typedef _MatrixType MatrixType; + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + Options = MatrixType::Options, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + /** \brief Scalar type for matrices of type \p _MatrixType. */ + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + /** \brief Complex scalar type for \p _MatrixType. + * + * This is \c std::complex if #Scalar is real (e.g., + * \c float or \c double) and just \c Scalar if #Scalar is + * complex. + */ + typedef std::complex ComplexScalar; + + /** \brief Type for the matrices in the Schur decomposition. + * + * This is a square matrix with entries of type #ComplexScalar. + * The size is the same as the size of \p _MatrixType. + */ + typedef Matrix ComplexMatrixType; + + /** \brief Default constructor. + * + * \param [in] size Positive integer, size of the matrix whose Schur decomposition will be computed. + * + * The default constructor is useful in cases in which the user + * intends to perform decompositions via compute(). The \p size + * parameter is only used as a hint. It is not an error to give a + * wrong \p size, but it may impair performance. + * + * \sa compute() for an example. + */ + explicit ComplexSchur(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime) + : m_matT(size,size), + m_matU(size,size), + m_hess(size), + m_isInitialized(false), + m_matUisUptodate(false), + m_maxIters(-1) + {} + + /** \brief Constructor; computes Schur decomposition of given matrix. + * + * \param[in] matrix Square matrix whose Schur decomposition is to be computed. + * \param[in] computeU If true, both T and U are computed; if false, only T is computed. + * + * This constructor calls compute() to compute the Schur decomposition. + * + * \sa matrixT() and matrixU() for examples. + */ + template + explicit ComplexSchur(const EigenBase& matrix, bool computeU = true) + : m_matT(matrix.rows(),matrix.cols()), + m_matU(matrix.rows(),matrix.cols()), + m_hess(matrix.rows()), + m_isInitialized(false), + m_matUisUptodate(false), + m_maxIters(-1) + { + compute(matrix.derived(), computeU); + } + + /** \brief Returns the unitary matrix in the Schur decomposition. + * + * \returns A const reference to the matrix U. + * + * It is assumed that either the constructor + * ComplexSchur(const MatrixType& matrix, bool computeU) or the + * member function compute(const MatrixType& matrix, bool computeU) + * has been called before to compute the Schur decomposition of a + * matrix, and that \p computeU was set to true (the default + * value). + * + * Example: \include ComplexSchur_matrixU.cpp + * Output: \verbinclude ComplexSchur_matrixU.out + */ + const ComplexMatrixType& matrixU() const + { + eigen_assert(m_isInitialized && "ComplexSchur is not initialized."); + eigen_assert(m_matUisUptodate && "The matrix U has not been computed during the ComplexSchur decomposition."); + return m_matU; + } + + /** \brief Returns the triangular matrix in the Schur decomposition. + * + * \returns A const reference to the matrix T. + * + * It is assumed that either the constructor + * ComplexSchur(const MatrixType& matrix, bool computeU) or the + * member function compute(const MatrixType& matrix, bool computeU) + * has been called before to compute the Schur decomposition of a + * matrix. + * + * Note that this function returns a plain square matrix. If you want to reference + * only the upper triangular part, use: + * \code schur.matrixT().triangularView() \endcode + * + * Example: \include ComplexSchur_matrixT.cpp + * Output: \verbinclude ComplexSchur_matrixT.out + */ + const ComplexMatrixType& matrixT() const + { + eigen_assert(m_isInitialized && "ComplexSchur is not initialized."); + return m_matT; + } + + /** \brief Computes Schur decomposition of given matrix. + * + * \param[in] matrix Square matrix whose Schur decomposition is to be computed. + * \param[in] computeU If true, both T and U are computed; if false, only T is computed. + + * \returns Reference to \c *this + * + * The Schur decomposition is computed by first reducing the + * matrix to Hessenberg form using the class + * HessenbergDecomposition. The Hessenberg matrix is then reduced + * to triangular form by performing QR iterations with a single + * shift. The cost of computing the Schur decomposition depends + * on the number of iterations; as a rough guide, it may be taken + * on the number of iterations; as a rough guide, it may be taken + * to be \f$25n^3\f$ complex flops, or \f$10n^3\f$ complex flops + * if \a computeU is false. + * + * Example: \include ComplexSchur_compute.cpp + * Output: \verbinclude ComplexSchur_compute.out + * + * \sa compute(const MatrixType&, bool, Index) + */ + template + ComplexSchur& compute(const EigenBase& matrix, bool computeU = true); + + /** \brief Compute Schur decomposition from a given Hessenberg matrix + * \param[in] matrixH Matrix in Hessenberg form H + * \param[in] matrixQ orthogonal matrix Q that transform a matrix A to H : A = Q H Q^T + * \param computeU Computes the matriX U of the Schur vectors + * \return Reference to \c *this + * + * This routine assumes that the matrix is already reduced in Hessenberg form matrixH + * using either the class HessenbergDecomposition or another mean. + * It computes the upper quasi-triangular matrix T of the Schur decomposition of H + * When computeU is true, this routine computes the matrix U such that + * A = U T U^T = (QZ) T (QZ)^T = Q H Q^T where A is the initial matrix + * + * NOTE Q is referenced if computeU is true; so, if the initial orthogonal matrix + * is not available, the user should give an identity matrix (Q.setIdentity()) + * + * \sa compute(const MatrixType&, bool) + */ + template + ComplexSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ, bool computeU=true); + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, \c NoConvergence otherwise. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "ComplexSchur is not initialized."); + return m_info; + } + + /** \brief Sets the maximum number of iterations allowed. + * + * If not specified by the user, the maximum number of iterations is m_maxIterationsPerRow times the size + * of the matrix. + */ + ComplexSchur& setMaxIterations(Index maxIters) + { + m_maxIters = maxIters; + return *this; + } + + /** \brief Returns the maximum number of iterations. */ + Index getMaxIterations() + { + return m_maxIters; + } + + /** \brief Maximum number of iterations per row. + * + * If not otherwise specified, the maximum number of iterations is this number times the size of the + * matrix. It is currently set to 30. + */ + static const int m_maxIterationsPerRow = 30; + + protected: + ComplexMatrixType m_matT, m_matU; + HessenbergDecomposition m_hess; + ComputationInfo m_info; + bool m_isInitialized; + bool m_matUisUptodate; + Index m_maxIters; + + private: + bool subdiagonalEntryIsNeglegible(Index i); + ComplexScalar computeShift(Index iu, Index iter); + void reduceToTriangularForm(bool computeU); + friend struct internal::complex_schur_reduce_to_hessenberg::IsComplex>; +}; + +/** If m_matT(i+1,i) is neglegible in floating point arithmetic + * compared to m_matT(i,i) and m_matT(j,j), then set it to zero and + * return true, else return false. */ +template +inline bool ComplexSchur::subdiagonalEntryIsNeglegible(Index i) +{ + RealScalar d = numext::norm1(m_matT.coeff(i,i)) + numext::norm1(m_matT.coeff(i+1,i+1)); + RealScalar sd = numext::norm1(m_matT.coeff(i+1,i)); + if (internal::isMuchSmallerThan(sd, d, NumTraits::epsilon())) + { + m_matT.coeffRef(i+1,i) = ComplexScalar(0); + return true; + } + return false; +} + + +/** Compute the shift in the current QR iteration. */ +template +typename ComplexSchur::ComplexScalar ComplexSchur::computeShift(Index iu, Index iter) +{ + using std::abs; + if (iter == 10 || iter == 20) + { + // exceptional shift, taken from http://www.netlib.org/eispack/comqr.f + return abs(numext::real(m_matT.coeff(iu,iu-1))) + abs(numext::real(m_matT.coeff(iu-1,iu-2))); + } + + // compute the shift as one of the eigenvalues of t, the 2x2 + // diagonal block on the bottom of the active submatrix + Matrix t = m_matT.template block<2,2>(iu-1,iu-1); + RealScalar normt = t.cwiseAbs().sum(); + t /= normt; // the normalization by sf is to avoid under/overflow + + ComplexScalar b = t.coeff(0,1) * t.coeff(1,0); + ComplexScalar c = t.coeff(0,0) - t.coeff(1,1); + ComplexScalar disc = sqrt(c*c + RealScalar(4)*b); + ComplexScalar det = t.coeff(0,0) * t.coeff(1,1) - b; + ComplexScalar trace = t.coeff(0,0) + t.coeff(1,1); + ComplexScalar eival1 = (trace + disc) / RealScalar(2); + ComplexScalar eival2 = (trace - disc) / RealScalar(2); + RealScalar eival1_norm = numext::norm1(eival1); + RealScalar eival2_norm = numext::norm1(eival2); + // A division by zero can only occur if eival1==eival2==0. + // In this case, det==0, and all we have to do is checking that eival2_norm!=0 + if(eival1_norm > eival2_norm) + eival2 = det / eival1; + else if(eival2_norm!=RealScalar(0)) + eival1 = det / eival2; + + // choose the eigenvalue closest to the bottom entry of the diagonal + if(numext::norm1(eival1-t.coeff(1,1)) < numext::norm1(eival2-t.coeff(1,1))) + return normt * eival1; + else + return normt * eival2; +} + + +template +template +ComplexSchur& ComplexSchur::compute(const EigenBase& matrix, bool computeU) +{ + m_matUisUptodate = false; + eigen_assert(matrix.cols() == matrix.rows()); + + if(matrix.cols() == 1) + { + m_matT = matrix.derived().template cast(); + if(computeU) m_matU = ComplexMatrixType::Identity(1,1); + m_info = Success; + m_isInitialized = true; + m_matUisUptodate = computeU; + return *this; + } + + internal::complex_schur_reduce_to_hessenberg::IsComplex>::run(*this, matrix.derived(), computeU); + computeFromHessenberg(m_matT, m_matU, computeU); + return *this; +} + +template +template +ComplexSchur& ComplexSchur::computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ, bool computeU) +{ + m_matT = matrixH; + if(computeU) + m_matU = matrixQ; + reduceToTriangularForm(computeU); + return *this; +} +namespace internal { + +/* Reduce given matrix to Hessenberg form */ +template +struct complex_schur_reduce_to_hessenberg +{ + // this is the implementation for the case IsComplex = true + static void run(ComplexSchur& _this, const MatrixType& matrix, bool computeU) + { + _this.m_hess.compute(matrix); + _this.m_matT = _this.m_hess.matrixH(); + if(computeU) _this.m_matU = _this.m_hess.matrixQ(); + } +}; + +template +struct complex_schur_reduce_to_hessenberg +{ + static void run(ComplexSchur& _this, const MatrixType& matrix, bool computeU) + { + typedef typename ComplexSchur::ComplexScalar ComplexScalar; + + // Note: m_hess is over RealScalar; m_matT and m_matU is over ComplexScalar + _this.m_hess.compute(matrix); + _this.m_matT = _this.m_hess.matrixH().template cast(); + if(computeU) + { + // This may cause an allocation which seems to be avoidable + MatrixType Q = _this.m_hess.matrixQ(); + _this.m_matU = Q.template cast(); + } + } +}; + +} // end namespace internal + +// Reduce the Hessenberg matrix m_matT to triangular form by QR iteration. +template +void ComplexSchur::reduceToTriangularForm(bool computeU) +{ + Index maxIters = m_maxIters; + if (maxIters == -1) + maxIters = m_maxIterationsPerRow * m_matT.rows(); + + // The matrix m_matT is divided in three parts. + // Rows 0,...,il-1 are decoupled from the rest because m_matT(il,il-1) is zero. + // Rows il,...,iu is the part we are working on (the active submatrix). + // Rows iu+1,...,end are already brought in triangular form. + Index iu = m_matT.cols() - 1; + Index il; + Index iter = 0; // number of iterations we are working on the (iu,iu) element + Index totalIter = 0; // number of iterations for whole matrix + + while(true) + { + // find iu, the bottom row of the active submatrix + while(iu > 0) + { + if(!subdiagonalEntryIsNeglegible(iu-1)) break; + iter = 0; + --iu; + } + + // if iu is zero then we are done; the whole matrix is triangularized + if(iu==0) break; + + // if we spent too many iterations, we give up + iter++; + totalIter++; + if(totalIter > maxIters) break; + + // find il, the top row of the active submatrix + il = iu-1; + while(il > 0 && !subdiagonalEntryIsNeglegible(il-1)) + { + --il; + } + + /* perform the QR step using Givens rotations. The first rotation + creates a bulge; the (il+2,il) element becomes nonzero. This + bulge is chased down to the bottom of the active submatrix. */ + + ComplexScalar shift = computeShift(iu, iter); + JacobiRotation rot; + rot.makeGivens(m_matT.coeff(il,il) - shift, m_matT.coeff(il+1,il)); + m_matT.rightCols(m_matT.cols()-il).applyOnTheLeft(il, il+1, rot.adjoint()); + m_matT.topRows((std::min)(il+2,iu)+1).applyOnTheRight(il, il+1, rot); + if(computeU) m_matU.applyOnTheRight(il, il+1, rot); + + for(Index i=il+1 ; i template inline \ +ComplexSchur >& \ +ComplexSchur >::compute(const EigenBase& matrix, bool computeU) \ +{ \ + typedef Matrix MatrixType; \ + typedef MatrixType::RealScalar RealScalar; \ + typedef std::complex ComplexScalar; \ +\ + eigen_assert(matrix.cols() == matrix.rows()); \ +\ + m_matUisUptodate = false; \ + if(matrix.cols() == 1) \ + { \ + m_matT = matrix.derived().template cast(); \ + if(computeU) m_matU = ComplexMatrixType::Identity(1,1); \ + m_info = Success; \ + m_isInitialized = true; \ + m_matUisUptodate = computeU; \ + return *this; \ + } \ + lapack_int n = internal::convert_index(matrix.cols()), sdim, info; \ + lapack_int matrix_order = LAPACKE_COLROW; \ + char jobvs, sort='N'; \ + LAPACK_##LAPACKE_PREFIX_U##_SELECT1 select = 0; \ + jobvs = (computeU) ? 'V' : 'N'; \ + m_matU.resize(n, n); \ + lapack_int ldvs = internal::convert_index(m_matU.outerStride()); \ + m_matT = matrix; \ + lapack_int lda = internal::convert_index(m_matT.outerStride()); \ + Matrix w; \ + w.resize(n, 1);\ + info = LAPACKE_##LAPACKE_PREFIX##gees( matrix_order, jobvs, sort, select, n, (LAPACKE_TYPE*)m_matT.data(), lda, &sdim, (LAPACKE_TYPE*)w.data(), (LAPACKE_TYPE*)m_matU.data(), ldvs ); \ + if(info == 0) \ + m_info = Success; \ + else \ + m_info = NoConvergence; \ +\ + m_isInitialized = true; \ + m_matUisUptodate = computeU; \ + return *this; \ +\ +} + +EIGEN_LAPACKE_SCHUR_COMPLEX(dcomplex, lapack_complex_double, z, Z, ColMajor, LAPACK_COL_MAJOR) +EIGEN_LAPACKE_SCHUR_COMPLEX(scomplex, lapack_complex_float, c, C, ColMajor, LAPACK_COL_MAJOR) +EIGEN_LAPACKE_SCHUR_COMPLEX(dcomplex, lapack_complex_double, z, Z, RowMajor, LAPACK_ROW_MAJOR) +EIGEN_LAPACKE_SCHUR_COMPLEX(scomplex, lapack_complex_float, c, C, RowMajor, LAPACK_ROW_MAJOR) + +} // end namespace Eigen + +#endif // EIGEN_COMPLEX_SCHUR_LAPACKE_H diff --git a/3rdparty/Eigen/src/Eigenvalues/EigenSolver.h b/3rdparty/Eigen/src/Eigenvalues/EigenSolver.h new file mode 100644 index 0000000..572b29e --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/EigenSolver.h @@ -0,0 +1,622 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2010,2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_EIGENSOLVER_H +#define EIGEN_EIGENSOLVER_H + +#include "./RealSchur.h" + +namespace Eigen { + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class EigenSolver + * + * \brief Computes eigenvalues and eigenvectors of general matrices + * + * \tparam _MatrixType the type of the matrix of which we are computing the + * eigendecomposition; this is expected to be an instantiation of the Matrix + * class template. Currently, only real matrices are supported. + * + * The eigenvalues and eigenvectors of a matrix \f$ A \f$ are scalars + * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda v \f$. If + * \f$ D \f$ is a diagonal matrix with the eigenvalues on the diagonal, and + * \f$ V \f$ is a matrix with the eigenvectors as its columns, then \f$ A V = + * V D \f$. The matrix \f$ V \f$ is almost always invertible, in which case we + * have \f$ A = V D V^{-1} \f$. This is called the eigendecomposition. + * + * The eigenvalues and eigenvectors of a matrix may be complex, even when the + * matrix is real. However, we can choose real matrices \f$ V \f$ and \f$ D + * \f$ satisfying \f$ A V = V D \f$, just like the eigendecomposition, if the + * matrix \f$ D \f$ is not required to be diagonal, but if it is allowed to + * have blocks of the form + * \f[ \begin{bmatrix} u & v \\ -v & u \end{bmatrix} \f] + * (where \f$ u \f$ and \f$ v \f$ are real numbers) on the diagonal. These + * blocks correspond to complex eigenvalue pairs \f$ u \pm iv \f$. We call + * this variant of the eigendecomposition the pseudo-eigendecomposition. + * + * Call the function compute() to compute the eigenvalues and eigenvectors of + * a given matrix. Alternatively, you can use the + * EigenSolver(const MatrixType&, bool) constructor which computes the + * eigenvalues and eigenvectors at construction time. Once the eigenvalue and + * eigenvectors are computed, they can be retrieved with the eigenvalues() and + * eigenvectors() functions. The pseudoEigenvalueMatrix() and + * pseudoEigenvectors() methods allow the construction of the + * pseudo-eigendecomposition. + * + * The documentation for EigenSolver(const MatrixType&, bool) contains an + * example of the typical use of this class. + * + * \note The implementation is adapted from + * JAMA (public domain). + * Their code is based on EISPACK. + * + * \sa MatrixBase::eigenvalues(), class ComplexEigenSolver, class SelfAdjointEigenSolver + */ +template class EigenSolver +{ + public: + + /** \brief Synonym for the template parameter \p _MatrixType. */ + typedef _MatrixType MatrixType; + + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + Options = MatrixType::Options, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + /** \brief Scalar type for matrices of type #MatrixType. */ + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + /** \brief Complex scalar type for #MatrixType. + * + * This is \c std::complex if #Scalar is real (e.g., + * \c float or \c double) and just \c Scalar if #Scalar is + * complex. + */ + typedef std::complex ComplexScalar; + + /** \brief Type for vector of eigenvalues as returned by eigenvalues(). + * + * This is a column vector with entries of type #ComplexScalar. + * The length of the vector is the size of #MatrixType. + */ + typedef Matrix EigenvalueType; + + /** \brief Type for matrix of eigenvectors as returned by eigenvectors(). + * + * This is a square matrix with entries of type #ComplexScalar. + * The size is the same as the size of #MatrixType. + */ + typedef Matrix EigenvectorsType; + + /** \brief Default constructor. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via EigenSolver::compute(const MatrixType&, bool). + * + * \sa compute() for an example. + */ + EigenSolver() : m_eivec(), m_eivalues(), m_isInitialized(false), m_eigenvectorsOk(false), m_realSchur(), m_matT(), m_tmp() {} + + /** \brief Default constructor with memory preallocation + * + * Like the default constructor but with preallocation of the internal data + * according to the specified problem \a size. + * \sa EigenSolver() + */ + explicit EigenSolver(Index size) + : m_eivec(size, size), + m_eivalues(size), + m_isInitialized(false), + m_eigenvectorsOk(false), + m_realSchur(size), + m_matT(size, size), + m_tmp(size) + {} + + /** \brief Constructor; computes eigendecomposition of given matrix. + * + * \param[in] matrix Square matrix whose eigendecomposition is to be computed. + * \param[in] computeEigenvectors If true, both the eigenvectors and the + * eigenvalues are computed; if false, only the eigenvalues are + * computed. + * + * This constructor calls compute() to compute the eigenvalues + * and eigenvectors. + * + * Example: \include EigenSolver_EigenSolver_MatrixType.cpp + * Output: \verbinclude EigenSolver_EigenSolver_MatrixType.out + * + * \sa compute() + */ + template + explicit EigenSolver(const EigenBase& matrix, bool computeEigenvectors = true) + : m_eivec(matrix.rows(), matrix.cols()), + m_eivalues(matrix.cols()), + m_isInitialized(false), + m_eigenvectorsOk(false), + m_realSchur(matrix.cols()), + m_matT(matrix.rows(), matrix.cols()), + m_tmp(matrix.cols()) + { + compute(matrix.derived(), computeEigenvectors); + } + + /** \brief Returns the eigenvectors of given matrix. + * + * \returns %Matrix whose columns are the (possibly complex) eigenvectors. + * + * \pre Either the constructor + * EigenSolver(const MatrixType&,bool) or the member function + * compute(const MatrixType&, bool) has been called before, and + * \p computeEigenvectors was set to true (the default). + * + * Column \f$ k \f$ of the returned matrix is an eigenvector corresponding + * to eigenvalue number \f$ k \f$ as returned by eigenvalues(). The + * eigenvectors are normalized to have (Euclidean) norm equal to one. The + * matrix returned by this function is the matrix \f$ V \f$ in the + * eigendecomposition \f$ A = V D V^{-1} \f$, if it exists. + * + * Example: \include EigenSolver_eigenvectors.cpp + * Output: \verbinclude EigenSolver_eigenvectors.out + * + * \sa eigenvalues(), pseudoEigenvectors() + */ + EigenvectorsType eigenvectors() const; + + /** \brief Returns the pseudo-eigenvectors of given matrix. + * + * \returns Const reference to matrix whose columns are the pseudo-eigenvectors. + * + * \pre Either the constructor + * EigenSolver(const MatrixType&,bool) or the member function + * compute(const MatrixType&, bool) has been called before, and + * \p computeEigenvectors was set to true (the default). + * + * The real matrix \f$ V \f$ returned by this function and the + * block-diagonal matrix \f$ D \f$ returned by pseudoEigenvalueMatrix() + * satisfy \f$ AV = VD \f$. + * + * Example: \include EigenSolver_pseudoEigenvectors.cpp + * Output: \verbinclude EigenSolver_pseudoEigenvectors.out + * + * \sa pseudoEigenvalueMatrix(), eigenvectors() + */ + const MatrixType& pseudoEigenvectors() const + { + eigen_assert(m_isInitialized && "EigenSolver is not initialized."); + eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); + return m_eivec; + } + + /** \brief Returns the block-diagonal matrix in the pseudo-eigendecomposition. + * + * \returns A block-diagonal matrix. + * + * \pre Either the constructor + * EigenSolver(const MatrixType&,bool) or the member function + * compute(const MatrixType&, bool) has been called before. + * + * The matrix \f$ D \f$ returned by this function is real and + * block-diagonal. The blocks on the diagonal are either 1-by-1 or 2-by-2 + * blocks of the form + * \f$ \begin{bmatrix} u & v \\ -v & u \end{bmatrix} \f$. + * These blocks are not sorted in any particular order. + * The matrix \f$ D \f$ and the matrix \f$ V \f$ returned by + * pseudoEigenvectors() satisfy \f$ AV = VD \f$. + * + * \sa pseudoEigenvectors() for an example, eigenvalues() + */ + MatrixType pseudoEigenvalueMatrix() const; + + /** \brief Returns the eigenvalues of given matrix. + * + * \returns A const reference to the column vector containing the eigenvalues. + * + * \pre Either the constructor + * EigenSolver(const MatrixType&,bool) or the member function + * compute(const MatrixType&, bool) has been called before. + * + * The eigenvalues are repeated according to their algebraic multiplicity, + * so there are as many eigenvalues as rows in the matrix. The eigenvalues + * are not sorted in any particular order. + * + * Example: \include EigenSolver_eigenvalues.cpp + * Output: \verbinclude EigenSolver_eigenvalues.out + * + * \sa eigenvectors(), pseudoEigenvalueMatrix(), + * MatrixBase::eigenvalues() + */ + const EigenvalueType& eigenvalues() const + { + eigen_assert(m_isInitialized && "EigenSolver is not initialized."); + return m_eivalues; + } + + /** \brief Computes eigendecomposition of given matrix. + * + * \param[in] matrix Square matrix whose eigendecomposition is to be computed. + * \param[in] computeEigenvectors If true, both the eigenvectors and the + * eigenvalues are computed; if false, only the eigenvalues are + * computed. + * \returns Reference to \c *this + * + * This function computes the eigenvalues of the real matrix \p matrix. + * The eigenvalues() function can be used to retrieve them. If + * \p computeEigenvectors is true, then the eigenvectors are also computed + * and can be retrieved by calling eigenvectors(). + * + * The matrix is first reduced to real Schur form using the RealSchur + * class. The Schur decomposition is then used to compute the eigenvalues + * and eigenvectors. + * + * The cost of the computation is dominated by the cost of the + * Schur decomposition, which is very approximately \f$ 25n^3 \f$ + * (where \f$ n \f$ is the size of the matrix) if \p computeEigenvectors + * is true, and \f$ 10n^3 \f$ if \p computeEigenvectors is false. + * + * This method reuses of the allocated data in the EigenSolver object. + * + * Example: \include EigenSolver_compute.cpp + * Output: \verbinclude EigenSolver_compute.out + */ + template + EigenSolver& compute(const EigenBase& matrix, bool computeEigenvectors = true); + + /** \returns NumericalIssue if the input contains INF or NaN values or overflow occurred. Returns Success otherwise. */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "EigenSolver is not initialized."); + return m_info; + } + + /** \brief Sets the maximum number of iterations allowed. */ + EigenSolver& setMaxIterations(Index maxIters) + { + m_realSchur.setMaxIterations(maxIters); + return *this; + } + + /** \brief Returns the maximum number of iterations. */ + Index getMaxIterations() + { + return m_realSchur.getMaxIterations(); + } + + private: + void doComputeEigenvectors(); + + protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + EIGEN_STATIC_ASSERT(!NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL); + } + + MatrixType m_eivec; + EigenvalueType m_eivalues; + bool m_isInitialized; + bool m_eigenvectorsOk; + ComputationInfo m_info; + RealSchur m_realSchur; + MatrixType m_matT; + + typedef Matrix ColumnVectorType; + ColumnVectorType m_tmp; +}; + +template +MatrixType EigenSolver::pseudoEigenvalueMatrix() const +{ + eigen_assert(m_isInitialized && "EigenSolver is not initialized."); + const RealScalar precision = RealScalar(2)*NumTraits::epsilon(); + Index n = m_eivalues.rows(); + MatrixType matD = MatrixType::Zero(n,n); + for (Index i=0; i(i,i) << numext::real(m_eivalues.coeff(i)), numext::imag(m_eivalues.coeff(i)), + -numext::imag(m_eivalues.coeff(i)), numext::real(m_eivalues.coeff(i)); + ++i; + } + } + return matD; +} + +template +typename EigenSolver::EigenvectorsType EigenSolver::eigenvectors() const +{ + eigen_assert(m_isInitialized && "EigenSolver is not initialized."); + eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); + const RealScalar precision = RealScalar(2)*NumTraits::epsilon(); + Index n = m_eivec.cols(); + EigenvectorsType matV(n,n); + for (Index j=0; j(); + matV.col(j).normalize(); + } + else + { + // we have a pair of complex eigen values + for (Index i=0; i +template +EigenSolver& +EigenSolver::compute(const EigenBase& matrix, bool computeEigenvectors) +{ + check_template_parameters(); + + using std::sqrt; + using std::abs; + using numext::isfinite; + eigen_assert(matrix.cols() == matrix.rows()); + + // Reduce to real Schur form. + m_realSchur.compute(matrix.derived(), computeEigenvectors); + + m_info = m_realSchur.info(); + + if (m_info == Success) + { + m_matT = m_realSchur.matrixT(); + if (computeEigenvectors) + m_eivec = m_realSchur.matrixU(); + + // Compute eigenvalues from matT + m_eivalues.resize(matrix.cols()); + Index i = 0; + while (i < matrix.cols()) + { + if (i == matrix.cols() - 1 || m_matT.coeff(i+1, i) == Scalar(0)) + { + m_eivalues.coeffRef(i) = m_matT.coeff(i, i); + if(!(isfinite)(m_eivalues.coeffRef(i))) + { + m_isInitialized = true; + m_eigenvectorsOk = false; + m_info = NumericalIssue; + return *this; + } + ++i; + } + else + { + Scalar p = Scalar(0.5) * (m_matT.coeff(i, i) - m_matT.coeff(i+1, i+1)); + Scalar z; + // Compute z = sqrt(abs(p * p + m_matT.coeff(i+1, i) * m_matT.coeff(i, i+1))); + // without overflow + { + Scalar t0 = m_matT.coeff(i+1, i); + Scalar t1 = m_matT.coeff(i, i+1); + Scalar maxval = numext::maxi(abs(p),numext::maxi(abs(t0),abs(t1))); + t0 /= maxval; + t1 /= maxval; + Scalar p0 = p/maxval; + z = maxval * sqrt(abs(p0 * p0 + t0 * t1)); + } + + m_eivalues.coeffRef(i) = ComplexScalar(m_matT.coeff(i+1, i+1) + p, z); + m_eivalues.coeffRef(i+1) = ComplexScalar(m_matT.coeff(i+1, i+1) + p, -z); + if(!((isfinite)(m_eivalues.coeffRef(i)) && (isfinite)(m_eivalues.coeffRef(i+1)))) + { + m_isInitialized = true; + m_eigenvectorsOk = false; + m_info = NumericalIssue; + return *this; + } + i += 2; + } + } + + // Compute eigenvectors. + if (computeEigenvectors) + doComputeEigenvectors(); + } + + m_isInitialized = true; + m_eigenvectorsOk = computeEigenvectors; + + return *this; +} + + +template +void EigenSolver::doComputeEigenvectors() +{ + using std::abs; + const Index size = m_eivec.cols(); + const Scalar eps = NumTraits::epsilon(); + + // inefficient! this is already computed in RealSchur + Scalar norm(0); + for (Index j = 0; j < size; ++j) + { + norm += m_matT.row(j).segment((std::max)(j-1,Index(0)), size-(std::max)(j-1,Index(0))).cwiseAbs().sum(); + } + + // Backsubstitute to find vectors of upper triangular form + if (norm == Scalar(0)) + { + return; + } + + for (Index n = size-1; n >= 0; n--) + { + Scalar p = m_eivalues.coeff(n).real(); + Scalar q = m_eivalues.coeff(n).imag(); + + // Scalar vector + if (q == Scalar(0)) + { + Scalar lastr(0), lastw(0); + Index l = n; + + m_matT.coeffRef(n,n) = Scalar(1); + for (Index i = n-1; i >= 0; i--) + { + Scalar w = m_matT.coeff(i,i) - p; + Scalar r = m_matT.row(i).segment(l,n-l+1).dot(m_matT.col(n).segment(l, n-l+1)); + + if (m_eivalues.coeff(i).imag() < Scalar(0)) + { + lastw = w; + lastr = r; + } + else + { + l = i; + if (m_eivalues.coeff(i).imag() == Scalar(0)) + { + if (w != Scalar(0)) + m_matT.coeffRef(i,n) = -r / w; + else + m_matT.coeffRef(i,n) = -r / (eps * norm); + } + else // Solve real equations + { + Scalar x = m_matT.coeff(i,i+1); + Scalar y = m_matT.coeff(i+1,i); + Scalar denom = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag(); + Scalar t = (x * lastr - lastw * r) / denom; + m_matT.coeffRef(i,n) = t; + if (abs(x) > abs(lastw)) + m_matT.coeffRef(i+1,n) = (-r - w * t) / x; + else + m_matT.coeffRef(i+1,n) = (-lastr - y * t) / lastw; + } + + // Overflow control + Scalar t = abs(m_matT.coeff(i,n)); + if ((eps * t) * t > Scalar(1)) + m_matT.col(n).tail(size-i) /= t; + } + } + } + else if (q < Scalar(0) && n > 0) // Complex vector + { + Scalar lastra(0), lastsa(0), lastw(0); + Index l = n-1; + + // Last vector component imaginary so matrix is triangular + if (abs(m_matT.coeff(n,n-1)) > abs(m_matT.coeff(n-1,n))) + { + m_matT.coeffRef(n-1,n-1) = q / m_matT.coeff(n,n-1); + m_matT.coeffRef(n-1,n) = -(m_matT.coeff(n,n) - p) / m_matT.coeff(n,n-1); + } + else + { + ComplexScalar cc = ComplexScalar(Scalar(0),-m_matT.coeff(n-1,n)) / ComplexScalar(m_matT.coeff(n-1,n-1)-p,q); + m_matT.coeffRef(n-1,n-1) = numext::real(cc); + m_matT.coeffRef(n-1,n) = numext::imag(cc); + } + m_matT.coeffRef(n,n-1) = Scalar(0); + m_matT.coeffRef(n,n) = Scalar(1); + for (Index i = n-2; i >= 0; i--) + { + Scalar ra = m_matT.row(i).segment(l, n-l+1).dot(m_matT.col(n-1).segment(l, n-l+1)); + Scalar sa = m_matT.row(i).segment(l, n-l+1).dot(m_matT.col(n).segment(l, n-l+1)); + Scalar w = m_matT.coeff(i,i) - p; + + if (m_eivalues.coeff(i).imag() < Scalar(0)) + { + lastw = w; + lastra = ra; + lastsa = sa; + } + else + { + l = i; + if (m_eivalues.coeff(i).imag() == RealScalar(0)) + { + ComplexScalar cc = ComplexScalar(-ra,-sa) / ComplexScalar(w,q); + m_matT.coeffRef(i,n-1) = numext::real(cc); + m_matT.coeffRef(i,n) = numext::imag(cc); + } + else + { + // Solve complex equations + Scalar x = m_matT.coeff(i,i+1); + Scalar y = m_matT.coeff(i+1,i); + Scalar vr = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag() - q * q; + Scalar vi = (m_eivalues.coeff(i).real() - p) * Scalar(2) * q; + if ((vr == Scalar(0)) && (vi == Scalar(0))) + vr = eps * norm * (abs(w) + abs(q) + abs(x) + abs(y) + abs(lastw)); + + ComplexScalar cc = ComplexScalar(x*lastra-lastw*ra+q*sa,x*lastsa-lastw*sa-q*ra) / ComplexScalar(vr,vi); + m_matT.coeffRef(i,n-1) = numext::real(cc); + m_matT.coeffRef(i,n) = numext::imag(cc); + if (abs(x) > (abs(lastw) + abs(q))) + { + m_matT.coeffRef(i+1,n-1) = (-ra - w * m_matT.coeff(i,n-1) + q * m_matT.coeff(i,n)) / x; + m_matT.coeffRef(i+1,n) = (-sa - w * m_matT.coeff(i,n) - q * m_matT.coeff(i,n-1)) / x; + } + else + { + cc = ComplexScalar(-lastra-y*m_matT.coeff(i,n-1),-lastsa-y*m_matT.coeff(i,n)) / ComplexScalar(lastw,q); + m_matT.coeffRef(i+1,n-1) = numext::real(cc); + m_matT.coeffRef(i+1,n) = numext::imag(cc); + } + } + + // Overflow control + Scalar t = numext::maxi(abs(m_matT.coeff(i,n-1)),abs(m_matT.coeff(i,n))); + if ((eps * t) * t > Scalar(1)) + m_matT.block(i, n-1, size-i, 2) /= t; + + } + } + + // We handled a pair of complex conjugate eigenvalues, so need to skip them both + n--; + } + else + { + eigen_assert(0 && "Internal bug in EigenSolver (INF or NaN has not been detected)"); // this should not happen + } + } + + // Back transformation to get eigenvectors of original matrix + for (Index j = size-1; j >= 0; j--) + { + m_tmp.noalias() = m_eivec.leftCols(j+1) * m_matT.col(j).segment(0, j+1); + m_eivec.col(j) = m_tmp; + } +} + +} // end namespace Eigen + +#endif // EIGEN_EIGENSOLVER_H diff --git a/3rdparty/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/3rdparty/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h new file mode 100644 index 0000000..87d789b --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h @@ -0,0 +1,418 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012-2016 Gael Guennebaud +// Copyright (C) 2010,2012 Jitse Niesen +// Copyright (C) 2016 Tobias Wood +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_GENERALIZEDEIGENSOLVER_H +#define EIGEN_GENERALIZEDEIGENSOLVER_H + +#include "./RealQZ.h" + +namespace Eigen { + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class GeneralizedEigenSolver + * + * \brief Computes the generalized eigenvalues and eigenvectors of a pair of general matrices + * + * \tparam _MatrixType the type of the matrices of which we are computing the + * eigen-decomposition; this is expected to be an instantiation of the Matrix + * class template. Currently, only real matrices are supported. + * + * The generalized eigenvalues and eigenvectors of a matrix pair \f$ A \f$ and \f$ B \f$ are scalars + * \f$ \lambda \f$ and vectors \f$ v \f$ such that \f$ Av = \lambda Bv \f$. If + * \f$ D \f$ is a diagonal matrix with the eigenvalues on the diagonal, and + * \f$ V \f$ is a matrix with the eigenvectors as its columns, then \f$ A V = + * B V D \f$. The matrix \f$ V \f$ is almost always invertible, in which case we + * have \f$ A = B V D V^{-1} \f$. This is called the generalized eigen-decomposition. + * + * The generalized eigenvalues and eigenvectors of a matrix pair may be complex, even when the + * matrices are real. Moreover, the generalized eigenvalue might be infinite if the matrix B is + * singular. To workaround this difficulty, the eigenvalues are provided as a pair of complex \f$ \alpha \f$ + * and real \f$ \beta \f$ such that: \f$ \lambda_i = \alpha_i / \beta_i \f$. If \f$ \beta_i \f$ is (nearly) zero, + * then one can consider the well defined left eigenvalue \f$ \mu = \beta_i / \alpha_i\f$ such that: + * \f$ \mu_i A v_i = B v_i \f$, or even \f$ \mu_i u_i^T A = u_i^T B \f$ where \f$ u_i \f$ is + * called the left eigenvector. + * + * Call the function compute() to compute the generalized eigenvalues and eigenvectors of + * a given matrix pair. Alternatively, you can use the + * GeneralizedEigenSolver(const MatrixType&, const MatrixType&, bool) constructor which computes the + * eigenvalues and eigenvectors at construction time. Once the eigenvalue and + * eigenvectors are computed, they can be retrieved with the eigenvalues() and + * eigenvectors() functions. + * + * Here is an usage example of this class: + * Example: \include GeneralizedEigenSolver.cpp + * Output: \verbinclude GeneralizedEigenSolver.out + * + * \sa MatrixBase::eigenvalues(), class ComplexEigenSolver, class SelfAdjointEigenSolver + */ +template class GeneralizedEigenSolver +{ + public: + + /** \brief Synonym for the template parameter \p _MatrixType. */ + typedef _MatrixType MatrixType; + + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + Options = MatrixType::Options, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + /** \brief Scalar type for matrices of type #MatrixType. */ + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + /** \brief Complex scalar type for #MatrixType. + * + * This is \c std::complex if #Scalar is real (e.g., + * \c float or \c double) and just \c Scalar if #Scalar is + * complex. + */ + typedef std::complex ComplexScalar; + + /** \brief Type for vector of real scalar values eigenvalues as returned by betas(). + * + * This is a column vector with entries of type #Scalar. + * The length of the vector is the size of #MatrixType. + */ + typedef Matrix VectorType; + + /** \brief Type for vector of complex scalar values eigenvalues as returned by alphas(). + * + * This is a column vector with entries of type #ComplexScalar. + * The length of the vector is the size of #MatrixType. + */ + typedef Matrix ComplexVectorType; + + /** \brief Expression type for the eigenvalues as returned by eigenvalues(). + */ + typedef CwiseBinaryOp,ComplexVectorType,VectorType> EigenvalueType; + + /** \brief Type for matrix of eigenvectors as returned by eigenvectors(). + * + * This is a square matrix with entries of type #ComplexScalar. + * The size is the same as the size of #MatrixType. + */ + typedef Matrix EigenvectorsType; + + /** \brief Default constructor. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via EigenSolver::compute(const MatrixType&, bool). + * + * \sa compute() for an example. + */ + GeneralizedEigenSolver() + : m_eivec(), + m_alphas(), + m_betas(), + m_valuesOkay(false), + m_vectorsOkay(false), + m_realQZ() + {} + + /** \brief Default constructor with memory preallocation + * + * Like the default constructor but with preallocation of the internal data + * according to the specified problem \a size. + * \sa GeneralizedEigenSolver() + */ + explicit GeneralizedEigenSolver(Index size) + : m_eivec(size, size), + m_alphas(size), + m_betas(size), + m_valuesOkay(false), + m_vectorsOkay(false), + m_realQZ(size), + m_tmp(size) + {} + + /** \brief Constructor; computes the generalized eigendecomposition of given matrix pair. + * + * \param[in] A Square matrix whose eigendecomposition is to be computed. + * \param[in] B Square matrix whose eigendecomposition is to be computed. + * \param[in] computeEigenvectors If true, both the eigenvectors and the + * eigenvalues are computed; if false, only the eigenvalues are computed. + * + * This constructor calls compute() to compute the generalized eigenvalues + * and eigenvectors. + * + * \sa compute() + */ + GeneralizedEigenSolver(const MatrixType& A, const MatrixType& B, bool computeEigenvectors = true) + : m_eivec(A.rows(), A.cols()), + m_alphas(A.cols()), + m_betas(A.cols()), + m_valuesOkay(false), + m_vectorsOkay(false), + m_realQZ(A.cols()), + m_tmp(A.cols()) + { + compute(A, B, computeEigenvectors); + } + + /* \brief Returns the computed generalized eigenvectors. + * + * \returns %Matrix whose columns are the (possibly complex) right eigenvectors. + * i.e. the eigenvectors that solve (A - l*B)x = 0. The ordering matches the eigenvalues. + * + * \pre Either the constructor + * GeneralizedEigenSolver(const MatrixType&,const MatrixType&, bool) or the member function + * compute(const MatrixType&, const MatrixType& bool) has been called before, and + * \p computeEigenvectors was set to true (the default). + * + * \sa eigenvalues() + */ + EigenvectorsType eigenvectors() const { + eigen_assert(m_vectorsOkay && "Eigenvectors for GeneralizedEigenSolver were not calculated."); + return m_eivec; + } + + /** \brief Returns an expression of the computed generalized eigenvalues. + * + * \returns An expression of the column vector containing the eigenvalues. + * + * It is a shortcut for \code this->alphas().cwiseQuotient(this->betas()); \endcode + * Not that betas might contain zeros. It is therefore not recommended to use this function, + * but rather directly deal with the alphas and betas vectors. + * + * \pre Either the constructor + * GeneralizedEigenSolver(const MatrixType&,const MatrixType&,bool) or the member function + * compute(const MatrixType&,const MatrixType&,bool) has been called before. + * + * The eigenvalues are repeated according to their algebraic multiplicity, + * so there are as many eigenvalues as rows in the matrix. The eigenvalues + * are not sorted in any particular order. + * + * \sa alphas(), betas(), eigenvectors() + */ + EigenvalueType eigenvalues() const + { + eigen_assert(m_valuesOkay && "GeneralizedEigenSolver is not initialized."); + return EigenvalueType(m_alphas,m_betas); + } + + /** \returns A const reference to the vectors containing the alpha values + * + * This vector permits to reconstruct the j-th eigenvalues as alphas(i)/betas(j). + * + * \sa betas(), eigenvalues() */ + ComplexVectorType alphas() const + { + eigen_assert(m_valuesOkay && "GeneralizedEigenSolver is not initialized."); + return m_alphas; + } + + /** \returns A const reference to the vectors containing the beta values + * + * This vector permits to reconstruct the j-th eigenvalues as alphas(i)/betas(j). + * + * \sa alphas(), eigenvalues() */ + VectorType betas() const + { + eigen_assert(m_valuesOkay && "GeneralizedEigenSolver is not initialized."); + return m_betas; + } + + /** \brief Computes generalized eigendecomposition of given matrix. + * + * \param[in] A Square matrix whose eigendecomposition is to be computed. + * \param[in] B Square matrix whose eigendecomposition is to be computed. + * \param[in] computeEigenvectors If true, both the eigenvectors and the + * eigenvalues are computed; if false, only the eigenvalues are + * computed. + * \returns Reference to \c *this + * + * This function computes the eigenvalues of the real matrix \p matrix. + * The eigenvalues() function can be used to retrieve them. If + * \p computeEigenvectors is true, then the eigenvectors are also computed + * and can be retrieved by calling eigenvectors(). + * + * The matrix is first reduced to real generalized Schur form using the RealQZ + * class. The generalized Schur decomposition is then used to compute the eigenvalues + * and eigenvectors. + * + * The cost of the computation is dominated by the cost of the + * generalized Schur decomposition. + * + * This method reuses of the allocated data in the GeneralizedEigenSolver object. + */ + GeneralizedEigenSolver& compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors = true); + + ComputationInfo info() const + { + eigen_assert(m_valuesOkay && "EigenSolver is not initialized."); + return m_realQZ.info(); + } + + /** Sets the maximal number of iterations allowed. + */ + GeneralizedEigenSolver& setMaxIterations(Index maxIters) + { + m_realQZ.setMaxIterations(maxIters); + return *this; + } + + protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + EIGEN_STATIC_ASSERT(!NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL); + } + + EigenvectorsType m_eivec; + ComplexVectorType m_alphas; + VectorType m_betas; + bool m_valuesOkay, m_vectorsOkay; + RealQZ m_realQZ; + ComplexVectorType m_tmp; +}; + +template +GeneralizedEigenSolver& +GeneralizedEigenSolver::compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors) +{ + check_template_parameters(); + + using std::sqrt; + using std::abs; + eigen_assert(A.cols() == A.rows() && B.cols() == A.rows() && B.cols() == B.rows()); + Index size = A.cols(); + m_valuesOkay = false; + m_vectorsOkay = false; + // Reduce to generalized real Schur form: + // A = Q S Z and B = Q T Z + m_realQZ.compute(A, B, computeEigenvectors); + if (m_realQZ.info() == Success) + { + // Resize storage + m_alphas.resize(size); + m_betas.resize(size); + if (computeEigenvectors) + { + m_eivec.resize(size,size); + m_tmp.resize(size); + } + + // Aliases: + Map v(reinterpret_cast(m_tmp.data()), size); + ComplexVectorType &cv = m_tmp; + const MatrixType &mS = m_realQZ.matrixS(); + const MatrixType &mT = m_realQZ.matrixT(); + + Index i = 0; + while (i < size) + { + if (i == size - 1 || mS.coeff(i+1, i) == Scalar(0)) + { + // Real eigenvalue + m_alphas.coeffRef(i) = mS.diagonal().coeff(i); + m_betas.coeffRef(i) = mT.diagonal().coeff(i); + if (computeEigenvectors) + { + v.setConstant(Scalar(0.0)); + v.coeffRef(i) = Scalar(1.0); + // For singular eigenvalues do nothing more + if(abs(m_betas.coeffRef(i)) >= (std::numeric_limits::min)()) + { + // Non-singular eigenvalue + const Scalar alpha = real(m_alphas.coeffRef(i)); + const Scalar beta = m_betas.coeffRef(i); + for (Index j = i-1; j >= 0; j--) + { + const Index st = j+1; + const Index sz = i-j; + if (j > 0 && mS.coeff(j, j-1) != Scalar(0)) + { + // 2x2 block + Matrix rhs = (alpha*mT.template block<2,Dynamic>(j-1,st,2,sz) - beta*mS.template block<2,Dynamic>(j-1,st,2,sz)) .lazyProduct( v.segment(st,sz) ); + Matrix lhs = beta * mS.template block<2,2>(j-1,j-1) - alpha * mT.template block<2,2>(j-1,j-1); + v.template segment<2>(j-1) = lhs.partialPivLu().solve(rhs); + j--; + } + else + { + v.coeffRef(j) = -v.segment(st,sz).transpose().cwiseProduct(beta*mS.block(j,st,1,sz) - alpha*mT.block(j,st,1,sz)).sum() / (beta*mS.coeffRef(j,j) - alpha*mT.coeffRef(j,j)); + } + } + } + m_eivec.col(i).real().noalias() = m_realQZ.matrixZ().transpose() * v; + m_eivec.col(i).real().normalize(); + m_eivec.col(i).imag().setConstant(0); + } + ++i; + } + else + { + // We need to extract the generalized eigenvalues of the pair of a general 2x2 block S and a positive diagonal 2x2 block T + // Then taking beta=T_00*T_11, we can avoid any division, and alpha is the eigenvalues of A = (U^-1 * S * U) * diag(T_11,T_00): + + // T = [a 0] + // [0 b] + RealScalar a = mT.diagonal().coeff(i), + b = mT.diagonal().coeff(i+1); + const RealScalar beta = m_betas.coeffRef(i) = m_betas.coeffRef(i+1) = a*b; + + // ^^ NOTE: using diagonal()(i) instead of coeff(i,i) workarounds a MSVC bug. + Matrix S2 = mS.template block<2,2>(i,i) * Matrix(b,a).asDiagonal(); + + Scalar p = Scalar(0.5) * (S2.coeff(0,0) - S2.coeff(1,1)); + Scalar z = sqrt(abs(p * p + S2.coeff(1,0) * S2.coeff(0,1))); + const ComplexScalar alpha = ComplexScalar(S2.coeff(1,1) + p, (beta > 0) ? z : -z); + m_alphas.coeffRef(i) = conj(alpha); + m_alphas.coeffRef(i+1) = alpha; + + if (computeEigenvectors) { + // Compute eigenvector in position (i+1) and then position (i) is just the conjugate + cv.setZero(); + cv.coeffRef(i+1) = Scalar(1.0); + // here, the "static_cast" workaound expression template issues. + cv.coeffRef(i) = -(static_cast(beta*mS.coeffRef(i,i+1)) - alpha*mT.coeffRef(i,i+1)) + / (static_cast(beta*mS.coeffRef(i,i)) - alpha*mT.coeffRef(i,i)); + for (Index j = i-1; j >= 0; j--) + { + const Index st = j+1; + const Index sz = i+1-j; + if (j > 0 && mS.coeff(j, j-1) != Scalar(0)) + { + // 2x2 block + Matrix rhs = (alpha*mT.template block<2,Dynamic>(j-1,st,2,sz) - beta*mS.template block<2,Dynamic>(j-1,st,2,sz)) .lazyProduct( cv.segment(st,sz) ); + Matrix lhs = beta * mS.template block<2,2>(j-1,j-1) - alpha * mT.template block<2,2>(j-1,j-1); + cv.template segment<2>(j-1) = lhs.partialPivLu().solve(rhs); + j--; + } else { + cv.coeffRef(j) = cv.segment(st,sz).transpose().cwiseProduct(beta*mS.block(j,st,1,sz) - alpha*mT.block(j,st,1,sz)).sum() + / (alpha*mT.coeffRef(j,j) - static_cast(beta*mS.coeffRef(j,j))); + } + } + m_eivec.col(i+1).noalias() = (m_realQZ.matrixZ().transpose() * cv); + m_eivec.col(i+1).normalize(); + m_eivec.col(i) = m_eivec.col(i+1).conjugate(); + } + i += 2; + } + } + + m_valuesOkay = true; + m_vectorsOkay = computeEigenvectors; + } + return *this; +} + +} // end namespace Eigen + +#endif // EIGEN_GENERALIZEDEIGENSOLVER_H diff --git a/3rdparty/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/3rdparty/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h new file mode 100644 index 0000000..d0f9091 --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h @@ -0,0 +1,226 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2010 Gael Guennebaud +// Copyright (C) 2010 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_GENERALIZEDSELFADJOINTEIGENSOLVER_H +#define EIGEN_GENERALIZEDSELFADJOINTEIGENSOLVER_H + +#include "./Tridiagonalization.h" + +namespace Eigen { + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class GeneralizedSelfAdjointEigenSolver + * + * \brief Computes eigenvalues and eigenvectors of the generalized selfadjoint eigen problem + * + * \tparam _MatrixType the type of the matrix of which we are computing the + * eigendecomposition; this is expected to be an instantiation of the Matrix + * class template. + * + * This class solves the generalized eigenvalue problem + * \f$ Av = \lambda Bv \f$. In this case, the matrix \f$ A \f$ should be + * selfadjoint and the matrix \f$ B \f$ should be positive definite. + * + * Only the \b lower \b triangular \b part of the input matrix is referenced. + * + * Call the function compute() to compute the eigenvalues and eigenvectors of + * a given matrix. Alternatively, you can use the + * GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int) + * constructor which computes the eigenvalues and eigenvectors at construction time. + * Once the eigenvalue and eigenvectors are computed, they can be retrieved with the eigenvalues() + * and eigenvectors() functions. + * + * The documentation for GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int) + * contains an example of the typical use of this class. + * + * \sa class SelfAdjointEigenSolver, class EigenSolver, class ComplexEigenSolver + */ +template +class GeneralizedSelfAdjointEigenSolver : public SelfAdjointEigenSolver<_MatrixType> +{ + typedef SelfAdjointEigenSolver<_MatrixType> Base; + public: + + typedef _MatrixType MatrixType; + + /** \brief Default constructor for fixed-size matrices. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via compute(). This constructor + * can only be used if \p _MatrixType is a fixed-size matrix; use + * GeneralizedSelfAdjointEigenSolver(Index) for dynamic-size matrices. + */ + GeneralizedSelfAdjointEigenSolver() : Base() {} + + /** \brief Constructor, pre-allocates memory for dynamic-size matrices. + * + * \param [in] size Positive integer, size of the matrix whose + * eigenvalues and eigenvectors will be computed. + * + * This constructor is useful for dynamic-size matrices, when the user + * intends to perform decompositions via compute(). The \p size + * parameter is only used as a hint. It is not an error to give a wrong + * \p size, but it may impair performance. + * + * \sa compute() for an example + */ + explicit GeneralizedSelfAdjointEigenSolver(Index size) + : Base(size) + {} + + /** \brief Constructor; computes generalized eigendecomposition of given matrix pencil. + * + * \param[in] matA Selfadjoint matrix in matrix pencil. + * Only the lower triangular part of the matrix is referenced. + * \param[in] matB Positive-definite matrix in matrix pencil. + * Only the lower triangular part of the matrix is referenced. + * \param[in] options A or-ed set of flags {#ComputeEigenvectors,#EigenvaluesOnly} | {#Ax_lBx,#ABx_lx,#BAx_lx}. + * Default is #ComputeEigenvectors|#Ax_lBx. + * + * This constructor calls compute(const MatrixType&, const MatrixType&, int) + * to compute the eigenvalues and (if requested) the eigenvectors of the + * generalized eigenproblem \f$ Ax = \lambda B x \f$ with \a matA the + * selfadjoint matrix \f$ A \f$ and \a matB the positive definite matrix + * \f$ B \f$. Each eigenvector \f$ x \f$ satisfies the property + * \f$ x^* B x = 1 \f$. The eigenvectors are computed if + * \a options contains ComputeEigenvectors. + * + * In addition, the two following variants can be solved via \p options: + * - \c ABx_lx: \f$ ABx = \lambda x \f$ + * - \c BAx_lx: \f$ BAx = \lambda x \f$ + * + * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType2.cpp + * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType2.out + * + * \sa compute(const MatrixType&, const MatrixType&, int) + */ + GeneralizedSelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB, + int options = ComputeEigenvectors|Ax_lBx) + : Base(matA.cols()) + { + compute(matA, matB, options); + } + + /** \brief Computes generalized eigendecomposition of given matrix pencil. + * + * \param[in] matA Selfadjoint matrix in matrix pencil. + * Only the lower triangular part of the matrix is referenced. + * \param[in] matB Positive-definite matrix in matrix pencil. + * Only the lower triangular part of the matrix is referenced. + * \param[in] options A or-ed set of flags {#ComputeEigenvectors,#EigenvaluesOnly} | {#Ax_lBx,#ABx_lx,#BAx_lx}. + * Default is #ComputeEigenvectors|#Ax_lBx. + * + * \returns Reference to \c *this + * + * According to \p options, this function computes eigenvalues and (if requested) + * the eigenvectors of one of the following three generalized eigenproblems: + * - \c Ax_lBx: \f$ Ax = \lambda B x \f$ + * - \c ABx_lx: \f$ ABx = \lambda x \f$ + * - \c BAx_lx: \f$ BAx = \lambda x \f$ + * with \a matA the selfadjoint matrix \f$ A \f$ and \a matB the positive definite + * matrix \f$ B \f$. + * In addition, each eigenvector \f$ x \f$ satisfies the property \f$ x^* B x = 1 \f$. + * + * The eigenvalues() function can be used to retrieve + * the eigenvalues. If \p options contains ComputeEigenvectors, then the + * eigenvectors are also computed and can be retrieved by calling + * eigenvectors(). + * + * The implementation uses LLT to compute the Cholesky decomposition + * \f$ B = LL^* \f$ and computes the classical eigendecomposition + * of the selfadjoint matrix \f$ L^{-1} A (L^*)^{-1} \f$ if \p options contains Ax_lBx + * and of \f$ L^{*} A L \f$ otherwise. This solves the + * generalized eigenproblem, because any solution of the generalized + * eigenproblem \f$ Ax = \lambda B x \f$ corresponds to a solution + * \f$ L^{-1} A (L^*)^{-1} (L^* x) = \lambda (L^* x) \f$ of the + * eigenproblem for \f$ L^{-1} A (L^*)^{-1} \f$. Similar statements + * can be made for the two other variants. + * + * Example: \include SelfAdjointEigenSolver_compute_MatrixType2.cpp + * Output: \verbinclude SelfAdjointEigenSolver_compute_MatrixType2.out + * + * \sa GeneralizedSelfAdjointEigenSolver(const MatrixType&, const MatrixType&, int) + */ + GeneralizedSelfAdjointEigenSolver& compute(const MatrixType& matA, const MatrixType& matB, + int options = ComputeEigenvectors|Ax_lBx); + + protected: + +}; + + +template +GeneralizedSelfAdjointEigenSolver& GeneralizedSelfAdjointEigenSolver:: +compute(const MatrixType& matA, const MatrixType& matB, int options) +{ + eigen_assert(matA.cols()==matA.rows() && matB.rows()==matA.rows() && matB.cols()==matB.rows()); + eigen_assert((options&~(EigVecMask|GenEigMask))==0 + && (options&EigVecMask)!=EigVecMask + && ((options&GenEigMask)==0 || (options&GenEigMask)==Ax_lBx + || (options&GenEigMask)==ABx_lx || (options&GenEigMask)==BAx_lx) + && "invalid option parameter"); + + bool computeEigVecs = ((options&EigVecMask)==0) || ((options&EigVecMask)==ComputeEigenvectors); + + // Compute the cholesky decomposition of matB = L L' = U'U + LLT cholB(matB); + + int type = (options&GenEigMask); + if(type==0) + type = Ax_lBx; + + if(type==Ax_lBx) + { + // compute C = inv(L) A inv(L') + MatrixType matC = matA.template selfadjointView(); + cholB.matrixL().template solveInPlace(matC); + cholB.matrixU().template solveInPlace(matC); + + Base::compute(matC, computeEigVecs ? ComputeEigenvectors : EigenvaluesOnly ); + + // transform back the eigen vectors: evecs = inv(U) * evecs + if(computeEigVecs) + cholB.matrixU().solveInPlace(Base::m_eivec); + } + else if(type==ABx_lx) + { + // compute C = L' A L + MatrixType matC = matA.template selfadjointView(); + matC = matC * cholB.matrixL(); + matC = cholB.matrixU() * matC; + + Base::compute(matC, computeEigVecs ? ComputeEigenvectors : EigenvaluesOnly); + + // transform back the eigen vectors: evecs = inv(U) * evecs + if(computeEigVecs) + cholB.matrixU().solveInPlace(Base::m_eivec); + } + else if(type==BAx_lx) + { + // compute C = L' A L + MatrixType matC = matA.template selfadjointView(); + matC = matC * cholB.matrixL(); + matC = cholB.matrixU() * matC; + + Base::compute(matC, computeEigVecs ? ComputeEigenvectors : EigenvaluesOnly); + + // transform back the eigen vectors: evecs = L * evecs + if(computeEigVecs) + Base::m_eivec = cholB.matrixL() * Base::m_eivec; + } + + return *this; +} + +} // end namespace Eigen + +#endif // EIGEN_GENERALIZEDSELFADJOINTEIGENSOLVER_H diff --git a/3rdparty/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/3rdparty/Eigen/src/Eigenvalues/HessenbergDecomposition.h new file mode 100644 index 0000000..1f21139 --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/HessenbergDecomposition.h @@ -0,0 +1,374 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud +// Copyright (C) 2010 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_HESSENBERGDECOMPOSITION_H +#define EIGEN_HESSENBERGDECOMPOSITION_H + +namespace Eigen { + +namespace internal { + +template struct HessenbergDecompositionMatrixHReturnType; +template +struct traits > +{ + typedef MatrixType ReturnType; +}; + +} + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class HessenbergDecomposition + * + * \brief Reduces a square matrix to Hessenberg form by an orthogonal similarity transformation + * + * \tparam _MatrixType the type of the matrix of which we are computing the Hessenberg decomposition + * + * This class performs an Hessenberg decomposition of a matrix \f$ A \f$. In + * the real case, the Hessenberg decomposition consists of an orthogonal + * matrix \f$ Q \f$ and a Hessenberg matrix \f$ H \f$ such that \f$ A = Q H + * Q^T \f$. An orthogonal matrix is a matrix whose inverse equals its + * transpose (\f$ Q^{-1} = Q^T \f$). A Hessenberg matrix has zeros below the + * subdiagonal, so it is almost upper triangular. The Hessenberg decomposition + * of a complex matrix is \f$ A = Q H Q^* \f$ with \f$ Q \f$ unitary (that is, + * \f$ Q^{-1} = Q^* \f$). + * + * Call the function compute() to compute the Hessenberg decomposition of a + * given matrix. Alternatively, you can use the + * HessenbergDecomposition(const MatrixType&) constructor which computes the + * Hessenberg decomposition at construction time. Once the decomposition is + * computed, you can use the matrixH() and matrixQ() functions to construct + * the matrices H and Q in the decomposition. + * + * The documentation for matrixH() contains an example of the typical use of + * this class. + * + * \sa class ComplexSchur, class Tridiagonalization, \ref QR_Module "QR Module" + */ +template class HessenbergDecomposition +{ + public: + + /** \brief Synonym for the template parameter \p _MatrixType. */ + typedef _MatrixType MatrixType; + + enum { + Size = MatrixType::RowsAtCompileTime, + SizeMinusOne = Size == Dynamic ? Dynamic : Size - 1, + Options = MatrixType::Options, + MaxSize = MatrixType::MaxRowsAtCompileTime, + MaxSizeMinusOne = MaxSize == Dynamic ? Dynamic : MaxSize - 1 + }; + + /** \brief Scalar type for matrices of type #MatrixType. */ + typedef typename MatrixType::Scalar Scalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + /** \brief Type for vector of Householder coefficients. + * + * This is column vector with entries of type #Scalar. The length of the + * vector is one less than the size of #MatrixType, if it is a fixed-side + * type. + */ + typedef Matrix CoeffVectorType; + + /** \brief Return type of matrixQ() */ + typedef HouseholderSequence::type> HouseholderSequenceType; + + typedef internal::HessenbergDecompositionMatrixHReturnType MatrixHReturnType; + + /** \brief Default constructor; the decomposition will be computed later. + * + * \param [in] size The size of the matrix whose Hessenberg decomposition will be computed. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via compute(). The \p size parameter is only + * used as a hint. It is not an error to give a wrong \p size, but it may + * impair performance. + * + * \sa compute() for an example. + */ + explicit HessenbergDecomposition(Index size = Size==Dynamic ? 2 : Size) + : m_matrix(size,size), + m_temp(size), + m_isInitialized(false) + { + if(size>1) + m_hCoeffs.resize(size-1); + } + + /** \brief Constructor; computes Hessenberg decomposition of given matrix. + * + * \param[in] matrix Square matrix whose Hessenberg decomposition is to be computed. + * + * This constructor calls compute() to compute the Hessenberg + * decomposition. + * + * \sa matrixH() for an example. + */ + template + explicit HessenbergDecomposition(const EigenBase& matrix) + : m_matrix(matrix.derived()), + m_temp(matrix.rows()), + m_isInitialized(false) + { + if(matrix.rows()<2) + { + m_isInitialized = true; + return; + } + m_hCoeffs.resize(matrix.rows()-1,1); + _compute(m_matrix, m_hCoeffs, m_temp); + m_isInitialized = true; + } + + /** \brief Computes Hessenberg decomposition of given matrix. + * + * \param[in] matrix Square matrix whose Hessenberg decomposition is to be computed. + * \returns Reference to \c *this + * + * The Hessenberg decomposition is computed by bringing the columns of the + * matrix successively in the required form using Householder reflections + * (see, e.g., Algorithm 7.4.2 in Golub \& Van Loan, %Matrix + * Computations). The cost is \f$ 10n^3/3 \f$ flops, where \f$ n \f$ + * denotes the size of the given matrix. + * + * This method reuses of the allocated data in the HessenbergDecomposition + * object. + * + * Example: \include HessenbergDecomposition_compute.cpp + * Output: \verbinclude HessenbergDecomposition_compute.out + */ + template + HessenbergDecomposition& compute(const EigenBase& matrix) + { + m_matrix = matrix.derived(); + if(matrix.rows()<2) + { + m_isInitialized = true; + return *this; + } + m_hCoeffs.resize(matrix.rows()-1,1); + _compute(m_matrix, m_hCoeffs, m_temp); + m_isInitialized = true; + return *this; + } + + /** \brief Returns the Householder coefficients. + * + * \returns a const reference to the vector of Householder coefficients + * + * \pre Either the constructor HessenbergDecomposition(const MatrixType&) + * or the member function compute(const MatrixType&) has been called + * before to compute the Hessenberg decomposition of a matrix. + * + * The Householder coefficients allow the reconstruction of the matrix + * \f$ Q \f$ in the Hessenberg decomposition from the packed data. + * + * \sa packedMatrix(), \ref Householder_Module "Householder module" + */ + const CoeffVectorType& householderCoefficients() const + { + eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized."); + return m_hCoeffs; + } + + /** \brief Returns the internal representation of the decomposition + * + * \returns a const reference to a matrix with the internal representation + * of the decomposition. + * + * \pre Either the constructor HessenbergDecomposition(const MatrixType&) + * or the member function compute(const MatrixType&) has been called + * before to compute the Hessenberg decomposition of a matrix. + * + * The returned matrix contains the following information: + * - the upper part and lower sub-diagonal represent the Hessenberg matrix H + * - the rest of the lower part contains the Householder vectors that, combined with + * Householder coefficients returned by householderCoefficients(), + * allows to reconstruct the matrix Q as + * \f$ Q = H_{N-1} \ldots H_1 H_0 \f$. + * Here, the matrices \f$ H_i \f$ are the Householder transformations + * \f$ H_i = (I - h_i v_i v_i^T) \f$ + * where \f$ h_i \f$ is the \f$ i \f$th Householder coefficient and + * \f$ v_i \f$ is the Householder vector defined by + * \f$ v_i = [ 0, \ldots, 0, 1, M(i+2,i), \ldots, M(N-1,i) ]^T \f$ + * with M the matrix returned by this function. + * + * See LAPACK for further details on this packed storage. + * + * Example: \include HessenbergDecomposition_packedMatrix.cpp + * Output: \verbinclude HessenbergDecomposition_packedMatrix.out + * + * \sa householderCoefficients() + */ + const MatrixType& packedMatrix() const + { + eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized."); + return m_matrix; + } + + /** \brief Reconstructs the orthogonal matrix Q in the decomposition + * + * \returns object representing the matrix Q + * + * \pre Either the constructor HessenbergDecomposition(const MatrixType&) + * or the member function compute(const MatrixType&) has been called + * before to compute the Hessenberg decomposition of a matrix. + * + * This function returns a light-weight object of template class + * HouseholderSequence. You can either apply it directly to a matrix or + * you can convert it to a matrix of type #MatrixType. + * + * \sa matrixH() for an example, class HouseholderSequence + */ + HouseholderSequenceType matrixQ() const + { + eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized."); + return HouseholderSequenceType(m_matrix, m_hCoeffs.conjugate()) + .setLength(m_matrix.rows() - 1) + .setShift(1); + } + + /** \brief Constructs the Hessenberg matrix H in the decomposition + * + * \returns expression object representing the matrix H + * + * \pre Either the constructor HessenbergDecomposition(const MatrixType&) + * or the member function compute(const MatrixType&) has been called + * before to compute the Hessenberg decomposition of a matrix. + * + * The object returned by this function constructs the Hessenberg matrix H + * when it is assigned to a matrix or otherwise evaluated. The matrix H is + * constructed from the packed matrix as returned by packedMatrix(): The + * upper part (including the subdiagonal) of the packed matrix contains + * the matrix H. It may sometimes be better to directly use the packed + * matrix instead of constructing the matrix H. + * + * Example: \include HessenbergDecomposition_matrixH.cpp + * Output: \verbinclude HessenbergDecomposition_matrixH.out + * + * \sa matrixQ(), packedMatrix() + */ + MatrixHReturnType matrixH() const + { + eigen_assert(m_isInitialized && "HessenbergDecomposition is not initialized."); + return MatrixHReturnType(*this); + } + + private: + + typedef Matrix VectorType; + typedef typename NumTraits::Real RealScalar; + static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs, VectorType& temp); + + protected: + MatrixType m_matrix; + CoeffVectorType m_hCoeffs; + VectorType m_temp; + bool m_isInitialized; +}; + +/** \internal + * Performs a tridiagonal decomposition of \a matA in place. + * + * \param matA the input selfadjoint matrix + * \param hCoeffs returned Householder coefficients + * + * The result is written in the lower triangular part of \a matA. + * + * Implemented from Golub's "%Matrix Computations", algorithm 8.3.1. + * + * \sa packedMatrix() + */ +template +void HessenbergDecomposition::_compute(MatrixType& matA, CoeffVectorType& hCoeffs, VectorType& temp) +{ + eigen_assert(matA.rows()==matA.cols()); + Index n = matA.rows(); + temp.resize(n); + for (Index i = 0; i struct HessenbergDecompositionMatrixHReturnType +: public ReturnByValue > +{ + public: + /** \brief Constructor. + * + * \param[in] hess Hessenberg decomposition + */ + HessenbergDecompositionMatrixHReturnType(const HessenbergDecomposition& hess) : m_hess(hess) { } + + /** \brief Hessenberg matrix in decomposition. + * + * \param[out] result Hessenberg matrix in decomposition \p hess which + * was passed to the constructor + */ + template + inline void evalTo(ResultType& result) const + { + result = m_hess.packedMatrix(); + Index n = result.rows(); + if (n>2) + result.bottomLeftCorner(n-2, n-2).template triangularView().setZero(); + } + + Index rows() const { return m_hess.packedMatrix().rows(); } + Index cols() const { return m_hess.packedMatrix().cols(); } + + protected: + const HessenbergDecomposition& m_hess; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_HESSENBERGDECOMPOSITION_H diff --git a/3rdparty/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/3rdparty/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h new file mode 100644 index 0000000..66e5a3d --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h @@ -0,0 +1,158 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2010 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_MATRIXBASEEIGENVALUES_H +#define EIGEN_MATRIXBASEEIGENVALUES_H + +namespace Eigen { + +namespace internal { + +template +struct eigenvalues_selector +{ + // this is the implementation for the case IsComplex = true + static inline typename MatrixBase::EigenvaluesReturnType const + run(const MatrixBase& m) + { + typedef typename Derived::PlainObject PlainObject; + PlainObject m_eval(m); + return ComplexEigenSolver(m_eval, false).eigenvalues(); + } +}; + +template +struct eigenvalues_selector +{ + static inline typename MatrixBase::EigenvaluesReturnType const + run(const MatrixBase& m) + { + typedef typename Derived::PlainObject PlainObject; + PlainObject m_eval(m); + return EigenSolver(m_eval, false).eigenvalues(); + } +}; + +} // end namespace internal + +/** \brief Computes the eigenvalues of a matrix + * \returns Column vector containing the eigenvalues. + * + * \eigenvalues_module + * This function computes the eigenvalues with the help of the EigenSolver + * class (for real matrices) or the ComplexEigenSolver class (for complex + * matrices). + * + * The eigenvalues are repeated according to their algebraic multiplicity, + * so there are as many eigenvalues as rows in the matrix. + * + * The SelfAdjointView class provides a better algorithm for selfadjoint + * matrices. + * + * Example: \include MatrixBase_eigenvalues.cpp + * Output: \verbinclude MatrixBase_eigenvalues.out + * + * \sa EigenSolver::eigenvalues(), ComplexEigenSolver::eigenvalues(), + * SelfAdjointView::eigenvalues() + */ +template +inline typename MatrixBase::EigenvaluesReturnType +MatrixBase::eigenvalues() const +{ + return internal::eigenvalues_selector::IsComplex>::run(derived()); +} + +/** \brief Computes the eigenvalues of a matrix + * \returns Column vector containing the eigenvalues. + * + * \eigenvalues_module + * This function computes the eigenvalues with the help of the + * SelfAdjointEigenSolver class. The eigenvalues are repeated according to + * their algebraic multiplicity, so there are as many eigenvalues as rows in + * the matrix. + * + * Example: \include SelfAdjointView_eigenvalues.cpp + * Output: \verbinclude SelfAdjointView_eigenvalues.out + * + * \sa SelfAdjointEigenSolver::eigenvalues(), MatrixBase::eigenvalues() + */ +template +EIGEN_DEVICE_FUNC inline typename SelfAdjointView::EigenvaluesReturnType +SelfAdjointView::eigenvalues() const +{ + PlainObject thisAsMatrix(*this); + return SelfAdjointEigenSolver(thisAsMatrix, false).eigenvalues(); +} + + + +/** \brief Computes the L2 operator norm + * \returns Operator norm of the matrix. + * + * \eigenvalues_module + * This function computes the L2 operator norm of a matrix, which is also + * known as the spectral norm. The norm of a matrix \f$ A \f$ is defined to be + * \f[ \|A\|_2 = \max_x \frac{\|Ax\|_2}{\|x\|_2} \f] + * where the maximum is over all vectors and the norm on the right is the + * Euclidean vector norm. The norm equals the largest singular value, which is + * the square root of the largest eigenvalue of the positive semi-definite + * matrix \f$ A^*A \f$. + * + * The current implementation uses the eigenvalues of \f$ A^*A \f$, as computed + * by SelfAdjointView::eigenvalues(), to compute the operator norm of a + * matrix. The SelfAdjointView class provides a better algorithm for + * selfadjoint matrices. + * + * Example: \include MatrixBase_operatorNorm.cpp + * Output: \verbinclude MatrixBase_operatorNorm.out + * + * \sa SelfAdjointView::eigenvalues(), SelfAdjointView::operatorNorm() + */ +template +inline typename MatrixBase::RealScalar +MatrixBase::operatorNorm() const +{ + using std::sqrt; + typename Derived::PlainObject m_eval(derived()); + // FIXME if it is really guaranteed that the eigenvalues are already sorted, + // then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough. + return sqrt((m_eval*m_eval.adjoint()) + .eval() + .template selfadjointView() + .eigenvalues() + .maxCoeff() + ); +} + +/** \brief Computes the L2 operator norm + * \returns Operator norm of the matrix. + * + * \eigenvalues_module + * This function computes the L2 operator norm of a self-adjoint matrix. For a + * self-adjoint matrix, the operator norm is the largest eigenvalue. + * + * The current implementation uses the eigenvalues of the matrix, as computed + * by eigenvalues(), to compute the operator norm of the matrix. + * + * Example: \include SelfAdjointView_operatorNorm.cpp + * Output: \verbinclude SelfAdjointView_operatorNorm.out + * + * \sa eigenvalues(), MatrixBase::operatorNorm() + */ +template +EIGEN_DEVICE_FUNC inline typename SelfAdjointView::RealScalar +SelfAdjointView::operatorNorm() const +{ + return eigenvalues().cwiseAbs().maxCoeff(); +} + +} // end namespace Eigen + +#endif diff --git a/3rdparty/Eigen/src/Eigenvalues/RealQZ.h b/3rdparty/Eigen/src/Eigenvalues/RealQZ.h new file mode 100644 index 0000000..5091301 --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/RealQZ.h @@ -0,0 +1,657 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Alexey Korepanov +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_REAL_QZ_H +#define EIGEN_REAL_QZ_H + +namespace Eigen { + + /** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class RealQZ + * + * \brief Performs a real QZ decomposition of a pair of square matrices + * + * \tparam _MatrixType the type of the matrix of which we are computing the + * real QZ decomposition; this is expected to be an instantiation of the + * Matrix class template. + * + * Given a real square matrices A and B, this class computes the real QZ + * decomposition: \f$ A = Q S Z \f$, \f$ B = Q T Z \f$ where Q and Z are + * real orthogonal matrixes, T is upper-triangular matrix, and S is upper + * quasi-triangular matrix. An orthogonal matrix is a matrix whose + * inverse is equal to its transpose, \f$ U^{-1} = U^T \f$. A quasi-triangular + * matrix is a block-triangular matrix whose diagonal consists of 1-by-1 + * blocks and 2-by-2 blocks where further reduction is impossible due to + * complex eigenvalues. + * + * The eigenvalues of the pencil \f$ A - z B \f$ can be obtained from + * 1x1 and 2x2 blocks on the diagonals of S and T. + * + * Call the function compute() to compute the real QZ decomposition of a + * given pair of matrices. Alternatively, you can use the + * RealQZ(const MatrixType& B, const MatrixType& B, bool computeQZ) + * constructor which computes the real QZ decomposition at construction + * time. Once the decomposition is computed, you can use the matrixS(), + * matrixT(), matrixQ() and matrixZ() functions to retrieve the matrices + * S, T, Q and Z in the decomposition. If computeQZ==false, some time + * is saved by not computing matrices Q and Z. + * + * Example: \include RealQZ_compute.cpp + * Output: \include RealQZ_compute.out + * + * \note The implementation is based on the algorithm in "Matrix Computations" + * by Gene H. Golub and Charles F. Van Loan, and a paper "An algorithm for + * generalized eigenvalue problems" by C.B.Moler and G.W.Stewart. + * + * \sa class RealSchur, class ComplexSchur, class EigenSolver, class ComplexEigenSolver + */ + + template class RealQZ + { + public: + typedef _MatrixType MatrixType; + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + Options = MatrixType::Options, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + typedef typename MatrixType::Scalar Scalar; + typedef std::complex::Real> ComplexScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + typedef Matrix EigenvalueType; + typedef Matrix ColumnVectorType; + + /** \brief Default constructor. + * + * \param [in] size Positive integer, size of the matrix whose QZ decomposition will be computed. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via compute(). The \p size parameter is only + * used as a hint. It is not an error to give a wrong \p size, but it may + * impair performance. + * + * \sa compute() for an example. + */ + explicit RealQZ(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime) : + m_S(size, size), + m_T(size, size), + m_Q(size, size), + m_Z(size, size), + m_workspace(size*2), + m_maxIters(400), + m_isInitialized(false), + m_computeQZ(true) + {} + + /** \brief Constructor; computes real QZ decomposition of given matrices + * + * \param[in] A Matrix A. + * \param[in] B Matrix B. + * \param[in] computeQZ If false, A and Z are not computed. + * + * This constructor calls compute() to compute the QZ decomposition. + */ + RealQZ(const MatrixType& A, const MatrixType& B, bool computeQZ = true) : + m_S(A.rows(),A.cols()), + m_T(A.rows(),A.cols()), + m_Q(A.rows(),A.cols()), + m_Z(A.rows(),A.cols()), + m_workspace(A.rows()*2), + m_maxIters(400), + m_isInitialized(false), + m_computeQZ(true) + { + compute(A, B, computeQZ); + } + + /** \brief Returns matrix Q in the QZ decomposition. + * + * \returns A const reference to the matrix Q. + */ + const MatrixType& matrixQ() const { + eigen_assert(m_isInitialized && "RealQZ is not initialized."); + eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition."); + return m_Q; + } + + /** \brief Returns matrix Z in the QZ decomposition. + * + * \returns A const reference to the matrix Z. + */ + const MatrixType& matrixZ() const { + eigen_assert(m_isInitialized && "RealQZ is not initialized."); + eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition."); + return m_Z; + } + + /** \brief Returns matrix S in the QZ decomposition. + * + * \returns A const reference to the matrix S. + */ + const MatrixType& matrixS() const { + eigen_assert(m_isInitialized && "RealQZ is not initialized."); + return m_S; + } + + /** \brief Returns matrix S in the QZ decomposition. + * + * \returns A const reference to the matrix S. + */ + const MatrixType& matrixT() const { + eigen_assert(m_isInitialized && "RealQZ is not initialized."); + return m_T; + } + + /** \brief Computes QZ decomposition of given matrix. + * + * \param[in] A Matrix A. + * \param[in] B Matrix B. + * \param[in] computeQZ If false, A and Z are not computed. + * \returns Reference to \c *this + */ + RealQZ& compute(const MatrixType& A, const MatrixType& B, bool computeQZ = true); + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, \c NoConvergence otherwise. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "RealQZ is not initialized."); + return m_info; + } + + /** \brief Returns number of performed QR-like iterations. + */ + Index iterations() const + { + eigen_assert(m_isInitialized && "RealQZ is not initialized."); + return m_global_iter; + } + + /** Sets the maximal number of iterations allowed to converge to one eigenvalue + * or decouple the problem. + */ + RealQZ& setMaxIterations(Index maxIters) + { + m_maxIters = maxIters; + return *this; + } + + private: + + MatrixType m_S, m_T, m_Q, m_Z; + Matrix m_workspace; + ComputationInfo m_info; + Index m_maxIters; + bool m_isInitialized; + bool m_computeQZ; + Scalar m_normOfT, m_normOfS; + Index m_global_iter; + + typedef Matrix Vector3s; + typedef Matrix Vector2s; + typedef Matrix Matrix2s; + typedef JacobiRotation JRs; + + void hessenbergTriangular(); + void computeNorms(); + Index findSmallSubdiagEntry(Index iu); + Index findSmallDiagEntry(Index f, Index l); + void splitOffTwoRows(Index i); + void pushDownZero(Index z, Index f, Index l); + void step(Index f, Index l, Index iter); + + }; // RealQZ + + /** \internal Reduces S and T to upper Hessenberg - triangular form */ + template + void RealQZ::hessenbergTriangular() + { + + const Index dim = m_S.cols(); + + // perform QR decomposition of T, overwrite T with R, save Q + HouseholderQR qrT(m_T); + m_T = qrT.matrixQR(); + m_T.template triangularView().setZero(); + m_Q = qrT.householderQ(); + // overwrite S with Q* S + m_S.applyOnTheLeft(m_Q.adjoint()); + // init Z as Identity + if (m_computeQZ) + m_Z = MatrixType::Identity(dim,dim); + // reduce S to upper Hessenberg with Givens rotations + for (Index j=0; j<=dim-3; j++) { + for (Index i=dim-1; i>=j+2; i--) { + JRs G; + // kill S(i,j) + if(m_S.coeff(i,j) != 0) + { + G.makeGivens(m_S.coeff(i-1,j), m_S.coeff(i,j), &m_S.coeffRef(i-1, j)); + m_S.coeffRef(i,j) = Scalar(0.0); + m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint()); + m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint()); + // update Q + if (m_computeQZ) + m_Q.applyOnTheRight(i-1,i,G); + } + // kill T(i,i-1) + if(m_T.coeff(i,i-1)!=Scalar(0)) + { + G.makeGivens(m_T.coeff(i,i), m_T.coeff(i,i-1), &m_T.coeffRef(i,i)); + m_T.coeffRef(i,i-1) = Scalar(0.0); + m_S.applyOnTheRight(i,i-1,G); + m_T.topRows(i).applyOnTheRight(i,i-1,G); + // update Z + if (m_computeQZ) + m_Z.applyOnTheLeft(i,i-1,G.adjoint()); + } + } + } + } + + /** \internal Computes vector L1 norms of S and T when in Hessenberg-Triangular form already */ + template + inline void RealQZ::computeNorms() + { + const Index size = m_S.cols(); + m_normOfS = Scalar(0.0); + m_normOfT = Scalar(0.0); + for (Index j = 0; j < size; ++j) + { + m_normOfS += m_S.col(j).segment(0, (std::min)(size,j+2)).cwiseAbs().sum(); + m_normOfT += m_T.row(j).segment(j, size - j).cwiseAbs().sum(); + } + } + + + /** \internal Look for single small sub-diagonal element S(res, res-1) and return res (or 0) */ + template + inline Index RealQZ::findSmallSubdiagEntry(Index iu) + { + using std::abs; + Index res = iu; + while (res > 0) + { + Scalar s = abs(m_S.coeff(res-1,res-1)) + abs(m_S.coeff(res,res)); + if (s == Scalar(0.0)) + s = m_normOfS; + if (abs(m_S.coeff(res,res-1)) < NumTraits::epsilon() * s) + break; + res--; + } + return res; + } + + /** \internal Look for single small diagonal element T(res, res) for res between f and l, and return res (or f-1) */ + template + inline Index RealQZ::findSmallDiagEntry(Index f, Index l) + { + using std::abs; + Index res = l; + while (res >= f) { + if (abs(m_T.coeff(res,res)) <= NumTraits::epsilon() * m_normOfT) + break; + res--; + } + return res; + } + + /** \internal decouple 2x2 diagonal block in rows i, i+1 if eigenvalues are real */ + template + inline void RealQZ::splitOffTwoRows(Index i) + { + using std::abs; + using std::sqrt; + const Index dim=m_S.cols(); + if (abs(m_S.coeff(i+1,i))==Scalar(0)) + return; + Index j = findSmallDiagEntry(i,i+1); + if (j==i-1) + { + // block of (S T^{-1}) + Matrix2s STi = m_T.template block<2,2>(i,i).template triangularView(). + template solve(m_S.template block<2,2>(i,i)); + Scalar p = Scalar(0.5)*(STi(0,0)-STi(1,1)); + Scalar q = p*p + STi(1,0)*STi(0,1); + if (q>=0) { + Scalar z = sqrt(q); + // one QR-like iteration for ABi - lambda I + // is enough - when we know exact eigenvalue in advance, + // convergence is immediate + JRs G; + if (p>=0) + G.makeGivens(p + z, STi(1,0)); + else + G.makeGivens(p - z, STi(1,0)); + m_S.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint()); + m_T.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint()); + // update Q + if (m_computeQZ) + m_Q.applyOnTheRight(i,i+1,G); + + G.makeGivens(m_T.coeff(i+1,i+1), m_T.coeff(i+1,i)); + m_S.topRows(i+2).applyOnTheRight(i+1,i,G); + m_T.topRows(i+2).applyOnTheRight(i+1,i,G); + // update Z + if (m_computeQZ) + m_Z.applyOnTheLeft(i+1,i,G.adjoint()); + + m_S.coeffRef(i+1,i) = Scalar(0.0); + m_T.coeffRef(i+1,i) = Scalar(0.0); + } + } + else + { + pushDownZero(j,i,i+1); + } + } + + /** \internal use zero in T(z,z) to zero S(l,l-1), working in block f..l */ + template + inline void RealQZ::pushDownZero(Index z, Index f, Index l) + { + JRs G; + const Index dim = m_S.cols(); + for (Index zz=z; zzf ? (zz-1) : zz; + G.makeGivens(m_T.coeff(zz, zz+1), m_T.coeff(zz+1, zz+1)); + m_S.rightCols(dim-firstColS).applyOnTheLeft(zz,zz+1,G.adjoint()); + m_T.rightCols(dim-zz).applyOnTheLeft(zz,zz+1,G.adjoint()); + m_T.coeffRef(zz+1,zz+1) = Scalar(0.0); + // update Q + if (m_computeQZ) + m_Q.applyOnTheRight(zz,zz+1,G); + // kill S(zz+1, zz-1) + if (zz>f) + { + G.makeGivens(m_S.coeff(zz+1, zz), m_S.coeff(zz+1,zz-1)); + m_S.topRows(zz+2).applyOnTheRight(zz, zz-1,G); + m_T.topRows(zz+1).applyOnTheRight(zz, zz-1,G); + m_S.coeffRef(zz+1,zz-1) = Scalar(0.0); + // update Z + if (m_computeQZ) + m_Z.applyOnTheLeft(zz,zz-1,G.adjoint()); + } + } + // finally kill S(l,l-1) + G.makeGivens(m_S.coeff(l,l), m_S.coeff(l,l-1)); + m_S.applyOnTheRight(l,l-1,G); + m_T.applyOnTheRight(l,l-1,G); + m_S.coeffRef(l,l-1)=Scalar(0.0); + // update Z + if (m_computeQZ) + m_Z.applyOnTheLeft(l,l-1,G.adjoint()); + } + + /** \internal QR-like iterative step for block f..l */ + template + inline void RealQZ::step(Index f, Index l, Index iter) + { + using std::abs; + const Index dim = m_S.cols(); + + // x, y, z + Scalar x, y, z; + if (iter==10) + { + // Wilkinson ad hoc shift + const Scalar + a11=m_S.coeff(f+0,f+0), a12=m_S.coeff(f+0,f+1), + a21=m_S.coeff(f+1,f+0), a22=m_S.coeff(f+1,f+1), a32=m_S.coeff(f+2,f+1), + b12=m_T.coeff(f+0,f+1), + b11i=Scalar(1.0)/m_T.coeff(f+0,f+0), + b22i=Scalar(1.0)/m_T.coeff(f+1,f+1), + a87=m_S.coeff(l-1,l-2), + a98=m_S.coeff(l-0,l-1), + b77i=Scalar(1.0)/m_T.coeff(l-2,l-2), + b88i=Scalar(1.0)/m_T.coeff(l-1,l-1); + Scalar ss = abs(a87*b77i) + abs(a98*b88i), + lpl = Scalar(1.5)*ss, + ll = ss*ss; + x = ll + a11*a11*b11i*b11i - lpl*a11*b11i + a12*a21*b11i*b22i + - a11*a21*b12*b11i*b11i*b22i; + y = a11*a21*b11i*b11i - lpl*a21*b11i + a21*a22*b11i*b22i + - a21*a21*b12*b11i*b11i*b22i; + z = a21*a32*b11i*b22i; + } + else if (iter==16) + { + // another exceptional shift + x = m_S.coeff(f,f)/m_T.coeff(f,f)-m_S.coeff(l,l)/m_T.coeff(l,l) + m_S.coeff(l,l-1)*m_T.coeff(l-1,l) / + (m_T.coeff(l-1,l-1)*m_T.coeff(l,l)); + y = m_S.coeff(f+1,f)/m_T.coeff(f,f); + z = 0; + } + else if (iter>23 && !(iter%8)) + { + // extremely exceptional shift + x = internal::random(-1.0,1.0); + y = internal::random(-1.0,1.0); + z = internal::random(-1.0,1.0); + } + else + { + // Compute the shifts: (x,y,z,0...) = (AB^-1 - l1 I) (AB^-1 - l2 I) e1 + // where l1 and l2 are the eigenvalues of the 2x2 matrix C = U V^-1 where + // U and V are 2x2 bottom right sub matrices of A and B. Thus: + // = AB^-1AB^-1 + l1 l2 I - (l1+l2)(AB^-1) + // = AB^-1AB^-1 + det(M) - tr(M)(AB^-1) + // Since we are only interested in having x, y, z with a correct ratio, we have: + const Scalar + a11 = m_S.coeff(f,f), a12 = m_S.coeff(f,f+1), + a21 = m_S.coeff(f+1,f), a22 = m_S.coeff(f+1,f+1), + a32 = m_S.coeff(f+2,f+1), + + a88 = m_S.coeff(l-1,l-1), a89 = m_S.coeff(l-1,l), + a98 = m_S.coeff(l,l-1), a99 = m_S.coeff(l,l), + + b11 = m_T.coeff(f,f), b12 = m_T.coeff(f,f+1), + b22 = m_T.coeff(f+1,f+1), + + b88 = m_T.coeff(l-1,l-1), b89 = m_T.coeff(l-1,l), + b99 = m_T.coeff(l,l); + + x = ( (a88/b88 - a11/b11)*(a99/b99 - a11/b11) - (a89/b99)*(a98/b88) + (a98/b88)*(b89/b99)*(a11/b11) ) * (b11/a21) + + a12/b22 - (a11/b11)*(b12/b22); + y = (a22/b22-a11/b11) - (a21/b11)*(b12/b22) - (a88/b88-a11/b11) - (a99/b99-a11/b11) + (a98/b88)*(b89/b99); + z = a32/b22; + } + + JRs G; + + for (Index k=f; k<=l-2; k++) + { + // variables for Householder reflections + Vector2s essential2; + Scalar tau, beta; + + Vector3s hr(x,y,z); + + // Q_k to annihilate S(k+1,k-1) and S(k+2,k-1) + hr.makeHouseholderInPlace(tau, beta); + essential2 = hr.template bottomRows<2>(); + Index fc=(std::max)(k-1,Index(0)); // first col to update + m_S.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data()); + m_T.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data()); + if (m_computeQZ) + m_Q.template middleCols<3>(k).applyHouseholderOnTheRight(essential2, tau, m_workspace.data()); + if (k>f) + m_S.coeffRef(k+2,k-1) = m_S.coeffRef(k+1,k-1) = Scalar(0.0); + + // Z_{k1} to annihilate T(k+2,k+1) and T(k+2,k) + hr << m_T.coeff(k+2,k+2),m_T.coeff(k+2,k),m_T.coeff(k+2,k+1); + hr.makeHouseholderInPlace(tau, beta); + essential2 = hr.template bottomRows<2>(); + { + Index lr = (std::min)(k+4,dim); // last row to update + Map > tmp(m_workspace.data(),lr); + // S + tmp = m_S.template middleCols<2>(k).topRows(lr) * essential2; + tmp += m_S.col(k+2).head(lr); + m_S.col(k+2).head(lr) -= tau*tmp; + m_S.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint(); + // T + tmp = m_T.template middleCols<2>(k).topRows(lr) * essential2; + tmp += m_T.col(k+2).head(lr); + m_T.col(k+2).head(lr) -= tau*tmp; + m_T.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint(); + } + if (m_computeQZ) + { + // Z + Map > tmp(m_workspace.data(),dim); + tmp = essential2.adjoint()*(m_Z.template middleRows<2>(k)); + tmp += m_Z.row(k+2); + m_Z.row(k+2) -= tau*tmp; + m_Z.template middleRows<2>(k) -= essential2 * (tau*tmp); + } + m_T.coeffRef(k+2,k) = m_T.coeffRef(k+2,k+1) = Scalar(0.0); + + // Z_{k2} to annihilate T(k+1,k) + G.makeGivens(m_T.coeff(k+1,k+1), m_T.coeff(k+1,k)); + m_S.applyOnTheRight(k+1,k,G); + m_T.applyOnTheRight(k+1,k,G); + // update Z + if (m_computeQZ) + m_Z.applyOnTheLeft(k+1,k,G.adjoint()); + m_T.coeffRef(k+1,k) = Scalar(0.0); + + // update x,y,z + x = m_S.coeff(k+1,k); + y = m_S.coeff(k+2,k); + if (k < l-2) + z = m_S.coeff(k+3,k); + } // loop over k + + // Q_{n-1} to annihilate y = S(l,l-2) + G.makeGivens(x,y); + m_S.applyOnTheLeft(l-1,l,G.adjoint()); + m_T.applyOnTheLeft(l-1,l,G.adjoint()); + if (m_computeQZ) + m_Q.applyOnTheRight(l-1,l,G); + m_S.coeffRef(l,l-2) = Scalar(0.0); + + // Z_{n-1} to annihilate T(l,l-1) + G.makeGivens(m_T.coeff(l,l),m_T.coeff(l,l-1)); + m_S.applyOnTheRight(l,l-1,G); + m_T.applyOnTheRight(l,l-1,G); + if (m_computeQZ) + m_Z.applyOnTheLeft(l,l-1,G.adjoint()); + m_T.coeffRef(l,l-1) = Scalar(0.0); + } + + template + RealQZ& RealQZ::compute(const MatrixType& A_in, const MatrixType& B_in, bool computeQZ) + { + + const Index dim = A_in.cols(); + + eigen_assert (A_in.rows()==dim && A_in.cols()==dim + && B_in.rows()==dim && B_in.cols()==dim + && "Need square matrices of the same dimension"); + + m_isInitialized = true; + m_computeQZ = computeQZ; + m_S = A_in; m_T = B_in; + m_workspace.resize(dim*2); + m_global_iter = 0; + + // entrance point: hessenberg triangular decomposition + hessenbergTriangular(); + // compute L1 vector norms of T, S into m_normOfS, m_normOfT + computeNorms(); + + Index l = dim-1, + f, + local_iter = 0; + + while (l>0 && local_iter0) m_S.coeffRef(f,f-1) = Scalar(0.0); + if (f == l) // One root found + { + l--; + local_iter = 0; + } + else if (f == l-1) // Two roots found + { + splitOffTwoRows(f); + l -= 2; + local_iter = 0; + } + else // No convergence yet + { + // if there's zero on diagonal of T, we can isolate an eigenvalue with Givens rotations + Index z = findSmallDiagEntry(f,l); + if (z>=f) + { + // zero found + pushDownZero(z,f,l); + } + else + { + // We are sure now that S.block(f,f, l-f+1,l-f+1) is underuced upper-Hessenberg + // and T.block(f,f, l-f+1,l-f+1) is invertible uper-triangular, which allows to + // apply a QR-like iteration to rows and columns f..l. + step(f,l, local_iter); + local_iter++; + m_global_iter++; + } + } + } + // check if we converged before reaching iterations limit + m_info = (local_iter j_left, j_right; + internal::real_2x2_jacobi_svd(m_T, i, i+1, &j_left, &j_right); + + // Apply resulting Jacobi rotations + m_S.applyOnTheLeft(i,i+1,j_left); + m_S.applyOnTheRight(i,i+1,j_right); + m_T.applyOnTheLeft(i,i+1,j_left); + m_T.applyOnTheRight(i,i+1,j_right); + m_T(i+1,i) = m_T(i,i+1) = Scalar(0); + + if(m_computeQZ) { + m_Q.applyOnTheRight(i,i+1,j_left.transpose()); + m_Z.applyOnTheLeft(i,i+1,j_right.transpose()); + } + + i++; + } + } + } + + return *this; + } // end compute + +} // end namespace Eigen + +#endif //EIGEN_REAL_QZ diff --git a/3rdparty/Eigen/src/Eigenvalues/RealSchur.h b/3rdparty/Eigen/src/Eigenvalues/RealSchur.h new file mode 100644 index 0000000..7304ef3 --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/RealSchur.h @@ -0,0 +1,558 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2010,2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_REAL_SCHUR_H +#define EIGEN_REAL_SCHUR_H + +#include "./HessenbergDecomposition.h" + +namespace Eigen { + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class RealSchur + * + * \brief Performs a real Schur decomposition of a square matrix + * + * \tparam _MatrixType the type of the matrix of which we are computing the + * real Schur decomposition; this is expected to be an instantiation of the + * Matrix class template. + * + * Given a real square matrix A, this class computes the real Schur + * decomposition: \f$ A = U T U^T \f$ where U is a real orthogonal matrix and + * T is a real quasi-triangular matrix. An orthogonal matrix is a matrix whose + * inverse is equal to its transpose, \f$ U^{-1} = U^T \f$. A quasi-triangular + * matrix is a block-triangular matrix whose diagonal consists of 1-by-1 + * blocks and 2-by-2 blocks with complex eigenvalues. The eigenvalues of the + * blocks on the diagonal of T are the same as the eigenvalues of the matrix + * A, and thus the real Schur decomposition is used in EigenSolver to compute + * the eigendecomposition of a matrix. + * + * Call the function compute() to compute the real Schur decomposition of a + * given matrix. Alternatively, you can use the RealSchur(const MatrixType&, bool) + * constructor which computes the real Schur decomposition at construction + * time. Once the decomposition is computed, you can use the matrixU() and + * matrixT() functions to retrieve the matrices U and T in the decomposition. + * + * The documentation of RealSchur(const MatrixType&, bool) contains an example + * of the typical use of this class. + * + * \note The implementation is adapted from + * JAMA (public domain). + * Their code is based on EISPACK. + * + * \sa class ComplexSchur, class EigenSolver, class ComplexEigenSolver + */ +template class RealSchur +{ + public: + typedef _MatrixType MatrixType; + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + Options = MatrixType::Options, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + typedef typename MatrixType::Scalar Scalar; + typedef std::complex::Real> ComplexScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + typedef Matrix EigenvalueType; + typedef Matrix ColumnVectorType; + + /** \brief Default constructor. + * + * \param [in] size Positive integer, size of the matrix whose Schur decomposition will be computed. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via compute(). The \p size parameter is only + * used as a hint. It is not an error to give a wrong \p size, but it may + * impair performance. + * + * \sa compute() for an example. + */ + explicit RealSchur(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime) + : m_matT(size, size), + m_matU(size, size), + m_workspaceVector(size), + m_hess(size), + m_isInitialized(false), + m_matUisUptodate(false), + m_maxIters(-1) + { } + + /** \brief Constructor; computes real Schur decomposition of given matrix. + * + * \param[in] matrix Square matrix whose Schur decomposition is to be computed. + * \param[in] computeU If true, both T and U are computed; if false, only T is computed. + * + * This constructor calls compute() to compute the Schur decomposition. + * + * Example: \include RealSchur_RealSchur_MatrixType.cpp + * Output: \verbinclude RealSchur_RealSchur_MatrixType.out + */ + template + explicit RealSchur(const EigenBase& matrix, bool computeU = true) + : m_matT(matrix.rows(),matrix.cols()), + m_matU(matrix.rows(),matrix.cols()), + m_workspaceVector(matrix.rows()), + m_hess(matrix.rows()), + m_isInitialized(false), + m_matUisUptodate(false), + m_maxIters(-1) + { + compute(matrix.derived(), computeU); + } + + /** \brief Returns the orthogonal matrix in the Schur decomposition. + * + * \returns A const reference to the matrix U. + * + * \pre Either the constructor RealSchur(const MatrixType&, bool) or the + * member function compute(const MatrixType&, bool) has been called before + * to compute the Schur decomposition of a matrix, and \p computeU was set + * to true (the default value). + * + * \sa RealSchur(const MatrixType&, bool) for an example + */ + const MatrixType& matrixU() const + { + eigen_assert(m_isInitialized && "RealSchur is not initialized."); + eigen_assert(m_matUisUptodate && "The matrix U has not been computed during the RealSchur decomposition."); + return m_matU; + } + + /** \brief Returns the quasi-triangular matrix in the Schur decomposition. + * + * \returns A const reference to the matrix T. + * + * \pre Either the constructor RealSchur(const MatrixType&, bool) or the + * member function compute(const MatrixType&, bool) has been called before + * to compute the Schur decomposition of a matrix. + * + * \sa RealSchur(const MatrixType&, bool) for an example + */ + const MatrixType& matrixT() const + { + eigen_assert(m_isInitialized && "RealSchur is not initialized."); + return m_matT; + } + + /** \brief Computes Schur decomposition of given matrix. + * + * \param[in] matrix Square matrix whose Schur decomposition is to be computed. + * \param[in] computeU If true, both T and U are computed; if false, only T is computed. + * \returns Reference to \c *this + * + * The Schur decomposition is computed by first reducing the matrix to + * Hessenberg form using the class HessenbergDecomposition. The Hessenberg + * matrix is then reduced to triangular form by performing Francis QR + * iterations with implicit double shift. The cost of computing the Schur + * decomposition depends on the number of iterations; as a rough guide, it + * may be taken to be \f$25n^3\f$ flops if \a computeU is true and + * \f$10n^3\f$ flops if \a computeU is false. + * + * Example: \include RealSchur_compute.cpp + * Output: \verbinclude RealSchur_compute.out + * + * \sa compute(const MatrixType&, bool, Index) + */ + template + RealSchur& compute(const EigenBase& matrix, bool computeU = true); + + /** \brief Computes Schur decomposition of a Hessenberg matrix H = Z T Z^T + * \param[in] matrixH Matrix in Hessenberg form H + * \param[in] matrixQ orthogonal matrix Q that transform a matrix A to H : A = Q H Q^T + * \param computeU Computes the matriX U of the Schur vectors + * \return Reference to \c *this + * + * This routine assumes that the matrix is already reduced in Hessenberg form matrixH + * using either the class HessenbergDecomposition or another mean. + * It computes the upper quasi-triangular matrix T of the Schur decomposition of H + * When computeU is true, this routine computes the matrix U such that + * A = U T U^T = (QZ) T (QZ)^T = Q H Q^T where A is the initial matrix + * + * NOTE Q is referenced if computeU is true; so, if the initial orthogonal matrix + * is not available, the user should give an identity matrix (Q.setIdentity()) + * + * \sa compute(const MatrixType&, bool) + */ + template + RealSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ, bool computeU); + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, \c NoConvergence otherwise. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "RealSchur is not initialized."); + return m_info; + } + + /** \brief Sets the maximum number of iterations allowed. + * + * If not specified by the user, the maximum number of iterations is m_maxIterationsPerRow times the size + * of the matrix. + */ + RealSchur& setMaxIterations(Index maxIters) + { + m_maxIters = maxIters; + return *this; + } + + /** \brief Returns the maximum number of iterations. */ + Index getMaxIterations() + { + return m_maxIters; + } + + /** \brief Maximum number of iterations per row. + * + * If not otherwise specified, the maximum number of iterations is this number times the size of the + * matrix. It is currently set to 40. + */ + static const int m_maxIterationsPerRow = 40; + + private: + + MatrixType m_matT; + MatrixType m_matU; + ColumnVectorType m_workspaceVector; + HessenbergDecomposition m_hess; + ComputationInfo m_info; + bool m_isInitialized; + bool m_matUisUptodate; + Index m_maxIters; + + typedef Matrix Vector3s; + + Scalar computeNormOfT(); + Index findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero); + void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift); + void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo); + void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector); + void performFrancisQRStep(Index il, Index im, Index iu, bool computeU, const Vector3s& firstHouseholderVector, Scalar* workspace); +}; + + +template +template +RealSchur& RealSchur::compute(const EigenBase& matrix, bool computeU) +{ + const Scalar considerAsZero = (std::numeric_limits::min)(); + + eigen_assert(matrix.cols() == matrix.rows()); + Index maxIters = m_maxIters; + if (maxIters == -1) + maxIters = m_maxIterationsPerRow * matrix.rows(); + + Scalar scale = matrix.derived().cwiseAbs().maxCoeff(); + if(scale +template +RealSchur& RealSchur::computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ, bool computeU) +{ + using std::abs; + + m_matT = matrixH; + m_workspaceVector.resize(m_matT.cols()); + if(computeU && !internal::is_same_dense(m_matU,matrixQ)) + m_matU = matrixQ; + + Index maxIters = m_maxIters; + if (maxIters == -1) + maxIters = m_maxIterationsPerRow * matrixH.rows(); + Scalar* workspace = &m_workspaceVector.coeffRef(0); + + // The matrix m_matT is divided in three parts. + // Rows 0,...,il-1 are decoupled from the rest because m_matT(il,il-1) is zero. + // Rows il,...,iu is the part we are working on (the active window). + // Rows iu+1,...,end are already brought in triangular form. + Index iu = m_matT.cols() - 1; + Index iter = 0; // iteration count for current eigenvalue + Index totalIter = 0; // iteration count for whole matrix + Scalar exshift(0); // sum of exceptional shifts + Scalar norm = computeNormOfT(); + // sub-diagonal entries smaller than considerAsZero will be treated as zero. + // We use eps^2 to enable more precision in small eigenvalues. + Scalar considerAsZero = numext::maxi( norm * numext::abs2(NumTraits::epsilon()), + (std::numeric_limits::min)() ); + + if(norm!=Scalar(0)) + { + while (iu >= 0) + { + Index il = findSmallSubdiagEntry(iu,considerAsZero); + + // Check for convergence + if (il == iu) // One root found + { + m_matT.coeffRef(iu,iu) = m_matT.coeff(iu,iu) + exshift; + if (iu > 0) + m_matT.coeffRef(iu, iu-1) = Scalar(0); + iu--; + iter = 0; + } + else if (il == iu-1) // Two roots found + { + splitOffTwoRows(iu, computeU, exshift); + iu -= 2; + iter = 0; + } + else // No convergence yet + { + // The firstHouseholderVector vector has to be initialized to something to get rid of a silly GCC warning (-O1 -Wall -DNDEBUG ) + Vector3s firstHouseholderVector = Vector3s::Zero(), shiftInfo; + computeShift(iu, iter, exshift, shiftInfo); + iter = iter + 1; + totalIter = totalIter + 1; + if (totalIter > maxIters) break; + Index im; + initFrancisQRStep(il, iu, shiftInfo, im, firstHouseholderVector); + performFrancisQRStep(il, im, iu, computeU, firstHouseholderVector, workspace); + } + } + } + if(totalIter <= maxIters) + m_info = Success; + else + m_info = NoConvergence; + + m_isInitialized = true; + m_matUisUptodate = computeU; + return *this; +} + +/** \internal Computes and returns vector L1 norm of T */ +template +inline typename MatrixType::Scalar RealSchur::computeNormOfT() +{ + const Index size = m_matT.cols(); + // FIXME to be efficient the following would requires a triangular reduxion code + // Scalar norm = m_matT.upper().cwiseAbs().sum() + // + m_matT.bottomLeftCorner(size-1,size-1).diagonal().cwiseAbs().sum(); + Scalar norm(0); + for (Index j = 0; j < size; ++j) + norm += m_matT.col(j).segment(0, (std::min)(size,j+2)).cwiseAbs().sum(); + return norm; +} + +/** \internal Look for single small sub-diagonal element and returns its index */ +template +inline Index RealSchur::findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero) +{ + using std::abs; + Index res = iu; + while (res > 0) + { + Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res)); + + s = numext::maxi(s * NumTraits::epsilon(), considerAsZero); + + if (abs(m_matT.coeff(res,res-1)) <= s) + break; + res--; + } + return res; +} + +/** \internal Update T given that rows iu-1 and iu decouple from the rest. */ +template +inline void RealSchur::splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift) +{ + using std::sqrt; + using std::abs; + const Index size = m_matT.cols(); + + // The eigenvalues of the 2x2 matrix [a b; c d] are + // trace +/- sqrt(discr/4) where discr = tr^2 - 4*det, tr = a + d, det = ad - bc + Scalar p = Scalar(0.5) * (m_matT.coeff(iu-1,iu-1) - m_matT.coeff(iu,iu)); + Scalar q = p * p + m_matT.coeff(iu,iu-1) * m_matT.coeff(iu-1,iu); // q = tr^2 / 4 - det = discr/4 + m_matT.coeffRef(iu,iu) += exshift; + m_matT.coeffRef(iu-1,iu-1) += exshift; + + if (q >= Scalar(0)) // Two real eigenvalues + { + Scalar z = sqrt(abs(q)); + JacobiRotation rot; + if (p >= Scalar(0)) + rot.makeGivens(p + z, m_matT.coeff(iu, iu-1)); + else + rot.makeGivens(p - z, m_matT.coeff(iu, iu-1)); + + m_matT.rightCols(size-iu+1).applyOnTheLeft(iu-1, iu, rot.adjoint()); + m_matT.topRows(iu+1).applyOnTheRight(iu-1, iu, rot); + m_matT.coeffRef(iu, iu-1) = Scalar(0); + if (computeU) + m_matU.applyOnTheRight(iu-1, iu, rot); + } + + if (iu > 1) + m_matT.coeffRef(iu-1, iu-2) = Scalar(0); +} + +/** \internal Form shift in shiftInfo, and update exshift if an exceptional shift is performed. */ +template +inline void RealSchur::computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo) +{ + using std::sqrt; + using std::abs; + shiftInfo.coeffRef(0) = m_matT.coeff(iu,iu); + shiftInfo.coeffRef(1) = m_matT.coeff(iu-1,iu-1); + shiftInfo.coeffRef(2) = m_matT.coeff(iu,iu-1) * m_matT.coeff(iu-1,iu); + + // Wilkinson's original ad hoc shift + if (iter == 10) + { + exshift += shiftInfo.coeff(0); + for (Index i = 0; i <= iu; ++i) + m_matT.coeffRef(i,i) -= shiftInfo.coeff(0); + Scalar s = abs(m_matT.coeff(iu,iu-1)) + abs(m_matT.coeff(iu-1,iu-2)); + shiftInfo.coeffRef(0) = Scalar(0.75) * s; + shiftInfo.coeffRef(1) = Scalar(0.75) * s; + shiftInfo.coeffRef(2) = Scalar(-0.4375) * s * s; + } + + // MATLAB's new ad hoc shift + if (iter == 30) + { + Scalar s = (shiftInfo.coeff(1) - shiftInfo.coeff(0)) / Scalar(2.0); + s = s * s + shiftInfo.coeff(2); + if (s > Scalar(0)) + { + s = sqrt(s); + if (shiftInfo.coeff(1) < shiftInfo.coeff(0)) + s = -s; + s = s + (shiftInfo.coeff(1) - shiftInfo.coeff(0)) / Scalar(2.0); + s = shiftInfo.coeff(0) - shiftInfo.coeff(2) / s; + exshift += s; + for (Index i = 0; i <= iu; ++i) + m_matT.coeffRef(i,i) -= s; + shiftInfo.setConstant(Scalar(0.964)); + } + } +} + +/** \internal Compute index im at which Francis QR step starts and the first Householder vector. */ +template +inline void RealSchur::initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector) +{ + using std::abs; + Vector3s& v = firstHouseholderVector; // alias to save typing + + for (im = iu-2; im >= il; --im) + { + const Scalar Tmm = m_matT.coeff(im,im); + const Scalar r = shiftInfo.coeff(0) - Tmm; + const Scalar s = shiftInfo.coeff(1) - Tmm; + v.coeffRef(0) = (r * s - shiftInfo.coeff(2)) / m_matT.coeff(im+1,im) + m_matT.coeff(im,im+1); + v.coeffRef(1) = m_matT.coeff(im+1,im+1) - Tmm - r - s; + v.coeffRef(2) = m_matT.coeff(im+2,im+1); + if (im == il) { + break; + } + const Scalar lhs = m_matT.coeff(im,im-1) * (abs(v.coeff(1)) + abs(v.coeff(2))); + const Scalar rhs = v.coeff(0) * (abs(m_matT.coeff(im-1,im-1)) + abs(Tmm) + abs(m_matT.coeff(im+1,im+1))); + if (abs(lhs) < NumTraits::epsilon() * rhs) + break; + } +} + +/** \internal Perform a Francis QR step involving rows il:iu and columns im:iu. */ +template +inline void RealSchur::performFrancisQRStep(Index il, Index im, Index iu, bool computeU, const Vector3s& firstHouseholderVector, Scalar* workspace) +{ + eigen_assert(im >= il); + eigen_assert(im <= iu-2); + + const Index size = m_matT.cols(); + + for (Index k = im; k <= iu-2; ++k) + { + bool firstIteration = (k == im); + + Vector3s v; + if (firstIteration) + v = firstHouseholderVector; + else + v = m_matT.template block<3,1>(k,k-1); + + Scalar tau, beta; + Matrix ess; + v.makeHouseholder(ess, tau, beta); + + if (beta != Scalar(0)) // if v is not zero + { + if (firstIteration && k > il) + m_matT.coeffRef(k,k-1) = -m_matT.coeff(k,k-1); + else if (!firstIteration) + m_matT.coeffRef(k,k-1) = beta; + + // These Householder transformations form the O(n^3) part of the algorithm + m_matT.block(k, k, 3, size-k).applyHouseholderOnTheLeft(ess, tau, workspace); + m_matT.block(0, k, (std::min)(iu,k+3) + 1, 3).applyHouseholderOnTheRight(ess, tau, workspace); + if (computeU) + m_matU.block(0, k, size, 3).applyHouseholderOnTheRight(ess, tau, workspace); + } + } + + Matrix v = m_matT.template block<2,1>(iu-1, iu-2); + Scalar tau, beta; + Matrix ess; + v.makeHouseholder(ess, tau, beta); + + if (beta != Scalar(0)) // if v is not zero + { + m_matT.coeffRef(iu-1, iu-2) = beta; + m_matT.block(iu-1, iu-1, 2, size-iu+1).applyHouseholderOnTheLeft(ess, tau, workspace); + m_matT.block(0, iu-1, iu+1, 2).applyHouseholderOnTheRight(ess, tau, workspace); + if (computeU) + m_matU.block(0, iu-1, size, 2).applyHouseholderOnTheRight(ess, tau, workspace); + } + + // clean up pollution due to round-off errors + for (Index i = im+2; i <= iu; ++i) + { + m_matT.coeffRef(i,i-2) = Scalar(0); + if (i > im+2) + m_matT.coeffRef(i,i-3) = Scalar(0); + } +} + +} // end namespace Eigen + +#endif // EIGEN_REAL_SCHUR_H diff --git a/3rdparty/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h b/3rdparty/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h new file mode 100644 index 0000000..2c22517 --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h @@ -0,0 +1,77 @@ +/* + Copyright (c) 2011, Intel Corporation. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of Intel Corporation nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + ******************************************************************************** + * Content : Eigen bindings to LAPACKe + * Real Schur needed to real unsymmetrical eigenvalues/eigenvectors. + ******************************************************************************** +*/ + +#ifndef EIGEN_REAL_SCHUR_LAPACKE_H +#define EIGEN_REAL_SCHUR_LAPACKE_H + +namespace Eigen { + +/** \internal Specialization for the data types supported by LAPACKe */ + +#define EIGEN_LAPACKE_SCHUR_REAL(EIGTYPE, LAPACKE_TYPE, LAPACKE_PREFIX, LAPACKE_PREFIX_U, EIGCOLROW, LAPACKE_COLROW) \ +template<> template inline \ +RealSchur >& \ +RealSchur >::compute(const EigenBase& matrix, bool computeU) \ +{ \ + eigen_assert(matrix.cols() == matrix.rows()); \ +\ + lapack_int n = internal::convert_index(matrix.cols()), sdim, info; \ + lapack_int matrix_order = LAPACKE_COLROW; \ + char jobvs, sort='N'; \ + LAPACK_##LAPACKE_PREFIX_U##_SELECT2 select = 0; \ + jobvs = (computeU) ? 'V' : 'N'; \ + m_matU.resize(n, n); \ + lapack_int ldvs = internal::convert_index(m_matU.outerStride()); \ + m_matT = matrix; \ + lapack_int lda = internal::convert_index(m_matT.outerStride()); \ + Matrix wr, wi; \ + wr.resize(n, 1); wi.resize(n, 1); \ + info = LAPACKE_##LAPACKE_PREFIX##gees( matrix_order, jobvs, sort, select, n, (LAPACKE_TYPE*)m_matT.data(), lda, &sdim, (LAPACKE_TYPE*)wr.data(), (LAPACKE_TYPE*)wi.data(), (LAPACKE_TYPE*)m_matU.data(), ldvs ); \ + if(info == 0) \ + m_info = Success; \ + else \ + m_info = NoConvergence; \ +\ + m_isInitialized = true; \ + m_matUisUptodate = computeU; \ + return *this; \ +\ +} + +EIGEN_LAPACKE_SCHUR_REAL(double, double, d, D, ColMajor, LAPACK_COL_MAJOR) +EIGEN_LAPACKE_SCHUR_REAL(float, float, s, S, ColMajor, LAPACK_COL_MAJOR) +EIGEN_LAPACKE_SCHUR_REAL(double, double, d, D, RowMajor, LAPACK_ROW_MAJOR) +EIGEN_LAPACKE_SCHUR_REAL(float, float, s, S, RowMajor, LAPACK_ROW_MAJOR) + +} // end namespace Eigen + +#endif // EIGEN_REAL_SCHUR_LAPACKE_H diff --git a/3rdparty/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/3rdparty/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h new file mode 100644 index 0000000..1469236 --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -0,0 +1,904 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2010 Gael Guennebaud +// Copyright (C) 2010 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SELFADJOINTEIGENSOLVER_H +#define EIGEN_SELFADJOINTEIGENSOLVER_H + +#include "./Tridiagonalization.h" + +namespace Eigen { + +template +class GeneralizedSelfAdjointEigenSolver; + +namespace internal { +template struct direct_selfadjoint_eigenvalues; + +template +EIGEN_DEVICE_FUNC +ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec); +} + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class SelfAdjointEigenSolver + * + * \brief Computes eigenvalues and eigenvectors of selfadjoint matrices + * + * \tparam _MatrixType the type of the matrix of which we are computing the + * eigendecomposition; this is expected to be an instantiation of the Matrix + * class template. + * + * A matrix \f$ A \f$ is selfadjoint if it equals its adjoint. For real + * matrices, this means that the matrix is symmetric: it equals its + * transpose. This class computes the eigenvalues and eigenvectors of a + * selfadjoint matrix. These are the scalars \f$ \lambda \f$ and vectors + * \f$ v \f$ such that \f$ Av = \lambda v \f$. The eigenvalues of a + * selfadjoint matrix are always real. If \f$ D \f$ is a diagonal matrix with + * the eigenvalues on the diagonal, and \f$ V \f$ is a matrix with the + * eigenvectors as its columns, then \f$ A = V D V^{-1} \f$. This is called the + * eigendecomposition. + * + * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal + * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then + * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is + * equal to its transpose, \f$ V^{-1} = V^T \f$. + * + * The algorithm exploits the fact that the matrix is selfadjoint, making it + * faster and more accurate than the general purpose eigenvalue algorithms + * implemented in EigenSolver and ComplexEigenSolver. + * + * Only the \b lower \b triangular \b part of the input matrix is referenced. + * + * Call the function compute() to compute the eigenvalues and eigenvectors of + * a given matrix. Alternatively, you can use the + * SelfAdjointEigenSolver(const MatrixType&, int) constructor which computes + * the eigenvalues and eigenvectors at construction time. Once the eigenvalue + * and eigenvectors are computed, they can be retrieved with the eigenvalues() + * and eigenvectors() functions. + * + * The documentation for SelfAdjointEigenSolver(const MatrixType&, int) + * contains an example of the typical use of this class. + * + * To solve the \em generalized eigenvalue problem \f$ Av = \lambda Bv \f$ and + * the likes, see the class GeneralizedSelfAdjointEigenSolver. + * + * \sa MatrixBase::eigenvalues(), class EigenSolver, class ComplexEigenSolver + */ +template class SelfAdjointEigenSolver +{ + public: + + typedef _MatrixType MatrixType; + enum { + Size = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + Options = MatrixType::Options, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + /** \brief Scalar type for matrices of type \p _MatrixType. */ + typedef typename MatrixType::Scalar Scalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + typedef Matrix EigenvectorsType; + + /** \brief Real scalar type for \p _MatrixType. + * + * This is just \c Scalar if #Scalar is real (e.g., \c float or + * \c double), and the type of the real part of \c Scalar if #Scalar is + * complex. + */ + typedef typename NumTraits::Real RealScalar; + + friend struct internal::direct_selfadjoint_eigenvalues::IsComplex>; + + /** \brief Type for vector of eigenvalues as returned by eigenvalues(). + * + * This is a column vector with entries of type #RealScalar. + * The length of the vector is the size of \p _MatrixType. + */ + typedef typename internal::plain_col_type::type RealVectorType; + typedef Tridiagonalization TridiagonalizationType; + typedef typename TridiagonalizationType::SubDiagonalType SubDiagonalType; + + /** \brief Default constructor for fixed-size matrices. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via compute(). This constructor + * can only be used if \p _MatrixType is a fixed-size matrix; use + * SelfAdjointEigenSolver(Index) for dynamic-size matrices. + * + * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver.cpp + * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver.out + */ + EIGEN_DEVICE_FUNC + SelfAdjointEigenSolver() + : m_eivec(), + m_eivalues(), + m_subdiag(), + m_hcoeffs(), + m_info(InvalidInput), + m_isInitialized(false), + m_eigenvectorsOk(false) + { } + + /** \brief Constructor, pre-allocates memory for dynamic-size matrices. + * + * \param [in] size Positive integer, size of the matrix whose + * eigenvalues and eigenvectors will be computed. + * + * This constructor is useful for dynamic-size matrices, when the user + * intends to perform decompositions via compute(). The \p size + * parameter is only used as a hint. It is not an error to give a wrong + * \p size, but it may impair performance. + * + * \sa compute() for an example + */ + EIGEN_DEVICE_FUNC + explicit SelfAdjointEigenSolver(Index size) + : m_eivec(size, size), + m_eivalues(size), + m_subdiag(size > 1 ? size - 1 : 1), + m_hcoeffs(size > 1 ? size - 1 : 1), + m_isInitialized(false), + m_eigenvectorsOk(false) + {} + + /** \brief Constructor; computes eigendecomposition of given matrix. + * + * \param[in] matrix Selfadjoint matrix whose eigendecomposition is to + * be computed. Only the lower triangular part of the matrix is referenced. + * \param[in] options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly. + * + * This constructor calls compute(const MatrixType&, int) to compute the + * eigenvalues of the matrix \p matrix. The eigenvectors are computed if + * \p options equals #ComputeEigenvectors. + * + * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.cpp + * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.out + * + * \sa compute(const MatrixType&, int) + */ + template + EIGEN_DEVICE_FUNC + explicit SelfAdjointEigenSolver(const EigenBase& matrix, int options = ComputeEigenvectors) + : m_eivec(matrix.rows(), matrix.cols()), + m_eivalues(matrix.cols()), + m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1), + m_hcoeffs(matrix.cols() > 1 ? matrix.cols() - 1 : 1), + m_isInitialized(false), + m_eigenvectorsOk(false) + { + compute(matrix.derived(), options); + } + + /** \brief Computes eigendecomposition of given matrix. + * + * \param[in] matrix Selfadjoint matrix whose eigendecomposition is to + * be computed. Only the lower triangular part of the matrix is referenced. + * \param[in] options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly. + * \returns Reference to \c *this + * + * This function computes the eigenvalues of \p matrix. The eigenvalues() + * function can be used to retrieve them. If \p options equals #ComputeEigenvectors, + * then the eigenvectors are also computed and can be retrieved by + * calling eigenvectors(). + * + * This implementation uses a symmetric QR algorithm. The matrix is first + * reduced to tridiagonal form using the Tridiagonalization class. The + * tridiagonal matrix is then brought to diagonal form with implicit + * symmetric QR steps with Wilkinson shift. Details can be found in + * Section 8.3 of Golub \& Van Loan, %Matrix Computations. + * + * The cost of the computation is about \f$ 9n^3 \f$ if the eigenvectors + * are required and \f$ 4n^3/3 \f$ if they are not required. + * + * This method reuses the memory in the SelfAdjointEigenSolver object that + * was allocated when the object was constructed, if the size of the + * matrix does not change. + * + * Example: \include SelfAdjointEigenSolver_compute_MatrixType.cpp + * Output: \verbinclude SelfAdjointEigenSolver_compute_MatrixType.out + * + * \sa SelfAdjointEigenSolver(const MatrixType&, int) + */ + template + EIGEN_DEVICE_FUNC + SelfAdjointEigenSolver& compute(const EigenBase& matrix, int options = ComputeEigenvectors); + + /** \brief Computes eigendecomposition of given matrix using a closed-form algorithm + * + * This is a variant of compute(const MatrixType&, int options) which + * directly solves the underlying polynomial equation. + * + * Currently only 2x2 and 3x3 matrices for which the sizes are known at compile time are supported (e.g., Matrix3d). + * + * This method is usually significantly faster than the QR iterative algorithm + * but it might also be less accurate. It is also worth noting that + * for 3x3 matrices it involves trigonometric operations which are + * not necessarily available for all scalar types. + * + * For the 3x3 case, we observed the following worst case relative error regarding the eigenvalues: + * - double: 1e-8 + * - float: 1e-3 + * + * \sa compute(const MatrixType&, int options) + */ + EIGEN_DEVICE_FUNC + SelfAdjointEigenSolver& computeDirect(const MatrixType& matrix, int options = ComputeEigenvectors); + + /** + *\brief Computes the eigen decomposition from a tridiagonal symmetric matrix + * + * \param[in] diag The vector containing the diagonal of the matrix. + * \param[in] subdiag The subdiagonal of the matrix. + * \param[in] options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly. + * \returns Reference to \c *this + * + * This function assumes that the matrix has been reduced to tridiagonal form. + * + * \sa compute(const MatrixType&, int) for more information + */ + SelfAdjointEigenSolver& computeFromTridiagonal(const RealVectorType& diag, const SubDiagonalType& subdiag , int options=ComputeEigenvectors); + + /** \brief Returns the eigenvectors of given matrix. + * + * \returns A const reference to the matrix whose columns are the eigenvectors. + * + * \pre The eigenvectors have been computed before. + * + * Column \f$ k \f$ of the returned matrix is an eigenvector corresponding + * to eigenvalue number \f$ k \f$ as returned by eigenvalues(). The + * eigenvectors are normalized to have (Euclidean) norm equal to one. If + * this object was used to solve the eigenproblem for the selfadjoint + * matrix \f$ A \f$, then the matrix returned by this function is the + * matrix \f$ V \f$ in the eigendecomposition \f$ A = V D V^{-1} \f$. + * + * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal + * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then + * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is + * equal to its transpose, \f$ V^{-1} = V^T \f$. + * + * Example: \include SelfAdjointEigenSolver_eigenvectors.cpp + * Output: \verbinclude SelfAdjointEigenSolver_eigenvectors.out + * + * \sa eigenvalues() + */ + EIGEN_DEVICE_FUNC + const EigenvectorsType& eigenvectors() const + { + eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); + return m_eivec; + } + + /** \brief Returns the eigenvalues of given matrix. + * + * \returns A const reference to the column vector containing the eigenvalues. + * + * \pre The eigenvalues have been computed before. + * + * The eigenvalues are repeated according to their algebraic multiplicity, + * so there are as many eigenvalues as rows in the matrix. The eigenvalues + * are sorted in increasing order. + * + * Example: \include SelfAdjointEigenSolver_eigenvalues.cpp + * Output: \verbinclude SelfAdjointEigenSolver_eigenvalues.out + * + * \sa eigenvectors(), MatrixBase::eigenvalues() + */ + EIGEN_DEVICE_FUNC + const RealVectorType& eigenvalues() const + { + eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + return m_eivalues; + } + + /** \brief Computes the positive-definite square root of the matrix. + * + * \returns the positive-definite square root of the matrix + * + * \pre The eigenvalues and eigenvectors of a positive-definite matrix + * have been computed before. + * + * The square root of a positive-definite matrix \f$ A \f$ is the + * positive-definite matrix whose square equals \f$ A \f$. This function + * uses the eigendecomposition \f$ A = V D V^{-1} \f$ to compute the + * square root as \f$ A^{1/2} = V D^{1/2} V^{-1} \f$. + * + * Example: \include SelfAdjointEigenSolver_operatorSqrt.cpp + * Output: \verbinclude SelfAdjointEigenSolver_operatorSqrt.out + * + * \sa operatorInverseSqrt(), MatrixFunctions Module + */ + EIGEN_DEVICE_FUNC + MatrixType operatorSqrt() const + { + eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); + return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint(); + } + + /** \brief Computes the inverse square root of the matrix. + * + * \returns the inverse positive-definite square root of the matrix + * + * \pre The eigenvalues and eigenvectors of a positive-definite matrix + * have been computed before. + * + * This function uses the eigendecomposition \f$ A = V D V^{-1} \f$ to + * compute the inverse square root as \f$ V D^{-1/2} V^{-1} \f$. This is + * cheaper than first computing the square root with operatorSqrt() and + * then its inverse with MatrixBase::inverse(). + * + * Example: \include SelfAdjointEigenSolver_operatorInverseSqrt.cpp + * Output: \verbinclude SelfAdjointEigenSolver_operatorInverseSqrt.out + * + * \sa operatorSqrt(), MatrixBase::inverse(), MatrixFunctions Module + */ + EIGEN_DEVICE_FUNC + MatrixType operatorInverseSqrt() const + { + eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); + return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint(); + } + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, \c NoConvergence otherwise. + */ + EIGEN_DEVICE_FUNC + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + return m_info; + } + + /** \brief Maximum number of iterations. + * + * The algorithm terminates if it does not converge within m_maxIterations * n iterations, where n + * denotes the size of the matrix. This value is currently set to 30 (copied from LAPACK). + */ + static const int m_maxIterations = 30; + + protected: + static EIGEN_DEVICE_FUNC + void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + + EigenvectorsType m_eivec; + RealVectorType m_eivalues; + typename TridiagonalizationType::SubDiagonalType m_subdiag; + typename TridiagonalizationType::CoeffVectorType m_hcoeffs; + ComputationInfo m_info; + bool m_isInitialized; + bool m_eigenvectorsOk; +}; + +namespace internal { +/** \internal + * + * \eigenvalues_module \ingroup Eigenvalues_Module + * + * Performs a QR step on a tridiagonal symmetric matrix represented as a + * pair of two vectors \a diag and \a subdiag. + * + * \param diag the diagonal part of the input selfadjoint tridiagonal matrix + * \param subdiag the sub-diagonal part of the input selfadjoint tridiagonal matrix + * \param start starting index of the submatrix to work on + * \param end last+1 index of the submatrix to work on + * \param matrixQ pointer to the column-major matrix holding the eigenvectors, can be 0 + * \param n size of the input matrix + * + * For compilation efficiency reasons, this procedure does not use eigen expression + * for its arguments. + * + * Implemented from Golub's "Matrix Computations", algorithm 8.3.2: + * "implicit symmetric QR step with Wilkinson shift" + */ +template +EIGEN_DEVICE_FUNC +static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n); +} + +template +template +EIGEN_DEVICE_FUNC +SelfAdjointEigenSolver& SelfAdjointEigenSolver +::compute(const EigenBase& a_matrix, int options) +{ + check_template_parameters(); + + const InputType &matrix(a_matrix.derived()); + + EIGEN_USING_STD(abs); + eigen_assert(matrix.cols() == matrix.rows()); + eigen_assert((options&~(EigVecMask|GenEigMask))==0 + && (options&EigVecMask)!=EigVecMask + && "invalid option parameter"); + bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; + Index n = matrix.cols(); + m_eivalues.resize(n,1); + + if(n==1) + { + m_eivec = matrix; + m_eivalues.coeffRef(0,0) = numext::real(m_eivec.coeff(0,0)); + if(computeEigenvectors) + m_eivec.setOnes(n,n); + m_info = Success; + m_isInitialized = true; + m_eigenvectorsOk = computeEigenvectors; + return *this; + } + + // declare some aliases + RealVectorType& diag = m_eivalues; + EigenvectorsType& mat = m_eivec; + + // map the matrix coefficients to [-1:1] to avoid over- and underflow. + mat = matrix.template triangularView(); + RealScalar scale = mat.cwiseAbs().maxCoeff(); + if(scale==RealScalar(0)) scale = RealScalar(1); + mat.template triangularView() /= scale; + m_subdiag.resize(n-1); + m_hcoeffs.resize(n-1); + internal::tridiagonalization_inplace(mat, diag, m_subdiag, m_hcoeffs, computeEigenvectors); + + m_info = internal::computeFromTridiagonal_impl(diag, m_subdiag, m_maxIterations, computeEigenvectors, m_eivec); + + // scale back the eigen values + m_eivalues *= scale; + + m_isInitialized = true; + m_eigenvectorsOk = computeEigenvectors; + return *this; +} + +template +SelfAdjointEigenSolver& SelfAdjointEigenSolver +::computeFromTridiagonal(const RealVectorType& diag, const SubDiagonalType& subdiag , int options) +{ + //TODO : Add an option to scale the values beforehand + bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; + + m_eivalues = diag; + m_subdiag = subdiag; + if (computeEigenvectors) + { + m_eivec.setIdentity(diag.size(), diag.size()); + } + m_info = internal::computeFromTridiagonal_impl(m_eivalues, m_subdiag, m_maxIterations, computeEigenvectors, m_eivec); + + m_isInitialized = true; + m_eigenvectorsOk = computeEigenvectors; + return *this; +} + +namespace internal { +/** + * \internal + * \brief Compute the eigendecomposition from a tridiagonal matrix + * + * \param[in,out] diag : On input, the diagonal of the matrix, on output the eigenvalues + * \param[in,out] subdiag : The subdiagonal part of the matrix (entries are modified during the decomposition) + * \param[in] maxIterations : the maximum number of iterations + * \param[in] computeEigenvectors : whether the eigenvectors have to be computed or not + * \param[out] eivec : The matrix to store the eigenvectors if computeEigenvectors==true. Must be allocated on input. + * \returns \c Success or \c NoConvergence + */ +template +EIGEN_DEVICE_FUNC +ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec) +{ + ComputationInfo info; + typedef typename MatrixType::Scalar Scalar; + + Index n = diag.size(); + Index end = n-1; + Index start = 0; + Index iter = 0; // total number of iterations + + typedef typename DiagType::RealScalar RealScalar; + const RealScalar considerAsZero = (std::numeric_limits::min)(); + const RealScalar precision_inv = RealScalar(1)/NumTraits::epsilon(); + while (end>0) + { + for (Index i = start; i0 && subdiag[end-1]==RealScalar(0)) + { + end--; + } + if (end<=0) + break; + + // if we spent too many iterations, we give up + iter++; + if(iter > maxIterations * n) break; + + start = end - 1; + while (start>0 && subdiag[start-1]!=0) + start--; + + internal::tridiagonal_qr_step(diag.data(), subdiag.data(), start, end, computeEigenvectors ? eivec.data() : (Scalar*)0, n); + } + if (iter <= maxIterations * n) + info = Success; + else + info = NoConvergence; + + // Sort eigenvalues and corresponding vectors. + // TODO make the sort optional ? + // TODO use a better sort algorithm !! + if (info == Success) + { + for (Index i = 0; i < n-1; ++i) + { + Index k; + diag.segment(i,n-i).minCoeff(&k); + if (k > 0) + { + numext::swap(diag[i], diag[k+i]); + if(computeEigenvectors) + eivec.col(i).swap(eivec.col(k+i)); + } + } + } + return info; +} + +template struct direct_selfadjoint_eigenvalues +{ + EIGEN_DEVICE_FUNC + static inline void run(SolverType& eig, const typename SolverType::MatrixType& A, int options) + { eig.compute(A,options); } +}; + +template struct direct_selfadjoint_eigenvalues +{ + typedef typename SolverType::MatrixType MatrixType; + typedef typename SolverType::RealVectorType VectorType; + typedef typename SolverType::Scalar Scalar; + typedef typename SolverType::EigenvectorsType EigenvectorsType; + + + /** \internal + * Computes the roots of the characteristic polynomial of \a m. + * For numerical stability m.trace() should be near zero and to avoid over- or underflow m should be normalized. + */ + EIGEN_DEVICE_FUNC + static inline void computeRoots(const MatrixType& m, VectorType& roots) + { + EIGEN_USING_STD(sqrt) + EIGEN_USING_STD(atan2) + EIGEN_USING_STD(cos) + EIGEN_USING_STD(sin) + const Scalar s_inv3 = Scalar(1)/Scalar(3); + const Scalar s_sqrt3 = sqrt(Scalar(3)); + + // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The + // eigenvalues are the roots to this equation, all guaranteed to be + // real-valued, because the matrix is symmetric. + Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(1,0)*m(2,0)*m(2,1) - m(0,0)*m(2,1)*m(2,1) - m(1,1)*m(2,0)*m(2,0) - m(2,2)*m(1,0)*m(1,0); + Scalar c1 = m(0,0)*m(1,1) - m(1,0)*m(1,0) + m(0,0)*m(2,2) - m(2,0)*m(2,0) + m(1,1)*m(2,2) - m(2,1)*m(2,1); + Scalar c2 = m(0,0) + m(1,1) + m(2,2); + + // Construct the parameters used in classifying the roots of the equation + // and in solving the equation for the roots in closed form. + Scalar c2_over_3 = c2*s_inv3; + Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3; + a_over_3 = numext::maxi(a_over_3, Scalar(0)); + + Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1)); + + Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b; + q = numext::maxi(q, Scalar(0)); + + // Compute the eigenvalues by solving for the roots of the polynomial. + Scalar rho = sqrt(a_over_3); + Scalar theta = atan2(sqrt(q),half_b)*s_inv3; // since sqrt(q) > 0, atan2 is in [0, pi] and theta is in [0, pi/3] + Scalar cos_theta = cos(theta); + Scalar sin_theta = sin(theta); + // roots are already sorted, since cos is monotonically decreasing on [0, pi] + roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3) + roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); // == 2*rho*cos(theta+ pi/3) + roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta; + } + + EIGEN_DEVICE_FUNC + static inline bool extract_kernel(MatrixType& mat, Ref res, Ref representative) + { + EIGEN_USING_STD(abs); + EIGEN_USING_STD(sqrt); + Index i0; + // Find non-zero column 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); + Scalar n0, n1; + VectorType c0, c1; + n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm(); + n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm(); + if(n0>n1) res = c0/sqrt(n0); + else res = c1/sqrt(n1); + + return true; + } + + EIGEN_DEVICE_FUNC + static inline void run(SolverType& solver, const MatrixType& mat, int options) + { + 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; + + EigenvectorsType& eivecs = solver.m_eivec; + VectorType& eivals = solver.m_eivalues; + + // 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); + // TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for computing the eigenvectors later + MatrixType scaledMat = mat.template selfadjointView(); + scaledMat.diagonal().array() -= shift; + Scalar scale = scaledMat.cwiseAbs().maxCoeff(); + if(scale > 0) scaledMat /= scale; // TODO for scale==0 we could save the remaining operations + + // compute the eigenvalues + computeRoots(scaledMat,eivals); + + // compute the eigenvectors + if(computeEigenvectors) + { + if((eivals(2)-eivals(0))<=Eigen::NumTraits::epsilon()) + { + // All three eigenvalues are numerically the same + eivecs.setIdentity(); + } + else + { + MatrixType tmp; + tmp = scaledMat; + + // Compute the eigenvector of the most distinct eigenvalue + Scalar d0 = eivals(2) - eivals(1); + Scalar d1 = eivals(1) - eivals(0); + Index k(0), l(2); + if(d0 > d1) + { + numext::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. + extract_kernel(tmp, eivecs.col(k), eivecs.col(l)); + } + + // Compute eigenvector of index l + if(d0<=2*Eigen::NumTraits::epsilon()*d1) + { + // If d0 is too small, then the two other eigenvalues are numerically the same, + // and thus we only have to ortho-normalize the near orthogonal vector we saved above. + eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l))*eivecs.col(l); + eivecs.col(l).normalize(); + } + else + { + tmp = scaledMat; + tmp.diagonal().array () -= eivals(l); + + VectorType dummy; + extract_kernel(tmp, eivecs.col(l), dummy); + } + + // 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 +template +struct direct_selfadjoint_eigenvalues +{ + typedef typename SolverType::MatrixType MatrixType; + typedef typename SolverType::RealVectorType VectorType; + typedef typename SolverType::Scalar Scalar; + typedef typename SolverType::EigenvectorsType EigenvectorsType; + + EIGEN_DEVICE_FUNC + static inline void computeRoots(const MatrixType& m, VectorType& roots) + { + EIGEN_USING_STD(sqrt); + const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*numext::abs2(m(1,0))); + const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1)); + roots(0) = t1 - t0; + roots(1) = t1 + t0; + } + + EIGEN_DEVICE_FUNC + static inline void run(SolverType& solver, const MatrixType& mat, int options) + { + EIGEN_USING_STD(sqrt); + EIGEN_USING_STD(abs); + + eigen_assert(mat.cols() == 2 && mat.cols() == mat.rows()); + eigen_assert((options&~(EigVecMask|GenEigMask))==0 + && (options&EigVecMask)!=EigVecMask + && "invalid option parameter"); + bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; + + EigenvectorsType& eivecs = solver.m_eivec; + VectorType& eivals = solver.m_eivalues; + + // 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(2); + MatrixType scaledMat = mat; + scaledMat.coeffRef(0,1) = mat.coeff(1,0); + scaledMat.diagonal().array() -= shift; + Scalar scale = scaledMat.cwiseAbs().maxCoeff(); + if(scale > Scalar(0)) + scaledMat /= scale; + + // Compute the eigenvalues + computeRoots(scaledMat,eivals); + + // compute the eigen vectors + if(computeEigenvectors) + { + if((eivals(1)-eivals(0))<=abs(eivals(1))*Eigen::NumTraits::epsilon()) + { + eivecs.setIdentity(); + } + else + { + scaledMat.diagonal().array () -= eivals(1); + Scalar a2 = numext::abs2(scaledMat(0,0)); + Scalar c2 = numext::abs2(scaledMat(1,1)); + Scalar b2 = numext::abs2(scaledMat(1,0)); + if(a2>c2) + { + eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0); + eivecs.col(1) /= sqrt(a2+b2); + } + else + { + eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0); + eivecs.col(1) /= sqrt(c2+b2); + } + + eivecs.col(0) << eivecs.col(1).unitOrthogonal(); + } + } + + // Rescale back to the original size. + eivals *= scale; + eivals.array() += shift; + + solver.m_info = Success; + solver.m_isInitialized = true; + solver.m_eigenvectorsOk = computeEigenvectors; + } +}; + +} + +template +EIGEN_DEVICE_FUNC +SelfAdjointEigenSolver& SelfAdjointEigenSolver +::computeDirect(const MatrixType& matrix, int options) +{ + internal::direct_selfadjoint_eigenvalues::IsComplex>::run(*this,matrix,options); + return *this; +} + +namespace internal { + +// Francis implicit QR step. +template +EIGEN_DEVICE_FUNC +static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n) +{ + // Wilkinson Shift. + RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5); + RealScalar e = subdiag[end-1]; + // Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still + // underflow thus leading to inf/NaN values when using the following commented code: + // RealScalar e2 = numext::abs2(subdiag[end-1]); + // RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2)); + // This explain the following, somewhat more complicated, version: + RealScalar mu = diag[end]; + if(td==RealScalar(0)) { + mu -= numext::abs(e); + } else if (e != RealScalar(0)) { + const RealScalar e2 = numext::abs2(e); + const RealScalar h = numext::hypot(td,e); + if(e2 == RealScalar(0)) { + mu -= e / ((td + (td>RealScalar(0) ? h : -h)) / e); + } else { + mu -= e2 / (td + (td>RealScalar(0) ? h : -h)); + } + } + + RealScalar x = diag[start] - mu; + RealScalar z = subdiag[start]; + // If z ever becomes zero, the Givens rotation will be the identity and + // z will stay zero for all future iterations. + for (Index k = start; k < end && z != RealScalar(0); ++k) + { + JacobiRotation rot; + rot.makeGivens(x, z); + + // do T = G' T G + RealScalar sdk = rot.s() * diag[k] + rot.c() * subdiag[k]; + RealScalar dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k+1]; + + diag[k] = rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k]) - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k+1]); + diag[k+1] = rot.s() * sdk + rot.c() * dkp1; + subdiag[k] = rot.c() * sdk - rot.s() * dkp1; + + if (k > start) + subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z; + + // "Chasing the bulge" to return to triangular form. + x = subdiag[k]; + if (k < end - 1) + { + z = -rot.s() * subdiag[k+1]; + subdiag[k + 1] = rot.c() * subdiag[k+1]; + } + + // apply the givens rotation to the unit matrix Q = Q * G + if (matrixQ) + { + // FIXME if StorageOrder == RowMajor this operation is not very efficient + Map > q(matrixQ,n,n); + q.applyOnTheRight(k,k+1,rot); + } + } +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SELFADJOINTEIGENSOLVER_H diff --git a/3rdparty/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h b/3rdparty/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h new file mode 100644 index 0000000..b0c947d --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h @@ -0,0 +1,87 @@ +/* + Copyright (c) 2011, Intel Corporation. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of Intel Corporation nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + ******************************************************************************** + * Content : Eigen bindings to LAPACKe + * Self-adjoint eigenvalues/eigenvectors. + ******************************************************************************** +*/ + +#ifndef EIGEN_SAEIGENSOLVER_LAPACKE_H +#define EIGEN_SAEIGENSOLVER_LAPACKE_H + +namespace Eigen { + +/** \internal Specialization for the data types supported by LAPACKe */ + +#define EIGEN_LAPACKE_EIG_SELFADJ_2(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME, EIGCOLROW ) \ +template<> template inline \ +SelfAdjointEigenSolver >& \ +SelfAdjointEigenSolver >::compute(const EigenBase& matrix, int options) \ +{ \ + eigen_assert(matrix.cols() == matrix.rows()); \ + eigen_assert((options&~(EigVecMask|GenEigMask))==0 \ + && (options&EigVecMask)!=EigVecMask \ + && "invalid option parameter"); \ + bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; \ + lapack_int n = internal::convert_index(matrix.cols()), lda, info; \ + m_eivalues.resize(n,1); \ + m_subdiag.resize(n-1); \ + m_eivec = matrix; \ +\ + if(n==1) \ + { \ + m_eivalues.coeffRef(0,0) = numext::real(m_eivec.coeff(0,0)); \ + if(computeEigenvectors) m_eivec.setOnes(n,n); \ + m_info = Success; \ + m_isInitialized = true; \ + m_eigenvectorsOk = computeEigenvectors; \ + return *this; \ + } \ +\ + lda = internal::convert_index(m_eivec.outerStride()); \ + char jobz, uplo='L'/*, range='A'*/; \ + jobz = computeEigenvectors ? 'V' : 'N'; \ +\ + info = LAPACKE_##LAPACKE_NAME( LAPACK_COL_MAJOR, jobz, uplo, n, (LAPACKE_TYPE*)m_eivec.data(), lda, (LAPACKE_RTYPE*)m_eivalues.data() ); \ + m_info = (info==0) ? Success : NoConvergence; \ + m_isInitialized = true; \ + m_eigenvectorsOk = computeEigenvectors; \ + return *this; \ +} + +#define EIGEN_LAPACKE_EIG_SELFADJ(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME ) \ + EIGEN_LAPACKE_EIG_SELFADJ_2(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME, ColMajor ) \ + EIGEN_LAPACKE_EIG_SELFADJ_2(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME, RowMajor ) + +EIGEN_LAPACKE_EIG_SELFADJ(double, double, double, dsyev) +EIGEN_LAPACKE_EIG_SELFADJ(float, float, float, ssyev) +EIGEN_LAPACKE_EIG_SELFADJ(dcomplex, lapack_complex_double, double, zheev) +EIGEN_LAPACKE_EIG_SELFADJ(scomplex, lapack_complex_float, float, cheev) + +} // end namespace Eigen + +#endif // EIGEN_SAEIGENSOLVER_H diff --git a/3rdparty/Eigen/src/Eigenvalues/Tridiagonalization.h b/3rdparty/Eigen/src/Eigenvalues/Tridiagonalization.h new file mode 100644 index 0000000..674c92a --- /dev/null +++ b/3rdparty/Eigen/src/Eigenvalues/Tridiagonalization.h @@ -0,0 +1,561 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2010 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_TRIDIAGONALIZATION_H +#define EIGEN_TRIDIAGONALIZATION_H + +namespace Eigen { + +namespace internal { + +template struct TridiagonalizationMatrixTReturnType; +template +struct traits > + : public traits +{ + typedef typename MatrixType::PlainObject ReturnType; // FIXME shall it be a BandMatrix? + enum { Flags = 0 }; +}; + +template +EIGEN_DEVICE_FUNC +void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs); +} + +/** \eigenvalues_module \ingroup Eigenvalues_Module + * + * + * \class Tridiagonalization + * + * \brief Tridiagonal decomposition of a selfadjoint matrix + * + * \tparam _MatrixType the type of the matrix of which we are computing the + * tridiagonal decomposition; this is expected to be an instantiation of the + * Matrix class template. + * + * This class performs a tridiagonal decomposition of a selfadjoint matrix \f$ A \f$ such that: + * \f$ A = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real symmetric tridiagonal matrix. + * + * A tridiagonal matrix is a matrix which has nonzero elements only on the + * main diagonal and the first diagonal below and above it. The Hessenberg + * decomposition of a selfadjoint matrix is in fact a tridiagonal + * decomposition. This class is used in SelfAdjointEigenSolver to compute the + * eigenvalues and eigenvectors of a selfadjoint matrix. + * + * Call the function compute() to compute the tridiagonal decomposition of a + * given matrix. Alternatively, you can use the Tridiagonalization(const MatrixType&) + * constructor which computes the tridiagonal Schur decomposition at + * construction time. Once the decomposition is computed, you can use the + * matrixQ() and matrixT() functions to retrieve the matrices Q and T in the + * decomposition. + * + * The documentation of Tridiagonalization(const MatrixType&) contains an + * example of the typical use of this class. + * + * \sa class HessenbergDecomposition, class SelfAdjointEigenSolver + */ +template class Tridiagonalization +{ + public: + + /** \brief Synonym for the template parameter \p _MatrixType. */ + typedef _MatrixType MatrixType; + + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + + enum { + Size = MatrixType::RowsAtCompileTime, + SizeMinusOne = Size == Dynamic ? Dynamic : (Size > 1 ? Size - 1 : 1), + Options = MatrixType::Options, + MaxSize = MatrixType::MaxRowsAtCompileTime, + MaxSizeMinusOne = MaxSize == Dynamic ? Dynamic : (MaxSize > 1 ? MaxSize - 1 : 1) + }; + + typedef Matrix CoeffVectorType; + typedef typename internal::plain_col_type::type DiagonalType; + typedef Matrix SubDiagonalType; + typedef typename internal::remove_all::type MatrixTypeRealView; + typedef internal::TridiagonalizationMatrixTReturnType MatrixTReturnType; + + typedef typename internal::conditional::IsComplex, + typename internal::add_const_on_value_type::RealReturnType>::type, + const Diagonal + >::type DiagonalReturnType; + + typedef typename internal::conditional::IsComplex, + typename internal::add_const_on_value_type::RealReturnType>::type, + const Diagonal + >::type SubDiagonalReturnType; + + /** \brief Return type of matrixQ() */ + typedef HouseholderSequence::type> HouseholderSequenceType; + + /** \brief Default constructor. + * + * \param [in] size Positive integer, size of the matrix whose tridiagonal + * decomposition will be computed. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via compute(). The \p size parameter is only + * used as a hint. It is not an error to give a wrong \p size, but it may + * impair performance. + * + * \sa compute() for an example. + */ + explicit Tridiagonalization(Index size = Size==Dynamic ? 2 : Size) + : m_matrix(size,size), + m_hCoeffs(size > 1 ? size-1 : 1), + m_isInitialized(false) + {} + + /** \brief Constructor; computes tridiagonal decomposition of given matrix. + * + * \param[in] matrix Selfadjoint matrix whose tridiagonal decomposition + * is to be computed. + * + * This constructor calls compute() to compute the tridiagonal decomposition. + * + * Example: \include Tridiagonalization_Tridiagonalization_MatrixType.cpp + * Output: \verbinclude Tridiagonalization_Tridiagonalization_MatrixType.out + */ + template + explicit Tridiagonalization(const EigenBase& matrix) + : m_matrix(matrix.derived()), + m_hCoeffs(matrix.cols() > 1 ? matrix.cols()-1 : 1), + m_isInitialized(false) + { + internal::tridiagonalization_inplace(m_matrix, m_hCoeffs); + m_isInitialized = true; + } + + /** \brief Computes tridiagonal decomposition of given matrix. + * + * \param[in] matrix Selfadjoint matrix whose tridiagonal decomposition + * is to be computed. + * \returns Reference to \c *this + * + * The tridiagonal decomposition is computed by bringing the columns of + * the matrix successively in the required form using Householder + * reflections. The cost is \f$ 4n^3/3 \f$ flops, where \f$ n \f$ denotes + * the size of the given matrix. + * + * This method reuses of the allocated data in the Tridiagonalization + * object, if the size of the matrix does not change. + * + * Example: \include Tridiagonalization_compute.cpp + * Output: \verbinclude Tridiagonalization_compute.out + */ + template + Tridiagonalization& compute(const EigenBase& matrix) + { + m_matrix = matrix.derived(); + m_hCoeffs.resize(matrix.rows()-1, 1); + internal::tridiagonalization_inplace(m_matrix, m_hCoeffs); + m_isInitialized = true; + return *this; + } + + /** \brief Returns the Householder coefficients. + * + * \returns a const reference to the vector of Householder coefficients + * + * \pre Either the constructor Tridiagonalization(const MatrixType&) or + * the member function compute(const MatrixType&) has been called before + * to compute the tridiagonal decomposition of a matrix. + * + * The Householder coefficients allow the reconstruction of the matrix + * \f$ Q \f$ in the tridiagonal decomposition from the packed data. + * + * Example: \include Tridiagonalization_householderCoefficients.cpp + * Output: \verbinclude Tridiagonalization_householderCoefficients.out + * + * \sa packedMatrix(), \ref Householder_Module "Householder module" + */ + inline CoeffVectorType householderCoefficients() const + { + eigen_assert(m_isInitialized && "Tridiagonalization is not initialized."); + return m_hCoeffs; + } + + /** \brief Returns the internal representation of the decomposition + * + * \returns a const reference to a matrix with the internal representation + * of the decomposition. + * + * \pre Either the constructor Tridiagonalization(const MatrixType&) or + * the member function compute(const MatrixType&) has been called before + * to compute the tridiagonal decomposition of a matrix. + * + * The returned matrix contains the following information: + * - the strict upper triangular part is equal to the input matrix A. + * - the diagonal and lower sub-diagonal represent the real tridiagonal + * symmetric matrix T. + * - the rest of the lower part contains the Householder vectors that, + * combined with Householder coefficients returned by + * householderCoefficients(), allows to reconstruct the matrix Q as + * \f$ Q = H_{N-1} \ldots H_1 H_0 \f$. + * Here, the matrices \f$ H_i \f$ are the Householder transformations + * \f$ H_i = (I - h_i v_i v_i^T) \f$ + * where \f$ h_i \f$ is the \f$ i \f$th Householder coefficient and + * \f$ v_i \f$ is the Householder vector defined by + * \f$ v_i = [ 0, \ldots, 0, 1, M(i+2,i), \ldots, M(N-1,i) ]^T \f$ + * with M the matrix returned by this function. + * + * See LAPACK for further details on this packed storage. + * + * Example: \include Tridiagonalization_packedMatrix.cpp + * Output: \verbinclude Tridiagonalization_packedMatrix.out + * + * \sa householderCoefficients() + */ + inline const MatrixType& packedMatrix() const + { + eigen_assert(m_isInitialized && "Tridiagonalization is not initialized."); + return m_matrix; + } + + /** \brief Returns the unitary matrix Q in the decomposition + * + * \returns object representing the matrix Q + * + * \pre Either the constructor Tridiagonalization(const MatrixType&) or + * the member function compute(const MatrixType&) has been called before + * to compute the tridiagonal decomposition of a matrix. + * + * This function returns a light-weight object of template class + * HouseholderSequence. You can either apply it directly to a matrix or + * you can convert it to a matrix of type #MatrixType. + * + * \sa Tridiagonalization(const MatrixType&) for an example, + * matrixT(), class HouseholderSequence + */ + HouseholderSequenceType matrixQ() const + { + eigen_assert(m_isInitialized && "Tridiagonalization is not initialized."); + return HouseholderSequenceType(m_matrix, m_hCoeffs.conjugate()) + .setLength(m_matrix.rows() - 1) + .setShift(1); + } + + /** \brief Returns an expression of the tridiagonal matrix T in the decomposition + * + * \returns expression object representing the matrix T + * + * \pre Either the constructor Tridiagonalization(const MatrixType&) or + * the member function compute(const MatrixType&) has been called before + * to compute the tridiagonal decomposition of a matrix. + * + * Currently, this function can be used to extract the matrix T from internal + * data and copy it to a dense matrix object. In most cases, it may be + * sufficient to directly use the packed matrix or the vector expressions + * returned by diagonal() and subDiagonal() instead of creating a new + * dense copy matrix with this function. + * + * \sa Tridiagonalization(const MatrixType&) for an example, + * matrixQ(), packedMatrix(), diagonal(), subDiagonal() + */ + MatrixTReturnType matrixT() const + { + eigen_assert(m_isInitialized && "Tridiagonalization is not initialized."); + return MatrixTReturnType(m_matrix.real()); + } + + /** \brief Returns the diagonal of the tridiagonal matrix T in the decomposition. + * + * \returns expression representing the diagonal of T + * + * \pre Either the constructor Tridiagonalization(const MatrixType&) or + * the member function compute(const MatrixType&) has been called before + * to compute the tridiagonal decomposition of a matrix. + * + * Example: \include Tridiagonalization_diagonal.cpp + * Output: \verbinclude Tridiagonalization_diagonal.out + * + * \sa matrixT(), subDiagonal() + */ + DiagonalReturnType diagonal() const; + + /** \brief Returns the subdiagonal of the tridiagonal matrix T in the decomposition. + * + * \returns expression representing the subdiagonal of T + * + * \pre Either the constructor Tridiagonalization(const MatrixType&) or + * the member function compute(const MatrixType&) has been called before + * to compute the tridiagonal decomposition of a matrix. + * + * \sa diagonal() for an example, matrixT() + */ + SubDiagonalReturnType subDiagonal() const; + + protected: + + MatrixType m_matrix; + CoeffVectorType m_hCoeffs; + bool m_isInitialized; +}; + +template +typename Tridiagonalization::DiagonalReturnType +Tridiagonalization::diagonal() const +{ + eigen_assert(m_isInitialized && "Tridiagonalization is not initialized."); + return m_matrix.diagonal().real(); +} + +template +typename Tridiagonalization::SubDiagonalReturnType +Tridiagonalization::subDiagonal() const +{ + eigen_assert(m_isInitialized && "Tridiagonalization is not initialized."); + return m_matrix.template diagonal<-1>().real(); +} + +namespace internal { + +/** \internal + * Performs a tridiagonal decomposition of the selfadjoint matrix \a matA in-place. + * + * \param[in,out] matA On input the selfadjoint matrix. Only the \b lower triangular part is referenced. + * On output, the strict upper part is left unchanged, and the lower triangular part + * represents the T and Q matrices in packed format has detailed below. + * \param[out] hCoeffs returned Householder coefficients (see below) + * + * On output, the tridiagonal selfadjoint matrix T is stored in the diagonal + * and lower sub-diagonal of the matrix \a matA. + * The unitary matrix Q is represented in a compact way as a product of + * Householder reflectors \f$ H_i \f$ such that: + * \f$ Q = H_{N-1} \ldots H_1 H_0 \f$. + * The Householder reflectors are defined as + * \f$ H_i = (I - h_i v_i v_i^T) \f$ + * where \f$ h_i = hCoeffs[i]\f$ is the \f$ i \f$th Householder coefficient and + * \f$ v_i \f$ is the Householder vector defined by + * \f$ v_i = [ 0, \ldots, 0, 1, matA(i+2,i), \ldots, matA(N-1,i) ]^T \f$. + * + * Implemented from Golub's "Matrix Computations", algorithm 8.3.1. + * + * \sa Tridiagonalization::packedMatrix() + */ +template +EIGEN_DEVICE_FUNC +void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs) +{ + using numext::conj; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + Index n = matA.rows(); + eigen_assert(n==matA.cols()); + eigen_assert(n==hCoeffs.size()+1 || n==1); + + for (Index i = 0; i() + * (conj(h) * matA.col(i).tail(remainingSize))); + + hCoeffs.tail(n-i-1) += (conj(h)*RealScalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1); + + matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView() + .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), Scalar(-1)); + + matA.col(i).coeffRef(i+1) = beta; + hCoeffs.coeffRef(i) = h; + } +} + +// forward declaration, implementation at the end of this file +template::IsComplex> +struct tridiagonalization_inplace_selector; + +/** \brief Performs a full tridiagonalization in place + * + * \param[in,out] mat On input, the selfadjoint matrix whose tridiagonal + * decomposition is to be computed. Only the lower triangular part referenced. + * The rest is left unchanged. On output, the orthogonal matrix Q + * in the decomposition if \p extractQ is true. + * \param[out] diag The diagonal of the tridiagonal matrix T in the + * decomposition. + * \param[out] subdiag The subdiagonal of the tridiagonal matrix T in + * the decomposition. + * \param[in] extractQ If true, the orthogonal matrix Q in the + * decomposition is computed and stored in \p mat. + * + * Computes the tridiagonal decomposition of the selfadjoint matrix \p mat in place + * such that \f$ mat = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real + * symmetric tridiagonal matrix. + * + * The tridiagonal matrix T is passed to the output parameters \p diag and \p subdiag. If + * \p extractQ is true, then the orthogonal matrix Q is passed to \p mat. Otherwise the lower + * part of the matrix \p mat is destroyed. + * + * The vectors \p diag and \p subdiag are not resized. The function + * assumes that they are already of the correct size. The length of the + * vector \p diag should equal the number of rows in \p mat, and the + * length of the vector \p subdiag should be one left. + * + * This implementation contains an optimized path for 3-by-3 matrices + * which is especially useful for plane fitting. + * + * \note Currently, it requires two temporary vectors to hold the intermediate + * Householder coefficients, and to reconstruct the matrix Q from the Householder + * reflectors. + * + * Example (this uses the same matrix as the example in + * Tridiagonalization::Tridiagonalization(const MatrixType&)): + * \include Tridiagonalization_decomposeInPlace.cpp + * Output: \verbinclude Tridiagonalization_decomposeInPlace.out + * + * \sa class Tridiagonalization + */ +template +EIGEN_DEVICE_FUNC +void tridiagonalization_inplace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, + CoeffVectorType& hcoeffs, bool extractQ) +{ + eigen_assert(mat.cols()==mat.rows() && diag.size()==mat.rows() && subdiag.size()==mat.rows()-1); + tridiagonalization_inplace_selector::run(mat, diag, subdiag, hcoeffs, extractQ); +} + +/** \internal + * General full tridiagonalization + */ +template +struct tridiagonalization_inplace_selector +{ + typedef typename Tridiagonalization::CoeffVectorType CoeffVectorType; + typedef typename Tridiagonalization::HouseholderSequenceType HouseholderSequenceType; + template + static EIGEN_DEVICE_FUNC + void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, CoeffVectorType& hCoeffs, bool extractQ) + { + tridiagonalization_inplace(mat, hCoeffs); + diag = mat.diagonal().real(); + subdiag = mat.template diagonal<-1>().real(); + if(extractQ) + mat = HouseholderSequenceType(mat, hCoeffs.conjugate()) + .setLength(mat.rows() - 1) + .setShift(1); + } +}; + +/** \internal + * Specialization for 3x3 real matrices. + * Especially useful for plane fitting. + */ +template +struct tridiagonalization_inplace_selector +{ + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + + template + static void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, CoeffVectorType&, bool extractQ) + { + using std::sqrt; + const RealScalar tol = (std::numeric_limits::min)(); + diag[0] = mat(0,0); + RealScalar v1norm2 = numext::abs2(mat(2,0)); + if(v1norm2 <= tol) + { + diag[1] = mat(1,1); + diag[2] = mat(2,2); + subdiag[0] = mat(1,0); + subdiag[1] = mat(2,1); + if (extractQ) + mat.setIdentity(); + } + else + { + RealScalar beta = sqrt(numext::abs2(mat(1,0)) + v1norm2); + RealScalar invBeta = RealScalar(1)/beta; + Scalar m01 = mat(1,0) * invBeta; + Scalar m02 = mat(2,0) * invBeta; + Scalar q = RealScalar(2)*m01*mat(2,1) + m02*(mat(2,2) - mat(1,1)); + diag[1] = mat(1,1) + m02*q; + diag[2] = mat(2,2) - m02*q; + subdiag[0] = beta; + subdiag[1] = mat(2,1) - m01 * q; + if (extractQ) + { + mat << 1, 0, 0, + 0, m01, m02, + 0, m02, -m01; + } + } + } +}; + +/** \internal + * Trivial specialization for 1x1 matrices + */ +template +struct tridiagonalization_inplace_selector +{ + typedef typename MatrixType::Scalar Scalar; + + template + static EIGEN_DEVICE_FUNC + void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType&, CoeffVectorType&, bool extractQ) + { + diag(0,0) = numext::real(mat(0,0)); + if(extractQ) + mat(0,0) = Scalar(1); + } +}; + +/** \internal + * \eigenvalues_module \ingroup Eigenvalues_Module + * + * \brief Expression type for return value of Tridiagonalization::matrixT() + * + * \tparam MatrixType type of underlying dense matrix + */ +template struct TridiagonalizationMatrixTReturnType +: public ReturnByValue > +{ + public: + /** \brief Constructor. + * + * \param[in] mat The underlying dense matrix + */ + TridiagonalizationMatrixTReturnType(const MatrixType& mat) : m_matrix(mat) { } + + template + inline void evalTo(ResultType& result) const + { + result.setZero(); + result.template diagonal<1>() = m_matrix.template diagonal<-1>().conjugate(); + result.diagonal() = m_matrix.diagonal(); + result.template diagonal<-1>() = m_matrix.template diagonal<-1>(); + } + + EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } + EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + + protected: + typename MatrixType::Nested m_matrix; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_TRIDIAGONALIZATION_H diff --git a/CMakeLists.txt b/CMakeLists.txt index 2067bc1..ed30fe4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,10 @@ -cmake_minimum_required(VERSION 3.5...3.18) -project(nano_fmm) +cmake_minimum_required(VERSION 3.15...3.26) + +# https://scikit-build-core.readthedocs.io/en/latest/cmakelists.html#accessing-information +project( + ${SKBUILD_PROJECT_NAME} + VERSION ${SKBUILD_PROJECT_VERSION} + LANGUAGES CXX) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) @@ -35,7 +40,16 @@ elseif(CMAKE_BUILD_TYPE STREQUAL "Release") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") endif() +# TODO, use set(PYBIND11_NEWPYTHON ON)? +# https://scikit-build-core.readthedocs.io/en/latest/getting_started.html +find_package(Python REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 CONFIG REQUIRED) + include_directories(3rdparty src) +file(GLOB_RECURSE SRCS src/*.cpp) +python_add_library(_core MODULE ${SRCS} WITH_SOABI) +target_link_libraries(_core PRIVATE pybind11::headers) +target_include_directories(_core PRIVATE src) option(BUILD_EXAMPLES "Build examples" OFF) if(BUILD_EXAMPLES) @@ -47,10 +61,5 @@ if(BUILD_EXAMPLES) endforeach(src) endif() -set(PYBIND11_CPP_STANDARD -std=c++17) -add_subdirectory(pybind11) -file(GLOB_RECURSE SRCS src/*.cpp) -pybind11_add_module(_nano_fmm ${SRCS}) - -target_compile_definitions(_nano_fmm - PRIVATE VERSION_INFO=${NANO_FMM_VERSION_INFO}) +target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) +install(TARGETS _core DESTINATION nano_fmm) diff --git a/Makefile b/Makefile index 98e3ebb..085294f 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,6 @@ PROJECT_SOURCE_DIR ?= $(abspath ./) PROJECT_NAME ?= $(shell basename $(PROJECT_SOURCE_DIR)) +NUM_JOBS ?= 8 all: @echo nothing special @@ -20,11 +21,6 @@ lint: lint_install: pre-commit install -build: - mkdir -p build && cd build && \ - cmake .. && make -.PHONY: build - docs_build: python3 -m pip install -r docs/requirements.txt mkdocs build @@ -53,20 +49,22 @@ test_in_dev_container: -v `pwd`:`pwd` -w `pwd` -it $(DEV_CONTAINER_IMAG) bash PYTHON ?= python3 +build: + $(PYTHON) -m pip install scikit_build_core pyproject_metadata pathspec pybind11 + CMAKE_BUILD_PARALLEL_LEVEL=$(NUM_JOBS) $(PYTHON) -m pip install --no-build-isolation -Ceditable.rebuild=true -Cbuild-dir=build -ve. python_install: - $(PYTHON) setup.py install --force -python_build: - $(PYTHON) setup.py bdist_wheel + $(PYTHON) -m pip install . --verbose +python_wheel: + $(PYTHON) -m pip wheel . -w build --verbose python_sdist: - $(PYTHON) setup.py sdist -python_sdist_wheel: - $(PYTHON) -m pip wheel dist/nano_fmm-*.tar.gz --no-deps + $(PYTHON) -m pip sdist . --verbose python_test: $(PYTHON) -c 'import nano_fmm; print(nano_fmm.add(1, 2))' $(PYTHON) -m nano_fmm add 1 2 $(PYTHON) -m nano_fmm pure_python_func --arg1=43234 python3 -m pip install pytest pytest tests +.PHONY: build # conda create -y -n py36 python=3.6 # conda create -y -n py37 python=3.7 @@ -90,6 +88,7 @@ python_build_py311: python_build_all: python_build_py36 python_build_py37 python_build_py38 python_build_py39 python_build_py310 python_build_py311 python_build_all_in_linux: docker run --rm -w `pwd` -v `pwd`:`pwd` -v `pwd`/build/linux:`pwd`/build -it $(DOCKER_TAG_LINUX) make python_build_all repair_wheels + make repair_wheels && rm -rf dist/*.whl && mv wheelhouse/*.whl dist && rm -rf wheelhouse python_build_all_in_macos: python_build_py38 python_build_py39 python_build_py310 python_build_py311 python_build_all_in_windows: python_build_all diff --git a/README.md b/README.md index 3dee0d4..fc6d7e2 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,6 @@ TODO - TODO: - collapse diff --git a/data/Makefile b/data/Makefile index 6ddd88c..156dec4 100644 --- a/data/Makefile +++ b/data/Makefile @@ -14,13 +14,13 @@ clean: # https://github.com/cubao/fmm/wiki/Sample-data pull: network.json -network.json: network.zip - unzip $< +network.json: | network.zip + unzip network.zip network.zip: curl -LO https://github.com/cubao/fmm/files/11698908/network.zip pull: suzhoubeizhan.json -suzhoubeizhan.json: suzhoubeizhan.zip - unzip $< +suzhoubeizhan.json: | suzhoubeizhan.zip + unzip suzhoubeizhan.zip suzhoubeizhan.zip: curl -LO https://github.com/cubao/fmm/files/12568463/suzhoubeizhan.zip diff --git a/nano_fmm/benchmarks.py b/nano_fmm/benchmarks.py deleted file mode 100755 index 6c76375..0000000 --- a/nano_fmm/benchmarks.py +++ /dev/null @@ -1 +0,0 @@ -from _nano_fmm.benchmarks import * # noqa: F403 diff --git a/nano_fmm/flatbush.py b/nano_fmm/flatbush.py deleted file mode 100755 index a73e405..0000000 --- a/nano_fmm/flatbush.py +++ /dev/null @@ -1 +0,0 @@ -from _nano_fmm.flatbush import * # noqa: F403 diff --git a/pybind11 b/pybind11 deleted file mode 160000 index 3cc7e42..0000000 --- a/pybind11 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3cc7e4258c15a6a19ba5e0b62a220b1a6196d4eb diff --git a/pyproject.toml b/pyproject.toml index 428cde0..eb224ce 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,48 +1,112 @@ [build-system] -requires = [ - "setuptools>=42", - "wheel", - "ninja", - "cmake>=3.12", +requires = ["scikit-build-core>=0.3.3", "pybind11"] +build-backend = "scikit_build_core.build" + + +[project] +name = "nano_fmm" +version = "0.1.2" +description="c++/python version of coordtransform" +readme = "README.md" +authors = [ + { name = "tzx", email = "dvorak4tzx@gmail.com" }, +] +requires-python = ">=3.7" +classifiers = [ + "Development Status :: 4 - Beta", + "License :: OSI Approved :: MIT License", + "Programming Language :: Python :: 3 :: Only", + "Programming Language :: Python :: 3.7", + "Programming Language :: Python :: 3.8", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", +] +dependencies = [ + "fire", + "loguru", + "numpy", ] -build-backend = "setuptools.build_meta" -[tool.mypy] -files = "setup.py" -python_version = "3.7" -strict = true -show_error_codes = true -enable_error_code = ["ignore-without-code", "redundant-expr", "truthy-bool"] -warn_unreachable = true +[project.urls] +Homepage = "https://nano-fmm.readthedocs.io" -[[tool.mypy.overrides]] -module = ["ninja"] -ignore_missing_imports = true +[project.optional-dependencies] +test = ["pytest"] + + +# docs: https://scikit-build-core.readthedocs.io/en/latest/configuration.html +[tool.scikit-build] +wheel.expand-macos-universal-tags = true +# cmake.build-type = "Debug" +# Setuptools-style build caching in a local directory +build-dir = "build/{wheel_tag}" +# Build stable ABI wheels for CPython 3.12+ +wheel.py-api = "cp312" + +[tool.scikit-build.cmake.define] +# SOME_DEFINE = "ON" [tool.pytest.ini_options] minversion = "6.0" addopts = ["-ra", "--showlocals", "--strict-markers", "--strict-config"] xfail_strict = true -filterwarnings = ["error"] +filterwarnings = [ + "error", + "ignore:(ast.Str|Attribute s|ast.NameConstant|ast.Num) is deprecated:DeprecationWarning:_pytest", # Python 3.12 +] testpaths = ["tests"] + [tool.cibuildwheel] test-command = "pytest {project}/tests" test-extras = ["test"] test-skip = ["*universal2:arm64"] -# Setuptools bug causes collision between pypy and cpython artifacts -before-build = "rm -rf {project}/build" +build-verbosity = 1 + +# Needed for full C++17 support +[tool.cibuildwheel.macos.environment] +MACOSX_DEPLOYMENT_TARGET = "10.14" [tool.ruff] +src = ["src"] +extend-ignore = [ + "UP007", + "PTH100", # TODO, use Pathlib + "PTH103", # TODO, use Pathlib + "PTH120", # TODO, use Pathlib + "PTH123", # TODO, use Pathlib +] + +[tool.ruff.lint] extend-select = [ - "B", # flake8-bugbear - "B904", - "I", # isort - "PGH", # pygrep-hooks - "RUF", # Ruff-specific - "UP", # pyupgrade + "B", # flake8-bugbear + "I", # isort + "ARG", # flake8-unused-arguments + "C4", # flake8-comprehensions + "EM", # flake8-errmsg + "ICN", # flake8-import-conventions + "G", # flake8-logging-format + "PGH", # pygrep-hooks + "PIE", # flake8-pie + "PL", # pylint + "PT", # flake8-pytest-style + "PTH", # flake8-use-pathlib + "RET", # flake8-return + "RUF", # Ruff-specific + "SIM", # flake8-simplify + "T20", # flake8-print + "UP", # pyupgrade + "YTT", # flake8-2020 + "EXE", # flake8-executable + "NPY", # NumPy specific rules + "PD", # pandas-vet ] -extend-ignore = [ - "E501", # Line too long +ignore = [ + "PLR", # Design related pylint codes ] -target-version = "py37" +isort.required-imports = ["from __future__ import annotations"] + +[tool.ruff.per-file-ignores] +"tests/**" = ["T20"] diff --git a/setup.py b/setup.py deleted file mode 100644 index 89ac7ae..0000000 --- a/setup.py +++ /dev/null @@ -1,144 +0,0 @@ -import os -import re -import subprocess -import sys -from pathlib import Path - -from setuptools import Extension, find_packages, setup -from setuptools.command.build_ext import build_ext - -# Convert distutils Windows platform specifiers to CMake -A arguments -PLAT_TO_CMAKE = { - "win32": "Win32", - "win-amd64": "x64", - "win-arm32": "ARM", - "win-arm64": "ARM64", -} - - -# A CMakeExtension needs a sourcedir instead of a file list. -# The name must be the _single_ output extension from the CMake build. -# If you need multiple extensions, see scikit-build. -class CMakeExtension(Extension): - def __init__(self, name: str, sourcedir: str = "") -> None: - super().__init__(name, sources=[]) - self.sourcedir = os.fspath(Path(sourcedir).resolve()) - - -class CMakeBuild(build_ext): - def build_extension(self, ext: CMakeExtension) -> None: - # Must be in this form due to bug in .resolve() only fixed in Python 3.10+ - ext_fullpath = Path.cwd() / self.get_ext_fullpath(ext.name) - extdir = ext_fullpath.parent.resolve() - - # Using this requires trailing slash for auto-detection & inclusion of - # auxiliary "native" libs - - debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug - cfg = "Debug" if debug else "Release" - - # CMake lets you override the generator - we need to check this. - # Can be set with Conda-Build, for example. - cmake_generator = os.environ.get("CMAKE_GENERATOR", "") - - # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON - # EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code - # from Python. - cmake_args = [ - f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}{os.sep}", - f"-DPYTHON_EXECUTABLE={sys.executable}", - f"-DCMAKE_BUILD_TYPE={cfg}", # not used on MSVC, but no harm - ] - build_args = [] - # Adding CMake arguments set as environment variable - # (needed e.g. to build for ARM OSx on conda-forge) - if "CMAKE_ARGS" in os.environ: - cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item] - - # In this example, we pass in the version to C++. You might not need to. - cmake_args += [f"-DNANO_FMM_VERSION_INFO={self.distribution.get_version()}"] - - if self.compiler.compiler_type != "msvc": - # Using Ninja-build since it a) is available as a wheel and b) - # multithreads automatically. MSVC would require all variables be - # exported for Ninja to pick it up, which is a little tricky to do. - # Users can override the generator with CMAKE_GENERATOR in CMake - # 3.15+. - if not cmake_generator or cmake_generator == "Ninja": - try: - import ninja - - ninja_executable_path = Path(ninja.BIN_DIR) / "ninja" - cmake_args += [ - "-GNinja", - f"-DCMAKE_MAKE_PROGRAM:FILEPATH={ninja_executable_path}", - ] - except ImportError: - pass - - else: - # Single config generators are handled "normally" - single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) - - # CMake allows an arch-in-generator style for backward compatibility - contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) - - # Specify the arch if using MSVC generator, but only if it doesn't - # contain a backward-compatibility arch spec already in the - # generator name. - if not single_config and not contains_arch: - cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]] - - # Multi-config generators have a different way to specify configs - if not single_config: - cmake_args += [ - f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}" - ] - build_args += ["--config", cfg] - - if sys.platform.startswith("darwin"): - # Cross-compile support for macOS - respect ARCHFLAGS if set - archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", "")) - if archs: - cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))] - - # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level - # across all generators. - if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ: - # self.parallel is a Python 3 only way to set parallel jobs by hand - # using -j in the build_ext call, not supported by pip or PyPA-build. - if hasattr(self, "parallel") and self.parallel: - # CMake 3.12+ only. - build_args += [f"-j{self.parallel}"] - - build_temp = Path(self.build_temp) / ext.name - if not build_temp.exists(): - build_temp.mkdir(parents=True) - - subprocess.run( - ["cmake", ext.sourcedir, *cmake_args], cwd=build_temp, check=True - ) - subprocess.run( - ["cmake", "--build", ".", *build_args], cwd=build_temp, check=True - ) - - -# The information here can also be placed in setup.cfg - better separation of -# logic and declaration, and simpler if you include description/version in a file. -setup( - name="nano_fmm", - version="0.0.1", - author="tzx", - author_email="dvorak4tzx@gmail.com", - url="https://nano-fmm.readthedocs.io", - description="nano fmm", - long_description=open("README.md", encoding="utf-8").read(), - long_description_content_type="text/markdown", - packages=find_packages(), - ext_modules=[CMakeExtension("nano_fmm")], - cmdclass={"build_ext": CMakeBuild}, - zip_safe=False, - python_requires=">=3.7", - install_requires=["fire"], - extras_require={"test": ["pytest>=6.0"]}, -) diff --git a/src/bindings/pybind11_network.cpp b/src/bindings/pybind11_network.cpp index c767dd8..c2e1922 100644 --- a/src/bindings/pybind11_network.cpp +++ b/src/bindings/pybind11_network.cpp @@ -6,7 +6,13 @@ #include "nano_fmm/network.hpp" #include "nano_fmm/indexer.hpp" + #include "spdlog/spdlog.h" +// fix exposed macro 'GetObject' from wingdi.h (included by spdlog.h) under +// windows, see https://github.com/Tencent/rapidjson/issues/1448 +#ifdef GetObject +#undef GetObject +#endif namespace nano_fmm { diff --git a/src/bindings/pybind11_packedrtree.cpp b/src/bindings/pybind11_packedrtree.cpp index 65234c9..1ee0530 100644 --- a/src/bindings/pybind11_packedrtree.cpp +++ b/src/bindings/pybind11_packedrtree.cpp @@ -5,7 +5,14 @@ #include #include "packedrtree.hpp" + #include "spdlog/spdlog.h" +// fix exposed macro 'GetObject' from wingdi.h (included by spdlog.h) under +// windows, see https://github.com/Tencent/rapidjson/issues/1448 +#ifdef GetObject +#undef GetObject +#endif + #include "nano_fmm/types.hpp" namespace nano_fmm diff --git a/src/bindings/utils.cpp b/src/bindings/pybind11_utils.cpp similarity index 92% rename from src/bindings/utils.cpp rename to src/bindings/pybind11_utils.cpp index 4ba23a0..cc21a2d 100644 --- a/src/bindings/utils.cpp +++ b/src/bindings/pybind11_utils.cpp @@ -7,7 +7,12 @@ #include "nano_fmm/utils.hpp" #include "spdlog/spdlog.h" -#include +#include "spdlog/sinks/stdout_sinks.h" +// fix exposed macro 'GetObject' from wingdi.h (included by spdlog.h) under +// windows, see https://github.com/Tencent/rapidjson/issues/1448 +#ifdef GetObject +#undef GetObject +#endif namespace nano_fmm { @@ -53,6 +58,7 @@ void bind_utils(py::module &m) .def("cheap_ruler_k", &utils::cheap_ruler_k, "latitude"_a) .def("cheap_ruler_k_lookup_table", &utils::cheap_ruler_k_lookup_table, "latitude"_a) + .def("offset", &utils::offset, "lla_src"_a, "lla_dst"_a) .def("bbox", py::overload_cast( &utils::bbox), diff --git a/src/main.cpp b/src/main.cpp index 5ed6a38..ec98973 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -18,7 +18,7 @@ void bind_rapidjson(py::module &m); void bind_utils(py::module &m); } // namespace nano_fmm -PYBIND11_MODULE(_nano_fmm, m) +PYBIND11_MODULE(_core, m) { m.def("add", &add, R"pbdoc( Add two numbers diff --git a/nano_fmm/__init__.py b/src/nano_fmm/__init__.py similarity index 59% rename from nano_fmm/__init__.py rename to src/nano_fmm/__init__.py index 6a8b8ad..ceef5a1 100644 --- a/nano_fmm/__init__.py +++ b/src/nano_fmm/__init__.py @@ -1,5 +1,7 @@ -from _nano_fmm import * # noqa: F403 -from _nano_fmm import __version__ # noqa: F401 +from __future__ import annotations + +from ._core import * # noqa: F403 +from ._core import __version__ # noqa: F401 def pure_python_func( diff --git a/nano_fmm/__main__.py b/src/nano_fmm/__main__.py similarity index 82% rename from nano_fmm/__main__.py rename to src/nano_fmm/__main__.py index 8f81dd8..9691134 100644 --- a/nano_fmm/__main__.py +++ b/src/nano_fmm/__main__.py @@ -1,3 +1,5 @@ +from __future__ import annotations + from nano_fmm import * # noqa: F403 if __name__ == "__main__": diff --git a/src/nano_fmm/benchmarks.py b/src/nano_fmm/benchmarks.py new file mode 100644 index 0000000..7b369ea --- /dev/null +++ b/src/nano_fmm/benchmarks.py @@ -0,0 +1,3 @@ +from __future__ import annotations + +from ._core.benchmarks import * # noqa: F403 diff --git a/src/nano_fmm/config.cpp b/src/nano_fmm/config.cpp old mode 100755 new mode 100644 diff --git a/nano_fmm/converter.py b/src/nano_fmm/converter.py old mode 100755 new mode 100644 similarity index 93% rename from nano_fmm/converter.py rename to src/nano_fmm/converter.py index a8214a6..da8c980 --- a/nano_fmm/converter.py +++ b/src/nano_fmm/converter.py @@ -1,5 +1,7 @@ +from __future__ import annotations + import os -from typing import Union +from typing import Optional, Union from loguru import logger @@ -9,7 +11,7 @@ def remap_network_with_string_id( network: Union[str, rapidjson], *, - export: str = None, + export: Optional[str] = None, ): """ network.json normally has: @@ -55,14 +57,13 @@ def remap_network_with_string_id( network.dump(export, indent=True) logger.info(f"wrote to {export}") return export - else: - return network, indexer + return network, indexer def remap_network_to_string_id( network: Union[str, rapidjson], *, - export: str = None, + export: Optional[str] = None, ): if isinstance(network, str): path = network @@ -83,8 +84,7 @@ def remap_network_to_string_id( network.dump(export, indent=True) logger.info(f"wrote to {export}") return export - else: - return network + return network if __name__ == "__main__": diff --git a/src/nano_fmm/flatbush.py b/src/nano_fmm/flatbush.py new file mode 100644 index 0000000..5c3dac6 --- /dev/null +++ b/src/nano_fmm/flatbush.py @@ -0,0 +1,3 @@ +from __future__ import annotations + +from ._core.flatbush import * # noqa: F403 diff --git a/src/nano_fmm/network.cpp b/src/nano_fmm/network.cpp index 6bf6dba..22ea961 100644 --- a/src/nano_fmm/network.cpp +++ b/src/nano_fmm/network.cpp @@ -1,11 +1,17 @@ #include "nano_fmm/network.hpp" #include "nano_fmm/utils.hpp" #include "nano_fmm/heap.hpp" + #include "spdlog/spdlog.h" +// fix exposed macro 'GetObject' from wingdi.h (included by spdlog.h) under +// windows, see https://github.com/Tencent/rapidjson/issues/1448 +#ifdef GetObject +#undef GetObject +#endif #include "nano_fmm/rapidjson_helpers.hpp" -#include +// #include namespace nano_fmm { @@ -145,10 +151,16 @@ Network::query(const Eigen::Vector3d &position, double radius, nearests.emplace_back(P, poly.segment(s).dir(), d, // pair.first, poly.range(s, t)); } - std::sort(nearests.begin(), nearests.end(), - [](auto &n1, auto &n2) { return n1.distance() < n2.distance(); }); - if (k && nearests.size() > *k) { + + if (k && *k < nearests.size()) { + std::partial_sort( + nearests.begin(), nearests.begin() + *k, nearests.end(), + [](auto &n1, auto &n2) { return n1.distance() < n2.distance(); }); nearests.resize(*k); + } else { + std::sort(nearests.begin(), nearests.end(), [](auto &n1, auto &n2) { + return n1.distance() < n2.distance(); + }); } return nearests; } @@ -210,6 +222,9 @@ MatchResult Network::match(const RowVectors &trajectory) const void Network::build(int execution_polylicy) const { + std::for_each(roads_.begin(), roads_.end(), + [](auto &pair) { pair.second.build(); }); + /* if (execution_polylicy == 1) { std::for_each(std::execution::par, roads_.begin(), roads_.end(), [](auto &pair) { pair.second.build(); }); @@ -220,6 +235,7 @@ void Network::build(int execution_polylicy) const std::for_each(std::execution::seq, roads_.begin(), roads_.end(), [](auto &pair) { pair.second.build(); }); } + */ rtree(); } diff --git a/src/nano_fmm/network.hpp b/src/nano_fmm/network.hpp index 8089d97..12f4fff 100644 --- a/src/nano_fmm/network.hpp +++ b/src/nano_fmm/network.hpp @@ -44,7 +44,7 @@ struct Network return *this; } - // query + // query: Candidate search (CS) std::vector query(const Eigen::Vector3d &position, double radius, std::optional k = std::nullopt, diff --git a/nano_fmm/osmnx_utils.py b/src/nano_fmm/osmnx_utils.py old mode 100755 new mode 100644 similarity index 94% rename from nano_fmm/osmnx_utils.py rename to src/nano_fmm/osmnx_utils.py index 88474bc..1a9b943 --- a/nano_fmm/osmnx_utils.py +++ b/src/nano_fmm/osmnx_utils.py @@ -1,7 +1,9 @@ +from __future__ import annotations + import json import os from collections import defaultdict -from typing import List +from typing import Optional import numpy as np import osmnx as ox @@ -38,8 +40,8 @@ def deduplicate_points(coords: np.ndarray) -> np.ndarray: def pull_map( output: str, *, - bbox: List[float] = None, - center_dist: List[float] = None, + bbox: Optional[list[float]] = None, + center_dist: Optional[list[float]] = None, network_type: str = "drive", ): if bbox is not None: @@ -62,9 +64,10 @@ def pull_map( simplify=False, ) else: - raise Exception( + err = ( "should specify --bbox=LEFT,BOTTOM,RIGHT,TOP or --center_dist=LON,LAT,DIST" ) + raise Exception(err) # G = ox.graph_from_address("350 5th Ave, New York, New York", network_type="drive") # G = ox.graph_from_place("Los Angeles, California", network_type="drive") diff --git a/src/nano_fmm/polyline.hpp b/src/nano_fmm/polyline.hpp index b6b8e5a..990b2f7 100644 --- a/src/nano_fmm/polyline.hpp +++ b/src/nano_fmm/polyline.hpp @@ -2,6 +2,11 @@ #include "nano_fmm/types.hpp" #include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif namespace nano_fmm { diff --git a/src/nano_fmm/randoms.hpp b/src/nano_fmm/randoms.hpp index a8e12bf..47e8243 100644 --- a/src/nano_fmm/randoms.hpp +++ b/src/nano_fmm/randoms.hpp @@ -2,7 +2,13 @@ #include #include + #include "spdlog/spdlog.h" +// fix exposed macro 'GetObject' from wingdi.h (included by spdlog.h) under +// windows, see https://github.com/Tencent/rapidjson/issues/1448 +#ifdef GetObject +#undef GetObject +#endif namespace nano_fmm { diff --git a/src/nano_fmm/rapidjson.cpp b/src/nano_fmm/rapidjson.cpp index f5ea7a0..01c60ce 100644 --- a/src/nano_fmm/rapidjson.cpp +++ b/src/nano_fmm/rapidjson.cpp @@ -4,6 +4,11 @@ #include "nano_fmm/network.hpp" #include "spdlog/spdlog.h" +// fix exposed macro 'GetObject' from wingdi.h (included by spdlog.h) under +// windows, see https://github.com/Tencent/rapidjson/issues/1448 +#ifdef GetObject +#undef GetObject +#endif namespace nano_fmm { diff --git a/nano_fmm/road_network.py b/src/nano_fmm/road_network.py old mode 100755 new mode 100644 similarity index 88% rename from nano_fmm/road_network.py rename to src/nano_fmm/road_network.py index b9f091b..6d16b14 --- a/nano_fmm/road_network.py +++ b/src/nano_fmm/road_network.py @@ -1,3 +1,5 @@ +from __future__ import annotations + from nano_fmm import Network diff --git a/src/nano_fmm/types.hpp b/src/nano_fmm/types.hpp index f93e22a..32ebb73 100644 --- a/src/nano_fmm/types.hpp +++ b/src/nano_fmm/types.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include namespace nano_fmm @@ -46,6 +47,7 @@ using RapidjsonValue = struct LineSegment { // LineSegment(A -> B) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW const Eigen::Vector3d A, B, AB; const double len2, inv_len2; LineSegment(const Eigen::Vector3d &a, const Eigen::Vector3d &b) diff --git a/src/nano_fmm/utils.hpp b/src/nano_fmm/utils.hpp index 03d4c09..df3e481 100644 --- a/src/nano_fmm/utils.hpp +++ b/src/nano_fmm/utils.hpp @@ -3,6 +3,10 @@ #include "nano_fmm/types.hpp" #include +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + namespace nano_fmm { namespace utils @@ -47,10 +51,17 @@ inline Eigen::Vector3d cheap_ruler_k_lookup_table(double latitude) return Ks[idx]; } +inline Eigen::Vector3d offset(const Eigen::Vector3d &lla_src, + const Eigen::Vector3d &lla_dst) +{ + return (lla_dst - lla_src).array() * + cheap_ruler_k_lookup_table(lla_src[1]).array(); +} + inline Eigen::Vector4d bbox(const Eigen::Vector2d &lon_lat, // double width, double height) { - auto k = cheap_ruler_k(lon_lat[1]); + auto k = cheap_ruler_k_lookup_table(lon_lat[1]); double dlon = 1.0 / k[0] * width / 2.0; double dlat = 1.0 / k[1] * height / 2.0; return Eigen::Vector4d(lon_lat[0] - dlon, lon_lat[1] - dlat, @@ -72,7 +83,7 @@ inline RowVectors lla2enu(const Eigen::Ref &llas, anchor_lla = llas.row(0); } if (!k) { - k = cheap_ruler_k((*anchor_lla)[1]); + k = cheap_ruler_k_lookup_table((*anchor_lla)[1]); } RowVectors enus = llas; for (int i = 0; i < 3; ++i) { @@ -89,7 +100,7 @@ inline RowVectors enu2lla(const Eigen::Ref &enus, return RowVectors(0, 3); } if (!k) { - k = cheap_ruler_k(anchor_lla[1]); + k = cheap_ruler_k_lookup_table(anchor_lla[1]); } RowVectors llas = enus; for (int i = 0; i < 3; ++i) { diff --git a/nano_fmm/utils.py b/src/nano_fmm/utils.py old mode 100755 new mode 100644 similarity index 79% rename from nano_fmm/utils.py rename to src/nano_fmm/utils.py index 62e1263..182fb23 --- a/nano_fmm/utils.py +++ b/src/nano_fmm/utils.py @@ -1,5 +1,8 @@ +from __future__ import annotations + import numpy as np -from _nano_fmm.utils import * # noqa: F403 + +from ._core.utils import * # noqa: F403 def bbox2llas(bbox: np.ndarray, *, alt: float = 0.0): diff --git a/tests/test_basic.py b/tests/test_basic.py index 46cf6cd..03ca8d6 100644 --- a/tests/test_basic.py +++ b/tests/test_basic.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import contextlib import io import json @@ -7,7 +9,6 @@ import time from collections import defaultdict from contextlib import contextmanager -from typing import Dict, List import numpy as np import pytest @@ -16,6 +17,10 @@ from nano_fmm import LineSegment, Network, rapidjson from nano_fmm import flatbush as fb +__PWD = os.path.abspath(os.path.dirname(__file__)) +__BUILD = os.path.abspath(f"{__PWD}/../build") +os.makedirs(__BUILD, exist_ok=True) + def test_add(): assert fmm.add(1, 2) == 3 @@ -23,35 +28,37 @@ def test_add(): def test_segment(): seg = LineSegment([0, 0, 0], [10, 0, 0]) - assert 4.0 == seg.distance([5.0, 4.0, 0.0]) - assert 5.0 == seg.distance([-4.0, 3.0, 0.0]) - assert 5.0 == seg.distance([14.0, 3.0, 0.0]) - assert 25.0 == seg.distance2([14.0, 3.0, 0.0]) + assert seg.distance([5.0, 4.0, 0.0]) == 4.0 + assert seg.distance([-4.0, 3.0, 0.0]) == 5.0 + assert seg.distance([14.0, 3.0, 0.0]) == 5.0 + assert seg.distance2([14.0, 3.0, 0.0]) == 25.0 assert seg.length == 10.0 assert seg.length2 == 100.0 - assert np.all(seg.A == [0, 0, 0]) - assert np.all(seg.B == [10, 0, 0]) - assert np.all(seg.AB == [10, 0, 0]) + assert np.all([0, 0, 0] == seg.A) + assert np.all([10, 0, 0] == seg.B) + assert np.all([10, 0, 0] == seg.AB) assert np.all(seg.dir == [1, 0, 0]) assert np.all(seg.interpolate(0.4) == [4, 0, 0]) assert seg.t([4, 0, 0]) == 0.4 PP, dist, t = seg.nearest([4, 1, 0]) - assert np.all(PP == [4, 0, 0]) + assert np.all([4, 0, 0] == PP) assert dist == 1.0 assert t == 0.4 seg = LineSegment([0, 0, 0], [0, 0, 0]) - assert 5.0 == seg.distance([3.0, 4.0, 0.0]) - assert 5.0 == seg.distance([-4.0, 3.0, 0.0]) - assert 13.0 == seg.distance([5.0, 12.0, 0.0]) + assert seg.distance([3.0, 4.0, 0.0]) == 5.0 + assert seg.distance([-4.0, 3.0, 0.0]) == 5.0 + assert seg.distance([5.0, 12.0, 0.0]) == 13.0 seg = LineSegment([0, 0, 0], [0, 0, 0]) assert seg.length == 0.0 assert seg.length2 == 0.0 assert seg.distance([0, 1, 0]) == 1.0 pt, d, t = seg.nearest([1, 0, 0]) - assert np.all(pt == [0, 0, 0]) and d == 1.0 and t == 0.0 + assert np.all(pt == [0, 0, 0]) + assert d == 1.0 + assert t == 0.0 def test_utils(): @@ -81,8 +88,8 @@ def test_polyline(): for i in range(2): seg1 = polyline.segment(i) seg2 = polyline2.segment(i) - assert np.max(np.fabs(seg1.A - seg2.A)) < 1e-9 - assert np.max(np.fabs(seg1.B - seg2.B)) < 1e-9 + assert np.max(np.fabs(seg1.A - seg2.A)) < 1e-2 + assert np.max(np.fabs(seg1.B - seg2.B)) < 1e-2 def test_cheap_ruler_k(): @@ -174,7 +181,7 @@ def test_cpp_migrated_2(): tree = fb.PackedRTree(nodes, extent) data = tree.to_bytes() assert len(data) == 120 - assert type(data) == bytes + assert isinstance(data, bytes) hits = tree.search(0, 0, 1, 1) assert len(hits) == 1 @@ -246,22 +253,26 @@ def test_polyline_nearest_slice(): pt, dist, seg_idx, t = polyline.nearest([1.5, 0, 0]) assert np.all(pt == [1.5, 0, 0]) assert dist == 0.0 - assert seg_idx == 0 and t == 0.5 + assert seg_idx == 0 + assert t == 0.5 pt, dist, seg_idx, t = polyline.nearest([1.5, 3, 0]) assert np.all(pt == [1.5, 0, 0]) assert dist == 3.0 - assert seg_idx == 0 and t == 0.5 + assert seg_idx == 0 + assert t == 0.5 pt, dist, seg_idx, t = polyline.nearest([1.5, 3, 0], seg_min=1) assert np.all(pt == [3, 0, 0]) assert np.fabs(dist - np.linalg.norm(pt - [1.5, 3, 0])) < 1e-9 - assert seg_idx == 1 and t == 0.0 + assert seg_idx == 1 + assert t == 0.0 pt, dist, seg_idx, t = polyline.nearest([5, 0, 0], seg_max=0) assert np.all(pt == [3, 0, 0]) assert dist == 2.0 - assert seg_idx == 0 and t == 1.0 + assert seg_idx == 0 + assert t == 1.0 assert np.all(polyline.slice(min=15) == [enus[-1], enus[-1]]) assert len(polyline.slice(min=16)) == 0 @@ -271,19 +282,21 @@ def test_polyline_nearest_slice(): polyline = fmm.Polyline(llas[:-1], is_wgs84=True) pt, dist, seg_idx, t = polyline.nearest(llas[-1]) assert np.fabs(pt - (llas[0] + llas[1]) / 2.0).max() < 1e-18 - assert np.fabs(dist - 3.0) < 1e-9 - assert seg_idx == 0 and t == 0.5 + assert np.fabs(dist - 3.0) < 1e-3 + assert seg_idx == 0 + assert abs(t - 0.5) < 1e-3 pt, dist, seg_idx, t = polyline.nearest(llas[-1], seg_min=1) assert np.all(pt == llas[1]) assert dist - assert seg_idx == 1 and t == 0.0 + assert seg_idx == 1 + assert t == 0.0 def build_network( *, - nodes: Dict[str, np.ndarray], - ways: Dict[str, List[str]], + nodes: dict[str, np.ndarray], + ways: dict[str, list[str]], is_wgs84: bool = False, ): node2nexts = defaultdict(list) @@ -314,7 +327,7 @@ def build_network( } -def two_way_streets(ways: Dict[str, List[str]]): +def two_way_streets(ways: dict[str, list[str]]): return { **ways, **({w[::-1]: nn[::-1] for w, nn in ways.items()}), @@ -447,9 +460,10 @@ def test_logging(): assert output_string == "This will be captured\n" buffer = io.StringIO() - with contextlib.redirect_stdout(buffer), contextlib.redirect_stderr(buffer): - with fmm.utils.ostream_redirect(stdout=True, stderr=True): - fmm.utils.logging("hello five") + with contextlib.redirect_stdout(buffer), contextlib.redirect_stderr( + buffer + ), fmm.utils.ostream_redirect(stdout=True, stderr=True): + fmm.utils.logging("hello five") output_string = buffer.getvalue() assert output_string == "std::cout: hello five\nstd::cerr: hello five\n" @@ -498,7 +512,7 @@ def test_json(): def test_project_point_rapidjson(): pt = fmm.ProjectedPoint() - with pytest.raises(Exception) as excinfo: + with pytest.raises(Exception) as excinfo: # noqa: PT011 pt.position[0] = 5 assert "read-only" in str(excinfo.value) j = pt.to_rapidjson() @@ -528,10 +542,10 @@ def test_ubodt_rapidjson(): def test_indexer(): indexer = fmm.Indexer() - assert "5" == indexer.id(5) - assert "10" == indexer.id(10) - assert "1000" == indexer.id(1000) - assert 1000 == indexer.id("1000") + assert indexer.id(5) == "5" + assert indexer.id(10) == "10" + assert indexer.id(1000) == "1000" + assert indexer.id("1000") == 1000 assert indexer.to_rapidjson()() == { "10": 10, "1000": 1000, @@ -539,14 +553,14 @@ def test_indexer(): } indexer = fmm.Indexer() - assert 1000000 == indexer.id("road1") - assert 1000001 == indexer.id("road2") - assert 1000002 == indexer.id("road3") - assert 1000001 == indexer.id("road2") - assert 13579 == indexer.id("13579") - assert "road3" == indexer.id(1000002) - assert 1000003 == indexer.id("1000002") - assert 1000005 == indexer.id("1000005") + assert indexer.id("road1") == 1000000 + assert indexer.id("road2") == 1000001 + assert indexer.id("road3") == 1000002 + assert indexer.id("road2") == 1000001 + assert indexer.id("13579") == 13579 + assert indexer.id(1000002) == "road3" + assert indexer.id("1000002") == 1000003 + assert indexer.id("1000005") == 1000005 assert indexer.to_rapidjson()() == { "1000002": 1000003, "1000005": 1000005, @@ -557,14 +571,14 @@ def test_indexer(): } indexer2 = fmm.Indexer().from_rapidjson(indexer.to_rapidjson()) - assert 1000000 == indexer2.id("road1") - assert 1000001 == indexer2.id("road2") - assert 1000002 == indexer2.id("road3") - assert 1000001 == indexer2.id("road2") - assert 13579 == indexer2.id("13579") - assert "road3" == indexer2.id(1000002) - assert 1000003 == indexer2.id("1000002") - assert 1000005 == indexer2.id("1000005") + assert indexer2.id("road1") == 1000000 + assert indexer2.id("road2") == 1000001 + assert indexer2.id("road3") == 1000002 + assert indexer2.id("road2") == 1000001 + assert indexer2.id("13579") == 13579 + assert indexer2.id(1000002) == "road3" + assert indexer2.id("1000002") == 1000003 + assert indexer2.id("1000005") == 1000005 assert indexer2.to_rapidjson() == indexer.to_rapidjson() indexer2.id("add another road") assert indexer2.to_rapidjson() != indexer.to_rapidjson() @@ -576,27 +590,27 @@ def test_indexer(): def test_network_read_write(): - network = Network.load("README.md") + network = Network.load(f"{__PWD}/README.md") assert network is None network = Network.load("missing_file") assert network is None from nano_fmm.converter import remap_network_with_string_id - geojson, _ = remap_network_with_string_id("data/suzhoubeizhan.json") + geojson, _ = remap_network_with_string_id(f"{__PWD}/../data/suzhoubeizhan.json") network = Network(is_wgs84=True) network.from_geojson(geojson) assert len(network.roads()) == 1016 - assert network.to_geojson().dump("build/network.geojson", indent=True) - assert network.to_rapidjson().dump("build/network.json", indent=True) + assert network.to_geojson().dump(f"{__BUILD}/network.geojson", indent=True) + assert network.to_rapidjson().dump(f"{__BUILD}/network.json", indent=True) - network = Network.load("build/network.geojson") - network.to_geojson().dump("build/network2.geojson", indent=True) - network.to_rapidjson().dump("build/network2.json", indent=True) + network = Network.load(f"{__BUILD}/network.geojson") + network.to_geojson().dump(f"{__BUILD}/network2.geojson", indent=True) + network.to_rapidjson().dump(f"{__BUILD}/network2.json", indent=True) - network = Network.load("build/network.json") - network.to_geojson().dump("build/network3.geojson", indent=True) - network.to_rapidjson().dump("build/network3.json", indent=True) + network = Network.load(f"{__BUILD}/network.json") + network.to_geojson().dump(f"{__BUILD}/network3.geojson", indent=True) + network.to_rapidjson().dump(f"{__BUILD}/network3.json", indent=True) rows = network.build_ubodt() rows = sorted(rows) @@ -604,7 +618,10 @@ def test_network_read_write(): def test_network_query(): - network = Network.load("build/network.geojson") + path = f"{__BUILD}/network.geojson" + if not os.path.isfile(path): # noqa: PTH113 + test_network_read_write() + network = Network.load(path) assert network.is_wgs84() assert len(network.roads()) == 1016 assert network.next_roads(1293) == {1297, 1298} @@ -619,7 +636,8 @@ def test_network_query(): seed_lla = [120.663031, 31.40531, 0] lla, dist, seg_idx, t = polyline.nearest(seed_lla) assert round(dist, 2) == 105.71 - assert seg_idx == 4 and t == 1.0 + assert seg_idx == 4 + assert t == 1.0 # 0---1---2---3---4---5 # ^ # t=1.0 @@ -655,26 +673,48 @@ def test_network_query_enu(): hits = network.query([5, 0, 0], radius=5) hits = [h.to_rapidjson()() for h in hits] - assert hits == [ - { - "position": [5.0, 0.0, 0.0], - "direction": [1.0, 0.0, 0.0], - "distance": 0.0, - "road_id": 1, - "offset": 5.0, - }, - { - "position": [5.0, 5.0, 0.0], - "direction": [1.0, 0.0, 0.0], - "distance": 5.0, - "road_id": 0, - "offset": 5.0, - }, - { - "position": [10.0, 0.0, 0.0], - "direction": [0.0, 1.0, 0.0], - "distance": 5.0, - "road_id": 2, - "offset": 5.0, - }, - ] + hit1 = { + "position": [5.0, 5.0, 0.0], + "direction": [1.0, 0.0, 0.0], + "distance": 5.0, + "road_id": 0, + "offset": 5.0, + } + hit2 = { + "position": [10.0, 0.0, 0.0], + "direction": [0.0, 1.0, 0.0], + "distance": 5.0, + "road_id": 2, + "offset": 5.0, + } + assert hits[0] == { + "position": [5.0, 0.0, 0.0], + "direction": [1.0, 0.0, 0.0], + "distance": 0.0, + "road_id": 1, + "offset": 5.0, + } + assert hits[1:] == [hit1, hit2] or hits[1:] == [hit2, hit1] + + +def pytest_main(dir: str, *, test_file: str): + # pytest test_cli.py + # pytest --capture=tee-sys test_cli.py + os.chdir(dir) + # https://docs.pytest.org/en/6.2.x/usage.html#calling-pytest-from-python-code + sys.exit( + pytest.main( + [ + dir, + *(["-k", test_file] if test_file else []), + "--capture", + "tee-sys", + "-vv", + "-x", + ] + ) + ) + + +if __name__ == "__main__": + pytest_main(__PWD, test_file=os.path.basename(__file__)) # noqa: PTH119