From c5db23f296ef8c481f1fc7441761ccc1924f6980 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 13 Aug 2022 18:32:02 -0700 Subject: [PATCH] [wpimath] Add Eigen sparse matrix and iterative solver support (#4349) These are useful for efficiently solving huge, but sparse systems of equations that occur often in optimization problems. --- upstream_utils/update_eigen.py | 6 + .../include/Eigen/IterativeLinearSolvers | 48 + .../eigen/include/Eigen/OrderingMethods | 70 + .../eigen/include/Eigen/SparseCholesky | 37 + .../thirdparty/eigen/include/Eigen/SparseCore | 69 + .../thirdparty/eigen/include/Eigen/SparseLU | 50 + .../thirdparty/eigen/include/Eigen/SparseQR | 36 + .../BasicPreconditioners.h | 226 ++ .../src/IterativeLinearSolvers/BiCGSTAB.h | 212 ++ .../ConjugateGradient.h | 229 ++ .../IncompleteCholesky.h | 394 ++++ .../IterativeLinearSolvers/IncompleteLUT.h | 453 ++++ .../IterativeSolverBase.h | 444 ++++ .../LeastSquareConjugateGradient.h | 198 ++ .../IterativeLinearSolvers/SolveWithGuess.h | 117 ++ .../include/Eigen/src/OrderingMethods/Amd.h | 435 ++++ .../Eigen/src/OrderingMethods/Eigen_Colamd.h | 1863 +++++++++++++++++ .../Eigen/src/OrderingMethods/Ordering.h | 153 ++ .../src/SparseCholesky/SimplicialCholesky.h | 697 ++++++ .../SparseCholesky/SimplicialCholesky_impl.h | 174 ++ .../include/Eigen/src/SparseCore/AmbiVector.h | 378 ++++ .../Eigen/src/SparseCore/CompressedStorage.h | 274 +++ .../ConservativeSparseSparseProduct.h | 352 ++++ .../Eigen/src/SparseCore/MappedSparseMatrix.h | 67 + .../Eigen/src/SparseCore/SparseAssign.h | 270 +++ .../Eigen/src/SparseCore/SparseBlock.h | 571 +++++ .../Eigen/src/SparseCore/SparseColEtree.h | 206 ++ .../src/SparseCore/SparseCompressedBase.h | 370 ++++ .../src/SparseCore/SparseCwiseBinaryOp.h | 722 +++++++ .../Eigen/src/SparseCore/SparseCwiseUnaryOp.h | 150 ++ .../Eigen/src/SparseCore/SparseDenseProduct.h | 342 +++ .../src/SparseCore/SparseDiagonalProduct.h | 138 ++ .../include/Eigen/src/SparseCore/SparseDot.h | 98 + .../Eigen/src/SparseCore/SparseFuzzy.h | 29 + .../include/Eigen/src/SparseCore/SparseMap.h | 305 +++ .../Eigen/src/SparseCore/SparseMatrix.h | 1518 ++++++++++++++ .../Eigen/src/SparseCore/SparseMatrixBase.h | 398 ++++ .../Eigen/src/SparseCore/SparsePermutation.h | 178 ++ .../Eigen/src/SparseCore/SparseProduct.h | 181 ++ .../Eigen/src/SparseCore/SparseRedux.h | 49 + .../include/Eigen/src/SparseCore/SparseRef.h | 397 ++++ .../src/SparseCore/SparseSelfAdjointView.h | 659 ++++++ .../Eigen/src/SparseCore/SparseSolverBase.h | 124 ++ .../SparseSparseProductWithPruning.h | 198 ++ .../Eigen/src/SparseCore/SparseTranspose.h | 92 + .../src/SparseCore/SparseTriangularView.h | 189 ++ .../include/Eigen/src/SparseCore/SparseUtil.h | 186 ++ .../Eigen/src/SparseCore/SparseVector.h | 478 +++++ .../include/Eigen/src/SparseCore/SparseView.h | 254 +++ .../Eigen/src/SparseCore/TriangularSolver.h | 315 +++ .../include/Eigen/src/SparseLU/SparseLU.h | 923 ++++++++ .../include/Eigen/src/SparseLU/SparseLUImpl.h | 66 + .../Eigen/src/SparseLU/SparseLU_Memory.h | 226 ++ .../Eigen/src/SparseLU/SparseLU_Structs.h | 110 + .../src/SparseLU/SparseLU_SupernodalMatrix.h | 375 ++++ .../Eigen/src/SparseLU/SparseLU_Utils.h | 80 + .../Eigen/src/SparseLU/SparseLU_column_bmod.h | 181 ++ .../Eigen/src/SparseLU/SparseLU_column_dfs.h | 179 ++ .../src/SparseLU/SparseLU_copy_to_ucol.h | 107 + .../Eigen/src/SparseLU/SparseLU_gemm_kernel.h | 280 +++ .../src/SparseLU/SparseLU_heap_relax_snode.h | 126 ++ .../Eigen/src/SparseLU/SparseLU_kernel_bmod.h | 130 ++ .../Eigen/src/SparseLU/SparseLU_panel_bmod.h | 223 ++ .../Eigen/src/SparseLU/SparseLU_panel_dfs.h | 258 +++ .../Eigen/src/SparseLU/SparseLU_pivotL.h | 137 ++ .../Eigen/src/SparseLU/SparseLU_pruneL.h | 136 ++ .../Eigen/src/SparseLU/SparseLU_relax_snode.h | 83 + .../include/Eigen/src/SparseQR/SparseQR.h | 758 +++++++ 68 files changed, 19777 insertions(+) create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h create mode 100644 wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h diff --git a/upstream_utils/update_eigen.py b/upstream_utils/update_eigen.py index 00e958d91e..5bfcf91113 100755 --- a/upstream_utils/update_eigen.py +++ b/upstream_utils/update_eigen.py @@ -56,10 +56,16 @@ def eigen_inclusions(dp, f): "Core", "Eigenvalues", "Householder", + "IterativeLinearSolvers", "Jacobi", "LU", + "OrderingMethods", "QR", "SVD", + "SparseCholesky", + "SparseCore", + "SparseLU", + "SparseQR", "StlSupport", "misc", "plugins", diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers new file mode 100644 index 0000000000..957d5750b2 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers @@ -0,0 +1,48 @@ +// 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_ITERATIVELINEARSOLVERS_MODULE_H +#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H + +#include "SparseCore" +#include "OrderingMethods" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** + * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module + * + * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. + * Those solvers are accessible via the following classes: + * - ConjugateGradient for selfadjoint (hermitian) matrices, + * - LeastSquaresConjugateGradient for rectangular least-square problems, + * - BiCGSTAB for general square matrices. + * + * These iterative solvers are associated with some preconditioners: + * - IdentityPreconditioner - not really useful + * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. + * - IncompleteLUT - incomplete LU factorization with dual thresholding + * + * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. + * + \code + #include + \endcode + */ + +#include "src/IterativeLinearSolvers/SolveWithGuess.h" +#include "src/IterativeLinearSolvers/IterativeSolverBase.h" +#include "src/IterativeLinearSolvers/BasicPreconditioners.h" +#include "src/IterativeLinearSolvers/ConjugateGradient.h" +#include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" +#include "src/IterativeLinearSolvers/BiCGSTAB.h" +#include "src/IterativeLinearSolvers/IncompleteLUT.h" +#include "src/IterativeLinearSolvers/IncompleteCholesky.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods new file mode 100644 index 0000000000..29691a62b4 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods @@ -0,0 +1,70 @@ +// 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_ORDERINGMETHODS_MODULE_H +#define EIGEN_ORDERINGMETHODS_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** + * \defgroup OrderingMethods_Module OrderingMethods module + * + * This module is currently for internal use only + * + * It defines various built-in and external ordering methods for sparse matrices. + * They are typically used to reduce the number of elements during + * the sparse matrix decomposition (LLT, LU, QR). + * Precisely, in a preprocessing step, a permutation matrix P is computed using + * those ordering methods and applied to the columns of the matrix. + * Using for instance the sparse Cholesky decomposition, it is expected that + * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A). + * + * + * Usage : + * \code + * #include + * \endcode + * + * A simple usage is as a template parameter in the sparse decomposition classes : + * + * \code + * SparseLU > solver; + * \endcode + * + * \code + * SparseQR > solver; + * \endcode + * + * It is possible as well to call directly a particular ordering method for your own purpose, + * \code + * AMDOrdering ordering; + * PermutationMatrix perm; + * SparseMatrix A; + * //Fill the matrix ... + * + * ordering(A, perm); // Call AMD + * \endcode + * + * \note Some of these methods (like AMD or METIS), need the sparsity pattern + * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, + * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method. + * If your matrix is already symmetric (at leat in structure), you can avoid that + * by calling the method with a SelfAdjointView type. + * + * \code + * // Call the ordering on the pattern of the lower triangular matrix A + * ordering(A.selfadjointView(), perm); + * \endcode + */ + +#include "src/OrderingMethods/Amd.h" +#include "src/OrderingMethods/Ordering.h" +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_ORDERINGMETHODS_MODULE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky new file mode 100644 index 0000000000..d2b1f1276d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky @@ -0,0 +1,37 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2013 Gael Guennebaud +// +// 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_SPARSECHOLESKY_MODULE_H +#define EIGEN_SPARSECHOLESKY_MODULE_H + +#include "SparseCore" +#include "OrderingMethods" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** + * \defgroup SparseCholesky_Module SparseCholesky module + * + * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices. + * Those decompositions are accessible via the following classes: + * - SimplicialLLt, + * - SimplicialLDLt + * + * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module. + * + * \code + * #include + * \endcode + */ + +#include "src/SparseCholesky/SimplicialCholesky.h" +#include "src/SparseCholesky/SimplicialCholesky_impl.h" +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSECHOLESKY_MODULE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore new file mode 100644 index 0000000000..76966c4c4c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore @@ -0,0 +1,69 @@ +// 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_SPARSECORE_MODULE_H +#define EIGEN_SPARSECORE_MODULE_H + +#include "Core" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include +#include +#include +#include +#include + +/** + * \defgroup SparseCore_Module SparseCore module + * + * This module provides a sparse matrix representation, and basic associated matrix manipulations + * and operations. + * + * See the \ref TutorialSparse "Sparse tutorial" + * + * \code + * #include + * \endcode + * + * This module depends on: Core. + */ + +#include "src/SparseCore/SparseUtil.h" +#include "src/SparseCore/SparseMatrixBase.h" +#include "src/SparseCore/SparseAssign.h" +#include "src/SparseCore/CompressedStorage.h" +#include "src/SparseCore/AmbiVector.h" +#include "src/SparseCore/SparseCompressedBase.h" +#include "src/SparseCore/SparseMatrix.h" +#include "src/SparseCore/SparseMap.h" +#include "src/SparseCore/MappedSparseMatrix.h" +#include "src/SparseCore/SparseVector.h" +#include "src/SparseCore/SparseRef.h" +#include "src/SparseCore/SparseCwiseUnaryOp.h" +#include "src/SparseCore/SparseCwiseBinaryOp.h" +#include "src/SparseCore/SparseTranspose.h" +#include "src/SparseCore/SparseBlock.h" +#include "src/SparseCore/SparseDot.h" +#include "src/SparseCore/SparseRedux.h" +#include "src/SparseCore/SparseView.h" +#include "src/SparseCore/SparseDiagonalProduct.h" +#include "src/SparseCore/ConservativeSparseSparseProduct.h" +#include "src/SparseCore/SparseSparseProductWithPruning.h" +#include "src/SparseCore/SparseProduct.h" +#include "src/SparseCore/SparseDenseProduct.h" +#include "src/SparseCore/SparseSelfAdjointView.h" +#include "src/SparseCore/SparseTriangularView.h" +#include "src/SparseCore/TriangularSolver.h" +#include "src/SparseCore/SparsePermutation.h" +#include "src/SparseCore/SparseFuzzy.h" +#include "src/SparseCore/SparseSolverBase.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSECORE_MODULE_H + diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU new file mode 100644 index 0000000000..37c4a5c5a8 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU @@ -0,0 +1,50 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2012 Gael Guennebaud +// +// 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_SPARSELU_MODULE_H +#define EIGEN_SPARSELU_MODULE_H + +#include "SparseCore" + +/** + * \defgroup SparseLU_Module SparseLU module + * This module defines a supernodal factorization of general sparse matrices. + * The code is fully optimized for supernode-panel updates with specialized kernels. + * Please, see the documentation of the SparseLU class for more details. + */ + +// Ordering interface +#include "OrderingMethods" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include "src/SparseLU/SparseLU_gemm_kernel.h" + +#include "src/SparseLU/SparseLU_Structs.h" +#include "src/SparseLU/SparseLU_SupernodalMatrix.h" +#include "src/SparseLU/SparseLUImpl.h" +#include "src/SparseCore/SparseColEtree.h" +#include "src/SparseLU/SparseLU_Memory.h" +#include "src/SparseLU/SparseLU_heap_relax_snode.h" +#include "src/SparseLU/SparseLU_relax_snode.h" +#include "src/SparseLU/SparseLU_pivotL.h" +#include "src/SparseLU/SparseLU_panel_dfs.h" +#include "src/SparseLU/SparseLU_kernel_bmod.h" +#include "src/SparseLU/SparseLU_panel_bmod.h" +#include "src/SparseLU/SparseLU_column_dfs.h" +#include "src/SparseLU/SparseLU_column_bmod.h" +#include "src/SparseLU/SparseLU_copy_to_ucol.h" +#include "src/SparseLU/SparseLU_pruneL.h" +#include "src/SparseLU/SparseLU_Utils.h" +#include "src/SparseLU/SparseLU.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSELU_MODULE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR new file mode 100644 index 0000000000..f5fc5fa7fe --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR @@ -0,0 +1,36 @@ +// 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_SPARSEQR_MODULE_H +#define EIGEN_SPARSEQR_MODULE_H + +#include "SparseCore" +#include "OrderingMethods" +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup SparseQR_Module SparseQR module + * \brief Provides QR decomposition for sparse matrices + * + * This module provides a simplicial version of the left-looking Sparse QR decomposition. + * The columns of the input matrix should be reordered to limit the fill-in during the + * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. + * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list + * of built-in and external ordering methods. + * + * \code + * #include + * \endcode + * + * + */ + +#include "src/SparseCore/SparseColEtree.h" +#include "src/SparseQR/SparseQR.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h new file mode 100644 index 0000000000..a117fc1551 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h @@ -0,0 +1,226 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011-2014 Gael Guennebaud +// +// 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_BASIC_PRECONDITIONERS_H +#define EIGEN_BASIC_PRECONDITIONERS_H + +namespace Eigen { + +/** \ingroup IterativeLinearSolvers_Module + * \brief A preconditioner based on the digonal entries + * + * This class allows to approximately solve for A.x = b problems assuming A is a diagonal matrix. + * In other words, this preconditioner neglects all off diagonal entries and, in Eigen's language, solves for: + \code + A.diagonal().asDiagonal() . x = b + \endcode + * + * \tparam _Scalar the type of the scalar. + * + * \implsparsesolverconcept + * + * This preconditioner is suitable for both selfadjoint and general problems. + * The diagonal entries are pre-inverted and stored into a dense vector. + * + * \note A variant that has yet to be implemented would attempt to preserve the norm of each column. + * + * \sa class LeastSquareDiagonalPreconditioner, class ConjugateGradient + */ +template +class DiagonalPreconditioner +{ + typedef _Scalar Scalar; + typedef Matrix Vector; + public: + typedef typename Vector::StorageIndex StorageIndex; + enum { + ColsAtCompileTime = Dynamic, + MaxColsAtCompileTime = Dynamic + }; + + DiagonalPreconditioner() : m_isInitialized(false) {} + + template + explicit DiagonalPreconditioner(const MatType& mat) : m_invdiag(mat.cols()) + { + compute(mat); + } + + EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_invdiag.size(); } + EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_invdiag.size(); } + + template + DiagonalPreconditioner& analyzePattern(const MatType& ) + { + return *this; + } + + template + DiagonalPreconditioner& factorize(const MatType& mat) + { + m_invdiag.resize(mat.cols()); + for(int j=0; j + DiagonalPreconditioner& compute(const MatType& mat) + { + return factorize(mat); + } + + /** \internal */ + template + void _solve_impl(const Rhs& b, Dest& x) const + { + x = m_invdiag.array() * b.array() ; + } + + template inline const Solve + solve(const MatrixBase& b) const + { + eigen_assert(m_isInitialized && "DiagonalPreconditioner is not initialized."); + eigen_assert(m_invdiag.size()==b.rows() + && "DiagonalPreconditioner::solve(): invalid number of rows of the right hand side matrix b"); + return Solve(*this, b.derived()); + } + + ComputationInfo info() { return Success; } + + protected: + Vector m_invdiag; + bool m_isInitialized; +}; + +/** \ingroup IterativeLinearSolvers_Module + * \brief Jacobi preconditioner for LeastSquaresConjugateGradient + * + * This class allows to approximately solve for A' A x = A' b problems assuming A' A is a diagonal matrix. + * In other words, this preconditioner neglects all off diagonal entries and, in Eigen's language, solves for: + \code + (A.adjoint() * A).diagonal().asDiagonal() * x = b + \endcode + * + * \tparam _Scalar the type of the scalar. + * + * \implsparsesolverconcept + * + * The diagonal entries are pre-inverted and stored into a dense vector. + * + * \sa class LeastSquaresConjugateGradient, class DiagonalPreconditioner + */ +template +class LeastSquareDiagonalPreconditioner : public DiagonalPreconditioner<_Scalar> +{ + typedef _Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef DiagonalPreconditioner<_Scalar> Base; + using Base::m_invdiag; + public: + + LeastSquareDiagonalPreconditioner() : Base() {} + + template + explicit LeastSquareDiagonalPreconditioner(const MatType& mat) : Base() + { + compute(mat); + } + + template + LeastSquareDiagonalPreconditioner& analyzePattern(const MatType& ) + { + return *this; + } + + template + LeastSquareDiagonalPreconditioner& factorize(const MatType& mat) + { + // Compute the inverse squared-norm of each column of mat + m_invdiag.resize(mat.cols()); + if(MatType::IsRowMajor) + { + m_invdiag.setZero(); + for(Index j=0; jRealScalar(0)) + m_invdiag(j) = RealScalar(1)/numext::real(m_invdiag(j)); + } + else + { + for(Index j=0; jRealScalar(0)) + m_invdiag(j) = RealScalar(1)/sum; + else + m_invdiag(j) = RealScalar(1); + } + } + Base::m_isInitialized = true; + return *this; + } + + template + LeastSquareDiagonalPreconditioner& compute(const MatType& mat) + { + return factorize(mat); + } + + ComputationInfo info() { return Success; } + + protected: +}; + +/** \ingroup IterativeLinearSolvers_Module + * \brief A naive preconditioner which approximates any matrix as the identity matrix + * + * \implsparsesolverconcept + * + * \sa class DiagonalPreconditioner + */ +class IdentityPreconditioner +{ + public: + + IdentityPreconditioner() {} + + template + explicit IdentityPreconditioner(const MatrixType& ) {} + + template + IdentityPreconditioner& analyzePattern(const MatrixType& ) { return *this; } + + template + IdentityPreconditioner& factorize(const MatrixType& ) { return *this; } + + template + IdentityPreconditioner& compute(const MatrixType& ) { return *this; } + + template + inline const Rhs& solve(const Rhs& b) const { return b; } + + ComputationInfo info() { return Success; } +}; + +} // end namespace Eigen + +#endif // EIGEN_BASIC_PRECONDITIONERS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h new file mode 100644 index 0000000000..153acef65b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h @@ -0,0 +1,212 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011-2014 Gael Guennebaud +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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_BICGSTAB_H +#define EIGEN_BICGSTAB_H + +namespace Eigen { + +namespace internal { + +/** \internal Low-level bi conjugate gradient stabilized algorithm + * \param mat The matrix A + * \param rhs The right hand side vector b + * \param x On input and initial solution, on output the computed solution. + * \param precond A preconditioner being able to efficiently solve for an + * approximation of Ax=b (regardless of b) + * \param iters On input the max number of iteration, on output the number of performed iterations. + * \param tol_error On input the tolerance error, on output an estimation of the relative error. + * \return false in the case of numerical issue, for example a break down of BiCGSTAB. + */ +template +bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, + const Preconditioner& precond, Index& iters, + typename Dest::RealScalar& tol_error) +{ + using std::sqrt; + using std::abs; + typedef typename Dest::RealScalar RealScalar; + typedef typename Dest::Scalar Scalar; + typedef Matrix VectorType; + RealScalar tol = tol_error; + Index maxIters = iters; + + Index n = mat.cols(); + VectorType r = rhs - mat * x; + VectorType r0 = r; + + RealScalar r0_sqnorm = r0.squaredNorm(); + RealScalar rhs_sqnorm = rhs.squaredNorm(); + if(rhs_sqnorm == 0) + { + x.setZero(); + return true; + } + Scalar rho = 1; + Scalar alpha = 1; + Scalar w = 1; + + VectorType v = VectorType::Zero(n), p = VectorType::Zero(n); + VectorType y(n), z(n); + VectorType kt(n), ks(n); + + VectorType s(n), t(n); + + RealScalar tol2 = tol*tol*rhs_sqnorm; + RealScalar eps2 = NumTraits::epsilon()*NumTraits::epsilon(); + Index i = 0; + Index restarts = 0; + + while ( r.squaredNorm() > tol2 && iRealScalar(0)) + w = t.dot(s) / tmp; + else + w = Scalar(0); + x += alpha * y + w * z; + r = s - w * t; + ++i; + } + tol_error = sqrt(r.squaredNorm()/rhs_sqnorm); + iters = i; + return true; +} + +} + +template< typename _MatrixType, + typename _Preconditioner = DiagonalPreconditioner > +class BiCGSTAB; + +namespace internal { + +template< typename _MatrixType, typename _Preconditioner> +struct traits > +{ + typedef _MatrixType MatrixType; + typedef _Preconditioner Preconditioner; +}; + +} + +/** \ingroup IterativeLinearSolvers_Module + * \brief A bi conjugate gradient stabilized solver for sparse square problems + * + * This class allows to solve for A.x = b sparse linear problems using a bi conjugate gradient + * stabilized algorithm. The vectors x and b can be either dense or sparse. + * + * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix. + * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner + * + * \implsparsesolverconcept + * + * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations() + * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations + * and NumTraits::epsilon() for the tolerance. + * + * The tolerance corresponds to the relative residual error: |Ax-b|/|b| + * + * \b Performance: when using sparse matrices, best performance is achied for a row-major sparse matrix format. + * Moreover, in this case multi-threading can be exploited if the user code is compiled with OpenMP enabled. + * See \ref TopicMultiThreading for details. + * + * This class can be used as the direct solver classes. Here is a typical usage example: + * \include BiCGSTAB_simple.cpp + * + * By default the iterations start with x=0 as an initial guess of the solution. + * One can control the start using the solveWithGuess() method. + * + * BiCGSTAB can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink. + * + * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner + */ +template< typename _MatrixType, typename _Preconditioner> +class BiCGSTAB : public IterativeSolverBase > +{ + typedef IterativeSolverBase Base; + using Base::matrix; + using Base::m_error; + using Base::m_iterations; + using Base::m_info; + using Base::m_isInitialized; +public: + typedef _MatrixType MatrixType; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef _Preconditioner Preconditioner; + +public: + + /** Default constructor. */ + BiCGSTAB() : Base() {} + + /** Initialize the solver with matrix \a A for further \c Ax=b solving. + * + * This constructor is a shortcut for the default constructor followed + * by a call to compute(). + * + * \warning this class stores a reference to the matrix A as well as some + * precomputed values that depend on it. Therefore, if \a A is changed + * this class becomes invalid. Call compute() to update it with the new + * matrix A, or modify a copy of A. + */ + template + explicit BiCGSTAB(const EigenBase& A) : Base(A.derived()) {} + + ~BiCGSTAB() {} + + /** \internal */ + template + void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const + { + m_iterations = Base::maxIterations(); + m_error = Base::m_tolerance; + + bool ret = internal::bicgstab(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error); + + m_info = (!ret) ? NumericalIssue + : m_error <= Base::m_tolerance ? Success + : NoConvergence; + } + +protected: + +}; + +} // end namespace Eigen + +#endif // EIGEN_BICGSTAB_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h new file mode 100644 index 0000000000..5d8c6b4339 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h @@ -0,0 +1,229 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011-2014 Gael Guennebaud +// +// 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_CONJUGATE_GRADIENT_H +#define EIGEN_CONJUGATE_GRADIENT_H + +namespace Eigen { + +namespace internal { + +/** \internal Low-level conjugate gradient algorithm + * \param mat The matrix A + * \param rhs The right hand side vector b + * \param x On input and initial solution, on output the computed solution. + * \param precond A preconditioner being able to efficiently solve for an + * approximation of Ax=b (regardless of b) + * \param iters On input the max number of iteration, on output the number of performed iterations. + * \param tol_error On input the tolerance error, on output an estimation of the relative error. + */ +template +EIGEN_DONT_INLINE +void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x, + const Preconditioner& precond, Index& iters, + typename Dest::RealScalar& tol_error) +{ + using std::sqrt; + using std::abs; + typedef typename Dest::RealScalar RealScalar; + typedef typename Dest::Scalar Scalar; + typedef Matrix VectorType; + + RealScalar tol = tol_error; + Index maxIters = iters; + + Index n = mat.cols(); + + VectorType residual = rhs - mat * x; //initial residual + + RealScalar rhsNorm2 = rhs.squaredNorm(); + if(rhsNorm2 == 0) + { + x.setZero(); + iters = 0; + tol_error = 0; + return; + } + const RealScalar considerAsZero = (std::numeric_limits::min)(); + RealScalar threshold = numext::maxi(RealScalar(tol*tol*rhsNorm2),considerAsZero); + RealScalar residualNorm2 = residual.squaredNorm(); + if (residualNorm2 < threshold) + { + iters = 0; + tol_error = sqrt(residualNorm2 / rhsNorm2); + return; + } + + VectorType p(n); + p = precond.solve(residual); // initial search direction + + VectorType z(n), tmp(n); + RealScalar absNew = numext::real(residual.dot(p)); // the square of the absolute value of r scaled by invM + Index i = 0; + while(i < maxIters) + { + tmp.noalias() = mat * p; // the bottleneck of the algorithm + + Scalar alpha = absNew / p.dot(tmp); // the amount we travel on dir + x += alpha * p; // update solution + residual -= alpha * tmp; // update residual + + residualNorm2 = residual.squaredNorm(); + if(residualNorm2 < threshold) + break; + + z = precond.solve(residual); // approximately solve for "A z = residual" + + RealScalar absOld = absNew; + absNew = numext::real(residual.dot(z)); // update the absolute value of r + RealScalar beta = absNew / absOld; // calculate the Gram-Schmidt value used to create the new search direction + p = z + beta * p; // update search direction + i++; + } + tol_error = sqrt(residualNorm2 / rhsNorm2); + iters = i; +} + +} + +template< typename _MatrixType, int _UpLo=Lower, + typename _Preconditioner = DiagonalPreconditioner > +class ConjugateGradient; + +namespace internal { + +template< typename _MatrixType, int _UpLo, typename _Preconditioner> +struct traits > +{ + typedef _MatrixType MatrixType; + typedef _Preconditioner Preconditioner; +}; + +} + +/** \ingroup IterativeLinearSolvers_Module + * \brief A conjugate gradient solver for sparse (or dense) self-adjoint problems + * + * This class allows to solve for A.x = b linear problems using an iterative conjugate gradient algorithm. + * The matrix A must be selfadjoint. The matrix A and the vectors x and b can be either dense or sparse. + * + * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix. + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower, + * \c Upper, or \c Lower|Upper in which the full matrix entries will be considered. + * Default is \c Lower, best performance is \c Lower|Upper. + * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner + * + * \implsparsesolverconcept + * + * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations() + * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations + * and NumTraits::epsilon() for the tolerance. + * + * The tolerance corresponds to the relative residual error: |Ax-b|/|b| + * + * \b Performance: Even though the default value of \c _UpLo is \c Lower, significantly higher performance is + * achieved when using a complete matrix and \b Lower|Upper as the \a _UpLo template parameter. Moreover, in this + * case multi-threading can be exploited if the user code is compiled with OpenMP enabled. + * See \ref TopicMultiThreading for details. + * + * This class can be used as the direct solver classes. Here is a typical usage example: + \code + int n = 10000; + VectorXd x(n), b(n); + SparseMatrix A(n,n); + // fill A and b + ConjugateGradient, Lower|Upper> cg; + cg.compute(A); + x = cg.solve(b); + std::cout << "#iterations: " << cg.iterations() << std::endl; + std::cout << "estimated error: " << cg.error() << std::endl; + // update b, and solve again + x = cg.solve(b); + \endcode + * + * By default the iterations start with x=0 as an initial guess of the solution. + * One can control the start using the solveWithGuess() method. + * + * ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink. + * + * \sa class LeastSquaresConjugateGradient, class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner + */ +template< typename _MatrixType, int _UpLo, typename _Preconditioner> +class ConjugateGradient : public IterativeSolverBase > +{ + typedef IterativeSolverBase Base; + using Base::matrix; + using Base::m_error; + using Base::m_iterations; + using Base::m_info; + using Base::m_isInitialized; +public: + typedef _MatrixType MatrixType; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef _Preconditioner Preconditioner; + + enum { + UpLo = _UpLo + }; + +public: + + /** Default constructor. */ + ConjugateGradient() : Base() {} + + /** Initialize the solver with matrix \a A for further \c Ax=b solving. + * + * This constructor is a shortcut for the default constructor followed + * by a call to compute(). + * + * \warning this class stores a reference to the matrix A as well as some + * precomputed values that depend on it. Therefore, if \a A is changed + * this class becomes invalid. Call compute() to update it with the new + * matrix A, or modify a copy of A. + */ + template + explicit ConjugateGradient(const EigenBase& A) : Base(A.derived()) {} + + ~ConjugateGradient() {} + + /** \internal */ + template + void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const + { + typedef typename Base::MatrixWrapper MatrixWrapper; + typedef typename Base::ActualMatrixType ActualMatrixType; + enum { + TransposeInput = (!MatrixWrapper::MatrixFree) + && (UpLo==(Lower|Upper)) + && (!MatrixType::IsRowMajor) + && (!NumTraits::IsComplex) + }; + typedef typename internal::conditional, ActualMatrixType const&>::type RowMajorWrapper; + EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(MatrixWrapper::MatrixFree,UpLo==(Lower|Upper)),MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY); + typedef typename internal::conditional::Type + >::type SelfAdjointWrapper; + + m_iterations = Base::maxIterations(); + m_error = Base::m_tolerance; + + RowMajorWrapper row_mat(matrix()); + internal::conjugate_gradient(SelfAdjointWrapper(row_mat), b, x, Base::m_preconditioner, m_iterations, m_error); + m_info = m_error <= Base::m_tolerance ? Success : NoConvergence; + } + +protected: + +}; + +} // end namespace Eigen + +#endif // EIGEN_CONJUGATE_GRADIENT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h new file mode 100644 index 0000000000..7803fd8170 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h @@ -0,0 +1,394 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2015 Gael Guennebaud +// +// 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_INCOMPLETE_CHOlESKY_H +#define EIGEN_INCOMPLETE_CHOlESKY_H + +#include +#include + +namespace Eigen { +/** + * \brief Modified Incomplete Cholesky with dual threshold + * + * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with + * Limited memory, SIAM J. Sci. Comput. 21(1), pp. 24-45, 1999 + * + * \tparam Scalar the scalar type of the input matrices + * \tparam _UpLo The triangular part that will be used for the computations. It can be Lower + * or Upper. Default is Lower. + * \tparam _OrderingType The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering, + * unless EIGEN_MPL2_ONLY is defined, in which case the default is NaturalOrdering. + * + * \implsparsesolverconcept + * + * It performs the following incomplete factorization: \f$ S P A P' S \approx L L' \f$ + * where L is a lower triangular factor, S is a diagonal scaling matrix, and P is a + * fill-in reducing permutation as computed by the ordering method. + * + * \b Shifting \b strategy: Let \f$ B = S P A P' S \f$ be the scaled matrix on which the factorization is carried out, + * and \f$ \beta \f$ be the minimum value of the diagonal. If \f$ \beta > 0 \f$ then, the factorization is directly performed + * on the matrix B. Otherwise, the factorization is performed on the shifted matrix \f$ B + (\sigma+|\beta| I \f$ where + * \f$ \sigma \f$ is the initial shift value as returned and set by setInitialShift() method. The default value is \f$ \sigma = 10^{-3} \f$. + * If the factorization fails, then the shift in doubled until it succeed or a maximum of ten attempts. If it still fails, as returned by + * the info() method, then you can either increase the initial shift, or better use another preconditioning technique. + * + */ +template > +class IncompleteCholesky : public SparseSolverBase > +{ + protected: + typedef SparseSolverBase > Base; + using Base::m_isInitialized; + public: + typedef typename NumTraits::Real RealScalar; + typedef _OrderingType OrderingType; + typedef typename OrderingType::PermutationType PermutationType; + typedef typename PermutationType::StorageIndex StorageIndex; + typedef SparseMatrix FactorType; + typedef Matrix VectorSx; + typedef Matrix VectorRx; + typedef Matrix VectorIx; + typedef std::vector > VectorList; + enum { UpLo = _UpLo }; + enum { + ColsAtCompileTime = Dynamic, + MaxColsAtCompileTime = Dynamic + }; + public: + + /** Default constructor leaving the object in a partly non-initialized stage. + * + * You must call compute() or the pair analyzePattern()/factorize() to make it valid. + * + * \sa IncompleteCholesky(const MatrixType&) + */ + IncompleteCholesky() : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false) {} + + /** Constructor computing the incomplete factorization for the given matrix \a matrix. + */ + template + IncompleteCholesky(const MatrixType& matrix) : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false) + { + compute(matrix); + } + + /** \returns number of rows of the factored matrix */ + EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_L.rows(); } + + /** \returns number of columns of the factored matrix */ + EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_L.cols(); } + + + /** \brief Reports whether previous computation was successful. + * + * It triggers an assertion if \c *this has not been initialized through the respective constructor, + * or a call to compute() or analyzePattern(). + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the matrix appears to be negative. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "IncompleteCholesky is not initialized."); + return m_info; + } + + /** \brief Set the initial shift parameter \f$ \sigma \f$. + */ + void setInitialShift(RealScalar shift) { m_initialShift = shift; } + + /** \brief Computes the fill reducing permutation vector using the sparsity pattern of \a mat + */ + template + void analyzePattern(const MatrixType& mat) + { + OrderingType ord; + PermutationType pinv; + ord(mat.template selfadjointView(), pinv); + if(pinv.size()>0) m_perm = pinv.inverse(); + else m_perm.resize(0); + m_L.resize(mat.rows(), mat.cols()); + m_analysisIsOk = true; + m_isInitialized = true; + m_info = Success; + } + + /** \brief Performs the numerical factorization of the input matrix \a mat + * + * The method analyzePattern() or compute() must have been called beforehand + * with a matrix having the same pattern. + * + * \sa compute(), analyzePattern() + */ + template + void factorize(const MatrixType& mat); + + /** Computes or re-computes the incomplete Cholesky factorization of the input matrix \a mat + * + * It is a shortcut for a sequential call to the analyzePattern() and factorize() methods. + * + * \sa analyzePattern(), factorize() + */ + template + void compute(const MatrixType& mat) + { + analyzePattern(mat); + factorize(mat); + } + + // internal + template + void _solve_impl(const Rhs& b, Dest& x) const + { + eigen_assert(m_factorizationIsOk && "factorize() should be called first"); + if (m_perm.rows() == b.rows()) x = m_perm * b; + else x = b; + x = m_scale.asDiagonal() * x; + x = m_L.template triangularView().solve(x); + x = m_L.adjoint().template triangularView().solve(x); + x = m_scale.asDiagonal() * x; + if (m_perm.rows() == b.rows()) + x = m_perm.inverse() * x; + } + + /** \returns the sparse lower triangular factor L */ + const FactorType& matrixL() const { eigen_assert("m_factorizationIsOk"); return m_L; } + + /** \returns a vector representing the scaling factor S */ + const VectorRx& scalingS() const { eigen_assert("m_factorizationIsOk"); return m_scale; } + + /** \returns the fill-in reducing permutation P (can be empty for a natural ordering) */ + const PermutationType& permutationP() const { eigen_assert("m_analysisIsOk"); return m_perm; } + + protected: + FactorType m_L; // The lower part stored in CSC + VectorRx m_scale; // The vector for scaling the matrix + RealScalar m_initialShift; // The initial shift parameter + bool m_analysisIsOk; + bool m_factorizationIsOk; + ComputationInfo m_info; + PermutationType m_perm; + + private: + inline void updateList(Ref colPtr, Ref rowIdx, Ref vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol); +}; + +// Based on the following paper: +// C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with +// Limited memory, SIAM J. Sci. Comput. 21(1), pp. 24-45, 1999 +// http://ftp.mcs.anl.gov/pub/tech_reports/reports/P682.pdf +template +template +void IncompleteCholesky::factorize(const _MatrixType& mat) +{ + using std::sqrt; + eigen_assert(m_analysisIsOk && "analyzePattern() should be called first"); + + // Dropping strategy : Keep only the p largest elements per column, where p is the number of elements in the column of the original matrix. Other strategies will be added + + // Apply the fill-reducing permutation computed in analyzePattern() + if (m_perm.rows() == mat.rows() ) // To detect the null permutation + { + // The temporary is needed to make sure that the diagonal entry is properly sorted + FactorType tmp(mat.rows(), mat.cols()); + tmp = mat.template selfadjointView<_UpLo>().twistedBy(m_perm); + m_L.template selfadjointView() = tmp.template selfadjointView(); + } + else + { + m_L.template selfadjointView() = mat.template selfadjointView<_UpLo>(); + } + + Index n = m_L.cols(); + Index nnz = m_L.nonZeros(); + Map vals(m_L.valuePtr(), nnz); //values + Map rowIdx(m_L.innerIndexPtr(), nnz); //Row indices + Map colPtr( m_L.outerIndexPtr(), n+1); // Pointer to the beginning of each row + VectorIx firstElt(n-1); // for each j, points to the next entry in vals that will be used in the factorization + VectorList listCol(n); // listCol(j) is a linked list of columns to update column j + VectorSx col_vals(n); // Store a nonzero values in each column + VectorIx col_irow(n); // Row indices of nonzero elements in each column + VectorIx col_pattern(n); + col_pattern.fill(-1); + StorageIndex col_nnz; + + + // Computes the scaling factors + m_scale.resize(n); + m_scale.setZero(); + for (Index j = 0; j < n; j++) + for (Index k = colPtr[j]; k < colPtr[j+1]; k++) + { + m_scale(j) += numext::abs2(vals(k)); + if(rowIdx[k]!=j) + m_scale(rowIdx[k]) += numext::abs2(vals(k)); + } + + m_scale = m_scale.cwiseSqrt().cwiseSqrt(); + + for (Index j = 0; j < n; ++j) + if(m_scale(j)>(std::numeric_limits::min)()) + m_scale(j) = RealScalar(1)/m_scale(j); + else + m_scale(j) = 1; + + // TODO disable scaling if not needed, i.e., if it is roughly uniform? (this will make solve() faster) + + // Scale and compute the shift for the matrix + RealScalar mindiag = NumTraits::highest(); + for (Index j = 0; j < n; j++) + { + for (Index k = colPtr[j]; k < colPtr[j+1]; k++) + vals[k] *= (m_scale(j)*m_scale(rowIdx[k])); + eigen_internal_assert(rowIdx[colPtr[j]]==j && "IncompleteCholesky: only the lower triangular part must be stored"); + mindiag = numext::mini(numext::real(vals[colPtr[j]]), mindiag); + } + + FactorType L_save = m_L; + + RealScalar shift = 0; + if(mindiag <= RealScalar(0.)) + shift = m_initialShift - mindiag; + + m_info = NumericalIssue; + + // Try to perform the incomplete factorization using the current shift + int iter = 0; + do + { + // Apply the shift to the diagonal elements of the matrix + for (Index j = 0; j < n; j++) + vals[colPtr[j]] += shift; + + // jki version of the Cholesky factorization + Index j=0; + for (; j < n; ++j) + { + // Left-looking factorization of the j-th column + // First, load the j-th column into col_vals + Scalar diag = vals[colPtr[j]]; // It is assumed that only the lower part is stored + col_nnz = 0; + for (Index i = colPtr[j] + 1; i < colPtr[j+1]; i++) + { + StorageIndex l = rowIdx[i]; + col_vals(col_nnz) = vals[i]; + col_irow(col_nnz) = l; + col_pattern(l) = col_nnz; + col_nnz++; + } + { + typename std::list::iterator k; + // Browse all previous columns that will update column j + for(k = listCol[j].begin(); k != listCol[j].end(); k++) + { + Index jk = firstElt(*k); // First element to use in the column + eigen_internal_assert(rowIdx[jk]==j); + Scalar v_j_jk = numext::conj(vals[jk]); + + jk += 1; + for (Index i = jk; i < colPtr[*k+1]; i++) + { + StorageIndex l = rowIdx[i]; + if(col_pattern[l]<0) + { + col_vals(col_nnz) = vals[i] * v_j_jk; + col_irow[col_nnz] = l; + col_pattern(l) = col_nnz; + col_nnz++; + } + else + col_vals(col_pattern[l]) -= vals[i] * v_j_jk; + } + updateList(colPtr,rowIdx,vals, *k, jk, firstElt, listCol); + } + } + + // Scale the current column + if(numext::real(diag) <= 0) + { + if(++iter>=10) + return; + + // increase shift + shift = numext::maxi(m_initialShift,RealScalar(2)*shift); + // restore m_L, col_pattern, and listCol + vals = Map(L_save.valuePtr(), nnz); + rowIdx = Map(L_save.innerIndexPtr(), nnz); + colPtr = Map(L_save.outerIndexPtr(), n+1); + col_pattern.fill(-1); + for(Index i=0; i cvals = col_vals.head(col_nnz); + Ref cirow = col_irow.head(col_nnz); + internal::QuickSplit(cvals,cirow, p); + // Insert the largest p elements in the matrix + Index cpt = 0; + for (Index i = colPtr[j]+1; i < colPtr[j+1]; i++) + { + vals[i] = col_vals(cpt); + rowIdx[i] = col_irow(cpt); + // restore col_pattern: + col_pattern(col_irow(cpt)) = -1; + cpt++; + } + // Get the first smallest row index and put it after the diagonal element + Index jk = colPtr(j)+1; + updateList(colPtr,rowIdx,vals,j,jk,firstElt,listCol); + } + + if(j==n) + { + m_factorizationIsOk = true; + m_info = Success; + } + } while(m_info!=Success); +} + +template +inline void IncompleteCholesky::updateList(Ref colPtr, Ref rowIdx, Ref vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol) +{ + if (jk < colPtr(col+1) ) + { + Index p = colPtr(col+1) - jk; + Index minpos; + rowIdx.segment(jk,p).minCoeff(&minpos); + minpos += jk; + if (rowIdx(minpos) != rowIdx(jk)) + { + //Swap + std::swap(rowIdx(jk),rowIdx(minpos)); + std::swap(vals(jk),vals(minpos)); + } + firstElt(col) = internal::convert_index(jk); + listCol[rowIdx(jk)].push_back(internal::convert_index(col)); + } +} + +} // end namespace Eigen + +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h new file mode 100644 index 0000000000..cdcf709eb6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h @@ -0,0 +1,453 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2014 Gael Guennebaud +// +// 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_INCOMPLETE_LUT_H +#define EIGEN_INCOMPLETE_LUT_H + + +namespace Eigen { + +namespace internal { + +/** \internal + * Compute a quick-sort split of a vector + * On output, the vector row is permuted such that its elements satisfy + * abs(row(i)) >= abs(row(ncut)) if incut + * \param row The vector of values + * \param ind The array of index for the elements in @p row + * \param ncut The number of largest elements to keep + **/ +template +Index QuickSplit(VectorV &row, VectorI &ind, Index ncut) +{ + typedef typename VectorV::RealScalar RealScalar; + using std::swap; + using std::abs; + Index mid; + Index n = row.size(); /* length of the vector */ + Index first, last ; + + ncut--; /* to fit the zero-based indices */ + first = 0; + last = n-1; + if (ncut < first || ncut > last ) return 0; + + do { + mid = first; + RealScalar abskey = abs(row(mid)); + for (Index j = first + 1; j <= last; j++) { + if ( abs(row(j)) > abskey) { + ++mid; + swap(row(mid), row(j)); + swap(ind(mid), ind(j)); + } + } + /* Interchange for the pivot element */ + swap(row(mid), row(first)); + swap(ind(mid), ind(first)); + + if (mid > ncut) last = mid - 1; + else if (mid < ncut ) first = mid + 1; + } while (mid != ncut ); + + return 0; /* mid is equal to ncut */ +} + +}// end namespace internal + +/** \ingroup IterativeLinearSolvers_Module + * \class IncompleteLUT + * \brief Incomplete LU factorization with dual-threshold strategy + * + * \implsparsesolverconcept + * + * During the numerical factorization, two dropping rules are used : + * 1) any element whose magnitude is less than some tolerance is dropped. + * This tolerance is obtained by multiplying the input tolerance @p droptol + * by the average magnitude of all the original elements in the current row. + * 2) After the elimination of the row, only the @p fill largest elements in + * the L part and the @p fill largest elements in the U part are kept + * (in addition to the diagonal element ). Note that @p fill is computed from + * the input parameter @p fillfactor which is used the ratio to control the fill_in + * relatively to the initial number of nonzero elements. + * + * The two extreme cases are when @p droptol=0 (to keep all the @p fill*2 largest elements) + * and when @p fill=n/2 with @p droptol being different to zero. + * + * References : Yousef Saad, ILUT: A dual threshold incomplete LU factorization, + * Numerical Linear Algebra with Applications, 1(4), pp 387-402, 1994. + * + * NOTE : The following implementation is derived from the ILUT implementation + * in the SPARSKIT package, Copyright (C) 2005, the Regents of the University of Minnesota + * released under the terms of the GNU LGPL: + * http://www-users.cs.umn.edu/~saad/software/SPARSKIT/README + * However, Yousef Saad gave us permission to relicense his ILUT code to MPL2. + * See the Eigen mailing list archive, thread: ILUT, date: July 8, 2012: + * http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2012/07/msg00064.html + * alternatively, on GMANE: + * http://comments.gmane.org/gmane.comp.lib.eigen/3302 + */ +template +class IncompleteLUT : public SparseSolverBase > +{ + protected: + typedef SparseSolverBase Base; + using Base::m_isInitialized; + public: + typedef _Scalar Scalar; + typedef _StorageIndex StorageIndex; + typedef typename NumTraits::Real RealScalar; + typedef Matrix Vector; + typedef Matrix VectorI; + typedef SparseMatrix FactorType; + + enum { + ColsAtCompileTime = Dynamic, + MaxColsAtCompileTime = Dynamic + }; + + public: + + IncompleteLUT() + : m_droptol(NumTraits::dummy_precision()), m_fillfactor(10), + m_analysisIsOk(false), m_factorizationIsOk(false) + {} + + template + explicit IncompleteLUT(const MatrixType& mat, const RealScalar& droptol=NumTraits::dummy_precision(), int fillfactor = 10) + : m_droptol(droptol),m_fillfactor(fillfactor), + m_analysisIsOk(false),m_factorizationIsOk(false) + { + eigen_assert(fillfactor != 0); + compute(mat); + } + + EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); } + + EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); } + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the matrix.appears to be negative. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "IncompleteLUT is not initialized."); + return m_info; + } + + template + void analyzePattern(const MatrixType& amat); + + template + void factorize(const MatrixType& amat); + + /** + * Compute an incomplete LU factorization with dual threshold on the matrix mat + * No pivoting is done in this version + * + **/ + template + IncompleteLUT& compute(const MatrixType& amat) + { + analyzePattern(amat); + factorize(amat); + return *this; + } + + void setDroptol(const RealScalar& droptol); + void setFillfactor(int fillfactor); + + template + void _solve_impl(const Rhs& b, Dest& x) const + { + x = m_Pinv * b; + x = m_lu.template triangularView().solve(x); + x = m_lu.template triangularView().solve(x); + x = m_P * x; + } + +protected: + + /** keeps off-diagonal entries; drops diagonal entries */ + struct keep_diag { + inline bool operator() (const Index& row, const Index& col, const Scalar&) const + { + return row!=col; + } + }; + +protected: + + FactorType m_lu; + RealScalar m_droptol; + int m_fillfactor; + bool m_analysisIsOk; + bool m_factorizationIsOk; + ComputationInfo m_info; + PermutationMatrix m_P; // Fill-reducing permutation + PermutationMatrix m_Pinv; // Inverse permutation +}; + +/** + * Set control parameter droptol + * \param droptol Drop any element whose magnitude is less than this tolerance + **/ +template +void IncompleteLUT::setDroptol(const RealScalar& droptol) +{ + this->m_droptol = droptol; +} + +/** + * Set control parameter fillfactor + * \param fillfactor This is used to compute the number @p fill_in of largest elements to keep on each row. + **/ +template +void IncompleteLUT::setFillfactor(int fillfactor) +{ + this->m_fillfactor = fillfactor; +} + +template +template +void IncompleteLUT::analyzePattern(const _MatrixType& amat) +{ + // Compute the Fill-reducing permutation + // Since ILUT does not perform any numerical pivoting, + // it is highly preferable to keep the diagonal through symmetric permutations. + // To this end, let's symmetrize the pattern and perform AMD on it. + SparseMatrix mat1 = amat; + SparseMatrix mat2 = amat.transpose(); + // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice. + // on the other hand for a really non-symmetric pattern, mat2*mat1 should be preferred... + SparseMatrix AtA = mat2 + mat1; + AMDOrdering ordering; + ordering(AtA,m_P); + m_Pinv = m_P.inverse(); // cache the inverse permutation + m_analysisIsOk = true; + m_factorizationIsOk = false; + m_isInitialized = true; +} + +template +template +void IncompleteLUT::factorize(const _MatrixType& amat) +{ + using std::sqrt; + using std::swap; + using std::abs; + using internal::convert_index; + + eigen_assert((amat.rows() == amat.cols()) && "The factorization should be done on a square matrix"); + Index n = amat.cols(); // Size of the matrix + m_lu.resize(n,n); + // Declare Working vectors and variables + Vector u(n) ; // real values of the row -- maximum size is n -- + VectorI ju(n); // column position of the values in u -- maximum size is n + VectorI jr(n); // Indicate the position of the nonzero elements in the vector u -- A zero location is indicated by -1 + + // Apply the fill-reducing permutation + eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); + SparseMatrix mat; + mat = amat.twistedBy(m_Pinv); + + // Initialization + jr.fill(-1); + ju.fill(0); + u.fill(0); + + // number of largest elements to keep in each row: + Index fill_in = (amat.nonZeros()*m_fillfactor)/n + 1; + if (fill_in > n) fill_in = n; + + // number of largest nonzero elements to keep in the L and the U part of the current row: + Index nnzL = fill_in/2; + Index nnzU = nnzL; + m_lu.reserve(n * (nnzL + nnzU + 1)); + + // global loop over the rows of the sparse matrix + for (Index ii = 0; ii < n; ii++) + { + // 1 - copy the lower and the upper part of the row i of mat in the working vector u + + Index sizeu = 1; // number of nonzero elements in the upper part of the current row + Index sizel = 0; // number of nonzero elements in the lower part of the current row + ju(ii) = convert_index(ii); + u(ii) = 0; + jr(ii) = convert_index(ii); + RealScalar rownorm = 0; + + typename FactorType::InnerIterator j_it(mat, ii); // Iterate through the current row ii + for (; j_it; ++j_it) + { + Index k = j_it.index(); + if (k < ii) + { + // copy the lower part + ju(sizel) = convert_index(k); + u(sizel) = j_it.value(); + jr(k) = convert_index(sizel); + ++sizel; + } + else if (k == ii) + { + u(ii) = j_it.value(); + } + else + { + // copy the upper part + Index jpos = ii + sizeu; + ju(jpos) = convert_index(k); + u(jpos) = j_it.value(); + jr(k) = convert_index(jpos); + ++sizeu; + } + rownorm += numext::abs2(j_it.value()); + } + + // 2 - detect possible zero row + if(rownorm==0) + { + m_info = NumericalIssue; + return; + } + // Take the 2-norm of the current row as a relative tolerance + rownorm = sqrt(rownorm); + + // 3 - eliminate the previous nonzero rows + Index jj = 0; + Index len = 0; + while (jj < sizel) + { + // In order to eliminate in the correct order, + // we must select first the smallest column index among ju(jj:sizel) + Index k; + Index minrow = ju.segment(jj,sizel-jj).minCoeff(&k); // k is relative to the segment + k += jj; + if (minrow != ju(jj)) + { + // swap the two locations + Index j = ju(jj); + swap(ju(jj), ju(k)); + jr(minrow) = convert_index(jj); + jr(j) = convert_index(k); + swap(u(jj), u(k)); + } + // Reset this location + jr(minrow) = -1; + + // Start elimination + typename FactorType::InnerIterator ki_it(m_lu, minrow); + while (ki_it && ki_it.index() < minrow) ++ki_it; + eigen_internal_assert(ki_it && ki_it.col()==minrow); + Scalar fact = u(jj) / ki_it.value(); + + // drop too small elements + if(abs(fact) <= m_droptol) + { + jj++; + continue; + } + + // linear combination of the current row ii and the row minrow + ++ki_it; + for (; ki_it; ++ki_it) + { + Scalar prod = fact * ki_it.value(); + Index j = ki_it.index(); + Index jpos = jr(j); + if (jpos == -1) // fill-in element + { + Index newpos; + if (j >= ii) // dealing with the upper part + { + newpos = ii + sizeu; + sizeu++; + eigen_internal_assert(sizeu<=n); + } + else // dealing with the lower part + { + newpos = sizel; + sizel++; + eigen_internal_assert(sizel<=ii); + } + ju(newpos) = convert_index(j); + u(newpos) = -prod; + jr(j) = convert_index(newpos); + } + else + u(jpos) -= prod; + } + // store the pivot element + u(len) = fact; + ju(len) = convert_index(minrow); + ++len; + + jj++; + } // end of the elimination on the row ii + + // reset the upper part of the pointer jr to zero + for(Index k = 0; k m_droptol * rownorm ) + { + ++len; + u(ii + len) = u(ii + k); + ju(ii + len) = ju(ii + k); + } + } + sizeu = len + 1; // +1 to take into account the diagonal element + len = (std::min)(sizeu, nnzU); + typename Vector::SegmentReturnType uu(u.segment(ii+1, sizeu-1)); + typename VectorI::SegmentReturnType juu(ju.segment(ii+1, sizeu-1)); + internal::QuickSplit(uu, juu, len); + + // store the largest elements of the U part + for(Index k = ii + 1; k < ii + len; k++) + m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k); + } + m_lu.finalize(); + m_lu.makeCompressed(); + + m_factorizationIsOk = true; + m_info = Success; +} + +} // end namespace Eigen + +#endif // EIGEN_INCOMPLETE_LUT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h new file mode 100644 index 0000000000..28a0c5109e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h @@ -0,0 +1,444 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011-2014 Gael Guennebaud +// +// 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_ITERATIVE_SOLVER_BASE_H +#define EIGEN_ITERATIVE_SOLVER_BASE_H + +namespace Eigen { + +namespace internal { + +template +struct is_ref_compatible_impl +{ +private: + template + struct any_conversion + { + template any_conversion(const volatile T&); + template any_conversion(T&); + }; + struct yes {int a[1];}; + struct no {int a[2];}; + + template + static yes test(const Ref&, int); + template + static no test(any_conversion, ...); + +public: + static MatrixType ms_from; + enum { value = sizeof(test(ms_from, 0))==sizeof(yes) }; +}; + +template +struct is_ref_compatible +{ + enum { value = is_ref_compatible_impl::type>::value }; +}; + +template::value> +class generic_matrix_wrapper; + +// We have an explicit matrix at hand, compatible with Ref<> +template +class generic_matrix_wrapper +{ +public: + typedef Ref ActualMatrixType; + template struct ConstSelfAdjointViewReturnType { + typedef typename ActualMatrixType::template ConstSelfAdjointViewReturnType::Type Type; + }; + + enum { + MatrixFree = false + }; + + generic_matrix_wrapper() + : m_dummy(0,0), m_matrix(m_dummy) + {} + + template + generic_matrix_wrapper(const InputType &mat) + : m_matrix(mat) + {} + + const ActualMatrixType& matrix() const + { + return m_matrix; + } + + template + void grab(const EigenBase &mat) + { + m_matrix.~Ref(); + ::new (&m_matrix) Ref(mat.derived()); + } + + void grab(const Ref &mat) + { + if(&(mat.derived()) != &m_matrix) + { + m_matrix.~Ref(); + ::new (&m_matrix) Ref(mat); + } + } + +protected: + MatrixType m_dummy; // used to default initialize the Ref<> object + ActualMatrixType m_matrix; +}; + +// MatrixType is not compatible with Ref<> -> matrix-free wrapper +template +class generic_matrix_wrapper +{ +public: + typedef MatrixType ActualMatrixType; + template struct ConstSelfAdjointViewReturnType + { + typedef ActualMatrixType Type; + }; + + enum { + MatrixFree = true + }; + + generic_matrix_wrapper() + : mp_matrix(0) + {} + + generic_matrix_wrapper(const MatrixType &mat) + : mp_matrix(&mat) + {} + + const ActualMatrixType& matrix() const + { + return *mp_matrix; + } + + void grab(const MatrixType &mat) + { + mp_matrix = &mat; + } + +protected: + const ActualMatrixType *mp_matrix; +}; + +} + +/** \ingroup IterativeLinearSolvers_Module + * \brief Base class for linear iterative solvers + * + * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner + */ +template< typename Derived> +class IterativeSolverBase : public SparseSolverBase +{ +protected: + typedef SparseSolverBase Base; + using Base::m_isInitialized; + +public: + typedef typename internal::traits::MatrixType MatrixType; + typedef typename internal::traits::Preconditioner Preconditioner; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef typename MatrixType::RealScalar RealScalar; + + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + +public: + + using Base::derived; + + /** Default constructor. */ + IterativeSolverBase() + { + init(); + } + + /** Initialize the solver with matrix \a A for further \c Ax=b solving. + * + * This constructor is a shortcut for the default constructor followed + * by a call to compute(). + * + * \warning this class stores a reference to the matrix A as well as some + * precomputed values that depend on it. Therefore, if \a A is changed + * this class becomes invalid. Call compute() to update it with the new + * matrix A, or modify a copy of A. + */ + template + explicit IterativeSolverBase(const EigenBase& A) + : m_matrixWrapper(A.derived()) + { + init(); + compute(matrix()); + } + + ~IterativeSolverBase() {} + + /** Initializes the iterative solver for the sparsity pattern of the matrix \a A for further solving \c Ax=b problems. + * + * Currently, this function mostly calls analyzePattern on the preconditioner. In the future + * we might, for instance, implement column reordering for faster matrix vector products. + */ + template + Derived& analyzePattern(const EigenBase& A) + { + grab(A.derived()); + m_preconditioner.analyzePattern(matrix()); + m_isInitialized = true; + m_analysisIsOk = true; + m_info = m_preconditioner.info(); + return derived(); + } + + /** Initializes the iterative solver with the numerical values of the matrix \a A for further solving \c Ax=b problems. + * + * Currently, this function mostly calls factorize on the preconditioner. + * + * \warning this class stores a reference to the matrix A as well as some + * precomputed values that depend on it. Therefore, if \a A is changed + * this class becomes invalid. Call compute() to update it with the new + * matrix A, or modify a copy of A. + */ + template + Derived& factorize(const EigenBase& A) + { + eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); + grab(A.derived()); + m_preconditioner.factorize(matrix()); + m_factorizationIsOk = true; + m_info = m_preconditioner.info(); + return derived(); + } + + /** Initializes the iterative solver with the matrix \a A for further solving \c Ax=b problems. + * + * Currently, this function mostly initializes/computes the preconditioner. In the future + * we might, for instance, implement column reordering for faster matrix vector products. + * + * \warning this class stores a reference to the matrix A as well as some + * precomputed values that depend on it. Therefore, if \a A is changed + * this class becomes invalid. Call compute() to update it with the new + * matrix A, or modify a copy of A. + */ + template + Derived& compute(const EigenBase& A) + { + grab(A.derived()); + m_preconditioner.compute(matrix()); + m_isInitialized = true; + m_analysisIsOk = true; + m_factorizationIsOk = true; + m_info = m_preconditioner.info(); + return derived(); + } + + /** \internal */ + EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return matrix().rows(); } + + /** \internal */ + EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return matrix().cols(); } + + /** \returns the tolerance threshold used by the stopping criteria. + * \sa setTolerance() + */ + RealScalar tolerance() const { return m_tolerance; } + + /** Sets the tolerance threshold used by the stopping criteria. + * + * This value is used as an upper bound to the relative residual error: |Ax-b|/|b|. + * The default value is the machine precision given by NumTraits::epsilon() + */ + Derived& setTolerance(const RealScalar& tolerance) + { + m_tolerance = tolerance; + return derived(); + } + + /** \returns a read-write reference to the preconditioner for custom configuration. */ + Preconditioner& preconditioner() { return m_preconditioner; } + + /** \returns a read-only reference to the preconditioner. */ + const Preconditioner& preconditioner() const { return m_preconditioner; } + + /** \returns the max number of iterations. + * It is either the value set by setMaxIterations or, by default, + * twice the number of columns of the matrix. + */ + Index maxIterations() const + { + return (m_maxIterations<0) ? 2*matrix().cols() : m_maxIterations; + } + + /** Sets the max number of iterations. + * Default is twice the number of columns of the matrix. + */ + Derived& setMaxIterations(Index maxIters) + { + m_maxIterations = maxIters; + return derived(); + } + + /** \returns the number of iterations performed during the last solve */ + Index iterations() const + { + eigen_assert(m_isInitialized && "ConjugateGradient is not initialized."); + return m_iterations; + } + + /** \returns the tolerance error reached during the last solve. + * It is a close approximation of the true relative residual error |Ax-b|/|b|. + */ + RealScalar error() const + { + eigen_assert(m_isInitialized && "ConjugateGradient is not initialized."); + return m_error; + } + + /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A + * and \a x0 as an initial solution. + * + * \sa solve(), compute() + */ + template + inline const SolveWithGuess + solveWithGuess(const MatrixBase& b, const Guess& x0) const + { + eigen_assert(m_isInitialized && "Solver is not initialized."); + eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b"); + return SolveWithGuess(derived(), b.derived(), x0); + } + + /** \returns Success if the iterations converged, and NoConvergence otherwise. */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "IterativeSolverBase is not initialized."); + return m_info; + } + + /** \internal */ + template + void _solve_with_guess_impl(const Rhs& b, SparseMatrixBase &aDest) const + { + eigen_assert(rows()==b.rows()); + + Index rhsCols = b.cols(); + Index size = b.rows(); + DestDerived& dest(aDest.derived()); + typedef typename DestDerived::Scalar DestScalar; + Eigen::Matrix tb(size); + Eigen::Matrix tx(cols()); + // We do not directly fill dest because sparse expressions have to be free of aliasing issue. + // For non square least-square problems, b and dest might not have the same size whereas they might alias each-other. + typename DestDerived::PlainObject tmp(cols(),rhsCols); + ComputationInfo global_info = Success; + for(Index k=0; k + typename internal::enable_if::type + _solve_with_guess_impl(const Rhs& b, MatrixBase &aDest) const + { + eigen_assert(rows()==b.rows()); + + Index rhsCols = b.cols(); + DestDerived& dest(aDest.derived()); + ComputationInfo global_info = Success; + for(Index k=0; k + typename internal::enable_if::type + _solve_with_guess_impl(const Rhs& b, MatrixBase &dest) const + { + derived()._solve_vector_with_guess_impl(b,dest.derived()); + } + + /** \internal default initial guess = 0 */ + template + void _solve_impl(const Rhs& b, Dest& x) const + { + x.setZero(); + derived()._solve_with_guess_impl(b,x); + } + +protected: + void init() + { + m_isInitialized = false; + m_analysisIsOk = false; + m_factorizationIsOk = false; + m_maxIterations = -1; + m_tolerance = NumTraits::epsilon(); + } + + typedef internal::generic_matrix_wrapper MatrixWrapper; + typedef typename MatrixWrapper::ActualMatrixType ActualMatrixType; + + const ActualMatrixType& matrix() const + { + return m_matrixWrapper.matrix(); + } + + template + void grab(const InputType &A) + { + m_matrixWrapper.grab(A); + } + + MatrixWrapper m_matrixWrapper; + Preconditioner m_preconditioner; + + Index m_maxIterations; + RealScalar m_tolerance; + + mutable RealScalar m_error; + mutable Index m_iterations; + mutable ComputationInfo m_info; + mutable bool m_analysisIsOk, m_factorizationIsOk; +}; + +} // end namespace Eigen + +#endif // EIGEN_ITERATIVE_SOLVER_BASE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h new file mode 100644 index 0000000000..203fd0ec63 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h @@ -0,0 +1,198 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Gael Guennebaud +// +// 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_LEAST_SQUARE_CONJUGATE_GRADIENT_H +#define EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H + +namespace Eigen { + +namespace internal { + +/** \internal Low-level conjugate gradient algorithm for least-square problems + * \param mat The matrix A + * \param rhs The right hand side vector b + * \param x On input and initial solution, on output the computed solution. + * \param precond A preconditioner being able to efficiently solve for an + * approximation of A'Ax=b (regardless of b) + * \param iters On input the max number of iteration, on output the number of performed iterations. + * \param tol_error On input the tolerance error, on output an estimation of the relative error. + */ +template +EIGEN_DONT_INLINE +void least_square_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x, + const Preconditioner& precond, Index& iters, + typename Dest::RealScalar& tol_error) +{ + using std::sqrt; + using std::abs; + typedef typename Dest::RealScalar RealScalar; + typedef typename Dest::Scalar Scalar; + typedef Matrix VectorType; + + RealScalar tol = tol_error; + Index maxIters = iters; + + Index m = mat.rows(), n = mat.cols(); + + VectorType residual = rhs - mat * x; + VectorType normal_residual = mat.adjoint() * residual; + + RealScalar rhsNorm2 = (mat.adjoint()*rhs).squaredNorm(); + if(rhsNorm2 == 0) + { + x.setZero(); + iters = 0; + tol_error = 0; + return; + } + RealScalar threshold = tol*tol*rhsNorm2; + RealScalar residualNorm2 = normal_residual.squaredNorm(); + if (residualNorm2 < threshold) + { + iters = 0; + tol_error = sqrt(residualNorm2 / rhsNorm2); + return; + } + + VectorType p(n); + p = precond.solve(normal_residual); // initial search direction + + VectorType z(n), tmp(m); + RealScalar absNew = numext::real(normal_residual.dot(p)); // the square of the absolute value of r scaled by invM + Index i = 0; + while(i < maxIters) + { + tmp.noalias() = mat * p; + + Scalar alpha = absNew / tmp.squaredNorm(); // the amount we travel on dir + x += alpha * p; // update solution + residual -= alpha * tmp; // update residual + normal_residual = mat.adjoint() * residual; // update residual of the normal equation + + residualNorm2 = normal_residual.squaredNorm(); + if(residualNorm2 < threshold) + break; + + z = precond.solve(normal_residual); // approximately solve for "A'A z = normal_residual" + + RealScalar absOld = absNew; + absNew = numext::real(normal_residual.dot(z)); // update the absolute value of r + RealScalar beta = absNew / absOld; // calculate the Gram-Schmidt value used to create the new search direction + p = z + beta * p; // update search direction + i++; + } + tol_error = sqrt(residualNorm2 / rhsNorm2); + iters = i; +} + +} + +template< typename _MatrixType, + typename _Preconditioner = LeastSquareDiagonalPreconditioner > +class LeastSquaresConjugateGradient; + +namespace internal { + +template< typename _MatrixType, typename _Preconditioner> +struct traits > +{ + typedef _MatrixType MatrixType; + typedef _Preconditioner Preconditioner; +}; + +} + +/** \ingroup IterativeLinearSolvers_Module + * \brief A conjugate gradient solver for sparse (or dense) least-square problems + * + * This class allows to solve for A x = b linear problems using an iterative conjugate gradient algorithm. + * The matrix A can be non symmetric and rectangular, but the matrix A' A should be positive-definite to guaranty stability. + * Otherwise, the SparseLU or SparseQR classes might be preferable. + * The matrix A and the vectors x and b can be either dense or sparse. + * + * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix. + * \tparam _Preconditioner the type of the preconditioner. Default is LeastSquareDiagonalPreconditioner + * + * \implsparsesolverconcept + * + * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations() + * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations + * and NumTraits::epsilon() for the tolerance. + * + * This class can be used as the direct solver classes. Here is a typical usage example: + \code + int m=1000000, n = 10000; + VectorXd x(n), b(m); + SparseMatrix A(m,n); + // fill A and b + LeastSquaresConjugateGradient > lscg; + lscg.compute(A); + x = lscg.solve(b); + std::cout << "#iterations: " << lscg.iterations() << std::endl; + std::cout << "estimated error: " << lscg.error() << std::endl; + // update b, and solve again + x = lscg.solve(b); + \endcode + * + * By default the iterations start with x=0 as an initial guess of the solution. + * One can control the start using the solveWithGuess() method. + * + * \sa class ConjugateGradient, SparseLU, SparseQR + */ +template< typename _MatrixType, typename _Preconditioner> +class LeastSquaresConjugateGradient : public IterativeSolverBase > +{ + typedef IterativeSolverBase Base; + using Base::matrix; + using Base::m_error; + using Base::m_iterations; + using Base::m_info; + using Base::m_isInitialized; +public: + typedef _MatrixType MatrixType; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef _Preconditioner Preconditioner; + +public: + + /** Default constructor. */ + LeastSquaresConjugateGradient() : Base() {} + + /** Initialize the solver with matrix \a A for further \c Ax=b solving. + * + * This constructor is a shortcut for the default constructor followed + * by a call to compute(). + * + * \warning this class stores a reference to the matrix A as well as some + * precomputed values that depend on it. Therefore, if \a A is changed + * this class becomes invalid. Call compute() to update it with the new + * matrix A, or modify a copy of A. + */ + template + explicit LeastSquaresConjugateGradient(const EigenBase& A) : Base(A.derived()) {} + + ~LeastSquaresConjugateGradient() {} + + /** \internal */ + template + void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const + { + m_iterations = Base::maxIterations(); + m_error = Base::m_tolerance; + + internal::least_square_conjugate_gradient(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error); + m_info = m_error <= Base::m_tolerance ? Success : NoConvergence; + } + +}; + +} // end namespace Eigen + +#endif // EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h new file mode 100644 index 0000000000..7b89657542 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h @@ -0,0 +1,117 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Gael Guennebaud +// +// 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_SOLVEWITHGUESS_H +#define EIGEN_SOLVEWITHGUESS_H + +namespace Eigen { + +template class SolveWithGuess; + +/** \class SolveWithGuess + * \ingroup IterativeLinearSolvers_Module + * + * \brief Pseudo expression representing a solving operation + * + * \tparam Decomposition the type of the matrix or decomposion object + * \tparam Rhstype the type of the right-hand side + * + * This class represents an expression of A.solve(B) + * and most of the time this is the only way it is used. + * + */ +namespace internal { + + +template +struct traits > + : traits > +{}; + +} + + +template +class SolveWithGuess : public internal::generic_xpr_base, MatrixXpr, typename internal::traits::StorageKind>::type +{ +public: + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::PlainObject PlainObject; + typedef typename internal::generic_xpr_base, MatrixXpr, typename internal::traits::StorageKind>::type Base; + typedef typename internal::ref_selector::type Nested; + + SolveWithGuess(const Decomposition &dec, const RhsType &rhs, const GuessType &guess) + : m_dec(dec), m_rhs(rhs), m_guess(guess) + {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + Index rows() const EIGEN_NOEXCEPT { return m_dec.cols(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } + + EIGEN_DEVICE_FUNC const Decomposition& dec() const { return m_dec; } + EIGEN_DEVICE_FUNC const RhsType& rhs() const { return m_rhs; } + EIGEN_DEVICE_FUNC const GuessType& guess() const { return m_guess; } + +protected: + const Decomposition &m_dec; + const RhsType &m_rhs; + const GuessType &m_guess; + +private: + Scalar coeff(Index row, Index col) const; + Scalar coeff(Index i) const; +}; + +namespace internal { + +// Evaluator of SolveWithGuess -> eval into a temporary +template +struct evaluator > + : public evaluator::PlainObject> +{ + typedef SolveWithGuess SolveType; + typedef typename SolveType::PlainObject PlainObject; + typedef evaluator Base; + + evaluator(const SolveType& solve) + : m_result(solve.rows(), solve.cols()) + { + ::new (static_cast(this)) Base(m_result); + m_result = solve.guess(); + solve.dec()._solve_with_guess_impl(solve.rhs(), m_result); + } + +protected: + PlainObject m_result; +}; + +// Specialization for "dst = dec.solveWithGuess(rhs)" +// NOTE we need to specialize it for Dense2Dense to avoid ambiguous specialization error and a Sparse2Sparse specialization must exist somewhere +template +struct Assignment, internal::assign_op, Dense2Dense> +{ + typedef SolveWithGuess SrcXprType; + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + dst = src.guess(); + src.dec()._solve_with_guess_impl(src.rhs(), dst/*, src.guess()*/); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SOLVEWITHGUESS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h new file mode 100644 index 0000000000..7ca3f33b12 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h @@ -0,0 +1,435 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Gael Guennebaud +// +// 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/. + +/* +NOTE: this routine has been adapted from the CSparse library: + +Copyright (c) 2006, Timothy A. Davis. +http://www.suitesparse.com + +The author of CSparse, Timothy A. Davis., has executed a license with Google LLC +to permit distribution of this code and derivative works as part of Eigen under +the Mozilla Public License v. 2.0, as stated at the top of this file. +*/ + +#ifndef EIGEN_SPARSE_AMD_H +#define EIGEN_SPARSE_AMD_H + +namespace Eigen { + +namespace internal { + +template inline T amd_flip(const T& i) { return -i-2; } +template inline T amd_unflip(const T& i) { return i<0 ? amd_flip(i) : i; } +template inline bool amd_marked(const T0* w, const T1& j) { return w[j]<0; } +template inline void amd_mark(const T0* w, const T1& j) { return w[j] = amd_flip(w[j]); } + +/* clear w */ +template +static StorageIndex cs_wclear (StorageIndex mark, StorageIndex lemax, StorageIndex *w, StorageIndex n) +{ + StorageIndex k; + if(mark < 2 || (mark + lemax < 0)) + { + for(k = 0; k < n; k++) + if(w[k] != 0) + w[k] = 1; + mark = 2; + } + return (mark); /* at this point, w[0..n-1] < mark holds */ +} + +/* depth-first search and postorder of a tree rooted at node j */ +template +StorageIndex cs_tdfs(StorageIndex j, StorageIndex k, StorageIndex *head, const StorageIndex *next, StorageIndex *post, StorageIndex *stack) +{ + StorageIndex i, p, top = 0; + if(!head || !next || !post || !stack) return (-1); /* check inputs */ + stack[0] = j; /* place j on the stack */ + while (top >= 0) /* while (stack is not empty) */ + { + p = stack[top]; /* p = top of stack */ + i = head[p]; /* i = youngest child of p */ + if(i == -1) + { + top--; /* p has no unordered children left */ + post[k++] = p; /* node p is the kth postordered node */ + } + else + { + head[p] = next[i]; /* remove i from children of p */ + stack[++top] = i; /* start dfs on child node i */ + } + } + return k; +} + + +/** \internal + * \ingroup OrderingMethods_Module + * Approximate minimum degree ordering algorithm. + * + * \param[in] C the input selfadjoint matrix stored in compressed column major format. + * \param[out] perm the permutation P reducing the fill-in of the input matrix \a C + * + * Note that the input matrix \a C must be complete, that is both the upper and lower parts have to be stored, as well as the diagonal entries. + * On exit the values of C are destroyed */ +template +void minimum_degree_ordering(SparseMatrix& C, PermutationMatrix& perm) +{ + using std::sqrt; + + StorageIndex d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1, + k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi, + ok, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t, h; + + StorageIndex n = StorageIndex(C.cols()); + dense = std::max (16, StorageIndex(10 * sqrt(double(n)))); /* find dense threshold */ + dense = (std::min)(n-2, dense); + + StorageIndex cnz = StorageIndex(C.nonZeros()); + perm.resize(n+1); + t = cnz + cnz/5 + 2*n; /* add elbow room to C */ + C.resizeNonZeros(t); + + // get workspace + ei_declare_aligned_stack_constructed_variable(StorageIndex,W,8*(n+1),0); + StorageIndex* len = W; + StorageIndex* nv = W + (n+1); + StorageIndex* next = W + 2*(n+1); + StorageIndex* head = W + 3*(n+1); + StorageIndex* elen = W + 4*(n+1); + StorageIndex* degree = W + 5*(n+1); + StorageIndex* w = W + 6*(n+1); + StorageIndex* hhead = W + 7*(n+1); + StorageIndex* last = perm.indices().data(); /* use P as workspace for last */ + + /* --- Initialize quotient graph ---------------------------------------- */ + StorageIndex* Cp = C.outerIndexPtr(); + StorageIndex* Ci = C.innerIndexPtr(); + for(k = 0; k < n; k++) + len[k] = Cp[k+1] - Cp[k]; + len[n] = 0; + nzmax = t; + + for(i = 0; i <= n; i++) + { + head[i] = -1; // degree list i is empty + last[i] = -1; + next[i] = -1; + hhead[i] = -1; // hash list i is empty + nv[i] = 1; // node i is just one node + w[i] = 1; // node i is alive + elen[i] = 0; // Ek of node i is empty + degree[i] = len[i]; // degree of node i + } + mark = internal::cs_wclear(0, 0, w, n); /* clear w */ + + /* --- Initialize degree lists ------------------------------------------ */ + for(i = 0; i < n; i++) + { + bool has_diag = false; + for(p = Cp[i]; p dense || !has_diag) /* node i is dense or has no structural diagonal element */ + { + nv[i] = 0; /* absorb i into element n */ + elen[i] = -1; /* node i is dead */ + nel++; + Cp[i] = amd_flip (n); + nv[n]++; + } + else + { + if(head[d] != -1) last[head[d]] = i; + next[i] = head[d]; /* put node i in degree list d */ + head[d] = i; + } + } + + elen[n] = -2; /* n is a dead element */ + Cp[n] = -1; /* n is a root of assembly tree */ + w[n] = 0; /* n is a dead element */ + + while (nel < n) /* while (selecting pivots) do */ + { + /* --- Select node of minimum approximate degree -------------------- */ + for(k = -1; mindeg < n && (k = head[mindeg]) == -1; mindeg++) {} + if(next[k] != -1) last[next[k]] = -1; + head[mindeg] = next[k]; /* remove k from degree list */ + elenk = elen[k]; /* elenk = |Ek| */ + nvk = nv[k]; /* # of nodes k represents */ + nel += nvk; /* nv[k] nodes of A eliminated */ + + /* --- Garbage collection ------------------------------------------- */ + if(elenk > 0 && cnz + mindeg >= nzmax) + { + for(j = 0; j < n; j++) + { + if((p = Cp[j]) >= 0) /* j is a live node or element */ + { + Cp[j] = Ci[p]; /* save first entry of object */ + Ci[p] = amd_flip (j); /* first entry is now amd_flip(j) */ + } + } + for(q = 0, p = 0; p < cnz; ) /* scan all of memory */ + { + if((j = amd_flip (Ci[p++])) >= 0) /* found object j */ + { + Ci[q] = Cp[j]; /* restore first entry of object */ + Cp[j] = q++; /* new pointer to object j */ + for(k3 = 0; k3 < len[j]-1; k3++) Ci[q++] = Ci[p++]; + } + } + cnz = q; /* Ci[cnz...nzmax-1] now free */ + } + + /* --- Construct new element ---------------------------------------- */ + dk = 0; + nv[k] = -nvk; /* flag k as in Lk */ + p = Cp[k]; + pk1 = (elenk == 0) ? p : cnz; /* do in place if elen[k] == 0 */ + pk2 = pk1; + for(k1 = 1; k1 <= elenk + 1; k1++) + { + if(k1 > elenk) + { + e = k; /* search the nodes in k */ + pj = p; /* list of nodes starts at Ci[pj]*/ + ln = len[k] - elenk; /* length of list of nodes in k */ + } + else + { + e = Ci[p++]; /* search the nodes in e */ + pj = Cp[e]; + ln = len[e]; /* length of list of nodes in e */ + } + for(k2 = 1; k2 <= ln; k2++) + { + i = Ci[pj++]; + if((nvi = nv[i]) <= 0) continue; /* node i dead, or seen */ + dk += nvi; /* degree[Lk] += size of node i */ + nv[i] = -nvi; /* negate nv[i] to denote i in Lk*/ + Ci[pk2++] = i; /* place i in Lk */ + if(next[i] != -1) last[next[i]] = last[i]; + if(last[i] != -1) /* remove i from degree list */ + { + next[last[i]] = next[i]; + } + else + { + head[degree[i]] = next[i]; + } + } + if(e != k) + { + Cp[e] = amd_flip (k); /* absorb e into k */ + w[e] = 0; /* e is now a dead element */ + } + } + if(elenk != 0) cnz = pk2; /* Ci[cnz...nzmax] is free */ + degree[k] = dk; /* external degree of k - |Lk\i| */ + Cp[k] = pk1; /* element k is in Ci[pk1..pk2-1] */ + len[k] = pk2 - pk1; + elen[k] = -2; /* k is now an element */ + + /* --- Find set differences ----------------------------------------- */ + mark = internal::cs_wclear(mark, lemax, w, n); /* clear w if necessary */ + for(pk = pk1; pk < pk2; pk++) /* scan 1: find |Le\Lk| */ + { + i = Ci[pk]; + if((eln = elen[i]) <= 0) continue;/* skip if elen[i] empty */ + nvi = -nv[i]; /* nv[i] was negated */ + wnvi = mark - nvi; + for(p = Cp[i]; p <= Cp[i] + eln - 1; p++) /* scan Ei */ + { + e = Ci[p]; + if(w[e] >= mark) + { + w[e] -= nvi; /* decrement |Le\Lk| */ + } + else if(w[e] != 0) /* ensure e is a live element */ + { + w[e] = degree[e] + wnvi; /* 1st time e seen in scan 1 */ + } + } + } + + /* --- Degree update ------------------------------------------------ */ + for(pk = pk1; pk < pk2; pk++) /* scan2: degree update */ + { + i = Ci[pk]; /* consider node i in Lk */ + p1 = Cp[i]; + p2 = p1 + elen[i] - 1; + pn = p1; + for(h = 0, d = 0, p = p1; p <= p2; p++) /* scan Ei */ + { + e = Ci[p]; + if(w[e] != 0) /* e is an unabsorbed element */ + { + dext = w[e] - mark; /* dext = |Le\Lk| */ + if(dext > 0) + { + d += dext; /* sum up the set differences */ + Ci[pn++] = e; /* keep e in Ei */ + h += e; /* compute the hash of node i */ + } + else + { + Cp[e] = amd_flip (k); /* aggressive absorb. e->k */ + w[e] = 0; /* e is a dead element */ + } + } + } + elen[i] = pn - p1 + 1; /* elen[i] = |Ei| */ + p3 = pn; + p4 = p1 + len[i]; + for(p = p2 + 1; p < p4; p++) /* prune edges in Ai */ + { + j = Ci[p]; + if((nvj = nv[j]) <= 0) continue; /* node j dead or in Lk */ + d += nvj; /* degree(i) += |j| */ + Ci[pn++] = j; /* place j in node list of i */ + h += j; /* compute hash for node i */ + } + if(d == 0) /* check for mass elimination */ + { + Cp[i] = amd_flip (k); /* absorb i into k */ + nvi = -nv[i]; + dk -= nvi; /* |Lk| -= |i| */ + nvk += nvi; /* |k| += nv[i] */ + nel += nvi; + nv[i] = 0; + elen[i] = -1; /* node i is dead */ + } + else + { + degree[i] = std::min (degree[i], d); /* update degree(i) */ + Ci[pn] = Ci[p3]; /* move first node to end */ + Ci[p3] = Ci[p1]; /* move 1st el. to end of Ei */ + Ci[p1] = k; /* add k as 1st element in of Ei */ + len[i] = pn - p1 + 1; /* new len of adj. list of node i */ + h %= n; /* finalize hash of i */ + next[i] = hhead[h]; /* place i in hash bucket */ + hhead[h] = i; + last[i] = h; /* save hash of i in last[i] */ + } + } /* scan2 is done */ + degree[k] = dk; /* finalize |Lk| */ + lemax = std::max(lemax, dk); + mark = internal::cs_wclear(mark+lemax, lemax, w, n); /* clear w */ + + /* --- Supernode detection ------------------------------------------ */ + for(pk = pk1; pk < pk2; pk++) + { + i = Ci[pk]; + if(nv[i] >= 0) continue; /* skip if i is dead */ + h = last[i]; /* scan hash bucket of node i */ + i = hhead[h]; + hhead[h] = -1; /* hash bucket will be empty */ + for(; i != -1 && next[i] != -1; i = next[i], mark++) + { + ln = len[i]; + eln = elen[i]; + for(p = Cp[i]+1; p <= Cp[i] + ln-1; p++) w[Ci[p]] = mark; + jlast = i; + for(j = next[i]; j != -1; ) /* compare i with all j */ + { + ok = (len[j] == ln) && (elen[j] == eln); + for(p = Cp[j] + 1; ok && p <= Cp[j] + ln - 1; p++) + { + if(w[Ci[p]] != mark) ok = 0; /* compare i and j*/ + } + if(ok) /* i and j are identical */ + { + Cp[j] = amd_flip (i); /* absorb j into i */ + nv[i] += nv[j]; + nv[j] = 0; + elen[j] = -1; /* node j is dead */ + j = next[j]; /* delete j from hash bucket */ + next[jlast] = j; + } + else + { + jlast = j; /* j and i are different */ + j = next[j]; + } + } + } + } + + /* --- Finalize new element------------------------------------------ */ + for(p = pk1, pk = pk1; pk < pk2; pk++) /* finalize Lk */ + { + i = Ci[pk]; + if((nvi = -nv[i]) <= 0) continue;/* skip if i is dead */ + nv[i] = nvi; /* restore nv[i] */ + d = degree[i] + dk - nvi; /* compute external degree(i) */ + d = std::min (d, n - nel - nvi); + if(head[d] != -1) last[head[d]] = i; + next[i] = head[d]; /* put i back in degree list */ + last[i] = -1; + head[d] = i; + mindeg = std::min (mindeg, d); /* find new minimum degree */ + degree[i] = d; + Ci[p++] = i; /* place i in Lk */ + } + nv[k] = nvk; /* # nodes absorbed into k */ + if((len[k] = p-pk1) == 0) /* length of adj list of element k*/ + { + Cp[k] = -1; /* k is a root of the tree */ + w[k] = 0; /* k is now a dead element */ + } + if(elenk != 0) cnz = p; /* free unused space in Lk */ + } + + /* --- Postordering ----------------------------------------------------- */ + for(i = 0; i < n; i++) Cp[i] = amd_flip (Cp[i]);/* fix assembly tree */ + for(j = 0; j <= n; j++) head[j] = -1; + for(j = n; j >= 0; j--) /* place unordered nodes in lists */ + { + if(nv[j] > 0) continue; /* skip if j is an element */ + next[j] = head[Cp[j]]; /* place j in list of its parent */ + head[Cp[j]] = j; + } + for(e = n; e >= 0; e--) /* place elements in lists */ + { + if(nv[e] <= 0) continue; /* skip unless e is an element */ + if(Cp[e] != -1) + { + next[e] = head[Cp[e]]; /* place e in list of its parent */ + head[Cp[e]] = e; + } + } + for(k = 0, i = 0; i <= n; i++) /* postorder the assembly tree */ + { + if(Cp[i] == -1) k = internal::cs_tdfs(i, k, head, next, perm.indices().data(), w); + } + + perm.indices().conservativeResize(n); +} + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_AMD_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h new file mode 100644 index 0000000000..8e339a704a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h @@ -0,0 +1,1863 @@ +// // This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Desire Nuentsa Wakam +// +// 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/. + +// This file is modified from the colamd/symamd library. The copyright is below + +// The authors of the code itself are Stefan I. Larimore and Timothy A. +// Davis (davis@cise.ufl.edu), University of Florida. The algorithm was +// developed in collaboration with John Gilbert, Xerox PARC, and Esmond +// Ng, Oak Ridge National Laboratory. +// +// Date: +// +// September 8, 2003. Version 2.3. +// +// Acknowledgements: +// +// This work was supported by the National Science Foundation, under +// grants DMS-9504974 and DMS-9803599. +// +// Notice: +// +// Copyright (c) 1998-2003 by the University of Florida. +// All Rights Reserved. +// +// THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY +// EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. +// +// Permission is hereby granted to use, copy, modify, and/or distribute +// this program, provided that the Copyright, this License, and the +// Availability of the original version is retained on all copies and made +// accessible to the end-user of any code or package that includes COLAMD +// or any modified version of COLAMD. +// +// Availability: +// +// The colamd/symamd library is available at +// +// http://www.suitesparse.com + + +#ifndef EIGEN_COLAMD_H +#define EIGEN_COLAMD_H + +namespace internal { + +namespace Colamd { + +/* Ensure that debugging is turned off: */ +#ifndef COLAMD_NDEBUG +#define COLAMD_NDEBUG +#endif /* NDEBUG */ + + +/* ========================================================================== */ +/* === Knob and statistics definitions ====================================== */ +/* ========================================================================== */ + +/* size of the knobs [ ] array. Only knobs [0..1] are currently used. */ +const int NKnobs = 20; + +/* number of output statistics. Only stats [0..6] are currently used. */ +const int NStats = 20; + +/* Indices into knobs and stats array. */ +enum KnobsStatsIndex { + /* knobs [0] and stats [0]: dense row knob and output statistic. */ + DenseRow = 0, + + /* knobs [1] and stats [1]: dense column knob and output statistic. */ + DenseCol = 1, + + /* stats [2]: memory defragmentation count output statistic */ + DefragCount = 2, + + /* stats [3]: colamd status: zero OK, > 0 warning or notice, < 0 error */ + Status = 3, + + /* stats [4..6]: error info, or info on jumbled columns */ + Info1 = 4, + Info2 = 5, + Info3 = 6 +}; + +/* error codes returned in stats [3]: */ +enum Status { + Ok = 0, + OkButJumbled = 1, + ErrorANotPresent = -1, + ErrorPNotPresent = -2, + ErrorNrowNegative = -3, + ErrorNcolNegative = -4, + ErrorNnzNegative = -5, + ErrorP0Nonzero = -6, + ErrorATooSmall = -7, + ErrorColLengthNegative = -8, + ErrorRowIndexOutOfBounds = -9, + ErrorOutOfMemory = -10, + ErrorInternalError = -999 +}; +/* ========================================================================== */ +/* === Definitions ========================================================== */ +/* ========================================================================== */ + +template +IndexType ones_complement(const IndexType r) { + return (-(r)-1); +} + +/* -------------------------------------------------------------------------- */ +const int Empty = -1; + +/* Row and column status */ +enum RowColumnStatus { + Alive = 0, + Dead = -1 +}; + +/* Column status */ +enum ColumnStatus { + DeadPrincipal = -1, + DeadNonPrincipal = -2 +}; + +/* ========================================================================== */ +/* === Colamd reporting mechanism =========================================== */ +/* ========================================================================== */ + +// == Row and Column structures == +template +struct ColStructure +{ + IndexType start ; /* index for A of first row in this column, or Dead */ + /* if column is dead */ + IndexType length ; /* number of rows in this column */ + union + { + IndexType thickness ; /* number of original columns represented by this */ + /* col, if the column is alive */ + IndexType parent ; /* parent in parent tree super-column structure, if */ + /* the column is dead */ + } shared1 ; + union + { + IndexType score ; /* the score used to maintain heap, if col is alive */ + IndexType order ; /* pivot ordering of this column, if col is dead */ + } shared2 ; + union + { + IndexType headhash ; /* head of a hash bucket, if col is at the head of */ + /* a degree list */ + IndexType hash ; /* hash value, if col is not in a degree list */ + IndexType prev ; /* previous column in degree list, if col is in a */ + /* degree list (but not at the head of a degree list) */ + } shared3 ; + union + { + IndexType degree_next ; /* next column, if col is in a degree list */ + IndexType hash_next ; /* next column, if col is in a hash list */ + } shared4 ; + + inline bool is_dead() const { return start < Alive; } + + inline bool is_alive() const { return start >= Alive; } + + inline bool is_dead_principal() const { return start == DeadPrincipal; } + + inline void kill_principal() { start = DeadPrincipal; } + + inline void kill_non_principal() { start = DeadNonPrincipal; } + +}; + +template +struct RowStructure +{ + IndexType start ; /* index for A of first col in this row */ + IndexType length ; /* number of principal columns in this row */ + union + { + IndexType degree ; /* number of principal & non-principal columns in row */ + IndexType p ; /* used as a row pointer in init_rows_cols () */ + } shared1 ; + union + { + IndexType mark ; /* for computing set differences and marking dead rows*/ + IndexType first_column ;/* first column in row (used in garbage collection) */ + } shared2 ; + + inline bool is_dead() const { return shared2.mark < Alive; } + + inline bool is_alive() const { return shared2.mark >= Alive; } + + inline void kill() { shared2.mark = Dead; } + +}; + +/* ========================================================================== */ +/* === Colamd recommended memory size ======================================= */ +/* ========================================================================== */ + +/* + The recommended length Alen of the array A passed to colamd is given by + the COLAMD_RECOMMENDED (nnz, n_row, n_col) macro. It returns -1 if any + argument is negative. 2*nnz space is required for the row and column + indices of the matrix. colamd_c (n_col) + colamd_r (n_row) space is + required for the Col and Row arrays, respectively, which are internal to + colamd. An additional n_col space is the minimal amount of "elbow room", + and nnz/5 more space is recommended for run time efficiency. + + This macro is not needed when using symamd. + + Explicit typecast to IndexType added Sept. 23, 2002, COLAMD version 2.2, to avoid + gcc -pedantic warning messages. +*/ +template +inline IndexType colamd_c(IndexType n_col) +{ return IndexType( ((n_col) + 1) * sizeof (ColStructure) / sizeof (IndexType) ) ; } + +template +inline IndexType colamd_r(IndexType n_row) +{ return IndexType(((n_row) + 1) * sizeof (RowStructure) / sizeof (IndexType)); } + +// Prototypes of non-user callable routines +template +static IndexType init_rows_cols (IndexType n_row, IndexType n_col, RowStructure Row [], ColStructure col [], IndexType A [], IndexType p [], IndexType stats[NStats] ); + +template +static void init_scoring (IndexType n_row, IndexType n_col, RowStructure Row [], ColStructure Col [], IndexType A [], IndexType head [], double knobs[NKnobs], IndexType *p_n_row2, IndexType *p_n_col2, IndexType *p_max_deg); + +template +static IndexType find_ordering (IndexType n_row, IndexType n_col, IndexType Alen, RowStructure Row [], ColStructure Col [], IndexType A [], IndexType head [], IndexType n_col2, IndexType max_deg, IndexType pfree); + +template +static void order_children (IndexType n_col, ColStructure Col [], IndexType p []); + +template +static void detect_super_cols (ColStructure Col [], IndexType A [], IndexType head [], IndexType row_start, IndexType row_length ) ; + +template +static IndexType garbage_collection (IndexType n_row, IndexType n_col, RowStructure Row [], ColStructure Col [], IndexType A [], IndexType *pfree) ; + +template +static inline IndexType clear_mark (IndexType n_row, RowStructure Row [] ) ; + +/* === No debugging ========================================================= */ + +#define COLAMD_DEBUG0(params) ; +#define COLAMD_DEBUG1(params) ; +#define COLAMD_DEBUG2(params) ; +#define COLAMD_DEBUG3(params) ; +#define COLAMD_DEBUG4(params) ; + +#define COLAMD_ASSERT(expression) ((void) 0) + + +/** + * \brief Returns the recommended value of Alen + * + * Returns recommended value of Alen for use by colamd. + * Returns -1 if any input argument is negative. + * The use of this routine or macro is optional. + * Note that the macro uses its arguments more than once, + * so be careful for side effects, if you pass expressions as arguments to COLAMD_RECOMMENDED. + * + * \param nnz nonzeros in A + * \param n_row number of rows in A + * \param n_col number of columns in A + * \return recommended value of Alen for use by colamd + */ +template +inline IndexType recommended ( IndexType nnz, IndexType n_row, IndexType n_col) +{ + if ((nnz) < 0 || (n_row) < 0 || (n_col) < 0) + return (-1); + else + return (2 * (nnz) + colamd_c (n_col) + colamd_r (n_row) + (n_col) + ((nnz) / 5)); +} + +/** + * \brief set default parameters The use of this routine is optional. + * + * Colamd: rows with more than (knobs [DenseRow] * n_col) + * entries are removed prior to ordering. Columns with more than + * (knobs [DenseCol] * n_row) entries are removed prior to + * ordering, and placed last in the output column ordering. + * + * DenseRow and DenseCol are defined as 0 and 1, + * respectively, in colamd.h. Default values of these two knobs + * are both 0.5. Currently, only knobs [0] and knobs [1] are + * used, but future versions may use more knobs. If so, they will + * be properly set to their defaults by the future version of + * colamd_set_defaults, so that the code that calls colamd will + * not need to change, assuming that you either use + * colamd_set_defaults, or pass a (double *) NULL pointer as the + * knobs array to colamd or symamd. + * + * \param knobs parameter settings for colamd + */ + +static inline void set_defaults(double knobs[NKnobs]) +{ + /* === Local variables ================================================== */ + + int i ; + + if (!knobs) + { + return ; /* no knobs to initialize */ + } + for (i = 0 ; i < NKnobs ; i++) + { + knobs [i] = 0 ; + } + knobs [Colamd::DenseRow] = 0.5 ; /* ignore rows over 50% dense */ + knobs [Colamd::DenseCol] = 0.5 ; /* ignore columns over 50% dense */ +} + +/** + * \brief Computes a column ordering using the column approximate minimum degree ordering + * + * Computes a column ordering (Q) of A such that P(AQ)=LU or + * (AQ)'AQ=LL' have less fill-in and require fewer floating point + * operations than factorizing the unpermuted matrix A or A'A, + * respectively. + * + * + * \param n_row number of rows in A + * \param n_col number of columns in A + * \param Alen, size of the array A + * \param A row indices of the matrix, of size ALen + * \param p column pointers of A, of size n_col+1 + * \param knobs parameter settings for colamd + * \param stats colamd output statistics and error codes + */ +template +static bool compute_ordering(IndexType n_row, IndexType n_col, IndexType Alen, IndexType *A, IndexType *p, double knobs[NKnobs], IndexType stats[NStats]) +{ + /* === Local variables ================================================== */ + + IndexType i ; /* loop index */ + IndexType nnz ; /* nonzeros in A */ + IndexType Row_size ; /* size of Row [], in integers */ + IndexType Col_size ; /* size of Col [], in integers */ + IndexType need ; /* minimum required length of A */ + Colamd::RowStructure *Row ; /* pointer into A of Row [0..n_row] array */ + Colamd::ColStructure *Col ; /* pointer into A of Col [0..n_col] array */ + IndexType n_col2 ; /* number of non-dense, non-empty columns */ + IndexType n_row2 ; /* number of non-dense, non-empty rows */ + IndexType ngarbage ; /* number of garbage collections performed */ + IndexType max_deg ; /* maximum row degree */ + double default_knobs [NKnobs] ; /* default knobs array */ + + + /* === Check the input arguments ======================================== */ + + if (!stats) + { + COLAMD_DEBUG0 (("colamd: stats not present\n")) ; + return (false) ; + } + for (i = 0 ; i < NStats ; i++) + { + stats [i] = 0 ; + } + stats [Colamd::Status] = Colamd::Ok ; + stats [Colamd::Info1] = -1 ; + stats [Colamd::Info2] = -1 ; + + if (!A) /* A is not present */ + { + stats [Colamd::Status] = Colamd::ErrorANotPresent ; + COLAMD_DEBUG0 (("colamd: A not present\n")) ; + return (false) ; + } + + if (!p) /* p is not present */ + { + stats [Colamd::Status] = Colamd::ErrorPNotPresent ; + COLAMD_DEBUG0 (("colamd: p not present\n")) ; + return (false) ; + } + + if (n_row < 0) /* n_row must be >= 0 */ + { + stats [Colamd::Status] = Colamd::ErrorNrowNegative ; + stats [Colamd::Info1] = n_row ; + COLAMD_DEBUG0 (("colamd: nrow negative %d\n", n_row)) ; + return (false) ; + } + + if (n_col < 0) /* n_col must be >= 0 */ + { + stats [Colamd::Status] = Colamd::ErrorNcolNegative ; + stats [Colamd::Info1] = n_col ; + COLAMD_DEBUG0 (("colamd: ncol negative %d\n", n_col)) ; + return (false) ; + } + + nnz = p [n_col] ; + if (nnz < 0) /* nnz must be >= 0 */ + { + stats [Colamd::Status] = Colamd::ErrorNnzNegative ; + stats [Colamd::Info1] = nnz ; + COLAMD_DEBUG0 (("colamd: number of entries negative %d\n", nnz)) ; + return (false) ; + } + + if (p [0] != 0) + { + stats [Colamd::Status] = Colamd::ErrorP0Nonzero ; + stats [Colamd::Info1] = p [0] ; + COLAMD_DEBUG0 (("colamd: p[0] not zero %d\n", p [0])) ; + return (false) ; + } + + /* === If no knobs, set default knobs =================================== */ + + if (!knobs) + { + set_defaults (default_knobs) ; + knobs = default_knobs ; + } + + /* === Allocate the Row and Col arrays from array A ===================== */ + + Col_size = colamd_c (n_col) ; + Row_size = colamd_r (n_row) ; + need = 2*nnz + n_col + Col_size + Row_size ; + + if (need > Alen) + { + /* not enough space in array A to perform the ordering */ + stats [Colamd::Status] = Colamd::ErrorATooSmall ; + stats [Colamd::Info1] = need ; + stats [Colamd::Info2] = Alen ; + COLAMD_DEBUG0 (("colamd: Need Alen >= %d, given only Alen = %d\n", need,Alen)); + return (false) ; + } + + Alen -= Col_size + Row_size ; + Col = (ColStructure *) &A [Alen] ; + Row = (RowStructure *) &A [Alen + Col_size] ; + + /* === Construct the row and column data structures ===================== */ + + if (!Colamd::init_rows_cols (n_row, n_col, Row, Col, A, p, stats)) + { + /* input matrix is invalid */ + COLAMD_DEBUG0 (("colamd: Matrix invalid\n")) ; + return (false) ; + } + + /* === Initialize scores, kill dense rows/columns ======================= */ + + Colamd::init_scoring (n_row, n_col, Row, Col, A, p, knobs, + &n_row2, &n_col2, &max_deg) ; + + /* === Order the supercolumns =========================================== */ + + ngarbage = Colamd::find_ordering (n_row, n_col, Alen, Row, Col, A, p, + n_col2, max_deg, 2*nnz) ; + + /* === Order the non-principal columns ================================== */ + + Colamd::order_children (n_col, Col, p) ; + + /* === Return statistics in stats ======================================= */ + + stats [Colamd::DenseRow] = n_row - n_row2 ; + stats [Colamd::DenseCol] = n_col - n_col2 ; + stats [Colamd::DefragCount] = ngarbage ; + COLAMD_DEBUG0 (("colamd: done.\n")) ; + return (true) ; +} + +/* ========================================================================== */ +/* === NON-USER-CALLABLE ROUTINES: ========================================== */ +/* ========================================================================== */ + +/* There are no user-callable routines beyond this point in the file */ + +/* ========================================================================== */ +/* === init_rows_cols ======================================================= */ +/* ========================================================================== */ + +/* + Takes the column form of the matrix in A and creates the row form of the + matrix. Also, row and column attributes are stored in the Col and Row + structs. If the columns are un-sorted or contain duplicate row indices, + this routine will also sort and remove duplicate row indices from the + column form of the matrix. Returns false if the matrix is invalid, + true otherwise. Not user-callable. +*/ +template +static IndexType init_rows_cols /* returns true if OK, or false otherwise */ + ( + /* === Parameters ======================================================= */ + + IndexType n_row, /* number of rows of A */ + IndexType n_col, /* number of columns of A */ + RowStructure Row [], /* of size n_row+1 */ + ColStructure Col [], /* of size n_col+1 */ + IndexType A [], /* row indices of A, of size Alen */ + IndexType p [], /* pointers to columns in A, of size n_col+1 */ + IndexType stats [NStats] /* colamd statistics */ + ) +{ + /* === Local variables ================================================== */ + + IndexType col ; /* a column index */ + IndexType row ; /* a row index */ + IndexType *cp ; /* a column pointer */ + IndexType *cp_end ; /* a pointer to the end of a column */ + IndexType *rp ; /* a row pointer */ + IndexType *rp_end ; /* a pointer to the end of a row */ + IndexType last_row ; /* previous row */ + + /* === Initialize columns, and check column pointers ==================== */ + + for (col = 0 ; col < n_col ; col++) + { + Col [col].start = p [col] ; + Col [col].length = p [col+1] - p [col] ; + + if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200 + { + /* column pointers must be non-decreasing */ + stats [Colamd::Status] = Colamd::ErrorColLengthNegative ; + stats [Colamd::Info1] = col ; + stats [Colamd::Info2] = Col [col].length ; + COLAMD_DEBUG0 (("colamd: col %d length %d < 0\n", col, Col [col].length)) ; + return (false) ; + } + + Col [col].shared1.thickness = 1 ; + Col [col].shared2.score = 0 ; + Col [col].shared3.prev = Empty ; + Col [col].shared4.degree_next = Empty ; + } + + /* p [0..n_col] no longer needed, used as "head" in subsequent routines */ + + /* === Scan columns, compute row degrees, and check row indices ========= */ + + stats [Info3] = 0 ; /* number of duplicate or unsorted row indices*/ + + for (row = 0 ; row < n_row ; row++) + { + Row [row].length = 0 ; + Row [row].shared2.mark = -1 ; + } + + for (col = 0 ; col < n_col ; col++) + { + last_row = -1 ; + + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + + while (cp < cp_end) + { + row = *cp++ ; + + /* make sure row indices within range */ + if (row < 0 || row >= n_row) + { + stats [Colamd::Status] = Colamd::ErrorRowIndexOutOfBounds ; + stats [Colamd::Info1] = col ; + stats [Colamd::Info2] = row ; + stats [Colamd::Info3] = n_row ; + COLAMD_DEBUG0 (("colamd: row %d col %d out of bounds\n", row, col)) ; + return (false) ; + } + + if (row <= last_row || Row [row].shared2.mark == col) + { + /* row index are unsorted or repeated (or both), thus col */ + /* is jumbled. This is a notice, not an error condition. */ + stats [Colamd::Status] = Colamd::OkButJumbled ; + stats [Colamd::Info1] = col ; + stats [Colamd::Info2] = row ; + (stats [Colamd::Info3]) ++ ; + COLAMD_DEBUG1 (("colamd: row %d col %d unsorted/duplicate\n",row,col)); + } + + if (Row [row].shared2.mark != col) + { + Row [row].length++ ; + } + else + { + /* this is a repeated entry in the column, */ + /* it will be removed */ + Col [col].length-- ; + } + + /* mark the row as having been seen in this column */ + Row [row].shared2.mark = col ; + + last_row = row ; + } + } + + /* === Compute row pointers ============================================= */ + + /* row form of the matrix starts directly after the column */ + /* form of matrix in A */ + Row [0].start = p [n_col] ; + Row [0].shared1.p = Row [0].start ; + Row [0].shared2.mark = -1 ; + for (row = 1 ; row < n_row ; row++) + { + Row [row].start = Row [row-1].start + Row [row-1].length ; + Row [row].shared1.p = Row [row].start ; + Row [row].shared2.mark = -1 ; + } + + /* === Create row form ================================================== */ + + if (stats [Status] == OkButJumbled) + { + /* if cols jumbled, watch for repeated row indices */ + for (col = 0 ; col < n_col ; col++) + { + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + while (cp < cp_end) + { + row = *cp++ ; + if (Row [row].shared2.mark != col) + { + A [(Row [row].shared1.p)++] = col ; + Row [row].shared2.mark = col ; + } + } + } + } + else + { + /* if cols not jumbled, we don't need the mark (this is faster) */ + for (col = 0 ; col < n_col ; col++) + { + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + while (cp < cp_end) + { + A [(Row [*cp++].shared1.p)++] = col ; + } + } + } + + /* === Clear the row marks and set row degrees ========================== */ + + for (row = 0 ; row < n_row ; row++) + { + Row [row].shared2.mark = 0 ; + Row [row].shared1.degree = Row [row].length ; + } + + /* === See if we need to re-create columns ============================== */ + + if (stats [Status] == OkButJumbled) + { + COLAMD_DEBUG0 (("colamd: reconstructing column form, matrix jumbled\n")) ; + + + /* === Compute col pointers ========================================= */ + + /* col form of the matrix starts at A [0]. */ + /* Note, we may have a gap between the col form and the row */ + /* form if there were duplicate entries, if so, it will be */ + /* removed upon the first garbage collection */ + Col [0].start = 0 ; + p [0] = Col [0].start ; + for (col = 1 ; col < n_col ; col++) + { + /* note that the lengths here are for pruned columns, i.e. */ + /* no duplicate row indices will exist for these columns */ + Col [col].start = Col [col-1].start + Col [col-1].length ; + p [col] = Col [col].start ; + } + + /* === Re-create col form =========================================== */ + + for (row = 0 ; row < n_row ; row++) + { + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + A [(p [*rp++])++] = row ; + } + } + } + + /* === Done. Matrix is not (or no longer) jumbled ====================== */ + + return (true) ; +} + + +/* ========================================================================== */ +/* === init_scoring ========================================================= */ +/* ========================================================================== */ + +/* + Kills dense or empty columns and rows, calculates an initial score for + each column, and places all columns in the degree lists. Not user-callable. +*/ +template +static void init_scoring + ( + /* === Parameters ======================================================= */ + + IndexType n_row, /* number of rows of A */ + IndexType n_col, /* number of columns of A */ + RowStructure Row [], /* of size n_row+1 */ + ColStructure Col [], /* of size n_col+1 */ + IndexType A [], /* column form and row form of A */ + IndexType head [], /* of size n_col+1 */ + double knobs [NKnobs],/* parameters */ + IndexType *p_n_row2, /* number of non-dense, non-empty rows */ + IndexType *p_n_col2, /* number of non-dense, non-empty columns */ + IndexType *p_max_deg /* maximum row degree */ + ) +{ + /* === Local variables ================================================== */ + + IndexType c ; /* a column index */ + IndexType r, row ; /* a row index */ + IndexType *cp ; /* a column pointer */ + IndexType deg ; /* degree of a row or column */ + IndexType *cp_end ; /* a pointer to the end of a column */ + IndexType *new_cp ; /* new column pointer */ + IndexType col_length ; /* length of pruned column */ + IndexType score ; /* current column score */ + IndexType n_col2 ; /* number of non-dense, non-empty columns */ + IndexType n_row2 ; /* number of non-dense, non-empty rows */ + IndexType dense_row_count ; /* remove rows with more entries than this */ + IndexType dense_col_count ; /* remove cols with more entries than this */ + IndexType min_score ; /* smallest column score */ + IndexType max_deg ; /* maximum row degree */ + IndexType next_col ; /* Used to add to degree list.*/ + + + /* === Extract knobs ==================================================== */ + + dense_row_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseRow] * n_col), n_col)) ; + dense_col_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseCol] * n_row), n_row)) ; + COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ; + max_deg = 0 ; + n_col2 = n_col ; + n_row2 = n_row ; + + /* === Kill empty columns =============================================== */ + + /* Put the empty columns at the end in their natural order, so that LU */ + /* factorization can proceed as far as possible. */ + for (c = n_col-1 ; c >= 0 ; c--) + { + deg = Col [c].length ; + if (deg == 0) + { + /* this is a empty column, kill and order it last */ + Col [c].shared2.order = --n_col2 ; + Col[c].kill_principal() ; + } + } + COLAMD_DEBUG1 (("colamd: null columns killed: %d\n", n_col - n_col2)) ; + + /* === Kill dense columns =============================================== */ + + /* Put the dense columns at the end, in their natural order */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* skip any dead columns */ + if (Col[c].is_dead()) + { + continue ; + } + deg = Col [c].length ; + if (deg > dense_col_count) + { + /* this is a dense column, kill and order it last */ + Col [c].shared2.order = --n_col2 ; + /* decrement the row degrees */ + cp = &A [Col [c].start] ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + Row [*cp++].shared1.degree-- ; + } + Col[c].kill_principal() ; + } + } + COLAMD_DEBUG1 (("colamd: Dense and null columns killed: %d\n", n_col - n_col2)) ; + + /* === Kill dense and empty rows ======================================== */ + + for (r = 0 ; r < n_row ; r++) + { + deg = Row [r].shared1.degree ; + COLAMD_ASSERT (deg >= 0 && deg <= n_col) ; + if (deg > dense_row_count || deg == 0) + { + /* kill a dense or empty row */ + Row[r].kill() ; + --n_row2 ; + } + else + { + /* keep track of max degree of remaining rows */ + max_deg = numext::maxi(max_deg, deg) ; + } + } + COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ; + + /* === Compute initial column scores ==================================== */ + + /* At this point the row degrees are accurate. They reflect the number */ + /* of "live" (non-dense) columns in each row. No empty rows exist. */ + /* Some "live" columns may contain only dead rows, however. These are */ + /* pruned in the code below. */ + + /* now find the initial matlab score for each column */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* skip dead column */ + if (Col[c].is_dead()) + { + continue ; + } + score = 0 ; + cp = &A [Col [c].start] ; + new_cp = cp ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + /* skip if dead */ + if (Row[row].is_dead()) + { + continue ; + } + /* compact the column */ + *new_cp++ = row ; + /* add row's external degree */ + score += Row [row].shared1.degree - 1 ; + /* guard against integer overflow */ + score = numext::mini(score, n_col) ; + } + /* determine pruned column length */ + col_length = (IndexType) (new_cp - &A [Col [c].start]) ; + if (col_length == 0) + { + /* a newly-made null column (all rows in this col are "dense" */ + /* and have already been killed) */ + COLAMD_DEBUG2 (("Newly null killed: %d\n", c)) ; + Col [c].shared2.order = --n_col2 ; + Col[c].kill_principal() ; + } + else + { + /* set column length and set score */ + COLAMD_ASSERT (score >= 0) ; + COLAMD_ASSERT (score <= n_col) ; + Col [c].length = col_length ; + Col [c].shared2.score = score ; + } + } + COLAMD_DEBUG1 (("colamd: Dense, null, and newly-null columns killed: %d\n", + n_col-n_col2)) ; + + /* At this point, all empty rows and columns are dead. All live columns */ + /* are "clean" (containing no dead rows) and simplicial (no supercolumns */ + /* yet). Rows may contain dead columns, but all live rows contain at */ + /* least one live column. */ + + /* === Initialize degree lists ========================================== */ + + + /* clear the hash buckets */ + for (c = 0 ; c <= n_col ; c++) + { + head [c] = Empty ; + } + min_score = n_col ; + /* place in reverse order, so low column indices are at the front */ + /* of the lists. This is to encourage natural tie-breaking */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* only add principal columns to degree lists */ + if (Col[c].is_alive()) + { + COLAMD_DEBUG4 (("place %d score %d minscore %d ncol %d\n", + c, Col [c].shared2.score, min_score, n_col)) ; + + /* === Add columns score to DList =============================== */ + + score = Col [c].shared2.score ; + + COLAMD_ASSERT (min_score >= 0) ; + COLAMD_ASSERT (min_score <= n_col) ; + COLAMD_ASSERT (score >= 0) ; + COLAMD_ASSERT (score <= n_col) ; + COLAMD_ASSERT (head [score] >= Empty) ; + + /* now add this column to dList at proper score location */ + next_col = head [score] ; + Col [c].shared3.prev = Empty ; + Col [c].shared4.degree_next = next_col ; + + /* if there already was a column with the same score, set its */ + /* previous pointer to this new column */ + if (next_col != Empty) + { + Col [next_col].shared3.prev = c ; + } + head [score] = c ; + + /* see if this score is less than current min */ + min_score = numext::mini(min_score, score) ; + + + } + } + + + /* === Return number of remaining columns, and max row degree =========== */ + + *p_n_col2 = n_col2 ; + *p_n_row2 = n_row2 ; + *p_max_deg = max_deg ; +} + + +/* ========================================================================== */ +/* === find_ordering ======================================================== */ +/* ========================================================================== */ + +/* + Order the principal columns of the supercolumn form of the matrix + (no supercolumns on input). Uses a minimum approximate column minimum + degree ordering method. Not user-callable. +*/ +template +static IndexType find_ordering /* return the number of garbage collections */ + ( + /* === Parameters ======================================================= */ + + IndexType n_row, /* number of rows of A */ + IndexType n_col, /* number of columns of A */ + IndexType Alen, /* size of A, 2*nnz + n_col or larger */ + RowStructure Row [], /* of size n_row+1 */ + ColStructure Col [], /* of size n_col+1 */ + IndexType A [], /* column form and row form of A */ + IndexType head [], /* of size n_col+1 */ + IndexType n_col2, /* Remaining columns to order */ + IndexType max_deg, /* Maximum row degree */ + IndexType pfree /* index of first free slot (2*nnz on entry) */ + ) +{ + /* === Local variables ================================================== */ + + IndexType k ; /* current pivot ordering step */ + IndexType pivot_col ; /* current pivot column */ + IndexType *cp ; /* a column pointer */ + IndexType *rp ; /* a row pointer */ + IndexType pivot_row ; /* current pivot row */ + IndexType *new_cp ; /* modified column pointer */ + IndexType *new_rp ; /* modified row pointer */ + IndexType pivot_row_start ; /* pointer to start of pivot row */ + IndexType pivot_row_degree ; /* number of columns in pivot row */ + IndexType pivot_row_length ; /* number of supercolumns in pivot row */ + IndexType pivot_col_score ; /* score of pivot column */ + IndexType needed_memory ; /* free space needed for pivot row */ + IndexType *cp_end ; /* pointer to the end of a column */ + IndexType *rp_end ; /* pointer to the end of a row */ + IndexType row ; /* a row index */ + IndexType col ; /* a column index */ + IndexType max_score ; /* maximum possible score */ + IndexType cur_score ; /* score of current column */ + unsigned int hash ; /* hash value for supernode detection */ + IndexType head_column ; /* head of hash bucket */ + IndexType first_col ; /* first column in hash bucket */ + IndexType tag_mark ; /* marker value for mark array */ + IndexType row_mark ; /* Row [row].shared2.mark */ + IndexType set_difference ; /* set difference size of row with pivot row */ + IndexType min_score ; /* smallest column score */ + IndexType col_thickness ; /* "thickness" (no. of columns in a supercol) */ + IndexType max_mark ; /* maximum value of tag_mark */ + IndexType pivot_col_thickness ; /* number of columns represented by pivot col */ + IndexType prev_col ; /* Used by Dlist operations. */ + IndexType next_col ; /* Used by Dlist operations. */ + IndexType ngarbage ; /* number of garbage collections performed */ + + + /* === Initialization and clear mark ==================================== */ + + max_mark = INT_MAX - n_col ; /* INT_MAX defined in */ + tag_mark = Colamd::clear_mark (n_row, Row) ; + min_score = 0 ; + ngarbage = 0 ; + COLAMD_DEBUG1 (("colamd: Ordering, n_col2=%d\n", n_col2)) ; + + /* === Order the columns ================================================ */ + + for (k = 0 ; k < n_col2 ; /* 'k' is incremented below */) + { + + /* === Select pivot column, and order it ============================ */ + + /* make sure degree list isn't empty */ + COLAMD_ASSERT (min_score >= 0) ; + COLAMD_ASSERT (min_score <= n_col) ; + COLAMD_ASSERT (head [min_score] >= Empty) ; + + /* get pivot column from head of minimum degree list */ + while (min_score < n_col && head [min_score] == Empty) + { + min_score++ ; + } + pivot_col = head [min_score] ; + COLAMD_ASSERT (pivot_col >= 0 && pivot_col <= n_col) ; + next_col = Col [pivot_col].shared4.degree_next ; + head [min_score] = next_col ; + if (next_col != Empty) + { + Col [next_col].shared3.prev = Empty ; + } + + COLAMD_ASSERT (Col[pivot_col].is_alive()) ; + COLAMD_DEBUG3 (("Pivot col: %d\n", pivot_col)) ; + + /* remember score for defrag check */ + pivot_col_score = Col [pivot_col].shared2.score ; + + /* the pivot column is the kth column in the pivot order */ + Col [pivot_col].shared2.order = k ; + + /* increment order count by column thickness */ + pivot_col_thickness = Col [pivot_col].shared1.thickness ; + k += pivot_col_thickness ; + COLAMD_ASSERT (pivot_col_thickness > 0) ; + + /* === Garbage_collection, if necessary ============================= */ + + needed_memory = numext::mini(pivot_col_score, n_col - k) ; + if (pfree + needed_memory >= Alen) + { + pfree = Colamd::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ; + ngarbage++ ; + /* after garbage collection we will have enough */ + COLAMD_ASSERT (pfree + needed_memory < Alen) ; + /* garbage collection has wiped out the Row[].shared2.mark array */ + tag_mark = Colamd::clear_mark (n_row, Row) ; + + } + + /* === Compute pivot row pattern ==================================== */ + + /* get starting location for this new merged row */ + pivot_row_start = pfree ; + + /* initialize new row counts to zero */ + pivot_row_degree = 0 ; + + /* tag pivot column as having been visited so it isn't included */ + /* in merged pivot row */ + Col [pivot_col].shared1.thickness = -pivot_col_thickness ; + + /* pivot row is the union of all rows in the pivot column pattern */ + cp = &A [Col [pivot_col].start] ; + cp_end = cp + Col [pivot_col].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + COLAMD_DEBUG4 (("Pivot col pattern %d %d\n", Row[row].is_alive(), row)) ; + /* skip if row is dead */ + if (Row[row].is_dead()) + { + continue ; + } + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + /* get a column */ + col = *rp++ ; + /* add the column, if alive and untagged */ + col_thickness = Col [col].shared1.thickness ; + if (col_thickness > 0 && Col[col].is_alive()) + { + /* tag column in pivot row */ + Col [col].shared1.thickness = -col_thickness ; + COLAMD_ASSERT (pfree < Alen) ; + /* place column in pivot row */ + A [pfree++] = col ; + pivot_row_degree += col_thickness ; + } + } + } + + /* clear tag on pivot column */ + Col [pivot_col].shared1.thickness = pivot_col_thickness ; + max_deg = numext::maxi(max_deg, pivot_row_degree) ; + + + /* === Kill all rows used to construct pivot row ==================== */ + + /* also kill pivot row, temporarily */ + cp = &A [Col [pivot_col].start] ; + cp_end = cp + Col [pivot_col].length ; + while (cp < cp_end) + { + /* may be killing an already dead row */ + row = *cp++ ; + COLAMD_DEBUG3 (("Kill row in pivot col: %d\n", row)) ; + Row[row].kill() ; + } + + /* === Select a row index to use as the new pivot row =============== */ + + pivot_row_length = pfree - pivot_row_start ; + if (pivot_row_length > 0) + { + /* pick the "pivot" row arbitrarily (first row in col) */ + pivot_row = A [Col [pivot_col].start] ; + COLAMD_DEBUG3 (("Pivotal row is %d\n", pivot_row)) ; + } + else + { + /* there is no pivot row, since it is of zero length */ + pivot_row = Empty ; + COLAMD_ASSERT (pivot_row_length == 0) ; + } + COLAMD_ASSERT (Col [pivot_col].length > 0 || pivot_row_length == 0) ; + + /* === Approximate degree computation =============================== */ + + /* Here begins the computation of the approximate degree. The column */ + /* score is the sum of the pivot row "length", plus the size of the */ + /* set differences of each row in the column minus the pattern of the */ + /* pivot row itself. The column ("thickness") itself is also */ + /* excluded from the column score (we thus use an approximate */ + /* external degree). */ + + /* The time taken by the following code (compute set differences, and */ + /* add them up) is proportional to the size of the data structure */ + /* being scanned - that is, the sum of the sizes of each column in */ + /* the pivot row. Thus, the amortized time to compute a column score */ + /* is proportional to the size of that column (where size, in this */ + /* context, is the column "length", or the number of row indices */ + /* in that column). The number of row indices in a column is */ + /* monotonically non-decreasing, from the length of the original */ + /* column on input to colamd. */ + + /* === Compute set differences ====================================== */ + + COLAMD_DEBUG3 (("** Computing set differences phase. **\n")) ; + + /* pivot row is currently dead - it will be revived later. */ + + COLAMD_DEBUG3 (("Pivot row: ")) ; + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + col = *rp++ ; + COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ; + COLAMD_DEBUG3 (("Col: %d\n", col)) ; + + /* clear tags used to construct pivot row pattern */ + col_thickness = -Col [col].shared1.thickness ; + COLAMD_ASSERT (col_thickness > 0) ; + Col [col].shared1.thickness = col_thickness ; + + /* === Remove column from degree list =========================== */ + + cur_score = Col [col].shared2.score ; + prev_col = Col [col].shared3.prev ; + next_col = Col [col].shared4.degree_next ; + COLAMD_ASSERT (cur_score >= 0) ; + COLAMD_ASSERT (cur_score <= n_col) ; + COLAMD_ASSERT (cur_score >= Empty) ; + if (prev_col == Empty) + { + head [cur_score] = next_col ; + } + else + { + Col [prev_col].shared4.degree_next = next_col ; + } + if (next_col != Empty) + { + Col [next_col].shared3.prev = prev_col ; + } + + /* === Scan the column ========================================== */ + + cp = &A [Col [col].start] ; + cp_end = cp + Col [col].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + /* skip if dead */ + if (Row[row].is_dead()) + { + continue ; + } + row_mark = Row [row].shared2.mark ; + COLAMD_ASSERT (row != pivot_row) ; + set_difference = row_mark - tag_mark ; + /* check if the row has been seen yet */ + if (set_difference < 0) + { + COLAMD_ASSERT (Row [row].shared1.degree <= max_deg) ; + set_difference = Row [row].shared1.degree ; + } + /* subtract column thickness from this row's set difference */ + set_difference -= col_thickness ; + COLAMD_ASSERT (set_difference >= 0) ; + /* absorb this row if the set difference becomes zero */ + if (set_difference == 0) + { + COLAMD_DEBUG3 (("aggressive absorption. Row: %d\n", row)) ; + Row[row].kill() ; + } + else + { + /* save the new mark */ + Row [row].shared2.mark = set_difference + tag_mark ; + } + } + } + + + /* === Add up set differences for each column ======================= */ + + COLAMD_DEBUG3 (("** Adding set differences phase. **\n")) ; + + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + /* get a column */ + col = *rp++ ; + COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ; + hash = 0 ; + cur_score = 0 ; + cp = &A [Col [col].start] ; + /* compact the column */ + new_cp = cp ; + cp_end = cp + Col [col].length ; + + COLAMD_DEBUG4 (("Adding set diffs for Col: %d.\n", col)) ; + + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + COLAMD_ASSERT(row >= 0 && row < n_row) ; + /* skip if dead */ + if (Row [row].is_dead()) + { + continue ; + } + row_mark = Row [row].shared2.mark ; + COLAMD_ASSERT (row_mark > tag_mark) ; + /* compact the column */ + *new_cp++ = row ; + /* compute hash function */ + hash += row ; + /* add set difference */ + cur_score += row_mark - tag_mark ; + /* integer overflow... */ + cur_score = numext::mini(cur_score, n_col) ; + } + + /* recompute the column's length */ + Col [col].length = (IndexType) (new_cp - &A [Col [col].start]) ; + + /* === Further mass elimination ================================= */ + + if (Col [col].length == 0) + { + COLAMD_DEBUG4 (("further mass elimination. Col: %d\n", col)) ; + /* nothing left but the pivot row in this column */ + Col[col].kill_principal() ; + pivot_row_degree -= Col [col].shared1.thickness ; + COLAMD_ASSERT (pivot_row_degree >= 0) ; + /* order it */ + Col [col].shared2.order = k ; + /* increment order count by column thickness */ + k += Col [col].shared1.thickness ; + } + else + { + /* === Prepare for supercolumn detection ==================== */ + + COLAMD_DEBUG4 (("Preparing supercol detection for Col: %d.\n", col)) ; + + /* save score so far */ + Col [col].shared2.score = cur_score ; + + /* add column to hash table, for supercolumn detection */ + hash %= n_col + 1 ; + + COLAMD_DEBUG4 ((" Hash = %d, n_col = %d.\n", hash, n_col)) ; + COLAMD_ASSERT (hash <= n_col) ; + + head_column = head [hash] ; + if (head_column > Empty) + { + /* degree list "hash" is non-empty, use prev (shared3) of */ + /* first column in degree list as head of hash bucket */ + first_col = Col [head_column].shared3.headhash ; + Col [head_column].shared3.headhash = col ; + } + else + { + /* degree list "hash" is empty, use head as hash bucket */ + first_col = - (head_column + 2) ; + head [hash] = - (col + 2) ; + } + Col [col].shared4.hash_next = first_col ; + + /* save hash function in Col [col].shared3.hash */ + Col [col].shared3.hash = (IndexType) hash ; + COLAMD_ASSERT (Col[col].is_alive()) ; + } + } + + /* The approximate external column degree is now computed. */ + + /* === Supercolumn detection ======================================== */ + + COLAMD_DEBUG3 (("** Supercolumn detection phase. **\n")) ; + + Colamd::detect_super_cols (Col, A, head, pivot_row_start, pivot_row_length) ; + + /* === Kill the pivotal column ====================================== */ + + Col[pivot_col].kill_principal() ; + + /* === Clear mark =================================================== */ + + tag_mark += (max_deg + 1) ; + if (tag_mark >= max_mark) + { + COLAMD_DEBUG2 (("clearing tag_mark\n")) ; + tag_mark = Colamd::clear_mark (n_row, Row) ; + } + + /* === Finalize the new pivot row, and column scores ================ */ + + COLAMD_DEBUG3 (("** Finalize scores phase. **\n")) ; + + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + /* compact the pivot row */ + new_rp = rp ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + col = *rp++ ; + /* skip dead columns */ + if (Col[col].is_dead()) + { + continue ; + } + *new_rp++ = col ; + /* add new pivot row to column */ + A [Col [col].start + (Col [col].length++)] = pivot_row ; + + /* retrieve score so far and add on pivot row's degree. */ + /* (we wait until here for this in case the pivot */ + /* row's degree was reduced due to mass elimination). */ + cur_score = Col [col].shared2.score + pivot_row_degree ; + + /* calculate the max possible score as the number of */ + /* external columns minus the 'k' value minus the */ + /* columns thickness */ + max_score = n_col - k - Col [col].shared1.thickness ; + + /* make the score the external degree of the union-of-rows */ + cur_score -= Col [col].shared1.thickness ; + + /* make sure score is less or equal than the max score */ + cur_score = numext::mini(cur_score, max_score) ; + COLAMD_ASSERT (cur_score >= 0) ; + + /* store updated score */ + Col [col].shared2.score = cur_score ; + + /* === Place column back in degree list ========================= */ + + COLAMD_ASSERT (min_score >= 0) ; + COLAMD_ASSERT (min_score <= n_col) ; + COLAMD_ASSERT (cur_score >= 0) ; + COLAMD_ASSERT (cur_score <= n_col) ; + COLAMD_ASSERT (head [cur_score] >= Empty) ; + next_col = head [cur_score] ; + Col [col].shared4.degree_next = next_col ; + Col [col].shared3.prev = Empty ; + if (next_col != Empty) + { + Col [next_col].shared3.prev = col ; + } + head [cur_score] = col ; + + /* see if this score is less than current min */ + min_score = numext::mini(min_score, cur_score) ; + + } + + /* === Resurrect the new pivot row ================================== */ + + if (pivot_row_degree > 0) + { + /* update pivot row length to reflect any cols that were killed */ + /* during super-col detection and mass elimination */ + Row [pivot_row].start = pivot_row_start ; + Row [pivot_row].length = (IndexType) (new_rp - &A[pivot_row_start]) ; + Row [pivot_row].shared1.degree = pivot_row_degree ; + Row [pivot_row].shared2.mark = 0 ; + /* pivot row is no longer dead */ + } + } + + /* === All principal columns have now been ordered ====================== */ + + return (ngarbage) ; +} + + +/* ========================================================================== */ +/* === order_children ======================================================= */ +/* ========================================================================== */ + +/* + The find_ordering routine has ordered all of the principal columns (the + representatives of the supercolumns). The non-principal columns have not + yet been ordered. This routine orders those columns by walking up the + parent tree (a column is a child of the column which absorbed it). The + final permutation vector is then placed in p [0 ... n_col-1], with p [0] + being the first column, and p [n_col-1] being the last. It doesn't look + like it at first glance, but be assured that this routine takes time linear + in the number of columns. Although not immediately obvious, the time + taken by this routine is O (n_col), that is, linear in the number of + columns. Not user-callable. +*/ +template +static inline void order_children +( + /* === Parameters ======================================================= */ + + IndexType n_col, /* number of columns of A */ + ColStructure Col [], /* of size n_col+1 */ + IndexType p [] /* p [0 ... n_col-1] is the column permutation*/ + ) +{ + /* === Local variables ================================================== */ + + IndexType i ; /* loop counter for all columns */ + IndexType c ; /* column index */ + IndexType parent ; /* index of column's parent */ + IndexType order ; /* column's order */ + + /* === Order each non-principal column ================================== */ + + for (i = 0 ; i < n_col ; i++) + { + /* find an un-ordered non-principal column */ + COLAMD_ASSERT (col_is_dead(Col, i)) ; + if (!Col[i].is_dead_principal() && Col [i].shared2.order == Empty) + { + parent = i ; + /* once found, find its principal parent */ + do + { + parent = Col [parent].shared1.parent ; + } while (!Col[parent].is_dead_principal()) ; + + /* now, order all un-ordered non-principal columns along path */ + /* to this parent. collapse tree at the same time */ + c = i ; + /* get order of parent */ + order = Col [parent].shared2.order ; + + do + { + COLAMD_ASSERT (Col [c].shared2.order == Empty) ; + + /* order this column */ + Col [c].shared2.order = order++ ; + /* collaps tree */ + Col [c].shared1.parent = parent ; + + /* get immediate parent of this column */ + c = Col [c].shared1.parent ; + + /* continue until we hit an ordered column. There are */ + /* guaranteed not to be anymore unordered columns */ + /* above an ordered column */ + } while (Col [c].shared2.order == Empty) ; + + /* re-order the super_col parent to largest order for this group */ + Col [parent].shared2.order = order ; + } + } + + /* === Generate the permutation ========================================= */ + + for (c = 0 ; c < n_col ; c++) + { + p [Col [c].shared2.order] = c ; + } +} + + +/* ========================================================================== */ +/* === detect_super_cols ==================================================== */ +/* ========================================================================== */ + +/* + Detects supercolumns by finding matches between columns in the hash buckets. + Check amongst columns in the set A [row_start ... row_start + row_length-1]. + The columns under consideration are currently *not* in the degree lists, + and have already been placed in the hash buckets. + + The hash bucket for columns whose hash function is equal to h is stored + as follows: + + if head [h] is >= 0, then head [h] contains a degree list, so: + + head [h] is the first column in degree bucket h. + Col [head [h]].headhash gives the first column in hash bucket h. + + otherwise, the degree list is empty, and: + + -(head [h] + 2) is the first column in hash bucket h. + + For a column c in a hash bucket, Col [c].shared3.prev is NOT a "previous + column" pointer. Col [c].shared3.hash is used instead as the hash number + for that column. The value of Col [c].shared4.hash_next is the next column + in the same hash bucket. + + Assuming no, or "few" hash collisions, the time taken by this routine is + linear in the sum of the sizes (lengths) of each column whose score has + just been computed in the approximate degree computation. + Not user-callable. +*/ +template +static void detect_super_cols +( + /* === Parameters ======================================================= */ + + ColStructure Col [], /* of size n_col+1 */ + IndexType A [], /* row indices of A */ + IndexType head [], /* head of degree lists and hash buckets */ + IndexType row_start, /* pointer to set of columns to check */ + IndexType row_length /* number of columns to check */ +) +{ + /* === Local variables ================================================== */ + + IndexType hash ; /* hash value for a column */ + IndexType *rp ; /* pointer to a row */ + IndexType c ; /* a column index */ + IndexType super_c ; /* column index of the column to absorb into */ + IndexType *cp1 ; /* column pointer for column super_c */ + IndexType *cp2 ; /* column pointer for column c */ + IndexType length ; /* length of column super_c */ + IndexType prev_c ; /* column preceding c in hash bucket */ + IndexType i ; /* loop counter */ + IndexType *rp_end ; /* pointer to the end of the row */ + IndexType col ; /* a column index in the row to check */ + IndexType head_column ; /* first column in hash bucket or degree list */ + IndexType first_col ; /* first column in hash bucket */ + + /* === Consider each column in the row ================================== */ + + rp = &A [row_start] ; + rp_end = rp + row_length ; + while (rp < rp_end) + { + col = *rp++ ; + if (Col[col].is_dead()) + { + continue ; + } + + /* get hash number for this column */ + hash = Col [col].shared3.hash ; + COLAMD_ASSERT (hash <= n_col) ; + + /* === Get the first column in this hash bucket ===================== */ + + head_column = head [hash] ; + if (head_column > Empty) + { + first_col = Col [head_column].shared3.headhash ; + } + else + { + first_col = - (head_column + 2) ; + } + + /* === Consider each column in the hash bucket ====================== */ + + for (super_c = first_col ; super_c != Empty ; + super_c = Col [super_c].shared4.hash_next) + { + COLAMD_ASSERT (Col [super_c].is_alive()) ; + COLAMD_ASSERT (Col [super_c].shared3.hash == hash) ; + length = Col [super_c].length ; + + /* prev_c is the column preceding column c in the hash bucket */ + prev_c = super_c ; + + /* === Compare super_c with all columns after it ================ */ + + for (c = Col [super_c].shared4.hash_next ; + c != Empty ; c = Col [c].shared4.hash_next) + { + COLAMD_ASSERT (c != super_c) ; + COLAMD_ASSERT (Col[c].is_alive()) ; + COLAMD_ASSERT (Col [c].shared3.hash == hash) ; + + /* not identical if lengths or scores are different */ + if (Col [c].length != length || + Col [c].shared2.score != Col [super_c].shared2.score) + { + prev_c = c ; + continue ; + } + + /* compare the two columns */ + cp1 = &A [Col [super_c].start] ; + cp2 = &A [Col [c].start] ; + + for (i = 0 ; i < length ; i++) + { + /* the columns are "clean" (no dead rows) */ + COLAMD_ASSERT ( cp1->is_alive() ); + COLAMD_ASSERT ( cp2->is_alive() ); + /* row indices will same order for both supercols, */ + /* no gather scatter necessary */ + if (*cp1++ != *cp2++) + { + break ; + } + } + + /* the two columns are different if the for-loop "broke" */ + if (i != length) + { + prev_c = c ; + continue ; + } + + /* === Got it! two columns are identical =================== */ + + COLAMD_ASSERT (Col [c].shared2.score == Col [super_c].shared2.score) ; + + Col [super_c].shared1.thickness += Col [c].shared1.thickness ; + Col [c].shared1.parent = super_c ; + Col[c].kill_non_principal() ; + /* order c later, in order_children() */ + Col [c].shared2.order = Empty ; + /* remove c from hash bucket */ + Col [prev_c].shared4.hash_next = Col [c].shared4.hash_next ; + } + } + + /* === Empty this hash bucket ======================================= */ + + if (head_column > Empty) + { + /* corresponding degree list "hash" is not empty */ + Col [head_column].shared3.headhash = Empty ; + } + else + { + /* corresponding degree list "hash" is empty */ + head [hash] = Empty ; + } + } +} + + +/* ========================================================================== */ +/* === garbage_collection =================================================== */ +/* ========================================================================== */ + +/* + Defragments and compacts columns and rows in the workspace A. Used when + all available memory has been used while performing row merging. Returns + the index of the first free position in A, after garbage collection. The + time taken by this routine is linear is the size of the array A, which is + itself linear in the number of nonzeros in the input matrix. + Not user-callable. +*/ +template +static IndexType garbage_collection /* returns the new value of pfree */ + ( + /* === Parameters ======================================================= */ + + IndexType n_row, /* number of rows */ + IndexType n_col, /* number of columns */ + RowStructure Row [], /* row info */ + ColStructure Col [], /* column info */ + IndexType A [], /* A [0 ... Alen-1] holds the matrix */ + IndexType *pfree /* &A [0] ... pfree is in use */ + ) +{ + /* === Local variables ================================================== */ + + IndexType *psrc ; /* source pointer */ + IndexType *pdest ; /* destination pointer */ + IndexType j ; /* counter */ + IndexType r ; /* a row index */ + IndexType c ; /* a column index */ + IndexType length ; /* length of a row or column */ + + /* === Defragment the columns =========================================== */ + + pdest = &A[0] ; + for (c = 0 ; c < n_col ; c++) + { + if (Col[c].is_alive()) + { + psrc = &A [Col [c].start] ; + + /* move and compact the column */ + COLAMD_ASSERT (pdest <= psrc) ; + Col [c].start = (IndexType) (pdest - &A [0]) ; + length = Col [c].length ; + for (j = 0 ; j < length ; j++) + { + r = *psrc++ ; + if (Row[r].is_alive()) + { + *pdest++ = r ; + } + } + Col [c].length = (IndexType) (pdest - &A [Col [c].start]) ; + } + } + + /* === Prepare to defragment the rows =================================== */ + + for (r = 0 ; r < n_row ; r++) + { + if (Row[r].is_alive()) + { + if (Row [r].length == 0) + { + /* this row is of zero length. cannot compact it, so kill it */ + COLAMD_DEBUG3 (("Defrag row kill\n")) ; + Row[r].kill() ; + } + else + { + /* save first column index in Row [r].shared2.first_column */ + psrc = &A [Row [r].start] ; + Row [r].shared2.first_column = *psrc ; + COLAMD_ASSERT (Row[r].is_alive()) ; + /* flag the start of the row with the one's complement of row */ + *psrc = ones_complement(r) ; + + } + } + } + + /* === Defragment the rows ============================================== */ + + psrc = pdest ; + while (psrc < pfree) + { + /* find a negative number ... the start of a row */ + if (*psrc++ < 0) + { + psrc-- ; + /* get the row index */ + r = ones_complement(*psrc) ; + COLAMD_ASSERT (r >= 0 && r < n_row) ; + /* restore first column index */ + *psrc = Row [r].shared2.first_column ; + COLAMD_ASSERT (Row[r].is_alive()) ; + + /* move and compact the row */ + COLAMD_ASSERT (pdest <= psrc) ; + Row [r].start = (IndexType) (pdest - &A [0]) ; + length = Row [r].length ; + for (j = 0 ; j < length ; j++) + { + c = *psrc++ ; + if (Col[c].is_alive()) + { + *pdest++ = c ; + } + } + Row [r].length = (IndexType) (pdest - &A [Row [r].start]) ; + + } + } + /* ensure we found all the rows */ + COLAMD_ASSERT (debug_rows == 0) ; + + /* === Return the new value of pfree ==================================== */ + + return ((IndexType) (pdest - &A [0])) ; +} + + +/* ========================================================================== */ +/* === clear_mark =========================================================== */ +/* ========================================================================== */ + +/* + Clears the Row [].shared2.mark array, and returns the new tag_mark. + Return value is the new tag_mark. Not user-callable. +*/ +template +static inline IndexType clear_mark /* return the new value for tag_mark */ + ( + /* === Parameters ======================================================= */ + + IndexType n_row, /* number of rows in A */ + RowStructure Row [] /* Row [0 ... n_row-1].shared2.mark is set to zero */ + ) +{ + /* === Local variables ================================================== */ + + IndexType r ; + + for (r = 0 ; r < n_row ; r++) + { + if (Row[r].is_alive()) + { + Row [r].shared2.mark = 0 ; + } + } + return (1) ; +} + +} // namespace Colamd + +} // namespace internal +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h new file mode 100644 index 0000000000..c578970142 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h @@ -0,0 +1,153 @@ + +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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_ORDERING_H +#define EIGEN_ORDERING_H + +namespace Eigen { + +#include "Eigen_Colamd.h" + +namespace internal { + +/** \internal + * \ingroup OrderingMethods_Module + * \param[in] A the input non-symmetric matrix + * \param[out] symmat the symmetric pattern A^T+A from the input matrix \a A. + * FIXME: The values should not be considered here + */ +template +void ordering_helper_at_plus_a(const MatrixType& A, MatrixType& symmat) +{ + MatrixType C; + C = A.transpose(); // NOTE: Could be costly + for (int i = 0; i < C.rows(); i++) + { + for (typename MatrixType::InnerIterator it(C, i); it; ++it) + it.valueRef() = typename MatrixType::Scalar(0); + } + symmat = C + A; +} + +} + +/** \ingroup OrderingMethods_Module + * \class AMDOrdering + * + * Functor computing the \em approximate \em minimum \em degree ordering + * If the matrix is not structurally symmetric, an ordering of A^T+A is computed + * \tparam StorageIndex The type of indices of the matrix + * \sa COLAMDOrdering + */ +template +class AMDOrdering +{ + public: + typedef PermutationMatrix PermutationType; + + /** Compute the permutation vector from a sparse matrix + * This routine is much faster if the input matrix is column-major + */ + template + void operator()(const MatrixType& mat, PermutationType& perm) + { + // Compute the symmetric pattern + SparseMatrix symm; + internal::ordering_helper_at_plus_a(mat,symm); + + // Call the AMD routine + //m_mat.prune(keep_diag()); + internal::minimum_degree_ordering(symm, perm); + } + + /** Compute the permutation with a selfadjoint matrix */ + template + void operator()(const SparseSelfAdjointView& mat, PermutationType& perm) + { + SparseMatrix C; C = mat; + + // Call the AMD routine + // m_mat.prune(keep_diag()); //Remove the diagonal elements + internal::minimum_degree_ordering(C, perm); + } +}; + +/** \ingroup OrderingMethods_Module + * \class NaturalOrdering + * + * Functor computing the natural ordering (identity) + * + * \note Returns an empty permutation matrix + * \tparam StorageIndex The type of indices of the matrix + */ +template +class NaturalOrdering +{ + public: + typedef PermutationMatrix PermutationType; + + /** Compute the permutation vector from a column-major sparse matrix */ + template + void operator()(const MatrixType& /*mat*/, PermutationType& perm) + { + perm.resize(0); + } + +}; + +/** \ingroup OrderingMethods_Module + * \class COLAMDOrdering + * + * \tparam StorageIndex The type of indices of the matrix + * + * Functor computing the \em column \em approximate \em minimum \em degree ordering + * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()). + */ +template +class COLAMDOrdering +{ + public: + typedef PermutationMatrix PermutationType; + typedef Matrix IndexVector; + + /** Compute the permutation vector \a perm form the sparse matrix \a mat + * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). + */ + template + void operator() (const MatrixType& mat, PermutationType& perm) + { + eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering"); + + StorageIndex m = StorageIndex(mat.rows()); + StorageIndex n = StorageIndex(mat.cols()); + StorageIndex nnz = StorageIndex(mat.nonZeros()); + // Get the recommended value of Alen to be used by colamd + StorageIndex Alen = internal::Colamd::recommended(nnz, m, n); + // Set the default parameters + double knobs [internal::Colamd::NKnobs]; + StorageIndex stats [internal::Colamd::NStats]; + internal::Colamd::set_defaults(knobs); + + IndexVector p(n+1), A(Alen); + for(StorageIndex i=0; i <= n; i++) p(i) = mat.outerIndexPtr()[i]; + for(StorageIndex i=0; i < nnz; i++) A(i) = mat.innerIndexPtr()[i]; + // Call Colamd routine to compute the ordering + StorageIndex info = internal::Colamd::compute_ordering(m, n, Alen, A.data(), p.data(), knobs, stats); + EIGEN_UNUSED_VARIABLE(info); + eigen_assert( info && "COLAMD failed " ); + + perm.resize(n); + for (StorageIndex i = 0; i < n; i++) perm.indices()(p(i)) = i; + } +}; + +} // end namespace Eigen + +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h new file mode 100644 index 0000000000..9f93e3255d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -0,0 +1,697 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2012 Gael Guennebaud +// +// 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_SIMPLICIAL_CHOLESKY_H +#define EIGEN_SIMPLICIAL_CHOLESKY_H + +namespace Eigen { + +enum SimplicialCholeskyMode { + SimplicialCholeskyLLT, + SimplicialCholeskyLDLT +}; + +namespace internal { + template + struct simplicial_cholesky_grab_input { + typedef CholMatrixType const * ConstCholMatrixPtr; + static void run(const InputMatrixType& input, ConstCholMatrixPtr &pmat, CholMatrixType &tmp) + { + tmp = input; + pmat = &tmp; + } + }; + + template + struct simplicial_cholesky_grab_input { + typedef MatrixType const * ConstMatrixPtr; + static void run(const MatrixType& input, ConstMatrixPtr &pmat, MatrixType &/*tmp*/) + { + pmat = &input; + } + }; +} // end namespace internal + +/** \ingroup SparseCholesky_Module + * \brief A base class for direct sparse Cholesky factorizations + * + * This is a base class for LL^T and LDL^T Cholesky factorizations of sparse matrices that are + * selfadjoint and positive definite. These factorizations allow for solving A.X = B where + * X and B can be either dense or sparse. + * + * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization + * such that the factorized matrix is P A P^-1. + * + * \tparam Derived the type of the derived class, that is the actual factorization type. + * + */ +template +class SimplicialCholeskyBase : public SparseSolverBase +{ + typedef SparseSolverBase Base; + using Base::m_isInitialized; + + public: + typedef typename internal::traits::MatrixType MatrixType; + typedef typename internal::traits::OrderingType OrderingType; + enum { UpLo = internal::traits::UpLo }; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix CholMatrixType; + typedef CholMatrixType const * ConstCholMatrixPtr; + typedef Matrix VectorType; + typedef Matrix VectorI; + + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + public: + + using Base::derived; + + /** Default constructor */ + SimplicialCholeskyBase() + : m_info(Success), + m_factorizationIsOk(false), + m_analysisIsOk(false), + m_shiftOffset(0), + m_shiftScale(1) + {} + + explicit SimplicialCholeskyBase(const MatrixType& matrix) + : m_info(Success), + m_factorizationIsOk(false), + m_analysisIsOk(false), + m_shiftOffset(0), + m_shiftScale(1) + { + derived().compute(matrix); + } + + ~SimplicialCholeskyBase() + { + } + + Derived& derived() { return *static_cast(this); } + const Derived& derived() const { return *static_cast(this); } + + inline Index cols() const { return m_matrix.cols(); } + inline Index rows() const { return m_matrix.rows(); } + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the matrix.appears to be negative. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "Decomposition is not initialized."); + return m_info; + } + + /** \returns the permutation P + * \sa permutationPinv() */ + const PermutationMatrix& permutationP() const + { return m_P; } + + /** \returns the inverse P^-1 of the permutation P + * \sa permutationP() */ + const PermutationMatrix& permutationPinv() const + { return m_Pinv; } + + /** Sets the shift parameters that will be used to adjust the diagonal coefficients during the numerical factorization. + * + * During the numerical factorization, the diagonal coefficients are transformed by the following linear model:\n + * \c d_ii = \a offset + \a scale * \c d_ii + * + * The default is the identity transformation with \a offset=0, and \a scale=1. + * + * \returns a reference to \c *this. + */ + Derived& setShift(const RealScalar& offset, const RealScalar& scale = 1) + { + m_shiftOffset = offset; + m_shiftScale = scale; + return derived(); + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal */ + template + void dumpMemory(Stream& s) + { + int total = 0; + s << " L: " << ((total+=(m_matrix.cols()+1) * sizeof(int) + m_matrix.nonZeros()*(sizeof(int)+sizeof(Scalar))) >> 20) << "Mb" << "\n"; + s << " diag: " << ((total+=m_diag.size() * sizeof(Scalar)) >> 20) << "Mb" << "\n"; + s << " tree: " << ((total+=m_parent.size() * sizeof(int)) >> 20) << "Mb" << "\n"; + s << " nonzeros: " << ((total+=m_nonZerosPerCol.size() * sizeof(int)) >> 20) << "Mb" << "\n"; + s << " perm: " << ((total+=m_P.size() * sizeof(int)) >> 20) << "Mb" << "\n"; + s << " perm^-1: " << ((total+=m_Pinv.size() * sizeof(int)) >> 20) << "Mb" << "\n"; + s << " TOTAL: " << (total>> 20) << "Mb" << "\n"; + } + + /** \internal */ + template + void _solve_impl(const MatrixBase &b, MatrixBase &dest) const + { + eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); + eigen_assert(m_matrix.rows()==b.rows()); + + if(m_info!=Success) + return; + + if(m_P.size()>0) + dest = m_P * b; + else + dest = b; + + if(m_matrix.nonZeros()>0) // otherwise L==I + derived().matrixL().solveInPlace(dest); + + if(m_diag.size()>0) + dest = m_diag.asDiagonal().inverse() * dest; + + if (m_matrix.nonZeros()>0) // otherwise U==I + derived().matrixU().solveInPlace(dest); + + if(m_P.size()>0) + dest = m_Pinv * dest; + } + + template + void _solve_impl(const SparseMatrixBase &b, SparseMatrixBase &dest) const + { + internal::solve_sparse_through_dense_panels(derived(), b, dest); + } + +#endif // EIGEN_PARSED_BY_DOXYGEN + + protected: + + /** Computes the sparse Cholesky decomposition of \a matrix */ + template + void compute(const MatrixType& matrix) + { + eigen_assert(matrix.rows()==matrix.cols()); + Index size = matrix.cols(); + CholMatrixType tmp(size,size); + ConstCholMatrixPtr pmat; + ordering(matrix, pmat, tmp); + analyzePattern_preordered(*pmat, DoLDLT); + factorize_preordered(*pmat); + } + + template + void factorize(const MatrixType& a) + { + eigen_assert(a.rows()==a.cols()); + Index size = a.cols(); + CholMatrixType tmp(size,size); + ConstCholMatrixPtr pmat; + + if(m_P.size() == 0 && (int(UpLo) & int(Upper)) == Upper) + { + // If there is no ordering, try to directly use the input matrix without any copy + internal::simplicial_cholesky_grab_input::run(a, pmat, tmp); + } + else + { + tmp.template selfadjointView() = a.template selfadjointView().twistedBy(m_P); + pmat = &tmp; + } + + factorize_preordered(*pmat); + } + + template + void factorize_preordered(const CholMatrixType& a); + + void analyzePattern(const MatrixType& a, bool doLDLT) + { + eigen_assert(a.rows()==a.cols()); + Index size = a.cols(); + CholMatrixType tmp(size,size); + ConstCholMatrixPtr pmat; + ordering(a, pmat, tmp); + analyzePattern_preordered(*pmat,doLDLT); + } + void analyzePattern_preordered(const CholMatrixType& a, bool doLDLT); + + void ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap); + + /** keeps off-diagonal entries; drops diagonal entries */ + struct keep_diag { + inline bool operator() (const Index& row, const Index& col, const Scalar&) const + { + return row!=col; + } + }; + + mutable ComputationInfo m_info; + bool m_factorizationIsOk; + bool m_analysisIsOk; + + CholMatrixType m_matrix; + VectorType m_diag; // the diagonal coefficients (LDLT mode) + VectorI m_parent; // elimination tree + VectorI m_nonZerosPerCol; + PermutationMatrix m_P; // the permutation + PermutationMatrix m_Pinv; // the inverse permutation + + RealScalar m_shiftOffset; + RealScalar m_shiftScale; +}; + +template > class SimplicialLLT; +template > class SimplicialLDLT; +template > class SimplicialCholesky; + +namespace internal { + +template struct traits > +{ + typedef _MatrixType MatrixType; + typedef _Ordering OrderingType; + enum { UpLo = _UpLo }; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix CholMatrixType; + typedef TriangularView MatrixL; + typedef TriangularView MatrixU; + static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); } + static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); } +}; + +template struct traits > +{ + typedef _MatrixType MatrixType; + typedef _Ordering OrderingType; + enum { UpLo = _UpLo }; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix CholMatrixType; + typedef TriangularView MatrixL; + typedef TriangularView MatrixU; + static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); } + static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); } +}; + +template struct traits > +{ + typedef _MatrixType MatrixType; + typedef _Ordering OrderingType; + enum { UpLo = _UpLo }; +}; + +} + +/** \ingroup SparseCholesky_Module + * \class SimplicialLLT + * \brief A direct sparse LLT Cholesky factorizations + * + * This class provides a LL^T Cholesky factorizations of sparse matrices that are + * selfadjoint and positive definite. The factorization allows for solving A.X = B where + * X and B can be either dense or sparse. + * + * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization + * such that the factorized matrix is P A P^-1. + * + * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower + * or Upper. Default is Lower. + * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<> + * + * \implsparsesolverconcept + * + * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering + */ +template + class SimplicialLLT : public SimplicialCholeskyBase > +{ +public: + typedef _MatrixType MatrixType; + enum { UpLo = _UpLo }; + typedef SimplicialCholeskyBase Base; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix CholMatrixType; + typedef Matrix VectorType; + typedef internal::traits Traits; + typedef typename Traits::MatrixL MatrixL; + typedef typename Traits::MatrixU MatrixU; +public: + /** Default constructor */ + SimplicialLLT() : Base() {} + /** Constructs and performs the LLT factorization of \a matrix */ + explicit SimplicialLLT(const MatrixType& matrix) + : Base(matrix) {} + + /** \returns an expression of the factor L */ + inline const MatrixL matrixL() const { + eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized"); + return Traits::getL(Base::m_matrix); + } + + /** \returns an expression of the factor U (= L^*) */ + inline const MatrixU matrixU() const { + eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized"); + return Traits::getU(Base::m_matrix); + } + + /** Computes the sparse Cholesky decomposition of \a matrix */ + SimplicialLLT& compute(const MatrixType& matrix) + { + Base::template compute(matrix); + return *this; + } + + /** Performs a symbolic decomposition on the sparcity of \a matrix. + * + * This function is particularly useful when solving for several problems having the same structure. + * + * \sa factorize() + */ + void analyzePattern(const MatrixType& a) + { + Base::analyzePattern(a, false); + } + + /** Performs a numeric decomposition of \a matrix + * + * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed. + * + * \sa analyzePattern() + */ + void factorize(const MatrixType& a) + { + Base::template factorize(a); + } + + /** \returns the determinant of the underlying matrix from the current factorization */ + Scalar determinant() const + { + Scalar detL = Base::m_matrix.diagonal().prod(); + return numext::abs2(detL); + } +}; + +/** \ingroup SparseCholesky_Module + * \class SimplicialLDLT + * \brief A direct sparse LDLT Cholesky factorizations without square root. + * + * This class provides a LDL^T Cholesky factorizations without square root of sparse matrices that are + * selfadjoint and positive definite. The factorization allows for solving A.X = B where + * X and B can be either dense or sparse. + * + * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization + * such that the factorized matrix is P A P^-1. + * + * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower + * or Upper. Default is Lower. + * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<> + * + * \implsparsesolverconcept + * + * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering + */ +template + class SimplicialLDLT : public SimplicialCholeskyBase > +{ +public: + typedef _MatrixType MatrixType; + enum { UpLo = _UpLo }; + typedef SimplicialCholeskyBase Base; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix CholMatrixType; + typedef Matrix VectorType; + typedef internal::traits Traits; + typedef typename Traits::MatrixL MatrixL; + typedef typename Traits::MatrixU MatrixU; +public: + /** Default constructor */ + SimplicialLDLT() : Base() {} + + /** Constructs and performs the LLT factorization of \a matrix */ + explicit SimplicialLDLT(const MatrixType& matrix) + : Base(matrix) {} + + /** \returns a vector expression of the diagonal D */ + inline const VectorType vectorD() const { + eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized"); + return Base::m_diag; + } + /** \returns an expression of the factor L */ + inline const MatrixL matrixL() const { + eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized"); + return Traits::getL(Base::m_matrix); + } + + /** \returns an expression of the factor U (= L^*) */ + inline const MatrixU matrixU() const { + eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized"); + return Traits::getU(Base::m_matrix); + } + + /** Computes the sparse Cholesky decomposition of \a matrix */ + SimplicialLDLT& compute(const MatrixType& matrix) + { + Base::template compute(matrix); + return *this; + } + + /** Performs a symbolic decomposition on the sparcity of \a matrix. + * + * This function is particularly useful when solving for several problems having the same structure. + * + * \sa factorize() + */ + void analyzePattern(const MatrixType& a) + { + Base::analyzePattern(a, true); + } + + /** Performs a numeric decomposition of \a matrix + * + * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed. + * + * \sa analyzePattern() + */ + void factorize(const MatrixType& a) + { + Base::template factorize(a); + } + + /** \returns the determinant of the underlying matrix from the current factorization */ + Scalar determinant() const + { + return Base::m_diag.prod(); + } +}; + +/** \deprecated use SimplicialLDLT or class SimplicialLLT + * \ingroup SparseCholesky_Module + * \class SimplicialCholesky + * + * \sa class SimplicialLDLT, class SimplicialLLT + */ +template + class SimplicialCholesky : public SimplicialCholeskyBase > +{ +public: + typedef _MatrixType MatrixType; + enum { UpLo = _UpLo }; + typedef SimplicialCholeskyBase Base; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix CholMatrixType; + typedef Matrix VectorType; + typedef internal::traits Traits; + typedef internal::traits > LDLTTraits; + typedef internal::traits > LLTTraits; + public: + SimplicialCholesky() : Base(), m_LDLT(true) {} + + explicit SimplicialCholesky(const MatrixType& matrix) + : Base(), m_LDLT(true) + { + compute(matrix); + } + + SimplicialCholesky& setMode(SimplicialCholeskyMode mode) + { + switch(mode) + { + case SimplicialCholeskyLLT: + m_LDLT = false; + break; + case SimplicialCholeskyLDLT: + m_LDLT = true; + break; + default: + break; + } + + return *this; + } + + inline const VectorType vectorD() const { + eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized"); + return Base::m_diag; + } + inline const CholMatrixType rawMatrix() const { + eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized"); + return Base::m_matrix; + } + + /** Computes the sparse Cholesky decomposition of \a matrix */ + SimplicialCholesky& compute(const MatrixType& matrix) + { + if(m_LDLT) + Base::template compute(matrix); + else + Base::template compute(matrix); + return *this; + } + + /** Performs a symbolic decomposition on the sparcity of \a matrix. + * + * This function is particularly useful when solving for several problems having the same structure. + * + * \sa factorize() + */ + void analyzePattern(const MatrixType& a) + { + Base::analyzePattern(a, m_LDLT); + } + + /** Performs a numeric decomposition of \a matrix + * + * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed. + * + * \sa analyzePattern() + */ + void factorize(const MatrixType& a) + { + if(m_LDLT) + Base::template factorize(a); + else + Base::template factorize(a); + } + + /** \internal */ + template + void _solve_impl(const MatrixBase &b, MatrixBase &dest) const + { + eigen_assert(Base::m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); + eigen_assert(Base::m_matrix.rows()==b.rows()); + + if(Base::m_info!=Success) + return; + + if(Base::m_P.size()>0) + dest = Base::m_P * b; + else + dest = b; + + if(Base::m_matrix.nonZeros()>0) // otherwise L==I + { + if(m_LDLT) + LDLTTraits::getL(Base::m_matrix).solveInPlace(dest); + else + LLTTraits::getL(Base::m_matrix).solveInPlace(dest); + } + + if(Base::m_diag.size()>0) + dest = Base::m_diag.real().asDiagonal().inverse() * dest; + + if (Base::m_matrix.nonZeros()>0) // otherwise I==I + { + if(m_LDLT) + LDLTTraits::getU(Base::m_matrix).solveInPlace(dest); + else + LLTTraits::getU(Base::m_matrix).solveInPlace(dest); + } + + if(Base::m_P.size()>0) + dest = Base::m_Pinv * dest; + } + + /** \internal */ + template + void _solve_impl(const SparseMatrixBase &b, SparseMatrixBase &dest) const + { + internal::solve_sparse_through_dense_panels(*this, b, dest); + } + + Scalar determinant() const + { + if(m_LDLT) + { + return Base::m_diag.prod(); + } + else + { + Scalar detL = Diagonal(Base::m_matrix).prod(); + return numext::abs2(detL); + } + } + + protected: + bool m_LDLT; +}; + +template +void SimplicialCholeskyBase::ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap) +{ + eigen_assert(a.rows()==a.cols()); + const Index size = a.rows(); + pmat = ≈ + // Note that ordering methods compute the inverse permutation + if(!internal::is_same >::value) + { + { + CholMatrixType C; + C = a.template selfadjointView(); + + OrderingType ordering; + ordering(C,m_Pinv); + } + + if(m_Pinv.size()>0) m_P = m_Pinv.inverse(); + else m_P.resize(0); + + ap.resize(size,size); + ap.template selfadjointView() = a.template selfadjointView().twistedBy(m_P); + } + else + { + m_Pinv.resize(0); + m_P.resize(0); + if(int(UpLo)==int(Lower) || MatrixType::IsRowMajor) + { + // we have to transpose the lower part to to the upper one + ap.resize(size,size); + ap.template selfadjointView() = a.template selfadjointView(); + } + else + internal::simplicial_cholesky_grab_input::run(a, pmat, ap); + } +} + +} // end namespace Eigen + +#endif // EIGEN_SIMPLICIAL_CHOLESKY_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h new file mode 100644 index 0000000000..72e1740c19 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h @@ -0,0 +1,174 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2012 Gael Guennebaud +// +// 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/. + +/* +NOTE: these functions have been adapted from the LDL library: + +LDL Copyright (c) 2005 by Timothy A. Davis. All Rights Reserved. + +The author of LDL, Timothy A. Davis., has executed a license with Google LLC +to permit distribution of this code and derivative works as part of Eigen under +the Mozilla Public License v. 2.0, as stated at the top of this file. + */ + +#ifndef EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H +#define EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H + +namespace Eigen { + +template +void SimplicialCholeskyBase::analyzePattern_preordered(const CholMatrixType& ap, bool doLDLT) +{ + const StorageIndex size = StorageIndex(ap.rows()); + m_matrix.resize(size, size); + m_parent.resize(size); + m_nonZerosPerCol.resize(size); + + ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0); + + for(StorageIndex k = 0; k < size; ++k) + { + /* L(k,:) pattern: all nodes reachable in etree from nz in A(0:k-1,k) */ + m_parent[k] = -1; /* parent of k is not yet known */ + tags[k] = k; /* mark node k as visited */ + m_nonZerosPerCol[k] = 0; /* count of nonzeros in column k of L */ + for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it) + { + StorageIndex i = it.index(); + if(i < k) + { + /* follow path from i to root of etree, stop at flagged node */ + for(; tags[i] != k; i = m_parent[i]) + { + /* find parent of i if not yet determined */ + if (m_parent[i] == -1) + m_parent[i] = k; + m_nonZerosPerCol[i]++; /* L (k,i) is nonzero */ + tags[i] = k; /* mark i as visited */ + } + } + } + } + + /* construct Lp index array from m_nonZerosPerCol column counts */ + StorageIndex* Lp = m_matrix.outerIndexPtr(); + Lp[0] = 0; + for(StorageIndex k = 0; k < size; ++k) + Lp[k+1] = Lp[k] + m_nonZerosPerCol[k] + (doLDLT ? 0 : 1); + + m_matrix.resizeNonZeros(Lp[size]); + + m_isInitialized = true; + m_info = Success; + m_analysisIsOk = true; + m_factorizationIsOk = false; +} + + +template +template +void SimplicialCholeskyBase::factorize_preordered(const CholMatrixType& ap) +{ + using std::sqrt; + + eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); + eigen_assert(ap.rows()==ap.cols()); + eigen_assert(m_parent.size()==ap.rows()); + eigen_assert(m_nonZerosPerCol.size()==ap.rows()); + + const StorageIndex size = StorageIndex(ap.rows()); + const StorageIndex* Lp = m_matrix.outerIndexPtr(); + StorageIndex* Li = m_matrix.innerIndexPtr(); + Scalar* Lx = m_matrix.valuePtr(); + + ei_declare_aligned_stack_constructed_variable(Scalar, y, size, 0); + ei_declare_aligned_stack_constructed_variable(StorageIndex, pattern, size, 0); + ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0); + + bool ok = true; + m_diag.resize(DoLDLT ? size : 0); + + for(StorageIndex k = 0; k < size; ++k) + { + // compute nonzero pattern of kth row of L, in topological order + y[k] = Scalar(0); // Y(0:k) is now all zero + StorageIndex top = size; // stack for pattern is empty + tags[k] = k; // mark node k as visited + m_nonZerosPerCol[k] = 0; // count of nonzeros in column k of L + for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it) + { + StorageIndex i = it.index(); + if(i <= k) + { + y[i] += numext::conj(it.value()); /* scatter A(i,k) into Y (sum duplicates) */ + Index len; + for(len = 0; tags[i] != k; i = m_parent[i]) + { + pattern[len++] = i; /* L(k,i) is nonzero */ + tags[i] = k; /* mark i as visited */ + } + while(len > 0) + pattern[--top] = pattern[--len]; + } + } + + /* compute numerical values kth row of L (a sparse triangular solve) */ + + RealScalar d = numext::real(y[k]) * m_shiftScale + m_shiftOffset; // get D(k,k), apply the shift function, and clear Y(k) + y[k] = Scalar(0); + for(; top < size; ++top) + { + Index i = pattern[top]; /* pattern[top:n-1] is pattern of L(:,k) */ + Scalar yi = y[i]; /* get and clear Y(i) */ + y[i] = Scalar(0); + + /* the nonzero entry L(k,i) */ + Scalar l_ki; + if(DoLDLT) + l_ki = yi / numext::real(m_diag[i]); + else + yi = l_ki = yi / Lx[Lp[i]]; + + Index p2 = Lp[i] + m_nonZerosPerCol[i]; + Index p; + for(p = Lp[i] + (DoLDLT ? 0 : 1); p < p2; ++p) + y[Li[p]] -= numext::conj(Lx[p]) * yi; + d -= numext::real(l_ki * numext::conj(yi)); + Li[p] = k; /* store L(k,i) in column form of L */ + Lx[p] = l_ki; + ++m_nonZerosPerCol[i]; /* increment count of nonzeros in col i */ + } + if(DoLDLT) + { + m_diag[k] = d; + if(d == RealScalar(0)) + { + ok = false; /* failure, D(k,k) is zero */ + break; + } + } + else + { + Index p = Lp[k] + m_nonZerosPerCol[k]++; + Li[p] = k ; /* store L(k,k) = sqrt (d) in column k */ + if(d <= RealScalar(0)) { + ok = false; /* failure, matrix is not positive definite */ + break; + } + Lx[p] = sqrt(d) ; + } + } + + m_info = ok ? Success : NumericalIssue; + m_factorizationIsOk = true; +} + +} // end namespace Eigen + +#endif // EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h new file mode 100644 index 0000000000..2cb7747cc9 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h @@ -0,0 +1,378 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// 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_AMBIVECTOR_H +#define EIGEN_AMBIVECTOR_H + +namespace Eigen { + +namespace internal { + +/** \internal + * Hybrid sparse/dense vector class designed for intensive read-write operations. + * + * See BasicSparseLLT and SparseProduct for usage examples. + */ +template +class AmbiVector +{ + public: + typedef _Scalar Scalar; + typedef _StorageIndex StorageIndex; + typedef typename NumTraits::Real RealScalar; + + explicit AmbiVector(Index size) + : m_buffer(0), m_zero(0), m_size(0), m_end(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1) + { + resize(size); + } + + void init(double estimatedDensity); + void init(int mode); + + Index nonZeros() const; + + /** Specifies a sub-vector to work on */ + void setBounds(Index start, Index end) { m_start = convert_index(start); m_end = convert_index(end); } + + void setZero(); + + void restart(); + Scalar& coeffRef(Index i); + Scalar& coeff(Index i); + + class Iterator; + + ~AmbiVector() { delete[] m_buffer; } + + void resize(Index size) + { + if (m_allocatedSize < size) + reallocate(size); + m_size = convert_index(size); + } + + StorageIndex size() const { return m_size; } + + protected: + StorageIndex convert_index(Index idx) + { + return internal::convert_index(idx); + } + + void reallocate(Index size) + { + // if the size of the matrix is not too large, let's allocate a bit more than needed such + // that we can handle dense vector even in sparse mode. + delete[] m_buffer; + if (size<1000) + { + Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar); + m_allocatedElements = convert_index((allocSize*sizeof(Scalar))/sizeof(ListEl)); + m_buffer = new Scalar[allocSize]; + } + else + { + m_allocatedElements = convert_index((size*sizeof(Scalar))/sizeof(ListEl)); + m_buffer = new Scalar[size]; + } + m_size = convert_index(size); + m_start = 0; + m_end = m_size; + } + + void reallocateSparse() + { + Index copyElements = m_allocatedElements; + m_allocatedElements = (std::min)(StorageIndex(m_allocatedElements*1.5),m_size); + Index allocSize = m_allocatedElements * sizeof(ListEl); + allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar); + Scalar* newBuffer = new Scalar[allocSize]; + std::memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl)); + delete[] m_buffer; + m_buffer = newBuffer; + } + + protected: + // element type of the linked list + struct ListEl + { + StorageIndex next; + StorageIndex index; + Scalar value; + }; + + // used to store data in both mode + Scalar* m_buffer; + Scalar m_zero; + StorageIndex m_size; + StorageIndex m_start; + StorageIndex m_end; + StorageIndex m_allocatedSize; + StorageIndex m_allocatedElements; + StorageIndex m_mode; + + // linked list mode + StorageIndex m_llStart; + StorageIndex m_llCurrent; + StorageIndex m_llSize; +}; + +/** \returns the number of non zeros in the current sub vector */ +template +Index AmbiVector<_Scalar,_StorageIndex>::nonZeros() const +{ + if (m_mode==IsSparse) + return m_llSize; + else + return m_end - m_start; +} + +template +void AmbiVector<_Scalar,_StorageIndex>::init(double estimatedDensity) +{ + if (estimatedDensity>0.1) + init(IsDense); + else + init(IsSparse); +} + +template +void AmbiVector<_Scalar,_StorageIndex>::init(int mode) +{ + m_mode = mode; + // This is only necessary in sparse mode, but we set these unconditionally to avoid some maybe-uninitialized warnings + // if (m_mode==IsSparse) + { + m_llSize = 0; + m_llStart = -1; + } +} + +/** Must be called whenever we might perform a write access + * with an index smaller than the previous one. + * + * Don't worry, this function is extremely cheap. + */ +template +void AmbiVector<_Scalar,_StorageIndex>::restart() +{ + m_llCurrent = m_llStart; +} + +/** Set all coefficients of current subvector to zero */ +template +void AmbiVector<_Scalar,_StorageIndex>::setZero() +{ + if (m_mode==IsDense) + { + for (Index i=m_start; i +_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeffRef(Index i) +{ + if (m_mode==IsDense) + return m_buffer[i]; + else + { + ListEl* EIGEN_RESTRICT llElements = reinterpret_cast(m_buffer); + // TODO factorize the following code to reduce code generation + eigen_assert(m_mode==IsSparse); + if (m_llSize==0) + { + // this is the first element + m_llStart = 0; + m_llCurrent = 0; + ++m_llSize; + llElements[0].value = Scalar(0); + llElements[0].index = convert_index(i); + llElements[0].next = -1; + return llElements[0].value; + } + else if (i=llElements[m_llCurrent].index && "you must call restart() before inserting an element with lower or equal index"); + while (nextel >= 0 && llElements[nextel].index<=i) + { + m_llCurrent = nextel; + nextel = llElements[nextel].next; + } + + if (llElements[m_llCurrent].index==i) + { + // the coefficient already exists and we found it ! + return llElements[m_llCurrent].value; + } + else + { + if (m_llSize>=m_allocatedElements) + { + reallocateSparse(); + llElements = reinterpret_cast(m_buffer); + } + eigen_internal_assert(m_llSize +_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeff(Index i) +{ + if (m_mode==IsDense) + return m_buffer[i]; + else + { + ListEl* EIGEN_RESTRICT llElements = reinterpret_cast(m_buffer); + eigen_assert(m_mode==IsSparse); + if ((m_llSize==0) || (i= 0 && llElements[elid].index +class AmbiVector<_Scalar,_StorageIndex>::Iterator +{ + public: + typedef _Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + + /** Default constructor + * \param vec the vector on which we iterate + * \param epsilon the minimal value used to prune zero coefficients. + * In practice, all coefficients having a magnitude smaller than \a epsilon + * are skipped. + */ + explicit Iterator(const AmbiVector& vec, const RealScalar& epsilon = 0) + : m_vector(vec) + { + using std::abs; + m_epsilon = epsilon; + m_isDense = m_vector.m_mode==IsDense; + if (m_isDense) + { + m_currentEl = 0; // this is to avoid a compilation warning + m_cachedValue = 0; // this is to avoid a compilation warning + m_cachedIndex = m_vector.m_start-1; + ++(*this); + } + else + { + ListEl* EIGEN_RESTRICT llElements = reinterpret_cast(m_vector.m_buffer); + m_currentEl = m_vector.m_llStart; + while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon) + m_currentEl = llElements[m_currentEl].next; + if (m_currentEl<0) + { + m_cachedValue = 0; // this is to avoid a compilation warning + m_cachedIndex = -1; + } + else + { + m_cachedIndex = llElements[m_currentEl].index; + m_cachedValue = llElements[m_currentEl].value; + } + } + } + + StorageIndex index() const { return m_cachedIndex; } + Scalar value() const { return m_cachedValue; } + + operator bool() const { return m_cachedIndex>=0; } + + Iterator& operator++() + { + using std::abs; + if (m_isDense) + { + do { + ++m_cachedIndex; + } while (m_cachedIndex(m_vector.m_buffer); + do { + m_currentEl = llElements[m_currentEl].next; + } while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon); + if (m_currentEl<0) + { + m_cachedIndex = -1; + } + else + { + m_cachedIndex = llElements[m_currentEl].index; + m_cachedValue = llElements[m_currentEl].value; + } + } + return *this; + } + + protected: + const AmbiVector& m_vector; // the target vector + StorageIndex m_currentEl; // the current element in sparse/linked-list mode + RealScalar m_epsilon; // epsilon used to prune zero coefficients + StorageIndex m_cachedIndex; // current coordinate + Scalar m_cachedValue; // current value + bool m_isDense; // mode of the vector +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_AMBIVECTOR_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h new file mode 100644 index 0000000000..acd986fab5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h @@ -0,0 +1,274 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_COMPRESSED_STORAGE_H +#define EIGEN_COMPRESSED_STORAGE_H + +namespace Eigen { + +namespace internal { + +/** \internal + * Stores a sparse set of values as a list of values and a list of indices. + * + */ +template +class CompressedStorage +{ + public: + + typedef _Scalar Scalar; + typedef _StorageIndex StorageIndex; + + protected: + + typedef typename NumTraits::Real RealScalar; + + public: + + CompressedStorage() + : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0) + {} + + explicit CompressedStorage(Index size) + : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0) + { + resize(size); + } + + CompressedStorage(const CompressedStorage& other) + : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0) + { + *this = other; + } + + CompressedStorage& operator=(const CompressedStorage& other) + { + resize(other.size()); + if(other.size()>0) + { + internal::smart_copy(other.m_values, other.m_values + m_size, m_values); + internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices); + } + return *this; + } + + void swap(CompressedStorage& other) + { + std::swap(m_values, other.m_values); + std::swap(m_indices, other.m_indices); + std::swap(m_size, other.m_size); + std::swap(m_allocatedSize, other.m_allocatedSize); + } + + ~CompressedStorage() + { + delete[] m_values; + delete[] m_indices; + } + + void reserve(Index size) + { + Index newAllocatedSize = m_size + size; + if (newAllocatedSize > m_allocatedSize) + reallocate(newAllocatedSize); + } + + void squeeze() + { + if (m_allocatedSize>m_size) + reallocate(m_size); + } + + void resize(Index size, double reserveSizeFactor = 0) + { + if (m_allocatedSize)(NumTraits::highest(), size + Index(reserveSizeFactor*double(size))); + if(realloc_size(i); + } + + inline Index size() const { return m_size; } + inline Index allocatedSize() const { return m_allocatedSize; } + inline void clear() { m_size = 0; } + + const Scalar* valuePtr() const { return m_values; } + Scalar* valuePtr() { return m_values; } + const StorageIndex* indexPtr() const { return m_indices; } + StorageIndex* indexPtr() { return m_indices; } + + inline Scalar& value(Index i) { eigen_internal_assert(m_values!=0); return m_values[i]; } + inline const Scalar& value(Index i) const { eigen_internal_assert(m_values!=0); return m_values[i]; } + + inline StorageIndex& index(Index i) { eigen_internal_assert(m_indices!=0); return m_indices[i]; } + inline const StorageIndex& index(Index i) const { eigen_internal_assert(m_indices!=0); return m_indices[i]; } + + /** \returns the largest \c k such that for all \c j in [0,k) index[\c j]\<\a key */ + inline Index searchLowerIndex(Index key) const + { + return searchLowerIndex(0, m_size, key); + } + + /** \returns the largest \c k in [start,end) such that for all \c j in [start,k) index[\c j]\<\a key */ + inline Index searchLowerIndex(Index start, Index end, Index key) const + { + while(end>start) + { + Index mid = (end+start)>>1; + if (m_indices[mid]=end) + return defaultValue; + else if (end>start && key==m_indices[end-1]) + return m_values[end-1]; + // ^^ optimization: let's first check if it is the last coefficient + // (very common in high level algorithms) + const Index id = searchLowerIndex(start,end-1,key); + return ((id=m_size || m_indices[id]!=key) + { + if (m_allocatedSize newValues(m_allocatedSize); + internal::scoped_array newIndices(m_allocatedSize); + + // copy first chunk + internal::smart_copy(m_values, m_values +id, newValues.ptr()); + internal::smart_copy(m_indices, m_indices+id, newIndices.ptr()); + + // copy the rest + if(m_size>id) + { + internal::smart_copy(m_values +id, m_values +m_size, newValues.ptr() +id+1); + internal::smart_copy(m_indices+id, m_indices+m_size, newIndices.ptr()+id+1); + } + std::swap(m_values,newValues.ptr()); + std::swap(m_indices,newIndices.ptr()); + } + else if(m_size>id) + { + internal::smart_memmove(m_values +id, m_values +m_size, m_values +id+1); + internal::smart_memmove(m_indices+id, m_indices+m_size, m_indices+id+1); + } + m_size++; + m_indices[id] = internal::convert_index(key); + m_values[id] = defaultValue; + } + return m_values[id]; + } + + void moveChunk(Index from, Index to, Index chunkSize) + { + eigen_internal_assert(to+chunkSize <= m_size); + if(to>from && from+chunkSize>to) + { + // move backward + internal::smart_memmove(m_values+from, m_values+from+chunkSize, m_values+to); + internal::smart_memmove(m_indices+from, m_indices+from+chunkSize, m_indices+to); + } + else + { + internal::smart_copy(m_values+from, m_values+from+chunkSize, m_values+to); + internal::smart_copy(m_indices+from, m_indices+from+chunkSize, m_indices+to); + } + } + + void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits::dummy_precision()) + { + Index k = 0; + Index n = size(); + for (Index i=0; i newValues(size); + internal::scoped_array newIndices(size); + Index copySize = (std::min)(size, m_size); + if (copySize>0) { + internal::smart_copy(m_values, m_values+copySize, newValues.ptr()); + internal::smart_copy(m_indices, m_indices+copySize, newIndices.ptr()); + } + std::swap(m_values,newValues.ptr()); + std::swap(m_indices,newIndices.ptr()); + m_allocatedSize = size; + } + + protected: + Scalar* m_values; + StorageIndex* m_indices; + Index m_size; + Index m_allocatedSize; + +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_COMPRESSED_STORAGE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h new file mode 100644 index 0000000000..948650253b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h @@ -0,0 +1,352 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2015 Gael Guennebaud +// +// 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_CONSERVATIVESPARSESPARSEPRODUCT_H +#define EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H + +namespace Eigen { + +namespace internal { + +template +static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, bool sortedInsertion = false) +{ + typedef typename remove_all::type::Scalar LhsScalar; + typedef typename remove_all::type::Scalar RhsScalar; + typedef typename remove_all::type::Scalar ResScalar; + + // make sure to call innerSize/outerSize since we fake the storage order. + Index rows = lhs.innerSize(); + Index cols = rhs.outerSize(); + eigen_assert(lhs.outerSize() == rhs.innerSize()); + + ei_declare_aligned_stack_constructed_variable(bool, mask, rows, 0); + ei_declare_aligned_stack_constructed_variable(ResScalar, values, rows, 0); + ei_declare_aligned_stack_constructed_variable(Index, indices, rows, 0); + + std::memset(mask,0,sizeof(bool)*rows); + + evaluator lhsEval(lhs); + evaluator rhsEval(rhs); + + // estimate the number of non zero entries + // given a rhs column containing Y non zeros, we assume that the respective Y columns + // of the lhs differs in average of one non zeros, thus the number of non zeros for + // the product of a rhs column with the lhs is X+Y where X is the average number of non zero + // per column of the lhs. + // Therefore, we have nnz(lhs*rhs) = nnz(lhs) + nnz(rhs) + Index estimated_nnz_prod = lhsEval.nonZerosEstimate() + rhsEval.nonZerosEstimate(); + + res.setZero(); + res.reserve(Index(estimated_nnz_prod)); + // we compute each column of the result, one after the other + for (Index j=0; j::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt) + { + RhsScalar y = rhsIt.value(); + Index k = rhsIt.index(); + for (typename evaluator::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt) + { + Index i = lhsIt.index(); + LhsScalar x = lhsIt.value(); + if(!mask[i]) + { + mask[i] = true; + values[i] = x * y; + indices[nnz] = i; + ++nnz; + } + else + values[i] += x * y; + } + } + if(!sortedInsertion) + { + // unordered insertion + for(Index k=0; k use a quick sort + // otherwise => loop through the entire vector + // In order to avoid to perform an expensive log2 when the + // result is clearly very sparse we use a linear bound up to 200. + if((nnz<200 && nnz1) std::sort(indices,indices+nnz); + for(Index k=0; k::Flags&RowMajorBit) ? RowMajor : ColMajor, + int RhsStorageOrder = (traits::Flags&RowMajorBit) ? RowMajor : ColMajor, + int ResStorageOrder = (traits::Flags&RowMajorBit) ? RowMajor : ColMajor> +struct conservative_sparse_sparse_product_selector; + +template +struct conservative_sparse_sparse_product_selector +{ + typedef typename remove_all::type LhsCleaned; + typedef typename LhsCleaned::Scalar Scalar; + + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrixAux; + typedef typename sparse_eval::type ColMajorMatrix; + + // If the result is tall and thin (in the extreme case a column vector) + // then it is faster to sort the coefficients inplace instead of transposing twice. + // FIXME, the following heuristic is probably not very good. + if(lhs.rows()>rhs.cols()) + { + ColMajorMatrix resCol(lhs.rows(),rhs.cols()); + // perform sorted insertion + internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol, true); + res = resCol.markAsRValue(); + } + else + { + ColMajorMatrixAux resCol(lhs.rows(),rhs.cols()); + // resort to transpose to sort the entries + internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol, false); + RowMajorMatrix resRow(resCol); + res = resRow.markAsRValue(); + } + } +}; + +template +struct conservative_sparse_sparse_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix RowMajorRhs; + typedef SparseMatrix RowMajorRes; + RowMajorRhs rhsRow = rhs; + RowMajorRes resRow(lhs.rows(), rhs.cols()); + internal::conservative_sparse_sparse_product_impl(rhsRow, lhs, resRow); + res = resRow; + } +}; + +template +struct conservative_sparse_sparse_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix RowMajorLhs; + typedef SparseMatrix RowMajorRes; + RowMajorLhs lhsRow = lhs; + RowMajorRes resRow(lhs.rows(), rhs.cols()); + internal::conservative_sparse_sparse_product_impl(rhs, lhsRow, resRow); + res = resRow; + } +}; + +template +struct conservative_sparse_sparse_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix RowMajorMatrix; + RowMajorMatrix resRow(lhs.rows(), rhs.cols()); + internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); + res = resRow; + } +}; + + +template +struct conservative_sparse_sparse_product_selector +{ + typedef typename traits::type>::Scalar Scalar; + + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix ColMajorMatrix; + ColMajorMatrix resCol(lhs.rows(), rhs.cols()); + internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol); + res = resCol; + } +}; + +template +struct conservative_sparse_sparse_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix ColMajorLhs; + typedef SparseMatrix ColMajorRes; + ColMajorLhs lhsCol = lhs; + ColMajorRes resCol(lhs.rows(), rhs.cols()); + internal::conservative_sparse_sparse_product_impl(lhsCol, rhs, resCol); + res = resCol; + } +}; + +template +struct conservative_sparse_sparse_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix ColMajorRhs; + typedef SparseMatrix ColMajorRes; + ColMajorRhs rhsCol = rhs; + ColMajorRes resCol(lhs.rows(), rhs.cols()); + internal::conservative_sparse_sparse_product_impl(lhs, rhsCol, resCol); + res = resCol; + } +}; + +template +struct conservative_sparse_sparse_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrix; + RowMajorMatrix resRow(lhs.rows(),rhs.cols()); + internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); + // sort the non zeros: + ColMajorMatrix resCol(resRow); + res = resCol; + } +}; + +} // end namespace internal + + +namespace internal { + +template +static void sparse_sparse_to_dense_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res) +{ + typedef typename remove_all::type::Scalar LhsScalar; + typedef typename remove_all::type::Scalar RhsScalar; + Index cols = rhs.outerSize(); + eigen_assert(lhs.outerSize() == rhs.innerSize()); + + evaluator lhsEval(lhs); + evaluator rhsEval(rhs); + + for (Index j=0; j::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt) + { + RhsScalar y = rhsIt.value(); + Index k = rhsIt.index(); + for (typename evaluator::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt) + { + Index i = lhsIt.index(); + LhsScalar x = lhsIt.value(); + res.coeffRef(i,j) += x * y; + } + } + } +} + + +} // end namespace internal + +namespace internal { + +template::Flags&RowMajorBit) ? RowMajor : ColMajor, + int RhsStorageOrder = (traits::Flags&RowMajorBit) ? RowMajor : ColMajor> +struct sparse_sparse_to_dense_product_selector; + +template +struct sparse_sparse_to_dense_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + internal::sparse_sparse_to_dense_product_impl(lhs, rhs, res); + } +}; + +template +struct sparse_sparse_to_dense_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix ColMajorLhs; + ColMajorLhs lhsCol(lhs); + internal::sparse_sparse_to_dense_product_impl(lhsCol, rhs, res); + } +}; + +template +struct sparse_sparse_to_dense_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + typedef SparseMatrix ColMajorRhs; + ColMajorRhs rhsCol(rhs); + internal::sparse_sparse_to_dense_product_impl(lhs, rhsCol, res); + } +}; + +template +struct sparse_sparse_to_dense_product_selector +{ + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res) + { + Transpose trRes(res); + internal::sparse_sparse_to_dense_product_impl >(rhs, lhs, trRes); + } +}; + + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h new file mode 100644 index 0000000000..67718c85be --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h @@ -0,0 +1,67 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_MAPPED_SPARSEMATRIX_H +#define EIGEN_MAPPED_SPARSEMATRIX_H + +namespace Eigen { + +/** \deprecated Use Map > + * \class MappedSparseMatrix + * + * \brief Sparse matrix + * + * \param _Scalar the scalar type, i.e. the type of the coefficients + * + * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme. + * + */ +namespace internal { +template +struct traits > : traits > +{}; +} // end namespace internal + +template +class MappedSparseMatrix + : public Map > +{ + typedef Map > Base; + + public: + + typedef typename Base::StorageIndex StorageIndex; + typedef typename Base::Scalar Scalar; + + inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0) + : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr) + {} + + /** Empty destructor */ + inline ~MappedSparseMatrix() {} +}; + +namespace internal { + +template +struct evaluator > + : evaluator > > +{ + typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType; + typedef evaluator > Base; + + evaluator() : Base() {} + explicit evaluator(const XprType &mat) : Base(mat) {} +}; + +} + +} // end namespace Eigen + +#endif // EIGEN_MAPPED_SPARSEMATRIX_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h new file mode 100644 index 0000000000..905485c88e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h @@ -0,0 +1,270 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSEASSIGN_H +#define EIGEN_SPARSEASSIGN_H + +namespace Eigen { + +template +template +Derived& SparseMatrixBase::operator=(const EigenBase &other) +{ + internal::call_assignment_no_alias(derived(), other.derived()); + return derived(); +} + +template +template +Derived& SparseMatrixBase::operator=(const ReturnByValue& other) +{ + // TODO use the evaluator mechanism + other.evalTo(derived()); + return derived(); +} + +template +template +inline Derived& SparseMatrixBase::operator=(const SparseMatrixBase& other) +{ + // by default sparse evaluation do not alias, so we can safely bypass the generic call_assignment routine + internal::Assignment > + ::run(derived(), other.derived(), internal::assign_op()); + return derived(); +} + +template +inline Derived& SparseMatrixBase::operator=(const Derived& other) +{ + internal::call_assignment_no_alias(derived(), other.derived()); + return derived(); +} + +namespace internal { + +template<> +struct storage_kind_to_evaluator_kind { + typedef IteratorBased Kind; +}; + +template<> +struct storage_kind_to_shape { + typedef SparseShape Shape; +}; + +struct Sparse2Sparse {}; +struct Sparse2Dense {}; + +template<> struct AssignmentKind { typedef Sparse2Sparse Kind; }; +template<> struct AssignmentKind { typedef Sparse2Sparse Kind; }; +template<> struct AssignmentKind { typedef Sparse2Dense Kind; }; +template<> struct AssignmentKind { typedef Sparse2Dense Kind; }; + + +template +void assign_sparse_to_sparse(DstXprType &dst, const SrcXprType &src) +{ + typedef typename DstXprType::Scalar Scalar; + typedef internal::evaluator DstEvaluatorType; + typedef internal::evaluator SrcEvaluatorType; + + SrcEvaluatorType srcEvaluator(src); + + const bool transpose = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit); + const Index outerEvaluationSize = (SrcEvaluatorType::Flags&RowMajorBit) ? src.rows() : src.cols(); + if ((!transpose) && src.isRValue()) + { + // eval without temporary + dst.resize(src.rows(), src.cols()); + dst.setZero(); + dst.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2)); + for (Index j=0; j::SupportedAccessPatterns & OuterRandomAccessPattern)==OuterRandomAccessPattern) || + (!((DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit)))) && + "the transpose operation is supposed to be handled in SparseMatrix::operator="); + + enum { Flip = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit) }; + + + DstXprType temp(src.rows(), src.cols()); + + temp.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2)); + for (Index j=0; j +struct Assignment +{ + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &/*func*/) + { + assign_sparse_to_sparse(dst.derived(), src.derived()); + } +}; + +// Generic Sparse to Dense assignment +template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak> +struct Assignment +{ + static void run(DstXprType &dst, const SrcXprType &src, const Functor &func) + { + if(internal::is_same >::value) + dst.setZero(); + + internal::evaluator srcEval(src); + resize_if_allowed(dst, src, func); + internal::evaluator dstEval(dst); + + const Index outerEvaluationSize = (internal::evaluator::Flags&RowMajorBit) ? src.rows() : src.cols(); + for (Index j=0; j::InnerIterator i(srcEval,j); i; ++i) + func.assignCoeff(dstEval.coeffRef(i.row(),i.col()), i.value()); + } +}; + +// Specialization for dense ?= dense +/- sparse and dense ?= sparse +/- dense +template +struct assignment_from_dense_op_sparse +{ + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void run(DstXprType &dst, const SrcXprType &src, const InitialFunc& /*func*/) + { + #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN + EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN + #endif + + call_assignment_no_alias(dst, src.lhs(), Func1()); + call_assignment_no_alias(dst, src.rhs(), Func2()); + } + + // Specialization for dense1 = sparse + dense2; -> dense1 = dense2; dense1 += sparse; + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename internal::enable_if::Shape,DenseShape>::value>::type + run(DstXprType &dst, const CwiseBinaryOp, const Lhs, const Rhs> &src, + const internal::assign_op& /*func*/) + { + #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN + EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN + #endif + + // Apply the dense matrix first, then the sparse one. + call_assignment_no_alias(dst, src.rhs(), Func1()); + call_assignment_no_alias(dst, src.lhs(), Func2()); + } + + // Specialization for dense1 = sparse - dense2; -> dense1 = -dense2; dense1 += sparse; + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename internal::enable_if::Shape,DenseShape>::value>::type + run(DstXprType &dst, const CwiseBinaryOp, const Lhs, const Rhs> &src, + const internal::assign_op& /*func*/) + { + #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN + EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN + #endif + + // Apply the dense matrix first, then the sparse one. + call_assignment_no_alias(dst, -src.rhs(), Func1()); + call_assignment_no_alias(dst, src.lhs(), add_assign_op()); + } +}; + +#define EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(ASSIGN_OP,BINOP,ASSIGN_OP2) \ + template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar> \ + struct Assignment, const Lhs, const Rhs>, internal::ASSIGN_OP, \ + Sparse2Dense, \ + typename internal::enable_if< internal::is_same::Shape,DenseShape>::value \ + || internal::is_same::Shape,DenseShape>::value>::type> \ + : assignment_from_dense_op_sparse, internal::ASSIGN_OP2 > \ + {} + +EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op, scalar_sum_op,add_assign_op); +EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_sum_op,add_assign_op); +EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_sum_op,sub_assign_op); + +EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op, scalar_difference_op,sub_assign_op); +EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_difference_op,sub_assign_op); +EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_difference_op,add_assign_op); + + +// Specialization for "dst = dec.solve(rhs)" +// NOTE we need to specialize it for Sparse2Sparse to avoid ambiguous specialization error +template +struct Assignment, internal::assign_op, Sparse2Sparse> +{ + typedef Solve SrcXprType; + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + src.dec()._solve_impl(src.rhs(), dst); + } +}; + +struct Diagonal2Sparse {}; + +template<> struct AssignmentKind { typedef Diagonal2Sparse Kind; }; + +template< typename DstXprType, typename SrcXprType, typename Functor> +struct Assignment +{ + typedef typename DstXprType::StorageIndex StorageIndex; + typedef typename DstXprType::Scalar Scalar; + + template + static void run(SparseMatrix &dst, const SrcXprType &src, const AssignFunc &func) + { dst.assignDiagonal(src.diagonal(), func); } + + template + static void run(SparseMatrixBase &dst, const SrcXprType &src, const internal::assign_op &/*func*/) + { dst.derived().diagonal() = src.diagonal(); } + + template + static void run(SparseMatrixBase &dst, const SrcXprType &src, const internal::add_assign_op &/*func*/) + { dst.derived().diagonal() += src.diagonal(); } + + template + static void run(SparseMatrixBase &dst, const SrcXprType &src, const internal::sub_assign_op &/*func*/) + { dst.derived().diagonal() -= src.diagonal(); } +}; +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSEASSIGN_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h new file mode 100644 index 0000000000..5b4f6cc9f3 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h @@ -0,0 +1,571 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSE_BLOCK_H +#define EIGEN_SPARSE_BLOCK_H + +namespace Eigen { + +// Subset of columns or rows +template +class BlockImpl + : public SparseMatrixBase > +{ + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef Block BlockType; +public: + enum { IsRowMajor = internal::traits::IsRowMajor }; +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; + typedef SparseMatrixBase Base; + using Base::convert_index; +public: + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) + + inline BlockImpl(XprType& xpr, Index i) + : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize) + {} + + inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) + : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols)) + {} + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + Index nonZeros() const + { + typedef internal::evaluator EvaluatorType; + EvaluatorType matEval(m_matrix); + Index nnz = 0; + Index end = m_outerStart + m_outerSize.value(); + for(Index j=m_outerStart; j::non_const_type m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + + protected: + // Disable assignment with clear error message. + // Note that simply removing operator= yields compilation errors with ICC+MSVC + template + BlockImpl& operator=(const T&) + { + EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY); + return *this; + } +}; + + +/*************************************************************************** +* specialization for SparseMatrix +***************************************************************************/ + +namespace internal { + +template +class sparse_matrix_block_impl + : public SparseCompressedBase > +{ + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef Block BlockType; + typedef SparseCompressedBase > Base; + using Base::convert_index; +public: + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + typedef typename Base::IndexVector IndexVector; + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index i) + : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize) + {} + + inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) + : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols)) + {} + + template + inline BlockType& operator=(const SparseMatrixBase& other) + { + typedef typename internal::remove_all::type _NestedMatrixType; + _NestedMatrixType& matrix = m_matrix; + // This assignment is slow if this vector set is not empty + // and/or it is not at the end of the nonzeros of the underlying matrix. + + // 1 - eval to a temporary to avoid transposition and/or aliasing issues + Ref > tmp(other.derived()); + eigen_internal_assert(tmp.outerSize()==m_outerSize.value()); + + // 2 - let's check whether there is enough allocated memory + Index nnz = tmp.nonZeros(); + Index start = m_outerStart==0 ? 0 : m_matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block + Index end = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending position of the current block + Index block_size = end - start; // available room in the current block + Index tail_size = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end; + + Index free_size = m_matrix.isCompressed() + ? Index(matrix.data().allocatedSize()) + block_size + : block_size; + + Index tmp_start = tmp.outerIndexPtr()[0]; + + bool update_trailing_pointers = false; + if(nnz>free_size) + { + // realloc manually to reduce copies + typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz); + + internal::smart_copy(m_matrix.valuePtr(), m_matrix.valuePtr() + start, newdata.valuePtr()); + internal::smart_copy(m_matrix.innerIndexPtr(), m_matrix.innerIndexPtr() + start, newdata.indexPtr()); + + internal::smart_copy(tmp.valuePtr() + tmp_start, tmp.valuePtr() + tmp_start + nnz, newdata.valuePtr() + start); + internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz, newdata.indexPtr() + start); + + internal::smart_copy(matrix.valuePtr()+end, matrix.valuePtr()+end + tail_size, newdata.valuePtr()+start+nnz); + internal::smart_copy(matrix.innerIndexPtr()+end, matrix.innerIndexPtr()+end + tail_size, newdata.indexPtr()+start+nnz); + + newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz); + + matrix.data().swap(newdata); + + update_trailing_pointers = true; + } + else + { + if(m_matrix.isCompressed() && nnz!=block_size) + { + // no need to realloc, simply copy the tail at its respective position and insert tmp + matrix.data().resize(start + nnz + tail_size); + + internal::smart_memmove(matrix.valuePtr()+end, matrix.valuePtr() + end+tail_size, matrix.valuePtr() + start+nnz); + internal::smart_memmove(matrix.innerIndexPtr()+end, matrix.innerIndexPtr() + end+tail_size, matrix.innerIndexPtr() + start+nnz); + + update_trailing_pointers = true; + } + + internal::smart_copy(tmp.valuePtr() + tmp_start, tmp.valuePtr() + tmp_start + nnz, matrix.valuePtr() + start); + internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz, matrix.innerIndexPtr() + start); + } + + // update outer index pointers and innerNonZeros + if(IsVectorAtCompileTime) + { + if(!m_matrix.isCompressed()) + matrix.innerNonZeroPtr()[m_outerStart] = StorageIndex(nnz); + matrix.outerIndexPtr()[m_outerStart] = StorageIndex(start); + } + else + { + StorageIndex p = StorageIndex(start); + for(Index k=0; k(tmp.innerVector(k).nonZeros()); + if(!m_matrix.isCompressed()) + matrix.innerNonZeroPtr()[m_outerStart+k] = nnz_k; + matrix.outerIndexPtr()[m_outerStart+k] = p; + p += nnz_k; + } + } + + if(update_trailing_pointers) + { + StorageIndex offset = internal::convert_index(nnz - block_size); + for(Index k = m_outerStart + m_outerSize.value(); k<=matrix.outerSize(); ++k) + { + matrix.outerIndexPtr()[k] += offset; + } + } + + return derived(); + } + + inline BlockType& operator=(const BlockType& other) + { + return operator=(other); + } + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr(); } + inline Scalar* valuePtr() + { return m_matrix.valuePtr(); } + + inline const StorageIndex* innerIndexPtr() const + { return m_matrix.innerIndexPtr(); } + inline StorageIndex* innerIndexPtr() + { return m_matrix.innerIndexPtr(); } + + inline const StorageIndex* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + inline StorageIndex* outerIndexPtr() + { return m_matrix.outerIndexPtr() + m_outerStart; } + + inline const StorageIndex* innerNonZeroPtr() const + { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); } + inline StorageIndex* innerNonZeroPtr() + { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); } + + bool isCompressed() const { return m_matrix.innerNonZeroPtr()==0; } + + inline Scalar& coeffRef(Index row, Index col) + { + return m_matrix.coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(Index row, Index col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(Index index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(sparse_matrix_block_impl); + eigen_assert(Base::nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + inline const SparseMatrixType& nestedExpression() const { return m_matrix; } + inline SparseMatrixType& nestedExpression() { return m_matrix; } + Index startRow() const { return IsRowMajor ? m_outerStart : 0; } + Index startCol() const { return IsRowMajor ? 0 : m_outerStart; } + Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename internal::ref_selector::non_const_type m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + +}; + +} // namespace internal + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public internal::sparse_matrix_block_impl,BlockRows,BlockCols> +{ +public: + typedef _StorageIndex StorageIndex; + typedef SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType; + typedef internal::sparse_matrix_block_impl Base; + inline BlockImpl(SparseMatrixType& xpr, Index i) + : Base(xpr, i) + {} + + inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) + : Base(xpr, startRow, startCol, blockRows, blockCols) + {} + + using Base::operator=; +}; + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public internal::sparse_matrix_block_impl,BlockRows,BlockCols> +{ +public: + typedef _StorageIndex StorageIndex; + typedef const SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType; + typedef internal::sparse_matrix_block_impl Base; + inline BlockImpl(SparseMatrixType& xpr, Index i) + : Base(xpr, i) + {} + + inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) + : Base(xpr, startRow, startCol, blockRows, blockCols) + {} + + using Base::operator=; +private: + template BlockImpl(const SparseMatrixBase& xpr, Index i); + template BlockImpl(const SparseMatrixBase& xpr); +}; + +//---------- + +/** Generic implementation of sparse Block expression. + * Real-only. + */ +template +class BlockImpl + : public SparseMatrixBase >, internal::no_assignment_operator +{ + typedef Block BlockType; + typedef SparseMatrixBase Base; + using Base::convert_index; +public: + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) + + typedef typename internal::remove_all::type _MatrixTypeNested; + + /** Column or Row constructor + */ + inline BlockImpl(XprType& xpr, Index i) + : m_matrix(xpr), + m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? convert_index(i) : 0), + m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? convert_index(i) : 0), + m_blockRows(BlockRows==1 ? 1 : xpr.rows()), + m_blockCols(BlockCols==1 ? 1 : xpr.cols()) + {} + + /** Dynamic-size constructor + */ + inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) + : m_matrix(xpr), m_startRow(convert_index(startRow)), m_startCol(convert_index(startCol)), m_blockRows(convert_index(blockRows)), m_blockCols(convert_index(blockCols)) + {} + + inline Index rows() const { return m_blockRows.value(); } + inline Index cols() const { return m_blockCols.value(); } + + inline Scalar& coeffRef(Index row, Index col) + { + return m_matrix.coeffRef(row + m_startRow.value(), col + m_startCol.value()); + } + + inline const Scalar coeff(Index row, Index col) const + { + return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value()); + } + + inline Scalar& coeffRef(Index index) + { + return m_matrix.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + inline const Scalar coeff(Index index) const + { + return m_matrix.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + inline const XprType& nestedExpression() const { return m_matrix; } + inline XprType& nestedExpression() { return m_matrix; } + Index startRow() const { return m_startRow.value(); } + Index startCol() const { return m_startCol.value(); } + Index blockRows() const { return m_blockRows.value(); } + Index blockCols() const { return m_blockCols.value(); } + + protected: +// friend class internal::GenericSparseBlockInnerIteratorImpl; + friend struct internal::unary_evaluator, internal::IteratorBased, Scalar >; + + Index nonZeros() const { return Dynamic; } + + typename internal::ref_selector::non_const_type m_matrix; + const internal::variable_if_dynamic m_startRow; + const internal::variable_if_dynamic m_startCol; + const internal::variable_if_dynamic m_blockRows; + const internal::variable_if_dynamic m_blockCols; + + protected: + // Disable assignment with clear error message. + // Note that simply removing operator= yields compilation errors with ICC+MSVC + template + BlockImpl& operator=(const T&) + { + EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY); + return *this; + } + +}; + +namespace internal { + +template +struct unary_evaluator, IteratorBased > + : public evaluator_base > +{ + class InnerVectorInnerIterator; + class OuterVectorInnerIterator; + public: + typedef Block XprType; + typedef typename XprType::StorageIndex StorageIndex; + typedef typename XprType::Scalar Scalar; + + enum { + IsRowMajor = XprType::IsRowMajor, + + OuterVector = (BlockCols==1 && ArgType::IsRowMajor) + | // FIXME | instead of || to please GCC 4.4.0 stupid warning "suggest parentheses around &&". + // revert to || as soon as not needed anymore. + (BlockRows==1 && !ArgType::IsRowMajor), + + CoeffReadCost = evaluator::CoeffReadCost, + Flags = XprType::Flags + }; + + typedef typename internal::conditional::type InnerIterator; + + explicit unary_evaluator(const XprType& op) + : m_argImpl(op.nestedExpression()), m_block(op) + {} + + inline Index nonZerosEstimate() const { + const Index nnz = m_block.nonZeros(); + if(nnz < 0) { + // Scale the non-zero estimate for the underlying expression linearly with block size. + // Return zero if the underlying block is empty. + const Index nested_sz = m_block.nestedExpression().size(); + return nested_sz == 0 ? 0 : m_argImpl.nonZerosEstimate() * m_block.size() / nested_sz; + } + return nnz; + } + + protected: + typedef typename evaluator::InnerIterator EvalIterator; + + evaluator m_argImpl; + const XprType &m_block; +}; + +template +class unary_evaluator, IteratorBased>::InnerVectorInnerIterator + : public EvalIterator +{ + // NOTE MSVC fails to compile if we don't explicitely "import" IsRowMajor from unary_evaluator + // because the base class EvalIterator has a private IsRowMajor enum too. (bug #1786) + // NOTE We cannot call it IsRowMajor because it would shadow unary_evaluator::IsRowMajor + enum { XprIsRowMajor = unary_evaluator::IsRowMajor }; + const XprType& m_block; + Index m_end; +public: + + EIGEN_STRONG_INLINE InnerVectorInnerIterator(const unary_evaluator& aEval, Index outer) + : EvalIterator(aEval.m_argImpl, outer + (XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol())), + m_block(aEval.m_block), + m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows()) + { + while( (EvalIterator::operator bool()) && (EvalIterator::index() < (XprIsRowMajor ? m_block.startCol() : m_block.startRow())) ) + EvalIterator::operator++(); + } + + inline StorageIndex index() const { return EvalIterator::index() - convert_index(XprIsRowMajor ? m_block.startCol() : m_block.startRow()); } + inline Index outer() const { return EvalIterator::outer() - (XprIsRowMajor ? m_block.startRow() : m_block.startCol()); } + inline Index row() const { return EvalIterator::row() - m_block.startRow(); } + inline Index col() const { return EvalIterator::col() - m_block.startCol(); } + + inline operator bool() const { return EvalIterator::operator bool() && EvalIterator::index() < m_end; } +}; + +template +class unary_evaluator, IteratorBased>::OuterVectorInnerIterator +{ + // NOTE see above + enum { XprIsRowMajor = unary_evaluator::IsRowMajor }; + const unary_evaluator& m_eval; + Index m_outerPos; + const Index m_innerIndex; + Index m_end; + EvalIterator m_it; +public: + + EIGEN_STRONG_INLINE OuterVectorInnerIterator(const unary_evaluator& aEval, Index outer) + : m_eval(aEval), + m_outerPos( (XprIsRowMajor ? aEval.m_block.startCol() : aEval.m_block.startRow()) ), + m_innerIndex(XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol()), + m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows()), + m_it(m_eval.m_argImpl, m_outerPos) + { + EIGEN_UNUSED_VARIABLE(outer); + eigen_assert(outer==0); + + while(m_it && m_it.index() < m_innerIndex) ++m_it; + if((!m_it) || (m_it.index()!=m_innerIndex)) + ++(*this); + } + + inline StorageIndex index() const { return convert_index(m_outerPos - (XprIsRowMajor ? m_eval.m_block.startCol() : m_eval.m_block.startRow())); } + inline Index outer() const { return 0; } + inline Index row() const { return XprIsRowMajor ? 0 : index(); } + inline Index col() const { return XprIsRowMajor ? index() : 0; } + + inline Scalar value() const { return m_it.value(); } + inline Scalar& valueRef() { return m_it.valueRef(); } + + inline OuterVectorInnerIterator& operator++() + { + // search next non-zero entry + while(++m_outerPos +struct unary_evaluator,BlockRows,BlockCols,true>, IteratorBased> + : evaluator,BlockRows,BlockCols,true> > > +{ + typedef Block,BlockRows,BlockCols,true> XprType; + typedef evaluator > Base; + explicit unary_evaluator(const XprType &xpr) : Base(xpr) {} +}; + +template +struct unary_evaluator,BlockRows,BlockCols,true>, IteratorBased> + : evaluator,BlockRows,BlockCols,true> > > +{ + typedef Block,BlockRows,BlockCols,true> XprType; + typedef evaluator > Base; + explicit unary_evaluator(const XprType &xpr) : Base(xpr) {} +}; + +} // end namespace internal + + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_BLOCK_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h new file mode 100644 index 0000000000..ebe02d1ab0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h @@ -0,0 +1,206 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + + +/* + + * NOTE: This file is the modified version of sp_coletree.c file in SuperLU + + * -- SuperLU routine (version 3.1) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * August 1, 2008 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSE_COLETREE_H +#define SPARSE_COLETREE_H + +namespace Eigen { + +namespace internal { + +/** Find the root of the tree/set containing the vertex i : Use Path halving */ +template +Index etree_find (Index i, IndexVector& pp) +{ + Index p = pp(i); // Parent + Index gp = pp(p); // Grand parent + while (gp != p) + { + pp(i) = gp; // Parent pointer on find path is changed to former grand parent + i = gp; + p = pp(i); + gp = pp(p); + } + return p; +} + +/** Compute the column elimination tree of a sparse matrix + * \param mat The matrix in column-major format. + * \param parent The elimination tree + * \param firstRowElt The column index of the first element in each row + * \param perm The permutation to apply to the column of \b mat + */ +template +int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowElt, typename MatrixType::StorageIndex *perm=0) +{ + typedef typename MatrixType::StorageIndex StorageIndex; + StorageIndex nc = convert_index(mat.cols()); // Number of columns + StorageIndex m = convert_index(mat.rows()); + StorageIndex diagSize = (std::min)(nc,m); + IndexVector root(nc); // root of subtree of etree + root.setZero(); + IndexVector pp(nc); // disjoint sets + pp.setZero(); // Initialize disjoint sets + parent.resize(mat.cols()); + //Compute first nonzero column in each row + firstRowElt.resize(m); + firstRowElt.setConstant(nc); + firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1); + bool found_diag; + for (StorageIndex col = 0; col < nc; col++) + { + StorageIndex pcol = col; + if(perm) pcol = perm[col]; + for (typename MatrixType::InnerIterator it(mat, pcol); it; ++it) + { + Index row = it.row(); + firstRowElt(row) = (std::min)(firstRowElt(row), col); + } + } + /* Compute etree by Liu's algorithm for symmetric matrices, + except use (firstRowElt[r],c) in place of an edge (r,c) of A. + Thus each row clique in A'*A is replaced by a star + centered at its first vertex, which has the same fill. */ + StorageIndex rset, cset, rroot; + for (StorageIndex col = 0; col < nc; col++) + { + found_diag = col>=m; + pp(col) = col; + cset = col; + root(cset) = col; + parent(col) = nc; + /* The diagonal element is treated here even if it does not exist in the matrix + * hence the loop is executed once more */ + StorageIndex pcol = col; + if(perm) pcol = perm[col]; + for (typename MatrixType::InnerIterator it(mat, pcol); it||!found_diag; ++it) + { // A sequence of interleaved find and union is performed + Index i = col; + if(it) i = it.index(); + if (i == col) found_diag = true; + + StorageIndex row = firstRowElt(i); + if (row >= col) continue; + rset = internal::etree_find(row, pp); // Find the name of the set containing row + rroot = root(rset); + if (rroot != col) + { + parent(rroot) = col; + pp(cset) = rset; + cset = rset; + root(cset) = col; + } + } + } + return 0; +} + +/** + * Depth-first search from vertex n. No recursion. + * This routine was contributed by Cédric Doucet, CEDRAT Group, Meylan, France. +*/ +template +void nr_etdfs (typename IndexVector::Scalar n, IndexVector& parent, IndexVector& first_kid, IndexVector& next_kid, IndexVector& post, typename IndexVector::Scalar postnum) +{ + typedef typename IndexVector::Scalar StorageIndex; + StorageIndex current = n, first, next; + while (postnum != n) + { + // No kid for the current node + first = first_kid(current); + + // no kid for the current node + if (first == -1) + { + // Numbering this node because it has no kid + post(current) = postnum++; + + // looking for the next kid + next = next_kid(current); + while (next == -1) + { + // No more kids : back to the parent node + current = parent(current); + // numbering the parent node + post(current) = postnum++; + + // Get the next kid + next = next_kid(current); + } + // stopping criterion + if (postnum == n+1) return; + + // Updating current node + current = next; + } + else + { + current = first; + } + } +} + + +/** + * \brief Post order a tree + * \param n the number of nodes + * \param parent Input tree + * \param post postordered tree + */ +template +void treePostorder(typename IndexVector::Scalar n, IndexVector& parent, IndexVector& post) +{ + typedef typename IndexVector::Scalar StorageIndex; + IndexVector first_kid, next_kid; // Linked list of children + StorageIndex postnum; + // Allocate storage for working arrays and results + first_kid.resize(n+1); + next_kid.setZero(n+1); + post.setZero(n+1); + + // Set up structure describing children + first_kid.setConstant(-1); + for (StorageIndex v = n-1; v >= 0; v--) + { + StorageIndex dad = parent(v); + next_kid(v) = first_kid(dad); + first_kid(dad) = v; + } + + // Depth-first search from dummy root vertex #n + postnum = 0; + internal::nr_etdfs(n, parent, first_kid, next_kid, post, postnum); +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // SPARSE_COLETREE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h new file mode 100644 index 0000000000..6a2c7a8ce6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h @@ -0,0 +1,370 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Gael Guennebaud +// +// 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_SPARSE_COMPRESSED_BASE_H +#define EIGEN_SPARSE_COMPRESSED_BASE_H + +namespace Eigen { + +template class SparseCompressedBase; + +namespace internal { + +template +struct traits > : traits +{}; + +} // end namespace internal + +/** \ingroup SparseCore_Module + * \class SparseCompressedBase + * \brief Common base class for sparse [compressed]-{row|column}-storage format. + * + * This class defines the common interface for all derived classes implementing the compressed sparse storage format, such as: + * - SparseMatrix + * - Ref + * - Map + * + */ +template +class SparseCompressedBase + : public SparseMatrixBase +{ + public: + typedef SparseMatrixBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(SparseCompressedBase) + using Base::operator=; + using Base::IsRowMajor; + + class InnerIterator; + class ReverseInnerIterator; + + protected: + typedef typename Base::IndexVector IndexVector; + Eigen::Map innerNonZeros() { return Eigen::Map(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); } + const Eigen::Map innerNonZeros() const { return Eigen::Map(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); } + + public: + + /** \returns the number of non zero coefficients */ + inline Index nonZeros() const + { + if(Derived::IsVectorAtCompileTime && outerIndexPtr()==0) + return derived().nonZeros(); + else if(isCompressed()) + return outerIndexPtr()[derived().outerSize()]-outerIndexPtr()[0]; + else if(derived().outerSize()==0) + return 0; + else + return innerNonZeros().sum(); + } + + /** \returns a const pointer to the array of values. + * This function is aimed at interoperability with other libraries. + * \sa innerIndexPtr(), outerIndexPtr() */ + inline const Scalar* valuePtr() const { return derived().valuePtr(); } + /** \returns a non-const pointer to the array of values. + * This function is aimed at interoperability with other libraries. + * \sa innerIndexPtr(), outerIndexPtr() */ + inline Scalar* valuePtr() { return derived().valuePtr(); } + + /** \returns a const pointer to the array of inner indices. + * This function is aimed at interoperability with other libraries. + * \sa valuePtr(), outerIndexPtr() */ + inline const StorageIndex* innerIndexPtr() const { return derived().innerIndexPtr(); } + /** \returns a non-const pointer to the array of inner indices. + * This function is aimed at interoperability with other libraries. + * \sa valuePtr(), outerIndexPtr() */ + inline StorageIndex* innerIndexPtr() { return derived().innerIndexPtr(); } + + /** \returns a const pointer to the array of the starting positions of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \warning it returns the null pointer 0 for SparseVector + * \sa valuePtr(), innerIndexPtr() */ + inline const StorageIndex* outerIndexPtr() const { return derived().outerIndexPtr(); } + /** \returns a non-const pointer to the array of the starting positions of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \warning it returns the null pointer 0 for SparseVector + * \sa valuePtr(), innerIndexPtr() */ + inline StorageIndex* outerIndexPtr() { return derived().outerIndexPtr(); } + + /** \returns a const pointer to the array of the number of non zeros of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \warning it returns the null pointer 0 in compressed mode */ + inline const StorageIndex* innerNonZeroPtr() const { return derived().innerNonZeroPtr(); } + /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \warning it returns the null pointer 0 in compressed mode */ + inline StorageIndex* innerNonZeroPtr() { return derived().innerNonZeroPtr(); } + + /** \returns whether \c *this is in compressed form. */ + inline bool isCompressed() const { return innerNonZeroPtr()==0; } + + /** \returns a read-only view of the stored coefficients as a 1D array expression. + * + * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise. + * + * \sa valuePtr(), isCompressed() */ + const Map > coeffs() const { eigen_assert(isCompressed()); return Array::Map(valuePtr(),nonZeros()); } + + /** \returns a read-write view of the stored coefficients as a 1D array expression + * + * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise. + * + * Here is an example: + * \include SparseMatrix_coeffs.cpp + * and the output is: + * \include SparseMatrix_coeffs.out + * + * \sa valuePtr(), isCompressed() */ + Map > coeffs() { eigen_assert(isCompressed()); return Array::Map(valuePtr(),nonZeros()); } + + protected: + /** Default constructor. Do nothing. */ + SparseCompressedBase() {} + + /** \internal return the index of the coeff at (row,col) or just before if it does not exist. + * This is an analogue of std::lower_bound. + */ + internal::LowerBoundIndex lower_bound(Index row, Index col) const + { + eigen_internal_assert(row>=0 && rowrows() && col>=0 && colcols()); + + const Index outer = Derived::IsRowMajor ? row : col; + const Index inner = Derived::IsRowMajor ? col : row; + + Index start = this->outerIndexPtr()[outer]; + Index end = this->isCompressed() ? this->outerIndexPtr()[outer+1] : this->outerIndexPtr()[outer] + this->innerNonZeroPtr()[outer]; + eigen_assert(end>=start && "you are using a non finalized sparse matrix or written coefficient does not exist"); + internal::LowerBoundIndex p; + p.value = std::lower_bound(this->innerIndexPtr()+start, this->innerIndexPtr()+end,inner) - this->innerIndexPtr(); + p.found = (p.valueinnerIndexPtr()[p.value]==inner); + return p; + } + + friend struct internal::evaluator >; + + private: + template explicit SparseCompressedBase(const SparseCompressedBase&); +}; + +template +class SparseCompressedBase::InnerIterator +{ + public: + InnerIterator() + : m_values(0), m_indices(0), m_outer(0), m_id(0), m_end(0) + {} + + InnerIterator(const InnerIterator& other) + : m_values(other.m_values), m_indices(other.m_indices), m_outer(other.m_outer), m_id(other.m_id), m_end(other.m_end) + {} + + InnerIterator& operator=(const InnerIterator& other) + { + m_values = other.m_values; + m_indices = other.m_indices; + const_cast(m_outer).setValue(other.m_outer.value()); + m_id = other.m_id; + m_end = other.m_end; + return *this; + } + + InnerIterator(const SparseCompressedBase& mat, Index outer) + : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer) + { + if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0) + { + m_id = 0; + m_end = mat.nonZeros(); + } + else + { + m_id = mat.outerIndexPtr()[outer]; + if(mat.isCompressed()) + m_end = mat.outerIndexPtr()[outer+1]; + else + m_end = m_id + mat.innerNonZeroPtr()[outer]; + } + } + + explicit InnerIterator(const SparseCompressedBase& mat) + : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_id(0), m_end(mat.nonZeros()) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + } + + explicit InnerIterator(const internal::CompressedStorage& data) + : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_id(0), m_end(data.size()) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + } + + inline InnerIterator& operator++() { m_id++; return *this; } + inline InnerIterator& operator+=(Index i) { m_id += i ; return *this; } + + inline InnerIterator operator+(Index i) + { + InnerIterator result = *this; + result += i; + return result; + } + + inline const Scalar& value() const { return m_values[m_id]; } + inline Scalar& valueRef() { return const_cast(m_values[m_id]); } + + inline StorageIndex index() const { return m_indices[m_id]; } + inline Index outer() const { return m_outer.value(); } + inline Index row() const { return IsRowMajor ? m_outer.value() : index(); } + inline Index col() const { return IsRowMajor ? index() : m_outer.value(); } + + inline operator bool() const { return (m_id < m_end); } + + protected: + const Scalar* m_values; + const StorageIndex* m_indices; + typedef internal::variable_if_dynamic OuterType; + const OuterType m_outer; + Index m_id; + Index m_end; + private: + // If you get here, then you're not using the right InnerIterator type, e.g.: + // SparseMatrix A; + // SparseMatrix::InnerIterator it(A,0); + template InnerIterator(const SparseMatrixBase&, Index outer); +}; + +template +class SparseCompressedBase::ReverseInnerIterator +{ + public: + ReverseInnerIterator(const SparseCompressedBase& mat, Index outer) + : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer) + { + if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0) + { + m_start = 0; + m_id = mat.nonZeros(); + } + else + { + m_start = mat.outerIndexPtr()[outer]; + if(mat.isCompressed()) + m_id = mat.outerIndexPtr()[outer+1]; + else + m_id = m_start + mat.innerNonZeroPtr()[outer]; + } + } + + explicit ReverseInnerIterator(const SparseCompressedBase& mat) + : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_start(0), m_id(mat.nonZeros()) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + } + + explicit ReverseInnerIterator(const internal::CompressedStorage& data) + : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_start(0), m_id(data.size()) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + } + + inline ReverseInnerIterator& operator--() { --m_id; return *this; } + inline ReverseInnerIterator& operator-=(Index i) { m_id -= i; return *this; } + + inline ReverseInnerIterator operator-(Index i) + { + ReverseInnerIterator result = *this; + result -= i; + return result; + } + + inline const Scalar& value() const { return m_values[m_id-1]; } + inline Scalar& valueRef() { return const_cast(m_values[m_id-1]); } + + inline StorageIndex index() const { return m_indices[m_id-1]; } + inline Index outer() const { return m_outer.value(); } + inline Index row() const { return IsRowMajor ? m_outer.value() : index(); } + inline Index col() const { return IsRowMajor ? index() : m_outer.value(); } + + inline operator bool() const { return (m_id > m_start); } + + protected: + const Scalar* m_values; + const StorageIndex* m_indices; + typedef internal::variable_if_dynamic OuterType; + const OuterType m_outer; + Index m_start; + Index m_id; +}; + +namespace internal { + +template +struct evaluator > + : evaluator_base +{ + typedef typename Derived::Scalar Scalar; + typedef typename Derived::InnerIterator InnerIterator; + + enum { + CoeffReadCost = NumTraits::ReadCost, + Flags = Derived::Flags + }; + + evaluator() : m_matrix(0), m_zero(0) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + explicit evaluator(const Derived &mat) : m_matrix(&mat), m_zero(0) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_matrix->nonZeros(); + } + + operator Derived&() { return m_matrix->const_cast_derived(); } + operator const Derived&() const { return *m_matrix; } + + typedef typename DenseCoeffsBase::CoeffReturnType CoeffReturnType; + const Scalar& coeff(Index row, Index col) const + { + Index p = find(row,col); + + if(p==Dynamic) + return m_zero; + else + return m_matrix->const_cast_derived().valuePtr()[p]; + } + + Scalar& coeffRef(Index row, Index col) + { + Index p = find(row,col); + eigen_assert(p!=Dynamic && "written coefficient does not exist"); + return m_matrix->const_cast_derived().valuePtr()[p]; + } + +protected: + + Index find(Index row, Index col) const + { + internal::LowerBoundIndex p = m_matrix->lower_bound(row,col); + return p.found ? p.value : Dynamic; + } + + const Derived *m_matrix; + const Scalar m_zero; +}; + +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_COMPRESSED_BASE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h new file mode 100644 index 0000000000..9b0d3f98dc --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h @@ -0,0 +1,722 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSE_CWISE_BINARY_OP_H +#define EIGEN_SPARSE_CWISE_BINARY_OP_H + +namespace Eigen { + +// Here we have to handle 3 cases: +// 1 - sparse op dense +// 2 - dense op sparse +// 3 - sparse op sparse +// We also need to implement a 4th iterator for: +// 4 - dense op dense +// Finally, we also need to distinguish between the product and other operations : +// configuration returned mode +// 1 - sparse op dense product sparse +// generic dense +// 2 - dense op sparse product sparse +// generic dense +// 3 - sparse op sparse product sparse +// generic sparse +// 4 - dense op dense product dense +// generic dense +// +// TODO to ease compiler job, we could specialize product/quotient with a scalar +// and fallback to cwise-unary evaluator using bind1st_op and bind2nd_op. + +template +class CwiseBinaryOpImpl + : public SparseMatrixBase > +{ + public: + typedef CwiseBinaryOp Derived; + typedef SparseMatrixBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(Derived) + CwiseBinaryOpImpl() + { + EIGEN_STATIC_ASSERT(( + (!internal::is_same::StorageKind, + typename internal::traits::StorageKind>::value) + || ((internal::evaluator::Flags&RowMajorBit) == (internal::evaluator::Flags&RowMajorBit))), + THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH); + } +}; + +namespace internal { + + +// Generic "sparse OP sparse" +template struct binary_sparse_evaluator; + +template +struct binary_evaluator, IteratorBased, IteratorBased> + : evaluator_base > +{ +protected: + typedef typename evaluator::InnerIterator LhsIterator; + typedef typename evaluator::InnerIterator RhsIterator; + typedef CwiseBinaryOp XprType; + typedef typename traits::Scalar Scalar; + typedef typename XprType::StorageIndex StorageIndex; +public: + + class InnerIterator + { + public: + + EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer) + : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor) + { + this->operator++(); + } + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + if (m_lhsIter && m_rhsIter && (m_lhsIter.index() == m_rhsIter.index())) + { + m_id = m_lhsIter.index(); + m_value = m_functor(m_lhsIter.value(), m_rhsIter.value()); + ++m_lhsIter; + ++m_rhsIter; + } + else if (m_lhsIter && (!m_rhsIter || (m_lhsIter.index() < m_rhsIter.index()))) + { + m_id = m_lhsIter.index(); + m_value = m_functor(m_lhsIter.value(), Scalar(0)); + ++m_lhsIter; + } + else if (m_rhsIter && (!m_lhsIter || (m_lhsIter.index() > m_rhsIter.index()))) + { + m_id = m_rhsIter.index(); + m_value = m_functor(Scalar(0), m_rhsIter.value()); + ++m_rhsIter; + } + else + { + m_value = Scalar(0); // this is to avoid a compilation warning + m_id = -1; + } + return *this; + } + + EIGEN_STRONG_INLINE Scalar value() const { return m_value; } + + EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; } + EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); } + EIGEN_STRONG_INLINE Index row() const { return Lhs::IsRowMajor ? m_lhsIter.row() : index(); } + EIGEN_STRONG_INLINE Index col() const { return Lhs::IsRowMajor ? index() : m_lhsIter.col(); } + + EIGEN_STRONG_INLINE operator bool() const { return m_id>=0; } + + protected: + LhsIterator m_lhsIter; + RhsIterator m_rhsIter; + const BinaryOp& m_functor; + Scalar m_value; + StorageIndex m_id; + }; + + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit binary_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_lhsImpl(xpr.lhs()), + m_rhsImpl(xpr.rhs()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_lhsImpl.nonZerosEstimate() + m_rhsImpl.nonZerosEstimate(); + } + +protected: + const BinaryOp m_functor; + evaluator m_lhsImpl; + evaluator m_rhsImpl; +}; + +// dense op sparse +template +struct binary_evaluator, IndexBased, IteratorBased> + : evaluator_base > +{ +protected: + typedef typename evaluator::InnerIterator RhsIterator; + typedef CwiseBinaryOp XprType; + typedef typename traits::Scalar Scalar; + typedef typename XprType::StorageIndex StorageIndex; +public: + + class InnerIterator + { + enum { IsRowMajor = (int(Rhs::Flags)&RowMajorBit)==RowMajorBit }; + public: + + EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer) + : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.rhs().innerSize()) + { + this->operator++(); + } + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + ++m_id; + if(m_id &m_lhsEval; + RhsIterator m_rhsIter; + const BinaryOp& m_functor; + Scalar m_value; + StorageIndex m_id; + StorageIndex m_innerSize; + }; + + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit binary_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_lhsImpl(xpr.lhs()), + m_rhsImpl(xpr.rhs()), + m_expr(xpr) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_expr.size(); + } + +protected: + const BinaryOp m_functor; + evaluator m_lhsImpl; + evaluator m_rhsImpl; + const XprType &m_expr; +}; + +// sparse op dense +template +struct binary_evaluator, IteratorBased, IndexBased> + : evaluator_base > +{ +protected: + typedef typename evaluator::InnerIterator LhsIterator; + typedef CwiseBinaryOp XprType; + typedef typename traits::Scalar Scalar; + typedef typename XprType::StorageIndex StorageIndex; +public: + + class InnerIterator + { + enum { IsRowMajor = (int(Lhs::Flags)&RowMajorBit)==RowMajorBit }; + public: + + EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer) + : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.lhs().innerSize()) + { + this->operator++(); + } + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + ++m_id; + if(m_id &m_rhsEval; + const BinaryOp& m_functor; + Scalar m_value; + StorageIndex m_id; + StorageIndex m_innerSize; + }; + + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit binary_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_lhsImpl(xpr.lhs()), + m_rhsImpl(xpr.rhs()), + m_expr(xpr) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_expr.size(); + } + +protected: + const BinaryOp m_functor; + evaluator m_lhsImpl; + evaluator m_rhsImpl; + const XprType &m_expr; +}; + +template::Kind, + typename RhsKind = typename evaluator_traits::Kind, + typename LhsScalar = typename traits::Scalar, + typename RhsScalar = typename traits::Scalar> struct sparse_conjunction_evaluator; + +// "sparse .* sparse" +template +struct binary_evaluator, Lhs, Rhs>, IteratorBased, IteratorBased> + : sparse_conjunction_evaluator, Lhs, Rhs> > +{ + typedef CwiseBinaryOp, Lhs, Rhs> XprType; + typedef sparse_conjunction_evaluator Base; + explicit binary_evaluator(const XprType& xpr) : Base(xpr) {} +}; +// "dense .* sparse" +template +struct binary_evaluator, Lhs, Rhs>, IndexBased, IteratorBased> + : sparse_conjunction_evaluator, Lhs, Rhs> > +{ + typedef CwiseBinaryOp, Lhs, Rhs> XprType; + typedef sparse_conjunction_evaluator Base; + explicit binary_evaluator(const XprType& xpr) : Base(xpr) {} +}; +// "sparse .* dense" +template +struct binary_evaluator, Lhs, Rhs>, IteratorBased, IndexBased> + : sparse_conjunction_evaluator, Lhs, Rhs> > +{ + typedef CwiseBinaryOp, Lhs, Rhs> XprType; + typedef sparse_conjunction_evaluator Base; + explicit binary_evaluator(const XprType& xpr) : Base(xpr) {} +}; + +// "sparse ./ dense" +template +struct binary_evaluator, Lhs, Rhs>, IteratorBased, IndexBased> + : sparse_conjunction_evaluator, Lhs, Rhs> > +{ + typedef CwiseBinaryOp, Lhs, Rhs> XprType; + typedef sparse_conjunction_evaluator Base; + explicit binary_evaluator(const XprType& xpr) : Base(xpr) {} +}; + +// "sparse && sparse" +template +struct binary_evaluator, IteratorBased, IteratorBased> + : sparse_conjunction_evaluator > +{ + typedef CwiseBinaryOp XprType; + typedef sparse_conjunction_evaluator Base; + explicit binary_evaluator(const XprType& xpr) : Base(xpr) {} +}; +// "dense && sparse" +template +struct binary_evaluator, IndexBased, IteratorBased> + : sparse_conjunction_evaluator > +{ + typedef CwiseBinaryOp XprType; + typedef sparse_conjunction_evaluator Base; + explicit binary_evaluator(const XprType& xpr) : Base(xpr) {} +}; +// "sparse && dense" +template +struct binary_evaluator, IteratorBased, IndexBased> + : sparse_conjunction_evaluator > +{ + typedef CwiseBinaryOp XprType; + typedef sparse_conjunction_evaluator Base; + explicit binary_evaluator(const XprType& xpr) : Base(xpr) {} +}; + +// "sparse ^ sparse" +template +struct sparse_conjunction_evaluator + : evaluator_base +{ +protected: + typedef typename XprType::Functor BinaryOp; + typedef typename XprType::Lhs LhsArg; + typedef typename XprType::Rhs RhsArg; + typedef typename evaluator::InnerIterator LhsIterator; + typedef typename evaluator::InnerIterator RhsIterator; + typedef typename XprType::StorageIndex StorageIndex; + typedef typename traits::Scalar Scalar; +public: + + class InnerIterator + { + public: + + EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer) + : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor) + { + while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index())) + { + if (m_lhsIter.index() < m_rhsIter.index()) + ++m_lhsIter; + else + ++m_rhsIter; + } + } + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + ++m_lhsIter; + ++m_rhsIter; + while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index())) + { + if (m_lhsIter.index() < m_rhsIter.index()) + ++m_lhsIter; + else + ++m_rhsIter; + } + return *this; + } + + EIGEN_STRONG_INLINE Scalar value() const { return m_functor(m_lhsIter.value(), m_rhsIter.value()); } + + EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); } + EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); } + EIGEN_STRONG_INLINE Index row() const { return m_lhsIter.row(); } + EIGEN_STRONG_INLINE Index col() const { return m_lhsIter.col(); } + + EIGEN_STRONG_INLINE operator bool() const { return (m_lhsIter && m_rhsIter); } + + protected: + LhsIterator m_lhsIter; + RhsIterator m_rhsIter; + const BinaryOp& m_functor; + }; + + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit sparse_conjunction_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_lhsImpl(xpr.lhs()), + m_rhsImpl(xpr.rhs()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return (std::min)(m_lhsImpl.nonZerosEstimate(), m_rhsImpl.nonZerosEstimate()); + } + +protected: + const BinaryOp m_functor; + evaluator m_lhsImpl; + evaluator m_rhsImpl; +}; + +// "dense ^ sparse" +template +struct sparse_conjunction_evaluator + : evaluator_base +{ +protected: + typedef typename XprType::Functor BinaryOp; + typedef typename XprType::Lhs LhsArg; + typedef typename XprType::Rhs RhsArg; + typedef evaluator LhsEvaluator; + typedef typename evaluator::InnerIterator RhsIterator; + typedef typename XprType::StorageIndex StorageIndex; + typedef typename traits::Scalar Scalar; +public: + + class InnerIterator + { + enum { IsRowMajor = (int(RhsArg::Flags)&RowMajorBit)==RowMajorBit }; + + public: + + EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer) + : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_outer(outer) + {} + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + ++m_rhsIter; + return *this; + } + + EIGEN_STRONG_INLINE Scalar value() const + { return m_functor(m_lhsEval.coeff(IsRowMajor?m_outer:m_rhsIter.index(),IsRowMajor?m_rhsIter.index():m_outer), m_rhsIter.value()); } + + EIGEN_STRONG_INLINE StorageIndex index() const { return m_rhsIter.index(); } + EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); } + EIGEN_STRONG_INLINE Index row() const { return m_rhsIter.row(); } + EIGEN_STRONG_INLINE Index col() const { return m_rhsIter.col(); } + + EIGEN_STRONG_INLINE operator bool() const { return m_rhsIter; } + + protected: + const LhsEvaluator &m_lhsEval; + RhsIterator m_rhsIter; + const BinaryOp& m_functor; + const Index m_outer; + }; + + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit sparse_conjunction_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_lhsImpl(xpr.lhs()), + m_rhsImpl(xpr.rhs()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_rhsImpl.nonZerosEstimate(); + } + +protected: + const BinaryOp m_functor; + evaluator m_lhsImpl; + evaluator m_rhsImpl; +}; + +// "sparse ^ dense" +template +struct sparse_conjunction_evaluator + : evaluator_base +{ +protected: + typedef typename XprType::Functor BinaryOp; + typedef typename XprType::Lhs LhsArg; + typedef typename XprType::Rhs RhsArg; + typedef typename evaluator::InnerIterator LhsIterator; + typedef evaluator RhsEvaluator; + typedef typename XprType::StorageIndex StorageIndex; + typedef typename traits::Scalar Scalar; +public: + + class InnerIterator + { + enum { IsRowMajor = (int(LhsArg::Flags)&RowMajorBit)==RowMajorBit }; + + public: + + EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer) + : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_outer(outer) + {} + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + ++m_lhsIter; + return *this; + } + + EIGEN_STRONG_INLINE Scalar value() const + { return m_functor(m_lhsIter.value(), + m_rhsEval.coeff(IsRowMajor?m_outer:m_lhsIter.index(),IsRowMajor?m_lhsIter.index():m_outer)); } + + EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); } + EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); } + EIGEN_STRONG_INLINE Index row() const { return m_lhsIter.row(); } + EIGEN_STRONG_INLINE Index col() const { return m_lhsIter.col(); } + + EIGEN_STRONG_INLINE operator bool() const { return m_lhsIter; } + + protected: + LhsIterator m_lhsIter; + const evaluator &m_rhsEval; + const BinaryOp& m_functor; + const Index m_outer; + }; + + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit sparse_conjunction_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_lhsImpl(xpr.lhs()), + m_rhsImpl(xpr.rhs()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_lhsImpl.nonZerosEstimate(); + } + +protected: + const BinaryOp m_functor; + evaluator m_lhsImpl; + evaluator m_rhsImpl; +}; + +} + +/*************************************************************************** +* Implementation of SparseMatrixBase and SparseCwise functions/operators +***************************************************************************/ + +template +template +Derived& SparseMatrixBase::operator+=(const EigenBase &other) +{ + call_assignment(derived(), other.derived(), internal::add_assign_op()); + return derived(); +} + +template +template +Derived& SparseMatrixBase::operator-=(const EigenBase &other) +{ + call_assignment(derived(), other.derived(), internal::assign_op()); + return derived(); +} + +template +template +EIGEN_STRONG_INLINE Derived & +SparseMatrixBase::operator-=(const SparseMatrixBase &other) +{ + return derived() = derived() - other.derived(); +} + +template +template +EIGEN_STRONG_INLINE Derived & +SparseMatrixBase::operator+=(const SparseMatrixBase& other) +{ + return derived() = derived() + other.derived(); +} + +template +template +Derived& SparseMatrixBase::operator+=(const DiagonalBase& other) +{ + call_assignment_no_alias(derived(), other.derived(), internal::add_assign_op()); + return derived(); +} + +template +template +Derived& SparseMatrixBase::operator-=(const DiagonalBase& other) +{ + call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op()); + return derived(); +} + +template +template +EIGEN_STRONG_INLINE const typename SparseMatrixBase::template CwiseProductDenseReturnType::Type +SparseMatrixBase::cwiseProduct(const MatrixBase &other) const +{ + return typename CwiseProductDenseReturnType::Type(derived(), other.derived()); +} + +template +EIGEN_STRONG_INLINE const CwiseBinaryOp, const DenseDerived, const SparseDerived> +operator+(const MatrixBase &a, const SparseMatrixBase &b) +{ + return CwiseBinaryOp, const DenseDerived, const SparseDerived>(a.derived(), b.derived()); +} + +template +EIGEN_STRONG_INLINE const CwiseBinaryOp, const SparseDerived, const DenseDerived> +operator+(const SparseMatrixBase &a, const MatrixBase &b) +{ + return CwiseBinaryOp, const SparseDerived, const DenseDerived>(a.derived(), b.derived()); +} + +template +EIGEN_STRONG_INLINE const CwiseBinaryOp, const DenseDerived, const SparseDerived> +operator-(const MatrixBase &a, const SparseMatrixBase &b) +{ + return CwiseBinaryOp, const DenseDerived, const SparseDerived>(a.derived(), b.derived()); +} + +template +EIGEN_STRONG_INLINE const CwiseBinaryOp, const SparseDerived, const DenseDerived> +operator-(const SparseMatrixBase &a, const MatrixBase &b) +{ + return CwiseBinaryOp, const SparseDerived, const DenseDerived>(a.derived(), b.derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_CWISE_BINARY_OP_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h new file mode 100644 index 0000000000..32dac0f786 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h @@ -0,0 +1,150 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2015 Gael Guennebaud +// +// 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_SPARSE_CWISE_UNARY_OP_H +#define EIGEN_SPARSE_CWISE_UNARY_OP_H + +namespace Eigen { + +namespace internal { + +template +struct unary_evaluator, IteratorBased> + : public evaluator_base > +{ + public: + typedef CwiseUnaryOp XprType; + + class InnerIterator; + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_argImpl.nonZerosEstimate(); + } + + protected: + typedef typename evaluator::InnerIterator EvalIterator; + + const UnaryOp m_functor; + evaluator m_argImpl; +}; + +template +class unary_evaluator, IteratorBased>::InnerIterator + : public unary_evaluator, IteratorBased>::EvalIterator +{ + protected: + typedef typename XprType::Scalar Scalar; + typedef typename unary_evaluator, IteratorBased>::EvalIterator Base; + public: + + EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer) + : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor) + {} + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { Base::operator++(); return *this; } + + EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); } + + protected: + const UnaryOp m_functor; + private: + Scalar& valueRef(); +}; + +template +struct unary_evaluator, IteratorBased> + : public evaluator_base > +{ + public: + typedef CwiseUnaryView XprType; + + class InnerIterator; + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + Flags = XprType::Flags + }; + + explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + protected: + typedef typename evaluator::InnerIterator EvalIterator; + + const ViewOp m_functor; + evaluator m_argImpl; +}; + +template +class unary_evaluator, IteratorBased>::InnerIterator + : public unary_evaluator, IteratorBased>::EvalIterator +{ + protected: + typedef typename XprType::Scalar Scalar; + typedef typename unary_evaluator, IteratorBased>::EvalIterator Base; + public: + + EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer) + : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor) + {} + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { Base::operator++(); return *this; } + + EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); } + EIGEN_STRONG_INLINE Scalar& valueRef() { return m_functor(Base::valueRef()); } + + protected: + const ViewOp m_functor; +}; + +} // end namespace internal + +template +EIGEN_STRONG_INLINE Derived& +SparseMatrixBase::operator*=(const Scalar& other) +{ + typedef typename internal::evaluator::InnerIterator EvalIterator; + internal::evaluator thisEval(derived()); + for (Index j=0; j +EIGEN_STRONG_INLINE Derived& +SparseMatrixBase::operator/=(const Scalar& other) +{ + typedef typename internal::evaluator::InnerIterator EvalIterator; + internal::evaluator thisEval(derived()); + for (Index j=0; j +// +// 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_SPARSEDENSEPRODUCT_H +#define EIGEN_SPARSEDENSEPRODUCT_H + +namespace Eigen { + +namespace internal { + +template <> struct product_promote_storage_type { typedef Sparse ret; }; +template <> struct product_promote_storage_type { typedef Sparse ret; }; + +template +struct sparse_time_dense_product_impl; + +template +struct sparse_time_dense_product_impl +{ + typedef typename internal::remove_all::type Lhs; + typedef typename internal::remove_all::type Rhs; + typedef typename internal::remove_all::type Res; + typedef typename evaluator::InnerIterator LhsInnerIterator; + typedef evaluator LhsEval; + static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha) + { + LhsEval lhsEval(lhs); + + Index n = lhs.outerSize(); +#ifdef EIGEN_HAS_OPENMP + Eigen::initParallel(); + Index threads = Eigen::nbThreads(); +#endif + + for(Index c=0; c1 && lhsEval.nonZerosEstimate() > 20000) + { + #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads) + for(Index i=0; i let's disable it for now as it is conflicting with generic scalar*matrix and matrix*scalar operators +// template +// struct ScalarBinaryOpTraits > +// { +// enum { +// Defined = 1 +// }; +// typedef typename CwiseUnaryOp, T2>::PlainObject ReturnType; +// }; + +template +struct sparse_time_dense_product_impl +{ + typedef typename internal::remove_all::type Lhs; + typedef typename internal::remove_all::type Rhs; + typedef typename internal::remove_all::type Res; + typedef evaluator LhsEval; + typedef typename LhsEval::InnerIterator LhsInnerIterator; + static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha) + { + LhsEval lhsEval(lhs); + for(Index c=0; c::ReturnType rhs_j(alpha * rhs.coeff(j,c)); + for(LhsInnerIterator it(lhsEval,j); it ;++it) + res.coeffRef(it.index(),c) += it.value() * rhs_j; + } + } + } +}; + +template +struct sparse_time_dense_product_impl +{ + typedef typename internal::remove_all::type Lhs; + typedef typename internal::remove_all::type Rhs; + typedef typename internal::remove_all::type Res; + typedef evaluator LhsEval; + typedef typename LhsEval::InnerIterator LhsInnerIterator; + static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha) + { + Index n = lhs.rows(); + LhsEval lhsEval(lhs); + +#ifdef EIGEN_HAS_OPENMP + Eigen::initParallel(); + Index threads = Eigen::nbThreads(); + // This 20000 threshold has been found experimentally on 2D and 3D Poisson problems. + // It basically represents the minimal amount of work to be done to be worth it. + if(threads>1 && lhsEval.nonZerosEstimate()*rhs.cols() > 20000) + { + #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads) + for(Index i=0; i +struct sparse_time_dense_product_impl +{ + typedef typename internal::remove_all::type Lhs; + typedef typename internal::remove_all::type Rhs; + typedef typename internal::remove_all::type Res; + typedef typename evaluator::InnerIterator LhsInnerIterator; + static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha) + { + evaluator lhsEval(lhs); + for(Index j=0; j +inline void sparse_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha) +{ + sparse_time_dense_product_impl::run(lhs, rhs, res, alpha); +} + +} // end namespace internal + +namespace internal { + +template +struct generic_product_impl + : generic_product_impl_base > +{ + typedef typename Product::Scalar Scalar; + + template + static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + LhsNested lhsNested(lhs); + RhsNested rhsNested(rhs); + internal::sparse_time_dense_product(lhsNested, rhsNested, dst, alpha); + } +}; + +template +struct generic_product_impl + : generic_product_impl +{}; + +template +struct generic_product_impl + : generic_product_impl_base > +{ + typedef typename Product::Scalar Scalar; + + template + static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + LhsNested lhsNested(lhs); + RhsNested rhsNested(rhs); + + // transpose everything + Transpose dstT(dst); + internal::sparse_time_dense_product(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha); + } +}; + +template +struct generic_product_impl + : generic_product_impl +{}; + +template +struct sparse_dense_outer_product_evaluator +{ +protected: + typedef typename conditional::type Lhs1; + typedef typename conditional::type ActualRhs; + typedef Product ProdXprType; + + // if the actual left-hand side is a dense vector, + // then build a sparse-view so that we can seamlessly iterate over it. + typedef typename conditional::StorageKind,Sparse>::value, + Lhs1, SparseView >::type ActualLhs; + typedef typename conditional::StorageKind,Sparse>::value, + Lhs1 const&, SparseView >::type LhsArg; + + typedef evaluator LhsEval; + typedef evaluator RhsEval; + typedef typename evaluator::InnerIterator LhsIterator; + typedef typename ProdXprType::Scalar Scalar; + +public: + enum { + Flags = NeedToTranspose ? RowMajorBit : 0, + CoeffReadCost = HugeCost + }; + + class InnerIterator : public LhsIterator + { + public: + InnerIterator(const sparse_dense_outer_product_evaluator &xprEval, Index outer) + : LhsIterator(xprEval.m_lhsXprImpl, 0), + m_outer(outer), + m_empty(false), + m_factor(get(xprEval.m_rhsXprImpl, outer, typename internal::traits::StorageKind() )) + {} + + EIGEN_STRONG_INLINE Index outer() const { return m_outer; } + EIGEN_STRONG_INLINE Index row() const { return NeedToTranspose ? m_outer : LhsIterator::index(); } + EIGEN_STRONG_INLINE Index col() const { return NeedToTranspose ? LhsIterator::index() : m_outer; } + + EIGEN_STRONG_INLINE Scalar value() const { return LhsIterator::value() * m_factor; } + EIGEN_STRONG_INLINE operator bool() const { return LhsIterator::operator bool() && (!m_empty); } + + protected: + Scalar get(const RhsEval &rhs, Index outer, Dense = Dense()) const + { + return rhs.coeff(outer); + } + + Scalar get(const RhsEval &rhs, Index outer, Sparse = Sparse()) + { + typename RhsEval::InnerIterator it(rhs, outer); + if (it && it.index()==0 && it.value()!=Scalar(0)) + return it.value(); + m_empty = true; + return Scalar(0); + } + + Index m_outer; + bool m_empty; + Scalar m_factor; + }; + + sparse_dense_outer_product_evaluator(const Lhs1 &lhs, const ActualRhs &rhs) + : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + // transpose case + sparse_dense_outer_product_evaluator(const ActualRhs &rhs, const Lhs1 &lhs) + : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + +protected: + const LhsArg m_lhs; + evaluator m_lhsXprImpl; + evaluator m_rhsXprImpl; +}; + +// sparse * dense outer product +template +struct product_evaluator, OuterProduct, SparseShape, DenseShape> + : sparse_dense_outer_product_evaluator +{ + typedef sparse_dense_outer_product_evaluator Base; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + + explicit product_evaluator(const XprType& xpr) + : Base(xpr.lhs(), xpr.rhs()) + {} + +}; + +template +struct product_evaluator, OuterProduct, DenseShape, SparseShape> + : sparse_dense_outer_product_evaluator +{ + typedef sparse_dense_outer_product_evaluator Base; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + + explicit product_evaluator(const XprType& xpr) + : Base(xpr.lhs(), xpr.rhs()) + {} + +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSEDENSEPRODUCT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h new file mode 100644 index 0000000000..941c03be3d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h @@ -0,0 +1,138 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-2015 Gael Guennebaud +// +// 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_SPARSE_DIAGONAL_PRODUCT_H +#define EIGEN_SPARSE_DIAGONAL_PRODUCT_H + +namespace Eigen { + +// The product of a diagonal matrix with a sparse matrix can be easily +// implemented using expression template. +// We have two consider very different cases: +// 1 - diag * row-major sparse +// => each inner vector <=> scalar * sparse vector product +// => so we can reuse CwiseUnaryOp::InnerIterator +// 2 - diag * col-major sparse +// => each inner vector <=> densevector * sparse vector cwise product +// => again, we can reuse specialization of CwiseBinaryOp::InnerIterator +// for that particular case +// The two other cases are symmetric. + +namespace internal { + +enum { + SDP_AsScalarProduct, + SDP_AsCwiseProduct +}; + +template +struct sparse_diagonal_product_evaluator; + +template +struct product_evaluator, ProductTag, DiagonalShape, SparseShape> + : public sparse_diagonal_product_evaluator +{ + typedef Product XprType; + enum { CoeffReadCost = HugeCost, Flags = Rhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags + + typedef sparse_diagonal_product_evaluator Base; + explicit product_evaluator(const XprType& xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {} +}; + +template +struct product_evaluator, ProductTag, SparseShape, DiagonalShape> + : public sparse_diagonal_product_evaluator, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct> +{ + typedef Product XprType; + enum { CoeffReadCost = HugeCost, Flags = Lhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags + + typedef sparse_diagonal_product_evaluator, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct> Base; + explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs().diagonal().transpose()) {} +}; + +template +struct sparse_diagonal_product_evaluator +{ +protected: + typedef typename evaluator::InnerIterator SparseXprInnerIterator; + typedef typename SparseXprType::Scalar Scalar; + +public: + class InnerIterator : public SparseXprInnerIterator + { + public: + InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer) + : SparseXprInnerIterator(xprEval.m_sparseXprImpl, outer), + m_coeff(xprEval.m_diagCoeffImpl.coeff(outer)) + {} + + EIGEN_STRONG_INLINE Scalar value() const { return m_coeff * SparseXprInnerIterator::value(); } + protected: + typename DiagonalCoeffType::Scalar m_coeff; + }; + + sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagonalCoeffType &diagCoeff) + : m_sparseXprImpl(sparseXpr), m_diagCoeffImpl(diagCoeff) + {} + + Index nonZerosEstimate() const { return m_sparseXprImpl.nonZerosEstimate(); } + +protected: + evaluator m_sparseXprImpl; + evaluator m_diagCoeffImpl; +}; + + +template +struct sparse_diagonal_product_evaluator +{ + typedef typename SparseXprType::Scalar Scalar; + typedef typename SparseXprType::StorageIndex StorageIndex; + + typedef typename nested_eval::type DiagCoeffNested; + + class InnerIterator + { + typedef typename evaluator::InnerIterator SparseXprIter; + public: + InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer) + : m_sparseIter(xprEval.m_sparseXprEval, outer), m_diagCoeffNested(xprEval.m_diagCoeffNested) + {} + + inline Scalar value() const { return m_sparseIter.value() * m_diagCoeffNested.coeff(index()); } + inline StorageIndex index() const { return m_sparseIter.index(); } + inline Index outer() const { return m_sparseIter.outer(); } + inline Index col() const { return SparseXprType::IsRowMajor ? m_sparseIter.index() : m_sparseIter.outer(); } + inline Index row() const { return SparseXprType::IsRowMajor ? m_sparseIter.outer() : m_sparseIter.index(); } + + EIGEN_STRONG_INLINE InnerIterator& operator++() { ++m_sparseIter; return *this; } + inline operator bool() const { return m_sparseIter; } + + protected: + SparseXprIter m_sparseIter; + DiagCoeffNested m_diagCoeffNested; + }; + + sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagCoeffType &diagCoeff) + : m_sparseXprEval(sparseXpr), m_diagCoeffNested(diagCoeff) + {} + + Index nonZerosEstimate() const { return m_sparseXprEval.nonZerosEstimate(); } + +protected: + evaluator m_sparseXprEval; + DiagCoeffNested m_diagCoeffNested; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_DIAGONAL_PRODUCT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h new file mode 100644 index 0000000000..38bc4aa9ea --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h @@ -0,0 +1,98 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// 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_SPARSE_DOT_H +#define EIGEN_SPARSE_DOT_H + +namespace Eigen { + +template +template +typename internal::traits::Scalar +SparseMatrixBase::dot(const MatrixBase& other) const +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) + EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) + EIGEN_STATIC_ASSERT((internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + eigen_assert(size() == other.size()); + eigen_assert(other.size()>0 && "you are using a non initialized vector"); + + internal::evaluator thisEval(derived()); + typename internal::evaluator::InnerIterator i(thisEval, 0); + Scalar res(0); + while (i) + { + res += numext::conj(i.value()) * other.coeff(i.index()); + ++i; + } + return res; +} + +template +template +typename internal::traits::Scalar +SparseMatrixBase::dot(const SparseMatrixBase& other) const +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) + EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) + EIGEN_STATIC_ASSERT((internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + eigen_assert(size() == other.size()); + + internal::evaluator thisEval(derived()); + typename internal::evaluator::InnerIterator i(thisEval, 0); + + internal::evaluator otherEval(other.derived()); + typename internal::evaluator::InnerIterator j(otherEval, 0); + + Scalar res(0); + while (i && j) + { + if (i.index()==j.index()) + { + res += numext::conj(i.value()) * j.value(); + ++i; ++j; + } + else if (i.index() +inline typename NumTraits::Scalar>::Real +SparseMatrixBase::squaredNorm() const +{ + return numext::real((*this).cwiseAbs2().sum()); +} + +template +inline typename NumTraits::Scalar>::Real +SparseMatrixBase::norm() const +{ + using std::sqrt; + return sqrt(squaredNorm()); +} + +template +inline typename NumTraits::Scalar>::Real +SparseMatrixBase::blueNorm() const +{ + return internal::blueNorm_impl(*this); +} +} // end namespace Eigen + +#endif // EIGEN_SPARSE_DOT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h new file mode 100644 index 0000000000..7d47eb94d2 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h @@ -0,0 +1,29 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSE_FUZZY_H +#define EIGEN_SPARSE_FUZZY_H + +namespace Eigen { + +template +template +bool SparseMatrixBase::isApprox(const SparseMatrixBase& other, const RealScalar &prec) const +{ + const typename internal::nested_eval::type actualA(derived()); + typename internal::conditional::type, + const PlainObject>::type actualB(other.derived()); + + return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm()); +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_FUZZY_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h new file mode 100644 index 0000000000..f99be3379d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h @@ -0,0 +1,305 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Gael Guennebaud +// +// 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_SPARSE_MAP_H +#define EIGEN_SPARSE_MAP_H + +namespace Eigen { + +namespace internal { + +template +struct traits, Options, StrideType> > + : public traits > +{ + typedef SparseMatrix PlainObjectType; + typedef traits TraitsBase; + enum { + Flags = TraitsBase::Flags & (~NestByRefBit) + }; +}; + +template +struct traits, Options, StrideType> > + : public traits > +{ + typedef SparseMatrix PlainObjectType; + typedef traits TraitsBase; + enum { + Flags = TraitsBase::Flags & (~ (NestByRefBit | LvalueBit)) + }; +}; + +} // end namespace internal + +template::has_write_access ? WriteAccessors : ReadOnlyAccessors +> class SparseMapBase; + +/** \ingroup SparseCore_Module + * class SparseMapBase + * \brief Common base class for Map and Ref instance of sparse matrix and vector. + */ +template +class SparseMapBase + : public SparseCompressedBase +{ + public: + typedef SparseCompressedBase Base; + typedef typename Base::Scalar Scalar; + typedef typename Base::StorageIndex StorageIndex; + enum { IsRowMajor = Base::IsRowMajor }; + using Base::operator=; + protected: + + typedef typename internal::conditional< + bool(internal::is_lvalue::value), + Scalar *, const Scalar *>::type ScalarPointer; + typedef typename internal::conditional< + bool(internal::is_lvalue::value), + StorageIndex *, const StorageIndex *>::type IndexPointer; + + Index m_outerSize; + Index m_innerSize; + Array m_zero_nnz; + IndexPointer m_outerIndex; + IndexPointer m_innerIndices; + ScalarPointer m_values; + IndexPointer m_innerNonZeros; + + public: + + /** \copydoc SparseMatrixBase::rows() */ + inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; } + /** \copydoc SparseMatrixBase::cols() */ + inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; } + /** \copydoc SparseMatrixBase::innerSize() */ + inline Index innerSize() const { return m_innerSize; } + /** \copydoc SparseMatrixBase::outerSize() */ + inline Index outerSize() const { return m_outerSize; } + /** \copydoc SparseCompressedBase::nonZeros */ + inline Index nonZeros() const { return m_zero_nnz[1]; } + + /** \copydoc SparseCompressedBase::isCompressed */ + bool isCompressed() const { return m_innerNonZeros==0; } + + //---------------------------------------- + // direct access interface + /** \copydoc SparseMatrix::valuePtr */ + inline const Scalar* valuePtr() const { return m_values; } + /** \copydoc SparseMatrix::innerIndexPtr */ + inline const StorageIndex* innerIndexPtr() const { return m_innerIndices; } + /** \copydoc SparseMatrix::outerIndexPtr */ + inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; } + /** \copydoc SparseMatrix::innerNonZeroPtr */ + inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; } + //---------------------------------------- + + /** \copydoc SparseMatrix::coeff */ + inline Scalar coeff(Index row, Index col) const + { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + Index start = m_outerIndex[outer]; + Index end = isCompressed() ? m_outerIndex[outer+1] : start + m_innerNonZeros[outer]; + if (start==end) + return Scalar(0); + else if (end>0 && inner==m_innerIndices[end-1]) + return m_values[end-1]; + // ^^ optimization: let's first check if it is the last coefficient + // (very common in high level algorithms) + + const StorageIndex* r = std::lower_bound(&m_innerIndices[start],&m_innerIndices[end-1],inner); + const Index id = r-&m_innerIndices[0]; + return ((*r==inner) && (id(nnz)), m_outerIndex(outerIndexPtr), + m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(innerNonZerosPtr) + {} + + // for vectors + inline SparseMapBase(Index size, Index nnz, IndexPointer innerIndexPtr, ScalarPointer valuePtr) + : m_outerSize(1), m_innerSize(size), m_zero_nnz(0,internal::convert_index(nnz)), m_outerIndex(m_zero_nnz.data()), + m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(0) + {} + + /** Empty destructor */ + inline ~SparseMapBase() {} + + protected: + inline SparseMapBase() {} +}; + +/** \ingroup SparseCore_Module + * class SparseMapBase + * \brief Common base class for writable Map and Ref instance of sparse matrix and vector. + */ +template +class SparseMapBase + : public SparseMapBase +{ + typedef MapBase ReadOnlyMapBase; + + public: + typedef SparseMapBase Base; + typedef typename Base::Scalar Scalar; + typedef typename Base::StorageIndex StorageIndex; + enum { IsRowMajor = Base::IsRowMajor }; + + using Base::operator=; + + public: + + //---------------------------------------- + // direct access interface + using Base::valuePtr; + using Base::innerIndexPtr; + using Base::outerIndexPtr; + using Base::innerNonZeroPtr; + /** \copydoc SparseMatrix::valuePtr */ + inline Scalar* valuePtr() { return Base::m_values; } + /** \copydoc SparseMatrix::innerIndexPtr */ + inline StorageIndex* innerIndexPtr() { return Base::m_innerIndices; } + /** \copydoc SparseMatrix::outerIndexPtr */ + inline StorageIndex* outerIndexPtr() { return Base::m_outerIndex; } + /** \copydoc SparseMatrix::innerNonZeroPtr */ + inline StorageIndex* innerNonZeroPtr() { return Base::m_innerNonZeros; } + //---------------------------------------- + + /** \copydoc SparseMatrix::coeffRef */ + inline Scalar& coeffRef(Index row, Index col) + { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + Index start = Base::m_outerIndex[outer]; + Index end = Base::isCompressed() ? Base::m_outerIndex[outer+1] : start + Base::m_innerNonZeros[outer]; + eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix"); + eigen_assert(end>start && "coeffRef cannot be called on a zero coefficient"); + StorageIndex* r = std::lower_bound(&Base::m_innerIndices[start],&Base::m_innerIndices[end],inner); + const Index id = r - &Base::m_innerIndices[0]; + eigen_assert((*r==inner) && (id(Base::m_values)[id]; + } + + inline SparseMapBase(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, + Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0) + : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr) + {} + + // for vectors + inline SparseMapBase(Index size, Index nnz, StorageIndex* innerIndexPtr, Scalar* valuePtr) + : Base(size, nnz, innerIndexPtr, valuePtr) + {} + + /** Empty destructor */ + inline ~SparseMapBase() {} + + protected: + inline SparseMapBase() {} +}; + +/** \ingroup SparseCore_Module + * + * \brief Specialization of class Map for SparseMatrix-like storage. + * + * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix. + * + * \sa class Map, class SparseMatrix, class Ref + */ +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +class Map, Options, StrideType> + : public SparseMapBase, Options, StrideType> > +#else +template +class Map + : public SparseMapBase +#endif +{ + public: + typedef SparseMapBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(Map) + enum { IsRowMajor = Base::IsRowMajor }; + + public: + + /** Constructs a read-write Map to a sparse matrix of size \a rows x \a cols, containing \a nnz non-zero coefficients, + * stored as a sparse format as defined by the pointers \a outerIndexPtr, \a innerIndexPtr, and \a valuePtr. + * If the optional parameter \a innerNonZerosPtr is the null pointer, then a standard compressed format is assumed. + * + * This constructor is available only if \c SparseMatrixType is non-const. + * + * More details on the expected storage schemes are given in the \ref TutorialSparse "manual pages". + */ + inline Map(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, + StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0) + : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr) + {} +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** Empty destructor */ + inline ~Map() {} +}; + +template +class Map, Options, StrideType> + : public SparseMapBase, Options, StrideType> > +{ + public: + typedef SparseMapBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(Map) + enum { IsRowMajor = Base::IsRowMajor }; + + public: +#endif + /** This is the const version of the above constructor. + * + * This constructor is available only if \c SparseMatrixType is const, e.g.: + * \code Map > \endcode + */ + inline Map(Index rows, Index cols, Index nnz, const StorageIndex* outerIndexPtr, + const StorageIndex* innerIndexPtr, const Scalar* valuePtr, const StorageIndex* innerNonZerosPtr = 0) + : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr) + {} + + /** Empty destructor */ + inline ~Map() {} +}; + +namespace internal { + +template +struct evaluator, Options, StrideType> > + : evaluator, Options, StrideType> > > +{ + typedef evaluator, Options, StrideType> > > Base; + typedef Map, Options, StrideType> XprType; + evaluator() : Base() {} + explicit evaluator(const XprType &mat) : Base(mat) {} +}; + +template +struct evaluator, Options, StrideType> > + : evaluator, Options, StrideType> > > +{ + typedef evaluator, Options, StrideType> > > Base; + typedef Map, Options, StrideType> XprType; + evaluator() : Base() {} + explicit evaluator(const XprType &mat) : Base(mat) {} +}; + +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_MAP_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h new file mode 100644 index 0000000000..616b4a0c24 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h @@ -0,0 +1,1518 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSEMATRIX_H +#define EIGEN_SPARSEMATRIX_H + +namespace Eigen { + +/** \ingroup SparseCore_Module + * + * \class SparseMatrix + * + * \brief A versatible sparse matrix representation + * + * This class implements a more versatile variants of the common \em compressed row/column storage format. + * Each colmun's (resp. row) non zeros are stored as a pair of value with associated row (resp. colmiun) index. + * All the non zeros are stored in a single large buffer. Unlike the \em compressed format, there might be extra + * space in between the nonzeros of two successive colmuns (resp. rows) such that insertion of new non-zero + * can be done with limited memory reallocation and copies. + * + * A call to the function makeCompressed() turns the matrix into the standard \em compressed format + * compatible with many library. + * + * More details on this storage sceheme are given in the \ref TutorialSparse "manual pages". + * + * \tparam _Scalar the scalar type, i.e. the type of the coefficients + * \tparam _Options Union of bit flags controlling the storage scheme. Currently the only possibility + * is ColMajor or RowMajor. The default is 0 which means column-major. + * \tparam _StorageIndex the type of the indices. It has to be a \b signed type (e.g., short, int, std::ptrdiff_t). Default is \c int. + * + * \warning In %Eigen 3.2, the undocumented type \c SparseMatrix::Index was improperly defined as the storage index type (e.g., int), + * whereas it is now (starting from %Eigen 3.3) deprecated and always defined as Eigen::Index. + * Codes making use of \c SparseMatrix::Index, might thus likely have to be changed to use \c SparseMatrix::StorageIndex instead. + * + * This class can be extended with the help of the plugin mechanism described on the page + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIX_PLUGIN. + */ + +namespace internal { +template +struct traits > +{ + typedef _Scalar Scalar; + typedef _StorageIndex StorageIndex; + typedef Sparse StorageKind; + typedef MatrixXpr XprKind; + enum { + RowsAtCompileTime = Dynamic, + ColsAtCompileTime = Dynamic, + MaxRowsAtCompileTime = Dynamic, + MaxColsAtCompileTime = Dynamic, + Flags = _Options | NestByRefBit | LvalueBit | CompressedAccessBit, + SupportedAccessPatterns = InnerRandomAccessPattern + }; +}; + +template +struct traits, DiagIndex> > +{ + typedef SparseMatrix<_Scalar, _Options, _StorageIndex> MatrixType; + typedef typename ref_selector::type MatrixTypeNested; + typedef typename remove_reference::type _MatrixTypeNested; + + typedef _Scalar Scalar; + typedef Dense StorageKind; + typedef _StorageIndex StorageIndex; + typedef MatrixXpr XprKind; + + enum { + RowsAtCompileTime = Dynamic, + ColsAtCompileTime = 1, + MaxRowsAtCompileTime = Dynamic, + MaxColsAtCompileTime = 1, + Flags = LvalueBit + }; +}; + +template +struct traits, DiagIndex> > + : public traits, DiagIndex> > +{ + enum { + Flags = 0 + }; +}; + +} // end namespace internal + +template +class SparseMatrix + : public SparseCompressedBase > +{ + typedef SparseCompressedBase Base; + using Base::convert_index; + friend class SparseVector<_Scalar,0,_StorageIndex>; + template + friend struct internal::Assignment; + public: + using Base::isCompressed; + using Base::nonZeros; + EIGEN_SPARSE_PUBLIC_INTERFACE(SparseMatrix) + using Base::operator+=; + using Base::operator-=; + + typedef MappedSparseMatrix Map; + typedef Diagonal DiagonalReturnType; + typedef Diagonal ConstDiagonalReturnType; + typedef typename Base::InnerIterator InnerIterator; + typedef typename Base::ReverseInnerIterator ReverseInnerIterator; + + + using Base::IsRowMajor; + typedef internal::CompressedStorage Storage; + enum { + Options = _Options + }; + + typedef typename Base::IndexVector IndexVector; + typedef typename Base::ScalarVector ScalarVector; + protected: + typedef SparseMatrix TransposedSparseMatrix; + + Index m_outerSize; + Index m_innerSize; + StorageIndex* m_outerIndex; + StorageIndex* m_innerNonZeros; // optional, if null then the data is compressed + Storage m_data; + + public: + + /** \returns the number of rows of the matrix */ + inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; } + /** \returns the number of columns of the matrix */ + inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; } + + /** \returns the number of rows (resp. columns) of the matrix if the storage order column major (resp. row major) */ + inline Index innerSize() const { return m_innerSize; } + /** \returns the number of columns (resp. rows) of the matrix if the storage order column major (resp. row major) */ + inline Index outerSize() const { return m_outerSize; } + + /** \returns a const pointer to the array of values. + * This function is aimed at interoperability with other libraries. + * \sa innerIndexPtr(), outerIndexPtr() */ + inline const Scalar* valuePtr() const { return m_data.valuePtr(); } + /** \returns a non-const pointer to the array of values. + * This function is aimed at interoperability with other libraries. + * \sa innerIndexPtr(), outerIndexPtr() */ + inline Scalar* valuePtr() { return m_data.valuePtr(); } + + /** \returns a const pointer to the array of inner indices. + * This function is aimed at interoperability with other libraries. + * \sa valuePtr(), outerIndexPtr() */ + inline const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); } + /** \returns a non-const pointer to the array of inner indices. + * This function is aimed at interoperability with other libraries. + * \sa valuePtr(), outerIndexPtr() */ + inline StorageIndex* innerIndexPtr() { return m_data.indexPtr(); } + + /** \returns a const pointer to the array of the starting positions of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \sa valuePtr(), innerIndexPtr() */ + inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; } + /** \returns a non-const pointer to the array of the starting positions of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \sa valuePtr(), innerIndexPtr() */ + inline StorageIndex* outerIndexPtr() { return m_outerIndex; } + + /** \returns a const pointer to the array of the number of non zeros of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \warning it returns the null pointer 0 in compressed mode */ + inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; } + /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors. + * This function is aimed at interoperability with other libraries. + * \warning it returns the null pointer 0 in compressed mode */ + inline StorageIndex* innerNonZeroPtr() { return m_innerNonZeros; } + + /** \internal */ + inline Storage& data() { return m_data; } + /** \internal */ + inline const Storage& data() const { return m_data; } + + /** \returns the value of the matrix at position \a i, \a j + * This function returns Scalar(0) if the element is an explicit \em zero */ + inline Scalar coeff(Index row, Index col) const + { + eigen_assert(row>=0 && row=0 && col=0 && row=0 && col=start && "you probably called coeffRef on a non finalized matrix"); + if(end<=start) + return insert(row,col); + const Index p = m_data.searchLowerIndex(start,end-1,StorageIndex(inner)); + if((pinnerSize() non zeros if reserve(Index) has not been called earlier. + * In this case, the insertion procedure is optimized for a \e sequential insertion mode where elements are assumed to be + * inserted by increasing outer-indices. + * + * If that's not the case, then it is strongly recommended to either use a triplet-list to assemble the matrix, or to first + * call reserve(const SizesType &) to reserve the appropriate number of non-zero elements per inner vector. + * + * Assuming memory has been appropriately reserved, this function performs a sorted insertion in O(1) + * if the elements of each inner vector are inserted in increasing inner index order, and in O(nnz_j) for a random insertion. + * + */ + Scalar& insert(Index row, Index col); + + public: + + /** Removes all non zeros but keep allocated memory + * + * This function does not free the currently allocated memory. To release as much as memory as possible, + * call \code mat.data().squeeze(); \endcode after resizing it. + * + * \sa resize(Index,Index), data() + */ + inline void setZero() + { + m_data.clear(); + memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex)); + if(m_innerNonZeros) + memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex)); + } + + /** Preallocates \a reserveSize non zeros. + * + * Precondition: the matrix must be in compressed mode. */ + inline void reserve(Index reserveSize) + { + eigen_assert(isCompressed() && "This function does not make sense in non compressed mode."); + m_data.reserve(reserveSize); + } + + #ifdef EIGEN_PARSED_BY_DOXYGEN + /** Preallocates \a reserveSize[\c j] non zeros for each column (resp. row) \c j. + * + * This function turns the matrix in non-compressed mode. + * + * The type \c SizesType must expose the following interface: + \code + typedef value_type; + const value_type& operator[](i) const; + \endcode + * for \c i in the [0,this->outerSize()[ range. + * Typical choices include std::vector, Eigen::VectorXi, Eigen::VectorXi::Constant, etc. + */ + template + inline void reserve(const SizesType& reserveSizes); + #else + template + inline void reserve(const SizesType& reserveSizes, const typename SizesType::value_type& enableif = + #if (!EIGEN_COMP_MSVC) || (EIGEN_COMP_MSVC>=1500) // MSVC 2005 fails to compile with this typename + typename + #endif + SizesType::value_type()) + { + EIGEN_UNUSED_VARIABLE(enableif); + reserveInnerVectors(reserveSizes); + } + #endif // EIGEN_PARSED_BY_DOXYGEN + protected: + template + inline void reserveInnerVectors(const SizesType& reserveSizes) + { + if(isCompressed()) + { + Index totalReserveSize = 0; + // turn the matrix into non-compressed mode + m_innerNonZeros = static_cast(std::malloc(m_outerSize * sizeof(StorageIndex))); + if (!m_innerNonZeros) internal::throw_std_bad_alloc(); + + // temporarily use m_innerSizes to hold the new starting points. + StorageIndex* newOuterIndex = m_innerNonZeros; + + StorageIndex count = 0; + for(Index j=0; j=0; --j) + { + StorageIndex innerNNZ = previousOuterIndex - m_outerIndex[j]; + for(Index i=innerNNZ-1; i>=0; --i) + { + m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i); + m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i); + } + previousOuterIndex = m_outerIndex[j]; + m_outerIndex[j] = newOuterIndex[j]; + m_innerNonZeros[j] = innerNNZ; + } + if(m_outerSize>0) + m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1]; + + m_data.resize(m_outerIndex[m_outerSize]); + } + else + { + StorageIndex* newOuterIndex = static_cast(std::malloc((m_outerSize+1)*sizeof(StorageIndex))); + if (!newOuterIndex) internal::throw_std_bad_alloc(); + + StorageIndex count = 0; + for(Index j=0; j(reserveSizes[j], alreadyReserved); + count += toReserve + m_innerNonZeros[j]; + } + newOuterIndex[m_outerSize] = count; + + m_data.resize(count); + for(Index j=m_outerSize-1; j>=0; --j) + { + Index offset = newOuterIndex[j] - m_outerIndex[j]; + if(offset>0) + { + StorageIndex innerNNZ = m_innerNonZeros[j]; + for(Index i=innerNNZ-1; i>=0; --i) + { + m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i); + m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i); + } + } + } + + std::swap(m_outerIndex, newOuterIndex); + std::free(newOuterIndex); + } + + } + public: + + //--- low level purely coherent filling --- + + /** \internal + * \returns a reference to the non zero coefficient at position \a row, \a col assuming that: + * - the nonzero does not already exist + * - the new coefficient is the last one according to the storage order + * + * Before filling a given inner vector you must call the statVec(Index) function. + * + * After an insertion session, you should call the finalize() function. + * + * \sa insert, insertBackByOuterInner, startVec */ + inline Scalar& insertBack(Index row, Index col) + { + return insertBackByOuterInner(IsRowMajor?row:col, IsRowMajor?col:row); + } + + /** \internal + * \sa insertBack, startVec */ + inline Scalar& insertBackByOuterInner(Index outer, Index inner) + { + eigen_assert(Index(m_outerIndex[outer+1]) == m_data.size() && "Invalid ordered insertion (invalid outer index)"); + eigen_assert( (m_outerIndex[outer+1]-m_outerIndex[outer]==0 || m_data.index(m_data.size()-1)(m_data.size()); + Index i = m_outerSize; + // find the last filled column + while (i>=0 && m_outerIndex[i]==0) + --i; + ++i; + while (i<=m_outerSize) + { + m_outerIndex[i] = size; + ++i; + } + } + } + + //--- + + template + void setFromTriplets(const InputIterators& begin, const InputIterators& end); + + template + void setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func); + + void sumupDuplicates() { collapseDuplicates(internal::scalar_sum_op()); } + + template + void collapseDuplicates(DupFunctor dup_func = DupFunctor()); + + //--- + + /** \internal + * same as insert(Index,Index) except that the indices are given relative to the storage order */ + Scalar& insertByOuterInner(Index j, Index i) + { + return insert(IsRowMajor ? j : i, IsRowMajor ? i : j); + } + + /** Turns the matrix into the \em compressed format. + */ + void makeCompressed() + { + if(isCompressed()) + return; + + eigen_internal_assert(m_outerIndex!=0 && m_outerSize>0); + + Index oldStart = m_outerIndex[1]; + m_outerIndex[1] = m_innerNonZeros[0]; + for(Index j=1; j0) + { + for(Index k=0; k(std::malloc(m_outerSize * sizeof(StorageIndex))); + for (Index i = 0; i < m_outerSize; i++) + { + m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i]; + } + } + + /** Suppresses all nonzeros which are \b much \b smaller \b than \a reference under the tolerance \a epsilon */ + void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits::dummy_precision()) + { + prune(default_prunning_func(reference,epsilon)); + } + + /** Turns the matrix into compressed format, and suppresses all nonzeros which do not satisfy the predicate \a keep. + * The functor type \a KeepFunc must implement the following function: + * \code + * bool operator() (const Index& row, const Index& col, const Scalar& value) const; + * \endcode + * \sa prune(Scalar,RealScalar) + */ + template + void prune(const KeepFunc& keep = KeepFunc()) + { + // TODO optimize the uncompressed mode to avoid moving and allocating the data twice + makeCompressed(); + + StorageIndex k = 0; + for(Index j=0; jrows() == rows && this->cols() == cols) return; + + // If one dimension is null, then there is nothing to be preserved + if(rows==0 || cols==0) return resize(rows,cols); + + Index innerChange = IsRowMajor ? cols - this->cols() : rows - this->rows(); + Index outerChange = IsRowMajor ? rows - this->rows() : cols - this->cols(); + StorageIndex newInnerSize = convert_index(IsRowMajor ? cols : rows); + + // Deals with inner non zeros + if (m_innerNonZeros) + { + // Resize m_innerNonZeros + StorageIndex *newInnerNonZeros = static_cast(std::realloc(m_innerNonZeros, (m_outerSize + outerChange) * sizeof(StorageIndex))); + if (!newInnerNonZeros) internal::throw_std_bad_alloc(); + m_innerNonZeros = newInnerNonZeros; + + for(Index i=m_outerSize; i(std::malloc((m_outerSize + outerChange) * sizeof(StorageIndex))); + if (!m_innerNonZeros) internal::throw_std_bad_alloc(); + for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++) + m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i]; + for(Index i = m_outerSize; i < m_outerSize + outerChange; i++) + m_innerNonZeros[i] = 0; + } + + // Change the m_innerNonZeros in case of a decrease of inner size + if (m_innerNonZeros && innerChange < 0) + { + for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++) + { + StorageIndex &n = m_innerNonZeros[i]; + StorageIndex start = m_outerIndex[i]; + while (n > 0 && m_data.index(start+n-1) >= newInnerSize) --n; + } + } + + m_innerSize = newInnerSize; + + // Re-allocate outer index structure if necessary + if (outerChange == 0) + return; + + StorageIndex *newOuterIndex = static_cast(std::realloc(m_outerIndex, (m_outerSize + outerChange + 1) * sizeof(StorageIndex))); + if (!newOuterIndex) internal::throw_std_bad_alloc(); + m_outerIndex = newOuterIndex; + if (outerChange > 0) + { + StorageIndex lastIdx = m_outerSize == 0 ? 0 : m_outerIndex[m_outerSize]; + for(Index i=m_outerSize; i(std::malloc((outerSize + 1) * sizeof(StorageIndex))); + if (!m_outerIndex) internal::throw_std_bad_alloc(); + + m_outerSize = outerSize; + } + if(m_innerNonZeros) + { + std::free(m_innerNonZeros); + m_innerNonZeros = 0; + } + memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex)); + } + + /** \internal + * Resize the nonzero vector to \a size */ + void resizeNonZeros(Index size) + { + m_data.resize(size); + } + + /** \returns a const expression of the diagonal coefficients. */ + const ConstDiagonalReturnType diagonal() const { return ConstDiagonalReturnType(*this); } + + /** \returns a read-write expression of the diagonal coefficients. + * \warning If the diagonal entries are written, then all diagonal + * entries \b must already exist, otherwise an assertion will be raised. + */ + DiagonalReturnType diagonal() { return DiagonalReturnType(*this); } + + /** Default constructor yielding an empty \c 0 \c x \c 0 matrix */ + inline SparseMatrix() + : m_outerSize(-1), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) + { + check_template_parameters(); + resize(0, 0); + } + + /** Constructs a \a rows \c x \a cols empty matrix */ + inline SparseMatrix(Index rows, Index cols) + : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) + { + check_template_parameters(); + resize(rows, cols); + } + + /** Constructs a sparse matrix from the sparse expression \a other */ + template + inline SparseMatrix(const SparseMatrixBase& other) + : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) + { + EIGEN_STATIC_ASSERT((internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + check_template_parameters(); + const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator::Flags & RowMajorBit); + if (needToTranspose) + *this = other.derived(); + else + { + #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + #endif + internal::call_assignment_no_alias(*this, other.derived()); + } + } + + /** Constructs a sparse matrix from the sparse selfadjoint view \a other */ + template + inline SparseMatrix(const SparseSelfAdjointView& other) + : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) + { + check_template_parameters(); + Base::operator=(other); + } + + /** Copy constructor (it performs a deep copy) */ + inline SparseMatrix(const SparseMatrix& other) + : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) + { + check_template_parameters(); + *this = other.derived(); + } + + /** \brief Copy constructor with in-place evaluation */ + template + SparseMatrix(const ReturnByValue& other) + : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) + { + check_template_parameters(); + initAssignment(other); + other.evalTo(*this); + } + + /** \brief Copy constructor with in-place evaluation */ + template + explicit SparseMatrix(const DiagonalBase& other) + : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0) + { + check_template_parameters(); + *this = other.derived(); + } + + /** Swaps the content of two sparse matrices of the same type. + * This is a fast operation that simply swaps the underlying pointers and parameters. */ + inline void swap(SparseMatrix& other) + { + //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n"); + std::swap(m_outerIndex, other.m_outerIndex); + std::swap(m_innerSize, other.m_innerSize); + std::swap(m_outerSize, other.m_outerSize); + std::swap(m_innerNonZeros, other.m_innerNonZeros); + m_data.swap(other.m_data); + } + + /** Sets *this to the identity matrix. + * This function also turns the matrix into compressed mode, and drop any reserved memory. */ + inline void setIdentity() + { + eigen_assert(rows() == cols() && "ONLY FOR SQUARED MATRICES"); + this->m_data.resize(rows()); + Eigen::Map(this->m_data.indexPtr(), rows()).setLinSpaced(0, StorageIndex(rows()-1)); + Eigen::Map(this->m_data.valuePtr(), rows()).setOnes(); + Eigen::Map(this->m_outerIndex, rows()+1).setLinSpaced(0, StorageIndex(rows())); + std::free(m_innerNonZeros); + m_innerNonZeros = 0; + } + inline SparseMatrix& operator=(const SparseMatrix& other) + { + if (other.isRValue()) + { + swap(other.const_cast_derived()); + } + else if(this!=&other) + { + #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + #endif + initAssignment(other); + if(other.isCompressed()) + { + internal::smart_copy(other.m_outerIndex, other.m_outerIndex + m_outerSize + 1, m_outerIndex); + m_data = other.m_data; + } + else + { + Base::operator=(other); + } + } + return *this; + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + inline SparseMatrix& operator=(const EigenBase& other) + { return Base::operator=(other.derived()); } + + template + inline SparseMatrix& operator=(const Product& other); +#endif // EIGEN_PARSED_BY_DOXYGEN + + template + EIGEN_DONT_INLINE SparseMatrix& operator=(const SparseMatrixBase& other); + + friend std::ostream & operator << (std::ostream & s, const SparseMatrix& m) + { + EIGEN_DBG_SPARSE( + s << "Nonzero entries:\n"; + if(m.isCompressed()) + { + for (Index i=0; i&>(m); + return s; + } + + /** Destructor */ + inline ~SparseMatrix() + { + std::free(m_outerIndex); + std::free(m_innerNonZeros); + } + + /** Overloaded for performance */ + Scalar sum() const; + +# ifdef EIGEN_SPARSEMATRIX_PLUGIN +# include EIGEN_SPARSEMATRIX_PLUGIN +# endif + +protected: + + template + void initAssignment(const Other& other) + { + resize(other.rows(), other.cols()); + if(m_innerNonZeros) + { + std::free(m_innerNonZeros); + m_innerNonZeros = 0; + } + } + + /** \internal + * \sa insert(Index,Index) */ + EIGEN_DONT_INLINE Scalar& insertCompressed(Index row, Index col); + + /** \internal + * A vector object that is equal to 0 everywhere but v at the position i */ + class SingletonVector + { + StorageIndex m_index; + StorageIndex m_value; + public: + typedef StorageIndex value_type; + SingletonVector(Index i, Index v) + : m_index(convert_index(i)), m_value(convert_index(v)) + {} + + StorageIndex operator[](Index i) const { return i==m_index ? m_value : 0; } + }; + + /** \internal + * \sa insert(Index,Index) */ + EIGEN_DONT_INLINE Scalar& insertUncompressed(Index row, Index col); + +public: + /** \internal + * \sa insert(Index,Index) */ + EIGEN_STRONG_INLINE Scalar& insertBackUncompressed(Index row, Index col) + { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(!isCompressed()); + eigen_assert(m_innerNonZeros[outer]<=(m_outerIndex[outer+1] - m_outerIndex[outer])); + + Index p = m_outerIndex[outer] + m_innerNonZeros[outer]++; + m_data.index(p) = convert_index(inner); + return (m_data.value(p) = Scalar(0)); + } +protected: + struct IndexPosPair { + IndexPosPair(Index a_i, Index a_p) : i(a_i), p(a_p) {} + Index i; + Index p; + }; + + /** \internal assign \a diagXpr to the diagonal of \c *this + * There are different strategies: + * 1 - if *this is overwritten (Func==assign_op) or *this is empty, then we can work treat *this as a dense vector expression. + * 2 - otherwise, for each diagonal coeff, + * 2.a - if it already exists, then we update it, + * 2.b - otherwise, if *this is uncompressed and that the current inner-vector has empty room for at least 1 element, then we perform an in-place insertion. + * 2.c - otherwise, we'll have to reallocate and copy everything, so instead of doing so for each new element, it is recorded in a std::vector. + * 3 - at the end, if some entries failed to be inserted in-place, then we alloc a new buffer, copy each chunk at the right position, and insert the new elements. + * + * TODO: some piece of code could be isolated and reused for a general in-place update strategy. + * TODO: if we start to defer the insertion of some elements (i.e., case 2.c executed once), + * then it *might* be better to disable case 2.b since they will have to be copied anyway. + */ + template + void assignDiagonal(const DiagXpr diagXpr, const Func& assignFunc) + { + Index n = diagXpr.size(); + + const bool overwrite = internal::is_same >::value; + if(overwrite) + { + if((this->rows()!=n) || (this->cols()!=n)) + this->resize(n, n); + } + + if(m_data.size()==0 || overwrite) + { + typedef Array ArrayXI; + this->makeCompressed(); + this->resizeNonZeros(n); + Eigen::Map(this->innerIndexPtr(), n).setLinSpaced(0,StorageIndex(n)-1); + Eigen::Map(this->outerIndexPtr(), n+1).setLinSpaced(0,StorageIndex(n)); + Eigen::Map > values = this->coeffs(); + values.setZero(); + internal::call_assignment_no_alias(values, diagXpr, assignFunc); + } + else + { + bool isComp = isCompressed(); + internal::evaluator diaEval(diagXpr); + std::vector newEntries; + + // 1 - try in-place update and record insertion failures + for(Index i = 0; ilower_bound(i,i); + Index p = lb.value; + if(lb.found) + { + // the coeff already exists + assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i)); + } + else if((!isComp) && m_innerNonZeros[i] < (m_outerIndex[i+1]-m_outerIndex[i])) + { + // non compressed mode with local room for inserting one element + m_data.moveChunk(p, p+1, m_outerIndex[i]+m_innerNonZeros[i]-p); + m_innerNonZeros[i]++; + m_data.value(p) = Scalar(0); + m_data.index(p) = StorageIndex(i); + assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i)); + } + else + { + // defer insertion + newEntries.push_back(IndexPosPair(i,p)); + } + } + // 2 - insert deferred entries + Index n_entries = Index(newEntries.size()); + if(n_entries>0) + { + Storage newData(m_data.size()+n_entries); + Index prev_p = 0; + Index prev_i = 0; + for(Index k=0; k::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE); + EIGEN_STATIC_ASSERT((Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS); + } + + struct default_prunning_func { + default_prunning_func(const Scalar& ref, const RealScalar& eps) : reference(ref), epsilon(eps) {} + inline bool operator() (const Index&, const Index&, const Scalar& value) const + { + return !internal::isMuchSmallerThan(value, reference, epsilon); + } + Scalar reference; + RealScalar epsilon; + }; +}; + +namespace internal { + +template +void set_from_triplets(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat, DupFunctor dup_func) +{ + enum { IsRowMajor = SparseMatrixType::IsRowMajor }; + typedef typename SparseMatrixType::Scalar Scalar; + typedef typename SparseMatrixType::StorageIndex StorageIndex; + SparseMatrix trMat(mat.rows(),mat.cols()); + + if(begin!=end) + { + // pass 1: count the nnz per inner-vector + typename SparseMatrixType::IndexVector wi(trMat.outerSize()); + wi.setZero(); + for(InputIterator it(begin); it!=end; ++it) + { + eigen_assert(it->row()>=0 && it->row()col()>=0 && it->col()col() : it->row())++; + } + + // pass 2: insert all the elements into trMat + trMat.reserve(wi); + for(InputIterator it(begin); it!=end; ++it) + trMat.insertBackUncompressed(it->row(),it->col()) = it->value(); + + // pass 3: + trMat.collapseDuplicates(dup_func); + } + + // pass 4: transposed copy -> implicit sorting + mat = trMat; +} + +} + + +/** Fill the matrix \c *this with the list of \em triplets defined by the iterator range \a begin - \a end. + * + * A \em triplet is a tuple (i,j,value) defining a non-zero element. + * The input list of triplets does not have to be sorted, and can contains duplicated elements. + * In any case, the result is a \b sorted and \b compressed sparse matrix where the duplicates have been summed up. + * This is a \em O(n) operation, with \em n the number of triplet elements. + * The initial contents of \c *this is destroyed. + * The matrix \c *this must be properly resized beforehand using the SparseMatrix(Index,Index) constructor, + * or the resize(Index,Index) method. The sizes are not extracted from the triplet list. + * + * The \a InputIterators value_type must provide the following interface: + * \code + * Scalar value() const; // the value + * Scalar row() const; // the row index i + * Scalar col() const; // the column index j + * \endcode + * See for instance the Eigen::Triplet template class. + * + * Here is a typical usage example: + * \code + typedef Triplet T; + std::vector tripletList; + tripletList.reserve(estimation_of_entries); + for(...) + { + // ... + tripletList.push_back(T(i,j,v_ij)); + } + SparseMatrixType m(rows,cols); + m.setFromTriplets(tripletList.begin(), tripletList.end()); + // m is ready to go! + * \endcode + * + * \warning The list of triplets is read multiple times (at least twice). Therefore, it is not recommended to define + * an abstract iterator over a complex data-structure that would be expensive to evaluate. The triplets should rather + * be explicitly stored into a std::vector for instance. + */ +template +template +void SparseMatrix::setFromTriplets(const InputIterators& begin, const InputIterators& end) +{ + internal::set_from_triplets >(begin, end, *this, internal::scalar_sum_op()); +} + +/** The same as setFromTriplets but when duplicates are met the functor \a dup_func is applied: + * \code + * value = dup_func(OldValue, NewValue) + * \endcode + * Here is a C++11 example keeping the latest entry only: + * \code + * mat.setFromTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; }); + * \endcode + */ +template +template +void SparseMatrix::setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func) +{ + internal::set_from_triplets, DupFunctor>(begin, end, *this, dup_func); +} + +/** \internal */ +template +template +void SparseMatrix::collapseDuplicates(DupFunctor dup_func) +{ + eigen_assert(!isCompressed()); + // TODO, in practice we should be able to use m_innerNonZeros for that task + IndexVector wi(innerSize()); + wi.fill(-1); + StorageIndex count = 0; + // for each inner-vector, wi[inner_index] will hold the position of first element into the index/value buffers + for(Index j=0; j=start) + { + // we already meet this entry => accumulate it + m_data.value(wi(i)) = dup_func(m_data.value(wi(i)), m_data.value(k)); + } + else + { + m_data.value(count) = m_data.value(k); + m_data.index(count) = m_data.index(k); + wi(i) = count; + ++count; + } + } + m_outerIndex[j] = start; + } + m_outerIndex[m_outerSize] = count; + + // turn the matrix into compressed form + std::free(m_innerNonZeros); + m_innerNonZeros = 0; + m_data.resize(m_outerIndex[m_outerSize]); +} + +template +template +EIGEN_DONT_INLINE SparseMatrix& SparseMatrix::operator=(const SparseMatrixBase& other) +{ + EIGEN_STATIC_ASSERT((internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + #endif + + const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator::Flags & RowMajorBit); + if (needToTranspose) + { + #ifdef EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN + EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN + #endif + // two passes algorithm: + // 1 - compute the number of coeffs per dest inner vector + // 2 - do the actual copy/eval + // Since each coeff of the rhs has to be evaluated twice, let's evaluate it if needed + typedef typename internal::nested_eval::type >::type OtherCopy; + typedef typename internal::remove_all::type _OtherCopy; + typedef internal::evaluator<_OtherCopy> OtherCopyEval; + OtherCopy otherCopy(other.derived()); + OtherCopyEval otherCopyEval(otherCopy); + + SparseMatrix dest(other.rows(),other.cols()); + Eigen::Map (dest.m_outerIndex,dest.outerSize()).setZero(); + + // pass 1 + // FIXME the above copy could be merged with that pass + for (Index j=0; jswap(dest); + return *this; + } + else + { + if(other.isRValue()) + { + initAssignment(other.derived()); + } + // there is no special optimization + return Base::operator=(other.derived()); + } +} + +template +typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insert(Index row, Index col) +{ + eigen_assert(row>=0 && row=0 && col(std::malloc(m_outerSize * sizeof(StorageIndex))); + if(!m_innerNonZeros) internal::throw_std_bad_alloc(); + + memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex)); + + // pack all inner-vectors to the end of the pre-allocated space + // and allocate the entire free-space to the first inner-vector + StorageIndex end = convert_index(m_data.allocatedSize()); + for(Index j=1; j<=m_outerSize; ++j) + m_outerIndex[j] = end; + } + else + { + // turn the matrix into non-compressed mode + m_innerNonZeros = static_cast(std::malloc(m_outerSize * sizeof(StorageIndex))); + if(!m_innerNonZeros) internal::throw_std_bad_alloc(); + for(Index j=0; j=0 && m_innerNonZeros[j]==0) + m_outerIndex[j--] = p; + + // push back the new element + ++m_innerNonZeros[outer]; + m_data.append(Scalar(0), inner); + + // check for reallocation + if(data_end != m_data.allocatedSize()) + { + // m_data has been reallocated + // -> move remaining inner-vectors back to the end of the free-space + // so that the entire free-space is allocated to the current inner-vector. + eigen_internal_assert(data_end < m_data.allocatedSize()); + StorageIndex new_end = convert_index(m_data.allocatedSize()); + for(Index k=outer+1; k<=m_outerSize; ++k) + if(m_outerIndex[k]==data_end) + m_outerIndex[k] = new_end; + } + return m_data.value(p); + } + + // Second case: the next inner-vector is packed to the end + // and the current inner-vector end match the used-space. + if(m_outerIndex[outer+1]==data_end && m_outerIndex[outer]+m_innerNonZeros[outer]==m_data.size()) + { + eigen_internal_assert(outer+1==m_outerSize || m_innerNonZeros[outer+1]==0); + + // add space for the new element + ++m_innerNonZeros[outer]; + m_data.resize(m_data.size()+1); + + // check for reallocation + if(data_end != m_data.allocatedSize()) + { + // m_data has been reallocated + // -> move remaining inner-vectors back to the end of the free-space + // so that the entire free-space is allocated to the current inner-vector. + eigen_internal_assert(data_end < m_data.allocatedSize()); + StorageIndex new_end = convert_index(m_data.allocatedSize()); + for(Index k=outer+1; k<=m_outerSize; ++k) + if(m_outerIndex[k]==data_end) + m_outerIndex[k] = new_end; + } + + // and insert it at the right position (sorted insertion) + Index startId = m_outerIndex[outer]; + Index p = m_outerIndex[outer]+m_innerNonZeros[outer]-1; + while ( (p > startId) && (m_data.index(p-1) > inner) ) + { + m_data.index(p) = m_data.index(p-1); + m_data.value(p) = m_data.value(p-1); + --p; + } + + m_data.index(p) = convert_index(inner); + return (m_data.value(p) = Scalar(0)); + } + + if(m_data.size() != m_data.allocatedSize()) + { + // make sure the matrix is compatible to random un-compressed insertion: + m_data.resize(m_data.allocatedSize()); + this->reserveInnerVectors(Array::Constant(m_outerSize, 2)); + } + + return insertUncompressed(row,col); +} + +template +EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertUncompressed(Index row, Index col) +{ + eigen_assert(!isCompressed()); + + const Index outer = IsRowMajor ? row : col; + const StorageIndex inner = convert_index(IsRowMajor ? col : row); + + Index room = m_outerIndex[outer+1] - m_outerIndex[outer]; + StorageIndex innerNNZ = m_innerNonZeros[outer]; + if(innerNNZ>=room) + { + // this inner vector is full, we need to reallocate the whole buffer :( + reserve(SingletonVector(outer,std::max(2,innerNNZ))); + } + + Index startId = m_outerIndex[outer]; + Index p = startId + m_innerNonZeros[outer]; + while ( (p > startId) && (m_data.index(p-1) > inner) ) + { + m_data.index(p) = m_data.index(p-1); + m_data.value(p) = m_data.value(p-1); + --p; + } + eigen_assert((p<=startId || m_data.index(p-1)!=inner) && "you cannot insert an element that already exists, you must call coeffRef to this end"); + + m_innerNonZeros[outer]++; + + m_data.index(p) = inner; + return (m_data.value(p) = Scalar(0)); +} + +template +EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertCompressed(Index row, Index col) +{ + eigen_assert(isCompressed()); + + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + Index previousOuter = outer; + if (m_outerIndex[outer+1]==0) + { + // we start a new inner vector + while (previousOuter>=0 && m_outerIndex[previousOuter]==0) + { + m_outerIndex[previousOuter] = convert_index(m_data.size()); + --previousOuter; + } + m_outerIndex[outer+1] = m_outerIndex[outer]; + } + + // here we have to handle the tricky case where the outerIndex array + // starts with: [ 0 0 0 0 0 1 ...] and we are inserted in, e.g., + // the 2nd inner vector... + bool isLastVec = (!(previousOuter==-1 && m_data.size()!=0)) + && (std::size_t(m_outerIndex[outer+1]) == m_data.size()); + + std::size_t startId = m_outerIndex[outer]; + // FIXME let's make sure sizeof(long int) == sizeof(std::size_t) + std::size_t p = m_outerIndex[outer+1]; + ++m_outerIndex[outer+1]; + + double reallocRatio = 1; + if (m_data.allocatedSize()<=m_data.size()) + { + // if there is no preallocated memory, let's reserve a minimum of 32 elements + if (m_data.size()==0) + { + m_data.reserve(32); + } + else + { + // we need to reallocate the data, to reduce multiple reallocations + // we use a smart resize algorithm based on the current filling ratio + // in addition, we use double to avoid integers overflows + double nnzEstimate = double(m_outerIndex[outer])*double(m_outerSize)/double(outer+1); + reallocRatio = (nnzEstimate-double(m_data.size()))/double(m_data.size()); + // furthermore we bound the realloc ratio to: + // 1) reduce multiple minor realloc when the matrix is almost filled + // 2) avoid to allocate too much memory when the matrix is almost empty + reallocRatio = (std::min)((std::max)(reallocRatio,1.5),8.); + } + } + m_data.resize(m_data.size()+1,reallocRatio); + + if (!isLastVec) + { + if (previousOuter==-1) + { + // oops wrong guess. + // let's correct the outer offsets + for (Index k=0; k<=(outer+1); ++k) + m_outerIndex[k] = 0; + Index k=outer+1; + while(m_outerIndex[k]==0) + m_outerIndex[k++] = 1; + while (k<=m_outerSize && m_outerIndex[k]!=0) + m_outerIndex[k++]++; + p = 0; + --k; + k = m_outerIndex[k]-1; + while (k>0) + { + m_data.index(k) = m_data.index(k-1); + m_data.value(k) = m_data.value(k-1); + k--; + } + } + else + { + // we are not inserting into the last inner vec + // update outer indices: + Index j = outer+2; + while (j<=m_outerSize && m_outerIndex[j]!=0) + m_outerIndex[j++]++; + --j; + // shift data of last vecs: + Index k = m_outerIndex[j]-1; + while (k>=Index(p)) + { + m_data.index(k) = m_data.index(k-1); + m_data.value(k) = m_data.value(k-1); + k--; + } + } + } + + while ( (p > startId) && (m_data.index(p-1) > inner) ) + { + m_data.index(p) = m_data.index(p-1); + m_data.value(p) = m_data.value(p-1); + --p; + } + + m_data.index(p) = inner; + return (m_data.value(p) = Scalar(0)); +} + +namespace internal { + +template +struct evaluator > + : evaluator > > +{ + typedef evaluator > > Base; + typedef SparseMatrix<_Scalar,_Options,_StorageIndex> SparseMatrixType; + evaluator() : Base() {} + explicit evaluator(const SparseMatrixType &mat) : Base(mat) {} +}; + +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSEMATRIX_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h new file mode 100644 index 0000000000..229449f022 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h @@ -0,0 +1,398 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSEMATRIXBASE_H +#define EIGEN_SPARSEMATRIXBASE_H + +namespace Eigen { + +/** \ingroup SparseCore_Module + * + * \class SparseMatrixBase + * + * \brief Base class of any sparse matrices or sparse expressions + * + * \tparam Derived is the derived type, e.g. a sparse matrix type, or an expression, etc. + * + * This class can be extended with the help of the plugin mechanism described on the page + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIXBASE_PLUGIN. + */ +template class SparseMatrixBase + : public EigenBase +{ + public: + + typedef typename internal::traits::Scalar Scalar; + + /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex, etc. + * + * It is an alias for the Scalar type */ + typedef Scalar value_type; + + typedef typename internal::packet_traits::type PacketScalar; + typedef typename internal::traits::StorageKind StorageKind; + + /** The integer type used to \b store indices within a SparseMatrix. + * For a \c SparseMatrix it an alias of the third template parameter \c IndexType. */ + typedef typename internal::traits::StorageIndex StorageIndex; + + typedef typename internal::add_const_on_value_type_if_arithmetic< + typename internal::packet_traits::type + >::type PacketReturnType; + + typedef SparseMatrixBase StorageBaseType; + + typedef Matrix IndexVector; + typedef Matrix ScalarVector; + + template + Derived& operator=(const EigenBase &other); + + enum { + + RowsAtCompileTime = internal::traits::RowsAtCompileTime, + /**< The number of rows at compile-time. This is just a copy of the value provided + * by the \a Derived type. If a value is not known at compile-time, + * it is set to the \a Dynamic constant. + * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */ + + ColsAtCompileTime = internal::traits::ColsAtCompileTime, + /**< The number of columns at compile-time. This is just a copy of the value provided + * by the \a Derived type. If a value is not known at compile-time, + * it is set to the \a Dynamic constant. + * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */ + + + SizeAtCompileTime = (internal::size_at_compile_time::RowsAtCompileTime, + internal::traits::ColsAtCompileTime>::ret), + /**< This is equal to the number of coefficients, i.e. the number of + * rows times the number of columns, or to \a Dynamic if this is not + * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */ + + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + + MaxSizeAtCompileTime = (internal::size_at_compile_time::ret), + + IsVectorAtCompileTime = RowsAtCompileTime == 1 || ColsAtCompileTime == 1, + /**< This is set to true if either the number of rows or the number of + * columns is known at compile-time to be equal to 1. Indeed, in that case, + * we are dealing with a column-vector (if there is only one column) or with + * a row-vector (if there is only one row). */ + + NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2, + /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors, + * and 2 for matrices. + */ + + Flags = internal::traits::Flags, + /**< This stores expression \ref flags flags which may or may not be inherited by new expressions + * constructed from this one. See the \ref flags "list of flags". + */ + + IsRowMajor = Flags&RowMajorBit ? 1 : 0, + + InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime) + : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime), + + #ifndef EIGEN_PARSED_BY_DOXYGEN + _HasDirectAccess = (int(Flags)&DirectAccessBit) ? 1 : 0 // workaround sunCC + #endif + }; + + /** \internal the return type of MatrixBase::adjoint() */ + typedef typename internal::conditional::IsComplex, + CwiseUnaryOp, Eigen::Transpose >, + Transpose + >::type AdjointReturnType; + typedef Transpose TransposeReturnType; + typedef typename internal::add_const >::type ConstTransposeReturnType; + + // FIXME storage order do not match evaluator storage order + typedef SparseMatrix PlainObject; + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** This is the "real scalar" type; if the \a Scalar type is already real numbers + * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If + * \a Scalar is \a std::complex then RealScalar is \a T. + * + * \sa class NumTraits + */ + typedef typename NumTraits::Real RealScalar; + + /** \internal the return type of coeff() + */ + typedef typename internal::conditional<_HasDirectAccess, const Scalar&, Scalar>::type CoeffReturnType; + + /** \internal Represents a matrix with all coefficients equal to one another*/ + typedef CwiseNullaryOp,Matrix > ConstantReturnType; + + /** type of the equivalent dense matrix */ + typedef Matrix DenseMatrixType; + /** type of the equivalent square matrix */ + typedef Matrix SquareMatrixType; + + inline const Derived& derived() const { return *static_cast(this); } + inline Derived& derived() { return *static_cast(this); } + inline Derived& const_cast_derived() const + { return *static_cast(const_cast(this)); } + + typedef EigenBase Base; + +#endif // not EIGEN_PARSED_BY_DOXYGEN + +#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::SparseMatrixBase +#ifdef EIGEN_PARSED_BY_DOXYGEN +#define EIGEN_DOC_UNARY_ADDONS(METHOD,OP) /**

This method does not change the sparsity of \c *this: the OP is applied to explicitly stored coefficients only. \sa SparseCompressedBase::coeffs()

*/ +#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL /**

\warning This method returns a read-only expression for any sparse matrices. \sa \ref TutorialSparse_SubMatrices "Sparse block operations"

*/ +#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND) /**

\warning This method returns a read-write expression for COND sparse matrices only. Otherwise, the returned expression is read-only. \sa \ref TutorialSparse_SubMatrices "Sparse block operations"

*/ +#else +#define EIGEN_DOC_UNARY_ADDONS(X,Y) +#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL +#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND) +#endif +# include "../plugins/CommonCwiseUnaryOps.h" +# include "../plugins/CommonCwiseBinaryOps.h" +# include "../plugins/MatrixCwiseUnaryOps.h" +# include "../plugins/MatrixCwiseBinaryOps.h" +# include "../plugins/BlockMethods.h" +# ifdef EIGEN_SPARSEMATRIXBASE_PLUGIN +# include EIGEN_SPARSEMATRIXBASE_PLUGIN +# endif +#undef EIGEN_CURRENT_STORAGE_BASE_CLASS +#undef EIGEN_DOC_UNARY_ADDONS +#undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL +#undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF + + /** \returns the number of rows. \sa cols() */ + inline Index rows() const { return derived().rows(); } + /** \returns the number of columns. \sa rows() */ + inline Index cols() const { return derived().cols(); } + /** \returns the number of coefficients, which is \a rows()*cols(). + * \sa rows(), cols(). */ + inline Index size() const { return rows() * cols(); } + /** \returns true if either the number of rows or the number of columns is equal to 1. + * In other words, this function returns + * \code rows()==1 || cols()==1 \endcode + * \sa rows(), cols(), IsVectorAtCompileTime. */ + inline bool isVector() const { return rows()==1 || cols()==1; } + /** \returns the size of the storage major dimension, + * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */ + Index outerSize() const { return (int(Flags)&RowMajorBit) ? this->rows() : this->cols(); } + /** \returns the size of the inner dimension according to the storage order, + * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */ + Index innerSize() const { return (int(Flags)&RowMajorBit) ? this->cols() : this->rows(); } + + bool isRValue() const { return m_isRValue; } + Derived& markAsRValue() { m_isRValue = true; return derived(); } + + SparseMatrixBase() : m_isRValue(false) { /* TODO check flags */ } + + + template + Derived& operator=(const ReturnByValue& other); + + template + inline Derived& operator=(const SparseMatrixBase& other); + + inline Derived& operator=(const Derived& other); + + protected: + + template + inline Derived& assign(const OtherDerived& other); + + template + inline void assignGeneric(const OtherDerived& other); + + public: + + friend std::ostream & operator << (std::ostream & s, const SparseMatrixBase& m) + { + typedef typename Derived::Nested Nested; + typedef typename internal::remove_all::type NestedCleaned; + + if (Flags&RowMajorBit) + { + Nested nm(m.derived()); + internal::evaluator thisEval(nm); + for (Index row=0; row::InnerIterator it(thisEval, row); it; ++it) + { + for ( ; col thisEval(nm); + if (m.cols() == 1) { + Index row = 0; + for (typename internal::evaluator::InnerIterator it(thisEval, 0); it; ++it) + { + for ( ; row trans = m; + s << static_cast >&>(trans); + } + } + return s; + } + + template + Derived& operator+=(const SparseMatrixBase& other); + template + Derived& operator-=(const SparseMatrixBase& other); + + template + Derived& operator+=(const DiagonalBase& other); + template + Derived& operator-=(const DiagonalBase& other); + + template + Derived& operator+=(const EigenBase &other); + template + Derived& operator-=(const EigenBase &other); + + Derived& operator*=(const Scalar& other); + Derived& operator/=(const Scalar& other); + + template struct CwiseProductDenseReturnType { + typedef CwiseBinaryOp::Scalar, + typename internal::traits::Scalar + >::ReturnType>, + const Derived, + const OtherDerived + > Type; + }; + + template + EIGEN_STRONG_INLINE const typename CwiseProductDenseReturnType::Type + cwiseProduct(const MatrixBase &other) const; + + // sparse * diagonal + template + const Product + operator*(const DiagonalBase &other) const + { return Product(derived(), other.derived()); } + + // diagonal * sparse + template friend + const Product + operator*(const DiagonalBase &lhs, const SparseMatrixBase& rhs) + { return Product(lhs.derived(), rhs.derived()); } + + // sparse * sparse + template + const Product + operator*(const SparseMatrixBase &other) const; + + // sparse * dense + template + const Product + operator*(const MatrixBase &other) const + { return Product(derived(), other.derived()); } + + // dense * sparse + template friend + const Product + operator*(const MatrixBase &lhs, const SparseMatrixBase& rhs) + { return Product(lhs.derived(), rhs.derived()); } + + /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */ + SparseSymmetricPermutationProduct twistedBy(const PermutationMatrix& perm) const + { + return SparseSymmetricPermutationProduct(derived(), perm); + } + + template + Derived& operator*=(const SparseMatrixBase& other); + + template + inline const TriangularView triangularView() const; + + template struct SelfAdjointViewReturnType { typedef SparseSelfAdjointView Type; }; + template struct ConstSelfAdjointViewReturnType { typedef const SparseSelfAdjointView Type; }; + + template inline + typename ConstSelfAdjointViewReturnType::Type selfadjointView() const; + template inline + typename SelfAdjointViewReturnType::Type selfadjointView(); + + template Scalar dot(const MatrixBase& other) const; + template Scalar dot(const SparseMatrixBase& other) const; + RealScalar squaredNorm() const; + RealScalar norm() const; + RealScalar blueNorm() const; + + TransposeReturnType transpose() { return TransposeReturnType(derived()); } + const ConstTransposeReturnType transpose() const { return ConstTransposeReturnType(derived()); } + const AdjointReturnType adjoint() const { return AdjointReturnType(transpose()); } + + DenseMatrixType toDense() const + { + return DenseMatrixType(derived()); + } + + template + bool isApprox(const SparseMatrixBase& other, + const RealScalar& prec = NumTraits::dummy_precision()) const; + + template + bool isApprox(const MatrixBase& other, + const RealScalar& prec = NumTraits::dummy_precision()) const + { return toDense().isApprox(other,prec); } + + /** \returns the matrix or vector obtained by evaluating this expression. + * + * Notice that in the case of a plain matrix or vector (not an expression) this function just returns + * a const reference, in order to avoid a useless copy. + */ + inline const typename internal::eval::type eval() const + { return typename internal::eval::type(derived()); } + + Scalar sum() const; + + inline const SparseView + pruned(const Scalar& reference = Scalar(0), const RealScalar& epsilon = NumTraits::dummy_precision()) const; + + protected: + + bool m_isRValue; + + static inline StorageIndex convert_index(const Index idx) { + return internal::convert_index(idx); + } + private: + template void evalTo(Dest &) const; +}; + +} // end namespace Eigen + +#endif // EIGEN_SPARSEMATRIXBASE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h new file mode 100644 index 0000000000..ef38357aef --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h @@ -0,0 +1,178 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Gael Guennebaud +// +// 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_SPARSE_PERMUTATION_H +#define EIGEN_SPARSE_PERMUTATION_H + +// This file implements sparse * permutation products + +namespace Eigen { + +namespace internal { + +template +struct permutation_matrix_product +{ + typedef typename nested_eval::type MatrixType; + typedef typename remove_all::type MatrixTypeCleaned; + + typedef typename MatrixTypeCleaned::Scalar Scalar; + typedef typename MatrixTypeCleaned::StorageIndex StorageIndex; + + enum { + SrcStorageOrder = MatrixTypeCleaned::Flags&RowMajorBit ? RowMajor : ColMajor, + MoveOuter = SrcStorageOrder==RowMajor ? Side==OnTheLeft : Side==OnTheRight + }; + + typedef typename internal::conditional, + SparseMatrix >::type ReturnType; + + template + static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr) + { + MatrixType mat(xpr); + if(MoveOuter) + { + SparseMatrix tmp(mat.rows(), mat.cols()); + Matrix sizes(mat.outerSize()); + for(Index j=0; j tmp(mat.rows(), mat.cols()); + Matrix sizes(tmp.outerSize()); + sizes.setZero(); + PermutationMatrix perm_cpy; + if((Side==OnTheLeft) ^ Transposed) + perm_cpy = perm; + else + perm_cpy = perm.transpose(); + + for(Index j=0; j struct product_promote_storage_type { typedef Sparse ret; }; +template struct product_promote_storage_type { typedef Sparse ret; }; + +// TODO, the following two overloads are only needed to define the right temporary type through +// typename traits >::ReturnType +// whereas it should be correctly handled by traits >::PlainObject + +template +struct product_evaluator, ProductTag, PermutationShape, SparseShape> + : public evaluator::ReturnType> +{ + typedef Product XprType; + typedef typename permutation_matrix_product::ReturnType PlainObject; + typedef evaluator Base; + + enum { + Flags = Base::Flags | EvalBeforeNestingBit + }; + + explicit product_evaluator(const XprType& xpr) + : m_result(xpr.rows(), xpr.cols()) + { + ::new (static_cast(this)) Base(m_result); + generic_product_impl::evalTo(m_result, xpr.lhs(), xpr.rhs()); + } + +protected: + PlainObject m_result; +}; + +template +struct product_evaluator, ProductTag, SparseShape, PermutationShape > + : public evaluator::ReturnType> +{ + typedef Product XprType; + typedef typename permutation_matrix_product::ReturnType PlainObject; + typedef evaluator Base; + + enum { + Flags = Base::Flags | EvalBeforeNestingBit + }; + + explicit product_evaluator(const XprType& xpr) + : m_result(xpr.rows(), xpr.cols()) + { + ::new (static_cast(this)) Base(m_result); + generic_product_impl::evalTo(m_result, xpr.lhs(), xpr.rhs()); + } + +protected: + PlainObject m_result; +}; + +} // end namespace internal + +/** \returns the matrix with the permutation applied to the columns + */ +template +inline const Product +operator*(const SparseMatrixBase& matrix, const PermutationBase& perm) +{ return Product(matrix.derived(), perm.derived()); } + +/** \returns the matrix with the permutation applied to the rows + */ +template +inline const Product +operator*( const PermutationBase& perm, const SparseMatrixBase& matrix) +{ return Product(perm.derived(), matrix.derived()); } + + +/** \returns the matrix with the inverse permutation applied to the columns. + */ +template +inline const Product, AliasFreeProduct> +operator*(const SparseMatrixBase& matrix, const InverseImpl& tperm) +{ + return Product, AliasFreeProduct>(matrix.derived(), tperm.derived()); +} + +/** \returns the matrix with the inverse permutation applied to the rows. + */ +template +inline const Product, SparseDerived, AliasFreeProduct> +operator*(const InverseImpl& tperm, const SparseMatrixBase& matrix) +{ + return Product, SparseDerived, AliasFreeProduct>(tperm.derived(), matrix.derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h new file mode 100644 index 0000000000..af8a7744dd --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h @@ -0,0 +1,181 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2015 Gael Guennebaud +// +// 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_SPARSEPRODUCT_H +#define EIGEN_SPARSEPRODUCT_H + +namespace Eigen { + +/** \returns an expression of the product of two sparse matrices. + * By default a conservative product preserving the symbolic non zeros is performed. + * The automatic pruning of the small values can be achieved by calling the pruned() function + * in which case a totally different product algorithm is employed: + * \code + * C = (A*B).pruned(); // suppress numerical zeros (exact) + * C = (A*B).pruned(ref); + * C = (A*B).pruned(ref,epsilon); + * \endcode + * where \c ref is a meaningful non zero reference value. + * */ +template +template +inline const Product +SparseMatrixBase::operator*(const SparseMatrixBase &other) const +{ + return Product(derived(), other.derived()); +} + +namespace internal { + +// sparse * sparse +template +struct generic_product_impl +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + evalTo(dst, lhs, rhs, typename evaluator_traits::Shape()); + } + + // dense += sparse * sparse + template + static void addTo(Dest& dst, const ActualLhs& lhs, const Rhs& rhs, typename enable_if::Shape,DenseShape>::value,int*>::type* = 0) + { + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + LhsNested lhsNested(lhs); + RhsNested rhsNested(rhs); + internal::sparse_sparse_to_dense_product_selector::type, + typename remove_all::type, Dest>::run(lhsNested,rhsNested,dst); + } + + // dense -= sparse * sparse + template + static void subTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, typename enable_if::Shape,DenseShape>::value,int*>::type* = 0) + { + addTo(dst, -lhs, rhs); + } + +protected: + + // sparse = sparse * sparse + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, SparseShape) + { + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + LhsNested lhsNested(lhs); + RhsNested rhsNested(rhs); + internal::conservative_sparse_sparse_product_selector::type, + typename remove_all::type, Dest>::run(lhsNested,rhsNested,dst); + } + + // dense = sparse * sparse + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, DenseShape) + { + dst.setZero(); + addTo(dst, lhs, rhs); + } +}; + +// sparse * sparse-triangular +template +struct generic_product_impl + : public generic_product_impl +{}; + +// sparse-triangular * sparse +template +struct generic_product_impl + : public generic_product_impl +{}; + +// dense = sparse-product (can be sparse*sparse, sparse*perm, etc.) +template< typename DstXprType, typename Lhs, typename Rhs> +struct Assignment, internal::assign_op::Scalar>, Sparse2Dense> +{ + typedef Product SrcXprType; + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + generic_product_impl::evalTo(dst,src.lhs(),src.rhs()); + } +}; + +// dense += sparse-product (can be sparse*sparse, sparse*perm, etc.) +template< typename DstXprType, typename Lhs, typename Rhs> +struct Assignment, internal::add_assign_op::Scalar>, Sparse2Dense> +{ + typedef Product SrcXprType; + static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op &) + { + generic_product_impl::addTo(dst,src.lhs(),src.rhs()); + } +}; + +// dense -= sparse-product (can be sparse*sparse, sparse*perm, etc.) +template< typename DstXprType, typename Lhs, typename Rhs> +struct Assignment, internal::sub_assign_op::Scalar>, Sparse2Dense> +{ + typedef Product SrcXprType; + static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op &) + { + generic_product_impl::subTo(dst,src.lhs(),src.rhs()); + } +}; + +template +struct unary_evaluator >, IteratorBased> + : public evaluator::PlainObject> +{ + typedef SparseView > XprType; + typedef typename XprType::PlainObject PlainObject; + typedef evaluator Base; + + explicit unary_evaluator(const XprType& xpr) + : m_result(xpr.rows(), xpr.cols()) + { + using std::abs; + ::new (static_cast(this)) Base(m_result); + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + LhsNested lhsNested(xpr.nestedExpression().lhs()); + RhsNested rhsNested(xpr.nestedExpression().rhs()); + + internal::sparse_sparse_product_with_pruning_selector::type, + typename remove_all::type, PlainObject>::run(lhsNested,rhsNested,m_result, + abs(xpr.reference())*xpr.epsilon()); + } + +protected: + PlainObject m_result; +}; + +} // end namespace internal + +// sparse matrix = sparse-product (can be sparse*sparse, sparse*perm, etc.) +template +template +SparseMatrix& SparseMatrix::operator=(const Product& src) +{ + // std::cout << "in Assignment : " << DstOptions << "\n"; + SparseMatrix dst(src.rows(),src.cols()); + internal::generic_product_impl::evalTo(dst,src.lhs(),src.rhs()); + this->swap(dst); + return *this; +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSEPRODUCT_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h new file mode 100644 index 0000000000..4587749627 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h @@ -0,0 +1,49 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSEREDUX_H +#define EIGEN_SPARSEREDUX_H + +namespace Eigen { + +template +typename internal::traits::Scalar +SparseMatrixBase::sum() const +{ + eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); + Scalar res(0); + internal::evaluator thisEval(derived()); + for (Index j=0; j::InnerIterator iter(thisEval,j); iter; ++iter) + res += iter.value(); + return res; +} + +template +typename internal::traits >::Scalar +SparseMatrix<_Scalar,_Options,_Index>::sum() const +{ + eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); + if(this->isCompressed()) + return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); + else + return Base::sum(); +} + +template +typename internal::traits >::Scalar +SparseVector<_Scalar,_Options,_Index>::sum() const +{ + eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); + return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSEREDUX_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h new file mode 100644 index 0000000000..748f87d626 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h @@ -0,0 +1,397 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Gael Guennebaud +// +// 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_SPARSE_REF_H +#define EIGEN_SPARSE_REF_H + +namespace Eigen { + +enum { + StandardCompressedFormat = 2 /**< used by Ref to specify whether the input storage must be in standard compressed form */ +}; + +namespace internal { + +template class SparseRefBase; + +template +struct traits, _Options, _StrideType> > + : public traits > +{ + typedef SparseMatrix PlainObjectType; + enum { + Options = _Options, + Flags = traits::Flags | CompressedAccessBit | NestByRefBit + }; + + template struct match { + enum { + StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)), + MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && StorageOrderMatch + }; + typedef typename internal::conditional::type type; + }; + +}; + +template +struct traits, _Options, _StrideType> > + : public traits, _Options, _StrideType> > +{ + enum { + Flags = (traits >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit + }; +}; + +template +struct traits, _Options, _StrideType> > + : public traits > +{ + typedef SparseVector PlainObjectType; + enum { + Options = _Options, + Flags = traits::Flags | CompressedAccessBit | NestByRefBit + }; + + template struct match { + enum { + MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && Derived::IsVectorAtCompileTime + }; + typedef typename internal::conditional::type type; + }; + +}; + +template +struct traits, _Options, _StrideType> > + : public traits, _Options, _StrideType> > +{ + enum { + Flags = (traits >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit + }; +}; + +template +struct traits > : public traits {}; + +template class SparseRefBase + : public SparseMapBase +{ +public: + + typedef SparseMapBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(SparseRefBase) + + SparseRefBase() + : Base(RowsAtCompileTime==Dynamic?0:RowsAtCompileTime,ColsAtCompileTime==Dynamic?0:ColsAtCompileTime, 0, 0, 0, 0, 0) + {} + +protected: + + template + void construct(Expression& expr) + { + if(expr.outerIndexPtr()==0) + ::new (static_cast(this)) Base(expr.size(), expr.nonZeros(), expr.innerIndexPtr(), expr.valuePtr()); + else + ::new (static_cast(this)) Base(expr.rows(), expr.cols(), expr.nonZeros(), expr.outerIndexPtr(), expr.innerIndexPtr(), expr.valuePtr(), expr.innerNonZeroPtr()); + } +}; + +} // namespace internal + + +/** + * \ingroup SparseCore_Module + * + * \brief A sparse matrix expression referencing an existing sparse expression + * + * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix. + * \tparam Options specifies whether the a standard compressed format is required \c Options is \c #StandardCompressedFormat, or \c 0. + * The default is \c 0. + * + * \sa class Ref + */ +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +class Ref, Options, StrideType > + : public internal::SparseRefBase, Options, StrideType > > +#else +template +class Ref + : public SparseMapBase // yes, that's weird to use Derived here, but that works! +#endif +{ + typedef SparseMatrix PlainObjectType; + typedef internal::traits Traits; + template + inline Ref(const SparseMatrix& expr); + template + inline Ref(const MappedSparseMatrix& expr); + public: + + typedef internal::SparseRefBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(Ref) + + + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + inline Ref(SparseMatrix& expr) + { + EIGEN_STATIC_ASSERT(bool(Traits::template match >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) ); + Base::construct(expr.derived()); + } + + template + inline Ref(MappedSparseMatrix& expr) + { + EIGEN_STATIC_ASSERT(bool(Traits::template match >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) ); + Base::construct(expr.derived()); + } + + template + inline Ref(const SparseCompressedBase& expr) + #else + /** Implicit constructor from any sparse expression (2D matrix or 1D vector) */ + template + inline Ref(SparseCompressedBase& expr) + #endif + { + EIGEN_STATIC_ASSERT(bool(internal::is_lvalue::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + EIGEN_STATIC_ASSERT(bool(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) ); + Base::construct(expr.const_cast_derived()); + } +}; + +// this is the const ref version +template +class Ref, Options, StrideType> + : public internal::SparseRefBase, Options, StrideType> > +{ + typedef SparseMatrix TPlainObjectType; + typedef internal::traits Traits; + public: + + typedef internal::SparseRefBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(Ref) + + template + inline Ref(const SparseMatrixBase& expr) : m_hasCopy(false) + { + construct(expr.derived(), typename Traits::template match::type()); + } + + inline Ref(const Ref& other) : Base(other), m_hasCopy(false) { + // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy + } + + template + inline Ref(const RefBase& other) : m_hasCopy(false) { + construct(other.derived(), typename Traits::template match::type()); + } + + ~Ref() { + if(m_hasCopy) { + TPlainObjectType* obj = reinterpret_cast(&m_storage); + obj->~TPlainObjectType(); + } + } + + protected: + + template + void construct(const Expression& expr,internal::true_type) + { + if((Options & int(StandardCompressedFormat)) && (!expr.isCompressed())) + { + TPlainObjectType* obj = reinterpret_cast(&m_storage); + ::new (obj) TPlainObjectType(expr); + m_hasCopy = true; + Base::construct(*obj); + } + else + { + Base::construct(expr); + } + } + + template + void construct(const Expression& expr, internal::false_type) + { + TPlainObjectType* obj = reinterpret_cast(&m_storage); + ::new (obj) TPlainObjectType(expr); + m_hasCopy = true; + Base::construct(*obj); + } + + protected: + typename internal::aligned_storage::type m_storage; + bool m_hasCopy; +}; + + + +/** + * \ingroup SparseCore_Module + * + * \brief A sparse vector expression referencing an existing sparse vector expression + * + * \tparam SparseVectorType the equivalent sparse vector type of the referenced data, it must be a template instance of class SparseVector. + * + * \sa class Ref + */ +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +class Ref, Options, StrideType > + : public internal::SparseRefBase, Options, StrideType > > +#else +template +class Ref + : public SparseMapBase +#endif +{ + typedef SparseVector PlainObjectType; + typedef internal::traits Traits; + template + inline Ref(const SparseVector& expr); + public: + + typedef internal::SparseRefBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(Ref) + + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + inline Ref(SparseVector& expr) + { + EIGEN_STATIC_ASSERT(bool(Traits::template match >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + Base::construct(expr.derived()); + } + + template + inline Ref(const SparseCompressedBase& expr) + #else + /** Implicit constructor from any 1D sparse vector expression */ + template + inline Ref(SparseCompressedBase& expr) + #endif + { + EIGEN_STATIC_ASSERT(bool(internal::is_lvalue::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + EIGEN_STATIC_ASSERT(bool(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + Base::construct(expr.const_cast_derived()); + } +}; + +// this is the const ref version +template +class Ref, Options, StrideType> + : public internal::SparseRefBase, Options, StrideType> > +{ + typedef SparseVector TPlainObjectType; + typedef internal::traits Traits; + public: + + typedef internal::SparseRefBase Base; + EIGEN_SPARSE_PUBLIC_INTERFACE(Ref) + + template + inline Ref(const SparseMatrixBase& expr) : m_hasCopy(false) + { + construct(expr.derived(), typename Traits::template match::type()); + } + + inline Ref(const Ref& other) : Base(other), m_hasCopy(false) { + // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy + } + + template + inline Ref(const RefBase& other) : m_hasCopy(false) { + construct(other.derived(), typename Traits::template match::type()); + } + + ~Ref() { + if(m_hasCopy) { + TPlainObjectType* obj = reinterpret_cast(&m_storage); + obj->~TPlainObjectType(); + } + } + + protected: + + template + void construct(const Expression& expr,internal::true_type) + { + Base::construct(expr); + } + + template + void construct(const Expression& expr, internal::false_type) + { + TPlainObjectType* obj = reinterpret_cast(&m_storage); + ::new (obj) TPlainObjectType(expr); + m_hasCopy = true; + Base::construct(*obj); + } + + protected: + typename internal::aligned_storage::type m_storage; + bool m_hasCopy; +}; + +namespace internal { + +// FIXME shall we introduce a general evaluatior_ref that we can specialize for any sparse object once, and thus remove this copy-pasta thing... + +template +struct evaluator, Options, StrideType> > + : evaluator, Options, StrideType> > > +{ + typedef evaluator, Options, StrideType> > > Base; + typedef Ref, Options, StrideType> XprType; + evaluator() : Base() {} + explicit evaluator(const XprType &mat) : Base(mat) {} +}; + +template +struct evaluator, Options, StrideType> > + : evaluator, Options, StrideType> > > +{ + typedef evaluator, Options, StrideType> > > Base; + typedef Ref, Options, StrideType> XprType; + evaluator() : Base() {} + explicit evaluator(const XprType &mat) : Base(mat) {} +}; + +template +struct evaluator, Options, StrideType> > + : evaluator, Options, StrideType> > > +{ + typedef evaluator, Options, StrideType> > > Base; + typedef Ref, Options, StrideType> XprType; + evaluator() : Base() {} + explicit evaluator(const XprType &mat) : Base(mat) {} +}; + +template +struct evaluator, Options, StrideType> > + : evaluator, Options, StrideType> > > +{ + typedef evaluator, Options, StrideType> > > Base; + typedef Ref, Options, StrideType> XprType; + evaluator() : Base() {} + explicit evaluator(const XprType &mat) : Base(mat) {} +}; + +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_REF_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h new file mode 100644 index 0000000000..85b00e10e9 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h @@ -0,0 +1,659 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-2014 Gael Guennebaud +// +// 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_SPARSE_SELFADJOINTVIEW_H +#define EIGEN_SPARSE_SELFADJOINTVIEW_H + +namespace Eigen { + +/** \ingroup SparseCore_Module + * \class SparseSelfAdjointView + * + * \brief Pseudo expression to manipulate a triangular sparse matrix as a selfadjoint matrix. + * + * \param MatrixType the type of the dense matrix storing the coefficients + * \param Mode can be either \c #Lower or \c #Upper + * + * This class is an expression of a sefladjoint matrix from a triangular part of a matrix + * with given dense storage of the coefficients. It is the return type of MatrixBase::selfadjointView() + * and most of the time this is the only way that it is used. + * + * \sa SparseMatrixBase::selfadjointView() + */ +namespace internal { + +template +struct traits > : traits { +}; + +template +void permute_symm_to_symm(const MatrixType& mat, SparseMatrix& _dest, const typename MatrixType::StorageIndex* perm = 0); + +template +void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix& _dest, const typename MatrixType::StorageIndex* perm = 0); + +} + +template class SparseSelfAdjointView + : public EigenBase > +{ + public: + + enum { + Mode = _Mode, + TransposeMode = ((Mode & Upper) ? Lower : 0) | ((Mode & Lower) ? Upper : 0), + RowsAtCompileTime = internal::traits::RowsAtCompileTime, + ColsAtCompileTime = internal::traits::ColsAtCompileTime + }; + + typedef EigenBase Base; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef Matrix VectorI; + typedef typename internal::ref_selector::non_const_type MatrixTypeNested; + typedef typename internal::remove_all::type _MatrixTypeNested; + + explicit inline SparseSelfAdjointView(MatrixType& matrix) : m_matrix(matrix) + { + eigen_assert(rows()==cols() && "SelfAdjointView is only for squared matrices"); + } + + inline Index rows() const { return m_matrix.rows(); } + inline Index cols() const { return m_matrix.cols(); } + + /** \internal \returns a reference to the nested matrix */ + const _MatrixTypeNested& matrix() const { return m_matrix; } + typename internal::remove_reference::type& matrix() { return m_matrix; } + + /** \returns an expression of the matrix product between a sparse self-adjoint matrix \c *this and a sparse matrix \a rhs. + * + * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product. + * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product. + */ + template + Product + operator*(const SparseMatrixBase& rhs) const + { + return Product(*this, rhs.derived()); + } + + /** \returns an expression of the matrix product between a sparse matrix \a lhs and a sparse self-adjoint matrix \a rhs. + * + * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product. + * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product. + */ + template friend + Product + operator*(const SparseMatrixBase& lhs, const SparseSelfAdjointView& rhs) + { + return Product(lhs.derived(), rhs); + } + + /** Efficient sparse self-adjoint matrix times dense vector/matrix product */ + template + Product + operator*(const MatrixBase& rhs) const + { + return Product(*this, rhs.derived()); + } + + /** Efficient dense vector/matrix times sparse self-adjoint matrix product */ + template friend + Product + operator*(const MatrixBase& lhs, const SparseSelfAdjointView& rhs) + { + return Product(lhs.derived(), rhs); + } + + /** Perform a symmetric rank K update of the selfadjoint matrix \c *this: + * \f$ this = this + \alpha ( u u^* ) \f$ where \a u is a vector or matrix. + * + * \returns a reference to \c *this + * + * To perform \f$ this = this + \alpha ( u^* u ) \f$ you can simply + * call this function with u.adjoint(). + */ + template + SparseSelfAdjointView& rankUpdate(const SparseMatrixBase& u, const Scalar& alpha = Scalar(1)); + + /** \returns an expression of P H P^-1 */ + // TODO implement twists in a more evaluator friendly fashion + SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode> twistedBy(const PermutationMatrix& perm) const + { + return SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode>(m_matrix, perm); + } + + template + SparseSelfAdjointView& operator=(const SparseSymmetricPermutationProduct& permutedMatrix) + { + internal::call_assignment_no_alias_no_transpose(*this, permutedMatrix); + return *this; + } + + SparseSelfAdjointView& operator=(const SparseSelfAdjointView& src) + { + PermutationMatrix pnull; + return *this = src.twistedBy(pnull); + } + + // Since we override the copy-assignment operator, we need to explicitly re-declare the copy-constructor + EIGEN_DEFAULT_COPY_CONSTRUCTOR(SparseSelfAdjointView) + + template + SparseSelfAdjointView& operator=(const SparseSelfAdjointView& src) + { + PermutationMatrix pnull; + return *this = src.twistedBy(pnull); + } + + void resize(Index rows, Index cols) + { + EIGEN_ONLY_USED_FOR_DEBUG(rows); + EIGEN_ONLY_USED_FOR_DEBUG(cols); + eigen_assert(rows == this->rows() && cols == this->cols() + && "SparseSelfadjointView::resize() does not actually allow to resize."); + } + + protected: + + MatrixTypeNested m_matrix; + //mutable VectorI m_countPerRow; + //mutable VectorI m_countPerCol; + private: + template void evalTo(Dest &) const; +}; + +/*************************************************************************** +* Implementation of SparseMatrixBase methods +***************************************************************************/ + +template +template +typename SparseMatrixBase::template ConstSelfAdjointViewReturnType::Type SparseMatrixBase::selfadjointView() const +{ + return SparseSelfAdjointView(derived()); +} + +template +template +typename SparseMatrixBase::template SelfAdjointViewReturnType::Type SparseMatrixBase::selfadjointView() +{ + return SparseSelfAdjointView(derived()); +} + +/*************************************************************************** +* Implementation of SparseSelfAdjointView methods +***************************************************************************/ + +template +template +SparseSelfAdjointView& +SparseSelfAdjointView::rankUpdate(const SparseMatrixBase& u, const Scalar& alpha) +{ + SparseMatrix tmp = u * u.adjoint(); + if(alpha==Scalar(0)) + m_matrix = tmp.template triangularView(); + else + m_matrix += alpha * tmp.template triangularView(); + + return *this; +} + +namespace internal { + +// TODO currently a selfadjoint expression has the form SelfAdjointView<.,.> +// in the future selfadjoint-ness should be defined by the expression traits +// such that Transpose > is valid. (currently TriangularBase::transpose() is overloaded to make it work) +template +struct evaluator_traits > +{ + typedef typename storage_kind_to_evaluator_kind::Kind Kind; + typedef SparseSelfAdjointShape Shape; +}; + +struct SparseSelfAdjoint2Sparse {}; + +template<> struct AssignmentKind { typedef SparseSelfAdjoint2Sparse Kind; }; +template<> struct AssignmentKind { typedef Sparse2Sparse Kind; }; + +template< typename DstXprType, typename SrcXprType, typename Functor> +struct Assignment +{ + typedef typename DstXprType::StorageIndex StorageIndex; + typedef internal::assign_op AssignOpType; + + template + static void run(SparseMatrix &dst, const SrcXprType &src, const AssignOpType&/*func*/) + { + internal::permute_symm_to_fullsymm(src.matrix(), dst); + } + + // FIXME: the handling of += and -= in sparse matrices should be cleanup so that next two overloads could be reduced to: + template + static void run(SparseMatrix &dst, const SrcXprType &src, const AssignFunc& func) + { + SparseMatrix tmp(src.rows(),src.cols()); + run(tmp, src, AssignOpType()); + call_assignment_no_alias_no_transpose(dst, tmp, func); + } + + template + static void run(SparseMatrix &dst, const SrcXprType &src, + const internal::add_assign_op& /* func */) + { + SparseMatrix tmp(src.rows(),src.cols()); + run(tmp, src, AssignOpType()); + dst += tmp; + } + + template + static void run(SparseMatrix &dst, const SrcXprType &src, + const internal::sub_assign_op& /* func */) + { + SparseMatrix tmp(src.rows(),src.cols()); + run(tmp, src, AssignOpType()); + dst -= tmp; + } + + template + static void run(DynamicSparseMatrix& dst, const SrcXprType &src, const AssignOpType&/*func*/) + { + // TODO directly evaluate into dst; + SparseMatrix tmp(dst.rows(),dst.cols()); + internal::permute_symm_to_fullsymm(src.matrix(), tmp); + dst = tmp; + } +}; + +} // end namespace internal + +/*************************************************************************** +* Implementation of sparse self-adjoint time dense matrix +***************************************************************************/ + +namespace internal { + +template +inline void sparse_selfadjoint_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha) +{ + EIGEN_ONLY_USED_FOR_DEBUG(alpha); + + typedef typename internal::nested_eval::type SparseLhsTypeNested; + typedef typename internal::remove_all::type SparseLhsTypeNestedCleaned; + typedef evaluator LhsEval; + typedef typename LhsEval::InnerIterator LhsIterator; + typedef typename SparseLhsType::Scalar LhsScalar; + + enum { + LhsIsRowMajor = (LhsEval::Flags&RowMajorBit)==RowMajorBit, + ProcessFirstHalf = + ((Mode&(Upper|Lower))==(Upper|Lower)) + || ( (Mode&Upper) && !LhsIsRowMajor) + || ( (Mode&Lower) && LhsIsRowMajor), + ProcessSecondHalf = !ProcessFirstHalf + }; + + SparseLhsTypeNested lhs_nested(lhs); + LhsEval lhsEval(lhs_nested); + + // work on one column at once + for (Index k=0; k::ReturnType rhs_j(alpha*rhs(j,k)); + // accumulator for partial scalar product + typename DenseResType::Scalar res_j(0); + for(; (ProcessFirstHalf ? i && i.index() < j : i) ; ++i) + { + LhsScalar lhs_ij = i.value(); + if(!LhsIsRowMajor) lhs_ij = numext::conj(lhs_ij); + res_j += lhs_ij * rhs.coeff(i.index(),k); + res(i.index(),k) += numext::conj(lhs_ij) * rhs_j; + } + res.coeffRef(j,k) += alpha * res_j; + + // handle diagonal coeff + if (ProcessFirstHalf && i && (i.index()==j)) + res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k); + } + } +} + + +template +struct generic_product_impl +: generic_product_impl_base > +{ + template + static void scaleAndAddTo(Dest& dst, const LhsView& lhsView, const Rhs& rhs, const typename Dest::Scalar& alpha) + { + typedef typename LhsView::_MatrixTypeNested Lhs; + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + LhsNested lhsNested(lhsView.matrix()); + RhsNested rhsNested(rhs); + + internal::sparse_selfadjoint_time_dense_product(lhsNested, rhsNested, dst, alpha); + } +}; + +template +struct generic_product_impl +: generic_product_impl_base > +{ + template + static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const RhsView& rhsView, const typename Dest::Scalar& alpha) + { + typedef typename RhsView::_MatrixTypeNested Rhs; + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + LhsNested lhsNested(lhs); + RhsNested rhsNested(rhsView.matrix()); + + // transpose everything + Transpose dstT(dst); + internal::sparse_selfadjoint_time_dense_product(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha); + } +}; + +// NOTE: these two overloads are needed to evaluate the sparse selfadjoint view into a full sparse matrix +// TODO: maybe the copy could be handled by generic_product_impl so that these overloads would not be needed anymore + +template +struct product_evaluator, ProductTag, SparseSelfAdjointShape, SparseShape> + : public evaluator::PlainObject> +{ + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + typedef evaluator Base; + + product_evaluator(const XprType& xpr) + : m_lhs(xpr.lhs()), m_result(xpr.rows(), xpr.cols()) + { + ::new (static_cast(this)) Base(m_result); + generic_product_impl::evalTo(m_result, m_lhs, xpr.rhs()); + } + +protected: + typename Rhs::PlainObject m_lhs; + PlainObject m_result; +}; + +template +struct product_evaluator, ProductTag, SparseShape, SparseSelfAdjointShape> + : public evaluator::PlainObject> +{ + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + typedef evaluator Base; + + product_evaluator(const XprType& xpr) + : m_rhs(xpr.rhs()), m_result(xpr.rows(), xpr.cols()) + { + ::new (static_cast(this)) Base(m_result); + generic_product_impl::evalTo(m_result, xpr.lhs(), m_rhs); + } + +protected: + typename Lhs::PlainObject m_rhs; + PlainObject m_result; +}; + +} // namespace internal + +/*************************************************************************** +* Implementation of symmetric copies and permutations +***************************************************************************/ +namespace internal { + +template +void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix& _dest, const typename MatrixType::StorageIndex* perm) +{ + typedef typename MatrixType::StorageIndex StorageIndex; + typedef typename MatrixType::Scalar Scalar; + typedef SparseMatrix Dest; + typedef Matrix VectorI; + typedef evaluator MatEval; + typedef typename evaluator::InnerIterator MatIterator; + + MatEval matEval(mat); + Dest& dest(_dest.derived()); + enum { + StorageOrderMatch = int(Dest::IsRowMajor) == int(MatrixType::IsRowMajor) + }; + + Index size = mat.rows(); + VectorI count; + count.resize(size); + count.setZero(); + dest.resize(size,size); + for(Index j = 0; jc) || ( Mode==Upper && r(it.index()); + Index r = it.row(); + Index c = it.col(); + + StorageIndex jp = perm ? perm[j] : j; + StorageIndex ip = perm ? perm[i] : i; + + if(Mode==int(Upper|Lower)) + { + Index k = count[StorageOrderMatch ? jp : ip]++; + dest.innerIndexPtr()[k] = StorageOrderMatch ? ip : jp; + dest.valuePtr()[k] = it.value(); + } + else if(r==c) + { + Index k = count[ip]++; + dest.innerIndexPtr()[k] = ip; + dest.valuePtr()[k] = it.value(); + } + else if(( (Mode&Lower)==Lower && r>c) || ( (Mode&Upper)==Upper && r +void permute_symm_to_symm(const MatrixType& mat, SparseMatrix& _dest, const typename MatrixType::StorageIndex* perm) +{ + typedef typename MatrixType::StorageIndex StorageIndex; + typedef typename MatrixType::Scalar Scalar; + SparseMatrix& dest(_dest.derived()); + typedef Matrix VectorI; + typedef evaluator MatEval; + typedef typename evaluator::InnerIterator MatIterator; + + enum { + SrcOrder = MatrixType::IsRowMajor ? RowMajor : ColMajor, + StorageOrderMatch = int(SrcOrder) == int(DstOrder), + DstMode = DstOrder==RowMajor ? (_DstMode==Upper ? Lower : Upper) : _DstMode, + SrcMode = SrcOrder==RowMajor ? (_SrcMode==Upper ? Lower : Upper) : _SrcMode + }; + + MatEval matEval(mat); + + Index size = mat.rows(); + VectorI count(size); + count.setZero(); + dest.resize(size,size); + for(StorageIndex j = 0; jj)) + continue; + + StorageIndex ip = perm ? perm[i] : i; + count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++; + } + } + dest.outerIndexPtr()[0] = 0; + for(Index j=0; jj)) + continue; + + StorageIndex jp = perm ? perm[j] : j; + StorageIndex ip = perm? perm[i] : i; + + Index k = count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++; + dest.innerIndexPtr()[k] = int(DstMode)==int(Lower) ? (std::max)(ip,jp) : (std::min)(ip,jp); + + if(!StorageOrderMatch) std::swap(ip,jp); + if( ((int(DstMode)==int(Lower) && ipjp))) + dest.valuePtr()[k] = numext::conj(it.value()); + else + dest.valuePtr()[k] = it.value(); + } + } +} + +} + +// TODO implement twists in a more evaluator friendly fashion + +namespace internal { + +template +struct traits > : traits { +}; + +} + +template +class SparseSymmetricPermutationProduct + : public EigenBase > +{ + public: + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::StorageIndex StorageIndex; + enum { + RowsAtCompileTime = internal::traits::RowsAtCompileTime, + ColsAtCompileTime = internal::traits::ColsAtCompileTime + }; + protected: + typedef PermutationMatrix Perm; + public: + typedef Matrix VectorI; + typedef typename MatrixType::Nested MatrixTypeNested; + typedef typename internal::remove_all::type NestedExpression; + + SparseSymmetricPermutationProduct(const MatrixType& mat, const Perm& perm) + : m_matrix(mat), m_perm(perm) + {} + + inline Index rows() const { return m_matrix.rows(); } + inline Index cols() const { return m_matrix.cols(); } + + const NestedExpression& matrix() const { return m_matrix; } + const Perm& perm() const { return m_perm; } + + protected: + MatrixTypeNested m_matrix; + const Perm& m_perm; + +}; + +namespace internal { + +template +struct Assignment, internal::assign_op, Sparse2Sparse> +{ + typedef SparseSymmetricPermutationProduct SrcXprType; + typedef typename DstXprType::StorageIndex DstIndex; + template + static void run(SparseMatrix &dst, const SrcXprType &src, const internal::assign_op &) + { + // internal::permute_symm_to_fullsymm(m_matrix,_dest,m_perm.indices().data()); + SparseMatrix tmp; + internal::permute_symm_to_fullsymm(src.matrix(),tmp,src.perm().indices().data()); + dst = tmp; + } + + template + static void run(SparseSelfAdjointView& dst, const SrcXprType &src, const internal::assign_op &) + { + internal::permute_symm_to_symm(src.matrix(),dst.matrix(),src.perm().indices().data()); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h new file mode 100644 index 0000000000..b4c9a422f0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h @@ -0,0 +1,124 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Gael Guennebaud +// +// 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_SPARSESOLVERBASE_H +#define EIGEN_SPARSESOLVERBASE_H + +namespace Eigen { + +namespace internal { + + /** \internal + * Helper functions to solve with a sparse right-hand-side and result. + * The rhs is decomposed into small vertical panels which are solved through dense temporaries. + */ +template +typename enable_if::type +solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest) +{ + EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); + typedef typename Dest::Scalar DestScalar; + // we process the sparse rhs per block of NbColsAtOnce columns temporarily stored into a dense matrix. + static const Index NbColsAtOnce = 4; + Index rhsCols = rhs.cols(); + Index size = rhs.rows(); + // the temporary matrices do not need more columns than NbColsAtOnce: + Index tmpCols = (std::min)(rhsCols, NbColsAtOnce); + Eigen::Matrix tmp(size,tmpCols); + Eigen::Matrix tmpX(size,tmpCols); + for(Index k=0; k(rhsCols-k, NbColsAtOnce); + tmp.leftCols(actualCols) = rhs.middleCols(k,actualCols); + tmpX.leftCols(actualCols) = dec.solve(tmp.leftCols(actualCols)); + dest.middleCols(k,actualCols) = tmpX.leftCols(actualCols).sparseView(); + } +} + +// Overload for vector as rhs +template +typename enable_if::type +solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest) +{ + typedef typename Dest::Scalar DestScalar; + Index size = rhs.rows(); + Eigen::Matrix rhs_dense(rhs); + Eigen::Matrix dest_dense(size); + dest_dense = dec.solve(rhs_dense); + dest = dest_dense.sparseView(); +} + +} // end namespace internal + +/** \class SparseSolverBase + * \ingroup SparseCore_Module + * \brief A base class for sparse solvers + * + * \tparam Derived the actual type of the solver. + * + */ +template +class SparseSolverBase : internal::noncopyable +{ + public: + + /** Default constructor */ + SparseSolverBase() + : m_isInitialized(false) + {} + + ~SparseSolverBase() + {} + + Derived& derived() { return *static_cast(this); } + const Derived& derived() const { return *static_cast(this); } + + /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A. + * + * \sa compute() + */ + template + inline const Solve + solve(const MatrixBase& b) const + { + eigen_assert(m_isInitialized && "Solver is not initialized."); + eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b"); + return Solve(derived(), b.derived()); + } + + /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A. + * + * \sa compute() + */ + template + inline const Solve + solve(const SparseMatrixBase& b) const + { + eigen_assert(m_isInitialized && "Solver is not initialized."); + eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b"); + return Solve(derived(), b.derived()); + } + + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal default implementation of solving with a sparse rhs */ + template + void _solve_impl(const SparseMatrixBase &b, SparseMatrixBase &dest) const + { + internal::solve_sparse_through_dense_panels(derived(), b.derived(), dest.derived()); + } + #endif // EIGEN_PARSED_BY_DOXYGEN + + protected: + + mutable bool m_isInitialized; +}; + +} // end namespace Eigen + +#endif // EIGEN_SPARSESOLVERBASE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h new file mode 100644 index 0000000000..88820a48f3 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h @@ -0,0 +1,198 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSESPARSEPRODUCTWITHPRUNING_H +#define EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H + +namespace Eigen { + +namespace internal { + + +// perform a pseudo in-place sparse * sparse product assuming all matrices are col major +template +static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, const typename ResultType::RealScalar& tolerance) +{ + // return sparse_sparse_product_with_pruning_impl2(lhs,rhs,res); + + typedef typename remove_all::type::Scalar RhsScalar; + typedef typename remove_all::type::Scalar ResScalar; + typedef typename remove_all::type::StorageIndex StorageIndex; + + // make sure to call innerSize/outerSize since we fake the storage order. + Index rows = lhs.innerSize(); + Index cols = rhs.outerSize(); + //Index size = lhs.outerSize(); + eigen_assert(lhs.outerSize() == rhs.innerSize()); + + // allocate a temporary buffer + AmbiVector tempVector(rows); + + // mimics a resizeByInnerOuter: + if(ResultType::IsRowMajor) + res.resize(cols, rows); + else + res.resize(rows, cols); + + evaluator lhsEval(lhs); + evaluator rhsEval(rhs); + + // estimate the number of non zero entries + // given a rhs column containing Y non zeros, we assume that the respective Y columns + // of the lhs differs in average of one non zeros, thus the number of non zeros for + // the product of a rhs column with the lhs is X+Y where X is the average number of non zero + // per column of the lhs. + // Therefore, we have nnz(lhs*rhs) = nnz(lhs) + nnz(rhs) + Index estimated_nnz_prod = lhsEval.nonZerosEstimate() + rhsEval.nonZerosEstimate(); + + res.reserve(estimated_nnz_prod); + double ratioColRes = double(estimated_nnz_prod)/(double(lhs.rows())*double(rhs.cols())); + for (Index j=0; j::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt) + { + // FIXME should be written like this: tmp += rhsIt.value() * lhs.col(rhsIt.index()) + tempVector.restart(); + RhsScalar x = rhsIt.value(); + for (typename evaluator::InnerIterator lhsIt(lhsEval, rhsIt.index()); lhsIt; ++lhsIt) + { + tempVector.coeffRef(lhsIt.index()) += lhsIt.value() * x; + } + } + res.startVec(j); + for (typename AmbiVector::Iterator it(tempVector,tolerance); it; ++it) + res.insertBackByOuterInner(j,it.index()) = it.value(); + } + res.finalize(); +} + +template::Flags&RowMajorBit, + int RhsStorageOrder = traits::Flags&RowMajorBit, + int ResStorageOrder = traits::Flags&RowMajorBit> +struct sparse_sparse_product_with_pruning_selector; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + typename remove_all::type _res(res.rows(), res.cols()); + internal::sparse_sparse_product_with_pruning_impl(lhs, rhs, _res, tolerance); + res.swap(_res); + } +}; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + // we need a col-major matrix to hold the result + typedef SparseMatrix SparseTemporaryType; + SparseTemporaryType _res(res.rows(), res.cols()); + internal::sparse_sparse_product_with_pruning_impl(lhs, rhs, _res, tolerance); + res = _res; + } +}; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + // let's transpose the product to get a column x column product + typename remove_all::type _res(res.rows(), res.cols()); + internal::sparse_sparse_product_with_pruning_impl(rhs, lhs, _res, tolerance); + res.swap(_res); + } +}; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + typedef SparseMatrix ColMajorMatrixLhs; + typedef SparseMatrix ColMajorMatrixRhs; + ColMajorMatrixLhs colLhs(lhs); + ColMajorMatrixRhs colRhs(rhs); + internal::sparse_sparse_product_with_pruning_impl(colLhs, colRhs, res, tolerance); + + // let's transpose the product to get a column x column product +// typedef SparseMatrix SparseTemporaryType; +// SparseTemporaryType _res(res.cols(), res.rows()); +// sparse_sparse_product_with_pruning_impl(rhs, lhs, _res); +// res = _res.transpose(); + } +}; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + typedef SparseMatrix RowMajorMatrixLhs; + RowMajorMatrixLhs rowLhs(lhs); + sparse_sparse_product_with_pruning_selector(rowLhs,rhs,res,tolerance); + } +}; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + typedef SparseMatrix RowMajorMatrixRhs; + RowMajorMatrixRhs rowRhs(rhs); + sparse_sparse_product_with_pruning_selector(lhs,rowRhs,res,tolerance); + } +}; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + typedef SparseMatrix ColMajorMatrixRhs; + ColMajorMatrixRhs colRhs(rhs); + internal::sparse_sparse_product_with_pruning_impl(lhs, colRhs, res, tolerance); + } +}; + +template +struct sparse_sparse_product_with_pruning_selector +{ + typedef typename ResultType::RealScalar RealScalar; + static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance) + { + typedef SparseMatrix ColMajorMatrixLhs; + ColMajorMatrixLhs colLhs(lhs); + internal::sparse_sparse_product_with_pruning_impl(colLhs, rhs, res, tolerance); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h new file mode 100644 index 0000000000..3757d4c6b0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h @@ -0,0 +1,92 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2015 Gael Guennebaud +// +// 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_SPARSETRANSPOSE_H +#define EIGEN_SPARSETRANSPOSE_H + +namespace Eigen { + +namespace internal { + template + class SparseTransposeImpl + : public SparseMatrixBase > + {}; + + template + class SparseTransposeImpl + : public SparseCompressedBase > + { + typedef SparseCompressedBase > Base; + public: + using Base::derived; + typedef typename Base::Scalar Scalar; + typedef typename Base::StorageIndex StorageIndex; + + inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); } + + inline const Scalar* valuePtr() const { return derived().nestedExpression().valuePtr(); } + inline const StorageIndex* innerIndexPtr() const { return derived().nestedExpression().innerIndexPtr(); } + inline const StorageIndex* outerIndexPtr() const { return derived().nestedExpression().outerIndexPtr(); } + inline const StorageIndex* innerNonZeroPtr() const { return derived().nestedExpression().innerNonZeroPtr(); } + + inline Scalar* valuePtr() { return derived().nestedExpression().valuePtr(); } + inline StorageIndex* innerIndexPtr() { return derived().nestedExpression().innerIndexPtr(); } + inline StorageIndex* outerIndexPtr() { return derived().nestedExpression().outerIndexPtr(); } + inline StorageIndex* innerNonZeroPtr() { return derived().nestedExpression().innerNonZeroPtr(); } + }; +} + +template class TransposeImpl + : public internal::SparseTransposeImpl +{ + protected: + typedef internal::SparseTransposeImpl Base; +}; + +namespace internal { + +template +struct unary_evaluator, IteratorBased> + : public evaluator_base > +{ + typedef typename evaluator::InnerIterator EvalIterator; + public: + typedef Transpose XprType; + + inline Index nonZerosEstimate() const { + return m_argImpl.nonZerosEstimate(); + } + + class InnerIterator : public EvalIterator + { + public: + EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer) + : EvalIterator(unaryOp.m_argImpl,outer) + {} + + Index row() const { return EvalIterator::col(); } + Index col() const { return EvalIterator::row(); } + }; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = XprType::Flags + }; + + explicit unary_evaluator(const XprType& op) :m_argImpl(op.nestedExpression()) {} + + protected: + evaluator m_argImpl; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSETRANSPOSE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h new file mode 100644 index 0000000000..9ac120266a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h @@ -0,0 +1,189 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-2015 Gael Guennebaud +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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_SPARSE_TRIANGULARVIEW_H +#define EIGEN_SPARSE_TRIANGULARVIEW_H + +namespace Eigen { + +/** \ingroup SparseCore_Module + * + * \brief Base class for a triangular part in a \b sparse matrix + * + * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be instantiated. + * It extends class TriangularView with additional methods which are available for sparse expressions only. + * + * \sa class TriangularView, SparseMatrixBase::triangularView() + */ +template class TriangularViewImpl + : public SparseMatrixBase > +{ + enum { SkipFirst = ((Mode&Lower) && !(MatrixType::Flags&RowMajorBit)) + || ((Mode&Upper) && (MatrixType::Flags&RowMajorBit)), + SkipLast = !SkipFirst, + SkipDiag = (Mode&ZeroDiag) ? 1 : 0, + HasUnitDiag = (Mode&UnitDiag) ? 1 : 0 + }; + + typedef TriangularView TriangularViewType; + + protected: + // dummy solve function to make TriangularView happy. + void solve() const; + + typedef SparseMatrixBase Base; + public: + + EIGEN_SPARSE_PUBLIC_INTERFACE(TriangularViewType) + + typedef typename MatrixType::Nested MatrixTypeNested; + typedef typename internal::remove_reference::type MatrixTypeNestedNonRef; + typedef typename internal::remove_all::type MatrixTypeNestedCleaned; + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _solve_impl(const RhsType &rhs, DstType &dst) const { + if(!(internal::is_same::value && internal::extract_data(dst) == internal::extract_data(rhs))) + dst = rhs; + this->solveInPlace(dst); + } + + /** Applies the inverse of \c *this to the dense vector or matrix \a other, "in-place" */ + template void solveInPlace(MatrixBase& other) const; + + /** Applies the inverse of \c *this to the sparse vector or matrix \a other, "in-place" */ + template void solveInPlace(SparseMatrixBase& other) const; + +}; + +namespace internal { + +template +struct unary_evaluator, IteratorBased> + : evaluator_base > +{ + typedef TriangularView XprType; + +protected: + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::StorageIndex StorageIndex; + typedef typename evaluator::InnerIterator EvalIterator; + + enum { SkipFirst = ((Mode&Lower) && !(ArgType::Flags&RowMajorBit)) + || ((Mode&Upper) && (ArgType::Flags&RowMajorBit)), + SkipLast = !SkipFirst, + SkipDiag = (Mode&ZeroDiag) ? 1 : 0, + HasUnitDiag = (Mode&UnitDiag) ? 1 : 0 + }; + +public: + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = XprType::Flags + }; + + explicit unary_evaluator(const XprType &xpr) : m_argImpl(xpr.nestedExpression()), m_arg(xpr.nestedExpression()) {} + + inline Index nonZerosEstimate() const { + return m_argImpl.nonZerosEstimate(); + } + + class InnerIterator : public EvalIterator + { + typedef EvalIterator Base; + public: + + EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& xprEval, Index outer) + : Base(xprEval.m_argImpl,outer), m_returnOne(false), m_containsDiag(Base::outer()index()<=outer : this->index()=Base::outer())) + { + if((!SkipFirst) && Base::operator bool()) + Base::operator++(); + m_returnOne = m_containsDiag; + } + } + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + if(HasUnitDiag && m_returnOne) + m_returnOne = false; + else + { + Base::operator++(); + if(HasUnitDiag && (!SkipFirst) && ((!Base::operator bool()) || Base::index()>=Base::outer())) + { + if((!SkipFirst) && Base::operator bool()) + Base::operator++(); + m_returnOne = m_containsDiag; + } + } + return *this; + } + + EIGEN_STRONG_INLINE operator bool() const + { + if(HasUnitDiag && m_returnOne) + return true; + if(SkipFirst) return Base::operator bool(); + else + { + if (SkipDiag) return (Base::operator bool() && this->index() < this->outer()); + else return (Base::operator bool() && this->index() <= this->outer()); + } + } + +// inline Index row() const { return (ArgType::Flags&RowMajorBit ? Base::outer() : this->index()); } +// inline Index col() const { return (ArgType::Flags&RowMajorBit ? this->index() : Base::outer()); } + inline StorageIndex index() const + { + if(HasUnitDiag && m_returnOne) return internal::convert_index(Base::outer()); + else return Base::index(); + } + inline Scalar value() const + { + if(HasUnitDiag && m_returnOne) return Scalar(1); + else return Base::value(); + } + + protected: + bool m_returnOne; + bool m_containsDiag; + private: + Scalar& valueRef(); + }; + +protected: + evaluator m_argImpl; + const ArgType& m_arg; +}; + +} // end namespace internal + +template +template +inline const TriangularView +SparseMatrixBase::triangularView() const +{ + return TriangularView(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_TRIANGULARVIEW_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h new file mode 100644 index 0000000000..ceb9368879 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h @@ -0,0 +1,186 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// +// 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_SPARSEUTIL_H +#define EIGEN_SPARSEUTIL_H + +namespace Eigen { + +#ifdef NDEBUG +#define EIGEN_DBG_SPARSE(X) +#else +#define EIGEN_DBG_SPARSE(X) X +#endif + +#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \ +template \ +EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::SparseMatrixBase& other) \ +{ \ + return Base::operator Op(other.derived()); \ +} \ +EIGEN_STRONG_INLINE Derived& operator Op(const Derived& other) \ +{ \ + return Base::operator Op(other); \ +} + +#define EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \ +template \ +EIGEN_STRONG_INLINE Derived& operator Op(const Other& scalar) \ +{ \ + return Base::operator Op(scalar); \ +} + +#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ +EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, =) + + +#define EIGEN_SPARSE_PUBLIC_INTERFACE(Derived) \ + EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) + + +const int CoherentAccessPattern = 0x1; +const int InnerRandomAccessPattern = 0x2 | CoherentAccessPattern; +const int OuterRandomAccessPattern = 0x4 | CoherentAccessPattern; +const int RandomAccessPattern = 0x8 | OuterRandomAccessPattern | InnerRandomAccessPattern; + +template class SparseMatrix; +template class DynamicSparseMatrix; +template class SparseVector; +template class MappedSparseMatrix; + +template class SparseSelfAdjointView; +template class SparseDiagonalProduct; +template class SparseView; + +template class SparseSparseProduct; +template class SparseTimeDenseProduct; +template class DenseTimeSparseProduct; +template class SparseDenseOuterProduct; + +template struct SparseSparseProductReturnType; +template::ColsAtCompileTime,internal::traits::RowsAtCompileTime)> struct DenseSparseProductReturnType; + +template::ColsAtCompileTime,internal::traits::RowsAtCompileTime)> struct SparseDenseProductReturnType; +template class SparseSymmetricPermutationProduct; + +namespace internal { + +template struct sparse_eval; + +template struct eval + : sparse_eval::RowsAtCompileTime,traits::ColsAtCompileTime,traits::Flags> +{}; + +template struct sparse_eval { + typedef typename traits::Scalar _Scalar; + typedef typename traits::StorageIndex _StorageIndex; + public: + typedef SparseVector<_Scalar, RowMajor, _StorageIndex> type; +}; + +template struct sparse_eval { + typedef typename traits::Scalar _Scalar; + typedef typename traits::StorageIndex _StorageIndex; + public: + typedef SparseVector<_Scalar, ColMajor, _StorageIndex> type; +}; + +// TODO this seems almost identical to plain_matrix_type +template struct sparse_eval { + typedef typename traits::Scalar _Scalar; + typedef typename traits::StorageIndex _StorageIndex; + enum { _Options = ((Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor }; + public: + typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type; +}; + +template struct sparse_eval { + typedef typename traits::Scalar _Scalar; + public: + typedef Matrix<_Scalar, 1, 1> type; +}; + +template struct plain_matrix_type +{ + typedef typename traits::Scalar _Scalar; + typedef typename traits::StorageIndex _StorageIndex; + enum { _Options = ((evaluator::Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor }; + public: + typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type; +}; + +template +struct plain_object_eval + : sparse_eval::RowsAtCompileTime,traits::ColsAtCompileTime, evaluator::Flags> +{}; + +template +struct solve_traits +{ + typedef typename sparse_eval::Flags>::type PlainObject; +}; + +template +struct generic_xpr_base +{ + typedef SparseMatrixBase type; +}; + +struct SparseTriangularShape { static std::string debugName() { return "SparseTriangularShape"; } }; +struct SparseSelfAdjointShape { static std::string debugName() { return "SparseSelfAdjointShape"; } }; + +template<> struct glue_shapes { typedef SparseSelfAdjointShape type; }; +template<> struct glue_shapes { typedef SparseTriangularShape type; }; + +// return type of SparseCompressedBase::lower_bound; +struct LowerBoundIndex { + LowerBoundIndex() : value(-1), found(false) {} + LowerBoundIndex(Index val, bool ok) : value(val), found(ok) {} + Index value; + bool found; +}; + +} // end namespace internal + +/** \ingroup SparseCore_Module + * + * \class Triplet + * + * \brief A small structure to hold a non zero as a triplet (i,j,value). + * + * \sa SparseMatrix::setFromTriplets() + */ +template::StorageIndex > +class Triplet +{ +public: + Triplet() : m_row(0), m_col(0), m_value(0) {} + + Triplet(const StorageIndex& i, const StorageIndex& j, const Scalar& v = Scalar(0)) + : m_row(i), m_col(j), m_value(v) + {} + + /** \returns the row index of the element */ + const StorageIndex& row() const { return m_row; } + + /** \returns the column index of the element */ + const StorageIndex& col() const { return m_col; } + + /** \returns the value of the element */ + const Scalar& value() const { return m_value; } +protected: + StorageIndex m_row, m_col; + Scalar m_value; +}; + +} // end namespace Eigen + +#endif // EIGEN_SPARSEUTIL_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h new file mode 100644 index 0000000000..05779be685 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h @@ -0,0 +1,478 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2015 Gael Guennebaud +// +// 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_SPARSEVECTOR_H +#define EIGEN_SPARSEVECTOR_H + +namespace Eigen { + +/** \ingroup SparseCore_Module + * \class SparseVector + * + * \brief a sparse vector class + * + * \tparam _Scalar the scalar type, i.e. the type of the coefficients + * + * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme. + * + * This class can be extended with the help of the plugin mechanism described on the page + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEVECTOR_PLUGIN. + */ + +namespace internal { +template +struct traits > +{ + typedef _Scalar Scalar; + typedef _StorageIndex StorageIndex; + typedef Sparse StorageKind; + typedef MatrixXpr XprKind; + enum { + IsColVector = (_Options & RowMajorBit) ? 0 : 1, + + RowsAtCompileTime = IsColVector ? Dynamic : 1, + ColsAtCompileTime = IsColVector ? 1 : Dynamic, + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + Flags = _Options | NestByRefBit | LvalueBit | (IsColVector ? 0 : RowMajorBit) | CompressedAccessBit, + SupportedAccessPatterns = InnerRandomAccessPattern + }; +}; + +// Sparse-Vector-Assignment kinds: +enum { + SVA_RuntimeSwitch, + SVA_Inner, + SVA_Outer +}; + +template< typename Dest, typename Src, + int AssignmentKind = !bool(Src::IsVectorAtCompileTime) ? SVA_RuntimeSwitch + : Src::InnerSizeAtCompileTime==1 ? SVA_Outer + : SVA_Inner> +struct sparse_vector_assign_selector; + +} + +template +class SparseVector + : public SparseCompressedBase > +{ + typedef SparseCompressedBase Base; + using Base::convert_index; + public: + EIGEN_SPARSE_PUBLIC_INTERFACE(SparseVector) + EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, +=) + EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, -=) + + typedef internal::CompressedStorage Storage; + enum { IsColVector = internal::traits::IsColVector }; + + enum { + Options = _Options + }; + + EIGEN_STRONG_INLINE Index rows() const { return IsColVector ? m_size : 1; } + EIGEN_STRONG_INLINE Index cols() const { return IsColVector ? 1 : m_size; } + EIGEN_STRONG_INLINE Index innerSize() const { return m_size; } + EIGEN_STRONG_INLINE Index outerSize() const { return 1; } + + EIGEN_STRONG_INLINE const Scalar* valuePtr() const { return m_data.valuePtr(); } + EIGEN_STRONG_INLINE Scalar* valuePtr() { return m_data.valuePtr(); } + + EIGEN_STRONG_INLINE const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); } + EIGEN_STRONG_INLINE StorageIndex* innerIndexPtr() { return m_data.indexPtr(); } + + inline const StorageIndex* outerIndexPtr() const { return 0; } + inline StorageIndex* outerIndexPtr() { return 0; } + inline const StorageIndex* innerNonZeroPtr() const { return 0; } + inline StorageIndex* innerNonZeroPtr() { return 0; } + + /** \internal */ + inline Storage& data() { return m_data; } + /** \internal */ + inline const Storage& data() const { return m_data; } + + inline Scalar coeff(Index row, Index col) const + { + eigen_assert(IsColVector ? (col==0 && row>=0 && row=0 && col=0 && i=0 && row=0 && col=0 && i=0 && row=0 && col=0 && i= startId) && (m_data.index(p) > i) ) + { + m_data.index(p+1) = m_data.index(p); + m_data.value(p+1) = m_data.value(p); + --p; + } + m_data.index(p+1) = convert_index(i); + m_data.value(p+1) = 0; + return m_data.value(p+1); + } + + /** + */ + inline void reserve(Index reserveSize) { m_data.reserve(reserveSize); } + + + inline void finalize() {} + + /** \copydoc SparseMatrix::prune(const Scalar&,const RealScalar&) */ + void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits::dummy_precision()) + { + m_data.prune(reference,epsilon); + } + + /** Resizes the sparse vector to \a rows x \a cols + * + * This method is provided for compatibility with matrices. + * For a column vector, \a cols must be equal to 1. + * For a row vector, \a rows must be equal to 1. + * + * \sa resize(Index) + */ + void resize(Index rows, Index cols) + { + eigen_assert((IsColVector ? cols : rows)==1 && "Outer dimension must equal 1"); + resize(IsColVector ? rows : cols); + } + + /** Resizes the sparse vector to \a newSize + * This method deletes all entries, thus leaving an empty sparse vector + * + * \sa conservativeResize(), setZero() */ + void resize(Index newSize) + { + m_size = newSize; + m_data.clear(); + } + + /** Resizes the sparse vector to \a newSize, while leaving old values untouched. + * + * If the size of the vector is decreased, then the storage of the out-of bounds coefficients is kept and reserved. + * Call .data().squeeze() to free extra memory. + * + * \sa reserve(), setZero() + */ + void conservativeResize(Index newSize) + { + if (newSize < m_size) + { + Index i = 0; + while (i + inline SparseVector(const SparseMatrixBase& other) + : m_size(0) + { + #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN + #endif + check_template_parameters(); + *this = other.derived(); + } + + inline SparseVector(const SparseVector& other) + : Base(other), m_size(0) + { + check_template_parameters(); + *this = other.derived(); + } + + /** Swaps the values of \c *this and \a other. + * Overloaded for performance: this version performs a \em shallow swap by swapping pointers and attributes only. + * \sa SparseMatrixBase::swap() + */ + inline void swap(SparseVector& other) + { + std::swap(m_size, other.m_size); + m_data.swap(other.m_data); + } + + template + inline void swap(SparseMatrix& other) + { + eigen_assert(other.outerSize()==1); + std::swap(m_size, other.m_innerSize); + m_data.swap(other.m_data); + } + + inline SparseVector& operator=(const SparseVector& other) + { + if (other.isRValue()) + { + swap(other.const_cast_derived()); + } + else + { + resize(other.size()); + m_data = other.m_data; + } + return *this; + } + + template + inline SparseVector& operator=(const SparseMatrixBase& other) + { + SparseVector tmp(other.size()); + internal::sparse_vector_assign_selector::run(tmp,other.derived()); + this->swap(tmp); + return *this; + } + + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + inline SparseVector& operator=(const SparseSparseProduct& product) + { + return Base::operator=(product); + } + #endif + + friend std::ostream & operator << (std::ostream & s, const SparseVector& m) + { + for (Index i=0; i::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE); + EIGEN_STATIC_ASSERT((_Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS); + } + + Storage m_data; + Index m_size; +}; + +namespace internal { + +template +struct evaluator > + : evaluator_base > +{ + typedef SparseVector<_Scalar,_Options,_Index> SparseVectorType; + typedef evaluator_base Base; + typedef typename SparseVectorType::InnerIterator InnerIterator; + typedef typename SparseVectorType::ReverseInnerIterator ReverseInnerIterator; + + enum { + CoeffReadCost = NumTraits<_Scalar>::ReadCost, + Flags = SparseVectorType::Flags + }; + + evaluator() : Base() {} + + explicit evaluator(const SparseVectorType &mat) : m_matrix(&mat) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + inline Index nonZerosEstimate() const { + return m_matrix->nonZeros(); + } + + operator SparseVectorType&() { return m_matrix->const_cast_derived(); } + operator const SparseVectorType&() const { return *m_matrix; } + + const SparseVectorType *m_matrix; +}; + +template< typename Dest, typename Src> +struct sparse_vector_assign_selector { + static void run(Dest& dst, const Src& src) { + eigen_internal_assert(src.innerSize()==src.size()); + typedef internal::evaluator SrcEvaluatorType; + SrcEvaluatorType srcEval(src); + for(typename SrcEvaluatorType::InnerIterator it(srcEval, 0); it; ++it) + dst.insert(it.index()) = it.value(); + } +}; + +template< typename Dest, typename Src> +struct sparse_vector_assign_selector { + static void run(Dest& dst, const Src& src) { + eigen_internal_assert(src.outerSize()==src.size()); + typedef internal::evaluator SrcEvaluatorType; + SrcEvaluatorType srcEval(src); + for(Index i=0; i +struct sparse_vector_assign_selector { + static void run(Dest& dst, const Src& src) { + if(src.outerSize()==1) sparse_vector_assign_selector::run(dst, src); + else sparse_vector_assign_selector::run(dst, src); + } +}; + +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSEVECTOR_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h new file mode 100644 index 0000000000..92b3d1f7ba --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h @@ -0,0 +1,254 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011-2014 Gael Guennebaud +// Copyright (C) 2010 Daniel Lowengrub +// +// 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_SPARSEVIEW_H +#define EIGEN_SPARSEVIEW_H + +namespace Eigen { + +namespace internal { + +template +struct traits > : traits +{ + typedef typename MatrixType::StorageIndex StorageIndex; + typedef Sparse StorageKind; + enum { + Flags = int(traits::Flags) & (RowMajorBit) + }; +}; + +} // end namespace internal + +/** \ingroup SparseCore_Module + * \class SparseView + * + * \brief Expression of a dense or sparse matrix with zero or too small values removed + * + * \tparam MatrixType the type of the object of which we are removing the small entries + * + * This class represents an expression of a given dense or sparse matrix with + * entries smaller than \c reference * \c epsilon are removed. + * It is the return type of MatrixBase::sparseView() and SparseMatrixBase::pruned() + * and most of the time this is the only way it is used. + * + * \sa MatrixBase::sparseView(), SparseMatrixBase::pruned() + */ +template +class SparseView : public SparseMatrixBase > +{ + typedef typename MatrixType::Nested MatrixTypeNested; + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef SparseMatrixBase Base; +public: + EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView) + typedef typename internal::remove_all::type NestedExpression; + + explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0), + const RealScalar &epsilon = NumTraits::dummy_precision()) + : m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {} + + inline Index rows() const { return m_matrix.rows(); } + inline Index cols() const { return m_matrix.cols(); } + + inline Index innerSize() const { return m_matrix.innerSize(); } + inline Index outerSize() const { return m_matrix.outerSize(); } + + /** \returns the nested expression */ + const typename internal::remove_all::type& + nestedExpression() const { return m_matrix; } + + Scalar reference() const { return m_reference; } + RealScalar epsilon() const { return m_epsilon; } + +protected: + MatrixTypeNested m_matrix; + Scalar m_reference; + RealScalar m_epsilon; +}; + +namespace internal { + +// TODO find a way to unify the two following variants +// This is tricky because implementing an inner iterator on top of an IndexBased evaluator is +// not easy because the evaluators do not expose the sizes of the underlying expression. + +template +struct unary_evaluator, IteratorBased> + : public evaluator_base > +{ + typedef typename evaluator::InnerIterator EvalIterator; + public: + typedef SparseView XprType; + + class InnerIterator : public EvalIterator + { + protected: + typedef typename XprType::Scalar Scalar; + public: + + EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer) + : EvalIterator(sve.m_argImpl,outer), m_view(sve.m_view) + { + incrementToNonZero(); + } + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + EvalIterator::operator++(); + incrementToNonZero(); + return *this; + } + + using EvalIterator::value; + + protected: + const XprType &m_view; + + private: + void incrementToNonZero() + { + while((bool(*this)) && internal::isMuchSmallerThan(value(), m_view.reference(), m_view.epsilon())) + { + EvalIterator::operator++(); + } + } + }; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = XprType::Flags + }; + + explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {} + + protected: + evaluator m_argImpl; + const XprType &m_view; +}; + +template +struct unary_evaluator, IndexBased> + : public evaluator_base > +{ + public: + typedef SparseView XprType; + protected: + enum { IsRowMajor = (XprType::Flags&RowMajorBit)==RowMajorBit }; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::StorageIndex StorageIndex; + public: + + class InnerIterator + { + public: + + EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer) + : m_sve(sve), m_inner(0), m_outer(outer), m_end(sve.m_view.innerSize()) + { + incrementToNonZero(); + } + + EIGEN_STRONG_INLINE InnerIterator& operator++() + { + m_inner++; + incrementToNonZero(); + return *this; + } + + EIGEN_STRONG_INLINE Scalar value() const + { + return (IsRowMajor) ? m_sve.m_argImpl.coeff(m_outer, m_inner) + : m_sve.m_argImpl.coeff(m_inner, m_outer); + } + + EIGEN_STRONG_INLINE StorageIndex index() const { return m_inner; } + inline Index row() const { return IsRowMajor ? m_outer : index(); } + inline Index col() const { return IsRowMajor ? index() : m_outer; } + + EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; } + + protected: + const unary_evaluator &m_sve; + Index m_inner; + const Index m_outer; + const Index m_end; + + private: + void incrementToNonZero() + { + while((bool(*this)) && internal::isMuchSmallerThan(value(), m_sve.m_view.reference(), m_sve.m_view.epsilon())) + { + m_inner++; + } + } + }; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = XprType::Flags + }; + + explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {} + + protected: + evaluator m_argImpl; + const XprType &m_view; +}; + +} // end namespace internal + +/** \ingroup SparseCore_Module + * + * \returns a sparse expression of the dense expression \c *this with values smaller than + * \a reference * \a epsilon removed. + * + * This method is typically used when prototyping to convert a quickly assembled dense Matrix \c D to a SparseMatrix \c S: + * \code + * MatrixXd D(n,m); + * SparseMatrix S; + * S = D.sparseView(); // suppress numerical zeros (exact) + * S = D.sparseView(reference); + * S = D.sparseView(reference,epsilon); + * \endcode + * where \a reference is a meaningful non zero reference value, + * and \a epsilon is a tolerance factor defaulting to NumTraits::dummy_precision(). + * + * \sa SparseMatrixBase::pruned(), class SparseView */ +template +const SparseView MatrixBase::sparseView(const Scalar& reference, + const typename NumTraits::Real& epsilon) const +{ + return SparseView(derived(), reference, epsilon); +} + +/** \returns an expression of \c *this with values smaller than + * \a reference * \a epsilon removed. + * + * This method is typically used in conjunction with the product of two sparse matrices + * to automatically prune the smallest values as follows: + * \code + * C = (A*B).pruned(); // suppress numerical zeros (exact) + * C = (A*B).pruned(ref); + * C = (A*B).pruned(ref,epsilon); + * \endcode + * where \c ref is a meaningful non zero reference value. + * */ +template +const SparseView +SparseMatrixBase::pruned(const Scalar& reference, + const RealScalar& epsilon) const +{ + return SparseView(derived(), reference, epsilon); +} + +} // end namespace Eigen + +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h new file mode 100644 index 0000000000..f9c56ba798 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h @@ -0,0 +1,315 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// 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_SPARSETRIANGULARSOLVER_H +#define EIGEN_SPARSETRIANGULARSOLVER_H + +namespace Eigen { + +namespace internal { + +template::Flags) & RowMajorBit> +struct sparse_solve_triangular_selector; + +// forward substitution, row-major +template +struct sparse_solve_triangular_selector +{ + typedef typename Rhs::Scalar Scalar; + typedef evaluator LhsEval; + typedef typename evaluator::InnerIterator LhsIterator; + static void run(const Lhs& lhs, Rhs& other) + { + LhsEval lhsEval(lhs); + for(Index col=0 ; col +struct sparse_solve_triangular_selector +{ + typedef typename Rhs::Scalar Scalar; + typedef evaluator LhsEval; + typedef typename evaluator::InnerIterator LhsIterator; + static void run(const Lhs& lhs, Rhs& other) + { + LhsEval lhsEval(lhs); + for(Index col=0 ; col=0 ; --i) + { + Scalar tmp = other.coeff(i,col); + Scalar l_ii(0); + LhsIterator it(lhsEval, i); + while(it && it.index() +struct sparse_solve_triangular_selector +{ + typedef typename Rhs::Scalar Scalar; + typedef evaluator LhsEval; + typedef typename evaluator::InnerIterator LhsIterator; + static void run(const Lhs& lhs, Rhs& other) + { + LhsEval lhsEval(lhs); + for(Index col=0 ; col +struct sparse_solve_triangular_selector +{ + typedef typename Rhs::Scalar Scalar; + typedef evaluator LhsEval; + typedef typename evaluator::InnerIterator LhsIterator; + static void run(const Lhs& lhs, Rhs& other) + { + LhsEval lhsEval(lhs); + for(Index col=0 ; col=0; --i) + { + Scalar& tmp = other.coeffRef(i,col); + if (tmp!=Scalar(0)) // optimization when other is actually sparse + { + if(!(Mode & UnitDiag)) + { + // TODO replace this by a binary search. make sure the binary search is safe for partially sorted elements + LhsIterator it(lhsEval, i); + while(it && it.index()!=i) + ++it; + eigen_assert(it && it.index()==i); + other.coeffRef(i,col) /= it.value(); + } + LhsIterator it(lhsEval, i); + for(; it && it.index() +template +void TriangularViewImpl::solveInPlace(MatrixBase& other) const +{ + eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows()); + eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower))); + + enum { copy = internal::traits::Flags & RowMajorBit }; + + typedef typename internal::conditional::type, OtherDerived&>::type OtherCopy; + OtherCopy otherCopy(other.derived()); + + internal::sparse_solve_triangular_selector::type, Mode>::run(derived().nestedExpression(), otherCopy); + + if (copy) + other = otherCopy; +} +#endif + +// pure sparse path + +namespace internal { + +template +struct sparse_solve_triangular_sparse_selector; + +// forward substitution, col-major +template +struct sparse_solve_triangular_sparse_selector +{ + typedef typename Rhs::Scalar Scalar; + typedef typename promote_index_type::StorageIndex, + typename traits::StorageIndex>::type StorageIndex; + static void run(const Lhs& lhs, Rhs& other) + { + const bool IsLower = (UpLo==Lower); + AmbiVector tempVector(other.rows()*2); + tempVector.setBounds(0,other.rows()); + + Rhs res(other.rows(), other.cols()); + res.reserve(other.nonZeros()); + + for(Index col=0 ; col=0; + i+=IsLower?1:-1) + { + tempVector.restart(); + Scalar& ci = tempVector.coeffRef(i); + if (ci!=Scalar(0)) + { + // find + typename Lhs::InnerIterator it(lhs, i); + if(!(Mode & UnitDiag)) + { + if (IsLower) + { + eigen_assert(it.index()==i); + ci /= it.value(); + } + else + ci /= lhs.coeff(i,i); + } + tempVector.restart(); + if (IsLower) + { + if (it.index()==i) + ++it; + for(; it; ++it) + tempVector.coeffRef(it.index()) -= ci * it.value(); + } + else + { + for(; it && it.index()::Iterator it(tempVector/*,1e-12*/); it; ++it) + { + ++ count; +// std::cerr << "fill " << it.index() << ", " << col << "\n"; +// std::cout << it.value() << " "; + // FIXME use insertBack + res.insert(it.index(), col) = it.value(); + } +// std::cout << "tempVector.nonZeros() == " << int(count) << " / " << (other.rows()) << "\n"; + } + res.finalize(); + other = res.markAsRValue(); + } +}; + +} // end namespace internal + +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +template +void TriangularViewImpl::solveInPlace(SparseMatrixBase& other) const +{ + eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows()); + eigen_assert( (!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower))); + +// enum { copy = internal::traits::Flags & RowMajorBit }; + +// typedef typename internal::conditional::type, OtherDerived&>::type OtherCopy; +// OtherCopy otherCopy(other.derived()); + + internal::sparse_solve_triangular_sparse_selector::run(derived().nestedExpression(), other.derived()); + +// if (copy) +// other = otherCopy; +} +#endif + +} // end namespace Eigen + +#endif // EIGEN_SPARSETRIANGULARSOLVER_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h new file mode 100644 index 0000000000..0c8d8939be --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h @@ -0,0 +1,923 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2012-2014 Gael Guennebaud +// +// 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_SPARSE_LU_H +#define EIGEN_SPARSE_LU_H + +namespace Eigen { + +template > class SparseLU; +template struct SparseLUMatrixLReturnType; +template struct SparseLUMatrixUReturnType; + +template +class SparseLUTransposeView : public SparseSolverBase > +{ +protected: + typedef SparseSolverBase > APIBase; + using APIBase::m_isInitialized; +public: + typedef typename SparseLUType::Scalar Scalar; + typedef typename SparseLUType::StorageIndex StorageIndex; + typedef typename SparseLUType::MatrixType MatrixType; + typedef typename SparseLUType::OrderingType OrderingType; + + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + SparseLUTransposeView() : m_sparseLU(NULL) {} + SparseLUTransposeView(const SparseLUTransposeView& view) { + this->m_sparseLU = view.m_sparseLU; + } + void setIsInitialized(const bool isInitialized) {this->m_isInitialized = isInitialized;} + void setSparseLU(SparseLUType* sparseLU) {m_sparseLU = sparseLU;} + using APIBase::_solve_impl; + template + bool _solve_impl(const MatrixBase &B, MatrixBase &X_base) const + { + Dest& X(X_base.derived()); + eigen_assert(m_sparseLU->info() == Success && "The matrix should be factorized first"); + EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0, + THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); + + + // this ugly const_cast_derived() helps to detect aliasing when applying the permutations + for(Index j = 0; j < B.cols(); ++j){ + X.col(j) = m_sparseLU->colsPermutation() * B.const_cast_derived().col(j); + } + //Forward substitution with transposed or adjoint of U + m_sparseLU->matrixU().template solveTransposedInPlace(X); + + //Backward substitution with transposed or adjoint of L + m_sparseLU->matrixL().template solveTransposedInPlace(X); + + // Permute back the solution + for (Index j = 0; j < B.cols(); ++j) + X.col(j) = m_sparseLU->rowsPermutation().transpose() * X.col(j); + return true; + } + inline Index rows() const { return m_sparseLU->rows(); } + inline Index cols() const { return m_sparseLU->cols(); } + +private: + SparseLUType *m_sparseLU; + SparseLUTransposeView& operator=(const SparseLUTransposeView&); +}; + + +/** \ingroup SparseLU_Module + * \class SparseLU + * + * \brief Sparse supernodal LU factorization for general matrices + * + * This class implements the supernodal LU factorization for general matrices. + * It uses the main techniques from the sequential SuperLU package + * (http://crd-legacy.lbl.gov/~xiaoye/SuperLU/). It handles transparently real + * and complex arithmetic with single and double precision, depending on the + * scalar type of your input matrix. + * The code has been optimized to provide BLAS-3 operations during supernode-panel updates. + * It benefits directly from the built-in high-performant Eigen BLAS routines. + * Moreover, when the size of a supernode is very small, the BLAS calls are avoided to + * enable a better optimization from the compiler. For best performance, + * you should compile it with NDEBUG flag to avoid the numerous bounds checking on vectors. + * + * An important parameter of this class is the ordering method. It is used to reorder the columns + * (and eventually the rows) of the matrix to reduce the number of new elements that are created during + * numerical factorization. The cheapest method available is COLAMD. + * See \link OrderingMethods_Module the OrderingMethods module \endlink for the list of + * built-in and external ordering methods. + * + * Simple example with key steps + * \code + * VectorXd x(n), b(n); + * SparseMatrix A; + * SparseLU, COLAMDOrdering > solver; + * // fill A and b; + * // Compute the ordering permutation vector from the structural pattern of A + * solver.analyzePattern(A); + * // Compute the numerical factorization + * solver.factorize(A); + * //Use the factors to solve the linear system + * x = solver.solve(b); + * \endcode + * + * \warning The input matrix A should be in a \b compressed and \b column-major form. + * Otherwise an expensive copy will be made. You can call the inexpensive makeCompressed() to get a compressed matrix. + * + * \note Unlike the initial SuperLU implementation, there is no step to equilibrate the matrix. + * For badly scaled matrices, this step can be useful to reduce the pivoting during factorization. + * If this is the case for your matrices, you can try the basic scaling method at + * "unsupported/Eigen/src/IterativeSolvers/Scaling.h" + * + * \tparam _MatrixType The type of the sparse matrix. It must be a column-major SparseMatrix<> + * \tparam _OrderingType The ordering method to use, either AMD, COLAMD or METIS. Default is COLMAD + * + * \implsparsesolverconcept + * + * \sa \ref TutorialSparseSolverConcept + * \sa \ref OrderingMethods_Module + */ +template +class SparseLU : public SparseSolverBase >, public internal::SparseLUImpl +{ + protected: + typedef SparseSolverBase > APIBase; + using APIBase::m_isInitialized; + public: + using APIBase::_solve_impl; + + typedef _MatrixType MatrixType; + typedef _OrderingType OrderingType; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix NCMatrix; + typedef internal::MappedSuperNodalMatrix SCMatrix; + typedef Matrix ScalarVector; + typedef Matrix IndexVector; + typedef PermutationMatrix PermutationType; + typedef internal::SparseLUImpl Base; + + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + public: + + SparseLU():m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1) + { + initperfvalues(); + } + explicit SparseLU(const MatrixType& matrix) + : m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1) + { + initperfvalues(); + compute(matrix); + } + + ~SparseLU() + { + // Free all explicit dynamic pointers + } + + void analyzePattern (const MatrixType& matrix); + void factorize (const MatrixType& matrix); + void simplicialfactorize(const MatrixType& matrix); + + /** + * Compute the symbolic and numeric factorization of the input sparse matrix. + * The input matrix should be in column-major storage. + */ + void compute (const MatrixType& matrix) + { + // Analyze + analyzePattern(matrix); + //Factorize + factorize(matrix); + } + + /** \returns an expression of the transposed of the factored matrix. + * + * A typical usage is to solve for the transposed problem A^T x = b: + * \code + * solver.compute(A); + * x = solver.transpose().solve(b); + * \endcode + * + * \sa adjoint(), solve() + */ + const SparseLUTransposeView > transpose() + { + SparseLUTransposeView > transposeView; + transposeView.setSparseLU(this); + transposeView.setIsInitialized(this->m_isInitialized); + return transposeView; + } + + + /** \returns an expression of the adjoint of the factored matrix + * + * A typical usage is to solve for the adjoint problem A' x = b: + * \code + * solver.compute(A); + * x = solver.adjoint().solve(b); + * \endcode + * + * For real scalar types, this function is equivalent to transpose(). + * + * \sa transpose(), solve() + */ + const SparseLUTransposeView > adjoint() + { + SparseLUTransposeView > adjointView; + adjointView.setSparseLU(this); + adjointView.setIsInitialized(this->m_isInitialized); + return adjointView; + } + + inline Index rows() const { return m_mat.rows(); } + inline Index cols() const { return m_mat.cols(); } + /** Indicate that the pattern of the input matrix is symmetric */ + void isSymmetric(bool sym) + { + m_symmetricmode = sym; + } + + /** \returns an expression of the matrix L, internally stored as supernodes + * The only operation available with this expression is the triangular solve + * \code + * y = b; matrixL().solveInPlace(y); + * \endcode + */ + SparseLUMatrixLReturnType matrixL() const + { + return SparseLUMatrixLReturnType(m_Lstore); + } + /** \returns an expression of the matrix U, + * The only operation available with this expression is the triangular solve + * \code + * y = b; matrixU().solveInPlace(y); + * \endcode + */ + SparseLUMatrixUReturnType > matrixU() const + { + return SparseLUMatrixUReturnType >(m_Lstore, m_Ustore); + } + + /** + * \returns a reference to the row matrix permutation \f$ P_r \f$ such that \f$P_r A P_c^T = L U\f$ + * \sa colsPermutation() + */ + inline const PermutationType& rowsPermutation() const + { + return m_perm_r; + } + /** + * \returns a reference to the column matrix permutation\f$ P_c^T \f$ such that \f$P_r A P_c^T = L U\f$ + * \sa rowsPermutation() + */ + inline const PermutationType& colsPermutation() const + { + return m_perm_c; + } + /** Set the threshold used for a diagonal entry to be an acceptable pivot. */ + void setPivotThreshold(const RealScalar& thresh) + { + m_diagpivotthresh = thresh; + } + +#ifdef EIGEN_PARSED_BY_DOXYGEN + /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A. + * + * \warning the destination matrix X in X = this->solve(B) must be colmun-major. + * + * \sa compute() + */ + template + inline const Solve solve(const MatrixBase& B) const; +#endif // EIGEN_PARSED_BY_DOXYGEN + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the LU factorization reports a problem, zero diagonal for instance + * \c InvalidInput if the input matrix is invalid + * + * \sa iparm() + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "Decomposition is not initialized."); + return m_info; + } + + /** + * \returns A string describing the type of error + */ + std::string lastErrorMessage() const + { + return m_lastError; + } + + template + bool _solve_impl(const MatrixBase &B, MatrixBase &X_base) const + { + Dest& X(X_base.derived()); + eigen_assert(m_factorizationIsOk && "The matrix should be factorized first"); + EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0, + THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); + + // Permute the right hand side to form X = Pr*B + // on return, X is overwritten by the computed solution + X.resize(B.rows(),B.cols()); + + // this ugly const_cast_derived() helps to detect aliasing when applying the permutations + for(Index j = 0; j < B.cols(); ++j) + X.col(j) = rowsPermutation() * B.const_cast_derived().col(j); + + //Forward substitution with L + this->matrixL().solveInPlace(X); + this->matrixU().solveInPlace(X); + + // Permute back the solution + for (Index j = 0; j < B.cols(); ++j) + X.col(j) = colsPermutation().inverse() * X.col(j); + + return true; + } + + /** + * \returns the absolute value of the determinant of the matrix of which + * *this is the QR decomposition. + * + * \warning a determinant can be very big or small, so for matrices + * of large enough dimension, there is a risk of overflow/underflow. + * One way to work around that is to use logAbsDeterminant() instead. + * + * \sa logAbsDeterminant(), signDeterminant() + */ + Scalar absDeterminant() + { + using std::abs; + eigen_assert(m_factorizationIsOk && "The matrix should be factorized first."); + // Initialize with the determinant of the row matrix + Scalar det = Scalar(1.); + // Note that the diagonal blocks of U are stored in supernodes, + // which are available in the L part :) + for (Index j = 0; j < this->cols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.index() == j) + { + det *= abs(it.value()); + break; + } + } + } + return det; + } + + /** \returns the natural log of the absolute value of the determinant of the matrix + * of which **this is the QR decomposition + * + * \note This method is useful to work around the risk of overflow/underflow that's + * inherent to the determinant computation. + * + * \sa absDeterminant(), signDeterminant() + */ + Scalar logAbsDeterminant() const + { + using std::log; + using std::abs; + + eigen_assert(m_factorizationIsOk && "The matrix should be factorized first."); + Scalar det = Scalar(0.); + for (Index j = 0; j < this->cols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.row() < j) continue; + if(it.row() == j) + { + det += log(abs(it.value())); + break; + } + } + } + return det; + } + + /** \returns A number representing the sign of the determinant + * + * \sa absDeterminant(), logAbsDeterminant() + */ + Scalar signDeterminant() + { + eigen_assert(m_factorizationIsOk && "The matrix should be factorized first."); + // Initialize with the determinant of the row matrix + Index det = 1; + // Note that the diagonal blocks of U are stored in supernodes, + // which are available in the L part :) + for (Index j = 0; j < this->cols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.index() == j) + { + if(it.value()<0) + det = -det; + else if(it.value()==0) + return 0; + break; + } + } + } + return det * m_detPermR * m_detPermC; + } + + /** \returns The determinant of the matrix. + * + * \sa absDeterminant(), logAbsDeterminant() + */ + Scalar determinant() + { + eigen_assert(m_factorizationIsOk && "The matrix should be factorized first."); + // Initialize with the determinant of the row matrix + Scalar det = Scalar(1.); + // Note that the diagonal blocks of U are stored in supernodes, + // which are available in the L part :) + for (Index j = 0; j < this->cols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.index() == j) + { + det *= it.value(); + break; + } + } + } + return (m_detPermR * m_detPermC) > 0 ? det : -det; + } + + Index nnzL() const { return m_nnzL; }; + Index nnzU() const { return m_nnzU; }; + + protected: + // Functions + void initperfvalues() + { + m_perfv.panel_size = 16; + m_perfv.relax = 1; + m_perfv.maxsuper = 128; + m_perfv.rowblk = 16; + m_perfv.colblk = 8; + m_perfv.fillfactor = 20; + } + + // Variables + mutable ComputationInfo m_info; + bool m_factorizationIsOk; + bool m_analysisIsOk; + std::string m_lastError; + NCMatrix m_mat; // The input (permuted ) matrix + SCMatrix m_Lstore; // The lower triangular matrix (supernodal) + MappedSparseMatrix m_Ustore; // The upper triangular matrix + PermutationType m_perm_c; // Column permutation + PermutationType m_perm_r ; // Row permutation + IndexVector m_etree; // Column elimination tree + + typename Base::GlobalLU_t m_glu; + + // SparseLU options + bool m_symmetricmode; + // values for performance + internal::perfvalues m_perfv; + RealScalar m_diagpivotthresh; // Specifies the threshold used for a diagonal entry to be an acceptable pivot + Index m_nnzL, m_nnzU; // Nonzeros in L and U factors + Index m_detPermR, m_detPermC; // Determinants of the permutation matrices + private: + // Disable copy constructor + SparseLU (const SparseLU& ); +}; // End class SparseLU + + + +// Functions needed by the anaysis phase +/** + * Compute the column permutation to minimize the fill-in + * + * - Apply this permutation to the input matrix - + * + * - Compute the column elimination tree on the permuted matrix + * + * - Postorder the elimination tree and the column permutation + * + */ +template +void SparseLU::analyzePattern(const MatrixType& mat) +{ + + //TODO It is possible as in SuperLU to compute row and columns scaling vectors to equilibrate the matrix mat. + + // Firstly, copy the whole input matrix. + m_mat = mat; + + // Compute fill-in ordering + OrderingType ord; + ord(m_mat,m_perm_c); + + // Apply the permutation to the column of the input matrix + if (m_perm_c.size()) + { + m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers. FIXME : This vector is filled but not subsequently used. + // Then, permute only the column pointers + ei_declare_aligned_stack_constructed_variable(StorageIndex,outerIndexPtr,mat.cols()+1,mat.isCompressed()?const_cast(mat.outerIndexPtr()):0); + + // If the input matrix 'mat' is uncompressed, then the outer-indices do not match the ones of m_mat, and a copy is thus needed. + if(!mat.isCompressed()) + IndexVector::Map(outerIndexPtr, mat.cols()+1) = IndexVector::Map(m_mat.outerIndexPtr(),mat.cols()+1); + + // Apply the permutation and compute the nnz per column. + for (Index i = 0; i < mat.cols(); i++) + { + m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i]; + m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i]; + } + } + + // Compute the column elimination tree of the permuted matrix + IndexVector firstRowElt; + internal::coletree(m_mat, m_etree,firstRowElt); + + // In symmetric mode, do not do postorder here + if (!m_symmetricmode) { + IndexVector post, iwork; + // Post order etree + internal::treePostorder(StorageIndex(m_mat.cols()), m_etree, post); + + + // Renumber etree in postorder + Index m = m_mat.cols(); + iwork.resize(m+1); + for (Index i = 0; i < m; ++i) iwork(post(i)) = post(m_etree(i)); + m_etree = iwork; + + // Postmultiply A*Pc by post, i.e reorder the matrix according to the postorder of the etree + PermutationType post_perm(m); + for (Index i = 0; i < m; i++) + post_perm.indices()(i) = post(i); + + // Combine the two permutations : postorder the permutation for future use + if(m_perm_c.size()) { + m_perm_c = post_perm * m_perm_c; + } + + } // end postordering + + m_analysisIsOk = true; +} + +// Functions needed by the numerical factorization phase + + +/** + * - Numerical factorization + * - Interleaved with the symbolic factorization + * On exit, info is + * + * = 0: successful factorization + * + * > 0: if info = i, and i is + * + * <= A->ncol: U(i,i) is exactly zero. The factorization has + * been completed, but the factor U is exactly singular, + * and division by zero will occur if it is used to solve a + * system of equations. + * + * > A->ncol: number of bytes allocated when memory allocation + * failure occurred, plus A->ncol. If lwork = -1, it is + * the estimated amount of space needed, plus A->ncol. + */ +template +void SparseLU::factorize(const MatrixType& matrix) +{ + using internal::emptyIdxLU; + eigen_assert(m_analysisIsOk && "analyzePattern() should be called first"); + eigen_assert((matrix.rows() == matrix.cols()) && "Only for squared matrices"); + + m_isInitialized = true; + + // Apply the column permutation computed in analyzepattern() + // m_mat = matrix * m_perm_c.inverse(); + m_mat = matrix; + if (m_perm_c.size()) + { + m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers. + //Then, permute only the column pointers + const StorageIndex * outerIndexPtr; + if (matrix.isCompressed()) outerIndexPtr = matrix.outerIndexPtr(); + else + { + StorageIndex* outerIndexPtr_t = new StorageIndex[matrix.cols()+1]; + for(Index i = 0; i <= matrix.cols(); i++) outerIndexPtr_t[i] = m_mat.outerIndexPtr()[i]; + outerIndexPtr = outerIndexPtr_t; + } + for (Index i = 0; i < matrix.cols(); i++) + { + m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i]; + m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i]; + } + if(!matrix.isCompressed()) delete[] outerIndexPtr; + } + else + { //FIXME This should not be needed if the empty permutation is handled transparently + m_perm_c.resize(matrix.cols()); + for(StorageIndex i = 0; i < matrix.cols(); ++i) m_perm_c.indices()(i) = i; + } + + Index m = m_mat.rows(); + Index n = m_mat.cols(); + Index nnz = m_mat.nonZeros(); + Index maxpanel = m_perfv.panel_size * m; + // Allocate working storage common to the factor routines + Index lwork = 0; + Index info = Base::memInit(m, n, nnz, lwork, m_perfv.fillfactor, m_perfv.panel_size, m_glu); + if (info) + { + m_lastError = "UNABLE TO ALLOCATE WORKING MEMORY\n\n" ; + m_factorizationIsOk = false; + return ; + } + + // Set up pointers for integer working arrays + IndexVector segrep(m); segrep.setZero(); + IndexVector parent(m); parent.setZero(); + IndexVector xplore(m); xplore.setZero(); + IndexVector repfnz(maxpanel); + IndexVector panel_lsub(maxpanel); + IndexVector xprune(n); xprune.setZero(); + IndexVector marker(m*internal::LUNoMarker); marker.setZero(); + + repfnz.setConstant(-1); + panel_lsub.setConstant(-1); + + // Set up pointers for scalar working arrays + ScalarVector dense; + dense.setZero(maxpanel); + ScalarVector tempv; + tempv.setZero(internal::LUnumTempV(m, m_perfv.panel_size, m_perfv.maxsuper, /*m_perfv.rowblk*/m) ); + + // Compute the inverse of perm_c + PermutationType iperm_c(m_perm_c.inverse()); + + // Identify initial relaxed snodes + IndexVector relax_end(n); + if ( m_symmetricmode == true ) + Base::heap_relax_snode(n, m_etree, m_perfv.relax, marker, relax_end); + else + Base::relax_snode(n, m_etree, m_perfv.relax, marker, relax_end); + + + m_perm_r.resize(m); + m_perm_r.indices().setConstant(-1); + marker.setConstant(-1); + m_detPermR = 1; // Record the determinant of the row permutation + + m_glu.supno(0) = emptyIdxLU; m_glu.xsup.setConstant(0); + m_glu.xsup(0) = m_glu.xlsub(0) = m_glu.xusub(0) = m_glu.xlusup(0) = Index(0); + + // Work on one 'panel' at a time. A panel is one of the following : + // (a) a relaxed supernode at the bottom of the etree, or + // (b) panel_size contiguous columns, defined by the user + Index jcol; + Index pivrow; // Pivotal row number in the original row matrix + Index nseg1; // Number of segments in U-column above panel row jcol + Index nseg; // Number of segments in each U-column + Index irep; + Index i, k, jj; + for (jcol = 0; jcol < n; ) + { + // Adjust panel size so that a panel won't overlap with the next relaxed snode. + Index panel_size = m_perfv.panel_size; // upper bound on panel width + for (k = jcol + 1; k < (std::min)(jcol+panel_size, n); k++) + { + if (relax_end(k) != emptyIdxLU) + { + panel_size = k - jcol; + break; + } + } + if (k == n) + panel_size = n - jcol; + + // Symbolic outer factorization on a panel of columns + Base::panel_dfs(m, panel_size, jcol, m_mat, m_perm_r.indices(), nseg1, dense, panel_lsub, segrep, repfnz, xprune, marker, parent, xplore, m_glu); + + // Numeric sup-panel updates in topological order + Base::panel_bmod(m, panel_size, jcol, nseg1, dense, tempv, segrep, repfnz, m_glu); + + // Sparse LU within the panel, and below the panel diagonal + for ( jj = jcol; jj< jcol + panel_size; jj++) + { + k = (jj - jcol) * m; // Column index for w-wide arrays + + nseg = nseg1; // begin after all the panel segments + //Depth-first-search for the current column + VectorBlock panel_lsubk(panel_lsub, k, m); + VectorBlock repfnz_k(repfnz, k, m); + info = Base::column_dfs(m, jj, m_perm_r.indices(), m_perfv.maxsuper, nseg, panel_lsubk, segrep, repfnz_k, xprune, marker, parent, xplore, m_glu); + if ( info ) + { + m_lastError = "UNABLE TO EXPAND MEMORY IN COLUMN_DFS() "; + m_info = NumericalIssue; + m_factorizationIsOk = false; + return; + } + // Numeric updates to this column + VectorBlock dense_k(dense, k, m); + VectorBlock segrep_k(segrep, nseg1, m-nseg1); + info = Base::column_bmod(jj, (nseg - nseg1), dense_k, tempv, segrep_k, repfnz_k, jcol, m_glu); + if ( info ) + { + m_lastError = "UNABLE TO EXPAND MEMORY IN COLUMN_BMOD() "; + m_info = NumericalIssue; + m_factorizationIsOk = false; + return; + } + + // Copy the U-segments to ucol(*) + info = Base::copy_to_ucol(jj, nseg, segrep, repfnz_k ,m_perm_r.indices(), dense_k, m_glu); + if ( info ) + { + m_lastError = "UNABLE TO EXPAND MEMORY IN COPY_TO_UCOL() "; + m_info = NumericalIssue; + m_factorizationIsOk = false; + return; + } + + // Form the L-segment + info = Base::pivotL(jj, m_diagpivotthresh, m_perm_r.indices(), iperm_c.indices(), pivrow, m_glu); + if ( info ) + { + m_lastError = "THE MATRIX IS STRUCTURALLY SINGULAR ... ZERO COLUMN AT "; + std::ostringstream returnInfo; + returnInfo << info; + m_lastError += returnInfo.str(); + m_info = NumericalIssue; + m_factorizationIsOk = false; + return; + } + + // Update the determinant of the row permutation matrix + // FIXME: the following test is not correct, we should probably take iperm_c into account and pivrow is not directly the row pivot. + if (pivrow != jj) m_detPermR = -m_detPermR; + + // Prune columns (0:jj-1) using column jj + Base::pruneL(jj, m_perm_r.indices(), pivrow, nseg, segrep, repfnz_k, xprune, m_glu); + + // Reset repfnz for this column + for (i = 0; i < nseg; i++) + { + irep = segrep(i); + repfnz_k(irep) = emptyIdxLU; + } + } // end SparseLU within the panel + jcol += panel_size; // Move to the next panel + } // end for -- end elimination + + m_detPermR = m_perm_r.determinant(); + m_detPermC = m_perm_c.determinant(); + + // Count the number of nonzeros in factors + Base::countnz(n, m_nnzL, m_nnzU, m_glu); + // Apply permutation to the L subscripts + Base::fixupL(n, m_perm_r.indices(), m_glu); + + // Create supernode matrix L + m_Lstore.setInfos(m, n, m_glu.lusup, m_glu.xlusup, m_glu.lsub, m_glu.xlsub, m_glu.supno, m_glu.xsup); + // Create the column major upper sparse matrix U; + new (&m_Ustore) MappedSparseMatrix ( m, n, m_nnzU, m_glu.xusub.data(), m_glu.usub.data(), m_glu.ucol.data() ); + + m_info = Success; + m_factorizationIsOk = true; +} + +template +struct SparseLUMatrixLReturnType : internal::no_assignment_operator +{ + typedef typename MappedSupernodalType::Scalar Scalar; + explicit SparseLUMatrixLReturnType(const MappedSupernodalType& mapL) : m_mapL(mapL) + { } + Index rows() const { return m_mapL.rows(); } + Index cols() const { return m_mapL.cols(); } + template + void solveInPlace( MatrixBase &X) const + { + m_mapL.solveInPlace(X); + } + template + void solveTransposedInPlace( MatrixBase &X) const + { + m_mapL.template solveTransposedInPlace(X); + } + + const MappedSupernodalType& m_mapL; +}; + +template +struct SparseLUMatrixUReturnType : internal::no_assignment_operator +{ + typedef typename MatrixLType::Scalar Scalar; + SparseLUMatrixUReturnType(const MatrixLType& mapL, const MatrixUType& mapU) + : m_mapL(mapL),m_mapU(mapU) + { } + Index rows() const { return m_mapL.rows(); } + Index cols() const { return m_mapL.cols(); } + + template void solveInPlace(MatrixBase &X) const + { + Index nrhs = X.cols(); + Index n = X.rows(); + // Backward solve with U + for (Index k = m_mapL.nsuper(); k >= 0; k--) + { + Index fsupc = m_mapL.supToCol()[k]; + Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension + Index nsupc = m_mapL.supToCol()[k+1] - fsupc; + Index luptr = m_mapL.colIndexPtr()[fsupc]; + + if (nsupc == 1) + { + for (Index j = 0; j < nrhs; j++) + { + X(fsupc, j) /= m_mapL.valuePtr()[luptr]; + } + } + else + { + // FIXME: the following lines should use Block expressions and not Map! + Map, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X.coeffRef(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + U = A.template triangularView().solve(U); + } + + for (Index j = 0; j < nrhs; ++j) + { + for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++) + { + typename MatrixUType::InnerIterator it(m_mapU, jcol); + for ( ; it; ++it) + { + Index irow = it.index(); + X(irow, j) -= X(jcol, j) * it.value(); + } + } + } + } // End For U-solve + } + + template void solveTransposedInPlace(MatrixBase &X) const + { + using numext::conj; + Index nrhs = X.cols(); + Index n = X.rows(); + // Forward solve with U + for (Index k = 0; k <= m_mapL.nsuper(); k++) + { + Index fsupc = m_mapL.supToCol()[k]; + Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension + Index nsupc = m_mapL.supToCol()[k+1] - fsupc; + Index luptr = m_mapL.colIndexPtr()[fsupc]; + + for (Index j = 0; j < nrhs; ++j) + { + for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++) + { + typename MatrixUType::InnerIterator it(m_mapU, jcol); + for ( ; it; ++it) + { + Index irow = it.index(); + X(jcol, j) -= X(irow, j) * (Conjugate? conj(it.value()): it.value()); + } + } + } + if (nsupc == 1) + { + for (Index j = 0; j < nrhs; j++) + { + X(fsupc, j) /= (Conjugate? conj(m_mapL.valuePtr()[luptr]) : m_mapL.valuePtr()[luptr]); + } + } + else + { + Map, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + if(Conjugate) + U = A.adjoint().template triangularView().solve(U); + else + U = A.transpose().template triangularView().solve(U); + } + }// End For U-solve + } + + + const MatrixLType& m_mapL; + const MatrixUType& m_mapU; +}; + +} // End namespace Eigen + +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h new file mode 100644 index 0000000000..fc0cfc4de1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h @@ -0,0 +1,66 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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 SPARSELU_IMPL_H +#define SPARSELU_IMPL_H + +namespace Eigen { +namespace internal { + +/** \ingroup SparseLU_Module + * \class SparseLUImpl + * Base class for sparseLU + */ +template +class SparseLUImpl +{ + public: + typedef Matrix ScalarVector; + typedef Matrix IndexVector; + typedef Matrix ScalarMatrix; + typedef Map > MappedMatrixBlock; + typedef typename ScalarVector::RealScalar RealScalar; + typedef Ref > BlockScalarVector; + typedef Ref > BlockIndexVector; + typedef LU_GlobalLU_t GlobalLU_t; + typedef SparseMatrix MatrixType; + + protected: + template + Index expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions); + Index memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu); + template + Index memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions); + void heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end); + void relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end); + Index snode_dfs(const Index jcol, const Index kcol,const MatrixType& mat, IndexVector& xprune, IndexVector& marker, GlobalLU_t& glu); + Index snode_bmod (const Index jcol, const Index fsupc, ScalarVector& dense, GlobalLU_t& glu); + Index pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu); + template + void dfs_kernel(const StorageIndex jj, IndexVector& perm_r, + Index& nseg, IndexVector& panel_lsub, IndexVector& segrep, + Ref repfnz_col, IndexVector& xprune, Ref marker, IndexVector& parent, + IndexVector& xplore, GlobalLU_t& glu, Index& nextl_col, Index krow, Traits& traits); + void panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu); + + void panel_bmod(const Index m, const Index w, const Index jcol, const Index nseg, ScalarVector& dense, ScalarVector& tempv, IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu); + Index column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg, BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu); + Index column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv, BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu); + Index copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu); + void pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg, const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu); + void countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu); + void fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu); + + template + friend struct column_dfs_traits; +}; + +} // end namespace internal +} // namespace Eigen + +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h new file mode 100644 index 0000000000..349bfd585b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h @@ -0,0 +1,226 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* + + * NOTE: This file is the modified version of [s,d,c,z]memory.c files in SuperLU + + * -- SuperLU routine (version 3.1) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * August 1, 2008 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ + +#ifndef EIGEN_SPARSELU_MEMORY +#define EIGEN_SPARSELU_MEMORY + +namespace Eigen { +namespace internal { + +enum { LUNoMarker = 3 }; +enum {emptyIdxLU = -1}; +inline Index LUnumTempV(Index& m, Index& w, Index& t, Index& b) +{ + return (std::max)(m, (t+b)*w); +} + +template< typename Scalar> +inline Index LUTempSpace(Index&m, Index& w) +{ + return (2*w + 4 + LUNoMarker) * m * sizeof(Index) + (w + 1) * m * sizeof(Scalar); +} + + + + +/** + * Expand the existing storage to accommodate more fill-ins + * \param vec Valid pointer to the vector to allocate or expand + * \param[in,out] length At input, contain the current length of the vector that is to be increased. At output, length of the newly allocated vector + * \param[in] nbElts Current number of elements in the factors + * \param keep_prev 1: use length and do not expand the vector; 0: compute new_len and expand + * \param[in,out] num_expansions Number of times the memory has been expanded + */ +template +template +Index SparseLUImpl::expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions) +{ + + float alpha = 1.5; // Ratio of the memory increase + Index new_len; // New size of the allocated memory + + if(num_expansions == 0 || keep_prev) + new_len = length ; // First time allocate requested + else + new_len = (std::max)(length+1,Index(alpha * length)); + + VectorType old_vec; // Temporary vector to hold the previous values + if (nbElts > 0 ) + old_vec = vec.segment(0,nbElts); + + //Allocate or expand the current vector +#ifdef EIGEN_EXCEPTIONS + try +#endif + { + vec.resize(new_len); + } +#ifdef EIGEN_EXCEPTIONS + catch(std::bad_alloc& ) +#else + if(!vec.size()) +#endif + { + if (!num_expansions) + { + // First time to allocate from LUMemInit() + // Let LUMemInit() deals with it. + return -1; + } + if (keep_prev) + { + // In this case, the memory length should not not be reduced + return new_len; + } + else + { + // Reduce the size and increase again + Index tries = 0; // Number of attempts + do + { + alpha = (alpha + 1)/2; + new_len = (std::max)(length+1,Index(alpha * length)); +#ifdef EIGEN_EXCEPTIONS + try +#endif + { + vec.resize(new_len); + } +#ifdef EIGEN_EXCEPTIONS + catch(std::bad_alloc& ) +#else + if (!vec.size()) +#endif + { + tries += 1; + if ( tries > 10) return new_len; + } + } while (!vec.size()); + } + } + //Copy the previous values to the newly allocated space + if (nbElts > 0) + vec.segment(0, nbElts) = old_vec; + + + length = new_len; + if(num_expansions) ++num_expansions; + return 0; +} + +/** + * \brief Allocate various working space for the numerical factorization phase. + * \param m number of rows of the input matrix + * \param n number of columns + * \param annz number of initial nonzeros in the matrix + * \param lwork if lwork=-1, this routine returns an estimated size of the required memory + * \param glu persistent data to facilitate multiple factors : will be deleted later ?? + * \param fillratio estimated ratio of fill in the factors + * \param panel_size Size of a panel + * \return an estimated size of the required memory if lwork = -1; otherwise, return the size of actually allocated memory when allocation failed, and 0 on success + * \note Unlike SuperLU, this routine does not support successive factorization with the same pattern and the same row permutation + */ +template +Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu) +{ + Index& num_expansions = glu.num_expansions; //No memory expansions so far + num_expansions = 0; + glu.nzumax = glu.nzlumax = (std::min)(fillratio * (annz+1) / n, m) * n; // estimated number of nonzeros in U + glu.nzlmax = (std::max)(Index(4), fillratio) * (annz+1) / 4; // estimated nnz in L factor + // Return the estimated size to the user if necessary + Index tempSpace; + tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar); + if (lwork == emptyIdxLU) + { + Index estimated_size; + estimated_size = (5 * n + 5) * sizeof(Index) + tempSpace + + (glu.nzlmax + glu.nzumax) * sizeof(Index) + (glu.nzlumax+glu.nzumax) * sizeof(Scalar) + n; + return estimated_size; + } + + // Setup the required space + + // First allocate Integer pointers for L\U factors + glu.xsup.resize(n+1); + glu.supno.resize(n+1); + glu.xlsub.resize(n+1); + glu.xlusup.resize(n+1); + glu.xusub.resize(n+1); + + // Reserve memory for L/U factors + do + { + if( (expand(glu.lusup, glu.nzlumax, 0, 0, num_expansions)<0) + || (expand(glu.ucol, glu.nzumax, 0, 0, num_expansions)<0) + || (expand (glu.lsub, glu.nzlmax, 0, 0, num_expansions)<0) + || (expand (glu.usub, glu.nzumax, 0, 1, num_expansions)<0) ) + { + //Reduce the estimated size and retry + glu.nzlumax /= 2; + glu.nzumax /= 2; + glu.nzlmax /= 2; + if (glu.nzlumax < annz ) return glu.nzlumax; + } + } while (!glu.lusup.size() || !glu.ucol.size() || !glu.lsub.size() || !glu.usub.size()); + + ++num_expansions; + return 0; + +} // end LuMemInit + +/** + * \brief Expand the existing storage + * \param vec vector to expand + * \param[in,out] maxlen On input, previous size of vec (Number of elements to copy ). on output, new size + * \param nbElts current number of elements in the vector. + * \param memtype Type of the element to expand + * \param num_expansions Number of expansions + * \return 0 on success, > 0 size of the memory allocated so far + */ +template +template +Index SparseLUImpl::memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions) +{ + Index failed_size; + if (memtype == USUB) + failed_size = this->expand(vec, maxlen, nbElts, 1, num_expansions); + else + failed_size = this->expand(vec, maxlen, nbElts, 0, num_expansions); + + if (failed_size) + return failed_size; + + return 0 ; +} + +} // end namespace internal + +} // end namespace Eigen +#endif // EIGEN_SPARSELU_MEMORY diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h new file mode 100644 index 0000000000..cf5ec449be --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h @@ -0,0 +1,110 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* + * NOTE: This file comes from a partly modified version of files slu_[s,d,c,z]defs.h + * -- SuperLU routine (version 4.1) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * November, 2010 + * + * Global data structures used in LU factorization - + * + * nsuper: #supernodes = nsuper + 1, numbered [0, nsuper]. + * (xsup,supno): supno[i] is the supernode no to which i belongs; + * xsup(s) points to the beginning of the s-th supernode. + * e.g. supno 0 1 2 2 3 3 3 4 4 4 4 4 (n=12) + * xsup 0 1 2 4 7 12 + * Note: dfs will be performed on supernode rep. relative to the new + * row pivoting ordering + * + * (xlsub,lsub): lsub[*] contains the compressed subscript of + * rectangular supernodes; xlsub[j] points to the starting + * location of the j-th column in lsub[*]. Note that xlsub + * is indexed by column. + * Storage: original row subscripts + * + * During the course of sparse LU factorization, we also use + * (xlsub,lsub) for the purpose of symmetric pruning. For each + * supernode {s,s+1,...,t=s+r} with first column s and last + * column t, the subscript set + * lsub[j], j=xlsub[s], .., xlsub[s+1]-1 + * is the structure of column s (i.e. structure of this supernode). + * It is used for the storage of numerical values. + * Furthermore, + * lsub[j], j=xlsub[t], .., xlsub[t+1]-1 + * is the structure of the last column t of this supernode. + * It is for the purpose of symmetric pruning. Therefore, the + * structural subscripts can be rearranged without making physical + * interchanges among the numerical values. + * + * However, if the supernode has only one column, then we + * only keep one set of subscripts. For any subscript interchange + * performed, similar interchange must be done on the numerical + * values. + * + * The last column structures (for pruning) will be removed + * after the numercial LU factorization phase. + * + * (xlusup,lusup): lusup[*] contains the numerical values of the + * rectangular supernodes; xlusup[j] points to the starting + * location of the j-th column in storage vector lusup[*] + * Note: xlusup is indexed by column. + * Each rectangular supernode is stored by column-major + * scheme, consistent with Fortran 2-dim array storage. + * + * (xusub,ucol,usub): ucol[*] stores the numerical values of + * U-columns outside the rectangular supernodes. The row + * subscript of nonzero ucol[k] is stored in usub[k]. + * xusub[i] points to the starting location of column i in ucol. + * Storage: new row subscripts; that is subscripts of PA. + */ + +#ifndef EIGEN_LU_STRUCTS +#define EIGEN_LU_STRUCTS +namespace Eigen { +namespace internal { + +typedef enum {LUSUP, UCOL, LSUB, USUB, LLVL, ULVL} MemType; + +template +struct LU_GlobalLU_t { + typedef typename IndexVector::Scalar StorageIndex; + IndexVector xsup; //First supernode column ... xsup(s) points to the beginning of the s-th supernode + IndexVector supno; // Supernode number corresponding to this column (column to supernode mapping) + ScalarVector lusup; // nonzero values of L ordered by columns + IndexVector lsub; // Compressed row indices of L rectangular supernodes. + IndexVector xlusup; // pointers to the beginning of each column in lusup + IndexVector xlsub; // pointers to the beginning of each column in lsub + Index nzlmax; // Current max size of lsub + Index nzlumax; // Current max size of lusup + ScalarVector ucol; // nonzero values of U ordered by columns + IndexVector usub; // row indices of U columns in ucol + IndexVector xusub; // Pointers to the beginning of each column of U in ucol + Index nzumax; // Current max size of ucol + Index n; // Number of columns in the matrix + Index num_expansions; +}; + +// Values to set for performance +struct perfvalues { + Index panel_size; // a panel consists of at most consecutive columns + Index relax; // To control degree of relaxing supernodes. If the number of nodes (columns) + // in a subtree of the elimination tree is less than relax, this subtree is considered + // as one supernode regardless of the row structures of those columns + Index maxsuper; // The maximum size for a supernode in complete LU + Index rowblk; // The minimum row dimension for 2-D blocking to be used; + Index colblk; // The minimum column dimension for 2-D blocking to be used; + Index fillfactor; // The estimated fills factors for L and U, compared with A +}; + +} // end namespace internal + +} // end namespace Eigen +#endif // EIGEN_LU_STRUCTS diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h new file mode 100644 index 0000000000..0be293d17f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h @@ -0,0 +1,375 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2012 Gael Guennebaud +// +// 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_SPARSELU_SUPERNODAL_MATRIX_H +#define EIGEN_SPARSELU_SUPERNODAL_MATRIX_H + +namespace Eigen { +namespace internal { + +/** \ingroup SparseLU_Module + * \brief a class to manipulate the L supernodal factor from the SparseLU factorization + * + * This class contain the data to easily store + * and manipulate the supernodes during the factorization and solution phase of Sparse LU. + * Only the lower triangular matrix has supernodes. + * + * NOTE : This class corresponds to the SCformat structure in SuperLU + * + */ +/* TODO + * InnerIterator as for sparsematrix + * SuperInnerIterator to iterate through all supernodes + * Function for triangular solve + */ +template +class MappedSuperNodalMatrix +{ + public: + typedef _Scalar Scalar; + typedef _StorageIndex StorageIndex; + typedef Matrix IndexVector; + typedef Matrix ScalarVector; + public: + MappedSuperNodalMatrix() + { + + } + MappedSuperNodalMatrix(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind, + IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col ) + { + setInfos(m, n, nzval, nzval_colptr, rowind, rowind_colptr, col_to_sup, sup_to_col); + } + + ~MappedSuperNodalMatrix() + { + + } + /** + * Set appropriate pointers for the lower triangular supernodal matrix + * These infos are available at the end of the numerical factorization + * FIXME This class will be modified such that it can be use in the course + * of the factorization. + */ + void setInfos(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind, + IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col ) + { + m_row = m; + m_col = n; + m_nzval = nzval.data(); + m_nzval_colptr = nzval_colptr.data(); + m_rowind = rowind.data(); + m_rowind_colptr = rowind_colptr.data(); + m_nsuper = col_to_sup(n); + m_col_to_sup = col_to_sup.data(); + m_sup_to_col = sup_to_col.data(); + } + + /** + * Number of rows + */ + Index rows() const { return m_row; } + + /** + * Number of columns + */ + Index cols() const { return m_col; } + + /** + * Return the array of nonzero values packed by column + * + * The size is nnz + */ + Scalar* valuePtr() { return m_nzval; } + + const Scalar* valuePtr() const + { + return m_nzval; + } + /** + * Return the pointers to the beginning of each column in \ref valuePtr() + */ + StorageIndex* colIndexPtr() + { + return m_nzval_colptr; + } + + const StorageIndex* colIndexPtr() const + { + return m_nzval_colptr; + } + + /** + * Return the array of compressed row indices of all supernodes + */ + StorageIndex* rowIndex() { return m_rowind; } + + const StorageIndex* rowIndex() const + { + return m_rowind; + } + + /** + * Return the location in \em rowvaluePtr() which starts each column + */ + StorageIndex* rowIndexPtr() { return m_rowind_colptr; } + + const StorageIndex* rowIndexPtr() const + { + return m_rowind_colptr; + } + + /** + * Return the array of column-to-supernode mapping + */ + StorageIndex* colToSup() { return m_col_to_sup; } + + const StorageIndex* colToSup() const + { + return m_col_to_sup; + } + /** + * Return the array of supernode-to-column mapping + */ + StorageIndex* supToCol() { return m_sup_to_col; } + + const StorageIndex* supToCol() const + { + return m_sup_to_col; + } + + /** + * Return the number of supernodes + */ + Index nsuper() const + { + return m_nsuper; + } + + class InnerIterator; + template + void solveInPlace( MatrixBase&X) const; + template + void solveTransposedInPlace( MatrixBase&X) const; + + + + + + protected: + Index m_row; // Number of rows + Index m_col; // Number of columns + Index m_nsuper; // Number of supernodes + Scalar* m_nzval; //array of nonzero values packed by column + StorageIndex* m_nzval_colptr; //nzval_colptr[j] Stores the location in nzval[] which starts column j + StorageIndex* m_rowind; // Array of compressed row indices of rectangular supernodes + StorageIndex* m_rowind_colptr; //rowind_colptr[j] stores the location in rowind[] which starts column j + StorageIndex* m_col_to_sup; // col_to_sup[j] is the supernode number to which column j belongs + StorageIndex* m_sup_to_col; //sup_to_col[s] points to the starting column of the s-th supernode + + private : +}; + +/** + * \brief InnerIterator class to iterate over nonzero values of the current column in the supernodal matrix L + * + */ +template +class MappedSuperNodalMatrix::InnerIterator +{ + public: + InnerIterator(const MappedSuperNodalMatrix& mat, Index outer) + : m_matrix(mat), + m_outer(outer), + m_supno(mat.colToSup()[outer]), + m_idval(mat.colIndexPtr()[outer]), + m_startidval(m_idval), + m_endidval(mat.colIndexPtr()[outer+1]), + m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]), + m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1]) + {} + inline InnerIterator& operator++() + { + m_idval++; + m_idrow++; + return *this; + } + inline Scalar value() const { return m_matrix.valuePtr()[m_idval]; } + + inline Scalar& valueRef() { return const_cast(m_matrix.valuePtr()[m_idval]); } + + inline Index index() const { return m_matrix.rowIndex()[m_idrow]; } + inline Index row() const { return index(); } + inline Index col() const { return m_outer; } + + inline Index supIndex() const { return m_supno; } + + inline operator bool() const + { + return ( (m_idval < m_endidval) && (m_idval >= m_startidval) + && (m_idrow < m_endidrow) ); + } + + protected: + const MappedSuperNodalMatrix& m_matrix; // Supernodal lower triangular matrix + const Index m_outer; // Current column + const Index m_supno; // Current SuperNode number + Index m_idval; // Index to browse the values in the current column + const Index m_startidval; // Start of the column value + const Index m_endidval; // End of the column value + Index m_idrow; // Index to browse the row indices + Index m_endidrow; // End index of row indices of the current column +}; + +/** + * \brief Solve with the supernode triangular matrix + * + */ +template +template +void MappedSuperNodalMatrix::solveInPlace( MatrixBase&X) const +{ + /* Explicit type conversion as the Index type of MatrixBase may be wider than Index */ +// eigen_assert(X.rows() <= NumTraits::highest()); +// eigen_assert(X.cols() <= NumTraits::highest()); + Index n = int(X.rows()); + Index nrhs = Index(X.cols()); + const Scalar * Lval = valuePtr(); // Nonzero values + Matrix work(n, nrhs); // working vector + work.setZero(); + for (Index k = 0; k <= nsuper(); k ++) + { + Index fsupc = supToCol()[k]; // First column of the current supernode + Index istart = rowIndexPtr()[fsupc]; // Pointer index to the subscript of the current column + Index nsupr = rowIndexPtr()[fsupc+1] - istart; // Number of rows in the current supernode + Index nsupc = supToCol()[k+1] - fsupc; // Number of columns in the current supernode + Index nrow = nsupr - nsupc; // Number of rows in the non-diagonal part of the supernode + Index irow; //Current index row + + if (nsupc == 1 ) + { + for (Index j = 0; j < nrhs; j++) + { + InnerIterator it(*this, fsupc); + ++it; // Skip the diagonal element + for (; it; ++it) + { + irow = it.row(); + X(irow, j) -= X(fsupc, j) * it.value(); + } + } + } + else + { + // The supernode has more than one column + Index luptr = colIndexPtr()[fsupc]; + Index lda = colIndexPtr()[fsupc+1] - luptr; + + // Triangular solve + Map, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + U = A.template triangularView().solve(U); + + // Matrix-vector product + new (&A) Map, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); + work.topRows(nrow).noalias() = A * U; + + //Begin Scatter + for (Index j = 0; j < nrhs; j++) + { + Index iptr = istart + nsupc; + for (Index i = 0; i < nrow; i++) + { + irow = rowIndex()[iptr]; + X(irow, j) -= work(i, j); // Scatter operation + work(i, j) = Scalar(0); + iptr++; + } + } + } + } +} + +template +template +void MappedSuperNodalMatrix::solveTransposedInPlace( MatrixBase&X) const +{ + using numext::conj; + Index n = int(X.rows()); + Index nrhs = Index(X.cols()); + const Scalar * Lval = valuePtr(); // Nonzero values + Matrix work(n, nrhs); // working vector + work.setZero(); + for (Index k = nsuper(); k >= 0; k--) + { + Index fsupc = supToCol()[k]; // First column of the current supernode + Index istart = rowIndexPtr()[fsupc]; // Pointer index to the subscript of the current column + Index nsupr = rowIndexPtr()[fsupc+1] - istart; // Number of rows in the current supernode + Index nsupc = supToCol()[k+1] - fsupc; // Number of columns in the current supernode + Index nrow = nsupr - nsupc; // Number of rows in the non-diagonal part of the supernode + Index irow; //Current index row + + if (nsupc == 1 ) + { + for (Index j = 0; j < nrhs; j++) + { + InnerIterator it(*this, fsupc); + ++it; // Skip the diagonal element + for (; it; ++it) + { + irow = it.row(); + X(fsupc,j) -= X(irow, j) * (Conjugate?conj(it.value()):it.value()); + } + } + } + else + { + // The supernode has more than one column + Index luptr = colIndexPtr()[fsupc]; + Index lda = colIndexPtr()[fsupc+1] - luptr; + + //Begin Gather + for (Index j = 0; j < nrhs; j++) + { + Index iptr = istart + nsupc; + for (Index i = 0; i < nrow; i++) + { + irow = rowIndex()[iptr]; + work.topRows(nrow)(i,j)= X(irow,j); // Gather operation + iptr++; + } + } + + // Matrix-vector product with transposed submatrix + Map, 0, OuterStride<> > A( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + if(Conjugate) + U = U - A.adjoint() * work.topRows(nrow); + else + U = U - A.transpose() * work.topRows(nrow); + + // Triangular solve (of transposed diagonal block) + new (&A) Map, 0, OuterStride<> > ( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + if(Conjugate) + U = A.adjoint().template triangularView().solve(U); + else + U = A.transpose().template triangularView().solve(U); + + } + + } +} + + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SPARSELU_MATRIX_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h new file mode 100644 index 0000000000..9e3dab44d9 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h @@ -0,0 +1,80 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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_SPARSELU_UTILS_H +#define EIGEN_SPARSELU_UTILS_H + +namespace Eigen { +namespace internal { + +/** + * \brief Count Nonzero elements in the factors + */ +template +void SparseLUImpl::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu) +{ + nnzL = 0; + nnzU = (glu.xusub)(n); + Index nsuper = (glu.supno)(n); + Index jlen; + Index i, j, fsupc; + if (n <= 0 ) return; + // For each supernode + for (i = 0; i <= nsuper; i++) + { + fsupc = glu.xsup(i); + jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); + + for (j = fsupc; j < glu.xsup(i+1); j++) + { + nnzL += jlen; + nnzU += j - fsupc + 1; + jlen--; + } + } +} + +/** + * \brief Fix up the data storage lsub for L-subscripts. + * + * It removes the subscripts sets for structural pruning, + * and applies permutation to the remaining subscripts + * + */ +template +void SparseLUImpl::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu) +{ + Index fsupc, i, j, k, jstart; + + StorageIndex nextl = 0; + Index nsuper = (glu.supno)(n); + + // For each supernode + for (i = 0; i <= nsuper; i++) + { + fsupc = glu.xsup(i); + jstart = glu.xlsub(fsupc); + glu.xlsub(fsupc) = nextl; + for (j = jstart; j < glu.xlsub(fsupc + 1); j++) + { + glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A + nextl++; + } + for (k = fsupc+1; k < glu.xsup(i+1); k++) + glu.xlsub(k) = nextl; // other columns in supernode i + } + + glu.xlsub(n) = nextl; +} + +} // end namespace internal + +} // end namespace Eigen +#endif // EIGEN_SPARSELU_UTILS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h new file mode 100644 index 0000000000..b57f06802e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h @@ -0,0 +1,181 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2012 Gael Guennebaud +// +// 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/. + +/* + + * NOTE: This file is the modified version of xcolumn_bmod.c file in SuperLU + + * -- SuperLU routine (version 3.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * October 15, 2003 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSELU_COLUMN_BMOD_H +#define SPARSELU_COLUMN_BMOD_H + +namespace Eigen { + +namespace internal { +/** + * \brief Performs numeric block updates (sup-col) in topological order + * + * \param jcol current column to update + * \param nseg Number of segments in the U part + * \param dense Store the full representation of the column + * \param tempv working array + * \param segrep segment representative ... + * \param repfnz ??? First nonzero column in each row ??? ... + * \param fpanelc First column in the current panel + * \param glu Global LU data. + * \return 0 - successful return + * > 0 - number of bytes allocated when run out of space + * + */ +template +Index SparseLUImpl::column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv, + BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu) +{ + Index jsupno, k, ksub, krep, ksupno; + Index lptr, nrow, isub, irow, nextlu, new_next, ufirst; + Index fsupc, nsupc, nsupr, luptr, kfnz, no_zeros; + /* krep = representative of current k-th supernode + * fsupc = first supernodal column + * nsupc = number of columns in a supernode + * nsupr = number of rows in a supernode + * luptr = location of supernodal LU-block in storage + * kfnz = first nonz in the k-th supernodal segment + * no_zeros = no lf leading zeros in a supernodal U-segment + */ + + jsupno = glu.supno(jcol); + // For each nonzero supernode segment of U[*,j] in topological order + k = nseg - 1; + Index d_fsupc; // distance between the first column of the current panel and the + // first column of the current snode + Index fst_col; // First column within small LU update + Index segsize; + for (ksub = 0; ksub < nseg; ksub++) + { + krep = segrep(k); k--; + ksupno = glu.supno(krep); + if (jsupno != ksupno ) + { + // outside the rectangular supernode + fsupc = glu.xsup(ksupno); + fst_col = (std::max)(fsupc, fpanelc); + + // Distance from the current supernode to the current panel; + // d_fsupc = 0 if fsupc > fpanelc + d_fsupc = fst_col - fsupc; + + luptr = glu.xlusup(fst_col) + d_fsupc; + lptr = glu.xlsub(fsupc) + d_fsupc; + + kfnz = repfnz(krep); + kfnz = (std::max)(kfnz, fpanelc); + + segsize = krep - kfnz + 1; + nsupc = krep - fst_col + 1; + nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); + nrow = nsupr - d_fsupc - nsupc; + Index lda = glu.xlusup(fst_col+1) - glu.xlusup(fst_col); + + + // Perform a triangular solver and block update, + // then scatter the result of sup-col update to dense + no_zeros = kfnz - fst_col; + if(segsize==1) + LU_kernel_bmod<1>::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); + else + LU_kernel_bmod::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); + } // end if jsupno + } // end for each segment + + // Process the supernodal portion of L\U[*,j] + nextlu = glu.xlusup(jcol); + fsupc = glu.xsup(jsupno); + + // copy the SPA dense into L\U[*,j] + Index mem; + new_next = nextlu + glu.xlsub(fsupc + 1) - glu.xlsub(fsupc); + Index offset = internal::first_multiple(new_next, internal::packet_traits::size) - new_next; + if(offset) + new_next += offset; + while (new_next > glu.nzlumax ) + { + mem = memXpand(glu.lusup, glu.nzlumax, nextlu, LUSUP, glu.num_expansions); + if (mem) return mem; + } + + for (isub = glu.xlsub(fsupc); isub < glu.xlsub(fsupc+1); isub++) + { + irow = glu.lsub(isub); + glu.lusup(nextlu) = dense(irow); + dense(irow) = Scalar(0.0); + ++nextlu; + } + + if(offset) + { + glu.lusup.segment(nextlu,offset).setZero(); + nextlu += offset; + } + glu.xlusup(jcol + 1) = StorageIndex(nextlu); // close L\U(*,jcol); + + /* For more updates within the panel (also within the current supernode), + * should start from the first column of the panel, or the first column + * of the supernode, whichever is bigger. There are two cases: + * 1) fsupc < fpanelc, then fst_col <-- fpanelc + * 2) fsupc >= fpanelc, then fst_col <-- fsupc + */ + fst_col = (std::max)(fsupc, fpanelc); + + if (fst_col < jcol) + { + // Distance between the current supernode and the current panel + // d_fsupc = 0 if fsupc >= fpanelc + d_fsupc = fst_col - fsupc; + + lptr = glu.xlsub(fsupc) + d_fsupc; + luptr = glu.xlusup(fst_col) + d_fsupc; + nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); // leading dimension + nsupc = jcol - fst_col; // excluding jcol + nrow = nsupr - d_fsupc - nsupc; + + // points to the beginning of jcol in snode L\U(jsupno) + ufirst = glu.xlusup(jcol) + d_fsupc; + Index lda = glu.xlusup(jcol+1) - glu.xlusup(jcol); + MappedMatrixBlock A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + VectorBlock u(glu.lusup, ufirst, nsupc); + u = A.template triangularView().solve(u); + + new (&A) MappedMatrixBlock ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); + VectorBlock l(glu.lusup, ufirst+nsupc, nrow); + l.noalias() -= A * u; + + } // End if fst_col + return 0; +} + +} // end namespace internal +} // end namespace Eigen + +#endif // SPARSELU_COLUMN_BMOD_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h new file mode 100644 index 0000000000..5a2c941b4a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h @@ -0,0 +1,179 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* + + * NOTE: This file is the modified version of [s,d,c,z]column_dfs.c file in SuperLU + + * -- SuperLU routine (version 2.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * November 15, 1997 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSELU_COLUMN_DFS_H +#define SPARSELU_COLUMN_DFS_H + +template class SparseLUImpl; +namespace Eigen { + +namespace internal { + +template +struct column_dfs_traits : no_assignment_operator +{ + typedef typename ScalarVector::Scalar Scalar; + typedef typename IndexVector::Scalar StorageIndex; + column_dfs_traits(Index jcol, Index& jsuper, typename SparseLUImpl::GlobalLU_t& glu, SparseLUImpl& luImpl) + : m_jcol(jcol), m_jsuper_ref(jsuper), m_glu(glu), m_luImpl(luImpl) + {} + bool update_segrep(Index /*krep*/, Index /*jj*/) + { + return true; + } + void mem_expand(IndexVector& lsub, Index& nextl, Index chmark) + { + if (nextl >= m_glu.nzlmax) + m_luImpl.memXpand(lsub, m_glu.nzlmax, nextl, LSUB, m_glu.num_expansions); + if (chmark != (m_jcol-1)) m_jsuper_ref = emptyIdxLU; + } + enum { ExpandMem = true }; + + Index m_jcol; + Index& m_jsuper_ref; + typename SparseLUImpl::GlobalLU_t& m_glu; + SparseLUImpl& m_luImpl; +}; + + +/** + * \brief Performs a symbolic factorization on column jcol and decide the supernode boundary + * + * A supernode representative is the last column of a supernode. + * The nonzeros in U[*,j] are segments that end at supernodes representatives. + * The routine returns a list of the supernodal representatives + * in topological order of the dfs that generates them. + * The location of the first nonzero in each supernodal segment + * (supernodal entry location) is also returned. + * + * \param m number of rows in the matrix + * \param jcol Current column + * \param perm_r Row permutation + * \param maxsuper Maximum number of column allowed in a supernode + * \param [in,out] nseg Number of segments in current U[*,j] - new segments appended + * \param lsub_col defines the rhs vector to start the dfs + * \param [in,out] segrep Segment representatives - new segments appended + * \param repfnz First nonzero location in each row + * \param xprune + * \param marker marker[i] == jj, if i was visited during dfs of current column jj; + * \param parent + * \param xplore working array + * \param glu global LU data + * \return 0 success + * > 0 number of bytes allocated when run out of space + * + */ +template +Index SparseLUImpl::column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg, + BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, + IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu) +{ + + Index jsuper = glu.supno(jcol); + Index nextl = glu.xlsub(jcol); + VectorBlock marker2(marker, 2*m, m); + + + column_dfs_traits traits(jcol, jsuper, glu, *this); + + // For each nonzero in A(*,jcol) do dfs + for (Index k = 0; ((k < m) ? lsub_col[k] != emptyIdxLU : false) ; k++) + { + Index krow = lsub_col(k); + lsub_col(k) = emptyIdxLU; + Index kmark = marker2(krow); + + // krow was visited before, go to the next nonz; + if (kmark == jcol) continue; + + dfs_kernel(StorageIndex(jcol), perm_r, nseg, glu.lsub, segrep, repfnz, xprune, marker2, parent, + xplore, glu, nextl, krow, traits); + } // for each nonzero ... + + Index fsupc; + StorageIndex nsuper = glu.supno(jcol); + StorageIndex jcolp1 = StorageIndex(jcol) + 1; + Index jcolm1 = jcol - 1; + + // check to see if j belongs in the same supernode as j-1 + if ( jcol == 0 ) + { // Do nothing for column 0 + nsuper = glu.supno(0) = 0 ; + } + else + { + fsupc = glu.xsup(nsuper); + StorageIndex jptr = glu.xlsub(jcol); // Not yet compressed + StorageIndex jm1ptr = glu.xlsub(jcolm1); + + // Use supernodes of type T2 : see SuperLU paper + if ( (nextl-jptr != jptr-jm1ptr-1) ) jsuper = emptyIdxLU; + + // Make sure the number of columns in a supernode doesn't + // exceed threshold + if ( (jcol - fsupc) >= maxsuper) jsuper = emptyIdxLU; + + /* If jcol starts a new supernode, reclaim storage space in + * glu.lsub from previous supernode. Note we only store + * the subscript set of the first and last columns of + * a supernode. (first for num values, last for pruning) + */ + if (jsuper == emptyIdxLU) + { // starts a new supernode + if ( (fsupc < jcolm1-1) ) + { // >= 3 columns in nsuper + StorageIndex ito = glu.xlsub(fsupc+1); + glu.xlsub(jcolm1) = ito; + StorageIndex istop = ito + jptr - jm1ptr; + xprune(jcolm1) = istop; // initialize xprune(jcol-1) + glu.xlsub(jcol) = istop; + + for (StorageIndex ifrom = jm1ptr; ifrom < nextl; ++ifrom, ++ito) + glu.lsub(ito) = glu.lsub(ifrom); + nextl = ito; // = istop + length(jcol) + } + nsuper++; + glu.supno(jcol) = nsuper; + } // if a new supernode + } // end else: jcol > 0 + + // Tidy up the pointers before exit + glu.xsup(nsuper+1) = jcolp1; + glu.supno(jcolp1) = nsuper; + xprune(jcol) = StorageIndex(nextl); // Initialize upper bound for pruning + glu.xlsub(jcolp1) = StorageIndex(nextl); + + return 0; +} + +} // end namespace internal + +} // end namespace Eigen + +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h new file mode 100644 index 0000000000..c32d8d8b14 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h @@ -0,0 +1,107 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. +/* + + * NOTE: This file is the modified version of [s,d,c,z]copy_to_ucol.c file in SuperLU + + * -- SuperLU routine (version 2.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * November 15, 1997 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSELU_COPY_TO_UCOL_H +#define SPARSELU_COPY_TO_UCOL_H + +namespace Eigen { +namespace internal { + +/** + * \brief Performs numeric block updates (sup-col) in topological order + * + * \param jcol current column to update + * \param nseg Number of segments in the U part + * \param segrep segment representative ... + * \param repfnz First nonzero column in each row ... + * \param perm_r Row permutation + * \param dense Store the full representation of the column + * \param glu Global LU data. + * \return 0 - successful return + * > 0 - number of bytes allocated when run out of space + * + */ +template +Index SparseLUImpl::copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, + BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu) +{ + Index ksub, krep, ksupno; + + Index jsupno = glu.supno(jcol); + + // For each nonzero supernode segment of U[*,j] in topological order + Index k = nseg - 1, i; + StorageIndex nextu = glu.xusub(jcol); + Index kfnz, isub, segsize; + Index new_next,irow; + Index fsupc, mem; + for (ksub = 0; ksub < nseg; ksub++) + { + krep = segrep(k); k--; + ksupno = glu.supno(krep); + if (jsupno != ksupno ) // should go into ucol(); + { + kfnz = repfnz(krep); + if (kfnz != emptyIdxLU) + { // Nonzero U-segment + fsupc = glu.xsup(ksupno); + isub = glu.xlsub(fsupc) + kfnz - fsupc; + segsize = krep - kfnz + 1; + new_next = nextu + segsize; + while (new_next > glu.nzumax) + { + mem = memXpand(glu.ucol, glu.nzumax, nextu, UCOL, glu.num_expansions); + if (mem) return mem; + mem = memXpand(glu.usub, glu.nzumax, nextu, USUB, glu.num_expansions); + if (mem) return mem; + + } + + for (i = 0; i < segsize; i++) + { + irow = glu.lsub(isub); + glu.usub(nextu) = perm_r(irow); // Unlike the L part, the U part is stored in its final order + glu.ucol(nextu) = dense(irow); + dense(irow) = Scalar(0.0); + nextu++; + isub++; + } + + } // end nonzero U-segment + + } // end if jsupno + + } // end for each segment + glu.xusub(jcol + 1) = nextu; // close U(*,jcol) + return 0; +} + +} // namespace internal +} // end namespace Eigen + +#endif // SPARSELU_COPY_TO_UCOL_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h new file mode 100644 index 0000000000..e37c2fe0d0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h @@ -0,0 +1,280 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Gael Guennebaud +// +// 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_SPARSELU_GEMM_KERNEL_H +#define EIGEN_SPARSELU_GEMM_KERNEL_H + +namespace Eigen { + +namespace internal { + + +/** \internal + * A general matrix-matrix product kernel optimized for the SparseLU factorization. + * - A, B, and C must be column major + * - lda and ldc must be multiples of the respective packet size + * - C must have the same alignment as A + */ +template +EIGEN_DONT_INLINE +void sparselu_gemm(Index m, Index n, Index d, const Scalar* A, Index lda, const Scalar* B, Index ldb, Scalar* C, Index ldc) +{ + using namespace Eigen::internal; + + typedef typename packet_traits::type Packet; + enum { + NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS, + PacketSize = packet_traits::size, + PM = 8, // peeling in M + RN = 2, // register blocking + RK = NumberOfRegisters>=16 ? 4 : 2, // register blocking + BM = 4096/sizeof(Scalar), // number of rows of A-C per chunk + SM = PM*PacketSize // step along M + }; + Index d_end = (d/RK)*RK; // number of columns of A (rows of B) suitable for full register blocking + Index n_end = (n/RN)*RN; // number of columns of B-C suitable for processing RN columns at once + Index i0 = internal::first_default_aligned(A,m); + + eigen_internal_assert(((lda%PacketSize)==0) && ((ldc%PacketSize)==0) && (i0==internal::first_default_aligned(C,m))); + + // handle the non aligned rows of A and C without any optimization: + for(Index i=0; i(BM, m-ib); // actual number of rows + Index actual_b_end1 = (actual_b/SM)*SM; // actual number of rows suitable for peeling + Index actual_b_end2 = (actual_b/PacketSize)*PacketSize; // actual number of rows suitable for vectorization + + // Let's process two columns of B-C at once + for(Index j=0; j(Bc0[0]); } + { b10 = pset1(Bc0[1]); } + if(RK==4) { b20 = pset1(Bc0[2]); } + if(RK==4) { b30 = pset1(Bc0[3]); } + { b01 = pset1(Bc1[0]); } + { b11 = pset1(Bc1[1]); } + if(RK==4) { b21 = pset1(Bc1[2]); } + if(RK==4) { b31 = pset1(Bc1[3]); } + + Packet a0, a1, a2, a3, c0, c1, t0, t1; + + const Scalar* A0 = A+ib+(k+0)*lda; + const Scalar* A1 = A+ib+(k+1)*lda; + const Scalar* A2 = A+ib+(k+2)*lda; + const Scalar* A3 = A+ib+(k+3)*lda; + + Scalar* C0 = C+ib+(j+0)*ldc; + Scalar* C1 = C+ib+(j+1)*ldc; + + a0 = pload(A0); + a1 = pload(A1); + if(RK==4) + { + a2 = pload(A2); + a3 = pload(A3); + } + else + { + // workaround "may be used uninitialized in this function" warning + a2 = a3 = a0; + } + +#define KMADD(c, a, b, tmp) {tmp = b; tmp = pmul(a,tmp); c = padd(c,tmp);} +#define WORK(I) \ + c0 = pload(C0+i+(I)*PacketSize); \ + c1 = pload(C1+i+(I)*PacketSize); \ + KMADD(c0, a0, b00, t0) \ + KMADD(c1, a0, b01, t1) \ + a0 = pload(A0+i+(I+1)*PacketSize); \ + KMADD(c0, a1, b10, t0) \ + KMADD(c1, a1, b11, t1) \ + a1 = pload(A1+i+(I+1)*PacketSize); \ + if(RK==4){ KMADD(c0, a2, b20, t0) }\ + if(RK==4){ KMADD(c1, a2, b21, t1) }\ + if(RK==4){ a2 = pload(A2+i+(I+1)*PacketSize); }\ + if(RK==4){ KMADD(c0, a3, b30, t0) }\ + if(RK==4){ KMADD(c1, a3, b31, t1) }\ + if(RK==4){ a3 = pload(A3+i+(I+1)*PacketSize); }\ + pstore(C0+i+(I)*PacketSize, c0); \ + pstore(C1+i+(I)*PacketSize, c1) + + // process rows of A' - C' with aggressive vectorization and peeling + for(Index i=0; i0) + { + const Scalar* Bc0 = B+(n-1)*ldb; + + for(Index k=0; k(Bc0[0]); + b10 = pset1(Bc0[1]); + if(RK==4) b20 = pset1(Bc0[2]); + if(RK==4) b30 = pset1(Bc0[3]); + + Packet a0, a1, a2, a3, c0, t0/*, t1*/; + + const Scalar* A0 = A+ib+(k+0)*lda; + const Scalar* A1 = A+ib+(k+1)*lda; + const Scalar* A2 = A+ib+(k+2)*lda; + const Scalar* A3 = A+ib+(k+3)*lda; + + Scalar* C0 = C+ib+(n_end)*ldc; + + a0 = pload(A0); + a1 = pload(A1); + if(RK==4) + { + a2 = pload(A2); + a3 = pload(A3); + } + else + { + // workaround "may be used uninitialized in this function" warning + a2 = a3 = a0; + } + +#define WORK(I) \ + c0 = pload(C0+i+(I)*PacketSize); \ + KMADD(c0, a0, b00, t0) \ + a0 = pload(A0+i+(I+1)*PacketSize); \ + KMADD(c0, a1, b10, t0) \ + a1 = pload(A1+i+(I+1)*PacketSize); \ + if(RK==4){ KMADD(c0, a2, b20, t0) }\ + if(RK==4){ a2 = pload(A2+i+(I+1)*PacketSize); }\ + if(RK==4){ KMADD(c0, a3, b30, t0) }\ + if(RK==4){ a3 = pload(A3+i+(I+1)*PacketSize); }\ + pstore(C0+i+(I)*PacketSize, c0); + + // aggressive vectorization and peeling + for(Index i=0; i0) + { + for(Index j=0; j1 ? Aligned : 0 + }; + typedef Map, Alignment > MapVector; + typedef Map, Alignment > ConstMapVector; + if(rd==1) MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b); + + else if(rd==2) MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b) + + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b); + + else MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b) + + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b) + + B[2+d_end+j*ldb] * ConstMapVector(A+(d_end+2)*lda+ib, actual_b); + } + } + + } // blocking on the rows of A and C +} +#undef KMADD + +} // namespace internal + +} // namespace Eigen + +#endif // EIGEN_SPARSELU_GEMM_KERNEL_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h new file mode 100644 index 0000000000..6f75d500e5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h @@ -0,0 +1,126 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* This file is a modified version of heap_relax_snode.c file in SuperLU + * -- SuperLU routine (version 3.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * October 15, 2003 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ + +#ifndef SPARSELU_HEAP_RELAX_SNODE_H +#define SPARSELU_HEAP_RELAX_SNODE_H + +namespace Eigen { +namespace internal { + +/** + * \brief Identify the initial relaxed supernodes + * + * This routine applied to a symmetric elimination tree. + * It assumes that the matrix has been reordered according to the postorder of the etree + * \param n The number of columns + * \param et elimination tree + * \param relax_columns Maximum number of columns allowed in a relaxed snode + * \param descendants Number of descendants of each node in the etree + * \param relax_end last column in a supernode + */ +template +void SparseLUImpl::heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end) +{ + + // The etree may not be postordered, but its heap ordered + IndexVector post; + internal::treePostorder(StorageIndex(n), et, post); // Post order etree + IndexVector inv_post(n+1); + for (StorageIndex i = 0; i < n+1; ++i) inv_post(post(i)) = i; // inv_post = post.inverse()??? + + // Renumber etree in postorder + IndexVector iwork(n); + IndexVector et_save(n+1); + for (Index i = 0; i < n; ++i) + { + iwork(post(i)) = post(et(i)); + } + et_save = et; // Save the original etree + et = iwork; + + // compute the number of descendants of each node in the etree + relax_end.setConstant(emptyIdxLU); + Index j, parent; + descendants.setZero(); + for (j = 0; j < n; j++) + { + parent = et(j); + if (parent != n) // not the dummy root + descendants(parent) += descendants(j) + 1; + } + // Identify the relaxed supernodes by postorder traversal of the etree + Index snode_start; // beginning of a snode + StorageIndex k; + Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree + Index nsuper_et = 0; // Number of relaxed snodes in the original etree + StorageIndex l; + for (j = 0; j < n; ) + { + parent = et(j); + snode_start = j; + while ( parent != n && descendants(parent) < relax_columns ) + { + j = parent; + parent = et(j); + } + // Found a supernode in postordered etree, j is the last column + ++nsuper_et_post; + k = StorageIndex(n); + for (Index i = snode_start; i <= j; ++i) + k = (std::min)(k, inv_post(i)); + l = inv_post(j); + if ( (l - k) == (j - snode_start) ) // Same number of columns in the snode + { + // This is also a supernode in the original etree + relax_end(k) = l; // Record last column + ++nsuper_et; + } + else + { + for (Index i = snode_start; i <= j; ++i) + { + l = inv_post(i); + if (descendants(i) == 0) + { + relax_end(l) = l; + ++nsuper_et; + } + } + } + j++; + // Search for a new leaf + while (descendants(j) != 0 && j < n) j++; + } // End postorder traversal of the etree + + // Recover the original etree + et = et_save; +} + +} // end namespace internal + +} // end namespace Eigen +#endif // SPARSELU_HEAP_RELAX_SNODE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h new file mode 100644 index 0000000000..8c1b3e8bc6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h @@ -0,0 +1,130 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2012 Gael Guennebaud +// +// 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 SPARSELU_KERNEL_BMOD_H +#define SPARSELU_KERNEL_BMOD_H + +namespace Eigen { +namespace internal { + +template struct LU_kernel_bmod +{ + /** \internal + * \brief Performs numeric block updates from a given supernode to a single column + * + * \param segsize Size of the segment (and blocks ) to use for updates + * \param[in,out] dense Packed values of the original matrix + * \param tempv temporary vector to use for updates + * \param lusup array containing the supernodes + * \param lda Leading dimension in the supernode + * \param nrow Number of rows in the rectangular part of the supernode + * \param lsub compressed row subscripts of supernodes + * \param lptr pointer to the first column of the current supernode in lsub + * \param no_zeros Number of nonzeros elements before the diagonal part of the supernode + */ + template + static EIGEN_DONT_INLINE void run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda, + const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros); +}; + +template +template +EIGEN_DONT_INLINE void LU_kernel_bmod::run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda, + const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros) +{ + typedef typename ScalarVector::Scalar Scalar; + // First, copy U[*,j] segment from dense(*) to tempv(*) + // The result of triangular solve is in tempv[*]; + // The result of matric-vector update is in dense[*] + Index isub = lptr + no_zeros; + Index i; + Index irow; + for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++) + { + irow = lsub(isub); + tempv(i) = dense(irow); + ++isub; + } + // Dense triangular solve -- start effective triangle + luptr += lda * no_zeros + no_zeros; + // Form Eigen matrix and vector + Map, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) ); + Map > u(tempv.data(), segsize); + + u = A.template triangularView().solve(u); + + // Dense matrix-vector product y <-- B*x + luptr += segsize; + const Index PacketSize = internal::packet_traits::size; + Index ldl = internal::first_multiple(nrow, PacketSize); + Map, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) ); + Index aligned_offset = internal::first_default_aligned(tempv.data()+segsize, PacketSize); + Index aligned_with_B_offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize))%PacketSize; + Map, 0, OuterStride<> > l(tempv.data()+segsize+aligned_offset+aligned_with_B_offset, nrow, OuterStride<>(ldl) ); + + l.setZero(); + internal::sparselu_gemm(l.rows(), l.cols(), B.cols(), B.data(), B.outerStride(), u.data(), u.outerStride(), l.data(), l.outerStride()); + + // Scatter tempv[] into SPA dense[] as a temporary storage + isub = lptr + no_zeros; + for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++) + { + irow = lsub(isub++); + dense(irow) = tempv(i); + } + + // Scatter l into SPA dense[] + for (i = 0; i < nrow; i++) + { + irow = lsub(isub++); + dense(irow) -= l(i); + } +} + +template <> struct LU_kernel_bmod<1> +{ + template + static EIGEN_DONT_INLINE void run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr, + const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros); +}; + + +template +EIGEN_DONT_INLINE void LU_kernel_bmod<1>::run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr, + const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros) +{ + typedef typename ScalarVector::Scalar Scalar; + typedef typename IndexVector::Scalar StorageIndex; + Scalar f = dense(lsub(lptr + no_zeros)); + luptr += lda * no_zeros + no_zeros + 1; + const Scalar* a(lusup.data() + luptr); + const StorageIndex* irow(lsub.data()+lptr + no_zeros + 1); + Index i = 0; + for (; i+1 < nrow; i+=2) + { + Index i0 = *(irow++); + Index i1 = *(irow++); + Scalar a0 = *(a++); + Scalar a1 = *(a++); + Scalar d0 = dense.coeff(i0); + Scalar d1 = dense.coeff(i1); + d0 -= f*a0; + d1 -= f*a1; + dense.coeffRef(i0) = d0; + dense.coeffRef(i1) = d1; + } + if(i +// Copyright (C) 2012 Gael Guennebaud +// +// 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/. + +/* + + * NOTE: This file is the modified version of [s,d,c,z]panel_bmod.c file in SuperLU + + * -- SuperLU routine (version 3.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * October 15, 2003 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSELU_PANEL_BMOD_H +#define SPARSELU_PANEL_BMOD_H + +namespace Eigen { +namespace internal { + +/** + * \brief Performs numeric block updates (sup-panel) in topological order. + * + * Before entering this routine, the original nonzeros in the panel + * were already copied into the spa[m,w] + * + * \param m number of rows in the matrix + * \param w Panel size + * \param jcol Starting column of the panel + * \param nseg Number of segments in the U part + * \param dense Store the full representation of the panel + * \param tempv working array + * \param segrep segment representative... first row in the segment + * \param repfnz First nonzero rows + * \param glu Global LU data. + * + * + */ +template +void SparseLUImpl::panel_bmod(const Index m, const Index w, const Index jcol, + const Index nseg, ScalarVector& dense, ScalarVector& tempv, + IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu) +{ + + Index ksub,jj,nextl_col; + Index fsupc, nsupc, nsupr, nrow; + Index krep, kfnz; + Index lptr; // points to the row subscripts of a supernode + Index luptr; // ... + Index segsize,no_zeros ; + // For each nonz supernode segment of U[*,j] in topological order + Index k = nseg - 1; + const Index PacketSize = internal::packet_traits::size; + + for (ksub = 0; ksub < nseg; ksub++) + { // For each updating supernode + /* krep = representative of current k-th supernode + * fsupc = first supernodal column + * nsupc = number of columns in a supernode + * nsupr = number of rows in a supernode + */ + krep = segrep(k); k--; + fsupc = glu.xsup(glu.supno(krep)); + nsupc = krep - fsupc + 1; + nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); + nrow = nsupr - nsupc; + lptr = glu.xlsub(fsupc); + + // loop over the panel columns to detect the actual number of columns and rows + Index u_rows = 0; + Index u_cols = 0; + for (jj = jcol; jj < jcol + w; jj++) + { + nextl_col = (jj-jcol) * m; + VectorBlock repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row + + kfnz = repfnz_col(krep); + if ( kfnz == emptyIdxLU ) + continue; // skip any zero segment + + segsize = krep - kfnz + 1; + u_cols++; + u_rows = (std::max)(segsize,u_rows); + } + + if(nsupc >= 2) + { + Index ldu = internal::first_multiple(u_rows, PacketSize); + Map > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu)); + + // gather U + Index u_col = 0; + for (jj = jcol; jj < jcol + w; jj++) + { + nextl_col = (jj-jcol) * m; + VectorBlock repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row + VectorBlock dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here + + kfnz = repfnz_col(krep); + if ( kfnz == emptyIdxLU ) + continue; // skip any zero segment + + segsize = krep - kfnz + 1; + luptr = glu.xlusup(fsupc); + no_zeros = kfnz - fsupc; + + Index isub = lptr + no_zeros; + Index off = u_rows-segsize; + for (Index i = 0; i < off; i++) U(i,u_col) = 0; + for (Index i = 0; i < segsize; i++) + { + Index irow = glu.lsub(isub); + U(i+off,u_col) = dense_col(irow); + ++isub; + } + u_col++; + } + // solve U = A^-1 U + luptr = glu.xlusup(fsupc); + Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc); + no_zeros = (krep - u_rows + 1) - fsupc; + luptr += lda * no_zeros + no_zeros; + MappedMatrixBlock A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) ); + U = A.template triangularView().solve(U); + + // update + luptr += u_rows; + MappedMatrixBlock B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) ); + eigen_assert(tempv.size()>w*ldu + nrow*w + 1); + + Index ldl = internal::first_multiple(nrow, PacketSize); + Index offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize)) % PacketSize; + MappedMatrixBlock L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl)); + + L.setZero(); + internal::sparselu_gemm(L.rows(), L.cols(), B.cols(), B.data(), B.outerStride(), U.data(), U.outerStride(), L.data(), L.outerStride()); + + // scatter U and L + u_col = 0; + for (jj = jcol; jj < jcol + w; jj++) + { + nextl_col = (jj-jcol) * m; + VectorBlock repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row + VectorBlock dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here + + kfnz = repfnz_col(krep); + if ( kfnz == emptyIdxLU ) + continue; // skip any zero segment + + segsize = krep - kfnz + 1; + no_zeros = kfnz - fsupc; + Index isub = lptr + no_zeros; + + Index off = u_rows-segsize; + for (Index i = 0; i < segsize; i++) + { + Index irow = glu.lsub(isub++); + dense_col(irow) = U.coeff(i+off,u_col); + U.coeffRef(i+off,u_col) = 0; + } + + // Scatter l into SPA dense[] + for (Index i = 0; i < nrow; i++) + { + Index irow = glu.lsub(isub++); + dense_col(irow) -= L.coeff(i,u_col); + L.coeffRef(i,u_col) = 0; + } + u_col++; + } + } + else // level 2 only + { + // Sequence through each column in the panel + for (jj = jcol; jj < jcol + w; jj++) + { + nextl_col = (jj-jcol) * m; + VectorBlock repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row + VectorBlock dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here + + kfnz = repfnz_col(krep); + if ( kfnz == emptyIdxLU ) + continue; // skip any zero segment + + segsize = krep - kfnz + 1; + luptr = glu.xlusup(fsupc); + + Index lda = glu.xlusup(fsupc+1)-glu.xlusup(fsupc);// nsupr + + // Perform a trianglar solve and block update, + // then scatter the result of sup-col update to dense[] + no_zeros = kfnz - fsupc; + if(segsize==1) LU_kernel_bmod<1>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); + else if(segsize==2) LU_kernel_bmod<2>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); + else if(segsize==3) LU_kernel_bmod<3>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); + else LU_kernel_bmod::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); + } // End for each column in the panel + } + + } // End for each updating supernode +} // end panel bmod + +} // end namespace internal + +} // end namespace Eigen + +#endif // SPARSELU_PANEL_BMOD_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h new file mode 100644 index 0000000000..155df73368 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h @@ -0,0 +1,258 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* + + * NOTE: This file is the modified version of [s,d,c,z]panel_dfs.c file in SuperLU + + * -- SuperLU routine (version 2.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * November 15, 1997 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSELU_PANEL_DFS_H +#define SPARSELU_PANEL_DFS_H + +namespace Eigen { + +namespace internal { + +template +struct panel_dfs_traits +{ + typedef typename IndexVector::Scalar StorageIndex; + panel_dfs_traits(Index jcol, StorageIndex* marker) + : m_jcol(jcol), m_marker(marker) + {} + bool update_segrep(Index krep, StorageIndex jj) + { + if(m_marker[krep] +template +void SparseLUImpl::dfs_kernel(const StorageIndex jj, IndexVector& perm_r, + Index& nseg, IndexVector& panel_lsub, IndexVector& segrep, + Ref repfnz_col, IndexVector& xprune, Ref marker, IndexVector& parent, + IndexVector& xplore, GlobalLU_t& glu, + Index& nextl_col, Index krow, Traits& traits + ) +{ + + StorageIndex kmark = marker(krow); + + // For each unmarked krow of jj + marker(krow) = jj; + StorageIndex kperm = perm_r(krow); + if (kperm == emptyIdxLU ) { + // krow is in L : place it in structure of L(*, jj) + panel_lsub(nextl_col++) = StorageIndex(krow); // krow is indexed into A + + traits.mem_expand(panel_lsub, nextl_col, kmark); + } + else + { + // krow is in U : if its supernode-representative krep + // has been explored, update repfnz(*) + // krep = supernode representative of the current row + StorageIndex krep = glu.xsup(glu.supno(kperm)+1) - 1; + // First nonzero element in the current column: + StorageIndex myfnz = repfnz_col(krep); + + if (myfnz != emptyIdxLU ) + { + // Representative visited before + if (myfnz > kperm ) repfnz_col(krep) = kperm; + + } + else + { + // Otherwise, perform dfs starting at krep + StorageIndex oldrep = emptyIdxLU; + parent(krep) = oldrep; + repfnz_col(krep) = kperm; + StorageIndex xdfs = glu.xlsub(krep); + Index maxdfs = xprune(krep); + + StorageIndex kpar; + do + { + // For each unmarked kchild of krep + while (xdfs < maxdfs) + { + StorageIndex kchild = glu.lsub(xdfs); + xdfs++; + StorageIndex chmark = marker(kchild); + + if (chmark != jj ) + { + marker(kchild) = jj; + StorageIndex chperm = perm_r(kchild); + + if (chperm == emptyIdxLU) + { + // case kchild is in L: place it in L(*, j) + panel_lsub(nextl_col++) = kchild; + traits.mem_expand(panel_lsub, nextl_col, chmark); + } + else + { + // case kchild is in U : + // chrep = its supernode-rep. If its rep has been explored, + // update its repfnz(*) + StorageIndex chrep = glu.xsup(glu.supno(chperm)+1) - 1; + myfnz = repfnz_col(chrep); + + if (myfnz != emptyIdxLU) + { // Visited before + if (myfnz > chperm) + repfnz_col(chrep) = chperm; + } + else + { // Cont. dfs at snode-rep of kchild + xplore(krep) = xdfs; + oldrep = krep; + krep = chrep; // Go deeper down G(L) + parent(krep) = oldrep; + repfnz_col(krep) = chperm; + xdfs = glu.xlsub(krep); + maxdfs = xprune(krep); + + } // end if myfnz != -1 + } // end if chperm == -1 + + } // end if chmark !=jj + } // end while xdfs < maxdfs + + // krow has no more unexplored nbrs : + // Place snode-rep krep in postorder DFS, if this + // segment is seen for the first time. (Note that + // "repfnz(krep)" may change later.) + // Baktrack dfs to its parent + if(traits.update_segrep(krep,jj)) + //if (marker1(krep) < jcol ) + { + segrep(nseg) = krep; + ++nseg; + //marker1(krep) = jj; + } + + kpar = parent(krep); // Pop recursion, mimic recursion + if (kpar == emptyIdxLU) + break; // dfs done + krep = kpar; + xdfs = xplore(krep); + maxdfs = xprune(krep); + + } while (kpar != emptyIdxLU); // Do until empty stack + + } // end if (myfnz = -1) + + } // end if (kperm == -1) +} + +/** + * \brief Performs a symbolic factorization on a panel of columns [jcol, jcol+w) + * + * A supernode representative is the last column of a supernode. + * The nonzeros in U[*,j] are segments that end at supernodes representatives + * + * The routine returns a list of the supernodal representatives + * in topological order of the dfs that generates them. This list is + * a superset of the topological order of each individual column within + * the panel. + * The location of the first nonzero in each supernodal segment + * (supernodal entry location) is also returned. Each column has + * a separate list for this purpose. + * + * Two markers arrays are used for dfs : + * marker[i] == jj, if i was visited during dfs of current column jj; + * marker1[i] >= jcol, if i was visited by earlier columns in this panel; + * + * \param[in] m number of rows in the matrix + * \param[in] w Panel size + * \param[in] jcol Starting column of the panel + * \param[in] A Input matrix in column-major storage + * \param[in] perm_r Row permutation + * \param[out] nseg Number of U segments + * \param[out] dense Accumulate the column vectors of the panel + * \param[out] panel_lsub Subscripts of the row in the panel + * \param[out] segrep Segment representative i.e first nonzero row of each segment + * \param[out] repfnz First nonzero location in each row + * \param[out] xprune The pruned elimination tree + * \param[out] marker work vector + * \param parent The elimination tree + * \param xplore work vector + * \param glu The global data structure + * + */ + +template +void SparseLUImpl::panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu) +{ + Index nextl_col; // Next available position in panel_lsub[*,jj] + + // Initialize pointers + VectorBlock marker1(marker, m, m); + nseg = 0; + + panel_dfs_traits traits(jcol, marker1.data()); + + // For each column in the panel + for (StorageIndex jj = StorageIndex(jcol); jj < jcol + w; jj++) + { + nextl_col = (jj - jcol) * m; + + VectorBlock repfnz_col(repfnz, nextl_col, m); // First nonzero location in each row + VectorBlock dense_col(dense,nextl_col, m); // Accumulate a column vector here + + + // For each nnz in A[*, jj] do depth first search + for (typename MatrixType::InnerIterator it(A, jj); it; ++it) + { + Index krow = it.row(); + dense_col(krow) = it.value(); + + StorageIndex kmark = marker(krow); + if (kmark == jj) + continue; // krow visited before, go to the next nonzero + + dfs_kernel(jj, perm_r, nseg, panel_lsub, segrep, repfnz_col, xprune, marker, parent, + xplore, glu, nextl_col, krow, traits); + }// end for nonzeros in column jj + + } // end for column jj +} + +} // end namespace internal +} // end namespace Eigen + +#endif // SPARSELU_PANEL_DFS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h new file mode 100644 index 0000000000..a86dac93fa --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h @@ -0,0 +1,137 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* + + * NOTE: This file is the modified version of xpivotL.c file in SuperLU + + * -- SuperLU routine (version 3.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * October 15, 2003 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSELU_PIVOTL_H +#define SPARSELU_PIVOTL_H + +namespace Eigen { +namespace internal { + +/** + * \brief Performs the numerical pivotin on the current column of L, and the CDIV operation. + * + * Pivot policy : + * (1) Compute thresh = u * max_(i>=j) abs(A_ij); + * (2) IF user specifies pivot row k and abs(A_kj) >= thresh THEN + * pivot row = k; + * ELSE IF abs(A_jj) >= thresh THEN + * pivot row = j; + * ELSE + * pivot row = m; + * + * Note: If you absolutely want to use a given pivot order, then set u=0.0. + * + * \param jcol The current column of L + * \param diagpivotthresh diagonal pivoting threshold + * \param[in,out] perm_r Row permutation (threshold pivoting) + * \param[in] iperm_c column permutation - used to finf diagonal of Pc*A*Pc' + * \param[out] pivrow The pivot row + * \param glu Global LU data + * \return 0 if success, i > 0 if U(i,i) is exactly zero + * + */ +template +Index SparseLUImpl::pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu) +{ + + Index fsupc = (glu.xsup)((glu.supno)(jcol)); // First column in the supernode containing the column jcol + Index nsupc = jcol - fsupc; // Number of columns in the supernode portion, excluding jcol; nsupc >=0 + Index lptr = glu.xlsub(fsupc); // pointer to the starting location of the row subscripts for this supernode portion + Index nsupr = glu.xlsub(fsupc+1) - lptr; // Number of rows in the supernode + Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc); // leading dimension + Scalar* lu_sup_ptr = &(glu.lusup.data()[glu.xlusup(fsupc)]); // Start of the current supernode + Scalar* lu_col_ptr = &(glu.lusup.data()[glu.xlusup(jcol)]); // Start of jcol in the supernode + StorageIndex* lsub_ptr = &(glu.lsub.data()[lptr]); // Start of row indices of the supernode + + // Determine the largest abs numerical value for partial pivoting + Index diagind = iperm_c(jcol); // diagonal index + RealScalar pivmax(-1.0); + Index pivptr = nsupc; + Index diag = emptyIdxLU; + RealScalar rtemp; + Index isub, icol, itemp, k; + for (isub = nsupc; isub < nsupr; ++isub) { + using std::abs; + rtemp = abs(lu_col_ptr[isub]); + if (rtemp > pivmax) { + pivmax = rtemp; + pivptr = isub; + } + if (lsub_ptr[isub] == diagind) diag = isub; + } + + // Test for singularity + if ( pivmax <= RealScalar(0.0) ) { + // if pivmax == -1, the column is structurally empty, otherwise it is only numerically zero + pivrow = pivmax < RealScalar(0.0) ? diagind : lsub_ptr[pivptr]; + perm_r(pivrow) = StorageIndex(jcol); + return (jcol+1); + } + + RealScalar thresh = diagpivotthresh * pivmax; + + // Choose appropriate pivotal element + + { + // Test if the diagonal element can be used as a pivot (given the threshold value) + if (diag >= 0 ) + { + // Diagonal element exists + using std::abs; + rtemp = abs(lu_col_ptr[diag]); + if (rtemp != RealScalar(0.0) && rtemp >= thresh) pivptr = diag; + } + pivrow = lsub_ptr[pivptr]; + } + + // Record pivot row + perm_r(pivrow) = StorageIndex(jcol); + // Interchange row subscripts + if (pivptr != nsupc ) + { + std::swap( lsub_ptr[pivptr], lsub_ptr[nsupc] ); + // Interchange numerical values as well, for the two rows in the whole snode + // such that L is indexed the same way as A + for (icol = 0; icol <= nsupc; icol++) + { + itemp = pivptr + icol * lda; + std::swap(lu_sup_ptr[itemp], lu_sup_ptr[nsupc + icol * lda]); + } + } + // cdiv operations + Scalar temp = Scalar(1.0) / lu_col_ptr[nsupc]; + for (k = nsupc+1; k < nsupr; k++) + lu_col_ptr[k] *= temp; + return 0; +} + +} // end namespace internal +} // end namespace Eigen + +#endif // SPARSELU_PIVOTL_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h new file mode 100644 index 0000000000..ad32fed5e6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h @@ -0,0 +1,136 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* + + * NOTE: This file is the modified version of [s,d,c,z]pruneL.c file in SuperLU + + * -- SuperLU routine (version 2.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * November 15, 1997 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ +#ifndef SPARSELU_PRUNEL_H +#define SPARSELU_PRUNEL_H + +namespace Eigen { +namespace internal { + +/** + * \brief Prunes the L-structure. + * + * It prunes the L-structure of supernodes whose L-structure contains the current pivot row "pivrow" + * + * + * \param jcol The current column of L + * \param[in] perm_r Row permutation + * \param[out] pivrow The pivot row + * \param nseg Number of segments + * \param segrep + * \param repfnz + * \param[out] xprune + * \param glu Global LU data + * + */ +template +void SparseLUImpl::pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg, + const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu) +{ + // For each supernode-rep irep in U(*,j] + Index jsupno = glu.supno(jcol); + Index i,irep,irep1; + bool movnum, do_prune = false; + Index kmin = 0, kmax = 0, minloc, maxloc,krow; + for (i = 0; i < nseg; i++) + { + irep = segrep(i); + irep1 = irep + 1; + do_prune = false; + + // Don't prune with a zero U-segment + if (repfnz(irep) == emptyIdxLU) continue; + + // If a snode overlaps with the next panel, then the U-segment + // is fragmented into two parts -- irep and irep1. We should let + // pruning occur at the rep-column in irep1s snode. + if (glu.supno(irep) == glu.supno(irep1) ) continue; // don't prune + + // If it has not been pruned & it has a nonz in row L(pivrow,i) + if (glu.supno(irep) != jsupno ) + { + if ( xprune (irep) >= glu.xlsub(irep1) ) + { + kmin = glu.xlsub(irep); + kmax = glu.xlsub(irep1) - 1; + for (krow = kmin; krow <= kmax; krow++) + { + if (glu.lsub(krow) == pivrow) + { + do_prune = true; + break; + } + } + } + + if (do_prune) + { + // do a quicksort-type partition + // movnum=true means that the num values have to be exchanged + movnum = false; + if (irep == glu.xsup(glu.supno(irep)) ) // Snode of size 1 + movnum = true; + + while (kmin <= kmax) + { + if (perm_r(glu.lsub(kmax)) == emptyIdxLU) + kmax--; + else if ( perm_r(glu.lsub(kmin)) != emptyIdxLU) + kmin++; + else + { + // kmin below pivrow (not yet pivoted), and kmax + // above pivrow: interchange the two suscripts + std::swap(glu.lsub(kmin), glu.lsub(kmax)); + + // If the supernode has only one column, then we + // only keep one set of subscripts. For any subscript + // intercnahge performed, similar interchange must be + // done on the numerical values. + if (movnum) + { + minloc = glu.xlusup(irep) + ( kmin - glu.xlsub(irep) ); + maxloc = glu.xlusup(irep) + ( kmax - glu.xlsub(irep) ); + std::swap(glu.lusup(minloc), glu.lusup(maxloc)); + } + kmin++; + kmax--; + } + } // end while + + xprune(irep) = StorageIndex(kmin); //Pruning + } // end if do_prune + } // end pruning + } // End for each U-segment +} + +} // end namespace internal +} // end namespace Eigen + +#endif // SPARSELU_PRUNEL_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h new file mode 100644 index 0000000000..c408d01b40 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h @@ -0,0 +1,83 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// +// 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/. + +/* This file is a modified version of heap_relax_snode.c file in SuperLU + * -- SuperLU routine (version 3.0) -- + * Univ. of California Berkeley, Xerox Palo Alto Research Center, + * and Lawrence Berkeley National Lab. + * October 15, 2003 + * + * Copyright (c) 1994 by Xerox Corporation. All rights reserved. + * + * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + * + * Permission is hereby granted to use or copy this program for any + * purpose, provided the above notices are retained on all copies. + * Permission to modify the code and to distribute modified code is + * granted, provided the above notices are retained, and a notice that + * the code was modified is included with the above copyright notice. + */ + +#ifndef SPARSELU_RELAX_SNODE_H +#define SPARSELU_RELAX_SNODE_H + +namespace Eigen { + +namespace internal { + +/** + * \brief Identify the initial relaxed supernodes + * + * This routine is applied to a column elimination tree. + * It assumes that the matrix has been reordered according to the postorder of the etree + * \param n the number of columns + * \param et elimination tree + * \param relax_columns Maximum number of columns allowed in a relaxed snode + * \param descendants Number of descendants of each node in the etree + * \param relax_end last column in a supernode + */ +template +void SparseLUImpl::relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end) +{ + + // compute the number of descendants of each node in the etree + Index parent; + relax_end.setConstant(emptyIdxLU); + descendants.setZero(); + for (Index j = 0; j < n; j++) + { + parent = et(j); + if (parent != n) // not the dummy root + descendants(parent) += descendants(j) + 1; + } + // Identify the relaxed supernodes by postorder traversal of the etree + Index snode_start; // beginning of a snode + for (Index j = 0; j < n; ) + { + parent = et(j); + snode_start = j; + while ( parent != n && descendants(parent) < relax_columns ) + { + j = parent; + parent = et(j); + } + // Found a supernode in postordered etree, j is the last column + relax_end(snode_start) = StorageIndex(j); // Record last column + j++; + // Search for a new leaf + while (descendants(j) != 0 && j < n) j++; + } // End postorder traversal of the etree + +} + +} // end namespace internal + +} // end namespace Eigen +#endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h new file mode 100644 index 0000000000..d1fb96f5cb --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h @@ -0,0 +1,758 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012-2013 Desire Nuentsa +// Copyright (C) 2012-2014 Gael Guennebaud +// +// 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_SPARSE_QR_H +#define EIGEN_SPARSE_QR_H + +namespace Eigen { + +template class SparseQR; +template struct SparseQRMatrixQReturnType; +template struct SparseQRMatrixQTransposeReturnType; +template struct SparseQR_QProduct; +namespace internal { + template struct traits > + { + typedef typename SparseQRType::MatrixType ReturnType; + typedef typename ReturnType::StorageIndex StorageIndex; + typedef typename ReturnType::StorageKind StorageKind; + enum { + RowsAtCompileTime = Dynamic, + ColsAtCompileTime = Dynamic + }; + }; + template struct traits > + { + typedef typename SparseQRType::MatrixType ReturnType; + }; + template struct traits > + { + typedef typename Derived::PlainObject ReturnType; + }; +} // End namespace internal + +/** + * \ingroup SparseQR_Module + * \class SparseQR + * \brief Sparse left-looking QR factorization with numerical column pivoting + * + * This class implements a left-looking QR decomposition of sparse matrices + * with numerical column pivoting. + * When a column has a norm less than a given tolerance + * it is implicitly permuted to the end. The QR factorization thus obtained is + * given by A*P = Q*R where R is upper triangular or trapezoidal. + * + * P is the column permutation which is the product of the fill-reducing and the + * numerical permutations. Use colsPermutation() to get it. + * + * Q is the orthogonal matrix represented as products of Householder reflectors. + * Use matrixQ() to get an expression and matrixQ().adjoint() to get the adjoint. + * You can then apply it to a vector. + * + * R is the sparse triangular or trapezoidal matrix. The later occurs when A is rank-deficient. + * matrixR().topLeftCorner(rank(), rank()) always returns a triangular factor of full rank. + * + * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<> + * \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module + * OrderingMethods \endlink module for the list of built-in and external ordering methods. + * + * \implsparsesolverconcept + * + * The numerical pivoting strategy and default threshold are the same as in SuiteSparse QR, and + * detailed in the following paper: + * + * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing + * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011. + * + * Even though it is qualified as "rank-revealing", this strategy might fail for some + * rank deficient problems. When this class is used to solve linear or least-square problems + * it is thus strongly recommended to check the accuracy of the computed solution. If it + * failed, it usually helps to increase the threshold with setPivotThreshold. + * + * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()). + * \warning For complex matrices matrixQ().transpose() will actually return the adjoint matrix. + * + */ +template +class SparseQR : public SparseSolverBase > +{ + protected: + typedef SparseSolverBase > Base; + using Base::m_isInitialized; + public: + using Base::_solve_impl; + typedef _MatrixType MatrixType; + typedef _OrderingType OrderingType; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::StorageIndex StorageIndex; + typedef SparseMatrix QRMatrixType; + typedef Matrix IndexVector; + typedef Matrix ScalarVector; + typedef PermutationMatrix PermutationType; + + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + public: + SparseQR () : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) + { } + + /** Construct a QR factorization of the matrix \a mat. + * + * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). + * + * \sa compute() + */ + explicit SparseQR(const MatrixType& mat) : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) + { + compute(mat); + } + + /** Computes the QR factorization of the sparse matrix \a mat. + * + * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). + * + * \sa analyzePattern(), factorize() + */ + void compute(const MatrixType& mat) + { + analyzePattern(mat); + factorize(mat); + } + void analyzePattern(const MatrixType& mat); + void factorize(const MatrixType& mat); + + /** \returns the number of rows of the represented matrix. + */ + inline Index rows() const { return m_pmat.rows(); } + + /** \returns the number of columns of the represented matrix. + */ + inline Index cols() const { return m_pmat.cols();} + + /** \returns a const reference to the \b sparse upper triangular matrix R of the QR factorization. + * \warning The entries of the returned matrix are not sorted. This means that using it in algorithms + * expecting sorted entries will fail. This include random coefficient accesses (SpaseMatrix::coeff()), + * and coefficient-wise operations. Matrix products and triangular solves are fine though. + * + * To sort the entries, you can assign it to a row-major matrix, and if a column-major matrix + * is required, you can copy it again: + * \code + * SparseMatrix R = qr.matrixR(); // column-major, not sorted! + * SparseMatrix Rr = qr.matrixR(); // row-major, sorted + * SparseMatrix Rc = Rr; // column-major, sorted + * \endcode + */ + const QRMatrixType& matrixR() const { return m_R; } + + /** \returns the number of non linearly dependent columns as determined by the pivoting threshold. + * + * \sa setPivotThreshold() + */ + Index rank() const + { + eigen_assert(m_isInitialized && "The factorization should be called first, use compute()"); + return m_nonzeropivots; + } + + /** \returns an expression of the matrix Q as products of sparse Householder reflectors. + * The common usage of this function is to apply it to a dense matrix or vector + * \code + * VectorXd B1, B2; + * // Initialize B1 + * B2 = matrixQ() * B1; + * \endcode + * + * To get a plain SparseMatrix representation of Q: + * \code + * SparseMatrix Q; + * Q = SparseQR >(A).matrixQ(); + * \endcode + * Internally, this call simply performs a sparse product between the matrix Q + * and a sparse identity matrix. However, due to the fact that the sparse + * reflectors are stored unsorted, two transpositions are needed to sort + * them before performing the product. + */ + SparseQRMatrixQReturnType matrixQ() const + { return SparseQRMatrixQReturnType(*this); } + + /** \returns a const reference to the column permutation P that was applied to A such that A*P = Q*R + * It is the combination of the fill-in reducing permutation and numerical column pivoting. + */ + const PermutationType& colsPermutation() const + { + eigen_assert(m_isInitialized && "Decomposition is not initialized."); + return m_outputPerm_c; + } + + /** \returns A string describing the type of error. + * This method is provided to ease debugging, not to handle errors. + */ + std::string lastErrorMessage() const { return m_lastError; } + + /** \internal */ + template + bool _solve_impl(const MatrixBase &B, MatrixBase &dest) const + { + eigen_assert(m_isInitialized && "The factorization should be called first, use compute()"); + eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix"); + + Index rank = this->rank(); + + // Compute Q^* * b; + typename Dest::PlainObject y, b; + y = this->matrixQ().adjoint() * B; + b = y; + + // Solve with the triangular matrix R + y.resize((std::max)(cols(),y.rows()),y.cols()); + y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView().solve(b.topRows(rank)); + y.bottomRows(y.rows()-rank).setZero(); + + // Apply the column permutation + if (m_perm_c.size()) dest = colsPermutation() * y.topRows(cols()); + else dest = y.topRows(cols()); + + m_info = Success; + return true; + } + + /** Sets the threshold that is used to determine linearly dependent columns during the factorization. + * + * In practice, if during the factorization the norm of the column that has to be eliminated is below + * this threshold, then the entire column is treated as zero, and it is moved at the end. + */ + void setPivotThreshold(const RealScalar& threshold) + { + m_useDefaultThreshold = false; + m_threshold = threshold; + } + + /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A. + * + * \sa compute() + */ + template + inline const Solve solve(const MatrixBase& B) const + { + eigen_assert(m_isInitialized && "The factorization should be called first, use compute()"); + eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix"); + return Solve(*this, B.derived()); + } + template + inline const Solve solve(const SparseMatrixBase& B) const + { + eigen_assert(m_isInitialized && "The factorization should be called first, use compute()"); + eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix"); + return Solve(*this, B.derived()); + } + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the QR factorization reports a numerical problem + * \c InvalidInput if the input matrix is invalid + * + * \sa iparm() + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "Decomposition is not initialized."); + return m_info; + } + + + /** \internal */ + inline void _sort_matrix_Q() + { + if(this->m_isQSorted) return; + // The matrix Q is sorted during the transposition + SparseMatrix mQrm(this->m_Q); + this->m_Q = mQrm; + this->m_isQSorted = true; + } + + + protected: + bool m_analysisIsok; + bool m_factorizationIsok; + mutable ComputationInfo m_info; + std::string m_lastError; + QRMatrixType m_pmat; // Temporary matrix + QRMatrixType m_R; // The triangular factor matrix + QRMatrixType m_Q; // The orthogonal reflectors + ScalarVector m_hcoeffs; // The Householder coefficients + PermutationType m_perm_c; // Fill-reducing Column permutation + PermutationType m_pivotperm; // The permutation for rank revealing + PermutationType m_outputPerm_c; // The final column permutation + RealScalar m_threshold; // Threshold to determine null Householder reflections + bool m_useDefaultThreshold; // Use default threshold + Index m_nonzeropivots; // Number of non zero pivots found + IndexVector m_etree; // Column elimination tree + IndexVector m_firstRowElt; // First element in each row + bool m_isQSorted; // whether Q is sorted or not + bool m_isEtreeOk; // whether the elimination tree match the initial input matrix + + template friend struct SparseQR_QProduct; + +}; + +/** \brief Preprocessing step of a QR factorization + * + * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). + * + * In this step, the fill-reducing permutation is computed and applied to the columns of A + * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited. + * + * \note In this step it is assumed that there is no empty row in the matrix \a mat. + */ +template +void SparseQR::analyzePattern(const MatrixType& mat) +{ + eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR"); + // Copy to a column major matrix if the input is rowmajor + typename internal::conditional::type matCpy(mat); + // Compute the column fill reducing ordering + OrderingType ord; + ord(matCpy, m_perm_c); + Index n = mat.cols(); + Index m = mat.rows(); + Index diagSize = (std::min)(m,n); + + if (!m_perm_c.size()) + { + m_perm_c.resize(n); + m_perm_c.indices().setLinSpaced(n, 0,StorageIndex(n-1)); + } + + // Compute the column elimination tree of the permuted matrix + m_outputPerm_c = m_perm_c.inverse(); + internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; + + m_R.resize(m, n); + m_Q.resize(m, diagSize); + + // Allocate space for nonzero elements: rough estimation + m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree + m_Q.reserve(2*mat.nonZeros()); + m_hcoeffs.resize(diagSize); + m_analysisIsok = true; +} + +/** \brief Performs the numerical QR factorization of the input matrix + * + * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with + * a matrix having the same sparsity pattern than \a mat. + * + * \param mat The sparse column-major matrix + */ +template +void SparseQR::factorize(const MatrixType& mat) +{ + using std::abs; + + eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step"); + StorageIndex m = StorageIndex(mat.rows()); + StorageIndex n = StorageIndex(mat.cols()); + StorageIndex diagSize = (std::min)(m,n); + IndexVector mark((std::max)(m,n)); mark.setConstant(-1); // Record the visited nodes + IndexVector Ridx(n), Qidx(m); // Store temporarily the row indexes for the current column of R and Q + Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q + ScalarVector tval(m); // The dense vector used to compute the current column + RealScalar pivotThreshold = m_threshold; + + m_R.setZero(); + m_Q.setZero(); + m_pmat = mat; + if(!m_isEtreeOk) + { + m_outputPerm_c = m_perm_c.inverse(); + internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; + } + + m_pmat.uncompress(); // To have the innerNonZeroPtr allocated + + // Apply the fill-in reducing permutation lazily: + { + // If the input is row major, copy the original column indices, + // otherwise directly use the input matrix + // + IndexVector originalOuterIndicesCpy; + const StorageIndex *originalOuterIndices = mat.outerIndexPtr(); + if(MatrixType::IsRowMajor) + { + originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1); + originalOuterIndices = originalOuterIndicesCpy.data(); + } + + for (int i = 0; i < n; i++) + { + Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i; + m_pmat.outerIndexPtr()[p] = originalOuterIndices[i]; + m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i]; + } + } + + /* Compute the default threshold as in MatLab, see: + * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing + * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 + */ + if(m_useDefaultThreshold) + { + RealScalar max2Norm = 0.0; + for (int j = 0; j < n; j++) max2Norm = numext::maxi(max2Norm, m_pmat.col(j).norm()); + if(max2Norm==RealScalar(0)) + max2Norm = RealScalar(1); + pivotThreshold = 20 * (m + n) * max2Norm * NumTraits::epsilon(); + } + + // Initialize the numerical permutation + m_pivotperm.setIdentity(n); + + StorageIndex nonzeroCol = 0; // Record the number of valid pivots + m_Q.startVec(0); + + // Left looking rank-revealing QR factorization: compute a column of R and Q at a time + for (StorageIndex col = 0; col < n; ++col) + { + mark.setConstant(-1); + m_R.startVec(col); + mark(nonzeroCol) = col; + Qidx(0) = nonzeroCol; + nzcolR = 0; nzcolQ = 1; + bool found_diag = nonzeroCol>=m; + tval.setZero(); + + // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e., + // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k. + // Note: if the diagonal entry does not exist, then its contribution must be explicitly added, + // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found. + for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) + { + StorageIndex curIdx = nonzeroCol; + if(itp) curIdx = StorageIndex(itp.row()); + if(curIdx == nonzeroCol) found_diag = true; + + // Get the nonzeros indexes of the current column of R + StorageIndex st = m_firstRowElt(curIdx); // The traversal of the etree starts here + if (st < 0 ) + { + m_lastError = "Empty row found during numerical factorization"; + m_info = InvalidInput; + return; + } + + // Traverse the etree + Index bi = nzcolR; + for (; mark(st) != col; st = m_etree(st)) + { + Ridx(nzcolR) = st; // Add this row to the list, + mark(st) = col; // and mark this row as visited + nzcolR++; + } + + // Reverse the list to get the topological ordering + Index nt = nzcolR-bi; + for(Index i = 0; i < nt/2; i++) std::swap(Ridx(bi+i), Ridx(nzcolR-i-1)); + + // Copy the current (curIdx,pcol) value of the input matrix + if(itp) tval(curIdx) = itp.value(); + else tval(curIdx) = Scalar(0); + + // Compute the pattern of Q(:,k) + if(curIdx > nonzeroCol && mark(curIdx) != col ) + { + Qidx(nzcolQ) = curIdx; // Add this row to the pattern of Q, + mark(curIdx) = col; // and mark it as visited + nzcolQ++; + } + } + + // Browse all the indexes of R(:,col) in reverse order + for (Index i = nzcolR-1; i >= 0; i--) + { + Index curIdx = Ridx(i); + + // Apply the curIdx-th householder vector to the current column (temporarily stored into tval) + Scalar tdot(0); + + // First compute q' * tval + tdot = m_Q.col(curIdx).dot(tval); + + tdot *= m_hcoeffs(curIdx); + + // Then update tval = tval - q * tau + // FIXME: tval -= tdot * m_Q.col(curIdx) should amount to the same (need to check/add support for efficient "dense ?= sparse") + for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq) + tval(itq.row()) -= itq.value() * tdot; + + // Detect fill-in for the current column of Q + if(m_etree(Ridx(i)) == nonzeroCol) + { + for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq) + { + StorageIndex iQ = StorageIndex(itq.row()); + if (mark(iQ) != col) + { + Qidx(nzcolQ++) = iQ; // Add this row to the pattern of Q, + mark(iQ) = col; // and mark it as visited + } + } + } + } // End update current column + + Scalar tau = RealScalar(0); + RealScalar beta = 0; + + if(nonzeroCol < diagSize) + { + // Compute the Householder reflection that eliminate the current column + // FIXME this step should call the Householder module. + Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0); + + // First, the squared norm of Q((col+1):m, col) + RealScalar sqrNorm = 0.; + for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq))); + if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0)) + { + beta = numext::real(c0); + tval(Qidx(0)) = 1; + } + else + { + using std::sqrt; + beta = sqrt(numext::abs2(c0) + sqrNorm); + if(numext::real(c0) >= RealScalar(0)) + beta = -beta; + tval(Qidx(0)) = 1; + for (Index itq = 1; itq < nzcolQ; ++itq) + tval(Qidx(itq)) /= (c0 - beta); + tau = numext::conj((beta-c0) / beta); + + } + } + + // Insert values in R + for (Index i = nzcolR-1; i >= 0; i--) + { + Index curIdx = Ridx(i); + if(curIdx < nonzeroCol) + { + m_R.insertBackByOuterInnerUnordered(col, curIdx) = tval(curIdx); + tval(curIdx) = Scalar(0.); + } + } + + if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold) + { + m_R.insertBackByOuterInner(col, nonzeroCol) = beta; + // The householder coefficient + m_hcoeffs(nonzeroCol) = tau; + // Record the householder reflections + for (Index itq = 0; itq < nzcolQ; ++itq) + { + Index iQ = Qidx(itq); + m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ); + tval(iQ) = Scalar(0.); + } + nonzeroCol++; + if(nonzeroCol +struct SparseQR_QProduct : ReturnByValue > +{ + typedef typename SparseQRType::QRMatrixType MatrixType; + typedef typename SparseQRType::Scalar Scalar; + // Get the references + SparseQR_QProduct(const SparseQRType& qr, const Derived& other, bool transpose) : + m_qr(qr),m_other(other),m_transpose(transpose) {} + inline Index rows() const { return m_qr.matrixQ().rows(); } + inline Index cols() const { return m_other.cols(); } + + // Assign to a vector + template + void evalTo(DesType& res) const + { + Index m = m_qr.rows(); + Index n = m_qr.cols(); + Index diagSize = (std::min)(m,n); + res = m_other; + if (m_transpose) + { + eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes"); + //Compute res = Q' * other column by column + for(Index j = 0; j < res.cols(); j++){ + for (Index k = 0; k < diagSize; k++) + { + Scalar tau = Scalar(0); + tau = m_qr.m_Q.col(k).dot(res.col(j)); + if(tau==Scalar(0)) continue; + tau = tau * m_qr.m_hcoeffs(k); + res.col(j) -= tau * m_qr.m_Q.col(k); + } + } + } + else + { + eigen_assert(m_qr.matrixQ().cols() == m_other.rows() && "Non conforming object sizes"); + + res.conservativeResize(rows(), cols()); + + // Compute res = Q * other column by column + for(Index j = 0; j < res.cols(); j++) + { + Index start_k = internal::is_identity::value ? numext::mini(j,diagSize-1) : diagSize-1; + for (Index k = start_k; k >=0; k--) + { + Scalar tau = Scalar(0); + tau = m_qr.m_Q.col(k).dot(res.col(j)); + if(tau==Scalar(0)) continue; + tau = tau * numext::conj(m_qr.m_hcoeffs(k)); + res.col(j) -= tau * m_qr.m_Q.col(k); + } + } + } + } + + const SparseQRType& m_qr; + const Derived& m_other; + bool m_transpose; // TODO this actually means adjoint +}; + +template +struct SparseQRMatrixQReturnType : public EigenBase > +{ + typedef typename SparseQRType::Scalar Scalar; + typedef Matrix DenseMatrix; + enum { + RowsAtCompileTime = Dynamic, + ColsAtCompileTime = Dynamic + }; + explicit SparseQRMatrixQReturnType(const SparseQRType& qr) : m_qr(qr) {} + template + SparseQR_QProduct operator*(const MatrixBase& other) + { + return SparseQR_QProduct(m_qr,other.derived(),false); + } + // To use for operations with the adjoint of Q + SparseQRMatrixQTransposeReturnType adjoint() const + { + return SparseQRMatrixQTransposeReturnType(m_qr); + } + inline Index rows() const { return m_qr.rows(); } + inline Index cols() const { return m_qr.rows(); } + // To use for operations with the transpose of Q FIXME this is the same as adjoint at the moment + SparseQRMatrixQTransposeReturnType transpose() const + { + return SparseQRMatrixQTransposeReturnType(m_qr); + } + const SparseQRType& m_qr; +}; + +// TODO this actually represents the adjoint of Q +template +struct SparseQRMatrixQTransposeReturnType +{ + explicit SparseQRMatrixQTransposeReturnType(const SparseQRType& qr) : m_qr(qr) {} + template + SparseQR_QProduct operator*(const MatrixBase& other) + { + return SparseQR_QProduct(m_qr,other.derived(), true); + } + const SparseQRType& m_qr; +}; + +namespace internal { + +template +struct evaluator_traits > +{ + typedef typename SparseQRType::MatrixType MatrixType; + typedef typename storage_kind_to_evaluator_kind::Kind Kind; + typedef SparseShape Shape; +}; + +template< typename DstXprType, typename SparseQRType> +struct Assignment, internal::assign_op, Sparse2Sparse> +{ + typedef SparseQRMatrixQReturnType SrcXprType; + typedef typename DstXprType::Scalar Scalar; + typedef typename DstXprType::StorageIndex StorageIndex; + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &/*func*/) + { + typename DstXprType::PlainObject idMat(src.rows(), src.cols()); + idMat.setIdentity(); + // Sort the sparse householder reflectors if needed + const_cast(&src.m_qr)->_sort_matrix_Q(); + dst = SparseQR_QProduct(src.m_qr, idMat, false); + } +}; + +template< typename DstXprType, typename SparseQRType> +struct Assignment, internal::assign_op, Sparse2Dense> +{ + typedef SparseQRMatrixQReturnType SrcXprType; + typedef typename DstXprType::Scalar Scalar; + typedef typename DstXprType::StorageIndex StorageIndex; + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &/*func*/) + { + dst = src.m_qr.matrixQ() * DstXprType::Identity(src.m_qr.rows(), src.m_qr.rows()); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif