From be4d35459a0f074e6da42fd8ff5ff63dd5b24857 Mon Sep 17 00:00:00 2001 From: camaj Date: Sun, 7 Apr 2024 18:31:17 -0700 Subject: [PATCH 001/148] add wpi files --- include/frc/Compiler.h | 601 ++ include/frc/EigenCore.h | 24 + include/frc/MathExtras.h | 643 ++ include/frc/MathUtil.h | 172 + include/frc/array.h | 100 + include/frc/bit.h | 99 + include/frc/geometry/Pose2d.h | 209 + include/frc/geometry/Pose2d.inc | 43 + include/frc/geometry/Rotation2d.h | 165 + include/frc/geometry/Rotation2d.inc | 60 + include/frc/geometry/Transform2d.h | 127 + include/frc/geometry/Transform2d.inc | 30 + include/frc/geometry/Translation2d.h | 210 + include/frc/geometry/Translation2d.inc | 54 + include/frc/geometry/Twist2d.h | 59 + include/frc/kinematics/ChassisSpeeds.h | 253 + include/frc/kinematics/Kinematics.h | 62 + .../frc/kinematics/SwerveDriveKinematics.h | 303 + .../frc/kinematics/SwerveDriveKinematics.inc | 175 + .../kinematics/SwerveDriveWheelPositions.h | 51 + include/frc/kinematics/SwerveModulePosition.h | 44 + include/frc/kinematics/SwerveModuleState.h | 50 + include/frc/kinematics/WheelPositions.h | 15 + include/frc/spline/CubicHermiteSpline.h | 116 + include/frc/spline/QuinticHermiteSpline.h | 126 + include/frc/spline/Spline.h | 142 + include/frc/spline/SplineHelper.h | 119 + include/frc/spline/SplineParameterizer.h | 142 + .../thirdparty/eigen/include/.clang-format | 12 + .../thirdparty/eigen/include/Eigen/Cholesky | 43 + .../frc/thirdparty/eigen/include/Eigen/Core | 414 ++ .../eigen/include/Eigen/Eigenvalues | 63 + .../eigen/include/Eigen/Householder | 31 + .../include/Eigen/IterativeLinearSolvers | 52 + .../frc/thirdparty/eigen/include/Eigen/Jacobi | 33 + include/frc/thirdparty/eigen/include/Eigen/LU | 46 + .../eigen/include/Eigen/OrderingMethods | 73 + include/frc/thirdparty/eigen/include/Eigen/QR | 48 + .../frc/thirdparty/eigen/include/Eigen/SVD | 56 + .../eigen/include/Eigen/SparseCholesky | 40 + .../thirdparty/eigen/include/Eigen/SparseCore | 70 + .../thirdparty/eigen/include/Eigen/SparseLU | 50 + .../thirdparty/eigen/include/Eigen/SparseQR | 38 + .../Eigen/src/Cholesky/InternalHeaderCheck.h | 3 + .../eigen/include/Eigen/src/Cholesky/LDLT.h | 649 ++ .../eigen/include/Eigen/src/Cholesky/LLT.h | 514 ++ .../Eigen/src/Core/ArithmeticSequence.h | 264 + .../eigen/include/Eigen/src/Core/Array.h | 369 + .../eigen/include/Eigen/src/Core/ArrayBase.h | 222 + .../include/Eigen/src/Core/ArrayWrapper.h | 173 + .../eigen/include/Eigen/src/Core/Assign.h | 80 + .../include/Eigen/src/Core/AssignEvaluator.h | 951 +++ .../eigen/include/Eigen/src/Core/BandMatrix.h | 338 + .../eigen/include/Eigen/src/Core/Block.h | 439 ++ .../include/Eigen/src/Core/CommaInitializer.h | 149 + .../Eigen/src/Core/ConditionEstimator.h | 173 + .../include/Eigen/src/Core/CoreEvaluators.h | 1666 +++++ .../include/Eigen/src/Core/CoreIterators.h | 141 + .../include/Eigen/src/Core/CwiseBinaryOp.h | 166 + .../include/Eigen/src/Core/CwiseNullaryOp.h | 971 +++ .../include/Eigen/src/Core/CwiseTernaryOp.h | 171 + .../include/Eigen/src/Core/CwiseUnaryOp.h | 91 + .../include/Eigen/src/Core/CwiseUnaryView.h | 137 + .../eigen/include/Eigen/src/Core/DenseBase.h | 645 ++ .../include/Eigen/src/Core/DenseCoeffsBase.h | 568 ++ .../include/Eigen/src/Core/DenseStorage.h | 650 ++ .../eigen/include/Eigen/src/Core/Diagonal.h | 221 + .../include/Eigen/src/Core/DiagonalMatrix.h | 414 ++ .../include/Eigen/src/Core/DiagonalProduct.h | 30 + .../eigen/include/Eigen/src/Core/Dot.h | 289 + .../eigen/include/Eigen/src/Core/EigenBase.h | 144 + .../Eigen/src/Core/ForceAlignedAccess.h | 131 + .../eigen/include/Eigen/src/Core/Fuzzy.h | 132 + .../include/Eigen/src/Core/GeneralProduct.h | 527 ++ .../Eigen/src/Core/GenericPacketMath.h | 1487 ++++ .../include/Eigen/src/Core/GlobalFunctions.h | 226 + .../eigen/include/Eigen/src/Core/IO.h | 233 + .../include/Eigen/src/Core/IndexedView.h | 243 + .../Eigen/src/Core/InternalHeaderCheck.h | 3 + .../eigen/include/Eigen/src/Core/Inverse.h | 108 + .../eigen/include/Eigen/src/Core/Map.h | 153 + .../eigen/include/Eigen/src/Core/MapBase.h | 283 + .../include/Eigen/src/Core/MathFunctions.h | 1798 +++++ .../Eigen/src/Core/MathFunctionsImpl.h | 321 + .../eigen/include/Eigen/src/Core/Matrix.h | 527 ++ .../eigen/include/Eigen/src/Core/MatrixBase.h | 542 ++ .../include/Eigen/src/Core/NestByValue.h | 91 + .../eigen/include/Eigen/src/Core/NoAlias.h | 102 + .../eigen/include/Eigen/src/Core/NumTraits.h | 329 + .../Eigen/src/Core/PartialReduxEvaluator.h | 209 + .../Eigen/src/Core/PermutationMatrix.h | 552 ++ .../include/Eigen/src/Core/PlainObjectBase.h | 1039 +++ .../eigen/include/Eigen/src/Core/Product.h | 174 + .../Eigen/src/Core/ProductEvaluators.h | 1155 +++ .../eigen/include/Eigen/src/Core/Random.h | 207 + .../eigen/include/Eigen/src/Core/Redux.h | 528 ++ .../eigen/include/Eigen/src/Core/Ref.h | 383 + .../eigen/include/Eigen/src/Core/Replicate.h | 133 + .../eigen/include/Eigen/src/Core/Reshaped.h | 398 ++ .../include/Eigen/src/Core/ReturnByValue.h | 115 + .../eigen/include/Eigen/src/Core/Reverse.h | 196 + .../eigen/include/Eigen/src/Core/Select.h | 156 + .../include/Eigen/src/Core/SelfAdjointView.h | 329 + .../Eigen/src/Core/SelfCwiseBinaryOp.h | 50 + .../Eigen/src/Core/SkewSymmetricMatrix3.h | 382 + .../eigen/include/Eigen/src/Core/Solve.h | 174 + .../include/Eigen/src/Core/SolveTriangular.h | 237 + .../eigen/include/Eigen/src/Core/SolverBase.h | 159 + .../eigen/include/Eigen/src/Core/StableNorm.h | 236 + .../include/Eigen/src/Core/StlIterators.h | 614 ++ .../eigen/include/Eigen/src/Core/Stride.h | 107 + .../eigen/include/Eigen/src/Core/Swap.h | 74 + .../eigen/include/Eigen/src/Core/Transpose.h | 425 ++ .../include/Eigen/src/Core/Transpositions.h | 323 + .../include/Eigen/src/Core/TriangularMatrix.h | 899 +++ .../include/Eigen/src/Core/VectorBlock.h | 83 + .../include/Eigen/src/Core/VectorwiseOp.h | 713 ++ .../eigen/include/Eigen/src/Core/Visitor.h | 789 +++ .../include/Eigen/src/Core/arch/AVX/Complex.h | 438 ++ .../Eigen/src/Core/arch/AVX/MathFunctions.h | 113 + .../Eigen/src/Core/arch/AVX/PacketMath.h | 2856 ++++++++ .../Eigen/src/Core/arch/AVX/TypeCasting.h | 227 + .../Eigen/src/Core/arch/Default/BFloat16.h | 816 +++ .../Eigen/src/Core/arch/Default/ConjHelper.h | 128 + .../arch/Default/GenericPacketMathFunctions.h | 2131 ++++++ .../Default/GenericPacketMathFunctionsFwd.h | 158 + .../Eigen/src/Core/arch/Default/Half.h | 968 +++ .../Eigen/src/Core/arch/Default/Settings.h | 47 + .../Eigen/src/Core/arch/NEON/Complex.h | 666 ++ .../Core/arch/NEON/GeneralBlockPanelKernel.h | 243 + .../Eigen/src/Core/arch/NEON/MathFunctions.h | 67 + .../Eigen/src/Core/arch/NEON/PacketMath.h | 6185 +++++++++++++++++ .../Eigen/src/Core/arch/NEON/TypeCasting.h | 1642 +++++ .../Eigen/src/Core/arch/NEON/UnaryFunctors.h | 57 + .../include/Eigen/src/Core/arch/SSE/Complex.h | 418 ++ .../Eigen/src/Core/arch/SSE/MathFunctions.h | 88 + .../Eigen/src/Core/arch/SSE/PacketMath.h | 2389 +++++++ .../Eigen/src/Core/arch/SSE/TypeCasting.h | 197 + .../src/Core/functors/AssignmentFunctors.h | 181 + .../Eigen/src/Core/functors/BinaryFunctors.h | 763 ++ .../Eigen/src/Core/functors/NullaryFunctors.h | 263 + .../Eigen/src/Core/functors/StlFunctors.h | 149 + .../Eigen/src/Core/functors/TernaryFunctors.h | 52 + .../Eigen/src/Core/functors/UnaryFunctors.h | 1311 ++++ .../Core/products/GeneralBlockPanelKernel.h | 3153 +++++++++ .../src/Core/products/GeneralMatrixMatrix.h | 457 ++ .../products/GeneralMatrixMatrixTriangular.h | 322 + .../src/Core/products/GeneralMatrixVector.h | 473 ++ .../Eigen/src/Core/products/Parallelizer.h | 276 + .../Core/products/SelfadjointMatrixMatrix.h | 483 ++ .../Core/products/SelfadjointMatrixVector.h | 246 + .../src/Core/products/SelfadjointProduct.h | 133 + .../Core/products/SelfadjointRank2Update.h | 95 + .../Core/products/TriangularMatrixMatrix.h | 404 ++ .../Core/products/TriangularMatrixVector.h | 327 + .../Core/products/TriangularSolverMatrix.h | 388 ++ .../Core/products/TriangularSolverVector.h | 122 + .../include/Eigen/src/Core/util/Assert.h | 158 + .../include/Eigen/src/Core/util/BlasUtil.h | 622 ++ .../src/Core/util/ConfigureVectorization.h | 510 ++ .../include/Eigen/src/Core/util/Constants.h | 595 ++ .../src/Core/util/DisableStupidWarnings.h | 155 + .../Eigen/src/Core/util/EmulateArray.h | 272 + .../Eigen/src/Core/util/ForwardDeclarations.h | 505 ++ .../Eigen/src/Core/util/IndexedViewHelper.h | 208 + .../Eigen/src/Core/util/IntegralConstant.h | 284 + .../include/Eigen/src/Core/util/Macros.h | 1311 ++++ .../Eigen/src/Core/util/MaxSizeVector.h | 139 + .../include/Eigen/src/Core/util/Memory.h | 1322 ++++ .../eigen/include/Eigen/src/Core/util/Meta.h | 717 ++ .../include/Eigen/src/Core/util/MoreMeta.h | 630 ++ .../src/Core/util/ReenableStupidWarnings.h | 44 + .../Eigen/src/Core/util/ReshapedHelper.h | 51 + .../include/Eigen/src/Core/util/Serializer.h | 208 + .../Eigen/src/Core/util/StaticAssert.h | 105 + .../Eigen/src/Core/util/SymbolicIndex.h | 301 + .../include/Eigen/src/Core/util/XprHelper.h | 1025 +++ .../src/Eigenvalues/ComplexEigenSolver.h | 315 + .../Eigen/src/Eigenvalues/ComplexSchur.h | 442 ++ .../Eigen/src/Eigenvalues/EigenSolver.h | 572 ++ .../src/Eigenvalues/GeneralizedEigenSolver.h | 402 ++ .../GeneralizedSelfAdjointEigenSolver.h | 212 + .../src/Eigenvalues/HessenbergDecomposition.h | 356 + .../src/Eigenvalues/InternalHeaderCheck.h | 3 + .../src/Eigenvalues/MatrixBaseEigenvalues.h | 142 + .../include/Eigen/src/Eigenvalues/RealQZ.h | 587 ++ .../include/Eigen/src/Eigenvalues/RealSchur.h | 518 ++ .../src/Eigenvalues/SelfAdjointEigenSolver.h | 850 +++ .../src/Eigenvalues/Tridiagonalization.h | 527 ++ .../Eigen/src/Householder/BlockHouseholder.h | 115 + .../Eigen/src/Householder/Householder.h | 155 + .../src/Householder/HouseholderSequence.h | 502 ++ .../src/Householder/InternalHeaderCheck.h | 3 + .../BasicPreconditioners.h | 213 + .../src/IterativeLinearSolvers/BiCGSTAB.h | 202 + .../ConjugateGradient.h | 217 + .../IncompleteCholesky.h | 382 + .../IterativeLinearSolvers/IncompleteLUT.h | 421 ++ .../InternalHeaderCheck.h | 3 + .../IterativeSolverBase.h | 395 ++ .../LeastSquareConjugateGradient.h | 193 + .../IterativeLinearSolvers/SolveWithGuess.h | 111 + .../Eigen/src/Jacobi/InternalHeaderCheck.h | 3 + .../eigen/include/Eigen/src/Jacobi/Jacobi.h | 427 ++ .../eigen/include/Eigen/src/LU/Determinant.h | 98 + .../eigen/include/Eigen/src/LU/FullPivLU.h | 812 +++ .../Eigen/src/LU/InternalHeaderCheck.h | 3 + .../eigen/include/Eigen/src/LU/InverseImpl.h | 353 + .../eigen/include/Eigen/src/LU/PartialPivLU.h | 575 ++ .../include/Eigen/src/LU/arch/InverseSize4.h | 353 + .../include/Eigen/src/OrderingMethods/Amd.h | 413 ++ .../Eigen/src/OrderingMethods/Eigen_Colamd.h | 1690 +++++ .../src/OrderingMethods/InternalHeaderCheck.h | 3 + .../Eigen/src/OrderingMethods/Ordering.h | 148 + .../Eigen/src/QR/ColPivHouseholderQR.h | 651 ++ .../src/QR/CompleteOrthogonalDecomposition.h | 628 ++ .../Eigen/src/QR/FullPivHouseholderQR.h | 699 ++ .../include/Eigen/src/QR/HouseholderQR.h | 499 ++ .../Eigen/src/QR/InternalHeaderCheck.h | 3 + .../eigen/include/Eigen/src/SVD/BDCSVD.h | 1479 ++++ .../Eigen/src/SVD/InternalHeaderCheck.h | 3 + .../eigen/include/Eigen/src/SVD/JacobiSVD.h | 819 +++ .../eigen/include/Eigen/src/SVD/SVDBase.h | 437 ++ .../Eigen/src/SVD/UpperBidiagonalization.h | 379 + .../src/SparseCholesky/InternalHeaderCheck.h | 3 + .../src/SparseCholesky/SimplicialCholesky.h | 632 ++ .../SparseCholesky/SimplicialCholesky_impl.h | 158 + .../include/Eigen/src/SparseCore/AmbiVector.h | 329 + .../Eigen/src/SparseCore/CompressedStorage.h | 201 + .../ConservativeSparseSparseProduct.h | 308 + .../src/SparseCore/InternalHeaderCheck.h | 3 + .../Eigen/src/SparseCore/SparseAssign.h | 279 + .../Eigen/src/SparseCore/SparseBlock.h | 534 ++ .../Eigen/src/SparseCore/SparseColEtree.h | 194 + .../src/SparseCore/SparseCompressedBase.h | 591 ++ .../src/SparseCore/SparseCwiseBinaryOp.h | 938 +++ .../Eigen/src/SparseCore/SparseCwiseUnaryOp.h | 142 + .../Eigen/src/SparseCore/SparseDenseProduct.h | 316 + .../src/SparseCore/SparseDiagonalProduct.h | 138 + .../include/Eigen/src/SparseCore/SparseDot.h | 93 + .../Eigen/src/SparseCore/SparseFuzzy.h | 31 + .../include/Eigen/src/SparseCore/SparseMap.h | 295 + .../Eigen/src/SparseCore/SparseMatrix.h | 1828 +++++ .../Eigen/src/SparseCore/SparseMatrixBase.h | 400 ++ .../Eigen/src/SparseCore/SparsePermutation.h | 249 + .../Eigen/src/SparseCore/SparseProduct.h | 178 + .../Eigen/src/SparseCore/SparseRedux.h | 47 + .../include/Eigen/src/SparseCore/SparseRef.h | 370 + .../src/SparseCore/SparseSelfAdjointView.h | 613 ++ .../Eigen/src/SparseCore/SparseSolverBase.h | 115 + .../SparseSparseProductWithPruning.h | 184 + .../Eigen/src/SparseCore/SparseTranspose.h | 83 + .../src/SparseCore/SparseTriangularView.h | 177 + .../include/Eigen/src/SparseCore/SparseUtil.h | 209 + .../Eigen/src/SparseCore/SparseVector.h | 505 ++ .../include/Eigen/src/SparseCore/SparseView.h | 225 + .../Eigen/src/SparseCore/TriangularSolver.h | 266 + .../Eigen/src/SparseLU/InternalHeaderCheck.h | 3 + .../include/Eigen/src/SparseLU/SparseLU.h | 890 +++ .../include/Eigen/src/SparseLU/SparseLUImpl.h | 79 + .../Eigen/src/SparseLU/SparseLU_Memory.h | 210 + .../Eigen/src/SparseLU/SparseLU_Structs.h | 113 + .../src/SparseLU/SparseLU_SupernodalMatrix.h | 319 + .../Eigen/src/SparseLU/SparseLU_Utils.h | 75 + .../Eigen/src/SparseLU/SparseLU_column_bmod.h | 177 + .../Eigen/src/SparseLU/SparseLU_column_dfs.h | 168 + .../src/SparseLU/SparseLU_copy_to_ucol.h | 106 + .../src/SparseLU/SparseLU_heap_relax_snode.h | 114 + .../Eigen/src/SparseLU/SparseLU_kernel_bmod.h | 133 + .../Eigen/src/SparseLU/SparseLU_panel_bmod.h | 215 + .../Eigen/src/SparseLU/SparseLU_panel_dfs.h | 235 + .../Eigen/src/SparseLU/SparseLU_pivotL.h | 136 + .../Eigen/src/SparseLU/SparseLU_pruneL.h | 130 + .../Eigen/src/SparseLU/SparseLU_relax_snode.h | 81 + .../Eigen/src/SparseQR/InternalHeaderCheck.h | 3 + .../include/Eigen/src/SparseQR/SparseQR.h | 706 ++ .../eigen/include/Eigen/src/misc/Image.h | 80 + .../Eigen/src/misc/InternalHeaderCheck.h | 3 + .../eigen/include/Eigen/src/misc/Kernel.h | 77 + .../eigen/include/Eigen/src/misc/RealSvd2x2.h | 53 + .../Eigen/src/plugins/ArrayCwiseBinaryOps.inc | 347 + .../Eigen/src/plugins/ArrayCwiseUnaryOps.inc | 525 ++ .../Eigen/src/plugins/BlockMethods.inc | 1370 ++++ .../src/plugins/CommonCwiseBinaryOps.inc | 133 + .../Eigen/src/plugins/CommonCwiseUnaryOps.inc | 152 + .../Eigen/src/plugins/IndexedViewMethods.inc | 351 + .../Eigen/src/plugins/InternalHeaderCheck.inc | 3 + .../src/plugins/MatrixCwiseBinaryOps.inc | 331 + .../Eigen/src/plugins/MatrixCwiseUnaryOps.inc | 112 + .../Eigen/src/plugins/ReshapedMethods.inc | 133 + .../include/unsupported/Eigen/MatrixFunctions | 504 ++ .../src/MatrixFunctions/InternalHeaderCheck.h | 4 + .../src/MatrixFunctions/MatrixExponential.h | 444 ++ .../src/MatrixFunctions/MatrixFunction.h | 543 ++ .../src/MatrixFunctions/MatrixLogarithm.h | 366 + .../Eigen/src/MatrixFunctions/MatrixPower.h | 694 ++ .../src/MatrixFunctions/MatrixSquareRoot.h | 346 + .../Eigen/src/MatrixFunctions/StemFunction.h | 115 + include/frc/thirdparty/gcem/include/gcem.hpp | 101 + .../thirdparty/gcem/include/gcem_incl/abs.hpp | 57 + .../gcem/include/gcem_incl/acos.hpp | 96 + .../gcem/include/gcem_incl/acosh.hpp | 80 + .../gcem/include/gcem_incl/asin.hpp | 94 + .../gcem/include/gcem_incl/asinh.hpp | 77 + .../gcem/include/gcem_incl/atan.hpp | 167 + .../gcem/include/gcem_incl/atan2.hpp | 100 + .../gcem/include/gcem_incl/atanh.hpp | 91 + .../gcem/include/gcem_incl/beta.hpp | 58 + .../gcem/include/gcem_incl/binomial_coef.hpp | 96 + .../gcem/include/gcem_incl/ceil.hpp | 142 + .../gcem/include/gcem_incl/copysign.hpp | 53 + .../thirdparty/gcem/include/gcem_incl/cos.hpp | 95 + .../gcem/include/gcem_incl/cosh.hpp | 77 + .../thirdparty/gcem/include/gcem_incl/erf.hpp | 155 + .../gcem/include/gcem_incl/erf_inv.hpp | 268 + .../thirdparty/gcem/include/gcem_incl/exp.hpp | 142 + .../gcem/include/gcem_incl/expm1.hpp | 88 + .../gcem/include/gcem_incl/factorial.hpp | 103 + .../gcem/include/gcem_incl/find_exponent.hpp | 62 + .../gcem/include/gcem_incl/find_fraction.hpp | 51 + .../gcem/include/gcem_incl/find_whole.hpp | 51 + .../gcem/include/gcem_incl/floor.hpp | 142 + .../gcem/include/gcem_incl/fmod.hpp | 82 + .../thirdparty/gcem/include/gcem_incl/gcd.hpp | 87 + .../gcem/include/gcem_incl/gcem_options.hpp | 213 + .../gcem/include/gcem_incl/hypot.hpp | 181 + .../include/gcem_incl/incomplete_beta.hpp | 199 + .../include/gcem_incl/incomplete_beta_inv.hpp | 357 + .../include/gcem_incl/incomplete_gamma.hpp | 252 + .../gcem_incl/incomplete_gamma_inv.hpp | 276 + .../gcem/include/gcem_incl/inv_sqrt.hpp | 93 + .../gcem/include/gcem_incl/is_even.hpp | 46 + .../gcem/include/gcem_incl/is_finite.hpp | 83 + .../gcem/include/gcem_incl/is_inf.hpp | 177 + .../gcem/include/gcem_incl/is_nan.hpp | 85 + .../gcem/include/gcem_incl/is_odd.hpp | 47 + .../gcem/include/gcem_incl/lbeta.hpp | 47 + .../thirdparty/gcem/include/gcem_incl/lcm.hpp | 70 + .../gcem/include/gcem_incl/lgamma.hpp | 147 + .../gcem/include/gcem_incl/lmgamma.hpp | 78 + .../thirdparty/gcem/include/gcem_incl/log.hpp | 198 + .../gcem/include/gcem_incl/log10.hpp | 71 + .../gcem/include/gcem_incl/log1p.hpp | 92 + .../gcem/include/gcem_incl/log2.hpp | 71 + .../include/gcem_incl/log_binomial_coef.hpp | 70 + .../gcem/include/gcem_incl/mantissa.hpp | 52 + .../thirdparty/gcem/include/gcem_incl/max.hpp | 53 + .../thirdparty/gcem/include/gcem_incl/min.hpp | 53 + .../gcem/include/gcem_incl/neg_zero.hpp | 42 + .../thirdparty/gcem/include/gcem_incl/pow.hpp | 94 + .../gcem/include/gcem_incl/pow_integral.hpp | 133 + .../quadrature/gauss_legendre_30.hpp | 96 + .../quadrature/gauss_legendre_50.hpp | 136 + .../gcem/include/gcem_incl/round.hpp | 137 + .../thirdparty/gcem/include/gcem_incl/sgn.hpp | 50 + .../gcem/include/gcem_incl/signbit.hpp | 56 + .../thirdparty/gcem/include/gcem_incl/sin.hpp | 97 + .../gcem/include/gcem_incl/sinh.hpp | 77 + .../gcem/include/gcem_incl/sqrt.hpp | 121 + .../thirdparty/gcem/include/gcem_incl/tan.hpp | 152 + .../gcem/include/gcem_incl/tanh.hpp | 101 + .../gcem/include/gcem_incl/tgamma.hpp | 92 + .../gcem/include/gcem_incl/trunc.hpp | 133 + include/frc/trajectory/Trajectory.h | 140 + include/frc/trajectory/TrajectoryConfig.h | 139 + include/frc/trajectory/TrajectoryGenerator.h | 129 + .../frc/trajectory/TrajectoryParameterizer.h | 106 + .../CentripetalAccelerationConstraint.h | 41 + .../constraint/EllipticalRegionConstraint.h | 91 + .../constraint/MaxVelocityConstraint.h | 38 + .../constraint/RectangularRegionConstraint.h | 76 + .../SwerveDriveKinematicsConstraint.h | 40 + .../SwerveDriveKinematicsConstraint.inc | 42 + .../constraint/TrajectoryConstraint.h | 79 + include/units/acceleration.h | 56 + include/units/angle.h | 56 + include/units/angular_acceleration.h | 42 + include/units/angular_jerk.h | 34 + include/units/angular_velocity.h | 60 + include/units/area.h | 56 + include/units/base.h | 3445 +++++++++ include/units/capacitance.h | 51 + include/units/charge.h | 53 + include/units/concentration.h | 56 + include/units/conductance.h | 51 + include/units/constants.h | 102 + include/units/current.h | 50 + include/units/curvature.h | 14 + include/units/data.h | 52 + include/units/data_transfer_rate.h | 57 + include/units/density.h | 70 + include/units/dimensionless.h | 36 + include/units/energy.h | 65 + include/units/force.h | 62 + include/units/formatter.h | 219 + include/units/frequency.h | 50 + include/units/illuminance.h | 61 + include/units/impedance.h | 50 + include/units/inductance.h | 50 + include/units/length.h | 72 + include/units/luminous_flux.h | 51 + include/units/luminous_intensity.h | 51 + include/units/magnetic_field_strength.h | 59 + include/units/magnetic_flux.h | 53 + include/units/mass.h | 58 + include/units/math.h | 752 ++ include/units/moment_of_inertia.h | 16 + include/units/power.h | 51 + include/units/pressure.h | 59 + include/units/radiation.h | 69 + include/units/solid_angle.h | 56 + include/units/substance.h | 49 + include/units/temperature.h | 59 + include/units/time.h | 57 + include/units/torque.h | 60 + include/units/velocity.h | 59 + include/units/voltage.h | 52 + include/units/volume.h | 83 + src/frc/geometry/Pose2d.cpp | 108 + src/frc/geometry/Rotation2d.cpp | 22 + src/frc/geometry/Transform2d.cpp | 24 + src/frc/geometry/Translation2d.cpp | 54 + src/frc/kinematics/SwerveDriveKinematics.cpp | 12 + src/frc/kinematics/SwerveModulePosition.cpp | 15 + src/frc/kinematics/SwerveModuleState.cpp | 24 + src/frc/spline/CubicHermiteSpline.cpp | 45 + src/frc/spline/QuinticHermiteSpline.cpp | 44 + src/frc/spline/SplineHelper.cpp | 280 + src/frc/spline/SplineParameterizer.cpp | 9 + src/frc/trajectory/Trajectory.cpp | 165 + src/frc/trajectory/TrajectoryGenerator.cpp | 148 + .../trajectory/TrajectoryParameterizer.cpp | 239 + .../CentripetalAccelerationConstraint.cpp | 39 + .../constraint/MaxVelocityConstraint.cpp | 23 + 434 files changed, 128065 insertions(+) create mode 100644 include/frc/Compiler.h create mode 100644 include/frc/EigenCore.h create mode 100644 include/frc/MathExtras.h create mode 100644 include/frc/MathUtil.h create mode 100644 include/frc/array.h create mode 100644 include/frc/bit.h create mode 100644 include/frc/geometry/Pose2d.h create mode 100644 include/frc/geometry/Pose2d.inc create mode 100644 include/frc/geometry/Rotation2d.h create mode 100644 include/frc/geometry/Rotation2d.inc create mode 100644 include/frc/geometry/Transform2d.h create mode 100644 include/frc/geometry/Transform2d.inc create mode 100644 include/frc/geometry/Translation2d.h create mode 100644 include/frc/geometry/Translation2d.inc create mode 100644 include/frc/geometry/Twist2d.h create mode 100644 include/frc/kinematics/ChassisSpeeds.h create mode 100644 include/frc/kinematics/Kinematics.h create mode 100644 include/frc/kinematics/SwerveDriveKinematics.h create mode 100644 include/frc/kinematics/SwerveDriveKinematics.inc create mode 100644 include/frc/kinematics/SwerveDriveWheelPositions.h create mode 100644 include/frc/kinematics/SwerveModulePosition.h create mode 100644 include/frc/kinematics/SwerveModuleState.h create mode 100644 include/frc/kinematics/WheelPositions.h create mode 100644 include/frc/spline/CubicHermiteSpline.h create mode 100644 include/frc/spline/QuinticHermiteSpline.h create mode 100644 include/frc/spline/Spline.h create mode 100644 include/frc/spline/SplineHelper.h create mode 100644 include/frc/spline/SplineParameterizer.h create mode 100644 include/frc/thirdparty/eigen/include/.clang-format create mode 100644 include/frc/thirdparty/eigen/include/Eigen/Cholesky create mode 100644 include/frc/thirdparty/eigen/include/Eigen/Core create mode 100644 include/frc/thirdparty/eigen/include/Eigen/Eigenvalues create mode 100644 include/frc/thirdparty/eigen/include/Eigen/Householder create mode 100644 include/frc/thirdparty/eigen/include/Eigen/IterativeLinearSolvers create mode 100644 include/frc/thirdparty/eigen/include/Eigen/Jacobi create mode 100644 include/frc/thirdparty/eigen/include/Eigen/LU create mode 100644 include/frc/thirdparty/eigen/include/Eigen/OrderingMethods create mode 100644 include/frc/thirdparty/eigen/include/Eigen/QR create mode 100644 include/frc/thirdparty/eigen/include/Eigen/SVD create mode 100644 include/frc/thirdparty/eigen/include/Eigen/SparseCholesky create mode 100644 include/frc/thirdparty/eigen/include/Eigen/SparseCore create mode 100644 include/frc/thirdparty/eigen/include/Eigen/SparseLU create mode 100644 include/frc/thirdparty/eigen/include/Eigen/SparseQR create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Array.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Assign.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Block.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Dot.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/IO.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Inverse.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Map.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/MapBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Matrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Product.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Random.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Redux.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Ref.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Replicate.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Reverse.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Select.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Solve.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Stride.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Swap.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Transpose.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/Visitor.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/UnaryFunctors.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/Assert.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Householder/Householder.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Householder/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Jacobi/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/LU/Determinant.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/LU/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/OrderingMethods/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/QR/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SVD/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCholesky/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseQR/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/misc/Image.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/misc/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/misc/Kernel.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/InternalHeaderCheck.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc create mode 100644 include/frc/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.inc create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/InternalHeaderCheck.h create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h create mode 100644 include/frc/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h create mode 100644 include/frc/thirdparty/gcem/include/gcem.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/abs.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/acos.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/acosh.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/asin.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/asinh.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/atan.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/atan2.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/atanh.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/beta.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/ceil.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/copysign.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/cos.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/cosh.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/erf.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/erf_inv.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/exp.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/expm1.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/factorial.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/find_exponent.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/find_fraction.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/find_whole.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/floor.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/fmod.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/gcd.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/gcem_options.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/hypot.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/is_even.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/is_finite.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/is_inf.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/is_nan.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/is_odd.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/lbeta.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/lcm.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/lgamma.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/lmgamma.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/log.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/log10.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/log1p.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/log2.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/mantissa.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/max.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/min.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/neg_zero.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/pow.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/pow_integral.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/round.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/sgn.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/signbit.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/sin.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/sinh.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/sqrt.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/tan.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/tanh.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/tgamma.hpp create mode 100644 include/frc/thirdparty/gcem/include/gcem_incl/trunc.hpp create mode 100644 include/frc/trajectory/Trajectory.h create mode 100644 include/frc/trajectory/TrajectoryConfig.h create mode 100644 include/frc/trajectory/TrajectoryGenerator.h create mode 100644 include/frc/trajectory/TrajectoryParameterizer.h create mode 100644 include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h create mode 100644 include/frc/trajectory/constraint/EllipticalRegionConstraint.h create mode 100644 include/frc/trajectory/constraint/MaxVelocityConstraint.h create mode 100644 include/frc/trajectory/constraint/RectangularRegionConstraint.h create mode 100644 include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h create mode 100644 include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc create mode 100644 include/frc/trajectory/constraint/TrajectoryConstraint.h create mode 100644 include/units/acceleration.h create mode 100644 include/units/angle.h create mode 100644 include/units/angular_acceleration.h create mode 100644 include/units/angular_jerk.h create mode 100644 include/units/angular_velocity.h create mode 100644 include/units/area.h create mode 100644 include/units/base.h create mode 100644 include/units/capacitance.h create mode 100644 include/units/charge.h create mode 100644 include/units/concentration.h create mode 100644 include/units/conductance.h create mode 100644 include/units/constants.h create mode 100644 include/units/current.h create mode 100644 include/units/curvature.h create mode 100644 include/units/data.h create mode 100644 include/units/data_transfer_rate.h create mode 100644 include/units/density.h create mode 100644 include/units/dimensionless.h create mode 100644 include/units/energy.h create mode 100644 include/units/force.h create mode 100644 include/units/formatter.h create mode 100644 include/units/frequency.h create mode 100644 include/units/illuminance.h create mode 100644 include/units/impedance.h create mode 100644 include/units/inductance.h create mode 100644 include/units/length.h create mode 100644 include/units/luminous_flux.h create mode 100644 include/units/luminous_intensity.h create mode 100644 include/units/magnetic_field_strength.h create mode 100644 include/units/magnetic_flux.h create mode 100644 include/units/mass.h create mode 100644 include/units/math.h create mode 100644 include/units/moment_of_inertia.h create mode 100644 include/units/power.h create mode 100644 include/units/pressure.h create mode 100644 include/units/radiation.h create mode 100644 include/units/solid_angle.h create mode 100644 include/units/substance.h create mode 100644 include/units/temperature.h create mode 100644 include/units/time.h create mode 100644 include/units/torque.h create mode 100644 include/units/velocity.h create mode 100644 include/units/voltage.h create mode 100644 include/units/volume.h create mode 100644 src/frc/geometry/Pose2d.cpp create mode 100644 src/frc/geometry/Rotation2d.cpp create mode 100644 src/frc/geometry/Transform2d.cpp create mode 100644 src/frc/geometry/Translation2d.cpp create mode 100644 src/frc/kinematics/SwerveDriveKinematics.cpp create mode 100644 src/frc/kinematics/SwerveModulePosition.cpp create mode 100644 src/frc/kinematics/SwerveModuleState.cpp create mode 100644 src/frc/spline/CubicHermiteSpline.cpp create mode 100644 src/frc/spline/QuinticHermiteSpline.cpp create mode 100644 src/frc/spline/SplineHelper.cpp create mode 100644 src/frc/spline/SplineParameterizer.cpp create mode 100644 src/frc/trajectory/Trajectory.cpp create mode 100644 src/frc/trajectory/TrajectoryGenerator.cpp create mode 100644 src/frc/trajectory/TrajectoryParameterizer.cpp create mode 100644 src/frc/trajectory/constraint/CentripetalAccelerationConstraint.cpp create mode 100644 src/frc/trajectory/constraint/MaxVelocityConstraint.cpp diff --git a/include/frc/Compiler.h b/include/frc/Compiler.h new file mode 100644 index 00000000..121ed541 --- /dev/null +++ b/include/frc/Compiler.h @@ -0,0 +1,601 @@ +//===-- llvm/Support/Compiler.h - Compiler abstraction support --*- C++ -*-===// +// +// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions. +// See https://llvm.org/LICENSE.txt for license information. +// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception +// +//===----------------------------------------------------------------------===// +// +// This file defines several macros, based on the current compiler. This allows +// use of compiler-specific features in a way that remains portable. This header +// can be included from either C or C++. +// +//===----------------------------------------------------------------------===// + +#ifndef WPIUTIL_WPI_COMPILER_H +#define WPIUTIL_WPI_COMPILER_H + + +#include + +#if defined(_MSC_VER) +#include +#endif + +#ifndef __has_feature +# define __has_feature(x) 0 +#endif + +#ifndef __has_extension +# define __has_extension(x) 0 +#endif + +#ifndef __has_attribute +# define __has_attribute(x) 0 +#endif + +#ifndef __has_builtin +# define __has_builtin(x) 0 +#endif + +#ifndef __has_include +# define __has_include(x) 0 +#endif + +// Only use __has_cpp_attribute in C++ mode. GCC defines __has_cpp_attribute in +// C mode, but the :: in __has_cpp_attribute(scoped::attribute) is invalid. +#ifndef LLVM_HAS_CPP_ATTRIBUTE +#if defined(__cplusplus) && defined(__has_cpp_attribute) +# define LLVM_HAS_CPP_ATTRIBUTE(x) __has_cpp_attribute(x) +#else +# define LLVM_HAS_CPP_ATTRIBUTE(x) 0 +#endif +#endif + +/// \macro LLVM_GNUC_PREREQ +/// Extend the default __GNUC_PREREQ even if glibc's features.h isn't +/// available. +#ifndef LLVM_GNUC_PREREQ +# if defined(__GNUC__) && defined(__GNUC_MINOR__) && defined(__GNUC_PATCHLEVEL__) +# define LLVM_GNUC_PREREQ(maj, min, patch) \ + ((__GNUC__ << 20) + (__GNUC_MINOR__ << 10) + __GNUC_PATCHLEVEL__ >= \ + ((maj) << 20) + ((min) << 10) + (patch)) +# elif defined(__GNUC__) && defined(__GNUC_MINOR__) +# define LLVM_GNUC_PREREQ(maj, min, patch) \ + ((__GNUC__ << 20) + (__GNUC_MINOR__ << 10) >= ((maj) << 20) + ((min) << 10)) +# else +# define LLVM_GNUC_PREREQ(maj, min, patch) 0 +# endif +#endif + +/// \macro LLVM_MSC_PREREQ +/// Is the compiler MSVC of at least the specified version? +/// The common \param version values to check for are: +/// * 1910: VS2017, version 15.1 & 15.2 +/// * 1911: VS2017, version 15.3 & 15.4 +/// * 1912: VS2017, version 15.5 +/// * 1913: VS2017, version 15.6 +/// * 1914: VS2017, version 15.7 +/// * 1915: VS2017, version 15.8 +/// * 1916: VS2017, version 15.9 +/// * 1920: VS2019, version 16.0 +/// * 1921: VS2019, version 16.1 +/// * 1922: VS2019, version 16.2 +/// * 1923: VS2019, version 16.3 +/// * 1924: VS2019, version 16.4 +/// * 1925: VS2019, version 16.5 +/// * 1926: VS2019, version 16.6 +/// * 1927: VS2019, version 16.7 +/// * 1928: VS2019, version 16.8 + 16.9 +/// * 1929: VS2019, version 16.10 + 16.11 +/// * 1930: VS2022, version 17.0 +#ifndef LLVM_MSC_PREREQ +#ifdef _MSC_VER +#define LLVM_MSC_PREREQ(version) (_MSC_VER >= (version)) + +// We require at least VS 2019. +#if !defined(LLVM_FORCE_USE_OLD_TOOLCHAIN) +#if !LLVM_MSC_PREREQ(1920) +#error LLVM requires at least VS 2019. +#endif +#endif + +#else +#define LLVM_MSC_PREREQ(version) 0 +#endif +#endif + +/// LLVM_LIBRARY_VISIBILITY - If a class marked with this attribute is linked +/// into a shared library, then the class should be private to the library and +/// not accessible from outside it. Can also be used to mark variables and +/// functions, making them private to any shared library they are linked into. +/// On PE/COFF targets, library visibility is the default, so this isn't needed. +/// +/// LLVM_EXTERNAL_VISIBILITY - classes, functions, and variables marked with +/// this attribute will be made public and visible outside of any shared library +/// they are linked in to. + +#if LLVM_HAS_CPP_ATTRIBUTE(gnu::visibility) +#define LLVM_ATTRIBUTE_VISIBILITY_HIDDEN [[gnu::visibility("hidden")]] +#define LLVM_ATTRIBUTE_VISIBILITY_DEFAULT [[gnu::visibility("default")]] +#elif __has_attribute(visibility) +#define LLVM_ATTRIBUTE_VISIBILITY_HIDDEN __attribute__((visibility("hidden"))) +#define LLVM_ATTRIBUTE_VISIBILITY_DEFAULT __attribute__((visibility("default"))) +#else +#define LLVM_ATTRIBUTE_VISIBILITY_HIDDEN +#define LLVM_ATTRIBUTE_VISIBILITY_DEFAULT +#endif + + +#if (!(defined(_WIN32) || defined(__CYGWIN__)) || \ + (defined(__MINGW32__) && defined(__clang__))) +#define LLVM_LIBRARY_VISIBILITY LLVM_ATTRIBUTE_VISIBILITY_HIDDEN +#if defined(LLVM_BUILD_LLVM_DYLIB) || defined(LLVM_BUILD_SHARED_LIBS) +#define LLVM_EXTERNAL_VISIBILITY LLVM_ATTRIBUTE_VISIBILITY_DEFAULT +#else +#define LLVM_EXTERNAL_VISIBILITY +#endif +#else +#define LLVM_LIBRARY_VISIBILITY +#define LLVM_EXTERNAL_VISIBILITY +#endif + +#ifndef LLVM_PREFETCH +#if defined(__GNUC__) +#define LLVM_PREFETCH(addr, rw, locality) __builtin_prefetch(addr, rw, locality) +#else +#define LLVM_PREFETCH(addr, rw, locality) +#endif +#endif + +#ifndef LLVM_ATTRIBUTE_USED +#if __has_attribute(used) +#define LLVM_ATTRIBUTE_USED __attribute__((__used__)) +#else +#define LLVM_ATTRIBUTE_USED +#endif +#endif + +#if defined(__clang__) +#define LLVM_DEPRECATED(MSG, FIX) __attribute__((deprecated(MSG, FIX))) +#else +#define LLVM_DEPRECATED(MSG, FIX) [[deprecated(MSG)]] +#endif + +// Indicate that a non-static, non-const C++ member function reinitializes +// the entire object to a known state, independent of the previous state of +// the object. +// +// The clang-tidy check bugprone-use-after-move recognizes this attribute as a +// marker that a moved-from object has left the indeterminate state and can be +// reused. +#if LLVM_HAS_CPP_ATTRIBUTE(clang::reinitializes) +#define LLVM_ATTRIBUTE_REINITIALIZES [[clang::reinitializes]] +#else +#define LLVM_ATTRIBUTE_REINITIALIZES +#endif + +// Some compilers warn about unused functions. When a function is sometimes +// used or not depending on build settings (e.g. a function only called from +// within "assert"), this attribute can be used to suppress such warnings. +// +// However, it shouldn't be used for unused *variables*, as those have a much +// more portable solution: +// (void)unused_var_name; +// Prefer cast-to-void wherever it is sufficient. +#ifndef LLVM_ATTRIBUTE_UNUSED +#if __has_attribute(unused) +#define LLVM_ATTRIBUTE_UNUSED __attribute__((__unused__)) +#else +#define LLVM_ATTRIBUTE_UNUSED +#endif +#endif + +// FIXME: Provide this for PE/COFF targets. +#if __has_attribute(weak) && !defined(__MINGW32__) && !defined(__CYGWIN__) && \ + !defined(_WIN32) +#define LLVM_ATTRIBUTE_WEAK __attribute__((__weak__)) +#else +#define LLVM_ATTRIBUTE_WEAK +#endif + +#ifndef LLVM_READNONE +// Prior to clang 3.2, clang did not accept any spelling of +// __has_attribute(const), so assume it is supported. +#if defined(__clang__) || defined(__GNUC__) +// aka 'CONST' but following LLVM Conventions. +#define LLVM_READNONE __attribute__((__const__)) +#else +#define LLVM_READNONE +#endif +#endif + +#ifndef LLVM_READONLY +#if __has_attribute(pure) || defined(__GNUC__) +// aka 'PURE' but following LLVM Conventions. +#define LLVM_READONLY __attribute__((__pure__)) +#else +#define LLVM_READONLY +#endif +#endif + +#if __has_attribute(minsize) +#define LLVM_ATTRIBUTE_MINSIZE __attribute__((minsize)) +#else +#define LLVM_ATTRIBUTE_MINSIZE +#endif + +#ifndef LLVM_LIKELY +#if __has_builtin(__builtin_expect) || defined(__GNUC__) +#define LLVM_LIKELY(EXPR) __builtin_expect((bool)(EXPR), true) +#define LLVM_UNLIKELY(EXPR) __builtin_expect((bool)(EXPR), false) +#else +#define LLVM_LIKELY(EXPR) (EXPR) +#define LLVM_UNLIKELY(EXPR) (EXPR) +#endif +#endif + +/// LLVM_ATTRIBUTE_NOINLINE - On compilers where we have a directive to do so, +/// mark a method "not for inlining". +#ifndef LLVM_ATTRIBUTE_NOINLINE +#if __has_attribute(noinline) +#define LLVM_ATTRIBUTE_NOINLINE __attribute__((noinline)) +#elif defined(_MSC_VER) +#define LLVM_ATTRIBUTE_NOINLINE __declspec(noinline) +#else +#define LLVM_ATTRIBUTE_NOINLINE +#endif +#endif + +/// LLVM_ATTRIBUTE_ALWAYS_INLINE - On compilers where we have a directive to do +/// so, mark a method "always inline" because it is performance sensitive. +#ifndef LLVM_ATTRIBUTE_ALWAYS_INLINE +#if __has_attribute(always_inline) +#define LLVM_ATTRIBUTE_ALWAYS_INLINE inline __attribute__((always_inline)) +#elif defined(_MSC_VER) +#define LLVM_ATTRIBUTE_ALWAYS_INLINE __forceinline +#else +#define LLVM_ATTRIBUTE_ALWAYS_INLINE inline +#endif +#endif + +/// LLVM_ATTRIBUTE_NO_DEBUG - On compilers where we have a directive to do +/// so, mark a method "no debug" because debug info makes the debugger +/// experience worse. +#if __has_attribute(nodebug) +#define LLVM_ATTRIBUTE_NODEBUG __attribute__((nodebug)) +#else +#define LLVM_ATTRIBUTE_NODEBUG +#endif + +#ifndef LLVM_ATTRIBUTE_RETURNS_NONNULL +#if __has_attribute(returns_nonnull) +#define LLVM_ATTRIBUTE_RETURNS_NONNULL __attribute__((returns_nonnull)) +#elif defined(_MSC_VER) +#define LLVM_ATTRIBUTE_RETURNS_NONNULL _Ret_notnull_ +#else +#define LLVM_ATTRIBUTE_RETURNS_NONNULL +#endif +#endif + +/// \macro LLVM_ATTRIBUTE_RETURNS_NOALIAS Used to mark a function as returning a +/// pointer that does not alias any other valid pointer. +#ifndef LLVM_ATTRIBUTE_RETURNS_NOALIAS +#ifdef __GNUC__ +#define LLVM_ATTRIBUTE_RETURNS_NOALIAS __attribute__((__malloc__)) +#elif defined(_MSC_VER) +#define LLVM_ATTRIBUTE_RETURNS_NOALIAS __declspec(restrict) +#else +#define LLVM_ATTRIBUTE_RETURNS_NOALIAS +#endif +#endif + +/// LLVM_FALLTHROUGH - Mark fallthrough cases in switch statements. +#ifndef LLVM_FALLTHROUGH +#if defined(__cplusplus) && __cplusplus > 201402L && LLVM_HAS_CPP_ATTRIBUTE(fallthrough) +#define LLVM_FALLTHROUGH [[fallthrough]] +#elif LLVM_HAS_CPP_ATTRIBUTE(gnu::fallthrough) +#define LLVM_FALLTHROUGH [[gnu::fallthrough]] +#elif __has_attribute(fallthrough) +#define LLVM_FALLTHROUGH __attribute__((fallthrough)) +#elif LLVM_HAS_CPP_ATTRIBUTE(clang::fallthrough) +#define LLVM_FALLTHROUGH [[clang::fallthrough]] +#else +#define LLVM_FALLTHROUGH +#endif +#endif + +/// LLVM_REQUIRE_CONSTANT_INITIALIZATION - Apply this to globals to ensure that +/// they are constant initialized. +#if LLVM_HAS_CPP_ATTRIBUTE(clang::require_constant_initialization) +#define LLVM_REQUIRE_CONSTANT_INITIALIZATION \ + [[clang::require_constant_initialization]] +#else +#define LLVM_REQUIRE_CONSTANT_INITIALIZATION +#endif + +/// LLVM_GSL_OWNER - Apply this to owning classes like SmallVector to enable +/// lifetime warnings. +#if LLVM_HAS_CPP_ATTRIBUTE(gsl::Owner) +#define LLVM_GSL_OWNER [[gsl::Owner]] +#else +#define LLVM_GSL_OWNER +#endif + +/// LLVM_GSL_POINTER - Apply this to non-owning classes like +/// std::string_view to enable lifetime warnings. +#if LLVM_HAS_CPP_ATTRIBUTE(gsl::Pointer) +#define LLVM_GSL_POINTER [[gsl::Pointer]] +#else +#define LLVM_GSL_POINTER +#endif + +/// LLVM_EXTENSION - Support compilers where we have a keyword to suppress +/// pedantic diagnostics. +#ifndef LLVM_EXTENSION +#ifdef __GNUC__ +#define LLVM_EXTENSION __extension__ +#else +#define LLVM_EXTENSION +#endif +#endif + +/// LLVM_BUILTIN_UNREACHABLE - On compilers which support it, expands +/// to an expression which states that it is undefined behavior for the +/// compiler to reach this point. Otherwise is not defined. +/// +/// '#else' is intentionally left out so that other macro logic (e.g., +/// LLVM_ASSUME_ALIGNED and wpi_unreachable()) can detect whether +/// LLVM_BUILTIN_UNREACHABLE has a definition. +#ifndef LLVM_BUILTIN_UNREACHABLE +#if __has_builtin(__builtin_unreachable) || defined(__GNUC__) +# define LLVM_BUILTIN_UNREACHABLE __builtin_unreachable() +#elif defined(_MSC_VER) +# define LLVM_BUILTIN_UNREACHABLE __assume(false) +#endif +#endif + +/// LLVM_BUILTIN_TRAP - On compilers which support it, expands to an expression +/// which causes the program to exit abnormally. +#ifndef LLVM_BUILTIN_TRAP +#if __has_builtin(__builtin_trap) || defined(__GNUC__) +# define LLVM_BUILTIN_TRAP __builtin_trap() +#elif defined(_MSC_VER) +// The __debugbreak intrinsic is supported by MSVC, does not require forward +// declarations involving platform-specific typedefs (unlike RaiseException), +// results in a call to vectored exception handlers, and encodes to a short +// instruction that still causes the trapping behavior we want. +# define LLVM_BUILTIN_TRAP __debugbreak() +#else +# define LLVM_BUILTIN_TRAP *(volatile int*)0x11 = 0 +#endif +#endif + +/// LLVM_BUILTIN_DEBUGTRAP - On compilers which support it, expands to +/// an expression which causes the program to break while running +/// under a debugger. +#ifndef LLVM_BUILTIN_DEBUGTRAP +#if __has_builtin(__builtin_debugtrap) +# define LLVM_BUILTIN_DEBUGTRAP __builtin_debugtrap() +#elif defined(_MSC_VER) +// The __debugbreak intrinsic is supported by MSVC and breaks while +// running under the debugger, and also supports invoking a debugger +// when the OS is configured appropriately. +# define LLVM_BUILTIN_DEBUGTRAP __debugbreak() +#else +// Just continue execution when built with compilers that have no +// support. This is a debugging aid and not intended to force the +// program to abort if encountered. +# define LLVM_BUILTIN_DEBUGTRAP +#endif +#endif + +/// \macro LLVM_ASSUME_ALIGNED +/// Returns a pointer with an assumed alignment. +#ifndef LLVM_ASSUME_ALIGNED +#if __has_builtin(__builtin_assume_aligned) || defined(__GNUC__) +# define LLVM_ASSUME_ALIGNED(p, a) __builtin_assume_aligned(p, a) +#elif defined(LLVM_BUILTIN_UNREACHABLE) +# define LLVM_ASSUME_ALIGNED(p, a) \ + (((uintptr_t(p) % (a)) == 0) ? (p) : (LLVM_BUILTIN_UNREACHABLE, (p))) +#else +# define LLVM_ASSUME_ALIGNED(p, a) (p) +#endif +#endif + +/// \macro LLVM_PACKED +/// Used to specify a packed structure. +/// LLVM_PACKED( +/// struct A { +/// int i; +/// int j; +/// int k; +/// long long l; +/// }); +/// +/// LLVM_PACKED_START +/// struct B { +/// int i; +/// int j; +/// int k; +/// long long l; +/// }; +/// LLVM_PACKED_END +#ifndef LLVM_PACKED +#ifdef _MSC_VER +# define LLVM_PACKED(d) __pragma(pack(push, 1)) d __pragma(pack(pop)) +# define LLVM_PACKED_START __pragma(pack(push, 1)) +# define LLVM_PACKED_END __pragma(pack(pop)) +#else +# define LLVM_PACKED(d) d __attribute__((packed)) +# define LLVM_PACKED_START _Pragma("pack(push, 1)") +# define LLVM_PACKED_END _Pragma("pack(pop)") +#endif +#endif + +/// \macro LLVM_MEMORY_SANITIZER_BUILD +/// Whether LLVM itself is built with MemorySanitizer instrumentation. +#if __has_feature(memory_sanitizer) +# define LLVM_MEMORY_SANITIZER_BUILD 1 +# include +# define LLVM_NO_SANITIZE_MEMORY_ATTRIBUTE __attribute__((no_sanitize_memory)) +#else +# define LLVM_MEMORY_SANITIZER_BUILD 0 +# define __msan_allocated_memory(p, size) +# define __msan_unpoison(p, size) +# define LLVM_NO_SANITIZE_MEMORY_ATTRIBUTE +#endif + +/// \macro LLVM_ADDRESS_SANITIZER_BUILD +/// Whether LLVM itself is built with AddressSanitizer instrumentation. +#if __has_feature(address_sanitizer) || defined(__SANITIZE_ADDRESS__) +# define LLVM_ADDRESS_SANITIZER_BUILD 1 +#if __has_include() +# include +#else +// These declarations exist to support ASan with MSVC. If MSVC eventually ships +// asan_interface.h in their headers, then we can remove this. +#ifdef __cplusplus +extern "C" { +#endif +void __asan_poison_memory_region(void const volatile *addr, size_t size); +void __asan_unpoison_memory_region(void const volatile *addr, size_t size); +#ifdef __cplusplus +} // extern "C" +#endif +#endif +#else +# define LLVM_ADDRESS_SANITIZER_BUILD 0 +# define __asan_poison_memory_region(p, size) +# define __asan_unpoison_memory_region(p, size) +#endif + +/// \macro LLVM_HWADDRESS_SANITIZER_BUILD +/// Whether LLVM itself is built with HWAddressSanitizer instrumentation. +#if __has_feature(hwaddress_sanitizer) +#define LLVM_HWADDRESS_SANITIZER_BUILD 1 +#else +#define LLVM_HWADDRESS_SANITIZER_BUILD 0 +#endif + +/// \macro LLVM_THREAD_SANITIZER_BUILD +/// Whether LLVM itself is built with ThreadSanitizer instrumentation. +#if __has_feature(thread_sanitizer) || defined(__SANITIZE_THREAD__) +# define LLVM_THREAD_SANITIZER_BUILD 1 +#else +# define LLVM_THREAD_SANITIZER_BUILD 0 +#endif + +#if LLVM_THREAD_SANITIZER_BUILD +// Thread Sanitizer is a tool that finds races in code. +// See http://code.google.com/p/data-race-test/wiki/DynamicAnnotations . +// tsan detects these exact functions by name. +#ifdef __cplusplus +extern "C" { +#endif +void AnnotateHappensAfter(const char *file, int line, const volatile void *cv); +void AnnotateHappensBefore(const char *file, int line, const volatile void *cv); +void AnnotateIgnoreWritesBegin(const char *file, int line); +void AnnotateIgnoreWritesEnd(const char *file, int line); +#ifdef __cplusplus +} +#endif + +// This marker is used to define a happens-before arc. The race detector will +// infer an arc from the begin to the end when they share the same pointer +// argument. +# define TsanHappensBefore(cv) AnnotateHappensBefore(__FILE__, __LINE__, cv) + +// This marker defines the destination of a happens-before arc. +# define TsanHappensAfter(cv) AnnotateHappensAfter(__FILE__, __LINE__, cv) + +// Ignore any races on writes between here and the next TsanIgnoreWritesEnd. +# define TsanIgnoreWritesBegin() AnnotateIgnoreWritesBegin(__FILE__, __LINE__) + +// Resume checking for racy writes. +# define TsanIgnoreWritesEnd() AnnotateIgnoreWritesEnd(__FILE__, __LINE__) +#else +# define TsanHappensBefore(cv) +# define TsanHappensAfter(cv) +# define TsanIgnoreWritesBegin() +# define TsanIgnoreWritesEnd() +#endif + +/// \macro LLVM_NO_SANITIZE +/// Disable a particular sanitizer for a function. +#ifndef LLVM_NO_SANITIZE +#if __has_attribute(no_sanitize) +#define LLVM_NO_SANITIZE(KIND) __attribute__((no_sanitize(KIND))) +#else +#define LLVM_NO_SANITIZE(KIND) +#endif +#endif + +/// Mark debug helper function definitions like dump() that should not be +/// stripped from debug builds. +/// Note that you should also surround dump() functions with +/// `#if !defined(NDEBUG) || defined(LLVM_ENABLE_DUMP)` so they do always +/// get stripped in release builds. +// FIXME: Move this to a private config.h as it's not usable in public headers. +#ifndef LLVM_DUMP_METHOD +#if !defined(NDEBUG) || defined(LLVM_ENABLE_DUMP) +#define LLVM_DUMP_METHOD LLVM_ATTRIBUTE_NOINLINE LLVM_ATTRIBUTE_USED +#else +#define LLVM_DUMP_METHOD LLVM_ATTRIBUTE_NOINLINE +#endif +#endif + +/// \macro LLVM_PRETTY_FUNCTION +/// Gets a user-friendly looking function signature for the current scope +/// using the best available method on each platform. The exact format of the +/// resulting string is implementation specific and non-portable, so this should +/// only be used, for example, for logging or diagnostics. +#ifndef LLVM_PRETTY_FUNCTION +#if defined(_MSC_VER) +#define LLVM_PRETTY_FUNCTION __FUNCSIG__ +#elif defined(__GNUC__) || defined(__clang__) +#define LLVM_PRETTY_FUNCTION __PRETTY_FUNCTION__ +#else +#define LLVM_PRETTY_FUNCTION __func__ +#endif +#endif + +/// \macro LLVM_THREAD_LOCAL +/// A thread-local storage specifier which can be used with globals, +/// extern globals, and static globals. +/// +/// This is essentially an extremely restricted analog to C++11's thread_local +/// support. It uses thread_local if available, falling back on gcc __thread +/// if not. __thread doesn't support many of the C++11 thread_local's +/// features. You should only use this for PODs that you can statically +/// initialize to some constant value. In almost all circumstances this is most +/// appropriate for use with a pointer, integer, or small aggregation of +/// pointers and integers. +#if __has_feature(cxx_thread_local) || defined(_MSC_VER) +#define LLVM_THREAD_LOCAL thread_local +#else +// Clang, GCC, and other compatible compilers used __thread prior to C++11 and +// we only need the restricted functionality that provides. +#define LLVM_THREAD_LOCAL __thread +#endif + +/// \macro LLVM_ENABLE_EXCEPTIONS +/// Whether LLVM is built with exception support. +#if __has_feature(cxx_exceptions) +#define LLVM_ENABLE_EXCEPTIONS 1 +#elif defined(__GNUC__) && defined(__EXCEPTIONS) +#define LLVM_ENABLE_EXCEPTIONS 1 +#elif defined(_MSC_VER) && defined(_CPPUNWIND) +#define LLVM_ENABLE_EXCEPTIONS 1 +#endif + +/// \macro LLVM_NO_PROFILE_INSTRUMENT_FUNCTION +/// Disable the profile instrument for a function. +#if __has_attribute(no_profile_instrument_function) +#define LLVM_NO_PROFILE_INSTRUMENT_FUNCTION \ + __attribute__((no_profile_instrument_function)) +#else +#define LLVM_NO_PROFILE_INSTRUMENT_FUNCTION +#endif + +#endif diff --git a/include/frc/EigenCore.h b/include/frc/EigenCore.h new file mode 100644 index 00000000..1ed56685 --- /dev/null +++ b/include/frc/EigenCore.h @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +// #include +#include "frc/thirdparty/eigen/include/Eigen/Core" + +namespace frc { + +template +using Vectord = Eigen::Vector; + +template +using Matrixd = Eigen::Matrix; + +} // namespace frc diff --git a/include/frc/MathExtras.h b/include/frc/MathExtras.h new file mode 100644 index 00000000..cf6c80ee --- /dev/null +++ b/include/frc/MathExtras.h @@ -0,0 +1,643 @@ +//===-- llvm/Support/MathExtras.h - Useful math functions -------*- C++ -*-===// +// +// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions. +// See https://llvm.org/LICENSE.txt for license information. +// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception +// +//===----------------------------------------------------------------------===// +// +// This file contains some functions that are useful for math stuff. +// +//===----------------------------------------------------------------------===// + +#ifndef WPIUTIL_WPI_MATHEXTRAS_H +#define WPIUTIL_WPI_MATHEXTRAS_H + +#include "frc/bit.h" +#include "frc/Compiler.h" +#include +#include +#include +#include +#include +#include +#include + +namespace wpi { + +/// Create a bitmask with the N right-most bits set to 1, and all other +/// bits set to 0. Only unsigned types are allowed. +template T maskTrailingOnes(unsigned N) { + static_assert(std::is_unsigned_v, "Invalid type!"); + const unsigned Bits = CHAR_BIT * sizeof(T); + assert(N <= Bits && "Invalid bit index"); + return N == 0 ? 0 : (T(-1) >> (Bits - N)); +} + +/// Create a bitmask with the N left-most bits set to 1, and all other +/// bits set to 0. Only unsigned types are allowed. +template T maskLeadingOnes(unsigned N) { + return ~maskTrailingOnes(CHAR_BIT * sizeof(T) - N); +} + +/// Create a bitmask with the N right-most bits set to 0, and all other +/// bits set to 1. Only unsigned types are allowed. +template T maskTrailingZeros(unsigned N) { + return maskLeadingOnes(CHAR_BIT * sizeof(T) - N); +} + +/// Create a bitmask with the N left-most bits set to 0, and all other +/// bits set to 1. Only unsigned types are allowed. +template T maskLeadingZeros(unsigned N) { + return maskTrailingOnes(CHAR_BIT * sizeof(T) - N); +} + +/// Macro compressed bit reversal table for 256 bits. +/// +/// http://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable +static const unsigned char BitReverseTable256[256] = { +#define R2(n) n, n + 2 * 64, n + 1 * 64, n + 3 * 64 +#define R4(n) R2(n), R2(n + 2 * 16), R2(n + 1 * 16), R2(n + 3 * 16) +#define R6(n) R4(n), R4(n + 2 * 4), R4(n + 1 * 4), R4(n + 3 * 4) + R6(0), R6(2), R6(1), R6(3) +#undef R2 +#undef R4 +#undef R6 +}; + +/// Reverse the bits in \p Val. +template T reverseBits(T Val) { +#if __has_builtin(__builtin_bitreverse8) + if constexpr (std::is_same_v) + return __builtin_bitreverse8(Val); +#endif +#if __has_builtin(__builtin_bitreverse16) + if constexpr (std::is_same_v) + return __builtin_bitreverse16(Val); +#endif +#if __has_builtin(__builtin_bitreverse32) + if constexpr (std::is_same_v) + return __builtin_bitreverse32(Val); +#endif +#if __has_builtin(__builtin_bitreverse64) + if constexpr (std::is_same_v) + return __builtin_bitreverse64(Val); +#endif + + unsigned char in[sizeof(Val)]; + unsigned char out[sizeof(Val)]; + std::memcpy(in, &Val, sizeof(Val)); + for (unsigned i = 0; i < sizeof(Val); ++i) + out[(sizeof(Val) - i) - 1] = BitReverseTable256[in[i]]; + std::memcpy(&Val, out, sizeof(Val)); + return Val; +} + +// NOTE: The following support functions use the _32/_64 extensions instead of +// type overloading so that signed and unsigned integers can be used without +// ambiguity. + +/// Return the high 32 bits of a 64 bit value. +constexpr inline uint32_t Hi_32(uint64_t Value) { + return static_cast(Value >> 32); +} + +/// Return the low 32 bits of a 64 bit value. +constexpr inline uint32_t Lo_32(uint64_t Value) { + return static_cast(Value); +} + +/// Make a 64-bit integer from a high / low pair of 32-bit integers. +constexpr inline uint64_t Make_64(uint32_t High, uint32_t Low) { + return ((uint64_t)High << 32) | (uint64_t)Low; +} + +/// Checks if an integer fits into the given bit width. +template constexpr inline bool isInt(int64_t x) { + if constexpr (N == 8) + return static_cast(x) == x; + if constexpr (N == 16) + return static_cast(x) == x; + if constexpr (N == 32) + return static_cast(x) == x; + if constexpr (N < 64) + return -(INT64_C(1) << (N - 1)) <= x && x < (INT64_C(1) << (N - 1)); + (void)x; // MSVC v19.25 warns that x is unused. + return true; +} + +/// Checks if a signed integer is an N bit number shifted left by S. +template +constexpr inline bool isShiftedInt(int64_t x) { + static_assert( + N > 0, "isShiftedInt<0> doesn't make sense (refers to a 0-bit number."); + static_assert(N + S <= 64, "isShiftedInt with N + S > 64 is too wide."); + return isInt(x) && (x % (UINT64_C(1) << S) == 0); +} + +/// Checks if an unsigned integer fits into the given bit width. +template constexpr inline bool isUInt(uint64_t x) { + static_assert(N > 0, "isUInt<0> doesn't make sense"); + if constexpr (N == 8) + return static_cast(x) == x; + if constexpr (N == 16) + return static_cast(x) == x; + if constexpr (N == 32) + return static_cast(x) == x; + if constexpr (N < 64) + return x < (UINT64_C(1) << (N)); + (void)x; // MSVC v19.25 warns that x is unused. + return true; +} + +/// Checks if a unsigned integer is an N bit number shifted left by S. +template +constexpr inline bool isShiftedUInt(uint64_t x) { + static_assert( + N > 0, "isShiftedUInt<0> doesn't make sense (refers to a 0-bit number)"); + static_assert(N + S <= 64, + "isShiftedUInt with N + S > 64 is too wide."); + // Per the two static_asserts above, S must be strictly less than 64. So + // 1 << S is not undefined behavior. + return isUInt(x) && (x % (UINT64_C(1) << S) == 0); +} + +/// Gets the maximum value for a N-bit unsigned integer. +inline uint64_t maxUIntN(uint64_t N) { + assert(N > 0 && N <= 64 && "integer width out of range"); + + // uint64_t(1) << 64 is undefined behavior, so we can't do + // (uint64_t(1) << N) - 1 + // without checking first that N != 64. But this works and doesn't have a + // branch. + return UINT64_MAX >> (64 - N); +} + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4146) +#endif + +/// Gets the minimum value for a N-bit signed integer. +inline int64_t minIntN(int64_t N) { + assert(N > 0 && N <= 64 && "integer width out of range"); + + return UINT64_C(1) + ~(UINT64_C(1) << (N - 1)); +} + +#ifdef _WIN32 +#pragma warning(pop) +#endif + +/// Gets the maximum value for a N-bit signed integer. +inline int64_t maxIntN(int64_t N) { + assert(N > 0 && N <= 64 && "integer width out of range"); + + // This relies on two's complement wraparound when N == 64, so we convert to + // int64_t only at the very end to avoid UB. + return (UINT64_C(1) << (N - 1)) - 1; +} + +/// Checks if an unsigned integer fits into the given (dynamic) bit width. +inline bool isUIntN(unsigned N, uint64_t x) { + return N >= 64 || x <= maxUIntN(N); +} + +/// Checks if an signed integer fits into the given (dynamic) bit width. +inline bool isIntN(unsigned N, int64_t x) { + return N >= 64 || (minIntN(N) <= x && x <= maxIntN(N)); +} + +/// Return true if the argument is a non-empty sequence of ones starting at the +/// least significant bit with the remainder zero (32 bit version). +/// Ex. isMask_32(0x0000FFFFU) == true. +constexpr inline bool isMask_32(uint32_t Value) { + return Value && ((Value + 1) & Value) == 0; +} + +/// Return true if the argument is a non-empty sequence of ones starting at the +/// least significant bit with the remainder zero (64 bit version). +constexpr inline bool isMask_64(uint64_t Value) { + return Value && ((Value + 1) & Value) == 0; +} + +/// Return true if the argument contains a non-empty sequence of ones with the +/// remainder zero (32 bit version.) Ex. isShiftedMask_32(0x0000FF00U) == true. +constexpr inline bool isShiftedMask_32(uint32_t Value) { + return Value && isMask_32((Value - 1) | Value); +} + +/// Return true if the argument contains a non-empty sequence of ones with the +/// remainder zero (64 bit version.) +constexpr inline bool isShiftedMask_64(uint64_t Value) { + return Value && isMask_64((Value - 1) | Value); +} + +/// Return true if the argument is a power of two > 0. +/// Ex. isPowerOf2_32(0x00100000U) == true (32 bit edition.) +constexpr inline bool isPowerOf2_32(uint32_t Value) { + return std::has_single_bit(Value); +} + +/// Return true if the argument is a power of two > 0 (64 bit edition.) +constexpr inline bool isPowerOf2_64(uint64_t Value) { + return std::has_single_bit(Value); +} + +/// Return true if the argument contains a non-empty sequence of ones with the +/// remainder zero (32 bit version.) Ex. isShiftedMask_32(0x0000FF00U) == true. +/// If true, \p MaskIdx will specify the index of the lowest set bit and \p +/// MaskLen is updated to specify the length of the mask, else neither are +/// updated. +inline bool isShiftedMask_32(uint32_t Value, unsigned &MaskIdx, + unsigned &MaskLen) { + if (!isShiftedMask_32(Value)) + return false; + MaskIdx = std::countr_zero(Value); + MaskLen = std::popcount(Value); + return true; +} + +/// Return true if the argument contains a non-empty sequence of ones with the +/// remainder zero (64 bit version.) If true, \p MaskIdx will specify the index +/// of the lowest set bit and \p MaskLen is updated to specify the length of the +/// mask, else neither are updated. +inline bool isShiftedMask_64(uint64_t Value, unsigned &MaskIdx, + unsigned &MaskLen) { + if (!isShiftedMask_64(Value)) + return false; + MaskIdx = std::countr_zero(Value); + MaskLen = std::popcount(Value); + return true; +} + +/// Compile time Log2. +/// Valid only for positive powers of two. +template constexpr inline size_t CTLog2() { + static_assert(kValue > 0 && wpi::isPowerOf2_64(kValue), + "Value is not a valid power of 2"); + return 1 + CTLog2(); +} + +template <> constexpr inline size_t CTLog2<1>() { return 0; } + +/// Return the floor log base 2 of the specified value, -1 if the value is zero. +/// (32 bit edition.) +/// Ex. Log2_32(32) == 5, Log2_32(1) == 0, Log2_32(0) == -1, Log2_32(6) == 2 +inline unsigned Log2_32(uint32_t Value) { + return static_cast(31 - std::countl_zero(Value)); +} + +/// Return the floor log base 2 of the specified value, -1 if the value is zero. +/// (64 bit edition.) +inline unsigned Log2_64(uint64_t Value) { + return static_cast(63 - std::countl_zero(Value)); +} + +/// Return the ceil log base 2 of the specified value, 32 if the value is zero. +/// (32 bit edition). +/// Ex. Log2_32_Ceil(32) == 5, Log2_32_Ceil(1) == 0, Log2_32_Ceil(6) == 3 +inline unsigned Log2_32_Ceil(uint32_t Value) { + return static_cast(32 - std::countl_zero(Value - 1)); +} + +/// Return the ceil log base 2 of the specified value, 64 if the value is zero. +/// (64 bit edition.) +inline unsigned Log2_64_Ceil(uint64_t Value) { + return static_cast(64 - std::countl_zero(Value - 1)); +} + +/// A and B are either alignments or offsets. Return the minimum alignment that +/// may be assumed after adding the two together. +constexpr inline uint64_t MinAlign(uint64_t A, uint64_t B) { + // The largest power of 2 that divides both A and B. + // + // Replace "-Value" by "1+~Value" in the following commented code to avoid + // MSVC warning C4146 + // return (A | B) & -(A | B); + return (A | B) & (1 + ~(A | B)); +} + +/// Returns the next power of two (in 64-bits) that is strictly greater than A. +/// Returns zero on overflow. +constexpr inline uint64_t NextPowerOf2(uint64_t A) { + A |= (A >> 1); + A |= (A >> 2); + A |= (A >> 4); + A |= (A >> 8); + A |= (A >> 16); + A |= (A >> 32); + return A + 1; +} + +/// Returns the power of two which is greater than or equal to the given value. +/// Essentially, it is a ceil operation across the domain of powers of two. +inline uint64_t PowerOf2Ceil(uint64_t A) { + if (!A) + return 0; + return NextPowerOf2(A - 1); +} + +/// Returns the next integer (mod 2**64) that is greater than or equal to +/// \p Value and is a multiple of \p Align. \p Align must be non-zero. +/// +/// Examples: +/// \code +/// alignTo(5, 8) = 8 +/// alignTo(17, 8) = 24 +/// alignTo(~0LL, 8) = 0 +/// alignTo(321, 255) = 510 +/// \endcode +inline uint64_t alignTo(uint64_t Value, uint64_t Align) { + assert(Align != 0u && "Align can't be 0."); + return (Value + Align - 1) / Align * Align; +} + +inline uint64_t alignToPowerOf2(uint64_t Value, uint64_t Align) { + assert(Align != 0 && (Align & (Align - 1)) == 0 && + "Align must be a power of 2"); + // Replace unary minus to avoid compilation error on Windows: + // "unary minus operator applied to unsigned type, result still unsigned" + uint64_t negAlign = (~Align) + 1; + return (Value + Align - 1) & negAlign; +} + +/// If non-zero \p Skew is specified, the return value will be a minimal integer +/// that is greater than or equal to \p Size and equal to \p A * N + \p Skew for +/// some integer N. If \p Skew is larger than \p A, its value is adjusted to '\p +/// Skew mod \p A'. \p Align must be non-zero. +/// +/// Examples: +/// \code +/// alignTo(5, 8, 7) = 7 +/// alignTo(17, 8, 1) = 17 +/// alignTo(~0LL, 8, 3) = 3 +/// alignTo(321, 255, 42) = 552 +/// \endcode +inline uint64_t alignTo(uint64_t Value, uint64_t Align, uint64_t Skew) { + assert(Align != 0u && "Align can't be 0."); + Skew %= Align; + return alignTo(Value - Skew, Align) + Skew; +} + +/// Returns the next integer (mod 2**64) that is greater than or equal to +/// \p Value and is a multiple of \c Align. \c Align must be non-zero. +template constexpr inline uint64_t alignTo(uint64_t Value) { + static_assert(Align != 0u, "Align must be non-zero"); + return (Value + Align - 1) / Align * Align; +} + +/// Returns the integer ceil(Numerator / Denominator). +inline uint64_t divideCeil(uint64_t Numerator, uint64_t Denominator) { + return alignTo(Numerator, Denominator) / Denominator; +} + +/// Returns the integer nearest(Numerator / Denominator). +inline uint64_t divideNearest(uint64_t Numerator, uint64_t Denominator) { + return (Numerator + (Denominator / 2)) / Denominator; +} + +/// Returns the largest uint64_t less than or equal to \p Value and is +/// \p Skew mod \p Align. \p Align must be non-zero +inline uint64_t alignDown(uint64_t Value, uint64_t Align, uint64_t Skew = 0) { + assert(Align != 0u && "Align can't be 0."); + Skew %= Align; + return (Value - Skew) / Align * Align + Skew; +} + +/// Sign-extend the number in the bottom B bits of X to a 32-bit integer. +/// Requires 0 < B <= 32. +template constexpr inline int32_t SignExtend32(uint32_t X) { + static_assert(B > 0, "Bit width can't be 0."); + static_assert(B <= 32, "Bit width out of range."); + return int32_t(X << (32 - B)) >> (32 - B); +} + +/// Sign-extend the number in the bottom B bits of X to a 32-bit integer. +/// Requires 0 < B <= 32. +inline int32_t SignExtend32(uint32_t X, unsigned B) { + assert(B > 0 && "Bit width can't be 0."); + assert(B <= 32 && "Bit width out of range."); + return int32_t(X << (32 - B)) >> (32 - B); +} + +/// Sign-extend the number in the bottom B bits of X to a 64-bit integer. +/// Requires 0 < B <= 64. +template constexpr inline int64_t SignExtend64(uint64_t x) { + static_assert(B > 0, "Bit width can't be 0."); + static_assert(B <= 64, "Bit width out of range."); + return int64_t(x << (64 - B)) >> (64 - B); +} + +/// Sign-extend the number in the bottom B bits of X to a 64-bit integer. +/// Requires 0 < B <= 64. +inline int64_t SignExtend64(uint64_t X, unsigned B) { + assert(B > 0 && "Bit width can't be 0."); + assert(B <= 64 && "Bit width out of range."); + return int64_t(X << (64 - B)) >> (64 - B); +} + +/// Subtract two unsigned integers, X and Y, of type T and return the absolute +/// value of the result. +template +std::enable_if_t, T> AbsoluteDifference(T X, T Y) { + return X > Y ? (X - Y) : (Y - X); +} + +/// Add two unsigned integers, X and Y, of type T. Clamp the result to the +/// maximum representable value of T on overflow. ResultOverflowed indicates if +/// the result is larger than the maximum representable value of type T. +template +std::enable_if_t, T> +SaturatingAdd(T X, T Y, bool *ResultOverflowed = nullptr) { + bool Dummy; + bool &Overflowed = ResultOverflowed ? *ResultOverflowed : Dummy; + // Hacker's Delight, p. 29 + T Z = X + Y; + Overflowed = (Z < X || Z < Y); + if (Overflowed) + return (std::numeric_limits::max)(); + else + return Z; +} + +/// Add multiple unsigned integers of type T. Clamp the result to the +/// maximum representable value of T on overflow. +template +std::enable_if_t, T> SaturatingAdd(T X, T Y, T Z, + Ts... Args) { + bool Overflowed = false; + T XY = SaturatingAdd(X, Y, &Overflowed); + if (Overflowed) + return SaturatingAdd((std::numeric_limits::max)(), T(1), Args...); + return SaturatingAdd(XY, Z, Args...); +} + +/// Multiply two unsigned integers, X and Y, of type T. Clamp the result to the +/// maximum representable value of T on overflow. ResultOverflowed indicates if +/// the result is larger than the maximum representable value of type T. +template +std::enable_if_t, T> +SaturatingMultiply(T X, T Y, bool *ResultOverflowed = nullptr) { + bool Dummy; + bool &Overflowed = ResultOverflowed ? *ResultOverflowed : Dummy; + + // Hacker's Delight, p. 30 has a different algorithm, but we don't use that + // because it fails for uint16_t (where multiplication can have undefined + // behavior due to promotion to int), and requires a division in addition + // to the multiplication. + + Overflowed = false; + + // Log2(Z) would be either Log2Z or Log2Z + 1. + // Special case: if X or Y is 0, Log2_64 gives -1, and Log2Z + // will necessarily be less than Log2Max as desired. + int Log2Z = Log2_64(X) + Log2_64(Y); + const T Max = (std::numeric_limits::max)(); + int Log2Max = Log2_64(Max); + if (Log2Z < Log2Max) { + return X * Y; + } + if (Log2Z > Log2Max) { + Overflowed = true; + return Max; + } + + // We're going to use the top bit, and maybe overflow one + // bit past it. Multiply all but the bottom bit then add + // that on at the end. + T Z = (X >> 1) * Y; + if (Z & ~(Max >> 1)) { + Overflowed = true; + return Max; + } + Z <<= 1; + if (X & 1) + return SaturatingAdd(Z, Y, ResultOverflowed); + + return Z; +} + +/// Multiply two unsigned integers, X and Y, and add the unsigned integer, A to +/// the product. Clamp the result to the maximum representable value of T on +/// overflow. ResultOverflowed indicates if the result is larger than the +/// maximum representable value of type T. +template +std::enable_if_t, T> +SaturatingMultiplyAdd(T X, T Y, T A, bool *ResultOverflowed = nullptr) { + bool Dummy; + bool &Overflowed = ResultOverflowed ? *ResultOverflowed : Dummy; + + T Product = SaturatingMultiply(X, Y, &Overflowed); + if (Overflowed) + return Product; + + return SaturatingAdd(A, Product, &Overflowed); +} + +/// Use this rather than HUGE_VALF; the latter causes warnings on MSVC. +extern const float huge_valf; + + +/// Add two signed integers, computing the two's complement truncated result, +/// returning true if overflow occurred. +template +std::enable_if_t, T> AddOverflow(T X, T Y, T &Result) { +#if __has_builtin(__builtin_add_overflow) + return __builtin_add_overflow(X, Y, &Result); +#else + // Perform the unsigned addition. + using U = std::make_unsigned_t; + const U UX = static_cast(X); + const U UY = static_cast(Y); + const U UResult = UX + UY; + + // Convert to signed. + Result = static_cast(UResult); + + // Adding two positive numbers should result in a positive number. + if (X > 0 && Y > 0) + return Result <= 0; + // Adding two negatives should result in a negative number. + if (X < 0 && Y < 0) + return Result >= 0; + return false; +#endif +} + +/// Subtract two signed integers, computing the two's complement truncated +/// result, returning true if an overflow ocurred. +template +std::enable_if_t, T> SubOverflow(T X, T Y, T &Result) { +#if __has_builtin(__builtin_sub_overflow) + return __builtin_sub_overflow(X, Y, &Result); +#else + // Perform the unsigned addition. + using U = std::make_unsigned_t; + const U UX = static_cast(X); + const U UY = static_cast(Y); + const U UResult = UX - UY; + + // Convert to signed. + Result = static_cast(UResult); + + // Subtracting a positive number from a negative results in a negative number. + if (X <= 0 && Y > 0) + return Result >= 0; + // Subtracting a negative number from a positive results in a positive number. + if (X >= 0 && Y < 0) + return Result <= 0; + return false; +#endif +} + +/// Multiply two signed integers, computing the two's complement truncated +/// result, returning true if an overflow ocurred. +template +std::enable_if_t, T> MulOverflow(T X, T Y, T &Result) { + // Perform the unsigned multiplication on absolute values. + using U = std::make_unsigned_t; + const U UX = X < 0 ? (0 - static_cast(X)) : static_cast(X); + const U UY = Y < 0 ? (0 - static_cast(Y)) : static_cast(Y); + const U UResult = UX * UY; + + // Convert to signed. + const bool IsNegative = (X < 0) ^ (Y < 0); + Result = IsNegative ? (0 - UResult) : UResult; + + // If any of the args was 0, result is 0 and no overflow occurs. + if (UX == 0 || UY == 0) + return false; + + // UX and UY are in [1, 2^n], where n is the number of digits. + // Check how the max allowed absolute value (2^n for negative, 2^(n-1) for + // positive) divided by an argument compares to the other. + if (IsNegative) + return UX > (static_cast((std::numeric_limits::max)()) + U(1)) / UY; + else + return UX > (static_cast((std::numeric_limits::max)())) / UY; +} + +// Typesafe implementation of the signum function. +// Returns -1 if negative, 1 if positive, 0 if 0. +template +constexpr int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +/** + * Linearly interpolates between two values. + * + * @param startValue The start value. + * @param endValue The end value. + * @param t The fraction for interpolation. + * + * @return The interpolated value. + */ +template +constexpr T Lerp(const T& startValue, const T& endValue, double t) { + return startValue + (endValue - startValue) * t; +} +} // End wpi namespace + +#endif diff --git a/include/frc/MathUtil.h b/include/frc/MathUtil.h new file mode 100644 index 00000000..a1da9672 --- /dev/null +++ b/include/frc/MathUtil.h @@ -0,0 +1,172 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "trajopt/SymbolExports.h" + +#include "units/angle.h" +#include "units/base.h" +#include "units/math.h" + +namespace frc { + +/** + * Returns 0.0 if the given value is within the specified range around zero. The + * remaining range between the deadband and the maximum magnitude is scaled from + * 0.0 to the maximum magnitude. + * + * @param value Value to clip. + * @param deadband Range around zero. + * @param maxMagnitude The maximum magnitude of the input (defaults to 1). Can + * be infinite. + * @return The value after the deadband is applied. + */ +template + requires std::is_arithmetic_v || units::traits::is_unit_t_v +T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { + T magnitude; + if constexpr (std::is_arithmetic_v) { + magnitude = std::abs(value); + } else { + magnitude = units::math::abs(value); + } + + if (magnitude > deadband) { + if (maxMagnitude / deadband > 1.0E12) { + // If max magnitude is sufficiently large, the implementation encounters + // roundoff error. Implementing the limiting behavior directly avoids + // the problem. + return value > T{0.0} ? value - deadband : value + deadband; + } + if (value > T{0.0}) { + // Map deadband to 0 and map max to max. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). + // x₁ = deadband + // y₁ = 0 + // x₂ = max + // y₂ = max + // + // y = (max - 0)/(max - deadband) (x - deadband) + 0 + // y = max/(max - deadband) (x - deadband) + // y = max (x - deadband)/(max - deadband) + return maxMagnitude * (value - deadband) / (maxMagnitude - deadband); + } else { + // Map -deadband to 0 and map -max to -max. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). + // x₁ = -deadband + // y₁ = 0 + // x₂ = -max + // y₂ = -max + // + // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 + // y = max/(max - deadband) (x + deadband) + // y = max (x + deadband)/(max - deadband) + return maxMagnitude * (value + deadband) / (maxMagnitude - deadband); + } + } else { + return T{0.0}; + } +} + +/** + * Returns modulus of input. + * + * @param input Input value to wrap. + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ +template +constexpr T InputModulus(T input, T minimumInput, T maximumInput) { + T modulus = maximumInput - minimumInput; + + // Wrap input if it's above the maximum input + int numMax = (input - minimumInput) / modulus; + input -= numMax * modulus; + + // Wrap input if it's below the minimum input + int numMin = (input - maximumInput) / modulus; + input -= numMin * modulus; + + return input; +} + +/** + * Checks if the given value matches an expected value within a certain + * tolerance. + * + * @param expected The expected value + * @param actual The actual value + * @param tolerance The allowed difference between the actual and the expected + * value + * @return Whether or not the actual value is within the allowed tolerance + */ +template + requires std::is_arithmetic_v || units::traits::is_unit_t_v +constexpr bool IsNear(T expected, T actual, T tolerance) { + if constexpr (std::is_arithmetic_v) { + return std::abs(expected - actual) < tolerance; + } else { + return units::math::abs(expected - actual) < tolerance; + } +} + +/** + * Checks if the given value matches an expected value within a certain + * tolerance. Supports continuous input for cases like absolute encoders. + * + * Continuous input means that the min and max value are considered to be the + * same point, and tolerances can be checked across them. A common example + * would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns + * true because 359 is 1 away from 360 (which is treated as the same as 0) and + * 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the + * given tolerance of 5. + * + * @param expected The expected value + * @param actual The actual value + * @param tolerance The allowed difference between the actual and the expected + * value + * @param min Smallest value before wrapping around to the largest value + * @param max Largest value before wrapping around to the smallest value + * @return Whether or not the actual value is within the allowed tolerance + */ +template + requires std::is_arithmetic_v || units::traits::is_unit_t_v +constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) { + T errorBound = (max - min) / 2.0; + T error = frc::InputModulus(expected - actual, -errorBound, errorBound); + + if constexpr (std::is_arithmetic_v) { + return std::abs(error) < tolerance; + } else { + return units::math::abs(error) < tolerance; + } +} + +/** + * Wraps an angle to the range -pi to pi radians (-180 to 180 degrees). + * + * @param angle Angle to wrap. + */ +TRAJOPT_DLLEXPORT +constexpr units::radian_t AngleModulus(units::radian_t angle) { + return InputModulus(angle, + units::radian_t{-std::numbers::pi}, + units::radian_t{std::numbers::pi}); +} + +} // namespace frc diff --git a/include/frc/array.h b/include/frc/array.h new file mode 100644 index 00000000..d6711f16 --- /dev/null +++ b/include/frc/array.h @@ -0,0 +1,100 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include +#include + +namespace wpi { + +struct empty_array_t {}; +constexpr empty_array_t empty_array; + +/** + * This class is a wrapper around std::array that does compile time size + * checking. + * + * std::array's implicit constructor can result in uninitialized elements if the + * number of arguments doesn't match the std::array size. + */ +template +class array : public std::array { + public: + constexpr explicit array(empty_array_t) {} + + template ... Ts> + requires(1 + sizeof...(Ts) == N) + constexpr array(T arg, Ts&&... args) // NOLINT + : std::array{std::forward(arg), std::forward(args)...} {} + + constexpr array(const array&) = default; + constexpr array& operator=(const array&) = default; + constexpr array(array&&) = default; + constexpr array& operator=(array&&) = default; + + constexpr array(const std::array& rhs) { // NOLINT + *static_cast*>(this) = rhs; + } + + constexpr array& operator=(const std::array& rhs) { + *static_cast*>(this) = rhs; + return *this; + } + + constexpr array(std::array&& rhs) { // NOLINT + *static_cast*>(this) = rhs; + } + + constexpr array& operator=(std::array&& rhs) { + *static_cast*>(this) = rhs; + return *this; + } +}; + +template ... Ts> +array(T, Ts...) -> array; + +} // namespace wpi + +template + requires(I < N) +constexpr T& get(wpi::array& arr) noexcept { + return std::get(static_cast>(arr)); +} + +template + requires(I < N) +constexpr T&& get(wpi::array&& arr) noexcept { + return std::move(std::get(arr)); +} + +template + requires(I < N) +constexpr const T& get(const wpi::array& arr) noexcept { + return std::get(static_cast>(arr)); +} + +template + requires(I < N) +constexpr const T&& get(const wpi::array&& arr) noexcept { + return std::move(std::get(arr)); +} + +// Enables structured bindings +namespace std { // NOLINT +// Partial specialization for wpi::array +template +struct tuple_size> : public integral_constant {}; + +// Partial specialization for wpi::array +template + requires(I < N) +struct tuple_element> { + using type = T; +}; +} // namespace std diff --git a/include/frc/bit.h b/include/frc/bit.h new file mode 100644 index 00000000..7259f7ae --- /dev/null +++ b/include/frc/bit.h @@ -0,0 +1,99 @@ +//===-- llvm/ADT/bit.h - C++20 ----------------------------*- C++ -*-===// +// +// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions. +// See https://llvm.org/LICENSE.txt for license information. +// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception +// +//===----------------------------------------------------------------------===// +/// +/// \file +/// This file implements the C++20 header. +/// +//===----------------------------------------------------------------------===// + +#ifndef WPIUTIL_WPI_BIT_H +#define WPIUTIL_WPI_BIT_H + +#include "frc/Compiler.h" +#include +#include +#include + +#if !__has_builtin(__builtin_bit_cast) +#include +#endif + +#if defined(_MSC_VER) && !defined(_DEBUG) +#include // for _byteswap_{ushort,ulong,uint64} +#endif + +namespace wpi { + +// This implementation of bit_cast is different from the C++20 one in two ways: +// - It isn't constexpr because that requires compiler support. +// - It requires trivially-constructible To, to avoid UB in the implementation. +template < + typename To, typename From, + typename = std::enable_if_t, + typename = std::enable_if_t::value>, + typename = std::enable_if_t::value>, + typename = std::enable_if_t::value>> +[[nodiscard]] inline To bit_cast(const From &from) noexcept { +#if __has_builtin(__builtin_bit_cast) + return __builtin_bit_cast(To, from); +#else + To to; + std::memcpy(&to, &from, sizeof(To)); + return to; +#endif +} + +/// Reverses the bytes in the given integer value V. +template >> +[[nodiscard]] constexpr T byteswap(T V) noexcept { + if constexpr (sizeof(T) == 1) { + return V; + } else if constexpr (sizeof(T) == 2) { + uint16_t UV = V; +#if defined(_MSC_VER) && !defined(_DEBUG) + // The DLL version of the runtime lacks these functions (bug!?), but in a + // release build they're replaced with BSWAP instructions anyway. + return _byteswap_ushort(UV); +#else + uint16_t Hi = UV << 8; + uint16_t Lo = UV >> 8; + return Hi | Lo; +#endif + } else if constexpr (sizeof(T) == 4) { + uint32_t UV = V; +#if __has_builtin(__builtin_bswap32) + return __builtin_bswap32(UV); +#elif defined(_MSC_VER) && !defined(_DEBUG) + return _byteswap_ulong(UV); +#else + uint32_t Byte0 = UV & 0x000000FF; + uint32_t Byte1 = UV & 0x0000FF00; + uint32_t Byte2 = UV & 0x00FF0000; + uint32_t Byte3 = UV & 0xFF000000; + return (Byte0 << 24) | (Byte1 << 8) | (Byte2 >> 8) | (Byte3 >> 24); +#endif + } else if constexpr (sizeof(T) == 8) { + uint64_t UV = V; +#if __has_builtin(__builtin_bswap64) + return __builtin_bswap64(UV); +#elif defined(_MSC_VER) && !defined(_DEBUG) + return _byteswap_uint64(UV); +#else + uint64_t Hi = wpi::byteswap(UV); + uint32_t Lo = wpi::byteswap(UV >> 32); + return (Hi << 32) | Lo; +#endif + } else { + static_assert(!sizeof(T *), "Don't know how to handle the given type."); + return 0; + } +} + +} // namespace wpi + +#endif diff --git a/include/frc/geometry/Pose2d.h b/include/frc/geometry/Pose2d.h new file mode 100644 index 00000000..109157e9 --- /dev/null +++ b/include/frc/geometry/Pose2d.h @@ -0,0 +1,209 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "trajopt/SymbolExports.h" + +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Twist2d.h" + +namespace frc { + +/** + * Represents a 2D pose containing translational and rotational elements. + */ +class TRAJOPT_DLLEXPORT Pose2d { + public: + /** + * Constructs a pose at the origin facing toward the positive X axis. + */ + constexpr Pose2d() = default; + + /** + * Constructs a pose with the specified translation and rotation. + * + * @param translation The translational component of the pose. + * @param rotation The rotational component of the pose. + */ + constexpr Pose2d(Translation2d translation, Rotation2d rotation); + + /** + * Constructs a pose with x and y translations instead of a separate + * Translation2d. + * + * @param x The x component of the translational component of the pose. + * @param y The y component of the translational component of the pose. + * @param rotation The rotational component of the pose. + */ + constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation); + + /** + * Transforms the pose by the given transformation and returns the new + * transformed pose. + * + * 
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [  0,    0, 1][transform.t]
+   *
+ * + * @param other The transform to transform the pose by. + * + * @return The transformed pose. + */ + constexpr Pose2d operator+(const Transform2d& other) const; + + /** + * Returns the Transform2d that maps the one pose to another. + * + * @param other The initial pose of the transformation. + * @return The transform that maps the other pose to the current pose. + */ + Transform2d operator-(const Pose2d& other) const; + + /** + * Checks equality between this Pose2d and another object. + */ + bool operator==(const Pose2d&) const = default; + + /** + * Returns the underlying translation. + * + * @return Reference to the translational component of the pose. + */ + constexpr const Translation2d& Translation() const { return m_translation; } + + /** + * Returns the X component of the pose's translation. + * + * @return The x component of the pose's translation. + */ + constexpr units::meter_t X() const { return m_translation.X(); } + + /** + * Returns the Y component of the pose's translation. + * + * @return The y component of the pose's translation. + */ + constexpr units::meter_t Y() const { return m_translation.Y(); } + + /** + * Returns the underlying rotation. + * + * @return Reference to the rotational component of the pose. + */ + constexpr const Rotation2d& Rotation() const { return m_rotation; } + + /** + * Multiplies the current pose by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Pose2d. + */ + constexpr Pose2d operator*(double scalar) const; + + /** + * Divides the current pose by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Pose2d. + */ + constexpr Pose2d operator/(double scalar) const; + + /** + * Rotates the pose around the origin and returns the new pose. + * + * @param other The rotation to transform the pose by. + * + * @return The rotated pose. + */ + constexpr Pose2d RotateBy(const Rotation2d& other) const; + + /** + * Transforms the pose by the given transformation and returns the new pose. + * See + operator for the matrix multiplication performed. + * + * @param other The transform to transform the pose by. + * + * @return The transformed pose. + */ + constexpr Pose2d TransformBy(const Transform2d& other) const; + + /** + * Returns the current pose relative to the given pose. + * + * This function can often be used for trajectory tracking or pose + * stabilization algorithms to get the error between the reference and the + * current pose. + * + * @param other The pose that is the origin of the new coordinate frame that + * the current pose will be converted into. + * + * @return The current pose relative to the new origin pose. + */ + Pose2d RelativeTo(const Pose2d& other) const; + + /** + * Obtain a new Pose2d from a (constant curvature) velocity. + * + * See https://file.tavsys.net/control/controls-engineering-in-frc.pdf section + * 10.2 "Pose exponential" for a derivation. + * + * The twist is a change in pose in the robot's coordinate frame since the + * previous pose update. When the user runs exp() on the previous known + * field-relative pose with the argument being the twist, the user will + * receive the new field-relative pose. + * + * "Exp" represents the pose exponential, which is solving a differential + * equation moving the pose forward in time. + * + * @param twist The change in pose in the robot's coordinate frame since the + * previous pose update. For example, if a non-holonomic robot moves forward + * 0.01 meters and changes angle by 0.5 degrees since the previous pose + * update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}. + * + * @return The new pose of the robot. + */ + Pose2d Exp(const Twist2d& twist) const; + + /** + * Returns a Twist2d that maps this pose to the end pose. If c is the output + * of a.Log(b), then a.Exp(c) would yield b. + * + * @param end The end pose for the transformation. + * + * @return The twist that maps this to end. + */ + Twist2d Log(const Pose2d& end) const; + + /** + * Returns the nearest Pose2d from a collection of poses + * @param poses The collection of poses. + * @return The nearest Pose2d from the collection. + */ + Pose2d Nearest(std::span poses) const; + + /** + * Returns the nearest Pose2d from a collection of poses + * @param poses The collection of poses. + * @return The nearest Pose2d from the collection. + */ + Pose2d Nearest(std::initializer_list poses) const; + + private: + Translation2d m_translation; + Rotation2d m_rotation; +}; + +} // namespace frc + +#include "frc/geometry/Pose2d.inc" diff --git a/include/frc/geometry/Pose2d.inc b/include/frc/geometry/Pose2d.inc new file mode 100644 index 00000000..559a0034 --- /dev/null +++ b/include/frc/geometry/Pose2d.inc @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "units/length.h" + +namespace frc { + +constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation) + : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} + +constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y, + Rotation2d rotation) + : m_translation{x, y}, m_rotation{std::move(rotation)} {} + +constexpr Pose2d Pose2d::operator+(const Transform2d& other) const { + return TransformBy(other); +} + +constexpr Pose2d Pose2d::operator*(double scalar) const { + return Pose2d{m_translation * scalar, m_rotation * scalar}; +} + +constexpr Pose2d Pose2d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + +constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const { + return {m_translation.RotateBy(other), m_rotation.RotateBy(other)}; +} + +constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const { + return {m_translation + (other.Translation().RotateBy(m_rotation)), + other.Rotation() + m_rotation}; +} + +} // namespace frc diff --git a/include/frc/geometry/Rotation2d.h b/include/frc/geometry/Rotation2d.h new file mode 100644 index 00000000..0cb605ef --- /dev/null +++ b/include/frc/geometry/Rotation2d.h @@ -0,0 +1,165 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" + +#include "units/angle.h" + +namespace frc { + +/** + * A rotation in a 2D coordinate frame represented by a point on the unit circle + * (cosine and sine). + * + * The angle is continuous, that is if a Rotation2d is constructed with 361 + * degrees, it will return 361 degrees. This allows algorithms that wouldn't + * want to see a discontinuity in the rotations as it sweeps past from 360 to 0 + * on the second time around. + */ +class TRAJOPT_DLLEXPORT Rotation2d { + public: + /** + * Constructs a Rotation2d with a default angle of 0 degrees. + */ + constexpr Rotation2d() = default; + + /** + * Constructs a Rotation2d with the given angle. + * + * @param value The value of the angle. + */ + constexpr Rotation2d(units::angle_unit auto value); // NOLINT + + /** + * Constructs a Rotation2d with the given x and y (cosine and sine) + * components. The x and y don't have to be normalized. + * + * @param x The x component or cosine of the rotation. + * @param y The y component or sine of the rotation. + */ + constexpr Rotation2d(double x, double y); + + /** + * Adds two rotations together, with the result being bounded between -pi and + * pi. + * + * For example, Rotation2d{30_deg} + Rotation2d{60_deg} equals + * Rotation2d{units::radian_t{std::numbers::pi/2.0}} + * + * @param other The rotation to add. + * + * @return The sum of the two rotations. + */ + constexpr Rotation2d operator+(const Rotation2d& other) const; + + /** + * Subtracts the new rotation from the current rotation and returns the new + * rotation. + * + * For example, Rotation2d{10_deg} - Rotation2d{100_deg} equals + * Rotation2d{units::radian_t{-std::numbers::pi/2.0}} + * + * @param other The rotation to subtract. + * + * @return The difference between the two rotations. + */ + constexpr Rotation2d operator-(const Rotation2d& other) const; + + /** + * Takes the inverse of the current rotation. This is simply the negative of + * the current angular value. + * + * @return The inverse of the current rotation. + */ + constexpr Rotation2d operator-() const; + + /** + * Multiplies the current rotation by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Rotation2d. + */ + constexpr Rotation2d operator*(double scalar) const; + + /** + * Divides the current rotation by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Rotation2d. + */ + constexpr Rotation2d operator/(double scalar) const; + + /** + * Checks equality between this Rotation2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + constexpr bool operator==(const Rotation2d& other) const; + + /** + * Adds the new rotation to the current rotation using a rotation matrix. + * + * 
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = std::atan2(sin_new, cos_new)
+   *
+ * + * @param other The rotation to rotate by. + * + * @return The new rotated Rotation2d. + */ + constexpr Rotation2d RotateBy(const Rotation2d& other) const; + + /** + * Returns the radian value of the rotation. + * + * @return The radian value of the rotation. + * @see AngleModulus to constrain the angle within (-pi, pi] + */ + constexpr units::radian_t Radians() const { return m_value; } + + /** + * Returns the degree value of the rotation. + * + * @return The degree value of the rotation. + * @see InputModulus to constrain the angle within (-180, 180] + */ + constexpr units::degree_t Degrees() const { return m_value; } + + /** + * Returns the cosine of the rotation. + * + * @return The cosine of the rotation. + */ + constexpr double Cos() const { return m_cos; } + + /** + * Returns the sine of the rotation. + * + * @return The sine of the rotation. + */ + constexpr double Sin() const { return m_sin; } + + /** + * Returns the tangent of the rotation. + * + * @return The tangent of the rotation. + */ + constexpr double Tan() const { return Sin() / Cos(); } + + private: + units::radian_t m_value = 0_rad; + double m_cos = 1; + double m_sin = 0; +}; + +} // namespace frc + +#include "frc/geometry/Rotation2d.inc" diff --git a/include/frc/geometry/Rotation2d.inc b/include/frc/geometry/Rotation2d.inc new file mode 100644 index 00000000..104b66b1 --- /dev/null +++ b/include/frc/geometry/Rotation2d.inc @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Rotation2d.h" +#include "units/angle.h" + +namespace frc { + +constexpr Rotation2d::Rotation2d(units::angle_unit auto value) + : m_value{value}, + m_cos{gcem::cos(value.template convert().value())}, + m_sin{gcem::sin(value.template convert().value())} {} + +constexpr Rotation2d::Rotation2d(double x, double y) { + double magnitude = gcem::hypot(x, y); + if (magnitude > 1e-6) { + m_sin = y / magnitude; + m_cos = x / magnitude; + } else { + m_sin = 0.0; + m_cos = 1.0; + } + m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; +} + +constexpr Rotation2d Rotation2d::operator-() const { + return Rotation2d{-m_value}; +} + +constexpr Rotation2d Rotation2d::operator*(double scalar) const { + return Rotation2d{m_value * scalar}; +} + +constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const { + return RotateBy(other); +} + +constexpr Rotation2d Rotation2d::operator-(const Rotation2d& other) const { + return *this + -other; +} + +constexpr Rotation2d Rotation2d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + +constexpr bool Rotation2d::operator==(const Rotation2d& other) const { + return gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin()) < 1E-9; +} + +constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const { + return {Cos() * other.Cos() - Sin() * other.Sin(), + Cos() * other.Sin() + Sin() * other.Cos()}; +} + +} // namespace frc diff --git a/include/frc/geometry/Transform2d.h b/include/frc/geometry/Transform2d.h new file mode 100644 index 00000000..7a697155 --- /dev/null +++ b/include/frc/geometry/Transform2d.h @@ -0,0 +1,127 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" + +#include "frc/geometry/Translation2d.h" + +namespace frc { + +class TRAJOPT_DLLEXPORT Pose2d; + +/** + * Represents a transformation for a Pose2d in the pose's frame. + */ +class TRAJOPT_DLLEXPORT Transform2d { + public: + /** + * Constructs the transform that maps the initial pose to the final pose. + * + * @param initial The initial pose for the transformation. + * @param final The final pose for the transformation. + */ + Transform2d(Pose2d initial, Pose2d final); + + /** + * Constructs a transform with the given translation and rotation components. + * + * @param translation Translational component of the transform. + * @param rotation Rotational component of the transform. + */ + constexpr Transform2d(Translation2d translation, Rotation2d rotation); + + /** + * Constructs a transform with x and y translations instead of a separate + * Translation2d. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + constexpr Transform2d(units::meter_t x, units::meter_t y, + Rotation2d rotation); + + /** + * Constructs the identity transform -- maps an initial pose to itself. + */ + constexpr Transform2d() = default; + + /** + * Returns the translation component of the transformation. + * + * @return Reference to the translational component of the transform. + */ + constexpr const Translation2d& Translation() const { return m_translation; } + + /** + * Returns the X component of the transformation's translation. + * + * @return The x component of the transformation's translation. + */ + constexpr units::meter_t X() const { return m_translation.X(); } + + /** + * Returns the Y component of the transformation's translation. + * + * @return The y component of the transformation's translation. + */ + constexpr units::meter_t Y() const { return m_translation.Y(); } + + /** + * Returns the rotational component of the transformation. + * + * @return Reference to the rotational component of the transform. + */ + constexpr const Rotation2d& Rotation() const { return m_rotation; } + + /** + * Invert the transformation. This is useful for undoing a transformation. + * + * @return The inverted transformation. + */ + constexpr Transform2d Inverse() const; + + /** + * Multiplies the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform2d. + */ + constexpr Transform2d operator*(double scalar) const { + return Transform2d(m_translation * scalar, m_rotation * scalar); + } + + /** + * Divides the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform2d. + */ + constexpr Transform2d operator/(double scalar) const { + return *this * (1.0 / scalar); + } + + /** + * Composes two transformations. The second transform is applied relative to + * the orientation of the first. + * + * @param other The transform to compose with this one. + * @return The composition of the two transformations. + */ + Transform2d operator+(const Transform2d& other) const; + + /** + * Checks equality between this Transform2d and another object. + */ + bool operator==(const Transform2d&) const = default; + + private: + Translation2d m_translation; + Rotation2d m_rotation; +}; +} // namespace frc + +#include "frc/geometry/Transform2d.inc" diff --git a/include/frc/geometry/Transform2d.inc b/include/frc/geometry/Transform2d.inc new file mode 100644 index 00000000..cc925148 --- /dev/null +++ b/include/frc/geometry/Transform2d.inc @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" +#include "frc/geometry/Translation2d.h" + +namespace frc { + +constexpr Transform2d::Transform2d(Translation2d translation, + Rotation2d rotation) + : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} + +constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y, + Rotation2d rotation) + : m_translation{x, y}, m_rotation{std::move(rotation)} {} + +constexpr Transform2d Transform2d::Inverse() const { + // We are rotating the difference between the translations + // using a clockwise rotation matrix. This transforms the global + // delta into a local delta (relative to the initial pose). + return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()}; +} + +} // namespace frc diff --git a/include/frc/geometry/Translation2d.h b/include/frc/geometry/Translation2d.h new file mode 100644 index 00000000..a4ae00b3 --- /dev/null +++ b/include/frc/geometry/Translation2d.h @@ -0,0 +1,210 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include +#include "trajopt/SymbolExports.h" + +#include "frc/geometry/Rotation2d.h" +#include "units/length.h" + +namespace frc { + +/** + * Represents a translation in 2D space. + * This object can be used to represent a point or a vector. + * + * This assumes that you are using conventional mathematical axes. + * When the robot is at the origin facing in the positive X direction, forward + * is positive X and left is positive Y. + */ +class TRAJOPT_DLLEXPORT Translation2d { + public: + /** + * Constructs a Translation2d with X and Y components equal to zero. + */ + constexpr Translation2d() = default; + + /** + * Constructs a Translation2d with the X and Y components equal to the + * provided values. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + */ + constexpr Translation2d(units::meter_t x, units::meter_t y); + + /** + * Constructs a Translation2d with the provided distance and angle. This is + * essentially converting from polar coordinates to Cartesian coordinates. + * + * @param distance The distance from the origin to the end of the translation. + * @param angle The angle between the x-axis and the translation vector. + */ + constexpr Translation2d(units::meter_t distance, const Rotation2d& angle); + + /** + * Constructs a Translation2d from the provided translation vector's X and Y + * components. The values are assumed to be in meters. + * + * @param vector The translation vector to represent. + */ + explicit Translation2d(const Eigen::Vector2d& vector); + + /** + * Calculates the distance between two translations in 2D space. + * + * The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²). + * + * @param other The translation to compute the distance to. + * + * @return The distance between the two translations. + */ + units::meter_t Distance(const Translation2d& other) const; + + /** + * Returns the X component of the translation. + * + * @return The X component of the translation. + */ + constexpr units::meter_t X() const { return m_x; } + + /** + * Returns the Y component of the translation. + * + * @return The Y component of the translation. + */ + constexpr units::meter_t Y() const { return m_y; } + + /** + * Returns a vector representation of this translation. + * + * @return A Vector representation of this translation. + */ + constexpr Eigen::Vector2d ToVector() const; + + /** + * Returns the norm, or distance from the origin to the translation. + * + * @return The norm of the translation. + */ + units::meter_t Norm() const; + + /** + * Returns the angle this translation forms with the positive X axis. + * + * @return The angle of the translation + */ + constexpr Rotation2d Angle() const; + + /** + * Applies a rotation to the translation in 2D space. + * + * This multiplies the translation vector by a counterclockwise rotation + * matrix of the given angle. + * + * 
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   *
+ * + * For example, rotating a Translation2d of <2, 0> by 90 degrees will + * return a Translation2d of <0, 2>. + * + * @param other The rotation to rotate the translation by. + * + * @return The new rotated translation. + */ + constexpr Translation2d RotateBy(const Rotation2d& other) const; + + /** + * Returns the sum of two translations in 2D space. + * + * For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} = + * Translation3d{3.0, 8.0}. + * + * @param other The translation to add. + * + * @return The sum of the translations. + */ + constexpr Translation2d operator+(const Translation2d& other) const; + + /** + * Returns the difference between two translations. + * + * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = + * Translation2d{4.0, 2.0}. + * + * @param other The translation to subtract. + * + * @return The difference between the two translations. + */ + constexpr Translation2d operator-(const Translation2d& other) const; + + /** + * Returns the inverse of the current translation. This is equivalent to + * rotating by 180 degrees, flipping the point over both axes, or negating all + * components of the translation. + * + * @return The inverse of the current translation. + */ + constexpr Translation2d operator-() const; + + /** + * Returns the translation multiplied by a scalar. + * + * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}. + * + * @param scalar The scalar to multiply by. + * + * @return The scaled translation. + */ + constexpr Translation2d operator*(double scalar) const; + + /** + * Returns the translation divided by a scalar. + * + * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}. + * + * @param scalar The scalar to divide by. + * + * @return The scaled translation. + */ + constexpr Translation2d operator/(double scalar) const; + + /** + * Checks equality between this Translation2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Translation2d& other) const; + + /** + * Returns the nearest Translation2d from a collection of translations + * @param translations The collection of translations. + * @return The nearest Translation2d from the collection. + */ + Translation2d Nearest(std::span translations) const; + + /** + * Returns the nearest Translation2d from a collection of translations + * @param translations The collection of translations. + * @return The nearest Translation2d from the collection. + */ + Translation2d Nearest( + std::initializer_list translations) const; + + private: + units::meter_t m_x = 0_m; + units::meter_t m_y = 0_m; +}; + +} // namespace frc + +#include "frc/geometry/Translation2d.inc" diff --git a/include/frc/geometry/Translation2d.inc b/include/frc/geometry/Translation2d.inc new file mode 100644 index 00000000..6b291e1f --- /dev/null +++ b/include/frc/geometry/Translation2d.inc @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "frc/geometry/Translation2d.h" +#include "units/length.h" + +namespace frc { + +constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y) + : m_x{x}, m_y{y} {} + +constexpr Translation2d::Translation2d(units::meter_t distance, + const Rotation2d& angle) + : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {} + +constexpr Eigen::Vector2d Translation2d::ToVector() const { + return Eigen::Vector2d{{m_x.value(), m_y.value()}}; +} + +constexpr Rotation2d Translation2d::Angle() const { + return Rotation2d{m_x.value(), m_y.value()}; +} + +constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const { + return {m_x * other.Cos() - m_y * other.Sin(), + m_x * other.Sin() + m_y * other.Cos()}; +} + +constexpr Translation2d Translation2d::operator+( + const Translation2d& other) const { + return {X() + other.X(), Y() + other.Y()}; +} + +constexpr Translation2d Translation2d::operator-( + const Translation2d& other) const { + return *this + -other; +} + +constexpr Translation2d Translation2d::operator-() const { + return {-m_x, -m_y}; +} + +constexpr Translation2d Translation2d::operator*(double scalar) const { + return {scalar * m_x, scalar * m_y}; +} + +constexpr Translation2d Translation2d::operator/(double scalar) const { + return operator*(1.0 / scalar); +} + +} // namespace frc diff --git a/include/frc/geometry/Twist2d.h b/include/frc/geometry/Twist2d.h new file mode 100644 index 00000000..fa96b9eb --- /dev/null +++ b/include/frc/geometry/Twist2d.h @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" + +#include "units/angle.h" +#include "units/length.h" +#include "units/math.h" + +namespace frc { +/** + * A change in distance along a 2D arc since the last pose update. We can use + * ideas from differential calculus to create new Pose2ds from a Twist2d and + * vice versa. + * + * A Twist can be used to represent a difference between two poses. + */ +struct TRAJOPT_DLLEXPORT Twist2d { + /** + * Linear "dx" component + */ + units::meter_t dx = 0_m; + + /** + * Linear "dy" component + */ + units::meter_t dy = 0_m; + + /** + * Angular "dtheta" component (radians) + */ + units::radian_t dtheta = 0_rad; + + /** + * Checks equality between this Twist2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Twist2d& other) const { + return units::math::abs(dx - other.dx) < 1E-9_m && + units::math::abs(dy - other.dy) < 1E-9_m && + units::math::abs(dtheta - other.dtheta) < 1E-9_rad; + } + + /** + * Scale this by a given factor. + * + * @param factor The factor by which to scale. + * @return The scaled Twist2d. + */ + constexpr Twist2d operator*(double factor) const { + return Twist2d{dx * factor, dy * factor, dtheta * factor}; + } +}; +} // namespace frc diff --git a/include/frc/kinematics/ChassisSpeeds.h b/include/frc/kinematics/ChassisSpeeds.h new file mode 100644 index 00000000..35fb290a --- /dev/null +++ b/include/frc/kinematics/ChassisSpeeds.h @@ -0,0 +1,253 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "units/angular_velocity.h" +#include "units/velocity.h" + +namespace frc { +/** + * Represents the speed of a robot chassis. Although this struct contains + * similar members compared to a Twist2d, they do NOT represent the same thing. + * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of + * reference, a ChassisSpeeds struct represents a robot's velocity. + * + * A strictly non-holonomic drivetrain, such as a differential drive, should + * never have a dy component because it can never move sideways. Holonomic + * drivetrains such as swerve and mecanum will often have all three components. + */ +struct TRAJOPT_DLLEXPORT ChassisSpeeds { + /** + * Velocity along the x-axis. (Fwd is +) + */ + units::meters_per_second_t vx = 0_mps; + + /** + * Velocity along the y-axis. (Left is +) + */ + units::meters_per_second_t vy = 0_mps; + + /** + * Represents the angular velocity of the robot frame. (CCW is +) + */ + units::radians_per_second_t omega = 0_rad_per_s; + + /** + * Disretizes a continuous-time chassis speed. + * + * This function converts a continuous-time chassis speed into a discrete-time + * one such that when the discrete-time chassis speed is applied for one + * timestep, the robot moves as if the velocity components are independent + * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the + * y-axis, and omega * dt around the z-axis). + * + * This is useful for compensating for translational skew when translating and + * rotating a swerve drivetrain. + * + * @param vx Forward velocity. + * @param vy Sideways velocity. + * @param omega Angular velocity. + * @param dt The duration of the timestep the speeds should be applied for. + * + * @return Discretized ChassisSpeeds. + */ + static ChassisSpeeds Discretize(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, + units::second_t dt) { + Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt}; + auto twist = Pose2d{}.Log(desiredDeltaPose); + return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt}; + } + + /** + * Disretizes a continuous-time chassis speed. + * + * This function converts a continuous-time chassis speed into a discrete-time + * one such that when the discrete-time chassis speed is applied for one + * timestep, the robot moves as if the velocity components are independent + * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the + * y-axis, and omega * dt around the z-axis). + * + * This is useful for compensating for translational skew when translating and + * rotating a swerve drivetrain. + * + * @param continuousSpeeds The continuous speeds. + * @param dt The duration of the timestep the speeds should be applied for. + * + * @return Discretized ChassisSpeeds. + */ + static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds, + units::second_t dt) { + return Discretize(continuousSpeeds.vx, continuousSpeeds.vy, + continuousSpeeds.omega, dt); + } + + /** + * Converts a user provided field-relative set of speeds into a robot-relative + * ChassisSpeeds object. + * + * @param vx The component of speed in the x direction relative to the field. + * Positive x is away from your alliance wall. + * @param vy The component of speed in the y direction relative to the field. + * Positive y is to your left when standing behind the alliance wall. + * @param omega The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The + * robot's angle is considered to be zero when it is facing directly away from + * your alliance station wall. Remember that this should be CCW positive. + * + * @return ChassisSpeeds object representing the speeds in the robot's frame + * of reference. + */ + static ChassisSpeeds FromFieldRelativeSpeeds( + units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, const Rotation2d& robotAngle) { + // CW rotation into chassis frame + auto rotated = + Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}} + .RotateBy(-robotAngle); + return {units::meters_per_second_t{rotated.X().value()}, + units::meters_per_second_t{rotated.Y().value()}, omega}; + } + + /** + * Converts a user provided field-relative ChassisSpeeds object into a + * robot-relative ChassisSpeeds object. + * + * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds + * in the field frame of reference. Positive x is away from your alliance + * wall. Positive y is to your left when standing behind the alliance wall. + * @param robotAngle The angle of the robot as measured by a gyroscope. The + * robot's angle is considered to be zero when it is facing directly away + * from your alliance station wall. Remember that this should be CCW + * positive. + * @return ChassisSpeeds object representing the speeds in the robot's frame + * of reference. + */ + static ChassisSpeeds FromFieldRelativeSpeeds( + const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) { + return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx, + fieldRelativeSpeeds.vy, + fieldRelativeSpeeds.omega, robotAngle); + } + + /** + * Converts a user provided robot-relative set of speeds into a field-relative + * ChassisSpeeds object. + * + * @param vx The component of speed in the x direction relative to the robot. + * Positive x is towards the robot's front. + * @param vy The component of speed in the y direction relative to the robot. + * Positive y is towards the robot's left. + * @param omega The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The + * robot's angle is considered to be zero when it is facing directly away from + * your alliance station wall. Remember that this should be CCW positive. + * + * @return ChassisSpeeds object representing the speeds in the field's frame + * of reference. + */ + static ChassisSpeeds FromRobotRelativeSpeeds( + units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, const Rotation2d& robotAngle) { + // CCW rotation out of chassis frame + auto rotated = + Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}} + .RotateBy(robotAngle); + return {units::meters_per_second_t{rotated.X().value()}, + units::meters_per_second_t{rotated.Y().value()}, omega}; + } + + /** + * Converts a user provided robot-relative ChassisSpeeds object into a + * field-relative ChassisSpeeds object. + * + * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds + * in the robot frame of reference. Positive x is the towards robot's + * front. Positive y is towards the robot's left. + * @param robotAngle The angle of the robot as measured by a gyroscope. The + * robot's angle is considered to be zero when it is facing directly away + * from your alliance station wall. Remember that this should be CCW + * positive. + * @return ChassisSpeeds object representing the speeds in the field's frame + * of reference. + */ + static ChassisSpeeds FromRobotRelativeSpeeds( + const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) { + return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx, + robotRelativeSpeeds.vy, + robotRelativeSpeeds.omega, robotAngle); + } + + /** + * Adds two ChassisSpeeds and returns the sum. + * + *

For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5} + * = ChassisSpeeds{3.0, 2.0, 2.0} + * + * @param other The ChassisSpeeds to add. + * + * @return The sum of the ChassisSpeeds. + */ + constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const { + return {vx + other.vx, vy + other.vy, omega + other.omega}; + } + + /** + * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and + * returns the difference. + * + *

For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} + * = ChassisSpeeds{4.0, 2.0, 1.0} + * + * @param other The ChassisSpeeds to subtract. + * + * @return The difference between the two ChassisSpeeds. + */ + constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const { + return *this + -other; + } + + /** + * Returns the inverse of the current ChassisSpeeds. + * This is equivalent to negating all components of the ChassisSpeeds. + * + * @return The inverse of the current ChassisSpeeds. + */ + constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; } + + /** + * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds. + * + *

For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 + * = ChassisSpeeds{4.0, 5.0, 1.0} + * + * @param scalar The scalar to multiply by. + * + * @return The scaled ChassisSpeeds. + */ + constexpr ChassisSpeeds operator*(double scalar) const { + return {scalar * vx, scalar * vy, scalar * omega}; + } + + /** + * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds. + * + *

For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 + * = ChassisSpeeds{1.0, 1.25, 0.5} + * + * @param scalar The scalar to divide by. + * + * @return The scaled ChassisSpeeds. + */ + constexpr ChassisSpeeds operator/(double scalar) const { + return operator*(1.0 / scalar); + } +}; +} // namespace frc diff --git a/include/frc/kinematics/Kinematics.h b/include/frc/kinematics/Kinematics.h new file mode 100644 index 00000000..2d8a6327 --- /dev/null +++ b/include/frc/kinematics/Kinematics.h @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" + +#include "frc/geometry/Twist2d.h" +#include "frc/kinematics/ChassisSpeeds.h" + +namespace frc { +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) + * into individual wheel speeds. Robot code should not use this directly- + * Instead, use the particular type for your drivetrain (e.g., + * DifferentialDriveKinematics). + * + * Inverse kinematics converts a desired chassis speed into wheel speeds whereas + * forward kinematics converts wheel speeds into chassis speed. + */ +template +class TRAJOPT_DLLEXPORT Kinematics { + public: + /** + * Performs forward kinematics to return the resulting chassis speed from the + * wheel speeds. This method is often used for odometry -- determining the + * robot's position on the field using data from the real-world speed of each + * wheel on the robot. + * + * @param wheelSpeeds The speeds of the wheels. + * @return The chassis speed. + */ + virtual ChassisSpeeds ToChassisSpeeds( + const WheelSpeeds& wheelSpeeds) const = 0; + + /** + * Performs inverse kinematics to return the wheel speeds from a desired + * chassis velocity. This method is often used to convert joystick values into + * wheel speeds. + * + * @param chassisSpeeds The desired chassis speed. + * @return The wheel speeds. + */ + virtual WheelSpeeds ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const = 0; + + /** + * Performs forward kinematics to return the resulting Twist2d from the given + * change in wheel positions. This method is often used for odometry -- + * determining the robot's position on the field using changes in the distance + * driven by each wheel on the robot. + * + * @param start The starting distances driven by the wheels. + * @param end The ending distances driven by the wheels. + * + * @return The resulting Twist2d in the robot's movement. + */ + virtual Twist2d ToTwist2d(const WheelPositions& start, + const WheelPositions& end) const = 0; +}; +} // namespace frc diff --git a/include/frc/kinematics/SwerveDriveKinematics.h b/include/frc/kinematics/SwerveDriveKinematics.h new file mode 100644 index 00000000..96171826 --- /dev/null +++ b/include/frc/kinematics/SwerveDriveKinematics.h @@ -0,0 +1,303 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include +#include "trajopt/SymbolExports.h" +#include "frc/array.h" + +#include "frc/EigenCore.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Twist2d.h" +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/Kinematics.h" +#include "frc/kinematics/SwerveDriveWheelPositions.h" +#include "frc/kinematics/SwerveModulePosition.h" +#include "frc/kinematics/SwerveModuleState.h" +#include "units/velocity.h" + +namespace frc { + +template +using SwerveDriveWheelSpeeds = wpi::array; + +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) + * into individual module states (speed and angle). + * + * The inverse kinematics (converting from a desired chassis velocity to + * individual module states) uses the relative locations of the modules with + * respect to the center of rotation. The center of rotation for inverse + * kinematics is also variable. This means that you can set your set your center + * of rotation in a corner of the robot to perform special evasion maneuvers. + * + * Forward kinematics (converting an array of module states into the overall + * chassis motion) is performs the exact opposite of what inverse kinematics + * does. Since this is an overdetermined system (more equations than variables), + * we use a least-squares approximation. + * + * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] + * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then + * multiply by [moduleStates] to get our chassis speeds. + * + * Forward kinematics is also used for odometry -- determining the position of + * the robot on the field using encoders and a gyro. + */ +template +class SwerveDriveKinematics + : public Kinematics, + SwerveDriveWheelPositions> { + public: + /** + * Constructs a swerve drive kinematics object. This takes in a variable + * number of module locations as Translation2ds. The order in which you pass + * in the module locations is the same order that you will receive the module + * states when performing inverse kinematics. It is also expected that you + * pass in the module states in the same order when calling the forward + * kinematics methods. + * + * @param moduleTranslations The locations of the modules relative to the + * physical center of the robot. + */ + template ... ModuleTranslations> + requires(sizeof...(ModuleTranslations) == NumModules) + explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations) + : m_modules{moduleTranslations...}, m_moduleHeadings(wpi::empty_array) { + for (size_t i = 0; i < NumModules; i++) { + // clang-format off + m_inverseKinematics.template block<2, 3>(i * 2, 0) << + 1, 0, (-m_modules[i].Y()).value(), + 0, 1, (+m_modules[i].X()).value(); + // clang-format on + } + + m_forwardKinematics = m_inverseKinematics.householderQr(); + } + + explicit SwerveDriveKinematics( + const wpi::array& modules) + : m_modules{modules}, m_moduleHeadings(wpi::empty_array) { + for (size_t i = 0; i < NumModules; i++) { + // clang-format off + m_inverseKinematics.template block<2, 3>(i * 2, 0) << + 1, 0, (-m_modules[i].Y()).value(), + 0, 1, (+m_modules[i].X()).value(); + // clang-format on + } + + m_forwardKinematics = m_inverseKinematics.householderQr(); + } + + SwerveDriveKinematics(const SwerveDriveKinematics&) = default; + + /** + * Reset the internal swerve module headings. + * @param moduleHeadings The swerve module headings. The order of the module + * headings should be same as passed into the constructor of this class. + */ + template ... ModuleHeadings> + requires(sizeof...(ModuleHeadings) == NumModules) + void ResetHeadings(ModuleHeadings&&... moduleHeadings) { + return this->ResetHeadings( + wpi::array{moduleHeadings...}); + } + + /** + * Reset the internal swerve module headings. + * @param moduleHeadings The swerve module headings. The order of the module + * headings should be same as passed into the constructor of this class. + */ + void ResetHeadings(wpi::array moduleHeadings); + + /** + * Performs inverse kinematics to return the module states from a desired + * chassis velocity. This method is often used to convert joystick values into + * module speeds and angles. + * + * This function also supports variable centers of rotation. During normal + * operations, the center of rotation is usually the same as the physical + * center of the robot; therefore, the argument is defaulted to that use case. + * However, if you wish to change the center of rotation for evasive + * maneuvers, vision alignment, or for any other use case, you can do so. + * + * In the case that the desired chassis speeds are zero (i.e. the robot will + * be stationary), the previously calculated module angle will be maintained. + * + * @param chassisSpeeds The desired chassis speed. + * @param centerOfRotation The center of rotation. For example, if you set the + * center of rotation at one corner of the robot and provide a chassis speed + * that only has a dtheta component, the robot will rotate around that corner. + * + * @return An array containing the module states. Use caution because these + * module states are not normalized. Sometimes, a user input may cause one of + * the module speeds to go above the attainable max velocity. Use the + * DesaturateWheelSpeeds(wpi::array*, + * units::meters_per_second_t) function to rectify this issue. In addition, + * you can leverage the power of C++17 to directly assign the module states to + * variables: + * + * @code{.cpp} + * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds); + * @endcode + */ + wpi::array ToSwerveModuleStates( + const ChassisSpeeds& chassisSpeeds, + const Translation2d& centerOfRotation = Translation2d{}) const; + + SwerveDriveWheelSpeeds ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const override { + return ToSwerveModuleStates(chassisSpeeds); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the + * given module states. This method is often used for odometry -- determining + * the robot's position on the field using data from the real-world speed and + * angle of each module on the robot. + * + * @param moduleStates The state of the modules (as a SwerveModuleState type) + * as measured from respective encoders and gyros. The order of the swerve + * module states should be same as passed into the constructor of this class. + * + * @return The resulting chassis speed. + */ + template ... ModuleStates> + requires(sizeof...(ModuleStates) == NumModules) + ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const { + return this->ToChassisSpeeds( + wpi::array{moduleStates...}); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the + * given module states. This method is often used for odometry -- determining + * the robot's position on the field using data from the real-world speed and + * angle of each module on the robot. + * + * @param moduleStates The state of the modules as an wpi::array of type + * SwerveModuleState, NumModules long as measured from respective encoders + * and gyros. The order of the swerve module states should be same as passed + * into the constructor of this class. + * + * @return The resulting chassis speed. + */ + ChassisSpeeds ToChassisSpeeds(const wpi::array& + moduleStates) const override; + + /** + * Performs forward kinematics to return the resulting Twist2d from the + * given module position deltas. This method is often used for odometry -- + * determining the robot's position on the field using data from the + * real-world position delta and angle of each module on the robot. + * + * @param moduleDeltas The latest change in position of the modules (as a + * SwerveModulePosition type) as measured from respective encoders and gyros. + * The order of the swerve module states should be same as passed into the + * constructor of this class. + * + * @return The resulting Twist2d. + */ + template ... ModuleDeltas> + requires(sizeof...(ModuleDeltas) == NumModules) + Twist2d ToTwist2d(ModuleDeltas&&... moduleDeltas) const { + return this->ToTwist2d( + wpi::array{moduleDeltas...}); + } + + /** + * Performs forward kinematics to return the resulting Twist2d from the + * given module position deltas. This method is often used for odometry -- + * determining the robot's position on the field using data from the + * real-world position delta and angle of each module on the robot. + * + * @param moduleDeltas The latest change in position of the modules (as a + * SwerveModulePosition type) as measured from respective encoders and gyros. + * The order of the swerve module states should be same as passed into the + * constructor of this class. + * + * @return The resulting Twist2d. + */ + Twist2d ToTwist2d( + wpi::array moduleDeltas) const; + + Twist2d ToTwist2d( + const SwerveDriveWheelPositions& start, + const SwerveDriveWheelPositions& end) const override { + auto result = + wpi::array(wpi::empty_array); + for (size_t i = 0; i < NumModules; i++) { + auto startModule = start.positions[i]; + auto endModule = end.positions[i]; + result[i] = {endModule.distance - startModule.distance, endModule.angle}; + } + return ToTwist2d(result); + } + + /** + * Renormalizes the wheel speeds if any individual speed is above the + * specified maximum. + * + * Sometimes, after inverse kinematics, the requested speed + * from one or more modules may be above the max attainable speed for the + * driving motor on that module. To fix this issue, one can reduce all the + * wheel speeds to make sure that all requested module speeds are at-or-below + * the absolute threshold, while maintaining the ratio of speeds between + * modules. + * + * @param moduleStates Reference to array of module states. The array will be + * mutated with the normalized speeds! + * @param attainableMaxSpeed The absolute max speed that a module can reach. + */ + static void DesaturateWheelSpeeds( + wpi::array* moduleStates, + units::meters_per_second_t attainableMaxSpeed); + + /** + * Renormalizes the wheel speeds if any individual speed is above the + * specified maximum, as well as getting rid of joystick saturation at edges + * of joystick. + * + * Sometimes, after inverse kinematics, the requested speed + * from one or more modules may be above the max attainable speed for the + * driving motor on that module. To fix this issue, one can reduce all the + * wheel speeds to make sure that all requested module speeds are at-or-below + * the absolute threshold, while maintaining the ratio of speeds between + * modules. + * + * @param moduleStates Reference to array of module states. The array will be + * mutated with the normalized speeds! + * @param desiredChassisSpeed The desired speed of the robot + * @param attainableMaxModuleSpeed The absolute max speed a module can reach + * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot + * can reach while translating + * @param attainableMaxRobotRotationSpeed The absolute max speed the robot can + * reach while rotating + */ + static void DesaturateWheelSpeeds( + wpi::array* moduleStates, + ChassisSpeeds desiredChassisSpeed, + units::meters_per_second_t attainableMaxModuleSpeed, + units::meters_per_second_t attainableMaxRobotTranslationSpeed, + units::radians_per_second_t attainableMaxRobotRotationSpeed); + + private: + mutable Matrixd m_inverseKinematics; + Eigen::HouseholderQR> m_forwardKinematics; + wpi::array m_modules; + mutable wpi::array m_moduleHeadings; + + mutable Translation2d m_previousCoR; +}; + +extern template class EXPORT_TEMPLATE_DECLARE(TRAJOPT_DLLEXPORT) + SwerveDriveKinematics<4>; + +} // namespace frc + +#include "SwerveDriveKinematics.inc" diff --git a/include/frc/kinematics/SwerveDriveKinematics.inc b/include/frc/kinematics/SwerveDriveKinematics.inc new file mode 100644 index 00000000..b9bf3401 --- /dev/null +++ b/include/frc/kinematics/SwerveDriveKinematics.inc @@ -0,0 +1,175 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "units/math.h" + +namespace frc { + +template +SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...) + -> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>; + +template +void SwerveDriveKinematics::ResetHeadings( + wpi::array moduleHeadings) { + for (size_t i = 0; i < NumModules; i++) { + m_moduleHeadings[i] = moduleHeadings[i]; + } +} + +template +wpi::array +SwerveDriveKinematics::ToSwerveModuleStates( + const ChassisSpeeds& chassisSpeeds, + const Translation2d& centerOfRotation) const { + wpi::array moduleStates(wpi::empty_array); + + if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps && + chassisSpeeds.omega == 0_rad_per_s) { + for (size_t i = 0; i < NumModules; i++) { + moduleStates[i] = {0_mps, m_moduleHeadings[i]}; + } + + return moduleStates; + } + + // We have a new center of rotation. We need to compute the matrix again. + if (centerOfRotation != m_previousCoR) { + for (size_t i = 0; i < NumModules; i++) { + // clang-format off + m_inverseKinematics.template block<2, 3>(i * 2, 0) = + Matrixd<2, 3>{ + {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()}, + {0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}}; + // clang-format on + } + m_previousCoR = centerOfRotation; + } + + Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(), + chassisSpeeds.vy.value(), + chassisSpeeds.omega.value()}; + + Matrixd moduleStateMatrix = + m_inverseKinematics * chassisSpeedsVector; + + for (size_t i = 0; i < NumModules; i++) { + units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)}; + units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)}; + + auto speed = units::math::hypot(x, y); + Rotation2d rotation{x.value(), y.value()}; + + moduleStates[i] = {speed, rotation}; + m_moduleHeadings[i] = rotation; + } + + return moduleStates; +} + +template +ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( + const wpi::array& moduleStates) const { + Matrixd moduleStateMatrix; + + for (size_t i = 0; i < NumModules; ++i) { + SwerveModuleState module = moduleStates[i]; + moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos(); + moduleStateMatrix(i * 2 + 1, 0) = module.speed.value() * module.angle.Sin(); + } + + Eigen::Vector3d chassisSpeedsVector = + m_forwardKinematics.solve(moduleStateMatrix); + + return {units::meters_per_second_t{chassisSpeedsVector(0)}, + units::meters_per_second_t{chassisSpeedsVector(1)}, + units::radians_per_second_t{chassisSpeedsVector(2)}}; +} + +template +Twist2d SwerveDriveKinematics::ToTwist2d( + wpi::array moduleDeltas) const { + Matrixd moduleDeltaMatrix; + + for (size_t i = 0; i < NumModules; ++i) { + SwerveModulePosition module = moduleDeltas[i]; + moduleDeltaMatrix(i * 2, 0) = module.distance.value() * module.angle.Cos(); + moduleDeltaMatrix(i * 2 + 1, 0) = + module.distance.value() * module.angle.Sin(); + } + + Eigen::Vector3d chassisDeltaVector = + m_forwardKinematics.solve(moduleDeltaMatrix); + + return {units::meter_t{chassisDeltaVector(0)}, + units::meter_t{chassisDeltaVector(1)}, + units::radian_t{chassisDeltaVector(2)}}; +} + +template +void SwerveDriveKinematics::DesaturateWheelSpeeds( + wpi::array* moduleStates, + units::meters_per_second_t attainableMaxSpeed) { + auto& states = *moduleStates; + auto realMaxSpeed = + units::math::abs(std::max_element(states.begin(), states.end(), + [](const auto& a, const auto& b) { + return units::math::abs(a.speed) < + units::math::abs(b.speed); + }) + ->speed); + + if (realMaxSpeed > attainableMaxSpeed) { + for (auto& module : states) { + module.speed = module.speed / realMaxSpeed * attainableMaxSpeed; + } + } +} + +template +void SwerveDriveKinematics::DesaturateWheelSpeeds( + wpi::array* moduleStates, + ChassisSpeeds desiredChassisSpeed, + units::meters_per_second_t attainableMaxModuleSpeed, + units::meters_per_second_t attainableMaxRobotTranslationSpeed, + units::radians_per_second_t attainableMaxRobotRotationSpeed) { + auto& states = *moduleStates; + + auto realMaxSpeed = + units::math::abs(std::max_element(states.begin(), states.end(), + [](const auto& a, const auto& b) { + return units::math::abs(a.speed) < + units::math::abs(b.speed); + }) + ->speed); + + if (attainableMaxRobotTranslationSpeed == 0_mps || + attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) { + return; + } + + auto translationalK = + units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) / + attainableMaxRobotTranslationSpeed; + + auto rotationalK = units::math::abs(desiredChassisSpeed.omega) / + attainableMaxRobotRotationSpeed; + + auto k = units::math::max(translationalK, rotationalK); + + auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed, + units::scalar_t{1}); + for (auto& module : states) { + module.speed = module.speed * scale; + } +} + +} // namespace frc diff --git a/include/frc/kinematics/SwerveDriveWheelPositions.h b/include/frc/kinematics/SwerveDriveWheelPositions.h new file mode 100644 index 00000000..ec6d2fe2 --- /dev/null +++ b/include/frc/kinematics/SwerveDriveWheelPositions.h @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "frc/MathExtras.h" +#include "trajopt/SymbolExports.h" +#include "frc/array.h" + +#include "frc/kinematics/SwerveModulePosition.h" + +namespace frc { +/** + * Represents the wheel positions for a swerve drive drivetrain. + */ +template +struct TRAJOPT_DLLEXPORT SwerveDriveWheelPositions { + /** + * The distances driven by the wheels. + */ + wpi::array positions; + + /** + * Checks equality between this SwerveDriveWheelPositions and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const SwerveDriveWheelPositions& other) const = default; + + /** + * Checks inequality between this SwerveDriveWheelPositions and another + * object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const SwerveDriveWheelPositions& other) const = default; + + SwerveDriveWheelPositions Interpolate( + const SwerveDriveWheelPositions& endValue, double t) const { + auto result = + wpi::array(wpi::empty_array); + for (size_t i = 0; i < NumModules; i++) { + result[i] = positions[i].Interpolate(endValue.positions[i], t); + } + return {result}; + } +}; +} // namespace frc diff --git a/include/frc/kinematics/SwerveModulePosition.h b/include/frc/kinematics/SwerveModulePosition.h new file mode 100644 index 00000000..8ac5f690 --- /dev/null +++ b/include/frc/kinematics/SwerveModulePosition.h @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "frc/MathExtras.h" +#include "trajopt/SymbolExports.h" + +#include "frc/geometry/Rotation2d.h" +#include "units/angle.h" +#include "units/length.h" +#include "units/math.h" + +namespace frc { +/** + * Represents the position of one swerve module. + */ +struct TRAJOPT_DLLEXPORT SwerveModulePosition { + /** + * Distance the wheel of a module has traveled + */ + units::meter_t distance = 0_m; + + /** + * Angle of the module. + */ + Rotation2d angle; + + /** + * Checks equality between this SwerveModulePosition and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const SwerveModulePosition& other) const; + + SwerveModulePosition Interpolate(const SwerveModulePosition& endValue, + double t) const { + return {wpi::Lerp(distance, endValue.distance, t), + wpi::Lerp(angle, endValue.angle, t)}; + } +}; +} // namespace frc diff --git a/include/frc/kinematics/SwerveModuleState.h b/include/frc/kinematics/SwerveModuleState.h new file mode 100644 index 00000000..18938e23 --- /dev/null +++ b/include/frc/kinematics/SwerveModuleState.h @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" + +#include "frc/geometry/Rotation2d.h" +#include "units/angle.h" +#include "units/math.h" +#include "units/velocity.h" + +namespace frc { +/** + * Represents the state of one swerve module. + */ +struct TRAJOPT_DLLEXPORT SwerveModuleState { + /** + * Speed of the wheel of the module. + */ + units::meters_per_second_t speed = 0_mps; + + /** + * Angle of the module. + */ + Rotation2d angle; + + /** + * Checks equality between this SwerveModuleState and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const SwerveModuleState& other) const; + + /** + * Minimize the change in heading the desired swerve module state would + * require by potentially reversing the direction the wheel spins. If this is + * used with the PIDController class's continuous input functionality, the + * furthest a wheel will ever rotate is 90 degrees. + * + * @param desiredState The desired state. + * @param currentAngle The current module angle. + */ + static SwerveModuleState Optimize(const SwerveModuleState& desiredState, + const Rotation2d& currentAngle); +}; +} // namespace frc + diff --git a/include/frc/kinematics/WheelPositions.h b/include/frc/kinematics/WheelPositions.h new file mode 100644 index 00000000..8867f666 --- /dev/null +++ b/include/frc/kinematics/WheelPositions.h @@ -0,0 +1,15 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace frc { +template +concept WheelPositions = + std::copy_constructible && requires(T a, T b, double t) { + { a.Interpolate(b, t) } -> std::convertible_to; + }; +} // namespace frc diff --git a/include/frc/spline/CubicHermiteSpline.h b/include/frc/spline/CubicHermiteSpline.h new file mode 100644 index 00000000..486a460d --- /dev/null +++ b/include/frc/spline/CubicHermiteSpline.h @@ -0,0 +1,116 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" +#include + +#include "frc/EigenCore.h" +#include "frc/spline/Spline.h" + +namespace frc { +/** + * Represents a hermite spline of degree 3. + */ +class TRAJOPT_DLLEXPORT CubicHermiteSpline : public Spline<3> { + public: + /** + * Constructs a cubic hermite spline with the specified control vectors. Each + * control vector contains info about the location of the point and its first + * derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + CubicHermiteSpline(wpi::array xInitialControlVector, + wpi::array xFinalControlVector, + wpi::array yInitialControlVector, + wpi::array yFinalControlVector); + + /** + * Returns the coefficients matrix. + * @return The coefficients matrix. + */ + Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; } + + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + const ControlVector& GetInitialControlVector() const override { + return m_initialControlVector; + } + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + const ControlVector& GetFinalControlVector() const override { + return m_finalControlVector; + } + + private: + Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero(); + + ControlVector m_initialControlVector; + ControlVector m_finalControlVector; + + /** + * Returns the hermite basis matrix for cubic hermite spline interpolation. + * @return The hermite basis matrix for cubic hermite spline interpolation. + */ + static Eigen::Matrix4d MakeHermiteBasis() { + // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find + // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀. + // + // P(i) = P(0) = a₀ + // P'(i) = P'(0) = a₁ + // P(i+1) = P(1) = a₃ + a₂ + a₁ + a₀ + // P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁ + // + // [P(i) ] = [0 0 0 1][a₃] + // [P'(i) ] = [0 0 1 0][a₂] + // [P(i+1) ] = [1 1 1 1][a₁] + // [P'(i+1)] = [3 2 1 0][a₀] + // + // To solve for the coefficients, we can invert the 4x4 matrix and move it + // to the other side of the equation. + // + // [a₃] = [ 2 1 -2 1][P(i) ] + // [a₂] = [-3 -2 3 -1][P'(i) ] + // [a₁] = [ 0 1 0 0][P(i+1) ] + // [a₀] = [ 1 0 0 0][P'(i+1)] + + static const Eigen::Matrix4d basis{{+2.0, +1.0, -2.0, +1.0}, + {-3.0, -2.0, +3.0, -1.0}, + {+0.0, +1.0, +0.0, +0.0}, + {+1.0, +0.0, +0.0, +0.0}}; + return basis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * + * @return The control vector matrix for a dimension. + */ + static Eigen::Vector4d ControlVectorFromArrays( + wpi::array initialVector, wpi::array finalVector) { + return Eigen::Vector4d{initialVector[0], initialVector[1], finalVector[0], + finalVector[1]}; + } +}; +} // namespace frc diff --git a/include/frc/spline/QuinticHermiteSpline.h b/include/frc/spline/QuinticHermiteSpline.h new file mode 100644 index 00000000..d48a346a --- /dev/null +++ b/include/frc/spline/QuinticHermiteSpline.h @@ -0,0 +1,126 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "trajopt/SymbolExports.h" +#include + +#include "frc/EigenCore.h" +#include "frc/spline/Spline.h" + +namespace frc { +/** + * Represents a hermite spline of degree 5. + */ +class TRAJOPT_DLLEXPORT QuinticHermiteSpline : public Spline<5> { + public: + /** + * Constructs a quintic hermite spline with the specified control vectors. + * Each control vector contains into about the location of the point, its + * first derivative, and its second derivative. + * + * @param xInitialControlVector The control vector for the initial point in + * the x dimension. + * @param xFinalControlVector The control vector for the final point in + * the x dimension. + * @param yInitialControlVector The control vector for the initial point in + * the y dimension. + * @param yFinalControlVector The control vector for the final point in + * the y dimension. + */ + QuinticHermiteSpline(wpi::array xInitialControlVector, + wpi::array xFinalControlVector, + wpi::array yInitialControlVector, + wpi::array yFinalControlVector); + + /** + * Returns the coefficients matrix. + * @return The coefficients matrix. + */ + Matrixd<6, 6> Coefficients() const override { return m_coefficients; } + + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + const ControlVector& GetInitialControlVector() const override { + return m_initialControlVector; + } + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + const ControlVector& GetFinalControlVector() const override { + return m_finalControlVector; + } + + private: + Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero(); + + ControlVector m_initialControlVector; + ControlVector m_finalControlVector; + + /** + * Returns the hermite basis matrix for quintic hermite spline interpolation. + * @return The hermite basis matrix for quintic hermite spline interpolation. + */ + static Matrixd<6, 6> MakeHermiteBasis() { + // Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors, + // we want to find the coefficients of the spline + // P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀. + // + // P(i) = P(0) = a₀ + // P'(i) = P'(0) = a₁ + // P''(i) = P"(0) = 2a₂ + // P(i+1) = P(1) = a₅ + a₄ + a₃ + a₂ + a₁ + a₀ + // P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁ + // P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂ + // + // [P(i) ] = [ 0 0 0 0 0 1][a₅] + // [P'(i) ] = [ 0 0 0 0 1 0][a₄] + // [P"(i) ] = [ 0 0 0 2 0 0][a₃] + // [P(i+1) ] = [ 1 1 1 1 1 1][a₂] + // [P'(i+1)] = [ 5 4 3 2 1 0][a₁] + // [P"(i+1)] = [20 12 6 2 0 0][a₀] + // + // To solve for the coefficients, we can invert the 6x6 matrix and move it + // to the other side of the equation. + // + // [a₅] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5][P(i) ] + // [a₄] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0][P'(i) ] + // [a₃] = [-10.0 -6.0 -1.5 10.0 -4.0 0.5][P"(i) ] + // [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ] + // [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)] + // [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)] + + static const Matrixd<6, 6> basis{ + {-06.0, -03.0, -00.5, +06.0, -03.0, +00.5}, + {+15.0, +08.0, +01.5, -15.0, +07.0, -01.0}, + {-10.0, -06.0, -01.5, +10.0, -04.0, +00.5}, + {+00.0, +00.0, +00.5, +00.0, +00.0, +00.0}, + {+00.0, +01.0, +00.0, +00.0, +00.0, +00.0}, + {+01.0, +00.0, +00.0, +00.0, +00.0, +00.0}}; + return basis; + } + + /** + * Returns the control vector for each dimension as a matrix from the + * user-provided arrays in the constructor. + * + * @param initialVector The control vector for the initial point. + * @param finalVector The control vector for the final point. + * + * @return The control vector matrix for a dimension. + */ + static Vectord<6> ControlVectorFromArrays(wpi::array initialVector, + wpi::array finalVector) { + return Vectord<6>{initialVector[0], initialVector[1], initialVector[2], + finalVector[0], finalVector[1], finalVector[2]}; + } +}; +} // namespace frc diff --git a/include/frc/spline/Spline.h b/include/frc/spline/Spline.h new file mode 100644 index 00000000..8d300fe8 --- /dev/null +++ b/include/frc/spline/Spline.h @@ -0,0 +1,142 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include + +#include "frc/EigenCore.h" +#include "frc/geometry/Pose2d.h" +#include "units/curvature.h" +#include "units/length.h" + +namespace frc { +/** + * Represents a two-dimensional parametric spline that interpolates between two + * points. + * + * @tparam Degree The degree of the spline. + */ +template +class Spline { + public: + using PoseWithCurvature = std::pair; + + Spline() = default; + + Spline(const Spline&) = default; + Spline& operator=(const Spline&) = default; + + Spline(Spline&&) = default; + Spline& operator=(Spline&&) = default; + + virtual ~Spline() = default; + + /** + * Represents a control vector for a spline. + * + * Each element in each array represents the value of the derivative at the + * index. For example, the value of x[2] is the second derivative in the x + * dimension. + */ + struct ControlVector { + /// The x components of the control vector. + wpi::array x; + + /// The y components of the control vector. + wpi::array y; + }; + + /** + * Gets the pose and curvature at some point t on the spline. + * + * @param t The point t + * @return The pose and curvature at that point. + */ + PoseWithCurvature GetPoint(double t) const { + Vectord polynomialBases; + + // Populate the polynomial bases + for (int i = 0; i <= Degree; i++) { + polynomialBases(i) = std::pow(t, Degree - i); + } + + // This simply multiplies by the coefficients. We need to divide out t some + // n number of times where n is the derivative we want to take. + Vectord<6> combined = Coefficients() * polynomialBases; + + double dx, dy, ddx, ddy; + + // If t = 0, all other terms in the equation cancel out to zero. We can use + // the last x^0 term in the equation. + if (t == 0.0) { + dx = Coefficients()(2, Degree - 1); + dy = Coefficients()(3, Degree - 1); + ddx = Coefficients()(4, Degree - 2); + ddy = Coefficients()(5, Degree - 2); + } else { + // Divide out t for first derivative. + dx = combined(2) / t; + dy = combined(3) / t; + + // Divide out t for second derivative. + ddx = combined(4) / t / t; + ddy = combined(5) / t / t; + } + + // Find the curvature. + const auto curvature = + (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy)); + + return { + {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}}, + units::curvature_t{curvature}}; + } + + /** + * Returns the coefficients of the spline. + * + * @return The coefficients of the spline. + */ + virtual Matrixd<6, Degree + 1> Coefficients() const = 0; + + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + virtual const ControlVector& GetInitialControlVector() const = 0; + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + virtual const ControlVector& GetFinalControlVector() const = 0; + + protected: + /** + * Converts a Translation2d into a vector that is compatible with Eigen. + * + * @param translation The Translation2d to convert. + * @return The vector. + */ + static Eigen::Vector2d ToVector(const Translation2d& translation) { + return Eigen::Vector2d{translation.X().value(), translation.Y().value()}; + } + + /** + * Converts an Eigen vector into a Translation2d. + * + * @param vector The vector to convert. + * @return The Translation2d. + */ + static Translation2d FromVector(const Eigen::Vector2d& vector) { + return Translation2d{units::meter_t{vector(0)}, units::meter_t{vector(1)}}; + } +}; +} // namespace frc diff --git a/include/frc/spline/SplineHelper.h b/include/frc/spline/SplineHelper.h new file mode 100644 index 00000000..34740326 --- /dev/null +++ b/include/frc/spline/SplineHelper.h @@ -0,0 +1,119 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "trajopt/SymbolExports.h" +#include + +#include "frc/spline/CubicHermiteSpline.h" +#include "frc/spline/QuinticHermiteSpline.h" + +namespace frc { +/** + * Helper class that is used to generate cubic and quintic splines from user + * provided waypoints. + */ +class TRAJOPT_DLLEXPORT SplineHelper { + public: + /** + * Returns 2 cubic control vectors from a set of exterior waypoints and + * interior translations. + * + * @param start The starting pose. + * @param interiorWaypoints The interior waypoints. + * @param end The ending pose. + * @return 2 cubic control vectors. + */ + static wpi::array::ControlVector, 2> + CubicControlVectorsFromWaypoints( + const Pose2d& start, const std::vector& interiorWaypoints, + const Pose2d& end); + + /** + * Returns quintic splines from a set of waypoints. + * + * @param waypoints The waypoints + * @return List of quintic splines. + */ + static std::vector QuinticSplinesFromWaypoints( + const std::vector& waypoints); + + /** + * Returns a set of cubic splines corresponding to the provided control + * vectors. The user is free to set the direction of the start and end + * point. The directions for the middle waypoints are determined + * automatically to ensure continuous curvature throughout the path. + * + * The derivation for the algorithm used can be found here: + * + * + * @param start The starting control vector. + * @param waypoints The middle waypoints. This can be left blank if you + * only wish to create a path with two waypoints. + * @param end The ending control vector. + * + * @return A vector of cubic hermite splines that interpolate through the + * provided waypoints. + */ + static std::vector CubicSplinesFromControlVectors( + const Spline<3>::ControlVector& start, + std::vector waypoints, + const Spline<3>::ControlVector& end); + + /** + * Returns a set of quintic splines corresponding to the provided control + * vectors. The user is free to set the direction of all waypoints. Continuous + * curvature is guaranteed throughout the path. + * + * @param controlVectors The control vectors. + * @return A vector of quintic hermite splines that interpolate through the + * provided waypoints. + */ + static std::vector QuinticSplinesFromControlVectors( + const std::vector::ControlVector>& controlVectors); + + /** + * Optimizes the curvature of 2 or more quintic splines at knot points. + * Overall, this reduces the integral of the absolute value of the second + * derivative across the set of splines. + * + * @param splines A vector of un-optimized quintic splines. + * @return A vector of optimized quintic splines. + */ + static std::vector OptimizeCurvature( + const std::vector& splines); + + private: + static Spline<3>::ControlVector CubicControlVector(double scalar, + const Pose2d& point) { + return {{point.X().value(), scalar * point.Rotation().Cos()}, + {point.Y().value(), scalar * point.Rotation().Sin()}}; + } + + static Spline<5>::ControlVector QuinticControlVector(double scalar, + const Pose2d& point) { + return {{point.X().value(), scalar * point.Rotation().Cos(), 0.0}, + {point.Y().value(), scalar * point.Rotation().Sin(), 0.0}}; + } + + /** + * Thomas algorithm for solving tridiagonal systems Af = d. + * + * @param a the values of A above the diagonal + * @param b the values of A on the diagonal + * @param c the values of A below the diagonal + * @param d the vector on the rhs + * @param solutionVector the unknown (solution) vector, modified in-place + */ + static void ThomasAlgorithm(const std::vector& a, + const std::vector& b, + const std::vector& c, + const std::vector& d, + std::vector* solutionVector); +}; +} // namespace frc diff --git a/include/frc/spline/SplineParameterizer.h b/include/frc/spline/SplineParameterizer.h new file mode 100644 index 00000000..40ec94f9 --- /dev/null +++ b/include/frc/spline/SplineParameterizer.h @@ -0,0 +1,142 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include + +#include "trajopt/SymbolExports.h" + +#include "frc/spline/Spline.h" +#include "units/angle.h" +#include "units/curvature.h" +#include "units/length.h" +#include "units/math.h" + +namespace frc { + +/** + * Class used to parameterize a spline by its arc length. + */ +class TRAJOPT_DLLEXPORT SplineParameterizer { + public: + using PoseWithCurvature = std::pair; + + struct MalformedSplineException : public std::runtime_error { + explicit MalformedSplineException(const char* what_arg) + : runtime_error(what_arg) {} + }; + + /** + * Parametrizes the spline. This method breaks up the spline into various + * arcs until their dx, dy, and dtheta are within specific tolerances. + * + * @param spline The spline to parameterize. + * @param t0 Starting internal spline parameter. It is recommended to leave + * this as default. + * @param t1 Ending internal spline parameter. It is recommended to leave this + * as default. + * + * @return A vector of poses and curvatures that represents various points on + * the spline. + */ + template + static std::vector Parameterize(const Spline& spline, + double t0 = 0.0, + double t1 = 1.0) { + std::vector splinePoints; + + // The parameterization does not add the initial point. Let's add that. + splinePoints.push_back(spline.GetPoint(t0)); + + // We use an "explicit stack" to simulate recursion, instead of a recursive + // function call This give us greater control, instead of a stack overflow + std::stack stack; + stack.emplace(StackContents{t0, t1}); + + StackContents current; + PoseWithCurvature start; + PoseWithCurvature end; + int iterations = 0; + + while (!stack.empty()) { + current = stack.top(); + stack.pop(); + start = spline.GetPoint(current.t0); + end = spline.GetPoint(current.t1); + + const auto twist = start.first.Log(end.first); + + if (units::math::abs(twist.dy) > kMaxDy || + units::math::abs(twist.dx) > kMaxDx || + units::math::abs(twist.dtheta) > kMaxDtheta) { + stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1}); + stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2}); + } else { + splinePoints.push_back(spline.GetPoint(current.t1)); + } + + if (iterations++ >= kMaxIterations) { + throw MalformedSplineException( + "Could not parameterize a malformed spline. " + "This means that you probably had two or more adjacent " + "waypoints that were very close together with headings " + "in opposing directions."); + } + } + + return splinePoints; + } + + private: + // Constraints for spline parameterization. + static constexpr units::meter_t kMaxDx = 5_in; + static constexpr units::meter_t kMaxDy = 0.05_in; + static constexpr units::radian_t kMaxDtheta = 0.0872_rad; + + struct StackContents { + double t0; + double t1; + }; + + /** + * A malformed spline does not actually explode the LIFO stack size. Instead, + * the stack size stays at a relatively small number (e.g. 30) and never + * decreases. Because of this, we must count iterations. Even long, complex + * paths don't usually go over 300 iterations, so hitting this maximum should + * definitely indicate something has gone wrong. + */ + static constexpr int kMaxIterations = 5000; + + friend class CubicHermiteSplineTest; + friend class QuinticHermiteSplineTest; +}; +} // namespace frc diff --git a/include/frc/thirdparty/eigen/include/.clang-format b/include/frc/thirdparty/eigen/include/.clang-format new file mode 100644 index 00000000..28251c66 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/.clang-format @@ -0,0 +1,12 @@ +--- +Language: Cpp +BasedOnStyle: Google +ColumnLimit: 120 +SortIncludes: false +AttributeMacros: +- EIGEN_STRONG_INLINE +- EIGEN_ALWAYS_INLINE +- EIGEN_DEVICE_FUNC +- EIGEN_DONT_INLINE +- EIGEN_DEPRECATED +- EIGEN_UNUSED diff --git a/include/frc/thirdparty/eigen/include/Eigen/Cholesky b/include/frc/thirdparty/eigen/include/Eigen/Cholesky new file mode 100644 index 00000000..c3bf8452 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/Cholesky @@ -0,0 +1,43 @@ +// 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_CHOLESKY_MODULE_H +#define EIGEN_CHOLESKY_MODULE_H + +#include "Core" +#include "Jacobi" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup Cholesky_Module Cholesky module + * + * + * + * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. + * Those decompositions are also accessible via the following methods: + * - MatrixBase::llt() + * - MatrixBase::ldlt() + * - SelfAdjointView::llt() + * - SelfAdjointView::ldlt() + * + * \code + * #include + * \endcode + */ + +// IWYU pragma: begin_exports +#include "src/Cholesky/LLT.h" +#include "src/Cholesky/LDLT.h" +#ifdef EIGEN_USE_LAPACKE +// #include "src/misc/lapacke_helpers.h" +// #include "src/Cholesky/LLT_LAPACKE.h" +#endif +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_CHOLESKY_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/Core b/include/frc/thirdparty/eigen/include/Eigen/Core new file mode 100644 index 00000000..a30eedae --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/Core @@ -0,0 +1,414 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2007-2011 Benoit Jacob +// +// 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_CORE_MODULE_H +#define EIGEN_CORE_MODULE_H + +// first thing Eigen does: stop the compiler from reporting useless warnings. +#include "src/Core/util/DisableStupidWarnings.h" + +// then include this file where all our macros are defined. It's really important to do it first because +// it's where we do all the compiler/OS/arch detections and define most defaults. +#include "src/Core/util/Macros.h" + +// This detects SSE/AVX/NEON/etc. and configure alignment settings +#include "src/Core/util/ConfigureVectorization.h" + +// We need cuda_runtime.h/hip_runtime.h to ensure that +// the EIGEN_USING_STD macro works properly on the device side +#if defined(EIGEN_CUDACC) +#include +#elif defined(EIGEN_HIPCC) +#include +#endif + +#ifdef EIGEN_EXCEPTIONS +#include +#endif + +// Disable the ipa-cp-clone optimization flag with MinGW 6.x or older (enabled by default with -O3) +// See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details. +#if EIGEN_COMP_MINGW && EIGEN_GNUC_STRICT_LESS_THAN(6, 0, 0) +#pragma GCC optimize("-fno-ipa-cp-clone") +#endif + +// Prevent ICC from specializing std::complex operators that silently fail +// on device. This allows us to use our own device-compatible specializations +// instead. +#if EIGEN_COMP_ICC && defined(EIGEN_GPU_COMPILE_PHASE) && !defined(_OVERRIDE_COMPLEX_SPECIALIZATION_) +#define _OVERRIDE_COMPLEX_SPECIALIZATION_ 1 +#endif +#include + +// this include file manages BLAS and MKL related macros +// and inclusion of their respective header files +// #include "src/Core/util/MKL_support.h" + +#if defined(EIGEN_HAS_CUDA_FP16) || defined(EIGEN_HAS_HIP_FP16) +#define EIGEN_HAS_GPU_FP16 +#endif + +#if defined(EIGEN_HAS_CUDA_BF16) || defined(EIGEN_HAS_HIP_BF16) +#define EIGEN_HAS_GPU_BF16 +#endif + +#if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE) +#define EIGEN_HAS_OPENMP +#endif + +#ifdef EIGEN_HAS_OPENMP +#include +#include +#endif + +// MSVC for windows mobile does not have the errno.h file +#if !(EIGEN_COMP_MSVC && EIGEN_OS_WINCE) && !EIGEN_COMP_ARM +#define EIGEN_HAS_ERRNO +#endif + +#ifdef EIGEN_HAS_ERRNO +#include +#endif +#include +#include +#include +#include +#ifndef EIGEN_NO_IO +#include +#include +#endif +#include +#include +#include +#include // for CHAR_BIT +// for min/max: +#include + +#include +#include + +// for std::is_nothrow_move_assignable +#include + +// for outputting debug info +#ifdef EIGEN_DEBUG_ASSIGN +#include +#endif + +// required for __cpuid, needs to be included after cmath +// also required for _BitScanReverse on Windows on ARM +#if EIGEN_COMP_MSVC && (EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM64) && !EIGEN_OS_WINCE +#include +#endif + +#if defined(EIGEN_USE_SYCL) +#undef min +#undef max +#undef isnan +#undef isinf +#undef isfinite +#include +#include +#include +#include +#include +#ifndef EIGEN_SYCL_LOCAL_THREAD_DIM0 +#define EIGEN_SYCL_LOCAL_THREAD_DIM0 16 +#endif +#ifndef EIGEN_SYCL_LOCAL_THREAD_DIM1 +#define EIGEN_SYCL_LOCAL_THREAD_DIM1 16 +#endif +#endif + +#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || \ + defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API || \ + defined EIGEN2_SUPPORT +// This will generate an error message: +#error Eigen2-support is only available up to version 3.2. Please go to "http://eigen.tuxfamily.org/index.php?title=Eigen2" for further information +#endif + +namespace Eigen { + +// we use size_t frequently and we'll never remember to prepend it with std:: every time just to +// ensure QNX/QCC support +using std::size_t; +// gcc 4.6.0 wants std:: for ptrdiff_t +using std::ptrdiff_t; + +} // namespace Eigen + +/** \defgroup Core_Module Core module + * This is the main module of Eigen providing dense matrix and vector support + * (both fixed and dynamic size) with all the features corresponding to a BLAS library + * and much more... + * + * \code + * #include + * \endcode + */ + +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +// #include "mkl_lapacke.h" +#else +// #include "src/misc/lapacke.h" +#endif +#endif + +// IWYU pragma: begin_exports +#include "src/Core/util/Constants.h" +#include "src/Core/util/Meta.h" +#include "src/Core/util/Assert.h" +#include "src/Core/util/ForwardDeclarations.h" +#include "src/Core/util/StaticAssert.h" +#include "src/Core/util/XprHelper.h" +#include "src/Core/util/Memory.h" +#include "src/Core/util/IntegralConstant.h" +#include "src/Core/util/Serializer.h" +#include "src/Core/util/SymbolicIndex.h" +#include "src/Core/util/EmulateArray.h" +#include "src/Core/util/MoreMeta.h" + +#include "src/Core/NumTraits.h" +#include "src/Core/MathFunctions.h" +#include "src/Core/GenericPacketMath.h" +#include "src/Core/MathFunctionsImpl.h" +#include "src/Core/arch/Default/ConjHelper.h" +// Generic half float support +#include "src/Core/arch/Default/Half.h" +#include "src/Core/arch/Default/BFloat16.h" +#include "src/Core/arch/Default/GenericPacketMathFunctionsFwd.h" + +#if defined EIGEN_VECTORIZE_AVX512 +#if defined EIGEN_VECTORIZE_AVX512FP16 +// #include "src/Core/arch/AVX512/PacketMathFP16.h" +#endif +#include "src/Core/arch/SSE/PacketMath.h" +#include "src/Core/arch/SSE/TypeCasting.h" +#include "src/Core/arch/SSE/Complex.h" +#include "src/Core/arch/AVX/PacketMath.h" +#include "src/Core/arch/AVX/TypeCasting.h" +#include "src/Core/arch/AVX/Complex.h" +// #include "src/Core/arch/AVX512/PacketMath.h" +// #include "src/Core/arch/AVX512/TypeCasting.h" +// #include "src/Core/arch/AVX512/Complex.h" +#include "src/Core/arch/SSE/MathFunctions.h" +#include "src/Core/arch/AVX/MathFunctions.h" +// #include "src/Core/arch/AVX512/MathFunctions.h" +// #include "src/Core/arch/AVX512/TrsmKernel.h" +#elif defined EIGEN_VECTORIZE_AVX + // Use AVX for floats and doubles, SSE for integers +#include "src/Core/arch/SSE/PacketMath.h" +#include "src/Core/arch/SSE/TypeCasting.h" +#include "src/Core/arch/SSE/Complex.h" +#include "src/Core/arch/AVX/PacketMath.h" +#include "src/Core/arch/AVX/TypeCasting.h" +#include "src/Core/arch/AVX/Complex.h" +#include "src/Core/arch/SSE/MathFunctions.h" +#include "src/Core/arch/AVX/MathFunctions.h" +#elif defined EIGEN_VECTORIZE_SSE +#include "src/Core/arch/SSE/PacketMath.h" +#include "src/Core/arch/SSE/TypeCasting.h" +#include "src/Core/arch/SSE/MathFunctions.h" +#include "src/Core/arch/SSE/Complex.h" +#elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX) +// #include "src/Core/arch/AltiVec/PacketMath.h" +// #include "src/Core/arch/AltiVec/TypeCasting.h" +// #include "src/Core/arch/AltiVec/MathFunctions.h" +// #include "src/Core/arch/AltiVec/Complex.h" +#elif defined EIGEN_VECTORIZE_NEON +#include "src/Core/arch/NEON/PacketMath.h" +#include "src/Core/arch/NEON/TypeCasting.h" +#include "src/Core/arch/NEON/MathFunctions.h" +#include "src/Core/arch/NEON/Complex.h" +#elif defined EIGEN_VECTORIZE_SVE +// #include "src/Core/arch/SVE/PacketMath.h" +// #include "src/Core/arch/SVE/TypeCasting.h" +// #include "src/Core/arch/SVE/MathFunctions.h" +#elif defined EIGEN_VECTORIZE_ZVECTOR +// #include "src/Core/arch/ZVector/PacketMath.h" +// #include "src/Core/arch/ZVector/MathFunctions.h" +// #include "src/Core/arch/ZVector/Complex.h" +#elif defined EIGEN_VECTORIZE_MSA +// #include "src/Core/arch/MSA/PacketMath.h" +// #include "src/Core/arch/MSA/MathFunctions.h" +// #include "src/Core/arch/MSA/Complex.h" +#elif defined EIGEN_VECTORIZE_HVX +// #include "src/Core/arch/HVX/PacketMath.h" +#endif + +#if defined EIGEN_VECTORIZE_GPU +// #include "src/Core/arch/GPU/PacketMath.h" +// #include "src/Core/arch/GPU/MathFunctions.h" +// #include "src/Core/arch/GPU/TypeCasting.h" +#endif + +#if defined(EIGEN_USE_SYCL) +// #include "src/Core/arch/SYCL/InteropHeaders.h" +#if !defined(EIGEN_DONT_VECTORIZE_SYCL) +// #include "src/Core/arch/SYCL/PacketMath.h" +// #include "src/Core/arch/SYCL/MathFunctions.h" +// #include "src/Core/arch/SYCL/TypeCasting.h" +#endif +#endif + +#include "src/Core/arch/Default/Settings.h" +// This file provides generic implementations valid for scalar as well +#include "src/Core/arch/Default/GenericPacketMathFunctions.h" + +#include "src/Core/functors/TernaryFunctors.h" +#include "src/Core/functors/BinaryFunctors.h" +#include "src/Core/functors/UnaryFunctors.h" +#include "src/Core/functors/NullaryFunctors.h" +#include "src/Core/functors/StlFunctors.h" +#include "src/Core/functors/AssignmentFunctors.h" + +// Specialized functors for GPU. +#ifdef EIGEN_GPUCC +// #include "src/Core/arch/GPU/Complex.h" +#endif + +// Specializations of vectorized activation functions for NEON. +#ifdef EIGEN_VECTORIZE_NEON +#include "src/Core/arch/NEON/UnaryFunctors.h" +#endif + +#include "src/Core/util/IndexedViewHelper.h" +#include "src/Core/util/ReshapedHelper.h" +#include "src/Core/ArithmeticSequence.h" +#ifndef EIGEN_NO_IO +#include "src/Core/IO.h" +#endif +#include "src/Core/DenseCoeffsBase.h" +#include "src/Core/DenseBase.h" +#include "src/Core/MatrixBase.h" +#include "src/Core/EigenBase.h" + +#include "src/Core/Product.h" +#include "src/Core/CoreEvaluators.h" +#include "src/Core/AssignEvaluator.h" + +#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874 + // at least confirmed with Doxygen 1.5.5 and 1.5.6 +#include "src/Core/Assign.h" +#endif + +#include "src/Core/ArrayBase.h" +#include "src/Core/util/BlasUtil.h" +#include "src/Core/DenseStorage.h" +#include "src/Core/NestByValue.h" + +// #include "src/Core/ForceAlignedAccess.h" + +#include "src/Core/ReturnByValue.h" +#include "src/Core/NoAlias.h" +#include "src/Core/PlainObjectBase.h" +#include "src/Core/Matrix.h" +#include "src/Core/Array.h" +#include "src/Core/CwiseTernaryOp.h" +#include "src/Core/CwiseBinaryOp.h" +#include "src/Core/CwiseUnaryOp.h" +#include "src/Core/CwiseNullaryOp.h" +#include "src/Core/CwiseUnaryView.h" +#include "src/Core/SelfCwiseBinaryOp.h" +#include "src/Core/Dot.h" +#include "src/Core/StableNorm.h" +#include "src/Core/Stride.h" +#include "src/Core/MapBase.h" +#include "src/Core/Map.h" +#include "src/Core/Ref.h" +#include "src/Core/Block.h" +#include "src/Core/VectorBlock.h" +#include "src/Core/IndexedView.h" +#include "src/Core/Reshaped.h" +#include "src/Core/Transpose.h" +#include "src/Core/DiagonalMatrix.h" +#include "src/Core/Diagonal.h" +#include "src/Core/DiagonalProduct.h" +#include "src/Core/SkewSymmetricMatrix3.h" +#include "src/Core/Redux.h" +#include "src/Core/Visitor.h" +#include "src/Core/Fuzzy.h" +#include "src/Core/Swap.h" +#include "src/Core/CommaInitializer.h" +#include "src/Core/GeneralProduct.h" +#include "src/Core/Solve.h" +#include "src/Core/Inverse.h" +#include "src/Core/SolverBase.h" +#include "src/Core/PermutationMatrix.h" +#include "src/Core/Transpositions.h" +#include "src/Core/TriangularMatrix.h" +#include "src/Core/SelfAdjointView.h" +#include "src/Core/products/GeneralBlockPanelKernel.h" +#ifdef EIGEN_GEMM_THREADPOOL +// #include "ThreadPool" +#endif +#include "src/Core/products/Parallelizer.h" +#include "src/Core/ProductEvaluators.h" +#include "src/Core/products/GeneralMatrixVector.h" +#include "src/Core/products/GeneralMatrixMatrix.h" +#include "src/Core/SolveTriangular.h" +#include "src/Core/products/GeneralMatrixMatrixTriangular.h" +#include "src/Core/products/SelfadjointMatrixVector.h" +#include "src/Core/products/SelfadjointMatrixMatrix.h" +#include "src/Core/products/SelfadjointProduct.h" +#include "src/Core/products/SelfadjointRank2Update.h" +#include "src/Core/products/TriangularMatrixVector.h" +#include "src/Core/products/TriangularMatrixMatrix.h" +#include "src/Core/products/TriangularSolverMatrix.h" +#include "src/Core/products/TriangularSolverVector.h" +#include "src/Core/BandMatrix.h" +#include "src/Core/CoreIterators.h" +#include "src/Core/ConditionEstimator.h" + +#if defined(EIGEN_VECTORIZE_VSX) +// #include "src/Core/arch/AltiVec/MatrixProduct.h" +#elif defined EIGEN_VECTORIZE_NEON +#include "src/Core/arch/NEON/GeneralBlockPanelKernel.h" +#endif + +#if defined(EIGEN_VECTORIZE_AVX512) +// #include "src/Core/arch/AVX512/GemmKernel.h" +#endif + +#if defined(EIGEN_VECTORIZE_HVX) +// #include "src/Core/arch/HVX/GeneralBlockPanelKernel.h" +#endif + +#include "src/Core/Select.h" +#include "src/Core/VectorwiseOp.h" +#include "src/Core/PartialReduxEvaluator.h" +#include "src/Core/Random.h" +#include "src/Core/Replicate.h" +#include "src/Core/Reverse.h" +#include "src/Core/ArrayWrapper.h" +#include "src/Core/StlIterators.h" + +#ifdef EIGEN_USE_BLAS +// #include "src/Core/products/GeneralMatrixMatrix_BLAS.h" +// #include "src/Core/products/GeneralMatrixVector_BLAS.h" +// #include "src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h" +// #include "src/Core/products/SelfadjointMatrixMatrix_BLAS.h" +// #include "src/Core/products/SelfadjointMatrixVector_BLAS.h" +// #include "src/Core/products/TriangularMatrixMatrix_BLAS.h" +// #include "src/Core/products/TriangularMatrixVector_BLAS.h" +// #include "src/Core/products/TriangularSolverMatrix_BLAS.h" +#endif // EIGEN_USE_BLAS + +#ifdef EIGEN_USE_MKL_VML +// #include "src/Core/Assign_MKL.h" +#endif + +#include "src/Core/GlobalFunctions.h" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_CORE_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/Eigenvalues b/include/frc/thirdparty/eigen/include/Eigen/Eigenvalues new file mode 100644 index 00000000..51438ef3 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/Eigenvalues @@ -0,0 +1,63 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_EIGENVALUES_MODULE_H +#define EIGEN_EIGENVALUES_MODULE_H + +#include "Core" + +#include "Cholesky" +#include "Jacobi" +#include "Householder" +#include "LU" +// #include "Geometry" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup Eigenvalues_Module Eigenvalues module + * + * + * + * This module mainly provides various eigenvalue solvers. + * This module also provides some MatrixBase methods, including: + * - MatrixBase::eigenvalues(), + * - MatrixBase::operatorNorm() + * + * \code + * #include + * \endcode + */ + +#include "src/misc/RealSvd2x2.h" + +// IWYU pragma: begin_exports +#include "src/Eigenvalues/Tridiagonalization.h" +#include "src/Eigenvalues/RealSchur.h" +#include "src/Eigenvalues/EigenSolver.h" +#include "src/Eigenvalues/SelfAdjointEigenSolver.h" +#include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" +#include "src/Eigenvalues/HessenbergDecomposition.h" +#include "src/Eigenvalues/ComplexSchur.h" +#include "src/Eigenvalues/ComplexEigenSolver.h" +#include "src/Eigenvalues/RealQZ.h" +#include "src/Eigenvalues/GeneralizedEigenSolver.h" +#include "src/Eigenvalues/MatrixBaseEigenvalues.h" +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +// #include "mkl_lapacke.h" +#else +// #include "src/misc/lapacke.h" +#endif +// #include "src/Eigenvalues/RealSchur_LAPACKE.h" +// #include "src/Eigenvalues/ComplexSchur_LAPACKE.h" +// #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" +#endif +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_EIGENVALUES_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/Householder b/include/frc/thirdparty/eigen/include/Eigen/Householder new file mode 100644 index 00000000..5070e070 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/Householder @@ -0,0 +1,31 @@ +// 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_HOUSEHOLDER_MODULE_H +#define EIGEN_HOUSEHOLDER_MODULE_H + +#include "Core" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup Householder_Module Householder module + * This module provides Householder transformations. + * + * \code + * #include + * \endcode + */ + +// IWYU pragma: begin_exports +#include "src/Householder/Householder.h" +#include "src/Householder/HouseholderSequence.h" +#include "src/Householder/BlockHouseholder.h" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_HOUSEHOLDER_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/IterativeLinearSolvers b/include/frc/thirdparty/eigen/include/Eigen/IterativeLinearSolvers new file mode 100644 index 00000000..fe5159e9 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/IterativeLinearSolvers @@ -0,0 +1,52 @@ +// 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, AccelerateSupport. + * + \code + #include + \endcode + */ + +// IWYU pragma: begin_exports +#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" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/Jacobi b/include/frc/thirdparty/eigen/include/Eigen/Jacobi new file mode 100644 index 00000000..31eb36a7 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/Jacobi @@ -0,0 +1,33 @@ +// 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_JACOBI_MODULE_H +#define EIGEN_JACOBI_MODULE_H + +#include "Core" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup Jacobi_Module Jacobi module + * This module provides Jacobi and Givens rotations. + * + * \code + * #include + * \endcode + * + * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation: + * - MatrixBase::applyOnTheLeft() + * - MatrixBase::applyOnTheRight(). + */ + +// IWYU pragma: begin_exports +#include "src/Jacobi/Jacobi.h" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_JACOBI_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/LU b/include/frc/thirdparty/eigen/include/Eigen/LU new file mode 100644 index 00000000..e58e895d --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/LU @@ -0,0 +1,46 @@ +// 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_LU_MODULE_H +#define EIGEN_LU_MODULE_H + +#include "Core" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup LU_Module LU module + * This module includes %LU decomposition and related notions such as matrix inversion and determinant. + * This module defines the following MatrixBase methods: + * - MatrixBase::inverse() + * - MatrixBase::determinant() + * + * \code + * #include + * \endcode + */ + +#include "src/misc/Kernel.h" +#include "src/misc/Image.h" + +// IWYU pragma: begin_exports +#include "src/LU/FullPivLU.h" +#include "src/LU/PartialPivLU.h" +#ifdef EIGEN_USE_LAPACKE +// #include "src/misc/lapacke_helpers.h" +// #include "src/LU/PartialPivLU_LAPACKE.h" +#endif +#include "src/LU/Determinant.h" +#include "src/LU/InverseImpl.h" + +#if defined EIGEN_VECTORIZE_SSE || defined EIGEN_VECTORIZE_NEON +#include "src/LU/arch/InverseSize4.h" +#endif +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_LU_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/OrderingMethods b/include/frc/thirdparty/eigen/include/Eigen/OrderingMethods new file mode 100644 index 00000000..921b8a01 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/OrderingMethods @@ -0,0 +1,73 @@ +// 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 + */ + +// IWYU pragma: begin_exports +#include "src/OrderingMethods/Amd.h" +#include "src/OrderingMethods/Ordering.h" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_ORDERINGMETHODS_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/QR b/include/frc/thirdparty/eigen/include/Eigen/QR new file mode 100644 index 00000000..9392ecbe --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/QR @@ -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_QR_MODULE_H +#define EIGEN_QR_MODULE_H + +#include "Core" + +#include "Cholesky" +#include "Jacobi" +#include "Householder" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup QR_Module QR module + * + * + * + * This module provides various QR decompositions + * This module also provides some MatrixBase methods, including: + * - MatrixBase::householderQr() + * - MatrixBase::colPivHouseholderQr() + * - MatrixBase::fullPivHouseholderQr() + * + * \code + * #include + * \endcode + */ + +// IWYU pragma: begin_exports +#include "src/QR/HouseholderQR.h" +#include "src/QR/FullPivHouseholderQR.h" +#include "src/QR/ColPivHouseholderQR.h" +#include "src/QR/CompleteOrthogonalDecomposition.h" +#ifdef EIGEN_USE_LAPACKE +// #include "src/misc/lapacke_helpers.h" +// #include "src/QR/HouseholderQR_LAPACKE.h" +// #include "src/QR/ColPivHouseholderQR_LAPACKE.h" +#endif +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_QR_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/SVD b/include/frc/thirdparty/eigen/include/Eigen/SVD new file mode 100644 index 00000000..66a7678e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/SVD @@ -0,0 +1,56 @@ +// 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_SVD_MODULE_H +#define EIGEN_SVD_MODULE_H + +#include "QR" +#include "Householder" +#include "Jacobi" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup SVD_Module SVD module + * + * + * + * This module provides SVD decomposition for matrices (both real and complex). + * Two decomposition algorithms are provided: + * - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very + * slow for larger ones. + * - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast + * for large problems. These decompositions are accessible via the respective classes and following MatrixBase methods: + * - MatrixBase::jacobiSvd() + * - MatrixBase::bdcSvd() + * + * \code + * #include + * \endcode + */ + +// IWYU pragma: begin_exports +#include "src/misc/RealSvd2x2.h" +#include "src/SVD/UpperBidiagonalization.h" +#include "src/SVD/SVDBase.h" +#include "src/SVD/JacobiSVD.h" +#include "src/SVD/BDCSVD.h" +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +// #include "mkl_lapacke.h" +#else +// #include "src/misc/lapacke.h" +#endif +#ifndef EIGEN_USE_LAPACKE_STRICT +// #include "src/SVD/JacobiSVD_LAPACKE.h" +#endif +// #include "src/SVD/BDCSVD_LAPACKE.h" +#endif +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SVD_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/SparseCholesky b/include/frc/thirdparty/eigen/include/Eigen/SparseCholesky new file mode 100644 index 00000000..6abdcd66 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/SparseCholesky @@ -0,0 +1,40 @@ +// 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 + */ + +// IWYU pragma: begin_exports +#include "src/SparseCholesky/SimplicialCholesky.h" +#include "src/SparseCholesky/SimplicialCholesky_impl.h" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSECHOLESKY_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/SparseCore b/include/frc/thirdparty/eigen/include/Eigen/SparseCore new file mode 100644 index 00000000..56a9401a --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/SparseCore @@ -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_SPARSECORE_MODULE_H +#define EIGEN_SPARSECORE_MODULE_H + +#include "Core" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include +#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. + */ + +// IWYU pragma: begin_exports +#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/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" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSECORE_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/SparseLU b/include/frc/thirdparty/eigen/include/Eigen/SparseLU new file mode 100644 index 00000000..6faf1306 --- /dev/null +++ b/include/frc/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" + +// IWYU pragma: begin_exports +#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" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSELU_MODULE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/SparseQR b/include/frc/thirdparty/eigen/include/Eigen/SparseQR new file mode 100644 index 00000000..b4f1cad6 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/SparseQR @@ -0,0 +1,38 @@ +// 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 + * + * + */ + +// IWYU pragma: begin_exports +#include "src/SparseCore/SparseColEtree.h" +#include "src/SparseQR/SparseQR.h" +// IWYU pragma: end_exports + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/InternalHeaderCheck.h b/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/InternalHeaderCheck.h new file mode 100644 index 00000000..5de2b219 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/InternalHeaderCheck.h @@ -0,0 +1,3 @@ +#ifndef EIGEN_CHOLESKY_MODULE_H +#error "Please include Eigen/Cholesky instead of including headers inside the src directory directly." +#endif diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h b/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h new file mode 100644 index 00000000..5d52ab20 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h @@ -0,0 +1,649 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2011 Gael Guennebaud +// Copyright (C) 2009 Keir Mierle +// Copyright (C) 2009 Benoit Jacob +// Copyright (C) 2011 Timothy E. Holy +// +// 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_LDLT_H +#define EIGEN_LDLT_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits > : traits { + typedef MatrixXpr XprKind; + typedef SolverStorage StorageKind; + typedef int StorageIndex; + enum { Flags = 0 }; +}; + +template +struct LDLT_Traits; + +// PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef +enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite }; +} // namespace internal + +/** \ingroup Cholesky_Module + * + * \class LDLT + * + * \brief Robust Cholesky decomposition of a matrix with pivoting + * + * \tparam MatrixType_ the type of the matrix of which to compute the LDL^T Cholesky decomposition + * \tparam UpLo_ the triangular part that will be used for the decomposition: Lower (default) or Upper. + * The other triangular part won't be read. + * + * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite + * matrix \f$ A \f$ such that \f$ A = P^TLDL^*P \f$, where P is a permutation matrix, L + * is lower triangular with a unit diagonal and D is a diagonal matrix. + * + * The decomposition uses pivoting to ensure stability, so that D will have + * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root + * on D also stabilizes the computation. + * + * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky + * decomposition to determine whether a system of equations has a solution. + * + * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism. + * + * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt(), class LLT + */ +template +class LDLT : public SolverBase > { + public: + typedef MatrixType_ MatrixType; + typedef SolverBase Base; + friend class SolverBase; + + EIGEN_GENERIC_PUBLIC_INTERFACE(LDLT) + enum { + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, + UpLo = UpLo_ + }; + typedef Matrix TmpMatrixType; + + typedef Transpositions TranspositionType; + typedef PermutationMatrix PermutationType; + + typedef internal::LDLT_Traits Traits; + + /** \brief Default Constructor. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via LDLT::compute(const MatrixType&). + */ + LDLT() : m_matrix(), m_transpositions(), m_sign(internal::ZeroSign), m_isInitialized(false) {} + + /** \brief Default Constructor with memory preallocation + * + * Like the default constructor but with preallocation of the internal data + * according to the specified problem \a size. + * \sa LDLT() + */ + explicit LDLT(Index size) + : m_matrix(size, size), + m_transpositions(size), + m_temporary(size), + m_sign(internal::ZeroSign), + m_isInitialized(false) {} + + /** \brief Constructor with decomposition + * + * This calculates the decomposition for the input \a matrix. + * + * \sa LDLT(Index size) + */ + template + explicit LDLT(const EigenBase& matrix) + : m_matrix(matrix.rows(), matrix.cols()), + m_transpositions(matrix.rows()), + m_temporary(matrix.rows()), + m_sign(internal::ZeroSign), + m_isInitialized(false) { + compute(matrix.derived()); + } + + /** \brief Constructs a LDLT factorization from a given matrix + * + * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c + * MatrixType is a Eigen::Ref. + * + * \sa LDLT(const EigenBase&) + */ + template + explicit LDLT(EigenBase& matrix) + : m_matrix(matrix.derived()), + m_transpositions(matrix.rows()), + m_temporary(matrix.rows()), + m_sign(internal::ZeroSign), + m_isInitialized(false) { + compute(matrix.derived()); + } + + /** Clear any existing decomposition + * \sa rankUpdate(w,sigma) + */ + void setZero() { m_isInitialized = false; } + + /** \returns a view of the upper triangular matrix U */ + inline typename Traits::MatrixU matrixU() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return Traits::getU(m_matrix); + } + + /** \returns a view of the lower triangular matrix L */ + inline typename Traits::MatrixL matrixL() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return Traits::getL(m_matrix); + } + + /** \returns the permutation matrix P as a transposition sequence. + */ + inline const TranspositionType& transpositionsP() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return m_transpositions; + } + + /** \returns the coefficients of the diagonal matrix D */ + inline Diagonal vectorD() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return m_matrix.diagonal(); + } + + /** \returns true if the matrix is positive (semidefinite) */ + inline bool isPositive() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign; + } + + /** \returns true if the matrix is negative (semidefinite) */ + inline bool isNegative(void) const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign; + } + +#ifdef EIGEN_PARSED_BY_DOXYGEN + /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A. + * + * This function also supports in-place solves using the syntax x = decompositionObject.solve(x) . + * + * \note_about_checking_solutions + * + * More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$ + * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$, + * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then + * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the + * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function + * computes the least-square solution of \f$ A x = b \f$ if \f$ A \f$ is singular. + * + * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt() + */ + template + inline const Solve solve(const MatrixBase& b) const; +#endif + + template + bool solveInPlace(MatrixBase& bAndX) const; + + template + LDLT& compute(const EigenBase& matrix); + + /** \returns an estimate of the reciprocal condition number of the matrix of + * which \c *this is the LDLT decomposition. + */ + RealScalar rcond() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return internal::rcond_estimate_helper(m_l1_norm, *this); + } + + template + LDLT& rankUpdate(const MatrixBase& w, const RealScalar& alpha = 1); + + /** \returns the internal LDLT decomposition matrix + * + * TODO: document the storage layout + */ + inline const MatrixType& matrixLDLT() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return m_matrix; + } + + MatrixType reconstructedMatrix() const; + + /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix + * is self-adjoint. + * + * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as: + * \code x = decomposition.adjoint().solve(b) \endcode + */ + const LDLT& adjoint() const { return *this; } + + EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the factorization failed because of a zero pivot. + */ + ComputationInfo info() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return m_info; + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + void _solve_impl(const RhsType& rhs, DstType& dst) const; + + template + void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const; +#endif + + protected: + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + + /** \internal + * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U. + * The strict upper part is used during the decomposition, the strict lower + * part correspond to the coefficients of L (its diagonal is equal to 1 and + * is not stored), and the diagonal entries correspond to D. + */ + MatrixType m_matrix; + RealScalar m_l1_norm; + TranspositionType m_transpositions; + TmpMatrixType m_temporary; + internal::SignMatrix m_sign; + bool m_isInitialized; + ComputationInfo m_info; +}; + +namespace internal { + +template +struct ldlt_inplace; + +template <> +struct ldlt_inplace { + template + static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) { + using std::abs; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename TranspositionType::StorageIndex IndexType; + eigen_assert(mat.rows() == mat.cols()); + const Index size = mat.rows(); + bool found_zero_pivot = false; + bool ret = true; + + if (size <= 1) { + transpositions.setIdentity(); + if (size == 0) + sign = ZeroSign; + else if (numext::real(mat.coeff(0, 0)) > static_cast(0)) + sign = PositiveSemiDef; + else if (numext::real(mat.coeff(0, 0)) < static_cast(0)) + sign = NegativeSemiDef; + else + sign = ZeroSign; + return true; + } + + for (Index k = 0; k < size; ++k) { + // Find largest diagonal element + Index index_of_biggest_in_corner; + mat.diagonal().tail(size - k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner); + index_of_biggest_in_corner += k; + + transpositions.coeffRef(k) = IndexType(index_of_biggest_in_corner); + if (k != index_of_biggest_in_corner) { + // apply the transposition while taking care to consider only + // the lower triangular part + Index s = size - index_of_biggest_in_corner - 1; // trailing size after the biggest element + mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k)); + mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s)); + std::swap(mat.coeffRef(k, k), mat.coeffRef(index_of_biggest_in_corner, index_of_biggest_in_corner)); + for (Index i = k + 1; i < index_of_biggest_in_corner; ++i) { + Scalar tmp = mat.coeffRef(i, k); + mat.coeffRef(i, k) = numext::conj(mat.coeffRef(index_of_biggest_in_corner, i)); + mat.coeffRef(index_of_biggest_in_corner, i) = numext::conj(tmp); + } + if (NumTraits::IsComplex) + mat.coeffRef(index_of_biggest_in_corner, k) = numext::conj(mat.coeff(index_of_biggest_in_corner, k)); + } + + // partition the matrix: + // A00 | - | - + // lu = A10 | A11 | - + // A20 | A21 | A22 + Index rs = size - k - 1; + Block A21(mat, k + 1, k, rs, 1); + Block A10(mat, k, 0, 1, k); + Block A20(mat, k + 1, 0, rs, k); + + if (k > 0) { + temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint(); + mat.coeffRef(k, k) -= (A10 * temp.head(k)).value(); + if (rs > 0) A21.noalias() -= A20 * temp.head(k); + } + + // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot + // was smaller than the cutoff value. However, since LDLT is not rank-revealing + // we should only make sure that we do not introduce INF or NaN values. + // Remark that LAPACK also uses 0 as the cutoff value. + RealScalar realAkk = numext::real(mat.coeffRef(k, k)); + bool pivot_is_valid = (abs(realAkk) > RealScalar(0)); + + if (k == 0 && !pivot_is_valid) { + // The entire diagonal is zero, there is nothing more to do + // except filling the transpositions, and checking whether the matrix is zero. + sign = ZeroSign; + for (Index j = 0; j < size; ++j) { + transpositions.coeffRef(j) = IndexType(j); + ret = ret && (mat.col(j).tail(size - j - 1).array() == Scalar(0)).all(); + } + return ret; + } + + if ((rs > 0) && pivot_is_valid) + A21 /= realAkk; + else if (rs > 0) + ret = ret && (A21.array() == Scalar(0)).all(); + + if (found_zero_pivot && pivot_is_valid) + ret = false; // factorization failed + else if (!pivot_is_valid) + found_zero_pivot = true; + + if (sign == PositiveSemiDef) { + if (realAkk < static_cast(0)) sign = Indefinite; + } else if (sign == NegativeSemiDef) { + if (realAkk > static_cast(0)) sign = Indefinite; + } else if (sign == ZeroSign) { + if (realAkk > static_cast(0)) + sign = PositiveSemiDef; + else if (realAkk < static_cast(0)) + sign = NegativeSemiDef; + } + } + + return ret; + } + + // Reference for the algorithm: Davis and Hager, "Multiple Rank + // Modifications of a Sparse Cholesky Factorization" (Algorithm 1) + // Trivial rearrangements of their computations (Timothy E. Holy) + // allow their algorithm to work for rank-1 updates even if the + // original matrix is not of full rank. + // Here only rank-1 updates are implemented, to reduce the + // requirement for intermediate storage and improve accuracy + template + static bool updateInPlace(MatrixType& mat, MatrixBase& w, + const typename MatrixType::RealScalar& sigma = 1) { + using numext::isfinite; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + + const Index size = mat.rows(); + eigen_assert(mat.cols() == size && w.size() == size); + + RealScalar alpha = 1; + + // Apply the update + for (Index j = 0; j < size; j++) { + // Check for termination due to an original decomposition of low-rank + if (!(isfinite)(alpha)) break; + + // Update the diagonal terms + RealScalar dj = numext::real(mat.coeff(j, j)); + Scalar wj = w.coeff(j); + RealScalar swj2 = sigma * numext::abs2(wj); + RealScalar gamma = dj * alpha + swj2; + + mat.coeffRef(j, j) += swj2 / alpha; + alpha += swj2 / dj; + + // Update the terms of L + Index rs = size - j - 1; + w.tail(rs) -= wj * mat.col(j).tail(rs); + if (!numext::is_exactly_zero(gamma)) mat.col(j).tail(rs) += (sigma * numext::conj(wj) / gamma) * w.tail(rs); + } + return true; + } + + template + static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, + const typename MatrixType::RealScalar& sigma = 1) { + // Apply the permutation to the input w + tmp = transpositions * w; + + return ldlt_inplace::updateInPlace(mat, tmp, sigma); + } +}; + +template <> +struct ldlt_inplace { + template + static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, + SignMatrix& sign) { + Transpose matt(mat); + return ldlt_inplace::unblocked(matt, transpositions, temp, sign); + } + + template + static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, + const typename MatrixType::RealScalar& sigma = 1) { + Transpose matt(mat); + return ldlt_inplace::update(matt, transpositions, tmp, w.conjugate(), sigma); + } +}; + +template +struct LDLT_Traits { + typedef const TriangularView MatrixL; + typedef const TriangularView MatrixU; + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); } +}; + +template +struct LDLT_Traits { + typedef const TriangularView MatrixL; + typedef const TriangularView MatrixU; + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); } +}; + +} // end namespace internal + +/** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix + */ +template +template +LDLT& LDLT::compute(const EigenBase& a) { + eigen_assert(a.rows() == a.cols()); + const Index size = a.rows(); + + m_matrix = a.derived(); + + // Compute matrix L1 norm = max abs column sum. + m_l1_norm = RealScalar(0); + // TODO move this code to SelfAdjointView + for (Index col = 0; col < size; ++col) { + RealScalar abs_col_sum; + if (UpLo_ == Lower) + abs_col_sum = + m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>(); + else + abs_col_sum = + m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>(); + if (abs_col_sum > m_l1_norm) m_l1_norm = abs_col_sum; + } + + m_transpositions.resize(size); + m_isInitialized = false; + m_temporary.resize(size); + m_sign = internal::ZeroSign; + + m_info = internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign) ? Success + : NumericalIssue; + + m_isInitialized = true; + return *this; +} + +/** Update the LDLT decomposition: given A = L D L^T, efficiently compute the decomposition of A + sigma w w^T. + * \param w a vector to be incorporated into the decomposition. + * \param sigma a scalar, +1 for updates and -1 for "downdates," which correspond to removing previously-added column + * vectors. Optional; default value is +1. \sa setZero() + */ +template +template +LDLT& LDLT::rankUpdate( + const MatrixBase& w, const typename LDLT::RealScalar& sigma) { + typedef typename TranspositionType::StorageIndex IndexType; + const Index size = w.rows(); + if (m_isInitialized) { + eigen_assert(m_matrix.rows() == size); + } else { + m_matrix.resize(size, size); + m_matrix.setZero(); + m_transpositions.resize(size); + for (Index i = 0; i < size; i++) m_transpositions.coeffRef(i) = IndexType(i); + m_temporary.resize(size); + m_sign = sigma >= 0 ? internal::PositiveSemiDef : internal::NegativeSemiDef; + m_isInitialized = true; + } + + internal::ldlt_inplace::update(m_matrix, m_transpositions, m_temporary, w, sigma); + + return *this; +} + +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +template +void LDLT::_solve_impl(const RhsType& rhs, DstType& dst) const { + _solve_impl_transposed(rhs, dst); +} + +template +template +void LDLT::_solve_impl_transposed(const RhsType& rhs, DstType& dst) const { + // dst = P b + dst = m_transpositions * rhs; + + // dst = L^-1 (P b) + // dst = L^-*T (P b) + matrixL().template conjugateIf().solveInPlace(dst); + + // dst = D^-* (L^-1 P b) + // dst = D^-1 (L^-*T P b) + // more precisely, use pseudo-inverse of D (see bug 241) + using std::abs; + const typename Diagonal::RealReturnType vecD(vectorD()); + // In some previous versions, tolerance was set to the max of 1/highest (or rather numeric_limits::min()) + // and the maximal diagonal entry * epsilon as motivated by LAPACK's xGELSS: + // RealScalar tolerance = numext::maxi(vecD.array().abs().maxCoeff() * NumTraits::epsilon(),RealScalar(1) + // / NumTraits::highest()); However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the + // highest diagonal element is not well justified and leads to numerical issues in some cases. Moreover, Lapack's + // xSYTRS routines use 0 for the tolerance. Using numeric_limits::min() gives us more robustness to denormals. + RealScalar tolerance = (std::numeric_limits::min)(); + for (Index i = 0; i < vecD.size(); ++i) { + if (abs(vecD(i)) > tolerance) + dst.row(i) /= vecD(i); + else + dst.row(i).setZero(); + } + + // dst = L^-* (D^-* L^-1 P b) + // dst = L^-T (D^-1 L^-*T P b) + matrixL().transpose().template conjugateIf().solveInPlace(dst); + + // dst = P^T (L^-* D^-* L^-1 P b) = A^-1 b + // dst = P^-T (L^-T D^-1 L^-*T P b) = A^-1 b + dst = m_transpositions.transpose() * dst; +} +#endif + +/** \internal use x = ldlt_object.solve(x); + * + * This is the \em in-place version of solve(). + * + * \param bAndX represents both the right-hand side matrix b and result x. + * + * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD. + * + * This version avoids a copy when the right hand side matrix b is not + * needed anymore. + * + * \sa LDLT::solve(), MatrixBase::ldlt() + */ +template +template +bool LDLT::solveInPlace(MatrixBase& bAndX) const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + eigen_assert(m_matrix.rows() == bAndX.rows()); + + bAndX = this->solve(bAndX); + + return true; +} + +/** \returns the matrix represented by the decomposition, + * i.e., it returns the product: P^T L D L^* P. + * This function is provided for debug purpose. */ +template +MatrixType LDLT::reconstructedMatrix() const { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + const Index size = m_matrix.rows(); + MatrixType res(size, size); + + // P + res.setIdentity(); + res = transpositionsP() * res; + // L^* P + res = matrixU() * res; + // D(L^*P) + res = vectorD().real().asDiagonal() * res; + // L(DL^*P) + res = matrixL() * res; + // P^T (LDL^*P) + res = transpositionsP().transpose() * res; + + return res; +} + +/** \cholesky_module + * \returns the Cholesky decomposition with full pivoting without square root of \c *this + * \sa MatrixBase::ldlt() + */ +template +inline const LDLT::PlainObject, UpLo> +SelfAdjointView::ldlt() const { + return LDLT(m_matrix); +} + +/** \cholesky_module + * \returns the Cholesky decomposition with full pivoting without square root of \c *this + * \sa SelfAdjointView::ldlt() + */ +template +inline const LDLT::PlainObject> MatrixBase::ldlt() const { + return LDLT(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_LDLT_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h b/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h new file mode 100644 index 00000000..01b44769 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h @@ -0,0 +1,514 @@ +// 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_LLT_H +#define EIGEN_LLT_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct traits > : traits { + typedef MatrixXpr XprKind; + typedef SolverStorage StorageKind; + typedef int StorageIndex; + enum { Flags = 0 }; +}; + +template +struct LLT_Traits; +} // namespace internal + +/** \ingroup Cholesky_Module + * + * \class LLT + * + * \brief Standard Cholesky decomposition (LL^T) of a matrix and associated features + * + * \tparam MatrixType_ the type of the matrix of which we are computing the LL^T Cholesky decomposition + * \tparam UpLo_ the triangular part that will be used for the decomposition: Lower (default) or Upper. + * The other triangular part won't be read. + * + * This class performs a LL^T Cholesky decomposition of a symmetric, positive definite + * matrix A such that A = LL^* = U^*U, where L is lower triangular. + * + * While the Cholesky decomposition is particularly useful to solve selfadjoint problems like D^*D x = b, + * for that purpose, we recommend the Cholesky decomposition without square root which is more stable + * and even faster. Nevertheless, this standard Cholesky decomposition remains useful in many other + * situations like generalised eigen problems with hermitian matrices. + * + * Remember that Cholesky decompositions are not rank-revealing. This LLT decomposition is only stable on positive + * definite matrices, use LDLT instead for the semidefinite case. Also, do not use a Cholesky decomposition to determine + * whether a system of equations has a solution. + * + * Example: \include LLT_example.cpp + * Output: \verbinclude LLT_example.out + * + * \b Performance: for best performance, it is recommended to use a column-major storage format + * with the Lower triangular part (the default), or, equivalently, a row-major storage format + * with the Upper triangular part. Otherwise, you might get a 20% slowdown for the full factorization + * step, and rank-updates can be up to 3 times slower. + * + * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism. + * + * Note that during the decomposition, only the lower (or upper, as defined by UpLo_) triangular part of A is + * considered. Therefore, the strict lower part does not have to store correct values. + * + * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT + */ +template +class LLT : public SolverBase > { + public: + typedef MatrixType_ MatrixType; + typedef SolverBase Base; + friend class SolverBase; + + EIGEN_GENERIC_PUBLIC_INTERFACE(LLT) + enum { MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime }; + + enum { PacketSize = internal::packet_traits::size, AlignmentMask = int(PacketSize) - 1, UpLo = UpLo_ }; + + typedef internal::LLT_Traits Traits; + + /** + * \brief Default Constructor. + * + * The default constructor is useful in cases in which the user intends to + * perform decompositions via LLT::compute(const MatrixType&). + */ + LLT() : m_matrix(), m_isInitialized(false) {} + + /** \brief Default Constructor with memory preallocation + * + * Like the default constructor but with preallocation of the internal data + * according to the specified problem \a size. + * \sa LLT() + */ + explicit LLT(Index size) : m_matrix(size, size), m_isInitialized(false) {} + + template + explicit LLT(const EigenBase& matrix) : m_matrix(matrix.rows(), matrix.cols()), m_isInitialized(false) { + compute(matrix.derived()); + } + + /** \brief Constructs a LLT factorization from a given matrix + * + * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when + * \c MatrixType is a Eigen::Ref. + * + * \sa LLT(const EigenBase&) + */ + template + explicit LLT(EigenBase& matrix) : m_matrix(matrix.derived()), m_isInitialized(false) { + compute(matrix.derived()); + } + + /** \returns a view of the upper triangular matrix U */ + inline typename Traits::MatrixU matrixU() const { + eigen_assert(m_isInitialized && "LLT is not initialized."); + return Traits::getU(m_matrix); + } + + /** \returns a view of the lower triangular matrix L */ + inline typename Traits::MatrixL matrixL() const { + eigen_assert(m_isInitialized && "LLT is not initialized."); + return Traits::getL(m_matrix); + } + +#ifdef EIGEN_PARSED_BY_DOXYGEN + /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A. + * + * Since this LLT class assumes anyway that the matrix A is invertible, the solution + * theoretically exists and is unique regardless of b. + * + * Example: \include LLT_solve.cpp + * Output: \verbinclude LLT_solve.out + * + * \sa solveInPlace(), MatrixBase::llt(), SelfAdjointView::llt() + */ + template + inline const Solve solve(const MatrixBase& b) const; +#endif + + template + void solveInPlace(const MatrixBase& bAndX) const; + + template + LLT& compute(const EigenBase& matrix); + + /** \returns an estimate of the reciprocal condition number of the matrix of + * which \c *this is the Cholesky decomposition. + */ + RealScalar rcond() const { + eigen_assert(m_isInitialized && "LLT is not initialized."); + eigen_assert(m_info == Success && "LLT failed because matrix appears to be negative"); + return internal::rcond_estimate_helper(m_l1_norm, *this); + } + + /** \returns the LLT decomposition matrix + * + * TODO: document the storage layout + */ + inline const MatrixType& matrixLLT() const { + eigen_assert(m_isInitialized && "LLT is not initialized."); + return m_matrix; + } + + MatrixType reconstructedMatrix() const; + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the matrix.appears not to be positive definite. + */ + ComputationInfo info() const { + eigen_assert(m_isInitialized && "LLT is not initialized."); + return m_info; + } + + /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix + * is self-adjoint. + * + * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as: + * \code x = decomposition.adjoint().solve(b) \endcode + */ + const LLT& adjoint() const EIGEN_NOEXCEPT { return *this; } + + inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } + inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + + template + LLT& rankUpdate(const VectorType& vec, const RealScalar& sigma = 1); + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + void _solve_impl(const RhsType& rhs, DstType& dst) const; + + template + void _solve_impl_transposed(const RhsType& rhs, DstType& dst) const; +#endif + + protected: + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + + /** \internal + * Used to compute and store L + * The strict upper part is not used and even not initialized. + */ + MatrixType m_matrix; + RealScalar m_l1_norm; + bool m_isInitialized; + ComputationInfo m_info; +}; + +namespace internal { + +template +struct llt_inplace; + +template +static Index llt_rank_update_lower(MatrixType& mat, const VectorType& vec, + const typename MatrixType::RealScalar& sigma) { + using std::sqrt; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::ColXpr ColXpr; + typedef internal::remove_all_t ColXprCleaned; + typedef typename ColXprCleaned::SegmentReturnType ColXprSegment; + typedef Matrix TempVectorType; + typedef typename TempVectorType::SegmentReturnType TempVecSegment; + + Index n = mat.cols(); + eigen_assert(mat.rows() == n && vec.size() == n); + + TempVectorType temp; + + if (sigma > 0) { + // This version is based on Givens rotations. + // It is faster than the other one below, but only works for updates, + // i.e., for sigma > 0 + temp = sqrt(sigma) * vec; + + for (Index i = 0; i < n; ++i) { + JacobiRotation g; + g.makeGivens(mat(i, i), -temp(i), &mat(i, i)); + + Index rs = n - i - 1; + if (rs > 0) { + ColXprSegment x(mat.col(i).tail(rs)); + TempVecSegment y(temp.tail(rs)); + apply_rotation_in_the_plane(x, y, g); + } + } + } else { + temp = vec; + RealScalar beta = 1; + for (Index j = 0; j < n; ++j) { + RealScalar Ljj = numext::real(mat.coeff(j, j)); + RealScalar dj = numext::abs2(Ljj); + Scalar wj = temp.coeff(j); + RealScalar swj2 = sigma * numext::abs2(wj); + RealScalar gamma = dj * beta + swj2; + + RealScalar x = dj + swj2 / beta; + if (x <= RealScalar(0)) return j; + RealScalar nLjj = sqrt(x); + mat.coeffRef(j, j) = nLjj; + beta += swj2 / dj; + + // Update the terms of L + Index rs = n - j - 1; + if (rs) { + temp.tail(rs) -= (wj / Ljj) * mat.col(j).tail(rs); + if (!numext::is_exactly_zero(gamma)) + mat.col(j).tail(rs) = + (nLjj / Ljj) * mat.col(j).tail(rs) + (nLjj * sigma * numext::conj(wj) / gamma) * temp.tail(rs); + } + } + } + return -1; +} + +template +struct llt_inplace { + typedef typename NumTraits::Real RealScalar; + template + static Index unblocked(MatrixType& mat) { + using std::sqrt; + + eigen_assert(mat.rows() == mat.cols()); + const Index size = mat.rows(); + for (Index k = 0; k < size; ++k) { + Index rs = size - k - 1; // remaining size + + Block A21(mat, k + 1, k, rs, 1); + Block A10(mat, k, 0, 1, k); + Block A20(mat, k + 1, 0, rs, k); + + RealScalar x = numext::real(mat.coeff(k, k)); + if (k > 0) x -= A10.squaredNorm(); + if (x <= RealScalar(0)) return k; + mat.coeffRef(k, k) = x = sqrt(x); + if (k > 0 && rs > 0) A21.noalias() -= A20 * A10.adjoint(); + if (rs > 0) A21 /= x; + } + return -1; + } + + template + static Index blocked(MatrixType& m) { + eigen_assert(m.rows() == m.cols()); + Index size = m.rows(); + if (size < 32) return unblocked(m); + + Index blockSize = size / 8; + blockSize = (blockSize / 16) * 16; + blockSize = (std::min)((std::max)(blockSize, Index(8)), Index(128)); + + for (Index k = 0; k < size; k += blockSize) { + // partition the matrix: + // A00 | - | - + // lu = A10 | A11 | - + // A20 | A21 | A22 + Index bs = (std::min)(blockSize, size - k); + Index rs = size - k - bs; + Block A11(m, k, k, bs, bs); + Block A21(m, k + bs, k, rs, bs); + Block A22(m, k + bs, k + bs, rs, rs); + + Index ret; + if ((ret = unblocked(A11)) >= 0) return k + ret; + if (rs > 0) A11.adjoint().template triangularView().template solveInPlace(A21); + if (rs > 0) + A22.template selfadjointView().rankUpdate(A21, + typename NumTraits::Literal(-1)); // bottleneck + } + return -1; + } + + template + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) { + return Eigen::internal::llt_rank_update_lower(mat, vec, sigma); + } +}; + +template +struct llt_inplace { + typedef typename NumTraits::Real RealScalar; + + template + static EIGEN_STRONG_INLINE Index unblocked(MatrixType& mat) { + Transpose matt(mat); + return llt_inplace::unblocked(matt); + } + template + static EIGEN_STRONG_INLINE Index blocked(MatrixType& mat) { + Transpose matt(mat); + return llt_inplace::blocked(matt); + } + template + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) { + Transpose matt(mat); + return llt_inplace::rankUpdate(matt, vec.conjugate(), sigma); + } +}; + +template +struct LLT_Traits { + typedef const TriangularView MatrixL; + typedef const TriangularView MatrixU; + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); } + static bool inplace_decomposition(MatrixType& m) { + return llt_inplace::blocked(m) == -1; + } +}; + +template +struct LLT_Traits { + typedef const TriangularView MatrixL; + typedef const TriangularView MatrixU; + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); } + static bool inplace_decomposition(MatrixType& m) { + return llt_inplace::blocked(m) == -1; + } +}; + +} // end namespace internal + +/** Computes / recomputes the Cholesky decomposition A = LL^* = U^*U of \a matrix + * + * \returns a reference to *this + * + * Example: \include TutorialLinAlgComputeTwice.cpp + * Output: \verbinclude TutorialLinAlgComputeTwice.out + */ +template +template +LLT& LLT::compute(const EigenBase& a) { + eigen_assert(a.rows() == a.cols()); + const Index size = a.rows(); + m_matrix.resize(size, size); + if (!internal::is_same_dense(m_matrix, a.derived())) m_matrix = a.derived(); + + // Compute matrix L1 norm = max abs column sum. + m_l1_norm = RealScalar(0); + // TODO move this code to SelfAdjointView + for (Index col = 0; col < size; ++col) { + RealScalar abs_col_sum; + if (UpLo_ == Lower) + abs_col_sum = + m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>(); + else + abs_col_sum = + m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>(); + if (abs_col_sum > m_l1_norm) m_l1_norm = abs_col_sum; + } + + m_isInitialized = true; + bool ok = Traits::inplace_decomposition(m_matrix); + m_info = ok ? Success : NumericalIssue; + + return *this; +} + +/** Performs a rank one update (or dowdate) of the current decomposition. + * If A = LL^* before the rank one update, + * then after it we have LL^* = A + sigma * v v^* where \a v must be a vector + * of same dimension. + */ +template +template +LLT& LLT::rankUpdate(const VectorType& v, const RealScalar& sigma) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType); + eigen_assert(v.size() == m_matrix.cols()); + eigen_assert(m_isInitialized); + if (internal::llt_inplace::rankUpdate(m_matrix, v, sigma) >= 0) + m_info = NumericalIssue; + else + m_info = Success; + + return *this; +} + +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +template +void LLT::_solve_impl(const RhsType& rhs, DstType& dst) const { + _solve_impl_transposed(rhs, dst); +} + +template +template +void LLT::_solve_impl_transposed(const RhsType& rhs, DstType& dst) const { + dst = rhs; + + matrixL().template conjugateIf().solveInPlace(dst); + matrixU().template conjugateIf().solveInPlace(dst); +} +#endif + +/** \internal use x = llt_object.solve(x); + * + * This is the \em in-place version of solve(). + * + * \param bAndX represents both the right-hand side matrix b and result x. + * + * This version avoids a copy when the right hand side matrix b is not needed anymore. + * + * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here. + * This function will const_cast it, so constness isn't honored here. + * + * \sa LLT::solve(), MatrixBase::llt() + */ +template +template +void LLT::solveInPlace(const MatrixBase& bAndX) const { + eigen_assert(m_isInitialized && "LLT is not initialized."); + eigen_assert(m_matrix.rows() == bAndX.rows()); + matrixL().solveInPlace(bAndX); + matrixU().solveInPlace(bAndX); +} + +/** \returns the matrix represented by the decomposition, + * i.e., it returns the product: L L^*. + * This function is provided for debug purpose. */ +template +MatrixType LLT::reconstructedMatrix() const { + eigen_assert(m_isInitialized && "LLT is not initialized."); + return matrixL() * matrixL().adjoint().toDenseMatrix(); +} + +/** \cholesky_module + * \returns the LLT decomposition of \c *this + * \sa SelfAdjointView::llt() + */ +template +inline const LLT::PlainObject> MatrixBase::llt() const { + return LLT(derived()); +} + +/** \cholesky_module + * \returns the LLT decomposition of \c *this + * \sa SelfAdjointView::llt() + */ +template +inline const LLT::PlainObject, UpLo> SelfAdjointView::llt() + const { + return LLT(m_matrix); +} + +} // end namespace Eigen + +#endif // EIGEN_LLT_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h new file mode 100644 index 00000000..0f45e89e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h @@ -0,0 +1,264 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2017 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_ARITHMETIC_SEQUENCE_H +#define EIGEN_ARITHMETIC_SEQUENCE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +// Helper to cleanup the type of the increment: +template +struct cleanup_seq_incr { + typedef typename cleanup_index_type::type type; +}; + +} // namespace internal + +//-------------------------------------------------------------------------------- +// seq(first,last,incr) and seqN(first,size,incr) +//-------------------------------------------------------------------------------- + +template > +class ArithmeticSequence; + +template +ArithmeticSequence::type, + typename internal::cleanup_index_type::type, + typename internal::cleanup_seq_incr::type> +seqN(FirstType first, SizeType size, IncrType incr); + +/** \class ArithmeticSequence + * \ingroup Core_Module + * + * This class represents an arithmetic progression \f$ a_0, a_1, a_2, ..., a_{n-1}\f$ defined by + * its \em first value \f$ a_0 \f$, its \em size (aka length) \em n, and the \em increment (aka stride) + * that is equal to \f$ a_{i+1}-a_{i}\f$ for any \em i. + * + * It is internally used as the return type of the Eigen::seq and Eigen::seqN functions, and as the input arguments + * of DenseBase::operator()(const RowIndices&, const ColIndices&), and most of the time this is the + * only way it is used. + * + * \tparam FirstType type of the first element, usually an Index, + * but internally it can be a symbolic expression + * \tparam SizeType type representing the size of the sequence, usually an Index + * or a compile time integral constant. Internally, it can also be a symbolic expression + * \tparam IncrType type of the increment, can be a runtime Index, or a compile time integral constant (default is + * compile-time 1) + * + * \sa Eigen::seq, Eigen::seqN, DenseBase::operator()(const RowIndices&, const ColIndices&), class IndexedView + */ +template +class ArithmeticSequence { + public: + ArithmeticSequence(FirstType first, SizeType size) : m_first(first), m_size(size) {} + ArithmeticSequence(FirstType first, SizeType size, IncrType incr) : m_first(first), m_size(size), m_incr(incr) {} + + enum { + SizeAtCompileTime = internal::get_fixed_value::value, + IncrAtCompileTime = internal::get_fixed_value::value + }; + + /** \returns the size, i.e., number of elements, of the sequence */ + Index size() const { return m_size; } + + /** \returns the first element \f$ a_0 \f$ in the sequence */ + Index first() const { return m_first; } + + /** \returns the value \f$ a_i \f$ at index \a i in the sequence. */ + Index operator[](Index i) const { return m_first + i * m_incr; } + + const FirstType& firstObject() const { return m_first; } + const SizeType& sizeObject() const { return m_size; } + const IncrType& incrObject() const { return m_incr; } + + protected: + FirstType m_first; + SizeType m_size; + IncrType m_incr; + + public: + auto reverse() const -> decltype(Eigen::seqN(m_first + (m_size + fix<-1>()) * m_incr, m_size, -m_incr)) { + return seqN(m_first + (m_size + fix<-1>()) * m_incr, m_size, -m_incr); + } +}; + +/** \returns an ArithmeticSequence starting at \a first, of length \a size, and increment \a incr + * + * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */ +template +ArithmeticSequence::type, + typename internal::cleanup_index_type::type, + typename internal::cleanup_seq_incr::type> +seqN(FirstType first, SizeType size, IncrType incr) { + return ArithmeticSequence::type, + typename internal::cleanup_index_type::type, + typename internal::cleanup_seq_incr::type>(first, size, incr); +} + +/** \returns an ArithmeticSequence starting at \a first, of length \a size, and unit increment + * + * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType) */ +template +ArithmeticSequence::type, + typename internal::cleanup_index_type::type> +seqN(FirstType first, SizeType size) { + return ArithmeticSequence::type, + typename internal::cleanup_index_type::type>(first, size); +} + +#ifdef EIGEN_PARSED_BY_DOXYGEN + +/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and with positive (or negative) increment \a + * incr + * + * It is essentially an alias to: + * \code + * seqN(f, (l-f+incr)/incr, incr); + * \endcode + * + * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType) + */ +template +auto seq(FirstType f, LastType l, IncrType incr); + +/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and unit increment + * + * It is essentially an alias to: + * \code + * seqN(f,l-f+1); + * \endcode + * + * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) + */ +template +auto seq(FirstType f, LastType l); + +#else // EIGEN_PARSED_BY_DOXYGEN + +template +auto seq(FirstType f, LastType l) + -> decltype(seqN(typename internal::cleanup_index_type::type(f), + (typename internal::cleanup_index_type::type(l) - + typename internal::cleanup_index_type::type(f) + fix<1>()))) { + return seqN(typename internal::cleanup_index_type::type(f), + (typename internal::cleanup_index_type::type(l) - + typename internal::cleanup_index_type::type(f) + fix<1>())); +} + +template +auto seq(FirstType f, LastType l, IncrType incr) + -> decltype(seqN(typename internal::cleanup_index_type::type(f), + (typename internal::cleanup_index_type::type(l) - + typename internal::cleanup_index_type::type(f) + + typename internal::cleanup_seq_incr::type(incr)) / + typename internal::cleanup_seq_incr::type(incr), + typename internal::cleanup_seq_incr::type(incr))) { + typedef typename internal::cleanup_seq_incr::type CleanedIncrType; + return seqN(typename internal::cleanup_index_type::type(f), + (typename internal::cleanup_index_type::type(l) - + typename internal::cleanup_index_type::type(f) + CleanedIncrType(incr)) / + CleanedIncrType(incr), + CleanedIncrType(incr)); +} + +#endif // EIGEN_PARSED_BY_DOXYGEN + +namespace placeholders { + +/** \cpp11 + * \returns a symbolic ArithmeticSequence representing the last \a size elements with increment \a incr. + * + * It is a shortcut for: \code seqN(last-(size-fix<1>)*incr, size, incr) \endcode + * + * \sa lastN(SizeType), seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */ +template +auto lastN(SizeType size, IncrType incr) + -> decltype(seqN(Eigen::placeholders::last - (size - fix<1>()) * incr, size, incr)) { + return seqN(Eigen::placeholders::last - (size - fix<1>()) * incr, size, incr); +} + +/** \cpp11 + * \returns a symbolic ArithmeticSequence representing the last \a size elements with a unit increment. + * + * It is a shortcut for: \code seq(last+fix<1>-size, last) \endcode + * + * \sa lastN(SizeType,IncrType, seqN(FirstType,SizeType), seq(FirstType,LastType) */ +template +auto lastN(SizeType size) -> decltype(seqN(Eigen::placeholders::last + fix<1>() - size, size)) { + return seqN(Eigen::placeholders::last + fix<1>() - size, size); +} + +} // namespace placeholders + +namespace internal { + +// Convert a symbolic span into a usable one (i.e., remove last/end "keywords") +template +struct make_size_type { + typedef std::conditional_t::value, Index, T> type; +}; + +template +struct IndexedViewCompatibleType, XprSize> { + typedef ArithmeticSequence::type, IncrType> type; +}; + +template +ArithmeticSequence::type, IncrType> makeIndexedViewCompatible( + const ArithmeticSequence& ids, Index size, SpecializedType) { + return ArithmeticSequence::type, IncrType>( + eval_expr_given_size(ids.firstObject(), size), eval_expr_given_size(ids.sizeObject(), size), ids.incrObject()); +} + +template +struct get_compile_time_incr > { + enum { value = get_fixed_value::value }; +}; + +} // end namespace internal + +/** \namespace Eigen::indexing + * \ingroup Core_Module + * + * The sole purpose of this namespace is to be able to import all functions + * and symbols that are expected to be used within operator() for indexing + * and slicing. If you already imported the whole Eigen namespace: + * \code using namespace Eigen; \endcode + * then you are already all set. Otherwise, if you don't want/cannot import + * the whole Eigen namespace, the following line: + * \code using namespace Eigen::indexing; \endcode + * is equivalent to: + * \code + using Eigen::fix; + using Eigen::seq; + using Eigen::seqN; + using Eigen::placeholders::all; + using Eigen::placeholders::last; + using Eigen::placeholders::lastN; // c++11 only + using Eigen::placeholders::lastp1; + \endcode + */ +namespace indexing { +using Eigen::fix; +using Eigen::seq; +using Eigen::seqN; +using Eigen::placeholders::all; +using Eigen::placeholders::last; +using Eigen::placeholders::lastN; +using Eigen::placeholders::lastp1; +} // namespace indexing + +} // end namespace Eigen + +#endif // EIGEN_ARITHMETIC_SEQUENCE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Array.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Array.h new file mode 100644 index 00000000..29c96821 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Array.h @@ -0,0 +1,369 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 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_ARRAY_H +#define EIGEN_ARRAY_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits> + : traits> { + typedef ArrayXpr XprKind; + typedef ArrayBase> XprBase; +}; +} // namespace internal + +/** \class Array + * \ingroup Core_Module + * + * \brief General-purpose arrays with easy API for coefficient-wise operations + * + * The %Array class is very similar to the Matrix class. It provides + * general-purpose one- and two-dimensional arrays. The difference between the + * %Array and the %Matrix class is primarily in the API: the API for the + * %Array class provides easy access to coefficient-wise operations, while the + * API for the %Matrix class provides easy access to linear-algebra + * operations. + * + * See documentation of class Matrix for detailed information on the template parameters + * storage layout. + * + * 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_ARRAY_PLUGIN. + * + * \sa \blank \ref TutorialArrayClass, \ref TopicClassHierarchy + */ +template +class Array : public PlainObjectBase> { + public: + typedef PlainObjectBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Array) + + enum { Options = Options_ }; + typedef typename Base::PlainObject PlainObject; + + protected: + template + friend struct internal::conservative_resize_like_impl; + + using Base::m_storage; + + public: + using Base::base; + using Base::coeff; + using Base::coeffRef; + + /** + * The usage of + * using Base::operator=; + * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped + * the usage of 'using'. This should be done only for operator=. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const EigenBase& other) { + return Base::operator=(other); + } + + /** Set all the entries to \a value. + * \sa DenseBase::setConstant(), DenseBase::fill() + */ + /* This overload is needed because the usage of + * using Base::operator=; + * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped + * the usage of 'using'. This should be done only for operator=. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const Scalar& value) { + Base::setConstant(value); + return *this; + } + + /** Copies the value of the expression \a other into \c *this with automatic resizing. + * + * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized), + * it will be initialized. + * + * Note that copying a row-vector into a vector (and conversely) is allowed. + * The resizing, if any, is then done in the appropriate way so that row-vectors + * remain row-vectors and vectors remain vectors. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const DenseBase& other) { + return Base::_set(other); + } + + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const Array& other) { return Base::_set(other); } + + /** Default constructor. + * + * For fixed-size matrices, does nothing. + * + * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix + * is called a null matrix. This constructor is the unique way to create null matrices: resizing + * a matrix to 0 is not supported. + * + * \sa resize(Index,Index) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array() : Base() { EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + // FIXME is it still needed ?? + /** \internal */ + EIGEN_DEVICE_FUNC Array(internal::constructor_without_unaligned_array_assert) + : Base(internal::constructor_without_unaligned_array_assert()){EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED} +#endif + + EIGEN_DEVICE_FUNC Array(Array && other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) + : Base(std::move(other)) { + } + EIGEN_DEVICE_FUNC Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) { + Base::operator=(std::move(other)); + return *this; + } + + /** \copydoc PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const + * ArgTypes&... args) + * + * Example: \include Array_variadic_ctor_cxx11.cpp + * Output: \verbinclude Array_variadic_ctor_cxx11.out + * + * \sa Array(const std::initializer_list>&) + * \sa Array(const Scalar&), Array(const Scalar&,const Scalar&) + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, + const ArgTypes&... args) + : Base(a0, a1, a2, a3, args...) {} + + /** \brief Constructs an array and initializes it from the coefficients given as initializer-lists grouped by row. + * \cpp11 + * + * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients: + * + * Example: \include Array_initializer_list_23_cxx11.cpp + * Output: \verbinclude Array_initializer_list_23_cxx11.out + * + * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is + * triggered. + * + * In the case of a compile-time column 1D array, implicit transposition from a single row is allowed. + * Therefore Array{{1,2,3,4,5}} is legal and the more verbose syntax + * Array{{1},{2},{3},{4},{5}} can be avoided: + * + * Example: \include Array_initializer_list_vector_cxx11.cpp + * Output: \verbinclude Array_initializer_list_vector_cxx11.out + * + * In the case of fixed-sized arrays, the initializer list sizes must exactly match the array sizes, + * and implicit transposition is allowed for compile-time 1D arrays only. + * + * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Array( + const std::initializer_list>& list) + : Base(list) {} + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Array(const T& x) { + Base::template _init1(x); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const T0& val0, const T1& val1) { + this->template _init2(val0, val1); + } + +#else + /** \brief Constructs a fixed-sized array initialized with coefficients starting at \a data */ + EIGEN_DEVICE_FUNC explicit Array(const Scalar* data); + /** Constructs a vector or row-vector with given dimension. \only_for_vectors + * + * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, + * it is redundant to pass the dimension here, so it makes more sense to use the default + * constructor Array() instead. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Array(Index dim); + /** constructs an initialized 1x1 Array with the given coefficient + * \sa const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args */ + Array(const Scalar& value); + /** constructs an uninitialized array with \a rows rows and \a cols columns. + * + * This is useful for dynamic-size arrays. For fixed-size arrays, + * it is redundant to pass these parameters, so one should use the default constructor + * Array() instead. */ + Array(Index rows, Index cols); + /** constructs an initialized 2D vector with given coefficients + * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) */ + Array(const Scalar& val0, const Scalar& val1); +#endif // end EIGEN_PARSED_BY_DOXYGEN + + /** constructs an initialized 3D vector with given coefficients + * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 3) + m_storage.data()[0] = val0; + m_storage.data()[1] = val1; + m_storage.data()[2] = val2; + } + /** constructs an initialized 4D vector with given coefficients + * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2, + const Scalar& val3) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 4) + m_storage.data()[0] = val0; + m_storage.data()[1] = val1; + m_storage.data()[2] = val2; + m_storage.data()[3] = val3; + } + + /** Copy constructor */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Array& other) : Base(other) {} + + private: + struct PrivateType {}; + + public: + /** \sa MatrixBase::operator=(const EigenBase&) */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array( + const EigenBase& other, + std::enable_if_t::value, PrivateType> = + PrivateType()) + : Base(other.derived()) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return 1; } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); } + +#ifdef EIGEN_ARRAY_PLUGIN +#include EIGEN_ARRAY_PLUGIN +#endif + + private: + template + friend struct internal::matrix_swap_impl; +}; + +/** \defgroup arraytypedefs Global array typedefs + * \ingroup Core_Module + * + * %Eigen defines several typedef shortcuts for most common 1D and 2D array types. + * + * The general patterns are the following: + * + * \c ArrayRowsColsType where \c Rows and \c Cols can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for + * dynamic size, and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c + * cd for complex double. + * + * For example, \c Array33d is a fixed-size 3x3 array type of doubles, and \c ArrayXXf is a dynamic-size matrix of + * floats. + * + * There are also \c ArraySizeType which are self-explanatory. For example, \c Array4cf is + * a fixed-size 1D array of 4 complex floats. + * + * With \cpp11, template alias are also defined for common sizes. + * They follow the same pattern as above except that the scalar type suffix is replaced by a + * template parameter, i.e.: + * - `ArrayRowsCols` where `Rows` and `Cols` can be \c 2,\c 3,\c 4, or \c X for fixed or dynamic size. + * - `ArraySize` where `Size` can be \c 2,\c 3,\c 4 or \c X for fixed or dynamic size 1D arrays. + * + * \sa class Array + */ + +#define EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ + /** \ingroup arraytypedefs */ \ + typedef Array Array##SizeSuffix##SizeSuffix##TypeSuffix; \ + /** \ingroup arraytypedefs */ \ + typedef Array Array##SizeSuffix##TypeSuffix; + +#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \ + /** \ingroup arraytypedefs */ \ + typedef Array Array##Size##X##TypeSuffix; \ + /** \ingroup arraytypedefs */ \ + typedef Array Array##X##Size##TypeSuffix; + +#define EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ + EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 2, 2) \ + EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 3, 3) \ + EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 4, 4) \ + EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Dynamic, X) \ + EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 2) \ + EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 3) \ + EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 4) + +EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(int, i) +EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(float, f) +EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(double, d) +EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(std::complex, cf) +EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(std::complex, cd) + +#undef EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES +#undef EIGEN_MAKE_ARRAY_TYPEDEFS +#undef EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS + +#define EIGEN_MAKE_ARRAY_TYPEDEFS(Size, SizeSuffix) \ + /** \ingroup arraytypedefs */ \ + /** \brief \cpp11 */ \ + template \ + using Array##SizeSuffix##SizeSuffix = Array; \ + /** \ingroup arraytypedefs */ \ + /** \brief \cpp11 */ \ + template \ + using Array##SizeSuffix = Array; + +#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Size) \ + /** \ingroup arraytypedefs */ \ + /** \brief \cpp11 */ \ + template \ + using Array##Size##X = Array; \ + /** \ingroup arraytypedefs */ \ + /** \brief \cpp11 */ \ + template \ + using Array##X##Size = Array; + +EIGEN_MAKE_ARRAY_TYPEDEFS(2, 2) +EIGEN_MAKE_ARRAY_TYPEDEFS(3, 3) +EIGEN_MAKE_ARRAY_TYPEDEFS(4, 4) +EIGEN_MAKE_ARRAY_TYPEDEFS(Dynamic, X) +EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(2) +EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(3) +EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(4) + +#undef EIGEN_MAKE_ARRAY_TYPEDEFS +#undef EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS + +#define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \ + using Eigen::Matrix##SizeSuffix##TypeSuffix; \ + using Eigen::Vector##SizeSuffix##TypeSuffix; \ + using Eigen::RowVector##SizeSuffix##TypeSuffix; + +#define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(TypeSuffix) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) + +#define EIGEN_USING_ARRAY_TYPEDEFS \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(i) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(f) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(d) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cf) \ + EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cd) + +} // end namespace Eigen + +#endif // EIGEN_ARRAY_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h new file mode 100644 index 00000000..6237df45 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h @@ -0,0 +1,222 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 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_ARRAYBASE_H +#define EIGEN_ARRAYBASE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +template +class MatrixWrapper; + +/** \class ArrayBase + * \ingroup Core_Module + * + * \brief Base class for all 1D and 2D array, and related expressions + * + * An array is similar to a dense vector or matrix. While matrices are mathematical + * objects with well defined linear algebra operators, an array is just a collection + * of scalar values arranged in a one or two dimensional fashion. As the main consequence, + * all operations applied to an array are performed coefficient wise. Furthermore, + * arrays support scalar math functions of the c++ standard library (e.g., std::sin(x)), and convenient + * constructors allowing to easily write generic code working for both scalar values + * and arrays. + * + * This class is the base that is inherited by all array expression types. + * + * \tparam Derived is the derived type, e.g., an array or an expression type. + * + * 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_ARRAYBASE_PLUGIN. + * + * \sa class MatrixBase, \ref TopicClassHierarchy + */ +template +class ArrayBase : public DenseBase { + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** The base class for a given storage type. */ + typedef ArrayBase StorageBaseType; + + typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl; + + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::packet_traits::type PacketScalar; + typedef typename NumTraits::Real RealScalar; + + typedef DenseBase Base; + using Base::ColsAtCompileTime; + using Base::Flags; + using Base::IsVectorAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxSizeAtCompileTime; + using Base::RowsAtCompileTime; + using Base::SizeAtCompileTime; + + using Base::coeff; + using Base::coeffRef; + using Base::cols; + using Base::const_cast_derived; + using Base::derived; + using Base::lazyAssign; + using Base::rows; + using Base::size; + using Base::operator-; + using Base::operator=; + using Base::operator+=; + using Base::operator-=; + using Base::operator*=; + using Base::operator/=; + + typedef typename Base::CoeffReturnType CoeffReturnType; + +#endif // not EIGEN_PARSED_BY_DOXYGEN + +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename Base::PlainObject PlainObject; + + /** \internal Represents a matrix with all coefficients equal to one another*/ + typedef CwiseNullaryOp, PlainObject> ConstantReturnType; +#endif // not EIGEN_PARSED_BY_DOXYGEN + +#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::ArrayBase +#define EIGEN_DOC_UNARY_ADDONS(X, Y) +#include "../plugins/MatrixCwiseUnaryOps.inc" +#include "../plugins/ArrayCwiseUnaryOps.inc" +#include "../plugins/CommonCwiseBinaryOps.inc" +#include "../plugins/MatrixCwiseBinaryOps.inc" +#include "../plugins/ArrayCwiseBinaryOps.inc" +#ifdef EIGEN_ARRAYBASE_PLUGIN +#include EIGEN_ARRAYBASE_PLUGIN +#endif +#undef EIGEN_CURRENT_STORAGE_BASE_CLASS +#undef EIGEN_DOC_UNARY_ADDONS + + /** Special case of the template operator=, in order to prevent the compiler + * from generating a default operator= (issue hit with g++ 4.1) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const ArrayBase& other) { + internal::call_assignment(derived(), other.derived()); + return derived(); + } + + /** Set all the entries to \a value. + * \sa DenseBase::setConstant(), DenseBase::fill() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Scalar& value) { + Base::setConstant(value); + return derived(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const Scalar& scalar); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const Scalar& scalar); + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const ArrayBase& other); + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const ArrayBase& other); + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const ArrayBase& other); + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const ArrayBase& other); + + public: + EIGEN_DEVICE_FUNC ArrayBase& array() { return *this; } + EIGEN_DEVICE_FUNC const ArrayBase& array() const { return *this; } + + /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array + * \sa MatrixBase::array() */ + EIGEN_DEVICE_FUNC MatrixWrapper matrix() { return MatrixWrapper(derived()); } + EIGEN_DEVICE_FUNC const MatrixWrapper matrix() const { + return MatrixWrapper(derived()); + } + + // template + // inline void evalTo(Dest& dst) const { dst = matrix(); } + + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(ArrayBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(ArrayBase) + + private: + explicit ArrayBase(Index); + ArrayBase(Index, Index); + template + explicit ArrayBase(const ArrayBase&); + + protected: + // mixing arrays and matrices is not legal + template + Derived& operator+=(const MatrixBase&) { + EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1, + YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); + return *this; + } + // mixing arrays and matrices is not legal + template + Derived& operator-=(const MatrixBase&) { + EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1, + YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); + return *this; + } +}; + +/** replaces \c *this by \c *this - \a other. + * + * \returns a reference to \c *this + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::sub_assign_op()); + return derived(); +} + +/** replaces \c *this by \c *this + \a other. + * + * \returns a reference to \c *this + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::add_assign_op()); + return derived(); +} + +/** replaces \c *this by \c *this * \a other coefficient wise. + * + * \returns a reference to \c *this + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator*=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::mul_assign_op()); + return derived(); +} + +/** replaces \c *this by \c *this / \a other coefficient wise. + * + * \returns a reference to \c *this + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator/=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::div_assign_op()); + return derived(); +} + +} // end namespace Eigen + +#endif // EIGEN_ARRAYBASE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h new file mode 100644 index 00000000..b45395d7 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h @@ -0,0 +1,173 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-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/. + +#ifndef EIGEN_ARRAYWRAPPER_H +#define EIGEN_ARRAYWRAPPER_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class ArrayWrapper + * \ingroup Core_Module + * + * \brief Expression of a mathematical vector or matrix as an array object + * + * This class is the return type of MatrixBase::array(), and most of the time + * this is the only way it is use. + * + * \sa MatrixBase::array(), class MatrixWrapper + */ + +namespace internal { +template +struct traits > : public traits > { + typedef ArrayXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits >::Flags, + LvalueBitFlag = is_lvalue::value ? LvalueBit : 0, + Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag + }; +}; +} // namespace internal + +template +class ArrayWrapper : public ArrayBase > { + public: + typedef ArrayBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(ArrayWrapper) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ArrayWrapper) + typedef internal::remove_all_t NestedExpression; + + typedef std::conditional_t::value, Scalar, const Scalar> + ScalarWithConstIfNotLvalue; + + typedef typename internal::ref_selector::non_const_type NestedExpressionType; + + using Base::coeffRef; + + EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { + return m_expression.outerStride(); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { + return m_expression.innerStride(); + } + + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_expression.data(); } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { + return m_expression.coeffRef(rowId, colId); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { return m_expression.coeffRef(index); } + + template + EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const { + dst = m_expression; + } + + EIGEN_DEVICE_FUNC const internal::remove_all_t& nestedExpression() const { + return m_expression; + } + + /** Forwards the resizing request to the nested expression + * \sa DenseBase::resize(Index) */ + EIGEN_DEVICE_FUNC void resize(Index newSize) { m_expression.resize(newSize); } + /** Forwards the resizing request to the nested expression + * \sa DenseBase::resize(Index,Index)*/ + EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) { m_expression.resize(rows, cols); } + + protected: + NestedExpressionType m_expression; +}; + +/** \class MatrixWrapper + * \ingroup Core_Module + * + * \brief Expression of an array as a mathematical vector or matrix + * + * This class is the return type of ArrayBase::matrix(), and most of the time + * this is the only way it is use. + * + * \sa MatrixBase::matrix(), class ArrayWrapper + */ + +namespace internal { +template +struct traits > : public traits > { + typedef MatrixXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits >::Flags, + LvalueBitFlag = is_lvalue::value ? LvalueBit : 0, + Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag + }; +}; +} // namespace internal + +template +class MatrixWrapper : public MatrixBase > { + public: + typedef MatrixBase > Base; + EIGEN_DENSE_PUBLIC_INTERFACE(MatrixWrapper) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MatrixWrapper) + typedef internal::remove_all_t NestedExpression; + + typedef std::conditional_t::value, Scalar, const Scalar> + ScalarWithConstIfNotLvalue; + + typedef typename internal::ref_selector::non_const_type NestedExpressionType; + + using Base::coeffRef; + + EIGEN_DEVICE_FUNC explicit inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { + return m_expression.outerStride(); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { + return m_expression.innerStride(); + } + + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_expression.data(); } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { + return m_expression.derived().coeffRef(rowId, colId); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { return m_expression.coeffRef(index); } + + EIGEN_DEVICE_FUNC const internal::remove_all_t& nestedExpression() const { + return m_expression; + } + + /** Forwards the resizing request to the nested expression + * \sa DenseBase::resize(Index) */ + EIGEN_DEVICE_FUNC void resize(Index newSize) { m_expression.resize(newSize); } + /** Forwards the resizing request to the nested expression + * \sa DenseBase::resize(Index,Index)*/ + EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) { m_expression.resize(rows, cols); } + + protected: + NestedExpressionType m_expression; +}; + +} // end namespace Eigen + +#endif // EIGEN_ARRAYWRAPPER_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Assign.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Assign.h new file mode 100644 index 00000000..4b30f7bb --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Assign.h @@ -0,0 +1,80 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2007 Michael Olbrich +// Copyright (C) 2006-2010 Benoit Jacob +// 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_ASSIGN_H +#define EIGEN_ASSIGN_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::lazyAssign(const DenseBase& other) { + enum { SameType = internal::is_same::value }; + + EIGEN_STATIC_ASSERT_LVALUE(Derived) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived, OtherDerived) + EIGEN_STATIC_ASSERT( + SameType, + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + eigen_assert(rows() == other.rows() && cols() == other.cols()); + internal::call_assignment_no_alias(derived(), other.derived()); + + return derived(); +} + +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) { + internal::call_assignment(derived(), other.derived()); + return derived(); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) { + internal::call_assignment(derived(), other.derived()); + return derived(); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const MatrixBase& other) { + internal::call_assignment(derived(), other.derived()); + return derived(); +} + +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const DenseBase& other) { + internal::call_assignment(derived(), other.derived()); + return derived(); +} + +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const EigenBase& other) { + internal::call_assignment(derived(), other.derived()); + return derived(); +} + +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=( + const ReturnByValue& other) { + other.derived().evalTo(derived()); + return derived(); +} + +} // end namespace Eigen + +#endif // EIGEN_ASSIGN_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h new file mode 100644 index 00000000..f7f0b238 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h @@ -0,0 +1,951 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob +// Copyright (C) 2011-2014 Gael Guennebaud +// Copyright (C) 2011-2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_ASSIGN_EVALUATOR_H +#define EIGEN_ASSIGN_EVALUATOR_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +// This implementation is based on Assign.h + +namespace internal { + +/*************************************************************************** + * Part 1 : the logic deciding a strategy for traversal and unrolling * + ***************************************************************************/ + +// copy_using_evaluator_traits is based on assign_traits + +template +struct copy_using_evaluator_traits { + typedef typename DstEvaluator::XprType Dst; + typedef typename Dst::Scalar DstScalar; + + enum { DstFlags = DstEvaluator::Flags, SrcFlags = SrcEvaluator::Flags }; + + public: + enum { + DstAlignment = DstEvaluator::Alignment, + SrcAlignment = SrcEvaluator::Alignment, + DstHasDirectAccess = (DstFlags & DirectAccessBit) == DirectAccessBit, + JointAlignment = plain_enum_min(DstAlignment, SrcAlignment) + }; + + private: + enum { + InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime) + : int(DstFlags) & RowMajorBit ? int(Dst::ColsAtCompileTime) + : int(Dst::RowsAtCompileTime), + InnerMaxSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime) + : int(DstFlags) & RowMajorBit ? int(Dst::MaxColsAtCompileTime) + : int(Dst::MaxRowsAtCompileTime), + RestrictedInnerSize = min_size_prefer_fixed(InnerSize, MaxPacketSize), + RestrictedLinearSize = min_size_prefer_fixed(Dst::SizeAtCompileTime, MaxPacketSize), + OuterStride = int(outer_stride_at_compile_time::ret), + MaxSizeAtCompileTime = Dst::SizeAtCompileTime + }; + + // TODO distinguish between linear traversal and inner-traversals + typedef typename find_best_packet::type LinearPacketType; + typedef typename find_best_packet::type InnerPacketType; + + enum { + LinearPacketSize = unpacket_traits::size, + InnerPacketSize = unpacket_traits::size + }; + + public: + enum { + LinearRequiredAlignment = unpacket_traits::alignment, + InnerRequiredAlignment = unpacket_traits::alignment + }; + + private: + enum { + DstIsRowMajor = DstFlags & RowMajorBit, + SrcIsRowMajor = SrcFlags & RowMajorBit, + StorageOrdersAgree = (int(DstIsRowMajor) == int(SrcIsRowMajor)), + MightVectorize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & ActualPacketAccessBit) && + bool(functor_traits::PacketAccess), + MayInnerVectorize = MightVectorize && int(InnerSize) != Dynamic && int(InnerSize) % int(InnerPacketSize) == 0 && + int(OuterStride) != Dynamic && int(OuterStride) % int(InnerPacketSize) == 0 && + (EIGEN_UNALIGNED_VECTORIZE || int(JointAlignment) >= int(InnerRequiredAlignment)), + MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit), + MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize) && bool(DstHasDirectAccess) && + (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment) >= int(LinearRequiredAlignment)) || + MaxSizeAtCompileTime == Dynamic), + /* If the destination isn't aligned, we have to do runtime checks and we don't unroll, + so it's only good for large enough sizes. */ + MaySliceVectorize = bool(MightVectorize) && bool(DstHasDirectAccess) && + (int(InnerMaxSize) == Dynamic || + int(InnerMaxSize) >= (EIGEN_UNALIGNED_VECTORIZE ? InnerPacketSize : (3 * InnerPacketSize))) + /* slice vectorization can be slow, so we only want it if the slices are big, which is + indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block + in a fixed-size matrix + However, with EIGEN_UNALIGNED_VECTORIZE and unrolling, slice vectorization is still worth it */ + }; + + public: + enum { + Traversal = int(Dst::SizeAtCompileTime) == 0 + ? int(AllAtOnceTraversal) // If compile-size is zero, traversing will fail at compile-time. + : (int(MayLinearVectorize) && (LinearPacketSize > InnerPacketSize)) ? int(LinearVectorizedTraversal) + : int(MayInnerVectorize) ? int(InnerVectorizedTraversal) + : int(MayLinearVectorize) ? int(LinearVectorizedTraversal) + : int(MaySliceVectorize) ? int(SliceVectorizedTraversal) + : int(MayLinearize) ? int(LinearTraversal) + : int(DefaultTraversal), + Vectorized = int(Traversal) == InnerVectorizedTraversal || int(Traversal) == LinearVectorizedTraversal || + int(Traversal) == SliceVectorizedTraversal + }; + + typedef std::conditional_t PacketType; + + private: + enum { + ActualPacketSize = int(Traversal) == LinearVectorizedTraversal ? LinearPacketSize + : Vectorized ? InnerPacketSize + : 1, + UnrollingLimit = EIGEN_UNROLLING_LIMIT * ActualPacketSize, + MayUnrollCompletely = + int(Dst::SizeAtCompileTime) != Dynamic && + int(Dst::SizeAtCompileTime) * (int(DstEvaluator::CoeffReadCost) + int(SrcEvaluator::CoeffReadCost)) <= + int(UnrollingLimit), + MayUnrollInner = + int(InnerSize) != Dynamic && + int(InnerSize) * (int(DstEvaluator::CoeffReadCost) + int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit) + }; + + public: + enum { + Unrolling = (int(Traversal) == int(InnerVectorizedTraversal) || int(Traversal) == int(DefaultTraversal)) + ? (int(MayUnrollCompletely) ? int(CompleteUnrolling) + : int(MayUnrollInner) ? int(InnerUnrolling) + : int(NoUnrolling)) + : int(Traversal) == int(LinearVectorizedTraversal) + ? (bool(MayUnrollCompletely) && + (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment) >= int(LinearRequiredAlignment))) + ? int(CompleteUnrolling) + : int(NoUnrolling)) + : int(Traversal) == int(LinearTraversal) + ? (bool(MayUnrollCompletely) ? int(CompleteUnrolling) : int(NoUnrolling)) +#if EIGEN_UNALIGNED_VECTORIZE + : int(Traversal) == int(SliceVectorizedTraversal) + ? (bool(MayUnrollInner) ? int(InnerUnrolling) : int(NoUnrolling)) +#endif + : int(NoUnrolling) + }; + +#ifdef EIGEN_DEBUG_ASSIGN + static void debug() { + std::cerr << "DstXpr: " << typeid(typename DstEvaluator::XprType).name() << std::endl; + std::cerr << "SrcXpr: " << typeid(typename SrcEvaluator::XprType).name() << std::endl; + std::cerr.setf(std::ios::hex, std::ios::basefield); + std::cerr << "DstFlags" + << " = " << DstFlags << " (" << demangle_flags(DstFlags) << " )" << std::endl; + std::cerr << "SrcFlags" + << " = " << SrcFlags << " (" << demangle_flags(SrcFlags) << " )" << std::endl; + std::cerr.unsetf(std::ios::hex); + EIGEN_DEBUG_VAR(DstAlignment) + EIGEN_DEBUG_VAR(SrcAlignment) + EIGEN_DEBUG_VAR(LinearRequiredAlignment) + EIGEN_DEBUG_VAR(InnerRequiredAlignment) + EIGEN_DEBUG_VAR(JointAlignment) + EIGEN_DEBUG_VAR(InnerSize) + EIGEN_DEBUG_VAR(InnerMaxSize) + EIGEN_DEBUG_VAR(LinearPacketSize) + EIGEN_DEBUG_VAR(InnerPacketSize) + EIGEN_DEBUG_VAR(ActualPacketSize) + EIGEN_DEBUG_VAR(StorageOrdersAgree) + EIGEN_DEBUG_VAR(MightVectorize) + EIGEN_DEBUG_VAR(MayLinearize) + EIGEN_DEBUG_VAR(MayInnerVectorize) + EIGEN_DEBUG_VAR(MayLinearVectorize) + EIGEN_DEBUG_VAR(MaySliceVectorize) + std::cerr << "Traversal" + << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl; + EIGEN_DEBUG_VAR(SrcEvaluator::CoeffReadCost) + EIGEN_DEBUG_VAR(DstEvaluator::CoeffReadCost) + EIGEN_DEBUG_VAR(Dst::SizeAtCompileTime) + EIGEN_DEBUG_VAR(UnrollingLimit) + EIGEN_DEBUG_VAR(MayUnrollCompletely) + EIGEN_DEBUG_VAR(MayUnrollInner) + std::cerr << "Unrolling" + << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl; + std::cerr << std::endl; + } +#endif +}; + +/*************************************************************************** + * Part 2 : meta-unrollers + ***************************************************************************/ + +/************************ +*** Default traversal *** +************************/ + +template +struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling { + // FIXME: this is not very clean, perhaps this information should be provided by the kernel? + typedef typename Kernel::DstEvaluatorType DstEvaluatorType; + typedef typename DstEvaluatorType::XprType DstXprType; + + enum { outer = Index / DstXprType::InnerSizeAtCompileTime, inner = Index % DstXprType::InnerSizeAtCompileTime }; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + kernel.assignCoeffByOuterInner(outer, inner); + copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); + } +}; + +template +struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {} +}; + +template +struct copy_using_evaluator_DefaultTraversal_InnerUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel, Index outer) { + kernel.assignCoeffByOuterInner(outer, Index_); + copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); + } +}; + +template +struct copy_using_evaluator_DefaultTraversal_InnerUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) {} +}; + +/*********************** +*** Linear traversal *** +***********************/ + +template +struct copy_using_evaluator_LinearTraversal_CompleteUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + kernel.assignCoeff(Index); + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } +}; + +template +struct copy_using_evaluator_LinearTraversal_CompleteUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) {} +}; + +/************************** +*** Inner vectorization *** +**************************/ + +template +struct copy_using_evaluator_innervec_CompleteUnrolling { + // FIXME: this is not very clean, perhaps this information should be provided by the kernel? + typedef typename Kernel::DstEvaluatorType DstEvaluatorType; + typedef typename DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { + outer = Index / DstXprType::InnerSizeAtCompileTime, + inner = Index % DstXprType::InnerSizeAtCompileTime, + SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, + DstAlignment = Kernel::AssignmentTraits::DstAlignment + }; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + kernel.template assignPacketByOuterInner(outer, inner); + enum { NextIndex = Index + unpacket_traits::size }; + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + } +}; + +template +struct copy_using_evaluator_innervec_CompleteUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {} +}; + +template +struct copy_using_evaluator_innervec_InnerUnrolling { + typedef typename Kernel::PacketType PacketType; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel, Index outer) { + kernel.template assignPacketByOuterInner(outer, Index_); + enum { NextIndex = Index_ + unpacket_traits::size }; + copy_using_evaluator_innervec_InnerUnrolling::run(kernel, + outer); + } +}; + +template +struct copy_using_evaluator_innervec_InnerUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) {} +}; + +/*************************************************************************** + * Part 3 : implementation of all cases + ***************************************************************************/ + +// dense_assignment_loop is based on assign_impl + +template +struct dense_assignment_loop; + +/************************ +***** Special Cases ***** +************************/ + +// Zero-sized assignment is a no-op. +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE EIGEN_CONSTEXPR run(Kernel& /*kernel*/) { + EIGEN_STATIC_ASSERT(int(Kernel::DstEvaluatorType::XprType::SizeAtCompileTime) == 0, + EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT) + } +}; + +/************************ +*** Default traversal *** +************************/ + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel& kernel) { + for (Index outer = 0; outer < kernel.outerSize(); ++outer) { + for (Index inner = 0; inner < kernel.innerSize(); ++inner) { + kernel.assignCoeffByOuterInner(outer, inner); + } + } + } +}; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); + } +}; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + + const Index outerSize = kernel.outerSize(); + for (Index outer = 0; outer < outerSize; ++outer) + copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, + outer); + } +}; + +/*************************** +*** Linear vectorization *** +***************************/ + +// The goal of unaligned_dense_assignment_loop is simply to factorize the handling +// of the non vectorizable beginning and ending parts + +template +struct unaligned_dense_assignment_loop { + // if IsAligned = true, then do nothing + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&, Index, Index) {} +}; + +template <> +struct unaligned_dense_assignment_loop { + // MSVC must not inline this functions. If it does, it fails to optimize the + // packet access path. + // FIXME check which version exhibits this issue +#if EIGEN_COMP_MSVC + template + static EIGEN_DONT_INLINE void run(Kernel& kernel, Index start, Index end) +#else + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel, Index start, Index end) +#endif + { + for (Index index = start; index < end; ++index) kernel.assignCoeff(index); + } +}; + +template +struct copy_using_evaluator_linearvec_CompleteUnrolling { + // FIXME: this is not very clean, perhaps this information should be provided by the kernel? + typedef typename Kernel::DstEvaluatorType DstEvaluatorType; + typedef typename DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, DstAlignment = Kernel::AssignmentTraits::DstAlignment }; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + kernel.template assignPacket(Index); + enum { NextIndex = Index + unpacket_traits::size }; + copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); + } +}; + +template +struct copy_using_evaluator_linearvec_CompleteUnrolling { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {} +}; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { + const Index size = kernel.size(); + typedef typename Kernel::Scalar Scalar; + typedef typename Kernel::PacketType PacketType; + enum { + requestedAlignment = Kernel::AssignmentTraits::LinearRequiredAlignment, + packetSize = unpacket_traits::size, + dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), + dstAlignment = packet_traits::AlignedOnScalar ? int(requestedAlignment) + : int(Kernel::AssignmentTraits::DstAlignment), + srcAlignment = Kernel::AssignmentTraits::JointAlignment + }; + const Index alignedStart = + dstIsAligned ? 0 : internal::first_aligned(kernel.dstDataPtr(), size); + const Index alignedEnd = alignedStart + ((size - alignedStart) / packetSize) * packetSize; + + unaligned_dense_assignment_loop::run(kernel, 0, alignedStart); + + for (Index index = alignedStart; index < alignedEnd; index += packetSize) + kernel.template assignPacket(index); + + unaligned_dense_assignment_loop<>::run(kernel, alignedEnd, size); + } +}; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { + size = DstXprType::SizeAtCompileTime, + packetSize = unpacket_traits::size, + alignedSize = (int(size) / packetSize) * packetSize + }; + + copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } +}; + +/************************** +*** Inner vectorization *** +**************************/ + +template +struct dense_assignment_loop { + typedef typename Kernel::PacketType PacketType; + enum { SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, DstAlignment = Kernel::AssignmentTraits::DstAlignment }; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { + const Index innerSize = kernel.innerSize(); + const Index outerSize = kernel.outerSize(); + const Index packetSize = unpacket_traits::size; + for (Index outer = 0; outer < outerSize; ++outer) + for (Index inner = 0; inner < innerSize; inner += packetSize) + kernel.template assignPacketByOuterInner(outer, inner); + } +}; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + } +}; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::AssignmentTraits Traits; + const Index outerSize = kernel.outerSize(); + for (Index outer = 0; outer < outerSize; ++outer) + copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); + } +}; + +/*********************** +*** Linear traversal *** +***********************/ + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { + const Index size = kernel.size(); + for (Index i = 0; i < size; ++i) kernel.assignCoeff(i); + } +}; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } +}; + +/************************** +*** Slice vectorization *** +***************************/ + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { + typedef typename Kernel::Scalar Scalar; + typedef typename Kernel::PacketType PacketType; + enum { + packetSize = unpacket_traits::size, + requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), + alignable = + packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), + dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), + dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) + }; + const Scalar* dst_ptr = kernel.dstDataPtr(); + if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { + // the pointer is not aligned-on scalar, so alignment is not possible + return dense_assignment_loop::run(kernel); + } + const Index packetAlignedMask = packetSize - 1; + const Index innerSize = kernel.innerSize(); + const Index outerSize = kernel.outerSize(); + const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; + Index alignedStart = + ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); + + for (Index outer = 0; outer < outerSize; ++outer) { + const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); + // do the non-vectorizable part of the assignment + for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); + + // do the vectorizable part of the assignment + for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) + kernel.template assignPacketByOuterInner(outer, inner); + + // do the non-vectorizable part of the assignment + for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); + + alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); + } + } +}; + +#if EIGEN_UNALIGNED_VECTORIZE +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { + innerSize = DstXprType::InnerSizeAtCompileTime, + packetSize = unpacket_traits::size, + vectorizableSize = (int(innerSize) / int(packetSize)) * int(packetSize), + size = DstXprType::SizeAtCompileTime + }; + + for (Index outer = 0; outer < kernel.outerSize(); ++outer) { + copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); + copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); + } + } +}; +#endif + +/*************************************************************************** + * Part 4 : Generic dense assignment kernel + ***************************************************************************/ + +// This class generalize the assignment of a coefficient (or packet) from one dense evaluator +// to another dense writable evaluator. +// It is parametrized by the two evaluators, and the actual assignment functor. +// This abstraction level permits to keep the evaluation loops as simple and as generic as possible. +// One can customize the assignment using this generic dense_assignment_kernel with different +// functors, or by completely overloading it, by-passing a functor. +template +class generic_dense_assignment_kernel { + protected: + typedef typename DstEvaluatorTypeT::XprType DstXprType; + typedef typename SrcEvaluatorTypeT::XprType SrcXprType; + + public: + typedef DstEvaluatorTypeT DstEvaluatorType; + typedef SrcEvaluatorTypeT SrcEvaluatorType; + typedef typename DstEvaluatorType::Scalar Scalar; + typedef copy_using_evaluator_traits AssignmentTraits; + typedef typename AssignmentTraits::PacketType PacketType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst, + const SrcEvaluatorType& src, + const Functor& func, DstXprType& dstExpr) + : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) { +#ifdef EIGEN_DEBUG_ASSIGN + AssignmentTraits::debug(); +#endif + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_dstExpr.size(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index innerSize() const EIGEN_NOEXCEPT { return m_dstExpr.innerSize(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerSize() const EIGEN_NOEXCEPT { return m_dstExpr.outerSize(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_dstExpr.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_dstExpr.cols(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { return m_dstExpr.outerStride(); } + + EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() EIGEN_NOEXCEPT { return m_dst; } + EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; } + + /// Assign src(row,col) to dst(row,col) through the assignment functor. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) { + m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col)); + } + + /// \sa assignCoeff(Index,Index) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index index) { + m_functor.assignCoeff(m_dst.coeffRef(index), m_src.coeff(index)); + } + + /// \sa assignCoeff(Index,Index) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) { + Index row = rowIndexByOuterInner(outer, inner); + Index col = colIndexByOuterInner(outer, inner); + assignCoeff(row, col); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index row, Index col) { + m_functor.template assignPacket(&m_dst.coeffRef(row, col), + m_src.template packet(row, col)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index index) { + m_functor.template assignPacket(&m_dst.coeffRef(index), m_src.template packet(index)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner) { + Index row = rowIndexByOuterInner(outer, inner); + Index col = colIndexByOuterInner(outer, inner); + assignPacket(row, col); + } + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) { + typedef typename DstEvaluatorType::ExpressionTraits Traits; + return int(Traits::RowsAtCompileTime) == 1 ? 0 + : int(Traits::ColsAtCompileTime) == 1 ? inner + : int(DstEvaluatorType::Flags) & RowMajorBit ? outer + : inner; + } + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) { + typedef typename DstEvaluatorType::ExpressionTraits Traits; + return int(Traits::ColsAtCompileTime) == 1 ? 0 + : int(Traits::RowsAtCompileTime) == 1 ? inner + : int(DstEvaluatorType::Flags) & RowMajorBit ? inner + : outer; + } + + EIGEN_DEVICE_FUNC const Scalar* dstDataPtr() const { return m_dstExpr.data(); } + + protected: + DstEvaluatorType& m_dst; + const SrcEvaluatorType& m_src; + const Functor& m_functor; + // TODO find a way to avoid the needs of the original expression + DstXprType& m_dstExpr; +}; + +// Special kernel used when computing small products whose operands have dynamic dimensions. It ensures that the +// PacketSize used is no larger than 4, thereby increasing the chance that vectorized instructions will be used +// when computing the product. + +template +class restricted_packet_dense_assignment_kernel + : public generic_dense_assignment_kernel { + protected: + typedef generic_dense_assignment_kernel Base; + + public: + typedef typename Base::Scalar Scalar; + typedef typename Base::DstXprType DstXprType; + typedef copy_using_evaluator_traits AssignmentTraits; + typedef typename AssignmentTraits::PacketType PacketType; + + EIGEN_DEVICE_FUNC restricted_packet_dense_assignment_kernel(DstEvaluatorTypeT& dst, const SrcEvaluatorTypeT& src, + const Functor& func, DstXprType& dstExpr) + : Base(dst, src, func, dstExpr) {} +}; + +/*************************************************************************** + * Part 5 : Entry point for dense rectangular assignment + ***************************************************************************/ + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, + const Functor& /*func*/) { + EIGEN_ONLY_USED_FOR_DEBUG(dst); + EIGEN_ONLY_USED_FOR_DEBUG(src); + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, + const internal::assign_op& /*func*/) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols); + eigen_assert(dst.rows() == dstRows && dst.cols() == dstCols); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_dense_assignment_loop(DstXprType& dst, + const SrcXprType& src, + const Functor& func) { + typedef evaluator DstEvaluatorType; + typedef evaluator SrcEvaluatorType; + + SrcEvaluatorType srcEvaluator(src); + + // NOTE To properly handle A = (A*A.transpose())/s with A rectangular, + // we need to resize the destination after the source evaluator has been created. + resize_if_allowed(dst, src, func); + + DstEvaluatorType dstEvaluator(dst); + + typedef generic_dense_assignment_kernel Kernel; + Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived()); + + dense_assignment_loop::run(kernel); +} + +// Specialization for filling the destination with a constant value. +#ifndef EIGEN_GPU_COMPILE_PHASE +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop( + DstXprType& dst, + const Eigen::CwiseNullaryOp, DstXprType>& src, + const internal::assign_op& func) { + resize_if_allowed(dst, src, func); + std::fill_n(dst.data(), dst.size(), src.functor()()); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src) { + call_dense_assignment_loop(dst, src, internal::assign_op()); +} + +/*************************************************************************** + * Part 6 : Generic assignment + ***************************************************************************/ + +// Based on the respective shapes of the destination and source, +// the class AssignmentKind determine the kind of assignment mechanism. +// AssignmentKind must define a Kind typedef. +template +struct AssignmentKind; + +// Assignment kind defined in this file: +struct Dense2Dense {}; +struct EigenBase2EigenBase {}; + +template +struct AssignmentKind { + typedef EigenBase2EigenBase Kind; +}; +template <> +struct AssignmentKind { + typedef Dense2Dense Kind; +}; + +// This is the main assignment class +template ::Shape, + typename evaluator_traits::Shape>::Kind, + typename EnableIf = void> +struct Assignment; + +// The only purpose of this call_assignment() function is to deal with noalias() / "assume-aliasing" and automatic +// transposition. Indeed, I (Gael) think that this concept of "assume-aliasing" was a mistake, and it makes thing quite +// complicated. So this intermediate function removes everything related to "assume-aliasing" such that Assignment does +// not has to bother about these annoying details. + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) { + call_assignment(dst, src, internal::assign_op()); +} +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(const Dst& dst, const Src& src) { + call_assignment(dst, src, internal::assign_op()); +} + +// Deal with "assume-aliasing" +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment( + Dst& dst, const Src& src, const Func& func, std::enable_if_t::value, void*> = 0) { + typename plain_matrix_type::type tmp(src); + call_assignment_no_alias(dst, tmp, func); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment( + Dst& dst, const Src& src, const Func& func, std::enable_if_t::value, void*> = 0) { + call_assignment_no_alias(dst, src, func); +} + +// by-pass "assume-aliasing" +// When there is no aliasing, we require that 'dst' has been properly resized +template class StorageBase, typename Src, typename Func> +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(NoAlias& dst, + const Src& src, const Func& func) { + call_assignment_no_alias(dst.expression(), src, func); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias(Dst& dst, const Src& src, + const Func& func) { + enum { + NeedToTranspose = ((int(Dst::RowsAtCompileTime) == 1 && int(Src::ColsAtCompileTime) == 1) || + (int(Dst::ColsAtCompileTime) == 1 && int(Src::RowsAtCompileTime) == 1)) && + int(Dst::SizeAtCompileTime) != 1 + }; + + typedef std::conditional_t, Dst> ActualDstTypeCleaned; + typedef std::conditional_t, Dst&> ActualDstType; + ActualDstType actualDst(dst); + + // TODO check whether this is the right place to perform these checks: + EIGEN_STATIC_ASSERT_LVALUE(Dst) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned, Src) + EIGEN_CHECK_BINARY_COMPATIBILIY(Func, typename ActualDstTypeCleaned::Scalar, typename Src::Scalar); + + Assignment::run(actualDst, src, func); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_restricted_packet_assignment_no_alias(Dst& dst, const Src& src, + const Func& func) { + typedef evaluator DstEvaluatorType; + typedef evaluator SrcEvaluatorType; + typedef restricted_packet_dense_assignment_kernel Kernel; + + EIGEN_STATIC_ASSERT_LVALUE(Dst) + EIGEN_CHECK_BINARY_COMPATIBILIY(Func, typename Dst::Scalar, typename Src::Scalar); + + SrcEvaluatorType srcEvaluator(src); + resize_if_allowed(dst, src, func); + + DstEvaluatorType dstEvaluator(dst); + Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived()); + + dense_assignment_loop::run(kernel); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias(Dst& dst, const Src& src) { + call_assignment_no_alias(dst, src, internal::assign_op()); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias_no_transpose(Dst& dst, + const Src& src, + const Func& func) { + // TODO check whether this is the right place to perform these checks: + EIGEN_STATIC_ASSERT_LVALUE(Dst) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Dst, Src) + EIGEN_CHECK_BINARY_COMPATIBILIY(Func, typename Dst::Scalar, typename Src::Scalar); + + Assignment::run(dst, src, func); +} +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias_no_transpose(Dst& dst, + const Src& src) { + call_assignment_no_alias_no_transpose(dst, src, internal::assign_op()); +} + +// forward declaration +template +EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src); + +// Generic Dense to Dense assignment +// Note that the last template argument "Weak" is needed to make it possible to perform +// both partial specialization+SFINAE without ambiguous specialization +template +struct Assignment { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) { +#ifndef EIGEN_NO_DEBUG + internal::check_for_aliasing(dst, src); +#endif + + call_dense_assignment_loop(dst, src, func); + } +}; + +// Generic assignment through evalTo. +// TODO: not sure we have to keep that one, but it helps porting current code to new evaluator mechanism. +// Note that the last template argument "Weak" is needed to make it possible to perform +// both partial specialization+SFINAE without ambiguous specialization +template +struct Assignment { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run( + DstXprType& dst, const SrcXprType& src, + const internal::assign_op& /*func*/) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols); + + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + src.evalTo(dst); + } + + // NOTE The following two functions are templated to avoid their instantiation if not needed + // This is needed because some expressions supports evalTo only and/or have 'void' as scalar type. + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run( + DstXprType& dst, const SrcXprType& src, + const internal::add_assign_op& /*func*/) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols); + + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + src.addTo(dst); + } + + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run( + DstXprType& dst, const SrcXprType& src, + const internal::sub_assign_op& /*func*/) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols); + + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + src.subTo(dst); + } +}; + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_ASSIGN_EVALUATOR_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h new file mode 100644 index 00000000..ca991cad --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h @@ -0,0 +1,338 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 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_BANDMATRIX_H +#define EIGEN_BANDMATRIX_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +class BandMatrixBase : public EigenBase { + public: + enum { + Flags = internal::traits::Flags, + CoeffReadCost = internal::traits::CoeffReadCost, + RowsAtCompileTime = internal::traits::RowsAtCompileTime, + ColsAtCompileTime = internal::traits::ColsAtCompileTime, + MaxRowsAtCompileTime = internal::traits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = internal::traits::MaxColsAtCompileTime, + Supers = internal::traits::Supers, + Subs = internal::traits::Subs, + Options = internal::traits::Options + }; + typedef typename internal::traits::Scalar Scalar; + typedef Matrix DenseMatrixType; + typedef typename DenseMatrixType::StorageIndex StorageIndex; + typedef typename internal::traits::CoefficientsType CoefficientsType; + typedef EigenBase Base; + + protected: + enum { + DataRowsAtCompileTime = ((Supers != Dynamic) && (Subs != Dynamic)) ? 1 + Supers + Subs : Dynamic, + SizeAtCompileTime = min_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime) + }; + + public: + using Base::cols; + using Base::derived; + using Base::rows; + + /** \returns the number of super diagonals */ + inline Index supers() const { return derived().supers(); } + + /** \returns the number of sub diagonals */ + inline Index subs() const { return derived().subs(); } + + /** \returns an expression of the underlying coefficient matrix */ + inline const CoefficientsType& coeffs() const { return derived().coeffs(); } + + /** \returns an expression of the underlying coefficient matrix */ + inline CoefficientsType& coeffs() { return derived().coeffs(); } + + /** \returns a vector expression of the \a i -th column, + * only the meaningful part is returned. + * \warning the internal storage must be column major. */ + inline Block col(Index i) { + EIGEN_STATIC_ASSERT((int(Options) & int(RowMajor)) == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); + Index start = 0; + Index len = coeffs().rows(); + if (i <= supers()) { + start = supers() - i; + len = (std::min)(rows(), std::max(0, coeffs().rows() - (supers() - i))); + } else if (i >= rows() - subs()) + len = std::max(0, coeffs().rows() - (i + 1 - rows() + subs())); + return Block(coeffs(), start, i, len, 1); + } + + /** \returns a vector expression of the main diagonal */ + inline Block diagonal() { + return Block(coeffs(), supers(), 0, 1, (std::min)(rows(), cols())); + } + + /** \returns a vector expression of the main diagonal (const version) */ + inline const Block diagonal() const { + return Block(coeffs(), supers(), 0, 1, (std::min)(rows(), cols())); + } + + template + struct DiagonalIntReturnType { + enum { + ReturnOpposite = + (int(Options) & int(SelfAdjoint)) && (((Index) > 0 && Supers == 0) || ((Index) < 0 && Subs == 0)), + Conjugate = ReturnOpposite && NumTraits::IsComplex, + ActualIndex = ReturnOpposite ? -Index : Index, + DiagonalSize = + (RowsAtCompileTime == Dynamic || ColsAtCompileTime == Dynamic) + ? Dynamic + : (ActualIndex < 0 ? min_size_prefer_dynamic(ColsAtCompileTime, RowsAtCompileTime + ActualIndex) + : min_size_prefer_dynamic(RowsAtCompileTime, ColsAtCompileTime - ActualIndex)) + }; + typedef Block BuildType; + typedef std::conditional_t, BuildType>, BuildType> + Type; + }; + + /** \returns a vector expression of the \a N -th sub or super diagonal */ + template + inline typename DiagonalIntReturnType::Type diagonal() { + return typename DiagonalIntReturnType::BuildType(coeffs(), supers() - N, (std::max)(0, N), 1, diagonalLength(N)); + } + + /** \returns a vector expression of the \a N -th sub or super diagonal */ + template + inline const typename DiagonalIntReturnType::Type diagonal() const { + return typename DiagonalIntReturnType::BuildType(coeffs(), supers() - N, (std::max)(0, N), 1, diagonalLength(N)); + } + + /** \returns a vector expression of the \a i -th sub or super diagonal */ + inline Block diagonal(Index i) { + eigen_assert((i < 0 && -i <= subs()) || (i >= 0 && i <= supers())); + return Block(coeffs(), supers() - i, std::max(0, i), 1, diagonalLength(i)); + } + + /** \returns a vector expression of the \a i -th sub or super diagonal */ + inline const Block diagonal(Index i) const { + eigen_assert((i < 0 && -i <= subs()) || (i >= 0 && i <= supers())); + return Block(coeffs(), supers() - i, std::max(0, i), 1, + diagonalLength(i)); + } + + template + inline void evalTo(Dest& dst) const { + dst.resize(rows(), cols()); + dst.setZero(); + dst.diagonal() = diagonal(); + for (Index i = 1; i <= supers(); ++i) dst.diagonal(i) = diagonal(i); + for (Index i = 1; i <= subs(); ++i) dst.diagonal(-i) = diagonal(-i); + } + + DenseMatrixType toDenseMatrix() const { + DenseMatrixType res(rows(), cols()); + evalTo(res); + return res; + } + + protected: + inline Index diagonalLength(Index i) const { + return i < 0 ? (std::min)(cols(), rows() + i) : (std::min)(rows(), cols() - i); + } +}; + +/** + * \class BandMatrix + * \ingroup Core_Module + * + * \brief Represents a rectangular matrix with a banded storage + * + * \tparam Scalar_ Numeric type, i.e. float, double, int + * \tparam Rows_ Number of rows, or \b Dynamic + * \tparam Cols_ Number of columns, or \b Dynamic + * \tparam Supers_ Number of super diagonal + * \tparam Subs_ Number of sub diagonal + * \tparam Options_ A combination of either \b #RowMajor or \b #ColMajor, and of \b #SelfAdjoint + * The former controls \ref TopicStorageOrders "storage order", and defaults to + * column-major. The latter controls whether the matrix represents a selfadjoint + * matrix in which case either Supers of Subs have to be null. + * + * \sa class TridiagonalMatrix + */ + +template +struct traits > { + typedef Scalar_ Scalar; + typedef Dense StorageKind; + typedef Eigen::Index StorageIndex; + enum { + CoeffReadCost = NumTraits::ReadCost, + RowsAtCompileTime = Rows_, + ColsAtCompileTime = Cols_, + MaxRowsAtCompileTime = Rows_, + MaxColsAtCompileTime = Cols_, + Flags = LvalueBit, + Supers = Supers_, + Subs = Subs_, + Options = Options_, + DataRowsAtCompileTime = ((Supers != Dynamic) && (Subs != Dynamic)) ? 1 + Supers + Subs : Dynamic + }; + typedef Matrix + CoefficientsType; +}; + +template +class BandMatrix : public BandMatrixBase > { + public: + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::StorageIndex StorageIndex; + typedef typename internal::traits::CoefficientsType CoefficientsType; + + explicit inline BandMatrix(Index rows = Rows, Index cols = Cols, Index supers = Supers, Index subs = Subs) + : m_coeffs(1 + supers + subs, cols), m_rows(rows), m_supers(supers), m_subs(subs) {} + + /** \returns the number of columns */ + inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); } + + /** \returns the number of rows */ + inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); } + + /** \returns the number of super diagonals */ + inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); } + + /** \returns the number of sub diagonals */ + inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); } + + inline const CoefficientsType& coeffs() const { return m_coeffs; } + inline CoefficientsType& coeffs() { return m_coeffs; } + + protected: + CoefficientsType m_coeffs; + internal::variable_if_dynamic m_rows; + internal::variable_if_dynamic m_supers; + internal::variable_if_dynamic m_subs; +}; + +template +class BandMatrixWrapper; + +template +struct traits > { + typedef typename CoefficientsType_::Scalar Scalar; + typedef typename CoefficientsType_::StorageKind StorageKind; + typedef typename CoefficientsType_::StorageIndex StorageIndex; + enum { + CoeffReadCost = internal::traits::CoeffReadCost, + RowsAtCompileTime = Rows_, + ColsAtCompileTime = Cols_, + MaxRowsAtCompileTime = Rows_, + MaxColsAtCompileTime = Cols_, + Flags = LvalueBit, + Supers = Supers_, + Subs = Subs_, + Options = Options_, + DataRowsAtCompileTime = ((Supers != Dynamic) && (Subs != Dynamic)) ? 1 + Supers + Subs : Dynamic + }; + typedef CoefficientsType_ CoefficientsType; +}; + +template +class BandMatrixWrapper + : public BandMatrixBase > { + public: + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::CoefficientsType CoefficientsType; + typedef typename internal::traits::StorageIndex StorageIndex; + + explicit inline BandMatrixWrapper(const CoefficientsType& coeffs, Index rows = Rows_, Index cols = Cols_, + Index supers = Supers_, Index subs = Subs_) + : m_coeffs(coeffs), m_rows(rows), m_supers(supers), m_subs(subs) { + EIGEN_UNUSED_VARIABLE(cols); + // eigen_assert(coeffs.cols()==cols() && (supers()+subs()+1)==coeffs.rows()); + } + + /** \returns the number of columns */ + inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); } + + /** \returns the number of rows */ + inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); } + + /** \returns the number of super diagonals */ + inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); } + + /** \returns the number of sub diagonals */ + inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); } + + inline const CoefficientsType& coeffs() const { return m_coeffs; } + + protected: + const CoefficientsType& m_coeffs; + internal::variable_if_dynamic m_rows; + internal::variable_if_dynamic m_supers; + internal::variable_if_dynamic m_subs; +}; + +/** + * \class TridiagonalMatrix + * \ingroup Core_Module + * + * \brief Represents a tridiagonal matrix with a compact banded storage + * + * \tparam Scalar Numeric type, i.e. float, double, int + * \tparam Size Number of rows and cols, or \b Dynamic + * \tparam Options Can be 0 or \b SelfAdjoint + * + * \sa class BandMatrix + */ +template +class TridiagonalMatrix : public BandMatrix { + typedef BandMatrix Base; + typedef typename Base::StorageIndex StorageIndex; + + public: + explicit TridiagonalMatrix(Index size = Size) : Base(size, size, Options & SelfAdjoint ? 0 : 1, 1) {} + + inline typename Base::template DiagonalIntReturnType<1>::Type super() { return Base::template diagonal<1>(); } + inline const typename Base::template DiagonalIntReturnType<1>::Type super() const { + return Base::template diagonal<1>(); + } + inline typename Base::template DiagonalIntReturnType<-1>::Type sub() { return Base::template diagonal<-1>(); } + inline const typename Base::template DiagonalIntReturnType<-1>::Type sub() const { + return Base::template diagonal<-1>(); + } + + protected: +}; + +struct BandShape {}; + +template +struct evaluator_traits > + : public evaluator_traits_base > { + typedef BandShape Shape; +}; + +template +struct evaluator_traits > + : public evaluator_traits_base > { + typedef BandShape Shape; +}; + +template <> +struct AssignmentKind { + typedef EigenBase2EigenBase Kind; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_BANDMATRIX_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Block.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Block.h new file mode 100644 index 00000000..9b16ed28 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Block.h @@ -0,0 +1,439 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2006-2010 Benoit Jacob +// +// 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_BLOCK_H +#define EIGEN_BLOCK_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits> : traits { + typedef typename traits::Scalar Scalar; + typedef typename traits::StorageKind StorageKind; + typedef typename traits::XprKind XprKind; + typedef typename ref_selector::type XprTypeNested; + typedef std::remove_reference_t XprTypeNested_; + enum { + MatrixRows = traits::RowsAtCompileTime, + MatrixCols = traits::ColsAtCompileTime, + RowsAtCompileTime = MatrixRows == 0 ? 0 : BlockRows, + ColsAtCompileTime = MatrixCols == 0 ? 0 : BlockCols, + MaxRowsAtCompileTime = BlockRows == 0 ? 0 + : RowsAtCompileTime != Dynamic ? int(RowsAtCompileTime) + : int(traits::MaxRowsAtCompileTime), + MaxColsAtCompileTime = BlockCols == 0 ? 0 + : ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime) + : int(traits::MaxColsAtCompileTime), + + XprTypeIsRowMajor = (int(traits::Flags) & RowMajorBit) != 0, + IsRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1 + : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0 + : XprTypeIsRowMajor, + HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor), + InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), + InnerStrideAtCompileTime = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time::ret) + : int(outer_stride_at_compile_time::ret), + OuterStrideAtCompileTime = HasSameStorageOrderAsXprType ? int(outer_stride_at_compile_time::ret) + : int(inner_stride_at_compile_time::ret), + + // FIXME, this traits is rather specialized for dense object and it needs to be cleaned further + FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, + FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0, + Flags = (traits::Flags & (DirectAccessBit | (InnerPanel_ ? CompressedAccessBit : 0))) | FlagsLvalueBit | + FlagsRowMajorBit, + // FIXME DirectAccessBit should not be handled by expressions + // + // Alignment is needed by MapBase's assertions + // We can sefely set it to false here. Internal alignment errors will be detected by an eigen_internal_assert in the + // respective evaluator + Alignment = 0, + InnerPanel = InnerPanel_ ? 1 : 0 + }; +}; + +template ::ret> +class BlockImpl_dense; + +} // end namespace internal + +template +class BlockImpl; + +/** \class Block + * \ingroup Core_Module + * + * \brief Expression of a fixed-size or dynamic-size block + * + * \tparam XprType the type of the expression in which we are taking a block + * \tparam BlockRows the number of rows of the block we are taking at compile time (optional) + * \tparam BlockCols the number of columns of the block we are taking at compile time (optional) + * \tparam InnerPanel is true, if the block maps to a set of rows of a row major matrix or + * to set of columns of a column major matrix (optional). The parameter allows to determine + * at compile time whether aligned access is possible on the block expression. + * + * This class represents an expression of either a fixed-size or dynamic-size block. It is the return + * type of DenseBase::block(Index,Index,Index,Index) and DenseBase::block(Index,Index) and + * most of the time this is the only way it is used. + * + * However, if you want to directly manipulate block expressions, + * for instance if you want to write a function returning such an expression, you + * will need to use this class. + * + * Here is an example illustrating the dynamic case: + * \include class_Block.cpp + * Output: \verbinclude class_Block.out + * + * \note Even though this expression has dynamic size, in the case where \a XprType + * has fixed size, this expression inherits a fixed maximal size which means that evaluating + * it does not cause a dynamic memory allocation. + * + * Here is an example illustrating the fixed-size case: + * \include class_FixedBlock.cpp + * Output: \verbinclude class_FixedBlock.out + * + * \sa DenseBase::block(Index,Index,Index,Index), DenseBase::block(Index,Index), class VectorBlock + */ +template +class Block + : public BlockImpl::StorageKind> { + typedef BlockImpl::StorageKind> Impl; + using BlockHelper = internal::block_xpr_helper; + + public: + // typedef typename Impl::Base Base; + typedef Impl Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(Block) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block) + + typedef internal::remove_all_t NestedExpression; + + /** Column or Row constructor + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Block(XprType& xpr, Index i) : Impl(xpr, i) { + eigen_assert((i >= 0) && (((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) && i < xpr.rows()) || + ((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) && i < xpr.cols()))); + } + + /** Fixed-size constructor + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Block(XprType& xpr, Index startRow, Index startCol) + : Impl(xpr, startRow, startCol) { + EIGEN_STATIC_ASSERT(RowsAtCompileTime != Dynamic && ColsAtCompileTime != Dynamic, + THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE) + eigen_assert(startRow >= 0 && BlockRows >= 0 && startRow + BlockRows <= xpr.rows() && startCol >= 0 && + BlockCols >= 0 && startCol + BlockCols <= xpr.cols()); + } + + /** Dynamic-size constructor + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Block(XprType& xpr, Index startRow, Index startCol, Index blockRows, + Index blockCols) + : Impl(xpr, startRow, startCol, blockRows, blockCols) { + eigen_assert((RowsAtCompileTime == Dynamic || RowsAtCompileTime == blockRows) && + (ColsAtCompileTime == Dynamic || ColsAtCompileTime == blockCols)); + eigen_assert(startRow >= 0 && blockRows >= 0 && startRow <= xpr.rows() - blockRows && startCol >= 0 && + blockCols >= 0 && startCol <= xpr.cols() - blockCols); + } + + // convert nested blocks (e.g. Block>) to a simple block expression (Block) + + using ConstUnwindReturnType = Block; + using UnwindReturnType = Block; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ConstUnwindReturnType unwind() const { + return ConstUnwindReturnType(BlockHelper::base(*this), BlockHelper::row(*this, 0), BlockHelper::col(*this, 0), + this->rows(), this->cols()); + } + + template ::value>> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE UnwindReturnType unwind() { + return UnwindReturnType(BlockHelper::base(*this), BlockHelper::row(*this, 0), BlockHelper::col(*this, 0), + this->rows(), this->cols()); + } +}; + +// The generic default implementation for dense block simply forward to the internal::BlockImpl_dense +// that must be specialized for direct and non-direct access... +template +class BlockImpl + : public internal::BlockImpl_dense { + typedef internal::BlockImpl_dense Impl; + typedef typename XprType::StorageIndex StorageIndex; + + public: + typedef Impl Base; + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index i) : Impl(xpr, i) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol) + : Impl(xpr, startRow, startCol) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, + Index blockCols) + : Impl(xpr, startRow, startCol, blockRows, blockCols) {} +}; + +namespace internal { + +/** \internal Internal implementation of dense Blocks in the general case. */ +template +class BlockImpl_dense : public internal::dense_xpr_base>::type { + typedef Block BlockType; + typedef typename internal::ref_selector::non_const_type XprTypeNested; + + public: + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(BlockType) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense) + + // class InnerIterator; // FIXME apparently never used + + /** Column or Row constructor + */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index i) + : m_xpr(xpr), + // It is a row if and only if BlockRows==1 and BlockCols==XprType::ColsAtCompileTime, + // and it is a column if and only if BlockRows==XprType::RowsAtCompileTime and BlockCols==1, + // all other cases are invalid. + // The case a 1x1 matrix seems ambiguous, but the result is the same anyway. + m_startRow((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) ? i : 0), + m_startCol((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) ? i : 0), + m_blockRows(BlockRows == 1 ? 1 : xpr.rows()), + m_blockCols(BlockCols == 1 ? 1 : xpr.cols()) {} + + /** Fixed-size constructor + */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol) + : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(BlockRows), m_blockCols(BlockCols) {} + + /** Dynamic-size constructor + */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol, Index blockRows, + Index blockCols) + : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(blockRows), m_blockCols(blockCols) {} + + EIGEN_DEVICE_FUNC inline Index rows() const { return m_blockRows.value(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_blockCols.value(); } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index rowId, Index colId) { + EIGEN_STATIC_ASSERT_LVALUE(XprType) + return m_xpr.coeffRef(rowId + m_startRow.value(), colId + m_startCol.value()); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { + return m_xpr.derived().coeffRef(rowId + m_startRow.value(), colId + m_startCol.value()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const { + return m_xpr.coeff(rowId + m_startRow.value(), colId + m_startCol.value()); + } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) { + EIGEN_STATIC_ASSERT_LVALUE(XprType) + return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { + return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const { + return m_xpr.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + template + EIGEN_DEVICE_FUNC inline PacketScalar packet(Index rowId, Index colId) const { + return m_xpr.template packet(rowId + m_startRow.value(), colId + m_startCol.value()); + } + + template + EIGEN_DEVICE_FUNC inline void writePacket(Index rowId, Index colId, const PacketScalar& val) { + m_xpr.template writePacket(rowId + m_startRow.value(), colId + m_startCol.value(), val); + } + + template + EIGEN_DEVICE_FUNC inline PacketScalar packet(Index index) const { + return m_xpr.template packet(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + template + EIGEN_DEVICE_FUNC inline void writePacket(Index index, const PacketScalar& val) { + m_xpr.template writePacket(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), val); + } + +#ifdef EIGEN_PARSED_BY_DOXYGEN + /** \sa MapBase::data() */ + EIGEN_DEVICE_FUNC inline const Scalar* data() const; + EIGEN_DEVICE_FUNC inline Index innerStride() const; + EIGEN_DEVICE_FUNC inline Index outerStride() const; +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t& nestedExpression() const { + return m_xpr; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE XprType& nestedExpression() { return m_xpr; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startRow() const EIGEN_NOEXCEPT { + return m_startRow.value(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startCol() const EIGEN_NOEXCEPT { + return m_startCol.value(); + } + + protected: + XprTypeNested m_xpr; + 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; +}; + +/** \internal Internal implementation of dense Blocks in the direct access case.*/ +template +class BlockImpl_dense + : public MapBase> { + typedef Block BlockType; + typedef typename internal::ref_selector::non_const_type XprTypeNested; + enum { XprTypeIsRowMajor = (int(traits::Flags) & RowMajorBit) != 0 }; + + /** \internal Returns base+offset (unless base is null, in which case returns null). + * Adding an offset to nullptr is undefined behavior, so we must avoid it. + */ + template + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE static Scalar* add_to_nullable_pointer(Scalar* base, + Index offset) { + return base != nullptr ? base + offset : nullptr; + } + + public: + typedef MapBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(BlockType) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense) + + /** Column or Row constructor + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, Index i) + : Base((BlockRows == 0 || BlockCols == 0) + ? nullptr + : add_to_nullable_pointer( + xpr.data(), + i * (((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) && (!XprTypeIsRowMajor)) || + ((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) && + (XprTypeIsRowMajor)) + ? xpr.innerStride() + : xpr.outerStride())), + BlockRows == 1 ? 1 : xpr.rows(), BlockCols == 1 ? 1 : xpr.cols()), + m_xpr(xpr), + m_startRow((BlockRows == 1) && (BlockCols == XprType::ColsAtCompileTime) ? i : 0), + m_startCol((BlockRows == XprType::RowsAtCompileTime) && (BlockCols == 1) ? i : 0) { + init(); + } + + /** Fixed-size constructor + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, Index startRow, Index startCol) + : Base((BlockRows == 0 || BlockCols == 0) + ? nullptr + : add_to_nullable_pointer(xpr.data(), + xpr.innerStride() * (XprTypeIsRowMajor ? startCol : startRow) + + xpr.outerStride() * (XprTypeIsRowMajor ? startRow : startCol))), + m_xpr(xpr), + m_startRow(startRow), + m_startCol(startCol) { + init(); + } + + /** Dynamic-size constructor + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, Index startRow, Index startCol, Index blockRows, + Index blockCols) + : Base((blockRows == 0 || blockCols == 0) + ? nullptr + : add_to_nullable_pointer(xpr.data(), + xpr.innerStride() * (XprTypeIsRowMajor ? startCol : startRow) + + xpr.outerStride() * (XprTypeIsRowMajor ? startRow : startCol)), + blockRows, blockCols), + m_xpr(xpr), + m_startRow(startRow), + m_startCol(startCol) { + init(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t& nestedExpression() const + EIGEN_NOEXCEPT { + return m_xpr; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE XprType& nestedExpression() { return m_xpr; } + + /** \sa MapBase::innerStride() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index innerStride() const EIGEN_NOEXCEPT { + return internal::traits::HasSameStorageOrderAsXprType ? m_xpr.innerStride() : m_xpr.outerStride(); + } + + /** \sa MapBase::outerStride() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { + return internal::traits::HasSameStorageOrderAsXprType ? m_xpr.outerStride() : m_xpr.innerStride(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startRow() const EIGEN_NOEXCEPT { + return m_startRow.value(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startCol() const EIGEN_NOEXCEPT { + return m_startCol.value(); + } + +#ifndef __SUNPRO_CC + // FIXME sunstudio is not friendly with the above friend... + // META-FIXME there is no 'friend' keyword around here. Is this obsolete? + protected: +#endif + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal used by allowAligned() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows, + Index blockCols) + : Base(data, blockRows, blockCols), m_xpr(xpr) { + init(); + } +#endif + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void init() { + m_outerStride = + internal::traits::HasSameStorageOrderAsXprType ? m_xpr.outerStride() : m_xpr.innerStride(); + } + + XprTypeNested m_xpr; + const internal::variable_if_dynamic + m_startRow; + const internal::variable_if_dynamic + m_startCol; + Index m_outerStride; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_BLOCK_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h new file mode 100644 index 00000000..c6291234 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h @@ -0,0 +1,149 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// 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_COMMAINITIALIZER_H +#define EIGEN_COMMAINITIALIZER_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class CommaInitializer + * \ingroup Core_Module + * + * \brief Helper class used by the comma initializer operator + * + * This class is internally used to implement the comma initializer feature. It is + * the return type of MatrixBase::operator<<, and most of the time this is the only + * way it is used. + * + * \sa \blank \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() + */ +template +struct CommaInitializer { + typedef typename XprType::Scalar Scalar; + + EIGEN_DEVICE_FUNC inline CommaInitializer(XprType& xpr, const Scalar& s) + : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) { + eigen_assert(m_xpr.rows() > 0 && m_xpr.cols() > 0 && "Cannot comma-initialize a 0x0 matrix (operator<<)"); + m_xpr.coeffRef(0, 0) = s; + } + + template + EIGEN_DEVICE_FUNC inline CommaInitializer(XprType& xpr, const DenseBase& other) + : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows()) { + eigen_assert(m_xpr.rows() >= other.rows() && m_xpr.cols() >= other.cols() && + "Cannot comma-initialize a 0x0 matrix (operator<<)"); + m_xpr.template block(0, 0, other.rows(), + other.cols()) = other; + } + + /* Copy/Move constructor which transfers ownership. This is crucial in + * absence of return value optimization to avoid assertions during destruction. */ + // FIXME in C++11 mode this could be replaced by a proper RValue constructor + EIGEN_DEVICE_FUNC inline CommaInitializer(const CommaInitializer& o) + : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) { + // Mark original object as finished. In absence of R-value references we need to const_cast: + const_cast(o).m_row = m_xpr.rows(); + const_cast(o).m_col = m_xpr.cols(); + const_cast(o).m_currentBlockRows = 0; + } + + /* inserts a scalar value in the target matrix */ + EIGEN_DEVICE_FUNC CommaInitializer &operator,(const Scalar& s) { + if (m_col == m_xpr.cols()) { + m_row += m_currentBlockRows; + m_col = 0; + m_currentBlockRows = 1; + eigen_assert(m_row < m_xpr.rows() && "Too many rows passed to comma initializer (operator<<)"); + } + eigen_assert(m_col < m_xpr.cols() && "Too many coefficients passed to comma initializer (operator<<)"); + eigen_assert(m_currentBlockRows == 1); + m_xpr.coeffRef(m_row, m_col++) = s; + return *this; + } + + /* inserts a matrix expression in the target matrix */ + template + EIGEN_DEVICE_FUNC CommaInitializer &operator,(const DenseBase& other) { + if (m_col == m_xpr.cols() && (other.cols() != 0 || other.rows() != m_currentBlockRows)) { + m_row += m_currentBlockRows; + m_col = 0; + m_currentBlockRows = other.rows(); + eigen_assert(m_row + m_currentBlockRows <= m_xpr.rows() && + "Too many rows passed to comma initializer (operator<<)"); + } + eigen_assert((m_col + other.cols() <= m_xpr.cols()) && + "Too many coefficients passed to comma initializer (operator<<)"); + eigen_assert(m_currentBlockRows == other.rows()); + m_xpr.template block(m_row, m_col, other.rows(), + other.cols()) = other; + m_col += other.cols(); + return *this; + } + + EIGEN_DEVICE_FUNC inline ~CommaInitializer() +#if defined VERIFY_RAISES_ASSERT && (!defined EIGEN_NO_ASSERTION_CHECKING) && defined EIGEN_EXCEPTIONS + EIGEN_EXCEPTION_SPEC(Eigen::eigen_assert_exception) +#endif + { + finished(); + } + + /** \returns the built matrix once all its coefficients have been set. + * Calling finished is 100% optional. Its purpose is to write expressions + * like this: + * \code + * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished()); + * \endcode + */ + EIGEN_DEVICE_FUNC inline XprType& finished() { + eigen_assert(((m_row + m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0) && m_col == m_xpr.cols() && + "Too few coefficients passed to comma initializer (operator<<)"); + return m_xpr; + } + + XprType& m_xpr; // target expression + Index m_row; // current row id + Index m_col; // current col id + Index m_currentBlockRows; // current block height +}; + +/** \anchor MatrixBaseCommaInitRef + * Convenient operator to set the coefficients of a matrix. + * + * The coefficients must be provided in a row major order and exactly match + * the size of the matrix. Otherwise an assertion is raised. + * + * Example: \include MatrixBase_set.cpp + * Output: \verbinclude MatrixBase_set.out + * + * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary + * order. + * + * \sa CommaInitializer::finished(), class CommaInitializer + */ +template +EIGEN_DEVICE_FUNC inline CommaInitializer DenseBase::operator<<(const Scalar& s) { + return CommaInitializer(*static_cast(this), s); +} + +/** \sa operator<<(const Scalar&) */ +template +template +EIGEN_DEVICE_FUNC inline CommaInitializer DenseBase::operator<<( + const DenseBase& other) { + return CommaInitializer(*static_cast(this), other); +} + +} // end namespace Eigen + +#endif // EIGEN_COMMAINITIALIZER_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h new file mode 100644 index 00000000..dd1770b1 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h @@ -0,0 +1,173 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Rasmus Munk Larsen (rmlarsen@google.com) +// +// 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_CONDITIONESTIMATOR_H +#define EIGEN_CONDITIONESTIMATOR_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct rcond_compute_sign { + static inline Vector run(const Vector& v) { + const RealVector v_abs = v.cwiseAbs(); + return (v_abs.array() == static_cast(0)) + .select(Vector::Ones(v.size()), v.cwiseQuotient(v_abs)); + } +}; + +// Partial specialization to avoid elementwise division for real vectors. +template +struct rcond_compute_sign { + static inline Vector run(const Vector& v) { + return (v.array() < static_cast(0)) + .select(-Vector::Ones(v.size()), Vector::Ones(v.size())); + } +}; + +/** + * \returns an estimate of ||inv(matrix)||_1 given a decomposition of + * \a matrix that implements .solve() and .adjoint().solve() methods. + * + * This function implements Algorithms 4.1 and 5.1 from + * http://www.maths.manchester.ac.uk/~higham/narep/narep135.pdf + * which also forms the basis for the condition number estimators in + * LAPACK. Since at most 10 calls to the solve method of dec are + * performed, the total cost is O(dims^2), as opposed to O(dims^3) + * needed to compute the inverse matrix explicitly. + * + * The most common usage is in estimating the condition number + * ||matrix||_1 * ||inv(matrix)||_1. The first term ||matrix||_1 can be + * computed directly in O(n^2) operations. + * + * Supports the following decompositions: FullPivLU, PartialPivLU, LDLT, and + * LLT. + * + * \sa FullPivLU, PartialPivLU, LDLT, LLT. + */ +template +typename Decomposition::RealScalar rcond_invmatrix_L1_norm_estimate(const Decomposition& dec) { + typedef typename Decomposition::MatrixType MatrixType; + typedef typename Decomposition::Scalar Scalar; + typedef typename Decomposition::RealScalar RealScalar; + typedef typename internal::plain_col_type::type Vector; + typedef typename internal::plain_col_type::type RealVector; + const bool is_complex = (NumTraits::IsComplex != 0); + + eigen_assert(dec.rows() == dec.cols()); + const Index n = dec.rows(); + if (n == 0) return 0; + + // Disable Index to float conversion warning +#ifdef __INTEL_COMPILER +#pragma warning push +#pragma warning(disable : 2259) +#endif + Vector v = dec.solve(Vector::Ones(n) / Scalar(n)); +#ifdef __INTEL_COMPILER +#pragma warning pop +#endif + + // lower_bound is a lower bound on + // ||inv(matrix)||_1 = sup_v ||inv(matrix) v||_1 / ||v||_1 + // and is the objective maximized by the ("super-") gradient ascent + // algorithm below. + RealScalar lower_bound = v.template lpNorm<1>(); + if (n == 1) return lower_bound; + + // Gradient ascent algorithm follows: We know that the optimum is achieved at + // one of the simplices v = e_i, so in each iteration we follow a + // super-gradient to move towards the optimal one. + RealScalar old_lower_bound = lower_bound; + Vector sign_vector(n); + Vector old_sign_vector; + Index v_max_abs_index = -1; + Index old_v_max_abs_index = v_max_abs_index; + for (int k = 0; k < 4; ++k) { + sign_vector = internal::rcond_compute_sign::run(v); + if (k > 0 && !is_complex && sign_vector == old_sign_vector) { + // Break if the solution stagnated. + break; + } + // v_max_abs_index = argmax |real( inv(matrix)^T * sign_vector )| + v = dec.adjoint().solve(sign_vector); + v.real().cwiseAbs().maxCoeff(&v_max_abs_index); + if (v_max_abs_index == old_v_max_abs_index) { + // Break if the solution stagnated. + break; + } + // Move to the new simplex e_j, where j = v_max_abs_index. + v = dec.solve(Vector::Unit(n, v_max_abs_index)); // v = inv(matrix) * e_j. + lower_bound = v.template lpNorm<1>(); + if (lower_bound <= old_lower_bound) { + // Break if the gradient step did not increase the lower_bound. + break; + } + if (!is_complex) { + old_sign_vector = sign_vector; + } + old_v_max_abs_index = v_max_abs_index; + old_lower_bound = lower_bound; + } + // The following calculates an independent estimate of ||matrix||_1 by + // multiplying matrix by a vector with entries of slowly increasing + // magnitude and alternating sign: + // v_i = (-1)^{i} (1 + (i / (dim-1))), i = 0,...,dim-1. + // This improvement to Hager's algorithm above is due to Higham. It was + // added to make the algorithm more robust in certain corner cases where + // large elements in the matrix might otherwise escape detection due to + // exact cancellation (especially when op and op_adjoint correspond to a + // sequence of backsubstitutions and permutations), which could cause + // Hager's algorithm to vastly underestimate ||matrix||_1. + Scalar alternating_sign(RealScalar(1)); + for (Index i = 0; i < n; ++i) { + // The static_cast is needed when Scalar is a complex and RealScalar implements expression templates + v[i] = alternating_sign * static_cast(RealScalar(1) + (RealScalar(i) / (RealScalar(n - 1)))); + alternating_sign = -alternating_sign; + } + v = dec.solve(v); + const RealScalar alternate_lower_bound = (2 * v.template lpNorm<1>()) / (3 * RealScalar(n)); + return numext::maxi(lower_bound, alternate_lower_bound); +} + +/** \brief Reciprocal condition number estimator. + * + * Computing a decomposition of a dense matrix takes O(n^3) operations, while + * this method estimates the condition number quickly and reliably in O(n^2) + * operations. + * + * \returns an estimate of the reciprocal condition number + * (1 / (||matrix||_1 * ||inv(matrix)||_1)) of matrix, given ||matrix||_1 and + * its decomposition. Supports the following decompositions: FullPivLU, + * PartialPivLU, LDLT, and LLT. + * + * \sa FullPivLU, PartialPivLU, LDLT, LLT. + */ +template +typename Decomposition::RealScalar rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, + const Decomposition& dec) { + typedef typename Decomposition::RealScalar RealScalar; + eigen_assert(dec.rows() == dec.cols()); + if (dec.rows() == 0) return NumTraits::infinity(); + if (numext::is_exactly_zero(matrix_norm)) return RealScalar(0); + if (dec.rows() == 1) return RealScalar(1); + const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec); + return (numext::is_exactly_zero(inverse_matrix_norm) ? RealScalar(0) + : (RealScalar(1) / inverse_matrix_norm) / matrix_norm); +} + +} // namespace internal + +} // namespace Eigen + +#endif diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h new file mode 100644 index 00000000..c6206005 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h @@ -0,0 +1,1666 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob +// Copyright (C) 2011-2014 Gael Guennebaud +// Copyright (C) 2011-2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_COREEVALUATORS_H +#define EIGEN_COREEVALUATORS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +// This class returns the evaluator kind from the expression storage kind. +// Default assumes index based accessors +template +struct storage_kind_to_evaluator_kind { + typedef IndexBased Kind; +}; + +// This class returns the evaluator shape from the expression storage kind. +// It can be Dense, Sparse, Triangular, Diagonal, SelfAdjoint, Band, etc. +template +struct storage_kind_to_shape; + +template <> +struct storage_kind_to_shape { + typedef DenseShape Shape; +}; +template <> +struct storage_kind_to_shape { + typedef SolverShape Shape; +}; +template <> +struct storage_kind_to_shape { + typedef PermutationShape Shape; +}; +template <> +struct storage_kind_to_shape { + typedef TranspositionsShape Shape; +}; + +// Evaluators have to be specialized with respect to various criteria such as: +// - storage/structure/shape +// - scalar type +// - etc. +// Therefore, we need specialization of evaluator providing additional template arguments for each kind of evaluators. +// We currently distinguish the following kind of evaluators: +// - unary_evaluator for expressions taking only one arguments (CwiseUnaryOp, CwiseUnaryView, Transpose, +// MatrixWrapper, ArrayWrapper, Reverse, Replicate) +// - binary_evaluator for expression taking two arguments (CwiseBinaryOp) +// - ternary_evaluator for expression taking three arguments (CwiseTernaryOp) +// - product_evaluator for linear algebra products (Product); special case of binary_evaluator because it requires +// additional tags for dispatching. +// - mapbase_evaluator for Map, Block, Ref +// - block_evaluator for Block (special dispatching to a mapbase_evaluator or unary_evaluator) + +template ::Kind, + typename Arg2Kind = typename evaluator_traits::Kind, + typename Arg3Kind = typename evaluator_traits::Kind, + typename Arg1Scalar = typename traits::Scalar, + typename Arg2Scalar = typename traits::Scalar, + typename Arg3Scalar = typename traits::Scalar> +struct ternary_evaluator; + +template ::Kind, + typename RhsKind = typename evaluator_traits::Kind, + typename LhsScalar = typename traits::Scalar, + typename RhsScalar = typename traits::Scalar> +struct binary_evaluator; + +template ::Kind, + typename Scalar = typename T::Scalar> +struct unary_evaluator; + +// evaluator_traits contains traits for evaluator + +template +struct evaluator_traits_base { + // by default, get evaluator kind and shape from storage + typedef typename storage_kind_to_evaluator_kind::StorageKind>::Kind Kind; + typedef typename storage_kind_to_shape::StorageKind>::Shape Shape; +}; + +// Default evaluator traits +template +struct evaluator_traits : public evaluator_traits_base {}; + +template ::Shape> +struct evaluator_assume_aliasing { + static const bool value = false; +}; + +// By default, we assume a unary expression: +template +struct evaluator : public unary_evaluator { + typedef unary_evaluator Base; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const T& xpr) : Base(xpr) {} +}; + +// TODO: Think about const-correctness +template +struct evaluator : evaluator { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const T& xpr) : evaluator(xpr) {} +}; + +// ---------- base class for all evaluators ---------- + +template +struct evaluator_base { + // TODO that's not very nice to have to propagate all these traits. They are currently only needed to handle + // outer,inner indices. + typedef traits ExpressionTraits; + + enum { Alignment = 0 }; + // noncopyable: + // Don't make this class inherit noncopyable as this kills EBO (Empty Base Optimization) + // and make complex evaluator much larger than then should do. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator_base() {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ~evaluator_base() {} + + private: + EIGEN_DEVICE_FUNC evaluator_base(const evaluator_base&); + EIGEN_DEVICE_FUNC const evaluator_base& operator=(const evaluator_base&); +}; + +// -------------------- Matrix and Array -------------------- +// +// evaluator is a common base class for the +// Matrix and Array evaluators. +// Here we directly specialize evaluator. This is not really a unary expression, and it is, by definition, dense, +// so no need for more sophisticated dispatching. + +// this helper permits to completely eliminate m_outerStride if it is known at compiletime. +template +class plainobjectbase_evaluator_data { + public: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) + : data(ptr) { +#ifndef EIGEN_INTERNAL_DEBUGGING + EIGEN_UNUSED_VARIABLE(outerStride); +#endif + eigen_internal_assert(outerStride == OuterStride); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { return OuterStride; } + const Scalar* data; +}; + +template +class plainobjectbase_evaluator_data { + public: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) + : data(ptr), m_outerStride(outerStride) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index outerStride() const { return m_outerStride; } + const Scalar* data; + + protected: + Index m_outerStride; +}; + +template +struct evaluator > : evaluator_base { + typedef PlainObjectBase PlainObjectType; + typedef typename PlainObjectType::Scalar Scalar; + typedef typename PlainObjectType::CoeffReturnType CoeffReturnType; + + enum { + IsRowMajor = PlainObjectType::IsRowMajor, + IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime, + RowsAtCompileTime = PlainObjectType::RowsAtCompileTime, + ColsAtCompileTime = PlainObjectType::ColsAtCompileTime, + + CoeffReadCost = NumTraits::ReadCost, + Flags = traits::EvaluatorFlags, + Alignment = traits::Alignment + }; + enum { + // We do not need to know the outer stride for vectors + OuterStrideAtCompileTime = IsVectorAtCompileTime ? 0 + : int(IsRowMajor) ? ColsAtCompileTime + : RowsAtCompileTime + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator() : m_d(0, OuterStrideAtCompileTime) { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const PlainObjectType& m) + : m_d(m.data(), IsVectorAtCompileTime ? 0 : m.outerStride()) { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + if (IsRowMajor) + return m_d.data[row * m_d.outerStride() + col]; + else + return m_d.data[row + col * m_d.outerStride()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { return m_d.data[index]; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { + if (IsRowMajor) + return const_cast(m_d.data)[row * m_d.outerStride() + col]; + else + return const_cast(m_d.data)[row + col * m_d.outerStride()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { return const_cast(m_d.data)[index]; } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + if (IsRowMajor) + return ploadt(m_d.data + row * m_d.outerStride() + col); + else + return ploadt(m_d.data + row + col * m_d.outerStride()); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + return ploadt(m_d.data + index); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { + if (IsRowMajor) + return pstoret(const_cast(m_d.data) + row * m_d.outerStride() + col, x); + else + return pstoret(const_cast(m_d.data) + row + col * m_d.outerStride(), x); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { + return pstoret(const_cast(m_d.data) + index, x); + } + + protected: + plainobjectbase_evaluator_data m_d; +}; + +template +struct evaluator > + : evaluator > > { + typedef Matrix XprType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator() {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& m) + : evaluator >(m) {} +}; + +template +struct evaluator > + : evaluator > > { + typedef Array XprType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator() {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& m) + : evaluator >(m) {} +}; + +// -------------------- Transpose -------------------- + +template +struct unary_evaluator, IndexBased> : evaluator_base > { + typedef Transpose XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = evaluator::Flags ^ RowMajorBit, + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& t) : m_argImpl(t.nestedExpression()) {} + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_argImpl.coeff(col, row); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { return m_argImpl.coeff(index); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { return m_argImpl.coeffRef(col, row); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename XprType::Scalar& coeffRef(Index index) { + return m_argImpl.coeffRef(index); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + return m_argImpl.template packet(col, row); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + return m_argImpl.template packet(index); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { + m_argImpl.template writePacket(col, row, x); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { + m_argImpl.template writePacket(index, x); + } + + protected: + evaluator m_argImpl; +}; + +// -------------------- CwiseNullaryOp -------------------- +// Like Matrix and Array, this is not really a unary expression, so we directly specialize evaluator. +// Likewise, there is not need to more sophisticated dispatching here. + +template ::value, + bool has_unary = has_unary_operator::value, + bool has_binary = has_binary_operator::value> +struct nullary_wrapper { + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const { + return op(i, j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { + return op(i); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const { + return op.template packetOp(i, j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { + return op.template packetOp(i); + } +}; + +template +struct nullary_wrapper { + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType = 0, IndexType = 0) const { + return op(); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType = 0, IndexType = 0) const { + return op.template packetOp(); + } +}; + +template +struct nullary_wrapper { + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j = 0) const { + return op(i, j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j = 0) const { + return op.template packetOp(i, j); + } +}; + +// We need the following specialization for vector-only functors assigned to a runtime vector, +// for instance, using linspace and assigning a RowVectorXd to a MatrixXd or even a row of a MatrixXd. +// In this case, i==0 and j is used for the actual iteration. +template +struct nullary_wrapper { + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const { + eigen_assert(i == 0 || j == 0); + return op(i + j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const { + eigen_assert(i == 0 || j == 0); + return op.template packetOp(i + j); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { + return op(i); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { + return op.template packetOp(i); + } +}; + +template +struct nullary_wrapper {}; + +#if 0 && EIGEN_COMP_MSVC > 0 +// Disable this ugly workaround. This is now handled in traits::match, +// but this piece of code might still become handly if some other weird compilation +// erros pop up again. + +// MSVC exhibits a weird compilation error when +// compiling: +// Eigen::MatrixXf A = MatrixXf::Random(3,3); +// Ref R = 2.f*A; +// and that has_*ary_operator> have not been instantiated yet. +// The "problem" is that evaluator<2.f*A> is instantiated by traits::match<2.f*A> +// and at that time has_*ary_operator returns true regardless of T. +// Then nullary_wrapper is badly instantiated as nullary_wrapper<.,.,true,true,true>. +// The trick is thus to defer the proper instantiation of nullary_wrapper when coeff(), +// and packet() are really instantiated as implemented below: + +// This is a simple wrapper around Index to enforce the re-instantiation of +// has_*ary_operator when needed. +template struct nullary_wrapper_workaround_msvc { + nullary_wrapper_workaround_msvc(const T&); + operator T()const; +}; + +template +struct nullary_wrapper +{ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().operator()(op,i,j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().operator()(op,i); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().template packetOp(op,i,j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().template packetOp(op,i); + } +}; +#endif // MSVC workaround + +template +struct evaluator > + : evaluator_base > { + typedef CwiseNullaryOp XprType; + typedef internal::remove_all_t PlainObjectTypeCleaned; + + enum { + CoeffReadCost = internal::functor_traits::Cost, + + Flags = (evaluator::Flags & + (HereditaryBits | (functor_has_linear_access::ret ? LinearAccessBit : 0) | + (functor_traits::PacketAccess ? PacketAccessBit : 0))) | + (functor_traits::IsRepeatable ? 0 : EvalBeforeNestingBit), + Alignment = AlignedMax + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& n) : m_functor(n.functor()), m_wrapper() { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(IndexType row, IndexType col) const { + return m_wrapper(m_functor, row, col); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(IndexType index) const { + return m_wrapper(m_functor, index); + } + + template + EIGEN_STRONG_INLINE PacketType packet(IndexType row, IndexType col) const { + return m_wrapper.template packetOp(m_functor, row, col); + } + + template + EIGEN_STRONG_INLINE PacketType packet(IndexType index) const { + return m_wrapper.template packetOp(m_functor, index); + } + + protected: + const NullaryOp m_functor; + const internal::nullary_wrapper m_wrapper; +}; + +// -------------------- CwiseUnaryOp -------------------- + +template +struct unary_evaluator, IndexBased> : evaluator_base > { + typedef CwiseUnaryOp XprType; + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + + Flags = evaluator::Flags & + (HereditaryBits | LinearAccessBit | (functor_traits::PacketAccess ? PacketAccessBit : 0)), + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& op) : m_d(op) { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_d.func()(m_d.argImpl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_d.func()(m_d.argImpl.coeff(index)); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + return m_d.func().packetOp(m_d.argImpl.template packet(row, col)); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + return m_d.func().packetOp(m_d.argImpl.template packet(index)); + } + + protected: + // this helper permits to completely eliminate the functor if it is empty + struct Data { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr) + : op(xpr.functor()), argImpl(xpr.nestedExpression()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryOp& func() const { return op; } + UnaryOp op; + evaluator argImpl; + }; + + Data m_d; +}; + +// ----------------------- Casting --------------------- + +template +struct unary_evaluator, ArgType>, IndexBased> { + using CastOp = core_cast_op; + using XprType = CwiseUnaryOp; + + // Use the largest packet type by default + using SrcPacketType = typename packet_traits::type; + static constexpr int SrcPacketSize = unpacket_traits::size; + static constexpr int SrcPacketBytes = SrcPacketSize * sizeof(SrcType); + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + PacketAccess = functor_traits::PacketAccess, + ActualPacketAccessBit = PacketAccess ? PacketAccessBit : 0, + Flags = evaluator::Flags & (HereditaryBits | LinearAccessBit | ActualPacketAccessBit), + IsRowMajor = (evaluator::Flags & RowMajorBit), + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& xpr) + : m_argImpl(xpr.nestedExpression()), m_rows(xpr.rows()), m_cols(xpr.cols()) { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + template + using AltSrcScalarOp = std::enable_if_t<(unpacket_traits::size < SrcPacketSize && + !find_packet_by_size::size>::value), + bool>; + template + using SrcPacketArgs1 = + std::enable_if_t<(find_packet_by_size::size>::value), bool>; + template + using SrcPacketArgs2 = std::enable_if_t<(unpacket_traits::size) == (2 * SrcPacketSize), bool>; + template + using SrcPacketArgs4 = std::enable_if_t<(unpacket_traits::size) == (4 * SrcPacketSize), bool>; + template + using SrcPacketArgs8 = std::enable_if_t<(unpacket_traits::size) == (8 * SrcPacketSize), bool>; + + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index, Index col, Index packetSize) const { + return col + packetSize <= cols(); + } + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index row, Index, Index packetSize) const { + return row + packetSize <= rows(); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index index, Index packetSize) const { + return index + packetSize <= size(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE SrcType srcCoeff(Index row, Index col, Index offset) const { + Index actualRow = IsRowMajor ? row : row + offset; + Index actualCol = IsRowMajor ? col + offset : col; + return m_argImpl.coeff(actualRow, actualCol); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE SrcType srcCoeff(Index index, Index offset) const { + Index actualIndex = index + offset; + return m_argImpl.coeff(actualIndex); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstType coeff(Index row, Index col) const { + return cast(srcCoeff(row, col, 0)); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstType coeff(Index index) const { + return cast(srcCoeff(index, 0)); + } + + template + EIGEN_STRONG_INLINE PacketType srcPacket(Index row, Index col, Index offset) const { + constexpr int PacketSize = unpacket_traits::size; + Index actualRow = IsRowMajor ? row : row + (offset * PacketSize); + Index actualCol = IsRowMajor ? col + (offset * PacketSize) : col; + eigen_assert(check_array_bounds(actualRow, actualCol, PacketSize) && "Array index out of bounds"); + return m_argImpl.template packet(actualRow, actualCol); + } + template + EIGEN_STRONG_INLINE PacketType srcPacket(Index index, Index offset) const { + constexpr int PacketSize = unpacket_traits::size; + Index actualIndex = index + (offset * PacketSize); + eigen_assert(check_array_bounds(actualIndex, PacketSize) && "Array index out of bounds"); + return m_argImpl.template packet(actualIndex); + } + + // There is no source packet type with equal or fewer elements than DstPacketType. + // This is problematic as the evaluation loop may attempt to access data outside the bounds of the array. + // For example, consider the cast utilizing pcast with an array of size 4: {0.0f,1.0f,2.0f,3.0f}. + // The first iteration of the evaulation loop will load 16 bytes: {0.0f,1.0f,2.0f,3.0f} and cast to {0.0,1.0}, which + // is acceptable. The second iteration will load 16 bytes: {2.0f,3.0f,?,?}, which is outside the bounds of the array. + + // Instead, perform runtime check to determine if the load would access data outside the bounds of the array. + // If not, perform full load. Otherwise, revert to a scalar loop to perform a partial load. + // In either case, perform a vectorized cast of the source packet. + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const { + constexpr int DstPacketSize = unpacket_traits::size; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + SrcPacketType src; + if (EIGEN_PREDICT_TRUE(check_array_bounds(row, col, SrcPacketSize))) { + src = srcPacket(row, col, 0); + } else { + Array srcArray; + for (size_t k = 0; k < DstPacketSize; k++) srcArray[k] = srcCoeff(row, col, k); + for (size_t k = DstPacketSize; k < SrcPacketSize; k++) srcArray[k] = SrcType(0); + src = pload(srcArray.data()); + } + return pcast(src); + } + // Use the source packet type with the same size as DstPacketType, if it exists + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const { + constexpr int DstPacketSize = unpacket_traits::size; + using SizedSrcPacketType = typename find_packet_by_size::type; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + return pcast(srcPacket(row, col, 0)); + } + // unpacket_traits::size == 2 * SrcPacketSize + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + return pcast(srcPacket(row, col, 0), + srcPacket(row, col, 1)); + } + // unpacket_traits::size == 4 * SrcPacketSize + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + return pcast(srcPacket(row, col, 0), srcPacket(row, col, 1), + srcPacket(row, col, 2), + srcPacket(row, col, 3)); + } + // unpacket_traits::size == 8 * SrcPacketSize + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + return pcast( + srcPacket(row, col, 0), srcPacket(row, col, 1), srcPacket(row, col, 2), + srcPacket(row, col, 3), srcPacket(row, col, 4), srcPacket(row, col, 5), + srcPacket(row, col, 6), srcPacket(row, col, 7)); + } + + // Analagous routines for linear access. + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index index) const { + constexpr int DstPacketSize = unpacket_traits::size; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + SrcPacketType src; + if (EIGEN_PREDICT_TRUE(check_array_bounds(index, SrcPacketSize))) { + src = srcPacket(index, 0); + } else { + Array srcArray; + for (size_t k = 0; k < DstPacketSize; k++) srcArray[k] = srcCoeff(index, k); + for (size_t k = DstPacketSize; k < SrcPacketSize; k++) srcArray[k] = SrcType(0); + src = pload(srcArray.data()); + } + return pcast(src); + } + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index index) const { + constexpr int DstPacketSize = unpacket_traits::size; + using SizedSrcPacketType = typename find_packet_by_size::type; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + return pcast(srcPacket(index, 0)); + } + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index index) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + return pcast(srcPacket(index, 0), srcPacket(index, 1)); + } + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index index) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + return pcast(srcPacket(index, 0), srcPacket(index, 1), + srcPacket(index, 2), srcPacket(index, 3)); + } + template = true> + EIGEN_STRONG_INLINE DstPacketType packet(Index index) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + return pcast(srcPacket(index, 0), srcPacket(index, 1), + srcPacket(index, 2), srcPacket(index, 3), + srcPacket(index, 4), srcPacket(index, 5), + srcPacket(index, 6), srcPacket(index, 7)); + } + + constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { return m_rows; } + constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { return m_cols; } + constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const { return m_rows * m_cols; } + + protected: + const evaluator m_argImpl; + const variable_if_dynamic m_rows; + const variable_if_dynamic m_cols; +}; + +// -------------------- CwiseTernaryOp -------------------- + +// this is a ternary expression +template +struct evaluator > + : public ternary_evaluator > { + typedef CwiseTernaryOp XprType; + typedef ternary_evaluator > Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +template +struct ternary_evaluator, IndexBased, IndexBased> + : evaluator_base > { + typedef CwiseTernaryOp XprType; + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + + Arg1Flags = evaluator::Flags, + Arg2Flags = evaluator::Flags, + Arg3Flags = evaluator::Flags, + SameType = is_same::value && + is_same::value, + StorageOrdersAgree = (int(Arg1Flags) & RowMajorBit) == (int(Arg2Flags) & RowMajorBit) && + (int(Arg1Flags) & RowMajorBit) == (int(Arg3Flags) & RowMajorBit), + Flags0 = (int(Arg1Flags) | int(Arg2Flags) | int(Arg3Flags)) & + (HereditaryBits | + (int(Arg1Flags) & int(Arg2Flags) & int(Arg3Flags) & + ((StorageOrdersAgree ? LinearAccessBit : 0) | + (functor_traits::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0)))), + Flags = (Flags0 & ~RowMajorBit) | (Arg1Flags & RowMajorBit), + Alignment = plain_enum_min(plain_enum_min(evaluator::Alignment, evaluator::Alignment), + evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC explicit ternary_evaluator(const XprType& xpr) : m_d(xpr) { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_d.func()(m_d.arg1Impl.coeff(row, col), m_d.arg2Impl.coeff(row, col), m_d.arg3Impl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_d.func()(m_d.arg1Impl.coeff(index), m_d.arg2Impl.coeff(index), m_d.arg3Impl.coeff(index)); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + return m_d.func().packetOp(m_d.arg1Impl.template packet(row, col), + m_d.arg2Impl.template packet(row, col), + m_d.arg3Impl.template packet(row, col)); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + return m_d.func().packetOp(m_d.arg1Impl.template packet(index), + m_d.arg2Impl.template packet(index), + m_d.arg3Impl.template packet(index)); + } + + protected: + // this helper permits to completely eliminate the functor if it is empty + struct Data { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr) + : op(xpr.functor()), arg1Impl(xpr.arg1()), arg2Impl(xpr.arg2()), arg3Impl(xpr.arg3()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const TernaryOp& func() const { return op; } + TernaryOp op; + evaluator arg1Impl; + evaluator arg2Impl; + evaluator arg3Impl; + }; + + Data m_d; +}; + +// -------------------- CwiseBinaryOp -------------------- + +// this is a binary expression +template +struct evaluator > : public binary_evaluator > { + typedef CwiseBinaryOp XprType; + typedef binary_evaluator > Base; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +template +struct binary_evaluator, IndexBased, IndexBased> + : evaluator_base > { + typedef CwiseBinaryOp XprType; + + enum { + CoeffReadCost = + int(evaluator::CoeffReadCost) + int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + + LhsFlags = evaluator::Flags, + RhsFlags = evaluator::Flags, + SameType = is_same::value, + StorageOrdersAgree = (int(LhsFlags) & RowMajorBit) == (int(RhsFlags) & RowMajorBit), + Flags0 = (int(LhsFlags) | int(RhsFlags)) & + (HereditaryBits | + (int(LhsFlags) & int(RhsFlags) & + ((StorageOrdersAgree ? LinearAccessBit : 0) | + (functor_traits::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0)))), + Flags = (Flags0 & ~RowMajorBit) | (LhsFlags & RowMajorBit), + Alignment = plain_enum_min(evaluator::Alignment, evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit binary_evaluator(const XprType& xpr) : m_d(xpr) { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_d.func()(m_d.lhsImpl.coeff(row, col), m_d.rhsImpl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_d.func()(m_d.lhsImpl.coeff(index), m_d.rhsImpl.coeff(index)); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + return m_d.func().packetOp(m_d.lhsImpl.template packet(row, col), + m_d.rhsImpl.template packet(row, col)); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + return m_d.func().packetOp(m_d.lhsImpl.template packet(index), + m_d.rhsImpl.template packet(index)); + } + + protected: + // this helper permits to completely eliminate the functor if it is empty + struct Data { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr) + : op(xpr.functor()), lhsImpl(xpr.lhs()), rhsImpl(xpr.rhs()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const BinaryOp& func() const { return op; } + BinaryOp op; + evaluator lhsImpl; + evaluator rhsImpl; + }; + + Data m_d; +}; + +// -------------------- CwiseUnaryView -------------------- + +template +struct unary_evaluator, IndexBased> + : evaluator_base > { + typedef CwiseUnaryView XprType; + + enum { + CoeffReadCost = int(evaluator::CoeffReadCost) + int(functor_traits::Cost), + + Flags = (evaluator::Flags & (HereditaryBits | LinearAccessBit | DirectAccessBit)), + + Alignment = 0 // FIXME it is not very clear why alignment is necessarily lost... + }; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) : m_d(op) { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_d.func()(m_d.argImpl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_d.func()(m_d.argImpl.coeff(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { + return m_d.func()(m_d.argImpl.coeffRef(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { + return m_d.func()(m_d.argImpl.coeffRef(index)); + } + + protected: + // this helper permits to completely eliminate the functor if it is empty + struct Data { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Data(const XprType& xpr) + : op(xpr.functor()), argImpl(xpr.nestedExpression()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryOp& func() const { return op; } + UnaryOp op; + evaluator argImpl; + }; + + Data m_d; +}; + +// -------------------- Map -------------------- + +// FIXME perhaps the PlainObjectType could be provided by Derived::PlainObject ? +// but that might complicate template specialization +template +struct mapbase_evaluator; + +template +struct mapbase_evaluator : evaluator_base { + typedef Derived XprType; + typedef typename XprType::PointerType PointerType; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + IsRowMajor = XprType::RowsAtCompileTime, + ColsAtCompileTime = XprType::ColsAtCompileTime, + CoeffReadCost = NumTraits::ReadCost + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit mapbase_evaluator(const XprType& map) + : m_data(const_cast(map.data())), + m_innerStride(map.innerStride()), + m_outerStride(map.outerStride()) { + EIGEN_STATIC_ASSERT(check_implication((evaluator::Flags & PacketAccessBit) != 0, + internal::inner_stride_at_compile_time::ret == 1), + PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_data[col * colStride() + row * rowStride()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_data[index * m_innerStride.value()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { + return m_data[col * colStride() + row * rowStride()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { return m_data[index * m_innerStride.value()]; } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + PointerType ptr = m_data + row * rowStride() + col * colStride(); + return internal::ploadt(ptr); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + return internal::ploadt(m_data + index * m_innerStride.value()); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { + PointerType ptr = m_data + row * rowStride() + col * colStride(); + return internal::pstoret(ptr, x); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { + internal::pstoret(m_data + index * m_innerStride.value(), x); + } + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowStride() const EIGEN_NOEXCEPT { + return XprType::IsRowMajor ? m_outerStride.value() : m_innerStride.value(); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colStride() const EIGEN_NOEXCEPT { + return XprType::IsRowMajor ? m_innerStride.value() : m_outerStride.value(); + } + + PointerType m_data; + const internal::variable_if_dynamic m_innerStride; + const internal::variable_if_dynamic m_outerStride; +}; + +template +struct evaluator > + : public mapbase_evaluator, PlainObjectType> { + typedef Map XprType; + typedef typename XprType::Scalar Scalar; + // TODO: should check for smaller packet types once we can handle multi-sized packet types + typedef typename packet_traits::type PacketScalar; + + enum { + InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0 + ? int(PlainObjectType::InnerStrideAtCompileTime) + : int(StrideType::InnerStrideAtCompileTime), + OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0 + ? int(PlainObjectType::OuterStrideAtCompileTime) + : int(StrideType::OuterStrideAtCompileTime), + HasNoInnerStride = InnerStrideAtCompileTime == 1, + HasNoOuterStride = StrideType::OuterStrideAtCompileTime == 0, + HasNoStride = HasNoInnerStride && HasNoOuterStride, + IsDynamicSize = PlainObjectType::SizeAtCompileTime == Dynamic, + + PacketAccessMask = bool(HasNoInnerStride) ? ~int(0) : ~int(PacketAccessBit), + LinearAccessMask = + bool(HasNoStride) || bool(PlainObjectType::IsVectorAtCompileTime) ? ~int(0) : ~int(LinearAccessBit), + Flags = int(evaluator::Flags) & (LinearAccessMask & PacketAccessMask), + + Alignment = int(MapOptions) & int(AlignedMask) + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& map) : mapbase_evaluator(map) {} +}; + +// -------------------- Ref -------------------- + +template +struct evaluator > + : public mapbase_evaluator, PlainObjectType> { + typedef Ref XprType; + + enum { + Flags = evaluator >::Flags, + Alignment = evaluator >::Alignment + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& ref) + : mapbase_evaluator(ref) {} +}; + +// -------------------- Block -------------------- + +template ::ret> +struct block_evaluator; + +template +struct evaluator > + : block_evaluator { + typedef Block XprType; + typedef typename XprType::Scalar Scalar; + // TODO: should check for smaller packet types once we can handle multi-sized packet types + typedef typename packet_traits::type PacketScalar; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + + RowsAtCompileTime = traits::RowsAtCompileTime, + ColsAtCompileTime = traits::ColsAtCompileTime, + MaxRowsAtCompileTime = traits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = traits::MaxColsAtCompileTime, + + ArgTypeIsRowMajor = (int(evaluator::Flags) & RowMajorBit) != 0, + IsRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1 + : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0 + : ArgTypeIsRowMajor, + HasSameStorageOrderAsArgType = (IsRowMajor == ArgTypeIsRowMajor), + InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), + InnerStrideAtCompileTime = HasSameStorageOrderAsArgType ? int(inner_stride_at_compile_time::ret) + : int(outer_stride_at_compile_time::ret), + OuterStrideAtCompileTime = HasSameStorageOrderAsArgType ? int(outer_stride_at_compile_time::ret) + : int(inner_stride_at_compile_time::ret), + MaskPacketAccessBit = (InnerStrideAtCompileTime == 1 || HasSameStorageOrderAsArgType) ? PacketAccessBit : 0, + + FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || + (InnerPanel && (evaluator::Flags & LinearAccessBit))) + ? LinearAccessBit + : 0, + FlagsRowMajorBit = XprType::Flags & RowMajorBit, + Flags0 = evaluator::Flags & ((HereditaryBits & ~RowMajorBit) | DirectAccessBit | MaskPacketAccessBit), + Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit, + + PacketAlignment = unpacket_traits::alignment, + Alignment0 = (InnerPanel && (OuterStrideAtCompileTime != Dynamic) && (OuterStrideAtCompileTime != 0) && + (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) + ? int(PacketAlignment) + : 0, + Alignment = plain_enum_min(evaluator::Alignment, Alignment0) + }; + typedef block_evaluator block_evaluator_type; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& block) : block_evaluator_type(block) { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } +}; + +// no direct-access => dispatch to a unary evaluator +template +struct block_evaluator + : unary_evaluator > { + typedef Block XprType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit block_evaluator(const XprType& block) + : unary_evaluator(block) {} +}; + +template +struct unary_evaluator, IndexBased> + : evaluator_base > { + typedef Block XprType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& block) + : m_argImpl(block.nestedExpression()), + m_startRow(block.startRow()), + m_startCol(block.startCol()), + m_linear_offset(ForwardLinearAccess + ? (ArgType::IsRowMajor + ? block.startRow() * block.nestedExpression().cols() + block.startCol() + : block.startCol() * block.nestedExpression().rows() + block.startRow()) + : 0) {} + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + RowsAtCompileTime = XprType::RowsAtCompileTime, + ForwardLinearAccess = (InnerPanel || int(XprType::IsRowMajor) == int(ArgType::IsRowMajor)) && + bool(evaluator::Flags & LinearAccessBit) + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_argImpl.coeff(m_startRow.value() + row, m_startCol.value() + col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return linear_coeff_impl(index, bool_constant()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { + return m_argImpl.coeffRef(m_startRow.value() + row, m_startCol.value() + col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { + return linear_coeffRef_impl(index, bool_constant()); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + return m_argImpl.template packet(m_startRow.value() + row, m_startCol.value() + col); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + if (ForwardLinearAccess) + return m_argImpl.template packet(m_linear_offset.value() + index); + else + return packet(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { + return m_argImpl.template writePacket(m_startRow.value() + row, m_startCol.value() + col, x); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { + if (ForwardLinearAccess) + return m_argImpl.template writePacket(m_linear_offset.value() + index, x); + else + return writePacket(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0, + x); + } + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType + linear_coeff_impl(Index index, internal::true_type /* ForwardLinearAccess */) const { + return m_argImpl.coeff(m_linear_offset.value() + index); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType + linear_coeff_impl(Index index, internal::false_type /* not ForwardLinearAccess */) const { + return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& linear_coeffRef_impl(Index index, + internal::true_type /* ForwardLinearAccess */) { + return m_argImpl.coeffRef(m_linear_offset.value() + index); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& linear_coeffRef_impl( + Index index, internal::false_type /* not ForwardLinearAccess */) { + return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0); + } + + evaluator m_argImpl; + const variable_if_dynamic m_startRow; + const variable_if_dynamic m_startCol; + const variable_if_dynamic m_linear_offset; +}; + +// TODO: This evaluator does not actually use the child evaluator; +// all action is via the data() as returned by the Block expression. + +template +struct block_evaluator + : mapbase_evaluator, + typename Block::PlainObject> { + typedef Block XprType; + typedef typename XprType::Scalar Scalar; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit block_evaluator(const XprType& block) + : mapbase_evaluator(block) { + eigen_internal_assert((internal::is_constant_evaluated() || + (std::uintptr_t(block.data()) % plain_enum_max(1, evaluator::Alignment)) == 0) && + "data is not aligned"); + } +}; + +// -------------------- Select -------------------- +// NOTE shall we introduce a ternary_evaluator? + +// TODO enable vectorization for Select +template +struct evaluator > + : evaluator_base > { + typedef Select XprType; + enum { + CoeffReadCost = evaluator::CoeffReadCost + + plain_enum_max(evaluator::CoeffReadCost, evaluator::CoeffReadCost), + + Flags = (unsigned int)evaluator::Flags & evaluator::Flags & HereditaryBits, + + Alignment = plain_enum_min(evaluator::Alignment, evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& select) + : m_conditionImpl(select.conditionMatrix()), m_thenImpl(select.thenMatrix()), m_elseImpl(select.elseMatrix()) { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + if (m_conditionImpl.coeff(row, col)) + return m_thenImpl.coeff(row, col); + else + return m_elseImpl.coeff(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + if (m_conditionImpl.coeff(index)) + return m_thenImpl.coeff(index); + else + return m_elseImpl.coeff(index); + } + + protected: + evaluator m_conditionImpl; + evaluator m_thenImpl; + evaluator m_elseImpl; +}; + +// -------------------- Replicate -------------------- + +template +struct unary_evaluator > + : evaluator_base > { + typedef Replicate XprType; + typedef typename XprType::CoeffReturnType CoeffReturnType; + enum { Factor = (RowFactor == Dynamic || ColFactor == Dynamic) ? Dynamic : RowFactor * ColFactor }; + typedef typename internal::nested_eval::type ArgTypeNested; + typedef internal::remove_all_t ArgTypeNestedCleaned; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + LinearAccessMask = XprType::IsVectorAtCompileTime ? LinearAccessBit : 0, + Flags = (evaluator::Flags & (HereditaryBits | LinearAccessMask) & ~RowMajorBit) | + (traits::Flags & RowMajorBit), + + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& replicate) + : m_arg(replicate.nestedExpression()), + m_argImpl(m_arg), + m_rows(replicate.nestedExpression().rows()), + m_cols(replicate.nestedExpression().cols()) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + // try to avoid using modulo; this is a pure optimization strategy + const Index actual_row = internal::traits::RowsAtCompileTime == 1 ? 0 + : RowFactor == 1 ? row + : row % m_rows.value(); + const Index actual_col = internal::traits::ColsAtCompileTime == 1 ? 0 + : ColFactor == 1 ? col + : col % m_cols.value(); + + return m_argImpl.coeff(actual_row, actual_col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + // try to avoid using modulo; this is a pure optimization strategy + const Index actual_index = internal::traits::RowsAtCompileTime == 1 + ? (ColFactor == 1 ? index : index % m_cols.value()) + : (RowFactor == 1 ? index : index % m_rows.value()); + + return m_argImpl.coeff(actual_index); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + const Index actual_row = internal::traits::RowsAtCompileTime == 1 ? 0 + : RowFactor == 1 ? row + : row % m_rows.value(); + const Index actual_col = internal::traits::ColsAtCompileTime == 1 ? 0 + : ColFactor == 1 ? col + : col % m_cols.value(); + + return m_argImpl.template packet(actual_row, actual_col); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + const Index actual_index = internal::traits::RowsAtCompileTime == 1 + ? (ColFactor == 1 ? index : index % m_cols.value()) + : (RowFactor == 1 ? index : index % m_rows.value()); + + return m_argImpl.template packet(actual_index); + } + + protected: + const ArgTypeNested m_arg; + evaluator m_argImpl; + const variable_if_dynamic m_rows; + const variable_if_dynamic m_cols; +}; + +// -------------------- MatrixWrapper and ArrayWrapper -------------------- +// +// evaluator_wrapper_base is a common base class for the +// MatrixWrapper and ArrayWrapper evaluators. + +template +struct evaluator_wrapper_base : evaluator_base { + typedef remove_all_t ArgType; + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = evaluator::Flags, + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator_wrapper_base(const ArgType& arg) : m_argImpl(arg) {} + + typedef typename ArgType::Scalar Scalar; + typedef typename ArgType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_argImpl.coeff(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { return m_argImpl.coeff(index); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { return m_argImpl.coeffRef(row, col); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { return m_argImpl.coeffRef(index); } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + return m_argImpl.template packet(row, col); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + return m_argImpl.template packet(index); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { + m_argImpl.template writePacket(row, col, x); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { + m_argImpl.template writePacket(index, x); + } + + protected: + evaluator m_argImpl; +}; + +template +struct unary_evaluator > : evaluator_wrapper_base > { + typedef MatrixWrapper XprType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& wrapper) + : evaluator_wrapper_base >(wrapper.nestedExpression()) {} +}; + +template +struct unary_evaluator > : evaluator_wrapper_base > { + typedef ArrayWrapper XprType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& wrapper) + : evaluator_wrapper_base >(wrapper.nestedExpression()) {} +}; + +// -------------------- Reverse -------------------- + +// defined in Reverse.h: +template +struct reverse_packet_cond; + +template +struct unary_evaluator > : evaluator_base > { + typedef Reverse XprType; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + IsRowMajor = XprType::IsRowMajor, + IsColMajor = !IsRowMajor, + ReverseRow = (Direction == Vertical) || (Direction == BothDirections), + ReverseCol = (Direction == Horizontal) || (Direction == BothDirections), + ReversePacket = (Direction == BothDirections) || ((Direction == Vertical) && IsColMajor) || + ((Direction == Horizontal) && IsRowMajor), + + CoeffReadCost = evaluator::CoeffReadCost, + + // let's enable LinearAccess only with vectorization because of the product overhead + // FIXME enable DirectAccess with negative strides? + Flags0 = evaluator::Flags, + LinearAccess = + ((Direction == BothDirections) && (int(Flags0) & PacketAccessBit)) || + ((ReverseRow && XprType::ColsAtCompileTime == 1) || (ReverseCol && XprType::RowsAtCompileTime == 1)) + ? LinearAccessBit + : 0, + + Flags = int(Flags0) & (HereditaryBits | PacketAccessBit | LinearAccess), + + Alignment = 0 // FIXME in some rare cases, Alignment could be preserved, like a Vector4f. + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit unary_evaluator(const XprType& reverse) + : m_argImpl(reverse.nestedExpression()), + m_rows(ReverseRow ? reverse.nestedExpression().rows() : 1), + m_cols(ReverseCol ? reverse.nestedExpression().cols() : 1) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + return m_argImpl.coeff(ReverseRow ? m_rows.value() - row - 1 : row, ReverseCol ? m_cols.value() - col - 1 : col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_argImpl.coeff(m_rows.value() * m_cols.value() - index - 1); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { + return m_argImpl.coeffRef(ReverseRow ? m_rows.value() - row - 1 : row, ReverseCol ? m_cols.value() - col - 1 : col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { + return m_argImpl.coeffRef(m_rows.value() * m_cols.value() - index - 1); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + enum { + PacketSize = unpacket_traits::size, + OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1, + OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1 + }; + typedef internal::reverse_packet_cond reverse_packet; + return reverse_packet::run(m_argImpl.template packet( + ReverseRow ? m_rows.value() - row - OffsetRow : row, ReverseCol ? m_cols.value() - col - OffsetCol : col)); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index index) const { + enum { PacketSize = unpacket_traits::size }; + return preverse( + m_argImpl.template packet(m_rows.value() * m_cols.value() - index - PacketSize)); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { + // FIXME we could factorize some code with packet(i,j) + enum { + PacketSize = unpacket_traits::size, + OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1, + OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1 + }; + typedef internal::reverse_packet_cond reverse_packet; + m_argImpl.template writePacket(ReverseRow ? m_rows.value() - row - OffsetRow : row, + ReverseCol ? m_cols.value() - col - OffsetCol : col, + reverse_packet::run(x)); + } + + template + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { + enum { PacketSize = unpacket_traits::size }; + m_argImpl.template writePacket(m_rows.value() * m_cols.value() - index - PacketSize, preverse(x)); + } + + protected: + evaluator m_argImpl; + + // If we do not reverse rows, then we do not need to know the number of rows; same for columns + // Nonetheless, in this case it is important to set to 1 such that the coeff(index) method works fine for vectors. + const variable_if_dynamic m_rows; + const variable_if_dynamic m_cols; +}; + +// -------------------- Diagonal -------------------- + +template +struct evaluator > : evaluator_base > { + typedef Diagonal XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + + Flags = + (unsigned int)(evaluator::Flags & (HereditaryBits | DirectAccessBit) & ~RowMajorBit) | LinearAccessBit, + + Alignment = 0 + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& diagonal) + : m_argImpl(diagonal.nestedExpression()), m_index(diagonal.index()) {} + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index) const { + return m_argImpl.coeff(row + rowOffset(), row + colOffset()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_argImpl.coeff(index + rowOffset(), index + colOffset()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index) { + return m_argImpl.coeffRef(row + rowOffset(), row + colOffset()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { + return m_argImpl.coeffRef(index + rowOffset(), index + colOffset()); + } + + protected: + evaluator m_argImpl; + const internal::variable_if_dynamicindex m_index; + + private: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowOffset() const { + return m_index.value() > 0 ? 0 : -m_index.value(); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colOffset() const { + return m_index.value() > 0 ? m_index.value() : 0; + } +}; + +//---------------------------------------------------------------------- +// deprecated code +//---------------------------------------------------------------------- + +// -------------------- EvalToTemp -------------------- + +// expression class for evaluating nested expression to a temporary + +template +class EvalToTemp; + +template +struct traits > : public traits {}; + +template +class EvalToTemp : public dense_xpr_base >::type { + public: + typedef typename dense_xpr_base::type Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(EvalToTemp) + + explicit EvalToTemp(const ArgType& arg) : m_arg(arg) {} + + const ArgType& arg() const { return m_arg; } + + EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_arg.rows(); } + + EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_arg.cols(); } + + private: + const ArgType& m_arg; +}; + +template +struct evaluator > : public evaluator { + typedef EvalToTemp XprType; + typedef typename ArgType::PlainObject PlainObject; + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : m_result(xpr.arg()) { + internal::construct_at(this, m_result); + } + + // This constructor is used when nesting an EvalTo evaluator in another evaluator + EIGEN_DEVICE_FUNC evaluator(const ArgType& arg) : m_result(arg) { internal::construct_at(this, m_result); } + + protected: + PlainObject m_result; +}; + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_COREEVALUATORS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h new file mode 100644 index 00000000..f62cf238 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h @@ -0,0 +1,141 @@ +// 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_COREITERATORS_H +#define EIGEN_COREITERATORS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/* This file contains the respective InnerIterator definition of the expressions defined in Eigen/Core + */ + +namespace internal { + +template +class inner_iterator_selector; + +} + +/** \class InnerIterator + * \brief An InnerIterator allows to loop over the element of any matrix expression. + * + * \warning To be used with care because an evaluator is constructed every time an InnerIterator iterator is + * constructed. + * + * TODO: add a usage example + */ +template +class InnerIterator { + protected: + typedef internal::inner_iterator_selector::Kind> IteratorType; + typedef internal::evaluator EvaluatorType; + typedef typename internal::traits::Scalar Scalar; + + public: + /** Construct an iterator over the \a outerId -th row or column of \a xpr */ + InnerIterator(const XprType &xpr, const Index &outerId) : m_eval(xpr), m_iter(m_eval, outerId, xpr.innerSize()) {} + + /// \returns the value of the current coefficient. + EIGEN_STRONG_INLINE Scalar value() const { return m_iter.value(); } + /** Increment the iterator \c *this to the next non-zero coefficient. + * Explicit zeros are not skipped over. To skip explicit zeros, see class SparseView + */ + EIGEN_STRONG_INLINE InnerIterator &operator++() { + m_iter.operator++(); + return *this; + } + EIGEN_STRONG_INLINE InnerIterator &operator+=(Index i) { + m_iter.operator+=(i); + return *this; + } + EIGEN_STRONG_INLINE InnerIterator operator+(Index i) { + InnerIterator result(*this); + result += i; + return result; + } + + /// \returns the column or row index of the current coefficient. + EIGEN_STRONG_INLINE Index index() const { return m_iter.index(); } + /// \returns the row index of the current coefficient. + EIGEN_STRONG_INLINE Index row() const { return m_iter.row(); } + /// \returns the column index of the current coefficient. + EIGEN_STRONG_INLINE Index col() const { return m_iter.col(); } + /// \returns \c true if the iterator \c *this still references a valid coefficient. + EIGEN_STRONG_INLINE operator bool() const { return m_iter; } + + protected: + EvaluatorType m_eval; + IteratorType m_iter; + + 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 EigenBase &, Index outer); +}; + +namespace internal { + +// Generic inner iterator implementation for dense objects +template +class inner_iterator_selector { + protected: + typedef evaluator EvaluatorType; + typedef typename traits::Scalar Scalar; + enum { IsRowMajor = (XprType::Flags & RowMajorBit) == RowMajorBit }; + + public: + EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &innerSize) + : m_eval(eval), m_inner(0), m_outer(outerId), m_end(innerSize) {} + + EIGEN_STRONG_INLINE Scalar value() const { + return (IsRowMajor) ? m_eval.coeff(m_outer, m_inner) : m_eval.coeff(m_inner, m_outer); + } + + EIGEN_STRONG_INLINE inner_iterator_selector &operator++() { + m_inner++; + return *this; + } + + EIGEN_STRONG_INLINE Index 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 EvaluatorType &m_eval; + Index m_inner; + const Index m_outer; + const Index m_end; +}; + +// For iterator-based evaluator, inner-iterator is already implemented as +// evaluator<>::InnerIterator +template +class inner_iterator_selector : public evaluator::InnerIterator { + protected: + typedef typename evaluator::InnerIterator Base; + typedef evaluator EvaluatorType; + + public: + EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, + const Index & /*innerSize*/) + : Base(eval, outerId) {} +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_COREITERATORS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h new file mode 100644 index 00000000..aa79b608 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h @@ -0,0 +1,166 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// 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_CWISE_BINARY_OP_H +#define EIGEN_CWISE_BINARY_OP_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits> { + // we must not inherit from traits since it has + // the potential to cause problems with MSVC + typedef remove_all_t Ancestor; + typedef typename traits::XprKind XprKind; + enum { + RowsAtCompileTime = traits::RowsAtCompileTime, + ColsAtCompileTime = traits::ColsAtCompileTime, + MaxRowsAtCompileTime = traits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = traits::MaxColsAtCompileTime + }; + + // even though we require Lhs and Rhs to have the same scalar type (see CwiseBinaryOp constructor), + // we still want to handle the case when the result type is different. + typedef typename result_of::type Scalar; + typedef typename cwise_promote_storage_type::StorageKind, typename traits::StorageKind, + BinaryOp>::ret StorageKind; + typedef typename promote_index_type::StorageIndex, typename traits::StorageIndex>::type + StorageIndex; + typedef typename Lhs::Nested LhsNested; + typedef typename Rhs::Nested RhsNested; + typedef std::remove_reference_t LhsNested_; + typedef std::remove_reference_t RhsNested_; + enum { + Flags = cwise_promote_storage_order::StorageKind, typename traits::StorageKind, + LhsNested_::Flags & RowMajorBit, RhsNested_::Flags & RowMajorBit>::value + }; +}; +} // end namespace internal + +template +class CwiseBinaryOpImpl; + +/** \class CwiseBinaryOp + * \ingroup Core_Module + * + * \brief Generic expression where a coefficient-wise binary operator is applied to two expressions + * + * \tparam BinaryOp template functor implementing the operator + * \tparam LhsType the type of the left-hand side + * \tparam RhsType the type of the right-hand side + * + * This class represents an expression where a coefficient-wise binary operator is applied to two expressions. + * It is the return type of binary operators, by which we mean only those binary operators where + * both the left-hand side and the right-hand side are Eigen expressions. + * For example, the return type of matrix1+matrix2 is a CwiseBinaryOp. + * + * Most of the time, this is the only way that it is used, so you typically don't have to name + * CwiseBinaryOp types explicitly. + * + * \sa MatrixBase::binaryExpr(const MatrixBase &,const CustomBinaryOp &) const, class CwiseUnaryOp, class + * CwiseNullaryOp + */ +template +class CwiseBinaryOp : public CwiseBinaryOpImpl::StorageKind, + typename internal::traits::StorageKind, BinaryOp>::ret>, + internal::no_assignment_operator { + public: + typedef internal::remove_all_t Functor; + typedef internal::remove_all_t Lhs; + typedef internal::remove_all_t Rhs; + + typedef typename CwiseBinaryOpImpl< + BinaryOp, LhsType, RhsType, + typename internal::cwise_promote_storage_type::StorageKind, + typename internal::traits::StorageKind, BinaryOp>::ret>::Base + Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseBinaryOp) + + EIGEN_CHECK_BINARY_COMPATIBILIY(BinaryOp, typename Lhs::Scalar, typename Rhs::Scalar) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs) + + typedef typename internal::ref_selector::type LhsNested; + typedef typename internal::ref_selector::type RhsNested; + typedef std::remove_reference_t LhsNested_; + typedef std::remove_reference_t RhsNested_; + +#if EIGEN_COMP_MSVC + // Required for Visual Studio or the Copy constructor will probably not get inlined! + EIGEN_STRONG_INLINE CwiseBinaryOp(const CwiseBinaryOp&) = default; +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs, + const BinaryOp& func = BinaryOp()) + : m_lhs(aLhs), m_rhs(aRhs), m_functor(func) { + eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { + // return the fixed size type if available to enable compile time optimizations + return internal::traits>::RowsAtCompileTime == Dynamic ? m_rhs.rows() + : m_lhs.rows(); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { + // return the fixed size type if available to enable compile time optimizations + return internal::traits>::ColsAtCompileTime == Dynamic ? m_rhs.cols() + : m_lhs.cols(); + } + + /** \returns the left hand side nested expression */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const LhsNested_& lhs() const { return m_lhs; } + /** \returns the right hand side nested expression */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const RhsNested_& rhs() const { return m_rhs; } + /** \returns the functor representing the binary operation */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const BinaryOp& functor() const { return m_functor; } + + protected: + LhsNested m_lhs; + RhsNested m_rhs; + const BinaryOp m_functor; +}; + +// Generic API dispatcher +template +class CwiseBinaryOpImpl : public internal::generic_xpr_base>::type { + public: + typedef typename internal::generic_xpr_base>::type Base; +}; + +/** replaces \c *this by \c *this - \a other. + * + * \returns a reference to \c *this + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator-=(const MatrixBase& other) { + call_assignment(derived(), other.derived(), internal::sub_assign_op()); + return derived(); +} + +/** replaces \c *this by \c *this + \a other. + * + * \returns a reference to \c *this + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator+=(const MatrixBase& other) { + call_assignment(derived(), other.derived(), internal::add_assign_op()); + return derived(); +} + +} // end namespace Eigen + +#endif // EIGEN_CWISE_BINARY_OP_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h new file mode 100644 index 00000000..39c33cf0 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h @@ -0,0 +1,971 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-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/. + +#ifndef EIGEN_CWISE_NULLARY_OP_H +#define EIGEN_CWISE_NULLARY_OP_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits > : traits { + enum { Flags = traits::Flags & RowMajorBit }; +}; + +} // namespace internal + +/** \class CwiseNullaryOp + * \ingroup Core_Module + * + * \brief Generic expression of a matrix where all coefficients are defined by a functor + * + * \tparam NullaryOp template functor implementing the operator + * \tparam PlainObjectType the underlying plain matrix/array type + * + * This class represents an expression of a generic nullary operator. + * It is the return type of the Ones(), Zero(), Constant(), Identity() and Random() methods, + * and most of the time this is the only way it is used. + * + * However, if you want to write a function returning such an expression, you + * will need to use this class. + * + * The functor NullaryOp must expose one of the following method: + + +
\c operator()() if the procedural generation does not depend on the coefficient entries + (e.g., random numbers)
\c operator()(Index i)if the procedural generation makes + sense for vectors only and that it depends on the coefficient index \c i (e.g., linspace)
\c + operator()(Index i,Index j)if the procedural generation depends on the matrix coordinates \c i, \c j (e.g., + to generate a checkerboard with 0 and 1)
+ * It is also possible to expose the last two operators if the generation makes sense for matrices but can be optimized + for vectors. + * + * See DenseBase::NullaryExpr(Index,const CustomNullaryOp&) for an example binding + * C++11 random number generators. + * + * A nullary expression can also be used to implement custom sophisticated matrix manipulations + * that cannot be covered by the existing set of natively supported matrix manipulations. + * See this \ref TopicCustomizing_NullaryExpr "page" for some examples and additional explanations + * on the behavior of CwiseNullaryOp. + * + * \sa class CwiseUnaryOp, class CwiseBinaryOp, DenseBase::NullaryExpr + */ +template +class CwiseNullaryOp : public internal::dense_xpr_base >::type, + internal::no_assignment_operator { + public: + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(CwiseNullaryOp) + + EIGEN_DEVICE_FUNC CwiseNullaryOp(Index rows, Index cols, const NullaryOp& func = NullaryOp()) + : m_rows(rows), m_cols(cols), m_functor(func) { + eigen_assert(rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) && cols >= 0 && + (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const { return m_cols.value(); } + + /** \returns the functor representing the nullary operation */ + EIGEN_DEVICE_FUNC const NullaryOp& functor() const { return m_functor; } + + protected: + const internal::variable_if_dynamic m_rows; + const internal::variable_if_dynamic m_cols; + const NullaryOp m_functor; +}; + +/** \returns an expression of a matrix defined by a custom functor \a func + * + * The parameters \a rows and \a cols are the number of rows and of columns of + * the returned matrix. Must be compatible with this MatrixBase type. + * + * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, + * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used + * instead. + * + * The template parameter \a CustomNullaryOp is the type of the functor. + * + * \sa class CwiseNullaryOp + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +#ifndef EIGEN_PARSED_BY_DOXYGEN + const CwiseNullaryOp::PlainObject> +#else + const CwiseNullaryOp +#endif + DenseBase::NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func) { + return CwiseNullaryOp(rows, cols, func); +} + +/** \returns an expression of a matrix defined by a custom functor \a func + * + * The parameter \a size is the size of the returned vector. + * Must be compatible with this MatrixBase type. + * + * \only_for_vectors + * + * This variant is meant to be used for dynamic-size vector types. For fixed-size types, + * it is redundant to pass \a size as argument, so Zero() should be used + * instead. + * + * The template parameter \a CustomNullaryOp is the type of the functor. + * + * Here is an example with C++11 random generators: \include random_cpp11.cpp + * Output: \verbinclude random_cpp11.out + * + * \sa class CwiseNullaryOp + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +#ifndef EIGEN_PARSED_BY_DOXYGEN + const CwiseNullaryOp::PlainObject> +#else + const CwiseNullaryOp +#endif + DenseBase::NullaryExpr(Index size, const CustomNullaryOp& func) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + if (RowsAtCompileTime == 1) + return CwiseNullaryOp(1, size, func); + else + return CwiseNullaryOp(size, 1, func); +} + +/** \returns an expression of a matrix defined by a custom functor \a func + * + * This variant is only for fixed-size DenseBase types. For dynamic-size types, you + * need to use the variants taking size arguments. + * + * The template parameter \a CustomNullaryOp is the type of the functor. + * + * \sa class CwiseNullaryOp + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +#ifndef EIGEN_PARSED_BY_DOXYGEN + const CwiseNullaryOp::PlainObject> +#else + const CwiseNullaryOp +#endif + DenseBase::NullaryExpr(const CustomNullaryOp& func) { + return CwiseNullaryOp(RowsAtCompileTime, ColsAtCompileTime, func); +} + +/** \returns an expression of a constant matrix of value \a value + * + * The parameters \a rows and \a cols are the number of rows and of columns of + * the returned matrix. Must be compatible with this DenseBase type. + * + * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, + * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used + * instead. + * + * The template parameter \a CustomNullaryOp is the type of the functor. + * + * \sa class CwiseNullaryOp + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType +DenseBase::Constant(Index rows, Index cols, const Scalar& value) { + return DenseBase::NullaryExpr(rows, cols, internal::scalar_constant_op(value)); +} + +/** \returns an expression of a constant matrix of value \a value + * + * The parameter \a size is the size of the returned vector. + * Must be compatible with this DenseBase type. + * + * \only_for_vectors + * + * This variant is meant to be used for dynamic-size vector types. For fixed-size types, + * it is redundant to pass \a size as argument, so Zero() should be used + * instead. + * + * The template parameter \a CustomNullaryOp is the type of the functor. + * + * \sa class CwiseNullaryOp + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType +DenseBase::Constant(Index size, const Scalar& value) { + return DenseBase::NullaryExpr(size, internal::scalar_constant_op(value)); +} + +/** \returns an expression of a constant matrix of value \a value + * + * This variant is only for fixed-size DenseBase types. For dynamic-size types, you + * need to use the variants taking size arguments. + * + * The template parameter \a CustomNullaryOp is the type of the functor. + * + * \sa class CwiseNullaryOp + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType +DenseBase::Constant(const Scalar& value) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) + return DenseBase::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, + internal::scalar_constant_op(value)); +} + +/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) + * + * \only_for_vectors + * + * Example: \include DenseBase_LinSpaced_seq_deprecated.cpp + * Output: \verbinclude DenseBase_LinSpaced_seq_deprecated.out + * + * \sa LinSpaced(Index,const Scalar&, const Scalar&), setLinSpaced(Index,const Scalar&,const Scalar&) + */ +template +EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase< + Derived>::RandomAccessLinSpacedReturnType +DenseBase::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return DenseBase::NullaryExpr(size, internal::linspaced_op(low, high, size)); +} + +/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) + * + * \sa LinSpaced(const Scalar&, const Scalar&) + */ +template +EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase< + Derived>::RandomAccessLinSpacedReturnType +DenseBase::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) + return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, + internal::linspaced_op(low, high, Derived::SizeAtCompileTime)); +} + +/** + * \brief Sets a linearly spaced vector. + * + * The function generates 'size' equally spaced values in the closed interval [low,high]. + * When size is set to 1, a vector of length 1 containing 'high' is returned. + * + * \only_for_vectors + * + * Example: \include DenseBase_LinSpaced.cpp + * Output: \verbinclude DenseBase_LinSpaced.out + * + * For integer scalar types, an even spacing is possible if and only if the length of the range, + * i.e., \c high-low is a scalar multiple of \c size-1, or if \c size is a scalar multiple of the + * number of values \c high-low+1 (meaning each value can be repeated the same number of time). + * If one of these two considions is not satisfied, then \c high is lowered to the largest value + * satisfying one of this constraint. + * Here are some examples: + * + * Example: \include DenseBase_LinSpacedInt.cpp + * Output: \verbinclude DenseBase_LinSpacedInt.out + * + * \sa setLinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessLinSpacedReturnType +DenseBase::LinSpaced(Index size, const Scalar& low, const Scalar& high) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return DenseBase::NullaryExpr(size, internal::linspaced_op(low, high, size)); +} + +/** + * \copydoc DenseBase::LinSpaced(Index, const Scalar&, const Scalar&) + * Special version for fixed size types which does not require the size parameter. + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessLinSpacedReturnType +DenseBase::LinSpaced(const Scalar& low, const Scalar& high) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) + return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, + internal::linspaced_op(low, high, Derived::SizeAtCompileTime)); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessEqualSpacedReturnType +DenseBase::EqualSpaced(Index size, const Scalar& low, const Scalar& step) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return DenseBase::NullaryExpr(size, internal::equalspaced_op(low, step)); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessEqualSpacedReturnType +DenseBase::EqualSpaced(const Scalar& low, const Scalar& step) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, internal::equalspaced_op(low, step)); +} + +/** \returns true if all coefficients in this matrix are approximately equal to \a val, to within precision \a prec */ +template +EIGEN_DEVICE_FUNC bool DenseBase::isApproxToConstant(const Scalar& val, const RealScalar& prec) const { + typename internal::nested_eval::type self(derived()); + for (Index j = 0; j < cols(); ++j) + for (Index i = 0; i < rows(); ++i) + if (!internal::isApprox(self.coeff(i, j), val, prec)) return false; + return true; +} + +/** This is just an alias for isApproxToConstant(). + * + * \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */ +template +EIGEN_DEVICE_FUNC bool DenseBase::isConstant(const Scalar& val, const RealScalar& prec) const { + return isApproxToConstant(val, prec); +} + +/** Alias for setConstant(): sets all coefficients in this expression to \a val. + * + * \sa setConstant(), Constant(), class CwiseNullaryOp + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void DenseBase::fill(const Scalar& val) { + setConstant(val); +} + +/** Sets all coefficients in this expression to value \a val. + * + * \sa fill(), setConstant(Index,const Scalar&), setConstant(Index,Index,const Scalar&), setZero(), setOnes(), + * Constant(), class CwiseNullaryOp, setZero(), setOnes() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setConstant(const Scalar& val) { + return derived() = Constant(rows(), cols(), val); +} + +/** Resizes to the given \a size, and sets all coefficients in this expression to the given value \a val. + * + * \only_for_vectors + * + * Example: \include Matrix_setConstant_int.cpp + * Output: \verbinclude Matrix_setConstant_int.out + * + * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,Index,const Scalar&), class CwiseNullaryOp, + * MatrixBase::Constant(const Scalar&) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setConstant(Index size, const Scalar& val) { + resize(size); + return setConstant(val); +} + +/** Resizes to the given size, and sets all coefficients in this expression to the given value \a val. + * + * \param rows the new number of rows + * \param cols the new number of columns + * \param val the value to which all coefficients are set + * + * Example: \include Matrix_setConstant_int_int.cpp + * Output: \verbinclude Matrix_setConstant_int_int.out + * + * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, + * MatrixBase::Constant(const Scalar&) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setConstant(Index rows, Index cols, + const Scalar& val) { + resize(rows, cols); + return setConstant(val); +} + +/** Resizes to the given size, changing only the number of columns, and sets all + * coefficients in this expression to the given value \a val. For the parameter + * of type NoChange_t, just pass the special value \c NoChange. + * + * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, + * MatrixBase::Constant(const Scalar&) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setConstant(NoChange_t, Index cols, + const Scalar& val) { + return setConstant(rows(), cols, val); +} + +/** Resizes to the given size, changing only the number of rows, and sets all + * coefficients in this expression to the given value \a val. For the parameter + * of type NoChange_t, just pass the special value \c NoChange. + * + * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, + * MatrixBase::Constant(const Scalar&) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setConstant(Index rows, NoChange_t, + const Scalar& val) { + return setConstant(rows, cols(), val); +} + +/** + * \brief Sets a linearly spaced vector. + * + * The function generates 'size' equally spaced values in the closed interval [low,high]. + * When size is set to 1, a vector of length 1 containing 'high' is returned. + * + * \only_for_vectors + * + * Example: \include DenseBase_setLinSpaced.cpp + * Output: \verbinclude DenseBase_setLinSpaced.out + * + * For integer scalar types, do not miss the explanations on the definition + * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink. + * + * \sa LinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(Index newSize, const Scalar& low, + const Scalar& high) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op(low, high, newSize)); +} + +/** + * \brief Sets a linearly spaced vector. + * + * The function fills \c *this with equally spaced values in the closed interval [low,high]. + * When size is set to 1, a vector of length 1 containing 'high' is returned. + * + * \only_for_vectors + * + * For integer scalar types, do not miss the explanations on the definition + * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink. + * + * \sa LinSpaced(Index,const Scalar&,const Scalar&), setLinSpaced(Index, const Scalar&, const Scalar&), CwiseNullaryOp + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(const Scalar& low, const Scalar& high) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return setLinSpaced(size(), low, high); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setEqualSpaced(Index newSize, const Scalar& low, + const Scalar& step) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return derived() = Derived::NullaryExpr(newSize, internal::equalspaced_op(low, step)); +} +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setEqualSpaced(const Scalar& low, + const Scalar& step) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return setEqualSpaced(size(), low, step); +} + +// zero: + +/** \returns an expression of a zero matrix. + * + * The parameters \a rows and \a cols are the number of rows and of columns of + * the returned matrix. Must be compatible with this MatrixBase type. + * + * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, + * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used + * instead. + * + * Example: \include MatrixBase_zero_int_int.cpp + * Output: \verbinclude MatrixBase_zero_int_int.out + * + * \sa Zero(), Zero(Index) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Zero( + Index rows, Index cols) { + return Constant(rows, cols, Scalar(0)); +} + +/** \returns an expression of a zero vector. + * + * The parameter \a size is the size of the returned vector. + * Must be compatible with this MatrixBase type. + * + * \only_for_vectors + * + * This variant is meant to be used for dynamic-size vector types. For fixed-size types, + * it is redundant to pass \a size as argument, so Zero() should be used + * instead. + * + * Example: \include MatrixBase_zero_int.cpp + * Output: \verbinclude MatrixBase_zero_int.out + * + * \sa Zero(), Zero(Index,Index) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Zero( + Index size) { + return Constant(size, Scalar(0)); +} + +/** \returns an expression of a fixed-size zero matrix or vector. + * + * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you + * need to use the variants taking size arguments. + * + * Example: \include MatrixBase_zero.cpp + * Output: \verbinclude MatrixBase_zero.out + * + * \sa Zero(Index), Zero(Index,Index) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Zero() { + return Constant(Scalar(0)); +} + +/** \returns true if *this is approximately equal to the zero matrix, + * within the precision given by \a prec. + * + * Example: \include MatrixBase_isZero.cpp + * Output: \verbinclude MatrixBase_isZero.out + * + * \sa class CwiseNullaryOp, Zero() + */ +template +EIGEN_DEVICE_FUNC bool DenseBase::isZero(const RealScalar& prec) const { + typename internal::nested_eval::type self(derived()); + for (Index j = 0; j < cols(); ++j) + for (Index i = 0; i < rows(); ++i) + if (!internal::isMuchSmallerThan(self.coeff(i, j), static_cast(1), prec)) return false; + return true; +} + +/** Sets all coefficients in this expression to zero. + * + * Example: \include MatrixBase_setZero.cpp + * Output: \verbinclude MatrixBase_setZero.out + * + * \sa class CwiseNullaryOp, Zero() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setZero() { + return setConstant(Scalar(0)); +} + +/** Resizes to the given \a size, and sets all coefficients in this expression to zero. + * + * \only_for_vectors + * + * Example: \include Matrix_setZero_int.cpp + * Output: \verbinclude Matrix_setZero_int.out + * + * \sa DenseBase::setZero(), setZero(Index,Index), class CwiseNullaryOp, DenseBase::Zero() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setZero(Index newSize) { + resize(newSize); + return setConstant(Scalar(0)); +} + +/** Resizes to the given size, and sets all coefficients in this expression to zero. + * + * \param rows the new number of rows + * \param cols the new number of columns + * + * Example: \include Matrix_setZero_int_int.cpp + * Output: \verbinclude Matrix_setZero_int_int.out + * + * \sa DenseBase::setZero(), setZero(Index), class CwiseNullaryOp, DenseBase::Zero() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setZero(Index rows, Index cols) { + resize(rows, cols); + return setConstant(Scalar(0)); +} + +/** Resizes to the given size, changing only the number of columns, and sets all + * coefficients in this expression to zero. For the parameter of type NoChange_t, + * just pass the special value \c NoChange. + * + * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(Index, NoChange_t), class CwiseNullaryOp, + * DenseBase::Zero() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setZero(NoChange_t, Index cols) { + return setZero(rows(), cols); +} + +/** Resizes to the given size, changing only the number of rows, and sets all + * coefficients in this expression to zero. For the parameter of type NoChange_t, + * just pass the special value \c NoChange. + * + * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(NoChange_t, Index), class CwiseNullaryOp, + * DenseBase::Zero() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setZero(Index rows, NoChange_t) { + return setZero(rows, cols()); +} + +// ones: + +/** \returns an expression of a matrix where all coefficients equal one. + * + * The parameters \a rows and \a cols are the number of rows and of columns of + * the returned matrix. Must be compatible with this MatrixBase type. + * + * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, + * it is redundant to pass \a rows and \a cols as arguments, so Ones() should be used + * instead. + * + * Example: \include MatrixBase_ones_int_int.cpp + * Output: \verbinclude MatrixBase_ones_int_int.out + * + * \sa Ones(), Ones(Index), isOnes(), class Ones + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Ones( + Index rows, Index cols) { + return Constant(rows, cols, Scalar(1)); +} + +/** \returns an expression of a vector where all coefficients equal one. + * + * The parameter \a newSize is the size of the returned vector. + * Must be compatible with this MatrixBase type. + * + * \only_for_vectors + * + * This variant is meant to be used for dynamic-size vector types. For fixed-size types, + * it is redundant to pass \a size as argument, so Ones() should be used + * instead. + * + * Example: \include MatrixBase_ones_int.cpp + * Output: \verbinclude MatrixBase_ones_int.out + * + * \sa Ones(), Ones(Index,Index), isOnes(), class Ones + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Ones( + Index newSize) { + return Constant(newSize, Scalar(1)); +} + +/** \returns an expression of a fixed-size matrix or vector where all coefficients equal one. + * + * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you + * need to use the variants taking size arguments. + * + * Example: \include MatrixBase_ones.cpp + * Output: \verbinclude MatrixBase_ones.out + * + * \sa Ones(Index), Ones(Index,Index), isOnes(), class Ones + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Ones() { + return Constant(Scalar(1)); +} + +/** \returns true if *this is approximately equal to the matrix where all coefficients + * are equal to 1, within the precision given by \a prec. + * + * Example: \include MatrixBase_isOnes.cpp + * Output: \verbinclude MatrixBase_isOnes.out + * + * \sa class CwiseNullaryOp, Ones() + */ +template +EIGEN_DEVICE_FUNC bool DenseBase::isOnes(const RealScalar& prec) const { + return isApproxToConstant(Scalar(1), prec); +} + +/** Sets all coefficients in this expression to one. + * + * Example: \include MatrixBase_setOnes.cpp + * Output: \verbinclude MatrixBase_setOnes.out + * + * \sa class CwiseNullaryOp, Ones() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setOnes() { + return setConstant(Scalar(1)); +} + +/** Resizes to the given \a newSize, and sets all coefficients in this expression to one. + * + * \only_for_vectors + * + * Example: \include Matrix_setOnes_int.cpp + * Output: \verbinclude Matrix_setOnes_int.out + * + * \sa MatrixBase::setOnes(), setOnes(Index,Index), class CwiseNullaryOp, MatrixBase::Ones() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setOnes(Index newSize) { + resize(newSize); + return setConstant(Scalar(1)); +} + +/** Resizes to the given size, and sets all coefficients in this expression to one. + * + * \param rows the new number of rows + * \param cols the new number of columns + * + * Example: \include Matrix_setOnes_int_int.cpp + * Output: \verbinclude Matrix_setOnes_int_int.out + * + * \sa MatrixBase::setOnes(), setOnes(Index), class CwiseNullaryOp, MatrixBase::Ones() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setOnes(Index rows, Index cols) { + resize(rows, cols); + return setConstant(Scalar(1)); +} + +/** Resizes to the given size, changing only the number of rows, and sets all + * coefficients in this expression to one. For the parameter of type NoChange_t, + * just pass the special value \c NoChange. + * + * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(NoChange_t, Index), class CwiseNullaryOp, + * MatrixBase::Ones() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setOnes(Index rows, NoChange_t) { + return setOnes(rows, cols()); +} + +/** Resizes to the given size, changing only the number of columns, and sets all + * coefficients in this expression to one. For the parameter of type NoChange_t, + * just pass the special value \c NoChange. + * + * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(Index, NoChange_t) class CwiseNullaryOp, + * MatrixBase::Ones() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setOnes(NoChange_t, Index cols) { + return setOnes(rows(), cols); +} + +// Identity: + +/** \returns an expression of the identity matrix (not necessarily square). + * + * The parameters \a rows and \a cols are the number of rows and of columns of + * the returned matrix. Must be compatible with this MatrixBase type. + * + * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, + * it is redundant to pass \a rows and \a cols as arguments, so Identity() should be used + * instead. + * + * Example: \include MatrixBase_identity_int_int.cpp + * Output: \verbinclude MatrixBase_identity_int_int.out + * + * \sa Identity(), setIdentity(), isIdentity() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::IdentityReturnType +MatrixBase::Identity(Index rows, Index cols) { + return DenseBase::NullaryExpr(rows, cols, internal::scalar_identity_op()); +} + +/** \returns an expression of the identity matrix (not necessarily square). + * + * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you + * need to use the variant taking size arguments. + * + * Example: \include MatrixBase_identity.cpp + * Output: \verbinclude MatrixBase_identity.out + * + * \sa Identity(Index,Index), setIdentity(), isIdentity() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::IdentityReturnType +MatrixBase::Identity() { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) + return MatrixBase::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_identity_op()); +} + +/** \returns true if *this is approximately equal to the identity matrix + * (not necessarily square), + * within the precision given by \a prec. + * + * Example: \include MatrixBase_isIdentity.cpp + * Output: \verbinclude MatrixBase_isIdentity.out + * + * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), setIdentity() + */ +template +bool MatrixBase::isIdentity(const RealScalar& prec) const { + typename internal::nested_eval::type self(derived()); + for (Index j = 0; j < cols(); ++j) { + for (Index i = 0; i < rows(); ++i) { + if (i == j) { + if (!internal::isApprox(self.coeff(i, j), static_cast(1), prec)) return false; + } else { + if (!internal::isMuchSmallerThan(self.coeff(i, j), static_cast(1), prec)) return false; + } + } + } + return true; +} + +namespace internal { + +template = 16)> +struct setIdentity_impl { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Derived& run(Derived& m) { + return m = Derived::Identity(m.rows(), m.cols()); + } +}; + +template +struct setIdentity_impl { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Derived& run(Derived& m) { + m.setZero(); + const Index size = numext::mini(m.rows(), m.cols()); + for (Index i = 0; i < size; ++i) m.coeffRef(i, i) = typename Derived::Scalar(1); + return m; + } +}; + +} // end namespace internal + +/** Writes the identity expression (not necessarily square) into *this. + * + * Example: \include MatrixBase_setIdentity.cpp + * Output: \verbinclude MatrixBase_setIdentity.out + * + * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), isIdentity() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity() { + return internal::setIdentity_impl::run(derived()); +} + +/** \brief Resizes to the given size, and writes the identity expression (not necessarily square) into *this. + * + * \param rows the new number of rows + * \param cols the new number of columns + * + * Example: \include Matrix_setIdentity_int_int.cpp + * Output: \verbinclude Matrix_setIdentity_int_int.out + * + * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity(Index rows, Index cols) { + derived().resize(rows, cols); + return setIdentity(); +} + +/** \returns an expression of the i-th unit (basis) vector. + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::Unit( + Index newSize, Index i) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return BasisReturnType(SquareMatrixType::Identity(newSize, newSize), i); +} + +/** \returns an expression of the i-th unit (basis) vector. + * + * \only_for_vectors + * + * This variant is for fixed-size vector only. + * + * \sa MatrixBase::Unit(Index,Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::Unit( + Index i) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + return BasisReturnType(SquareMatrixType::Identity(), i); +} + +/** \returns an expression of the X axis unit vector (1{,0}^*) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), + * MatrixBase::UnitW() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::UnitX() { + return Derived::Unit(0); +} + +/** \returns an expression of the Y axis unit vector (0,1{,0}^*) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), + * MatrixBase::UnitW() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::UnitY() { + return Derived::Unit(1); +} + +/** \returns an expression of the Z axis unit vector (0,0,1{,0}^*) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), + * MatrixBase::UnitW() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::UnitZ() { + return Derived::Unit(2); +} + +/** \returns an expression of the W axis unit vector (0,0,0,1) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), + * MatrixBase::UnitW() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::UnitW() { + return Derived::Unit(3); +} + +/** \brief Set the coefficients of \c *this to the i-th unit (basis) vector + * + * \param i index of the unique coefficient to be set to 1 + * + * \only_for_vectors + * + * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::setUnit(Index i) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + eigen_assert(i < size()); + derived().setZero(); + derived().coeffRef(i) = Scalar(1); + return derived(); +} + +/** \brief Resizes to the given \a newSize, and writes the i-th unit (basis) vector into *this. + * + * \param newSize the new size of the vector + * \param i index of the unique coefficient to be set to 1 + * + * \only_for_vectors + * + * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::setUnit(Index newSize, Index i) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + eigen_assert(i < newSize); + derived().resize(newSize); + return setUnit(i); +} + +} // end namespace Eigen + +#endif // EIGEN_CWISE_NULLARY_OP_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h new file mode 100644 index 00000000..9bb0d407 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h @@ -0,0 +1,171 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// Copyright (C) 2016 Eugene Brevdo +// +// 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_CWISE_TERNARY_OP_H +#define EIGEN_CWISE_TERNARY_OP_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits> { + // we must not inherit from traits since it has + // the potential to cause problems with MSVC + typedef remove_all_t Ancestor; + typedef typename traits::XprKind XprKind; + enum { + RowsAtCompileTime = traits::RowsAtCompileTime, + ColsAtCompileTime = traits::ColsAtCompileTime, + MaxRowsAtCompileTime = traits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = traits::MaxColsAtCompileTime + }; + + // even though we require Arg1, Arg2, and Arg3 to have the same scalar type + // (see CwiseTernaryOp constructor), + // we still want to handle the case when the result type is different. + typedef typename result_of::type Scalar; + + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::StorageIndex StorageIndex; + + typedef typename Arg1::Nested Arg1Nested; + typedef typename Arg2::Nested Arg2Nested; + typedef typename Arg3::Nested Arg3Nested; + typedef std::remove_reference_t Arg1Nested_; + typedef std::remove_reference_t Arg2Nested_; + typedef std::remove_reference_t Arg3Nested_; + enum { Flags = Arg1Nested_::Flags & RowMajorBit }; +}; +} // end namespace internal + +template +class CwiseTernaryOpImpl; + +/** \class CwiseTernaryOp + * \ingroup Core_Module + * + * \brief Generic expression where a coefficient-wise ternary operator is + * applied to two expressions + * + * \tparam TernaryOp template functor implementing the operator + * \tparam Arg1Type the type of the first argument + * \tparam Arg2Type the type of the second argument + * \tparam Arg3Type the type of the third argument + * + * This class represents an expression where a coefficient-wise ternary + * operator is applied to three expressions. + * It is the return type of ternary operators, by which we mean only those + * ternary operators where + * all three arguments are Eigen expressions. + * For example, the return type of betainc(matrix1, matrix2, matrix3) is a + * CwiseTernaryOp. + * + * Most of the time, this is the only way that it is used, so you typically + * don't have to name + * CwiseTernaryOp types explicitly. + * + * \sa MatrixBase::ternaryExpr(const MatrixBase &, const + * MatrixBase &, const CustomTernaryOp &) const, class CwiseBinaryOp, + * class CwiseUnaryOp, class CwiseNullaryOp + */ +template +class CwiseTernaryOp : public CwiseTernaryOpImpl::StorageKind>, + internal::no_assignment_operator { + public: + typedef internal::remove_all_t Arg1; + typedef internal::remove_all_t Arg2; + typedef internal::remove_all_t Arg3; + + // require the sizes to match + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg2) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg3) + + // The index types should match + EIGEN_STATIC_ASSERT((internal::is_same::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + EIGEN_STATIC_ASSERT((internal::is_same::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + + typedef typename CwiseTernaryOpImpl::StorageKind>::Base Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseTernaryOp) + + typedef typename internal::ref_selector::type Arg1Nested; + typedef typename internal::ref_selector::type Arg2Nested; + typedef typename internal::ref_selector::type Arg3Nested; + typedef std::remove_reference_t Arg1Nested_; + typedef std::remove_reference_t Arg2Nested_; + typedef std::remove_reference_t Arg3Nested_; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CwiseTernaryOp(const Arg1& a1, const Arg2& a2, const Arg3& a3, + const TernaryOp& func = TernaryOp()) + : m_arg1(a1), m_arg2(a2), m_arg3(a3), m_functor(func) { + eigen_assert(a1.rows() == a2.rows() && a1.cols() == a2.cols() && a1.rows() == a3.rows() && a1.cols() == a3.cols()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { + // return the fixed size type if available to enable compile time + // optimizations + if (internal::traits>::RowsAtCompileTime == Dynamic && + internal::traits>::RowsAtCompileTime == Dynamic) + return m_arg3.rows(); + else if (internal::traits>::RowsAtCompileTime == Dynamic && + internal::traits>::RowsAtCompileTime == Dynamic) + return m_arg2.rows(); + else + return m_arg1.rows(); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { + // return the fixed size type if available to enable compile time + // optimizations + if (internal::traits>::ColsAtCompileTime == Dynamic && + internal::traits>::ColsAtCompileTime == Dynamic) + return m_arg3.cols(); + else if (internal::traits>::ColsAtCompileTime == Dynamic && + internal::traits>::ColsAtCompileTime == Dynamic) + return m_arg2.cols(); + else + return m_arg1.cols(); + } + + /** \returns the first argument nested expression */ + EIGEN_DEVICE_FUNC const Arg1Nested_& arg1() const { return m_arg1; } + /** \returns the first argument nested expression */ + EIGEN_DEVICE_FUNC const Arg2Nested_& arg2() const { return m_arg2; } + /** \returns the third argument nested expression */ + EIGEN_DEVICE_FUNC const Arg3Nested_& arg3() const { return m_arg3; } + /** \returns the functor representing the ternary operation */ + EIGEN_DEVICE_FUNC const TernaryOp& functor() const { return m_functor; } + + protected: + Arg1Nested m_arg1; + Arg2Nested m_arg2; + Arg3Nested m_arg3; + const TernaryOp m_functor; +}; + +// Generic API dispatcher +template +class CwiseTernaryOpImpl : public internal::generic_xpr_base>::type { + public: + typedef typename internal::generic_xpr_base>::type Base; +}; + +} // end namespace Eigen + +#endif // EIGEN_CWISE_TERNARY_OP_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h new file mode 100644 index 00000000..42ed459a --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h @@ -0,0 +1,91 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// 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_CWISE_UNARY_OP_H +#define EIGEN_CWISE_UNARY_OP_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits > : traits { + typedef typename result_of::type Scalar; + typedef typename XprType::Nested XprTypeNested; + typedef std::remove_reference_t XprTypeNested_; + enum { Flags = XprTypeNested_::Flags & RowMajorBit }; +}; +} // namespace internal + +template +class CwiseUnaryOpImpl; + +/** \class CwiseUnaryOp + * \ingroup Core_Module + * + * \brief Generic expression where a coefficient-wise unary operator is applied to an expression + * + * \tparam UnaryOp template functor implementing the operator + * \tparam XprType the type of the expression to which we are applying the unary operator + * + * This class represents an expression where a unary operator is applied to an expression. + * It is the return type of all operations taking exactly 1 input expression, regardless of the + * presence of other inputs such as scalars. For example, the operator* in the expression 3*matrix + * is considered unary, because only the right-hand side is an expression, and its + * return type is a specialization of CwiseUnaryOp. + * + * Most of the time, this is the only way that it is used, so you typically don't have to name + * CwiseUnaryOp types explicitly. + * + * \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp + */ +template +class CwiseUnaryOp : public CwiseUnaryOpImpl::StorageKind>, + internal::no_assignment_operator { + public: + typedef typename CwiseUnaryOpImpl::StorageKind>::Base Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryOp) + typedef typename internal::ref_selector::type XprTypeNested; + typedef internal::remove_all_t NestedExpression; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit CwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp()) + : m_xpr(xpr), m_functor(func) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.cols(); } + + /** \returns the functor representing the unary operation */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryOp& functor() const { return m_functor; } + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t& nestedExpression() const { + return m_xpr; + } + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE internal::remove_all_t& nestedExpression() { return m_xpr; } + + protected: + XprTypeNested m_xpr; + const UnaryOp m_functor; +}; + +// Generic API dispatcher +template +class CwiseUnaryOpImpl : public internal::generic_xpr_base >::type { + public: + typedef typename internal::generic_xpr_base >::type Base; +}; + +} // end namespace Eigen + +#endif // EIGEN_CWISE_UNARY_OP_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h new file mode 100644 index 00000000..725b3371 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h @@ -0,0 +1,137 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-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/. + +#ifndef EIGEN_CWISE_UNARY_VIEW_H +#define EIGEN_CWISE_UNARY_VIEW_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits > : traits { + typedef typename result_of::Scalar&)>::type Scalar; + typedef typename MatrixType::Nested MatrixTypeNested; + typedef remove_all_t MatrixTypeNested_; + enum { + FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, + Flags = + traits::Flags & + (RowMajorBit | FlagsLvalueBit | DirectAccessBit), // FIXME DirectAccessBit should not be handled by expressions + MatrixTypeInnerStride = inner_stride_at_compile_time::ret, + // need to cast the sizeof's from size_t to int explicitly, otherwise: + // "error: no integral type can represent all of the enumerator values + InnerStrideAtCompileTime = + StrideType::InnerStrideAtCompileTime == 0 + ? (MatrixTypeInnerStride == Dynamic + ? int(Dynamic) + : int(MatrixTypeInnerStride) * int(sizeof(typename traits::Scalar) / sizeof(Scalar))) + : int(StrideType::InnerStrideAtCompileTime), + + OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0 + ? (outer_stride_at_compile_time::ret == Dynamic + ? int(Dynamic) + : outer_stride_at_compile_time::ret * + int(sizeof(typename traits::Scalar) / sizeof(Scalar))) + : int(StrideType::OuterStrideAtCompileTime) + }; +}; +} // namespace internal + +template +class CwiseUnaryViewImpl; + +/** \class CwiseUnaryView + * \ingroup Core_Module + * + * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector + * + * \tparam ViewOp template functor implementing the view + * \tparam MatrixType the type of the matrix we are applying the unary operator + * + * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector. + * It is the return type of real() and imag(), and most of the time this is the only way it is used. + * + * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp + */ +template +class CwiseUnaryView + : public CwiseUnaryViewImpl::StorageKind> { + public: + typedef typename CwiseUnaryViewImpl::StorageKind>::Base Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryView) + typedef typename internal::ref_selector::non_const_type MatrixTypeNested; + typedef internal::remove_all_t NestedExpression; + + explicit EIGEN_DEVICE_FUNC inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp()) + : m_matrix(mat), m_functor(func) {} + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView) + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + + /** \returns the functor representing unary operation */ + EIGEN_DEVICE_FUNC const ViewOp& functor() const { return m_functor; } + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC const internal::remove_all_t& nestedExpression() const { return m_matrix; } + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC std::remove_reference_t& nestedExpression() { return m_matrix; } + + protected: + MatrixTypeNested m_matrix; + ViewOp m_functor; +}; + +// Generic API dispatcher +template +class CwiseUnaryViewImpl : public internal::generic_xpr_base >::type { + public: + typedef typename internal::generic_xpr_base >::type Base; +}; + +template +class CwiseUnaryViewImpl + : public internal::dense_xpr_base >::type { + public: + typedef CwiseUnaryView Derived; + typedef typename internal::dense_xpr_base >::type Base; + + EIGEN_DENSE_PUBLIC_INTERFACE(Derived) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl) + + EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { + return StrideType::InnerStrideAtCompileTime != 0 + ? int(StrideType::InnerStrideAtCompileTime) + : derived().nestedExpression().innerStride() * sizeof(typename internal::traits::Scalar) / + sizeof(Scalar); + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { + return StrideType::OuterStrideAtCompileTime != 0 + ? int(StrideType::OuterStrideAtCompileTime) + : derived().nestedExpression().outerStride() * sizeof(typename internal::traits::Scalar) / + sizeof(Scalar); + } + + protected: + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(CwiseUnaryViewImpl) +}; + +} // end namespace Eigen + +#endif // EIGEN_CWISE_UNARY_VIEW_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h new file mode 100644 index 00000000..5ab54efa --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h @@ -0,0 +1,645 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2007-2010 Benoit Jacob +// Copyright (C) 2008-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/. + +#ifndef EIGEN_DENSEBASE_H +#define EIGEN_DENSEBASE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +// The index type defined by EIGEN_DEFAULT_DENSE_INDEX_TYPE must be a signed type. +EIGEN_STATIC_ASSERT(NumTraits::IsSigned, THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE) + +/** \class DenseBase + * \ingroup Core_Module + * + * \brief Base class for all dense matrices, vectors, and arrays + * + * This class is the base that is inherited by all dense objects (matrix, vector, arrays, + * and related expression types). The common Eigen API for dense objects is contained in this class. + * + * \tparam Derived is the derived type, e.g., a matrix type or an expression. + * + * 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_DENSEBASE_PLUGIN. + * + * \sa \blank \ref TopicClassHierarchy + */ +template +class DenseBase +#ifndef EIGEN_PARSED_BY_DOXYGEN + : public DenseCoeffsBase::value> +#else + : public DenseCoeffsBase +#endif // not EIGEN_PARSED_BY_DOXYGEN +{ + public: + /** Inner iterator type to iterate over the coefficients of a row or column. + * \sa class InnerIterator + */ + typedef Eigen::InnerIterator InnerIterator; + + typedef typename internal::traits::StorageKind StorageKind; + + /** + * \brief The type used to store indices + * \details This typedef is relevant for types that store multiple indices such as + * PermutationMatrix or Transpositions, otherwise it defaults to Eigen::Index + * \sa \blank \ref TopicPreprocessorDirectives, Eigen::Index, SparseMatrixBase. + */ + typedef typename internal::traits::StorageIndex StorageIndex; + + /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex, etc. */ + 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 NumTraits::Real RealScalar; + typedef DenseCoeffsBase::value> Base; + + using Base::coeff; + using Base::coeffByOuterInner; + using Base::colIndexByOuterInner; + using Base::cols; + using Base::const_cast_derived; + using Base::derived; + using Base::rowIndexByOuterInner; + using Base::rows; + using Base::size; + using Base::operator(); + using Base::operator[]; + using Base::colStride; + using Base::innerStride; + using Base::outerStride; + using Base::rowStride; + using Base::stride; + using Base::w; + using Base::x; + using Base::y; + using Base::z; + typedef typename Base::CoeffReturnType CoeffReturnType; + + 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_of_xpr_at_compile_time::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 = internal::traits::MaxRowsAtCompileTime, + /**< This value is equal to the maximum possible number of rows that this expression + * might have. If this expression might have an arbitrarily high number of rows, + * this value is set to \a Dynamic. + * + * This value is useful to know when evaluating an expression, in order to determine + * whether it is possible to avoid doing a dynamic memory allocation. + * + * \sa RowsAtCompileTime, MaxColsAtCompileTime, MaxSizeAtCompileTime + */ + + MaxColsAtCompileTime = internal::traits::MaxColsAtCompileTime, + /**< This value is equal to the maximum possible number of columns that this expression + * might have. If this expression might have an arbitrarily high number of columns, + * this value is set to \a Dynamic. + * + * This value is useful to know when evaluating an expression, in order to determine + * whether it is possible to avoid doing a dynamic memory allocation. + * + * \sa ColsAtCompileTime, MaxRowsAtCompileTime, MaxSizeAtCompileTime + */ + + MaxSizeAtCompileTime = internal::size_at_compile_time(internal::traits::MaxRowsAtCompileTime, + internal::traits::MaxColsAtCompileTime), + /**< This value is equal to the maximum possible number of coefficients that this expression + * might have. If this expression might have an arbitrarily high number of coefficients, + * this value is set to \a Dynamic. + * + * This value is useful to know when evaluating an expression, in order to determine + * whether it is possible to avoid doing a dynamic memory allocation. + * + * \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime + */ + + IsVectorAtCompileTime = + internal::traits::RowsAtCompileTime == 1 || internal::traits::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 = int(Flags) & RowMajorBit, /**< True if this expression has row-major storage order. */ + + InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime) + : int(IsRowMajor) ? int(ColsAtCompileTime) + : int(RowsAtCompileTime), + + InnerStrideAtCompileTime = internal::inner_stride_at_compile_time::ret, + OuterStrideAtCompileTime = internal::outer_stride_at_compile_time::ret + }; + + typedef typename internal::find_best_packet::type PacketScalar; + + enum { IsPlainObjectBase = 0 }; + + /** The plain matrix type corresponding to this expression. + * \sa PlainObject */ + typedef Matrix::Scalar, internal::traits::RowsAtCompileTime, + internal::traits::ColsAtCompileTime, + AutoAlign | (internal::traits::Flags & RowMajorBit ? RowMajor : ColMajor), + internal::traits::MaxRowsAtCompileTime, internal::traits::MaxColsAtCompileTime> + PlainMatrix; + + /** The plain array type corresponding to this expression. + * \sa PlainObject */ + typedef Array::Scalar, internal::traits::RowsAtCompileTime, + internal::traits::ColsAtCompileTime, + AutoAlign | (internal::traits::Flags & RowMajorBit ? RowMajor : ColMajor), + internal::traits::MaxRowsAtCompileTime, internal::traits::MaxColsAtCompileTime> + PlainArray; + + /** \brief The plain matrix or array type corresponding to this expression. + * + * This is not necessarily exactly the return type of eval(). In the case of plain matrices, + * the return type of eval() is a const reference to a matrix, not a matrix! It is however guaranteed + * that the return type of eval() is either PlainObject or const PlainObject&. + */ + typedef std::conditional_t::XprKind, MatrixXpr>::value, + PlainMatrix, PlainArray> + PlainObject; + + /** \returns the outer size. + * + * \note For a vector, this returns just 1. For a matrix (non-vector), this is the major dimension + * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of columns for a + * column-major matrix, and the number of rows for a row-major matrix. */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerSize() const { + return IsVectorAtCompileTime ? 1 : int(IsRowMajor) ? this->rows() : this->cols(); + } + + /** \returns the inner size. + * + * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension + * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a + * column-major matrix, and the number of columns for a row-major matrix. */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index innerSize() const { + return IsVectorAtCompileTime ? this->size() : int(IsRowMajor) ? this->cols() : this->rows(); + } + + /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are + * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and + * does nothing else. + */ + EIGEN_DEVICE_FUNC void resize(Index newSize) { + EIGEN_ONLY_USED_FOR_DEBUG(newSize); + eigen_assert(newSize == this->size() && "DenseBase::resize() does not actually allow to resize."); + } + /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are + * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and + * does nothing else. + */ + EIGEN_DEVICE_FUNC 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() && + "DenseBase::resize() does not actually allow to resize."); + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal Represents a matrix with all coefficients equal to one another*/ + typedef CwiseNullaryOp, PlainObject> ConstantReturnType; + /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */ + EIGEN_DEPRECATED typedef CwiseNullaryOp, PlainObject> SequentialLinSpacedReturnType; + /** \internal Represents a vector with linearly spaced coefficients that allows random access. */ + typedef CwiseNullaryOp, PlainObject> RandomAccessLinSpacedReturnType; + /** \internal Represents a vector with equally spaced coefficients that allows random access. */ + typedef CwiseNullaryOp, PlainObject> RandomAccessEqualSpacedReturnType; + /** \internal the return type of MatrixBase::eigenvalues() */ + typedef Matrix::Scalar>::Real, + internal::traits::ColsAtCompileTime, 1> + EigenvaluesReturnType; + +#endif // not EIGEN_PARSED_BY_DOXYGEN + + /** Copies \a other into *this. \returns a reference to *this. */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& other); + + /** Special case of the template operator=, in order to prevent the compiler + * from generating a default operator= (issue hit with g++ 4.1) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& other); + + template + EIGEN_DEVICE_FUNC Derived& operator=(const EigenBase& other); + + template + EIGEN_DEVICE_FUNC Derived& operator+=(const EigenBase& other); + + template + EIGEN_DEVICE_FUNC Derived& operator-=(const EigenBase& other); + + template + EIGEN_DEVICE_FUNC Derived& operator=(const ReturnByValue& func); + + /** \internal + * Copies \a other into *this without evaluating other. \returns a reference to *this. */ + template + /** \deprecated */ + EIGEN_DEPRECATED EIGEN_DEVICE_FUNC Derived& lazyAssign(const DenseBase& other); + + EIGEN_DEVICE_FUNC CommaInitializer operator<<(const Scalar& s); + + template + /** \deprecated it now returns \c *this */ + EIGEN_DEPRECATED const Derived& flagged() const { + return derived(); + } + + template + EIGEN_DEVICE_FUNC CommaInitializer operator<<(const DenseBase& other); + + typedef Transpose TransposeReturnType; + EIGEN_DEVICE_FUNC TransposeReturnType transpose(); + typedef Transpose ConstTransposeReturnType; + EIGEN_DEVICE_FUNC const ConstTransposeReturnType transpose() const; + EIGEN_DEVICE_FUNC void transposeInPlace(); + + EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index rows, Index cols, const Scalar& value); + EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index size, const Scalar& value); + EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(const Scalar& value); + + EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, Index size, + const Scalar& low, + const Scalar& high); + EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, + const Scalar& low, + const Scalar& high); + + EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Index size, const Scalar& low, + const Scalar& high); + EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(const Scalar& low, const Scalar& high); + + EIGEN_DEVICE_FUNC static const RandomAccessEqualSpacedReturnType EqualSpaced(Index size, const Scalar& low, + const Scalar& step); + EIGEN_DEVICE_FUNC static const RandomAccessEqualSpacedReturnType EqualSpaced(const Scalar& low, const Scalar& step); + + template + EIGEN_DEVICE_FUNC static const CwiseNullaryOp NullaryExpr(Index rows, Index cols, + const CustomNullaryOp& func); + template + EIGEN_DEVICE_FUNC static const CwiseNullaryOp NullaryExpr(Index size, + const CustomNullaryOp& func); + template + EIGEN_DEVICE_FUNC static const CwiseNullaryOp NullaryExpr(const CustomNullaryOp& func); + + EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index rows, Index cols); + EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index size); + EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(); + EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index rows, Index cols); + EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index size); + EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(); + + EIGEN_DEVICE_FUNC void fill(const Scalar& value); + EIGEN_DEVICE_FUNC Derived& setConstant(const Scalar& value); + EIGEN_DEVICE_FUNC Derived& setLinSpaced(Index size, const Scalar& low, const Scalar& high); + EIGEN_DEVICE_FUNC Derived& setLinSpaced(const Scalar& low, const Scalar& high); + EIGEN_DEVICE_FUNC Derived& setEqualSpaced(Index size, const Scalar& low, const Scalar& step); + EIGEN_DEVICE_FUNC Derived& setEqualSpaced(const Scalar& low, const Scalar& step); + EIGEN_DEVICE_FUNC Derived& setZero(); + EIGEN_DEVICE_FUNC Derived& setOnes(); + EIGEN_DEVICE_FUNC Derived& setRandom(); + + template + EIGEN_DEVICE_FUNC bool isApprox(const DenseBase& other, + const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isMuchSmallerThan(const RealScalar& other, + const RealScalar& prec = NumTraits::dummy_precision()) const; + template + EIGEN_DEVICE_FUNC bool isMuchSmallerThan(const DenseBase& other, + const RealScalar& prec = NumTraits::dummy_precision()) const; + + EIGEN_DEVICE_FUNC bool isApproxToConstant(const Scalar& value, + const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isConstant(const Scalar& value, + const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isZero(const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isOnes(const RealScalar& prec = NumTraits::dummy_precision()) const; + + EIGEN_DEVICE_FUNC inline bool hasNaN() const; + EIGEN_DEVICE_FUNC inline bool allFinite() const; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const Scalar& other); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const Scalar& other); + + typedef internal::add_const_on_value_type_t::type> EvalReturnType; + /** \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. + * + * \warning Be careful with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page + * \endlink. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EvalReturnType eval() const { + // Even though MSVC does not honor strong inlining when the return type + // is a dynamic matrix, we desperately need strong inlining for fixed + // size types on MSVC. + return typename internal::eval::type(derived()); + } + + /** swaps *this with the expression \a other. + * + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(const DenseBase& other) { + EIGEN_STATIC_ASSERT(!OtherDerived::IsPlainObjectBase, THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + eigen_assert(rows() == other.rows() && cols() == other.cols()); + call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op()); + } + + /** swaps *this with the matrix or array \a other. + * + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(PlainObjectBase& other) { + eigen_assert(rows() == other.rows() && cols() == other.cols()); + call_assignment(derived(), other.derived(), internal::swap_assign_op()); + } + + EIGEN_DEVICE_FUNC inline const NestByValue nestByValue() const; + EIGEN_DEVICE_FUNC inline const ForceAlignedAccess forceAlignedAccess() const; + EIGEN_DEVICE_FUNC inline ForceAlignedAccess forceAlignedAccess(); + template + EIGEN_DEVICE_FUNC inline const std::conditional_t, Derived&> + forceAlignedAccessIf() const; + template + EIGEN_DEVICE_FUNC inline std::conditional_t, Derived&> forceAlignedAccessIf(); + + EIGEN_DEVICE_FUNC Scalar sum() const; + EIGEN_DEVICE_FUNC Scalar mean() const; + EIGEN_DEVICE_FUNC Scalar trace() const; + + EIGEN_DEVICE_FUNC Scalar prod() const; + + template + EIGEN_DEVICE_FUNC typename internal::traits::Scalar minCoeff() const; + template + EIGEN_DEVICE_FUNC typename internal::traits::Scalar maxCoeff() const; + + // By default, the fastest version with undefined NaN propagation semantics is + // used. + // TODO(rmlarsen): Replace with default template argument when we move to + // c++11 or beyond. + EIGEN_DEVICE_FUNC inline typename internal::traits::Scalar minCoeff() const { + return minCoeff(); + } + EIGEN_DEVICE_FUNC inline typename internal::traits::Scalar maxCoeff() const { + return maxCoeff(); + } + + template + EIGEN_DEVICE_FUNC typename internal::traits::Scalar minCoeff(IndexType* row, IndexType* col) const; + template + EIGEN_DEVICE_FUNC typename internal::traits::Scalar maxCoeff(IndexType* row, IndexType* col) const; + template + EIGEN_DEVICE_FUNC typename internal::traits::Scalar minCoeff(IndexType* index) const; + template + EIGEN_DEVICE_FUNC typename internal::traits::Scalar maxCoeff(IndexType* index) const; + + // TODO(rmlarsen): Replace these methods with a default template argument. + template + EIGEN_DEVICE_FUNC inline typename internal::traits::Scalar minCoeff(IndexType* row, IndexType* col) const { + return minCoeff(row, col); + } + template + EIGEN_DEVICE_FUNC inline typename internal::traits::Scalar maxCoeff(IndexType* row, IndexType* col) const { + return maxCoeff(row, col); + } + template + EIGEN_DEVICE_FUNC inline typename internal::traits::Scalar minCoeff(IndexType* index) const { + return minCoeff(index); + } + template + EIGEN_DEVICE_FUNC inline typename internal::traits::Scalar maxCoeff(IndexType* index) const { + return maxCoeff(index); + } + + template + EIGEN_DEVICE_FUNC Scalar redux(const BinaryOp& func) const; + + template + EIGEN_DEVICE_FUNC void visit(Visitor& func) const; + + /** \returns a WithFormat proxy object allowing to print a matrix the with given + * format \a fmt. + * + * See class IOFormat for some examples. + * + * \sa class IOFormat, class WithFormat + */ + inline const WithFormat format(const IOFormat& fmt) const { return WithFormat(derived(), fmt); } + + /** \returns the unique coefficient of a 1x1 expression */ + EIGEN_DEVICE_FUNC CoeffReturnType value() const { + EIGEN_STATIC_ASSERT_SIZE_1x1(Derived) eigen_assert(this->rows() == 1 && this->cols() == 1); + return derived().coeff(0, 0); + } + + EIGEN_DEVICE_FUNC bool all() const; + EIGEN_DEVICE_FUNC bool any() const; + EIGEN_DEVICE_FUNC Index count() const; + + typedef VectorwiseOp RowwiseReturnType; + typedef const VectorwiseOp ConstRowwiseReturnType; + typedef VectorwiseOp ColwiseReturnType; + typedef const VectorwiseOp ConstColwiseReturnType; + + /** \returns a VectorwiseOp wrapper of *this for broadcasting and partial reductions + * + * Example: \include MatrixBase_rowwise.cpp + * Output: \verbinclude MatrixBase_rowwise.out + * + * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting + */ + // Code moved here due to a CUDA compiler bug + EIGEN_DEVICE_FUNC inline ConstRowwiseReturnType rowwise() const { return ConstRowwiseReturnType(derived()); } + EIGEN_DEVICE_FUNC RowwiseReturnType rowwise(); + + /** \returns a VectorwiseOp wrapper of *this broadcasting and partial reductions + * + * Example: \include MatrixBase_colwise.cpp + * Output: \verbinclude MatrixBase_colwise.out + * + * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting + */ + EIGEN_DEVICE_FUNC inline ConstColwiseReturnType colwise() const { return ConstColwiseReturnType(derived()); } + EIGEN_DEVICE_FUNC ColwiseReturnType colwise(); + + typedef CwiseNullaryOp, PlainObject> RandomReturnType; + static const RandomReturnType Random(Index rows, Index cols); + static const RandomReturnType Random(Index size); + static const RandomReturnType Random(); + + template + inline EIGEN_DEVICE_FUNC + CwiseTernaryOp::Scalar, + typename DenseBase::Scalar, Scalar>, + ThenDerived, ElseDerived, Derived> + select(const DenseBase& thenMatrix, const DenseBase& elseMatrix) const; + + template + inline EIGEN_DEVICE_FUNC + CwiseTernaryOp::Scalar, + typename DenseBase::Scalar, Scalar>, + ThenDerived, typename DenseBase::ConstantReturnType, Derived> + select(const DenseBase& thenMatrix, const typename DenseBase::Scalar& elseScalar) const; + + template + inline EIGEN_DEVICE_FUNC + CwiseTernaryOp::Scalar, + typename DenseBase::Scalar, Scalar>, + typename DenseBase::ConstantReturnType, ElseDerived, Derived> + select(const typename DenseBase::Scalar& thenScalar, const DenseBase& elseMatrix) const; + + template + RealScalar lpNorm() const; + + template + EIGEN_DEVICE_FUNC const Replicate replicate() const; + /** + * \return an expression of the replication of \c *this + * + * Example: \include MatrixBase_replicate_int_int.cpp + * Output: \verbinclude MatrixBase_replicate_int_int.out + * + * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate + */ + // Code moved here due to a CUDA compiler bug + EIGEN_DEVICE_FUNC const Replicate replicate(Index rowFactor, Index colFactor) const { + return Replicate(derived(), rowFactor, colFactor); + } + + typedef Reverse ReverseReturnType; + typedef const Reverse ConstReverseReturnType; + EIGEN_DEVICE_FUNC ReverseReturnType reverse(); + /** This is the const version of reverse(). */ + // Code moved here due to a CUDA compiler bug + EIGEN_DEVICE_FUNC ConstReverseReturnType reverse() const { return ConstReverseReturnType(derived()); } + EIGEN_DEVICE_FUNC void reverseInPlace(); + +#ifdef EIGEN_PARSED_BY_DOXYGEN + /** STL-like RandomAccessIterator + * iterator type as returned by the begin() and end() methods. + */ + typedef random_access_iterator_type iterator; + /** This is the const version of iterator (aka read-only) */ + typedef random_access_iterator_type const_iterator; +#else + typedef std::conditional_t<(Flags & DirectAccessBit) == DirectAccessBit, + internal::pointer_based_stl_iterator, + internal::generic_randaccess_stl_iterator > + iterator_type; + + typedef std::conditional_t<(Flags & DirectAccessBit) == DirectAccessBit, + internal::pointer_based_stl_iterator, + internal::generic_randaccess_stl_iterator > + const_iterator_type; + + // Stl-style iterators are supported only for vectors. + + typedef std::conditional_t iterator; + + typedef std::conditional_t const_iterator; +#endif + + inline iterator begin(); + inline const_iterator begin() const; + inline const_iterator cbegin() const; + inline iterator end(); + inline const_iterator end() const; + inline const_iterator cend() const; + +#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::DenseBase +#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL +#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND) +#define EIGEN_DOC_UNARY_ADDONS(X, Y) +#include "../plugins/CommonCwiseUnaryOps.inc" +#include "../plugins/BlockMethods.inc" +#include "../plugins/IndexedViewMethods.inc" +#include "../plugins/ReshapedMethods.inc" +#ifdef EIGEN_DENSEBASE_PLUGIN +#include EIGEN_DENSEBASE_PLUGIN +#endif +#undef EIGEN_CURRENT_STORAGE_BASE_CLASS +#undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL +#undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF +#undef EIGEN_DOC_UNARY_ADDONS + + // disable the use of evalTo for dense objects with a nice compilation error + template + EIGEN_DEVICE_FUNC inline void evalTo(Dest&) const { + EIGEN_STATIC_ASSERT((internal::is_same::value), + THE_EVAL_EVALTO_FUNCTION_SHOULD_NEVER_BE_CALLED_FOR_DENSE_OBJECTS); + } + + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(DenseBase) + /** Default constructor. Do nothing. */ + EIGEN_DEVICE_FUNC constexpr DenseBase() { + /* Just checks for self-consistency of the flags. + * Only do it when debugging Eigen, as this borders on paranoia and could slow compilation down + */ +#ifdef EIGEN_INTERNAL_DEBUGGING + EIGEN_STATIC_ASSERT( + (internal::check_implication(MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1, int(IsRowMajor)) && + internal::check_implication(MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1, int(!IsRowMajor))), + INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION) +#endif + } + + private: + EIGEN_DEVICE_FUNC explicit DenseBase(int); + EIGEN_DEVICE_FUNC DenseBase(int, int); + template + EIGEN_DEVICE_FUNC explicit DenseBase(const DenseBase&); +}; + +} // end namespace Eigen + +#endif // EIGEN_DENSEBASE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h new file mode 100644 index 00000000..48c6d730 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h @@ -0,0 +1,568 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2010 Benoit Jacob +// +// 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_DENSECOEFFSBASE_H +#define EIGEN_DENSECOEFFSBASE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct add_const_on_value_type_if_arithmetic { + typedef std::conditional_t::value, T, add_const_on_value_type_t> type; +}; +} // namespace internal + +/** \brief Base class providing read-only coefficient access to matrices and arrays. + * \ingroup Core_Module + * \tparam Derived Type of the derived class + * + * \note #ReadOnlyAccessors Constant indicating read-only access + * + * This class defines the \c operator() \c const function and friends, which can be used to read specific + * entries of a matrix or array. + * + * \sa DenseCoeffsBase, DenseCoeffsBase, + * \ref TopicClassHierarchy + */ +template +class DenseCoeffsBase : public EigenBase { + public: + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::packet_traits::type PacketScalar; + + // Explanation for this CoeffReturnType typedef. + // - This is the return type of the coeff() method. + // - The LvalueBit means exactly that we can offer a coeffRef() method, which means exactly that we can get references + // to coeffs, which means exactly that we can have coeff() return a const reference (as opposed to returning a value). + // - The is_arithmetic check is required since "const int", "const double", etc. will cause warnings on some systems + // while the declaration of "const T", where T is a non arithmetic type does not. Always returning "const Scalar&" is + // not possible, since the underlying expressions might not offer a valid address the reference could be referring to. + typedef std::conditional_t::Flags& LvalueBit), const Scalar&, + std::conditional_t::value, Scalar, const Scalar>> + CoeffReturnType; + + typedef typename internal::add_const_on_value_type_if_arithmetic::type>::type + PacketReturnType; + + typedef EigenBase Base; + using Base::cols; + using Base::derived; + using Base::rows; + using Base::size; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) const { + return int(Derived::RowsAtCompileTime) == 1 ? 0 + : int(Derived::ColsAtCompileTime) == 1 ? inner + : int(Derived::Flags) & RowMajorBit ? outer + : inner; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) const { + return int(Derived::ColsAtCompileTime) == 1 ? 0 + : int(Derived::RowsAtCompileTime) == 1 ? inner + : int(Derived::Flags) & RowMajorBit ? inner + : outer; + } + + /** Short version: don't use this function, use + * \link operator()(Index,Index) const \endlink instead. + * + * Long version: this function is similar to + * \link operator()(Index,Index) const \endlink, but without the assertion. + * Use this for limiting the performance cost of debugging code when doing + * repeated coefficient access. Only use this when it is guaranteed that the + * parameters \a row and \a col are in range. + * + * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this + * function equivalent to \link operator()(Index,Index) const \endlink. + * + * \sa operator()(Index,Index) const, coeffRef(Index,Index), coeff(Index) const + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return internal::evaluator(derived()).coeff(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeffByOuterInner(Index outer, Index inner) const { + return coeff(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner)); + } + + /** \returns the coefficient at given the given row and column. + * + * \sa operator()(Index,Index), operator[](Index) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator()(Index row, Index col) const { + eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return coeff(row, col); + } + + /** Short version: don't use this function, use + * \link operator[](Index) const \endlink instead. + * + * Long version: this function is similar to + * \link operator[](Index) const \endlink, but without the assertion. + * Use this for limiting the performance cost of debugging code when doing + * repeated coefficient access. Only use this when it is guaranteed that the + * parameter \a index is in range. + * + * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this + * function equivalent to \link operator[](Index) const \endlink. + * + * \sa operator[](Index) const, coeffRef(Index), coeff(Index,Index) const + */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + EIGEN_STATIC_ASSERT(internal::evaluator::Flags & LinearAccessBit, + THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS) + eigen_internal_assert(index >= 0 && index < size()); + return internal::evaluator(derived()).coeff(index); + } + + /** \returns the coefficient at given index. + * + * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit. + * + * \sa operator[](Index), operator()(Index,Index) const, x() const, y() const, + * z() const, w() const + */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator[](Index index) const { + EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime, + THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD) + eigen_assert(index >= 0 && index < size()); + return coeff(index); + } + + /** \returns the coefficient at given index. + * + * This is synonymous to operator[](Index) const. + * + * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit. + * + * \sa operator[](Index), operator()(Index,Index) const, x() const, y() const, + * z() const, w() const + */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator()(Index index) const { + eigen_assert(index >= 0 && index < size()); + return coeff(index); + } + + /** equivalent to operator[](0). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType x() const { return (*this)[0]; } + + /** equivalent to operator[](1). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType y() const { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 2, OUT_OF_RANGE_ACCESS); + return (*this)[1]; + } + + /** equivalent to operator[](2). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType z() const { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 3, OUT_OF_RANGE_ACCESS); + return (*this)[2]; + } + + /** equivalent to operator[](3). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType w() const { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 4, OUT_OF_RANGE_ACCESS); + return (*this)[3]; + } + + /** \internal + * \returns the packet of coefficients starting at the given row and column. It is your responsibility + * to ensure that a packet really starts there. This method is only available on expressions having the + * PacketAccessBit. + * + * The \a LoadMode parameter may have the value \a #Aligned or \a #Unaligned. Its effect is to select + * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets + * starting at an address which is a multiple of the packet size. + */ + + template + EIGEN_STRONG_INLINE PacketReturnType packet(Index row, Index col) const { + typedef typename internal::packet_traits::type DefaultPacketType; + eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return internal::evaluator(derived()).template packet(row, col); + } + + /** \internal */ + template + EIGEN_STRONG_INLINE PacketReturnType packetByOuterInner(Index outer, Index inner) const { + return packet(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner)); + } + + /** \internal + * \returns the packet of coefficients starting at the given index. It is your responsibility + * to ensure that a packet really starts there. This method is only available on expressions having the + * PacketAccessBit and the LinearAccessBit. + * + * The \a LoadMode parameter may have the value \a #Aligned or \a #Unaligned. Its effect is to select + * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets + * starting at an address which is a multiple of the packet size. + */ + + template + EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const { + EIGEN_STATIC_ASSERT(internal::evaluator::Flags & LinearAccessBit, + THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS) + typedef typename internal::packet_traits::type DefaultPacketType; + eigen_internal_assert(index >= 0 && index < size()); + return internal::evaluator(derived()).template packet(index); + } + + protected: + // explanation: DenseBase is doing "using ..." on the methods from DenseCoeffsBase. + // But some methods are only available in the DirectAccess case. + // So we add dummy methods here with these names, so that "using... " doesn't fail. + // It's not private so that the child class DenseBase can access them, and it's not public + // either since it's an implementation detail, so has to be protected. + void coeffRef(); + void coeffRefByOuterInner(); + void writePacket(); + void writePacketByOuterInner(); + void copyCoeff(); + void copyCoeffByOuterInner(); + void copyPacket(); + void copyPacketByOuterInner(); + void stride(); + void innerStride(); + void outerStride(); + void rowStride(); + void colStride(); +}; + +/** \brief Base class providing read/write coefficient access to matrices and arrays. + * \ingroup Core_Module + * \tparam Derived Type of the derived class + * + * \note #WriteAccessors Constant indicating read/write access + * + * This class defines the non-const \c operator() function and friends, which can be used to write specific + * entries of a matrix or array. This class inherits DenseCoeffsBase which + * defines the const variant for reading specific entries. + * + * \sa DenseCoeffsBase, \ref TopicClassHierarchy + */ +template +class DenseCoeffsBase : public DenseCoeffsBase { + public: + typedef DenseCoeffsBase Base; + + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::packet_traits::type PacketScalar; + typedef typename NumTraits::Real RealScalar; + + using Base::coeff; + using Base::colIndexByOuterInner; + using Base::cols; + using Base::derived; + using Base::rowIndexByOuterInner; + using Base::rows; + using Base::size; + using Base::operator[]; + using Base::operator(); + using Base::w; + using Base::x; + using Base::y; + using Base::z; + + /** Short version: don't use this function, use + * \link operator()(Index,Index) \endlink instead. + * + * Long version: this function is similar to + * \link operator()(Index,Index) \endlink, but without the assertion. + * Use this for limiting the performance cost of debugging code when doing + * repeated coefficient access. Only use this when it is guaranteed that the + * parameters \a row and \a col are in range. + * + * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this + * function equivalent to \link operator()(Index,Index) \endlink. + * + * \sa operator()(Index,Index), coeff(Index, Index) const, coeffRef(Index) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { + eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return internal::evaluator(derived()).coeffRef(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRefByOuterInner(Index outer, Index inner) { + return coeffRef(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner)); + } + + /** \returns a reference to the coefficient at given the given row and column. + * + * \sa operator[](Index) + */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(Index row, Index col) { + eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return coeffRef(row, col); + } + + /** Short version: don't use this function, use + * \link operator[](Index) \endlink instead. + * + * Long version: this function is similar to + * \link operator[](Index) \endlink, but without the assertion. + * Use this for limiting the performance cost of debugging code when doing + * repeated coefficient access. Only use this when it is guaranteed that the + * parameters \a row and \a col are in range. + * + * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this + * function equivalent to \link operator[](Index) \endlink. + * + * \sa operator[](Index), coeff(Index) const, coeffRef(Index,Index) + */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { + EIGEN_STATIC_ASSERT(internal::evaluator::Flags & LinearAccessBit, + THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS) + eigen_internal_assert(index >= 0 && index < size()); + return internal::evaluator(derived()).coeffRef(index); + } + + /** \returns a reference to the coefficient at given index. + * + * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit. + * + * \sa operator[](Index) const, operator()(Index,Index), x(), y(), z(), w() + */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator[](Index index) { + EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime, + THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD) + eigen_assert(index >= 0 && index < size()); + return coeffRef(index); + } + + /** \returns a reference to the coefficient at given index. + * + * This is synonymous to operator[](Index). + * + * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit. + * + * \sa operator[](Index) const, operator()(Index,Index), x(), y(), z(), w() + */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(Index index) { + eigen_assert(index >= 0 && index < size()); + return coeffRef(index); + } + + /** equivalent to operator[](0). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& x() { return (*this)[0]; } + + /** equivalent to operator[](1). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& y() { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 2, OUT_OF_RANGE_ACCESS); + return (*this)[1]; + } + + /** equivalent to operator[](2). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& z() { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 3, OUT_OF_RANGE_ACCESS); + return (*this)[2]; + } + + /** equivalent to operator[](3). */ + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& w() { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 4, OUT_OF_RANGE_ACCESS); + return (*this)[3]; + } +}; + +/** \brief Base class providing direct read-only coefficient access to matrices and arrays. + * \ingroup Core_Module + * \tparam Derived Type of the derived class + * + * \note #DirectAccessors Constant indicating direct access + * + * This class defines functions to work with strides which can be used to access entries directly. This class + * inherits DenseCoeffsBase which defines functions to access entries read-only using + * \c operator() . + * + * \sa \blank \ref TopicClassHierarchy + */ +template +class DenseCoeffsBase : public DenseCoeffsBase { + public: + typedef DenseCoeffsBase Base; + typedef typename internal::traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + + using Base::cols; + using Base::derived; + using Base::rows; + using Base::size; + + /** \returns the pointer increment between two consecutive elements within a slice in the inner direction. + * + * \sa outerStride(), rowStride(), colStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { return derived().innerStride(); } + + /** \returns the pointer increment between two consecutive inner slices (for example, between two consecutive columns + * in a column-major matrix). + * + * \sa innerStride(), rowStride(), colStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { return derived().outerStride(); } + + // FIXME shall we remove it ? + EIGEN_CONSTEXPR inline Index stride() const { return Derived::IsVectorAtCompileTime ? innerStride() : outerStride(); } + + /** \returns the pointer increment between two consecutive rows. + * + * \sa innerStride(), outerStride(), colStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rowStride() const { + return Derived::IsRowMajor ? outerStride() : innerStride(); + } + + /** \returns the pointer increment between two consecutive columns. + * + * \sa innerStride(), outerStride(), rowStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index colStride() const { + return Derived::IsRowMajor ? innerStride() : outerStride(); + } +}; + +/** \brief Base class providing direct read/write coefficient access to matrices and arrays. + * \ingroup Core_Module + * \tparam Derived Type of the derived class + * + * \note #DirectWriteAccessors Constant indicating direct access + * + * This class defines functions to work with strides which can be used to access entries directly. This class + * inherits DenseCoeffsBase which defines functions to access entries read/write using + * \c operator(). + * + * \sa \blank \ref TopicClassHierarchy + */ +template +class DenseCoeffsBase : public DenseCoeffsBase { + public: + typedef DenseCoeffsBase Base; + typedef typename internal::traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + + using Base::cols; + using Base::derived; + using Base::rows; + using Base::size; + + /** \returns the pointer increment between two consecutive elements within a slice in the inner direction. + * + * \sa outerStride(), rowStride(), colStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return derived().innerStride(); } + + /** \returns the pointer increment between two consecutive inner slices (for example, between two consecutive columns + * in a column-major matrix). + * + * \sa innerStride(), rowStride(), colStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return derived().outerStride(); } + + // FIXME shall we remove it ? + EIGEN_CONSTEXPR inline Index stride() const EIGEN_NOEXCEPT { + return Derived::IsVectorAtCompileTime ? innerStride() : outerStride(); + } + + /** \returns the pointer increment between two consecutive rows. + * + * \sa innerStride(), outerStride(), colStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rowStride() const EIGEN_NOEXCEPT { + return Derived::IsRowMajor ? outerStride() : innerStride(); + } + + /** \returns the pointer increment between two consecutive columns. + * + * \sa innerStride(), outerStride(), rowStride() + */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index colStride() const EIGEN_NOEXCEPT { + return Derived::IsRowMajor ? innerStride() : outerStride(); + } +}; + +namespace internal { + +template +struct first_aligned_impl { + static EIGEN_CONSTEXPR inline Index run(const Derived&) EIGEN_NOEXCEPT { return 0; } +}; + +template +struct first_aligned_impl { + static inline Index run(const Derived& m) { return internal::first_aligned(m.data(), m.size()); } +}; + +/** \internal \returns the index of the first element of the array stored by \a m that is properly aligned with respect + * to \a Alignment for vectorization. + * + * \tparam Alignment requested alignment in Bytes. + * + * There is also the variant first_aligned(const Scalar*, Integer) defined in Memory.h. See it for more + * documentation. + */ +template +static inline Index first_aligned(const DenseBase& m) { + enum { ReturnZero = (int(evaluator::Alignment) >= Alignment) || !(Derived::Flags & DirectAccessBit) }; + return first_aligned_impl::run(m.derived()); +} + +template +static inline Index first_default_aligned(const DenseBase& m) { + typedef typename Derived::Scalar Scalar; + typedef typename packet_traits::type DefaultPacketType; + return internal::first_aligned::alignment), Derived>(m); +} + +template ::ret> +struct inner_stride_at_compile_time { + enum { ret = traits::InnerStrideAtCompileTime }; +}; + +template +struct inner_stride_at_compile_time { + enum { ret = 0 }; +}; + +template ::ret> +struct outer_stride_at_compile_time { + enum { ret = traits::OuterStrideAtCompileTime }; +}; + +template +struct outer_stride_at_compile_time { + enum { ret = 0 }; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_DENSECOEFFSBASE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h new file mode 100644 index 00000000..f6169398 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h @@ -0,0 +1,650 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2006-2009 Benoit Jacob +// Copyright (C) 2010-2013 Hauke Heibel +// +// 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_MATRIXSTORAGE_H +#define EIGEN_MATRIXSTORAGE_H + +#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN +#define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X) \ + X; \ + EIGEN_DENSE_STORAGE_CTOR_PLUGIN; +#else +#define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X) +#endif + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +struct constructor_without_unaligned_array_assert {}; + +template +EIGEN_DEVICE_FUNC constexpr void check_static_allocation_size() { +// if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit +#if EIGEN_STACK_ALLOCATION_LIMIT + EIGEN_STATIC_ASSERT(Size * sizeof(T) <= EIGEN_STACK_ALLOCATION_LIMIT, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); +#endif +} + +/** \internal + * Static array. If the MatrixOrArrayOptions require auto-alignment, the array will be automatically aligned: + * to 16 bytes boundary if the total size is a multiple of 16 bytes. + */ +template ::value> +struct plain_array { + T array[Size]; + + EIGEN_DEVICE_FUNC constexpr plain_array() { check_static_allocation_size(); } + + EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) { + check_static_allocation_size(); + } +}; + +#if defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) +#define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) +#else +#define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \ + eigen_assert((internal::is_constant_evaluated() || (std::uintptr_t(array) & (sizemask)) == 0) && \ + "this assertion is explained here: " \ + "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \ + " **** READ THIS WEB PAGE !!! ****"); +#endif + +template +struct plain_array { + EIGEN_ALIGN_TO_BOUNDARY(8) T array[Size]; + + EIGEN_DEVICE_FUNC constexpr plain_array() { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(7); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) { + check_static_allocation_size(); + } +}; + +template +struct plain_array { + EIGEN_ALIGN_TO_BOUNDARY(16) T array[Size]; + + EIGEN_DEVICE_FUNC constexpr plain_array() { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(15); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) { + check_static_allocation_size(); + } +}; + +template +struct plain_array { + EIGEN_ALIGN_TO_BOUNDARY(32) T array[Size]; + + EIGEN_DEVICE_FUNC constexpr plain_array() { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(31); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) { + check_static_allocation_size(); + } +}; + +template +struct plain_array { + EIGEN_ALIGN_TO_BOUNDARY(64) T array[Size]; + + EIGEN_DEVICE_FUNC constexpr plain_array() { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(63); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) { + check_static_allocation_size(); + } +}; + +template +struct plain_array { + T array[1]; + EIGEN_DEVICE_FUNC constexpr plain_array() {} + EIGEN_DEVICE_FUNC constexpr plain_array(constructor_without_unaligned_array_assert) {} +}; + +struct plain_array_helper { + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static void copy( + const plain_array& src, const Eigen::Index size, + plain_array& dst) { + smart_copy(src.array, src.array + size, dst.array); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static void swap(plain_array& a, + const Eigen::Index a_size, + plain_array& b, + const Eigen::Index b_size) { + if (a_size < b_size) { + std::swap_ranges(b.array, b.array + a_size, a.array); + smart_move(b.array + a_size, b.array + b_size, a.array + a_size); + } else if (a_size > b_size) { + std::swap_ranges(a.array, a.array + b_size, b.array); + smart_move(a.array + b_size, a.array + a_size, b.array + b_size); + } else { + std::swap_ranges(a.array, a.array + a_size, b.array); + } + } +}; + +} // end namespace internal + +/** \internal + * + * \class DenseStorage + * \ingroup Core_Module + * + * \brief Stores the data of a matrix + * + * This class stores the data of fixed-size, dynamic-size or mixed matrices + * in a way as compact as possible. + * + * \sa Matrix + */ +template +class DenseStorage; + +// purely fixed-size matrix +template +class DenseStorage { + internal::plain_array m_data; + + public: + constexpr EIGEN_DEVICE_FUNC DenseStorage(){EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN( + Index size = + Size)} EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()) {} +#if defined(EIGEN_DENSE_STORAGE_CTOR_PLUGIN) + EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other) + : m_data(other.m_data){EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size)} +#else + EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage&) = default; +#endif + EIGEN_DEVICE_FUNC constexpr DenseStorage + & + operator=(const DenseStorage&) = default; + EIGEN_DEVICE_FUNC constexpr DenseStorage(DenseStorage&&) = default; + EIGEN_DEVICE_FUNC constexpr DenseStorage& operator=(DenseStorage&&) = default; + EIGEN_DEVICE_FUNC constexpr DenseStorage(Index size, Index rows, Index cols) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size == rows * cols && rows == Rows_ && cols == Cols_); + EIGEN_UNUSED_VARIABLE(size); + EIGEN_UNUSED_VARIABLE(rows); + EIGEN_UNUSED_VARIABLE(cols); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { numext::swap(m_data, other.m_data); } + EIGEN_DEVICE_FUNC static constexpr Index rows(void) EIGEN_NOEXCEPT { return Rows_; } + EIGEN_DEVICE_FUNC static constexpr Index cols(void) EIGEN_NOEXCEPT { return Cols_; } + EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index, Index) {} + EIGEN_DEVICE_FUNC constexpr void resize(Index, Index, Index) {} + EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; } + EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; } +}; + +// null matrix +template +class DenseStorage { + public: + static_assert(Rows_ * Cols_ == 0, "The fixed number of rows times columns must equal the storage size."); + EIGEN_DEVICE_FUNC constexpr DenseStorage() {} + EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) {} + EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage&) {} + EIGEN_DEVICE_FUNC constexpr DenseStorage& operator=(const DenseStorage&) { return *this; } + EIGEN_DEVICE_FUNC constexpr DenseStorage(Index, Index, Index) {} + EIGEN_DEVICE_FUNC constexpr void swap(DenseStorage&) {} + EIGEN_DEVICE_FUNC static constexpr Index rows(void) EIGEN_NOEXCEPT { return Rows_; } + EIGEN_DEVICE_FUNC static constexpr Index cols(void) EIGEN_NOEXCEPT { return Cols_; } + EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index, Index) {} + EIGEN_DEVICE_FUNC constexpr void resize(Index, Index, Index) {} + EIGEN_DEVICE_FUNC constexpr const T* data() const { return 0; } + EIGEN_DEVICE_FUNC constexpr T* data() { return 0; } +}; + +// more specializations for null matrices; these are necessary to resolve ambiguities +template +class DenseStorage { + Index m_rows; + Index m_cols; + + public: + EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : DenseStorage() {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_rows(other.m_rows), m_cols(other.m_cols) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + m_rows = other.m_rows; + m_cols = other.m_cols; + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index cols) : m_rows(rows), m_cols(cols) { + eigen_assert(m_rows * m_cols == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { + numext::swap(m_rows, other.m_rows); + numext::swap(m_cols, other.m_cols); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_rows; } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_cols; } + EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index cols) { + m_rows = rows; + m_cols = cols; + eigen_assert(m_rows * m_cols == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index cols) { + m_rows = rows; + m_cols = cols; + eigen_assert(m_rows * m_cols == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC const T* data() const { return nullptr; } + EIGEN_DEVICE_FUNC T* data() { return nullptr; } +}; + +template +class DenseStorage { + Index m_cols; + + public: + EIGEN_DEVICE_FUNC DenseStorage() : m_cols(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : DenseStorage() {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_cols(other.m_cols) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + m_cols = other.m_cols; + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(Index, Index, Index cols) : m_cols(cols) { + eigen_assert(Rows_ * m_cols == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { numext::swap(m_cols, other.m_cols); } + EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT { return Rows_; } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols(void) const EIGEN_NOEXCEPT { return m_cols; } + EIGEN_DEVICE_FUNC void conservativeResize(Index, Index, Index cols) { + m_cols = cols; + eigen_assert(Rows_ * m_cols == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC void resize(Index, Index, Index cols) { + m_cols = cols; + eigen_assert(Rows_ * m_cols == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC const T* data() const { return nullptr; } + EIGEN_DEVICE_FUNC T* data() { return nullptr; } +}; + +template +class DenseStorage { + Index m_rows; + + public: + EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : DenseStorage() {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_rows(other.m_rows) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + m_rows = other.m_rows; + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index) : m_rows(rows) { + eigen_assert(m_rows * Cols_ == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { numext::swap(m_rows, other.m_rows); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows(void) const EIGEN_NOEXCEPT { return m_rows; } + EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) EIGEN_NOEXCEPT { return Cols_; } + EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index) { + m_rows = rows; + eigen_assert(m_rows * Cols_ == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index) { + m_rows = rows; + eigen_assert(m_rows * Cols_ == 0 && "The number of rows times columns must equal the storage size."); + } + EIGEN_DEVICE_FUNC const T* data() const { return nullptr; } + EIGEN_DEVICE_FUNC T* data() { return nullptr; } +}; + +// dynamic-size matrix with fixed-size storage +template +class DenseStorage { + internal::plain_array m_data; + Index m_rows; + Index m_cols; + + public: + EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(), m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other) + : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows), m_cols(other.m_cols) { + internal::plain_array_helper::copy(other.m_data, m_rows * m_cols, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + if (this != &other) { + m_rows = other.m_rows; + m_cols = other.m_cols; + internal::plain_array_helper::copy(other.m_data, m_rows * m_cols, m_data); + } + return *this; + } + EIGEN_DEVICE_FUNC constexpr DenseStorage(Index, Index rows, Index cols) : m_rows(rows), m_cols(cols) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { + internal::plain_array_helper::swap(m_data, m_rows * m_cols, other.m_data, other.m_rows * other.m_cols); + numext::swap(m_rows, other.m_rows); + numext::swap(m_cols, other.m_cols); + } + EIGEN_DEVICE_FUNC constexpr Index rows() const { return m_rows; } + EIGEN_DEVICE_FUNC constexpr Index cols() const { return m_cols; } + EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index rows, Index cols) { + m_rows = rows; + m_cols = cols; + } + EIGEN_DEVICE_FUNC constexpr void resize(Index, Index rows, Index cols) { + m_rows = rows; + m_cols = cols; + } + EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; } + EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; } +}; + +// dynamic-size matrix with fixed-size storage and fixed width +template +class DenseStorage { + internal::plain_array m_data; + Index m_rows; + + public: + EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_rows(0) {} + EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {} + EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other) + : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows) { + internal::plain_array_helper::copy(other.m_data, m_rows * Cols_, m_data); + } + + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + if (this != &other) { + m_rows = other.m_rows; + internal::plain_array_helper::copy(other.m_data, m_rows * Cols_, m_data); + } + return *this; + } + EIGEN_DEVICE_FUNC constexpr DenseStorage(Index, Index rows, Index) : m_rows(rows) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { + internal::plain_array_helper::swap(m_data, m_rows * Cols_, other.m_data, other.m_rows * Cols_); + numext::swap(m_rows, other.m_rows); + } + EIGEN_DEVICE_FUNC constexpr Index rows(void) const EIGEN_NOEXCEPT { return m_rows; } + EIGEN_DEVICE_FUNC constexpr Index cols(void) const EIGEN_NOEXCEPT { return Cols_; } + EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index rows, Index) { m_rows = rows; } + EIGEN_DEVICE_FUNC constexpr void resize(Index, Index rows, Index) { m_rows = rows; } + EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; } + EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; } +}; + +// dynamic-size matrix with fixed-size storage and fixed height +template +class DenseStorage { + internal::plain_array m_data; + Index m_cols; + + public: + EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_cols(0) {} + EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {} + EIGEN_DEVICE_FUNC constexpr DenseStorage(const DenseStorage& other) + : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(other.m_cols) { + internal::plain_array_helper::copy(other.m_data, Rows_ * m_cols, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + if (this != &other) { + m_cols = other.m_cols; + internal::plain_array_helper::copy(other.m_data, Rows_ * m_cols, m_data); + } + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(Index, Index, Index cols) : m_cols(cols) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { + internal::plain_array_helper::swap(m_data, Rows_ * m_cols, other.m_data, Rows_ * other.m_cols); + numext::swap(m_cols, other.m_cols); + } + EIGEN_DEVICE_FUNC constexpr Index rows(void) const EIGEN_NOEXCEPT { return Rows_; } + EIGEN_DEVICE_FUNC constexpr Index cols(void) const EIGEN_NOEXCEPT { return m_cols; } + EIGEN_DEVICE_FUNC constexpr void conservativeResize(Index, Index, Index cols) { m_cols = cols; } + EIGEN_DEVICE_FUNC constexpr void resize(Index, Index, Index cols) { m_cols = cols; } + EIGEN_DEVICE_FUNC constexpr const T* data() const { return m_data.array; } + EIGEN_DEVICE_FUNC constexpr T* data() { return m_data.array; } +}; + +// purely dynamic matrix. +template +class DenseStorage { + T* m_data; + Index m_rows; + Index m_cols; + + public: + EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(0), m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(0), m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) + : m_data(internal::conditional_aligned_new_auto(size)), + m_rows(rows), + m_cols(cols) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size == rows * cols && rows >= 0 && cols >= 0); + } + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) + : m_data(internal::conditional_aligned_new_auto(other.m_rows * other.m_cols)), + m_rows(other.m_rows), + m_cols(other.m_cols) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows * m_cols) + internal::smart_copy(other.m_data, other.m_data + other.m_rows * other.m_cols, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + if (this != &other) { + DenseStorage tmp(other); + this->swap(tmp); + } + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)), + m_rows(std::move(other.m_rows)), + m_cols(std::move(other.m_cols)) { + other.m_data = nullptr; + other.m_rows = 0; + other.m_cols = 0; + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT { + numext::swap(m_data, other.m_data); + numext::swap(m_rows, other.m_rows); + numext::swap(m_cols, other.m_cols); + return *this; + } + EIGEN_DEVICE_FUNC ~DenseStorage() { + internal::conditional_aligned_delete_auto(m_data, m_rows * m_cols); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { + numext::swap(m_data, other.m_data); + numext::swap(m_rows, other.m_rows); + numext::swap(m_cols, other.m_cols); + } + EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT { return m_rows; } + EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT { return m_cols; } + void conservativeResize(Index size, Index rows, Index cols) { + m_data = + internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows * m_cols); + m_rows = rows; + m_cols = cols; + } + EIGEN_DEVICE_FUNC void resize(Index size, Index rows, Index cols) { + if (size != m_rows * m_cols) { + internal::conditional_aligned_delete_auto(m_data, m_rows * m_cols); + if (size > 0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative + m_data = internal::conditional_aligned_new_auto(size); + else + m_data = 0; + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + } + m_rows = rows; + m_cols = cols; + } + EIGEN_DEVICE_FUNC const T* data() const { return m_data; } + EIGEN_DEVICE_FUNC T* data() { return m_data; } +}; + +// matrix with dynamic width and fixed height (so that matrix has dynamic size). +template +class DenseStorage { + T* m_data; + Index m_cols; + + public: + EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(0), m_cols(0) {} + explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {} + EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) + : m_data(internal::conditional_aligned_new_auto(size)), m_cols(cols) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size == rows * cols && rows == Rows_ && cols >= 0); + EIGEN_UNUSED_VARIABLE(rows); + } + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) + : m_data(internal::conditional_aligned_new_auto(Rows_ * other.m_cols)), + m_cols(other.m_cols) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_cols * Rows_) + internal::smart_copy(other.m_data, other.m_data + Rows_ * m_cols, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + if (this != &other) { + DenseStorage tmp(other); + this->swap(tmp); + } + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)), + m_cols(std::move(other.m_cols)) { + other.m_data = nullptr; + other.m_cols = 0; + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT { + numext::swap(m_data, other.m_data); + numext::swap(m_cols, other.m_cols); + return *this; + } + EIGEN_DEVICE_FUNC ~DenseStorage() { + internal::conditional_aligned_delete_auto(m_data, Rows_ * m_cols); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { + numext::swap(m_data, other.m_data); + numext::swap(m_cols, other.m_cols); + } + EIGEN_DEVICE_FUNC static constexpr Index rows(void) EIGEN_NOEXCEPT { return Rows_; } + EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT { return m_cols; } + EIGEN_DEVICE_FUNC void conservativeResize(Index size, Index, Index cols) { + m_data = + internal::conditional_aligned_realloc_new_auto(m_data, size, Rows_ * m_cols); + m_cols = cols; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index, Index cols) { + if (size != Rows_ * m_cols) { + internal::conditional_aligned_delete_auto(m_data, Rows_ * m_cols); + if (size > 0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative + m_data = internal::conditional_aligned_new_auto(size); + else + m_data = 0; + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + } + m_cols = cols; + } + EIGEN_DEVICE_FUNC const T* data() const { return m_data; } + EIGEN_DEVICE_FUNC T* data() { return m_data; } +}; + +// matrix with dynamic height and fixed width (so that matrix has dynamic size). +template +class DenseStorage { + T* m_data; + Index m_rows; + + public: + EIGEN_DEVICE_FUNC constexpr DenseStorage() : m_data(0), m_rows(0) {} + explicit constexpr DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {} + EIGEN_DEVICE_FUNC constexpr DenseStorage(Index size, Index rows, Index cols) + : m_data(internal::conditional_aligned_new_auto(size)), m_rows(rows) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size == rows * cols && rows >= 0 && cols == Cols_); + EIGEN_UNUSED_VARIABLE(cols); + } + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) + : m_data(internal::conditional_aligned_new_auto(other.m_rows * Cols_)), + m_rows(other.m_rows) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows * Cols_) + internal::smart_copy(other.m_data, other.m_data + other.m_rows * Cols_, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { + if (this != &other) { + DenseStorage tmp(other); + this->swap(tmp); + } + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)), + m_rows(std::move(other.m_rows)) { + other.m_data = nullptr; + other.m_rows = 0; + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT { + numext::swap(m_data, other.m_data); + numext::swap(m_rows, other.m_rows); + return *this; + } + EIGEN_DEVICE_FUNC ~DenseStorage() { + internal::conditional_aligned_delete_auto(m_data, Cols_ * m_rows); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { + numext::swap(m_data, other.m_data); + numext::swap(m_rows, other.m_rows); + } + EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT { return m_rows; } + EIGEN_DEVICE_FUNC static constexpr Index cols(void) { return Cols_; } + void conservativeResize(Index size, Index rows, Index) { + m_data = + internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows * Cols_); + m_rows = rows; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index rows, Index) { + if (size != m_rows * Cols_) { + internal::conditional_aligned_delete_auto(m_data, Cols_ * m_rows); + if (size > 0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative + m_data = internal::conditional_aligned_new_auto(size); + else + m_data = 0; + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + } + m_rows = rows; + } + EIGEN_DEVICE_FUNC const T* data() const { return m_data; } + EIGEN_DEVICE_FUNC T* data() { return m_data; } +}; + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h new file mode 100644 index 00000000..8d27857e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h @@ -0,0 +1,221 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2007-2009 Benoit Jacob +// Copyright (C) 2009-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/. + +#ifndef EIGEN_DIAGONAL_H +#define EIGEN_DIAGONAL_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class Diagonal + * \ingroup Core_Module + * + * \brief Expression of a diagonal/subdiagonal/superdiagonal in a matrix + * + * \tparam MatrixType the type of the object in which we are taking a sub/main/super diagonal + * \tparam DiagIndex the index of the sub/super diagonal. The default is 0 and it means the main diagonal. + * A positive value means a superdiagonal, a negative value means a subdiagonal. + * You can also use DynamicIndex so the index can be set at runtime. + * + * The matrix is not required to be square. + * + * This class represents an expression of the main diagonal, or any sub/super diagonal + * of a square matrix. It is the return type of MatrixBase::diagonal() and MatrixBase::diagonal(Index) and most of the + * time this is the only way it is used. + * + * \sa MatrixBase::diagonal(), MatrixBase::diagonal(Index) + */ + +namespace internal { +template +struct traits > : traits { + typedef typename ref_selector::type MatrixTypeNested; + typedef std::remove_reference_t MatrixTypeNested_; + typedef typename MatrixType::StorageKind StorageKind; + enum { + RowsAtCompileTime = (int(DiagIndex) == DynamicIndex || int(MatrixType::SizeAtCompileTime) == Dynamic) + ? Dynamic + : (plain_enum_min(MatrixType::RowsAtCompileTime - plain_enum_max(-DiagIndex, 0), + MatrixType::ColsAtCompileTime - plain_enum_max(DiagIndex, 0))), + ColsAtCompileTime = 1, + MaxRowsAtCompileTime = + int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic + : DiagIndex == DynamicIndex + ? min_size_prefer_fixed(MatrixType::MaxRowsAtCompileTime, MatrixType::MaxColsAtCompileTime) + : (plain_enum_min(MatrixType::MaxRowsAtCompileTime - plain_enum_max(-DiagIndex, 0), + MatrixType::MaxColsAtCompileTime - plain_enum_max(DiagIndex, 0))), + MaxColsAtCompileTime = 1, + MaskLvalueBit = is_lvalue::value ? LvalueBit : 0, + Flags = (unsigned int)MatrixTypeNested_::Flags & (RowMajorBit | MaskLvalueBit | DirectAccessBit) & + ~RowMajorBit, // FIXME DirectAccessBit should not be handled by expressions + MatrixTypeOuterStride = outer_stride_at_compile_time::ret, + InnerStrideAtCompileTime = MatrixTypeOuterStride == Dynamic ? Dynamic : MatrixTypeOuterStride + 1, + OuterStrideAtCompileTime = 0 + }; +}; +} // namespace internal + +template +class Diagonal : public internal::dense_xpr_base >::type { + public: + enum { DiagIndex = DiagIndex_ }; + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Diagonal) + + EIGEN_DEVICE_FUNC explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) + : m_matrix(matrix), m_index(a_index) { + eigen_assert(a_index <= m_matrix.cols() && -a_index <= m_matrix.rows()); + } + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Diagonal) + + EIGEN_DEVICE_FUNC inline Index rows() const { + return m_index.value() < 0 ? numext::mini(m_matrix.cols(), m_matrix.rows() + m_index.value()) + : numext::mini(m_matrix.rows(), m_matrix.cols() - m_index.value()); + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return 1; } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { + return m_matrix.outerStride() + 1; + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return 0; } + + typedef std::conditional_t::value, Scalar, const Scalar> ScalarWithConstIfNotLvalue; + + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return &(m_matrix.coeffRef(rowOffset(), colOffset())); } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(m_matrix.coeffRef(rowOffset(), colOffset())); } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index) { + EIGEN_STATIC_ASSERT_LVALUE(MatrixType) + return m_matrix.coeffRef(row + rowOffset(), row + colOffset()); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index row, Index) const { + return m_matrix.coeffRef(row + rowOffset(), row + colOffset()); + } + + EIGEN_DEVICE_FUNC inline CoeffReturnType coeff(Index row, Index) const { + return m_matrix.coeff(row + rowOffset(), row + colOffset()); + } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index idx) { + EIGEN_STATIC_ASSERT_LVALUE(MatrixType) + return m_matrix.coeffRef(idx + rowOffset(), idx + colOffset()); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index idx) const { + return m_matrix.coeffRef(idx + rowOffset(), idx + colOffset()); + } + + EIGEN_DEVICE_FUNC inline CoeffReturnType coeff(Index idx) const { + return m_matrix.coeff(idx + rowOffset(), idx + colOffset()); + } + + EIGEN_DEVICE_FUNC inline const internal::remove_all_t& nestedExpression() const { + return m_matrix; + } + + EIGEN_DEVICE_FUNC inline Index index() const { return m_index.value(); } + + protected: + typename internal::ref_selector::non_const_type m_matrix; + const internal::variable_if_dynamicindex m_index; + + private: + // some compilers may fail to optimize std::max etc in case of compile-time constants... + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index absDiagIndex() const EIGEN_NOEXCEPT { + return m_index.value() > 0 ? m_index.value() : -m_index.value(); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowOffset() const EIGEN_NOEXCEPT { + return m_index.value() > 0 ? 0 : -m_index.value(); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colOffset() const EIGEN_NOEXCEPT { + return m_index.value() > 0 ? m_index.value() : 0; + } + // trigger a compile-time error if someone try to call packet + template + typename MatrixType::PacketReturnType packet(Index) const; + template + typename MatrixType::PacketReturnType packet(Index, Index) const; +}; + +/** \returns an expression of the main diagonal of the matrix \c *this + * + * \c *this is not required to be square. + * + * Example: \include MatrixBase_diagonal.cpp + * Output: \verbinclude MatrixBase_diagonal.out + * + * \sa class Diagonal */ +template +EIGEN_DEVICE_FUNC inline typename MatrixBase::DiagonalReturnType MatrixBase::diagonal() { + return DiagonalReturnType(derived()); +} + +/** This is the const version of diagonal(). */ +template +EIGEN_DEVICE_FUNC inline const typename MatrixBase::ConstDiagonalReturnType MatrixBase::diagonal() + const { + return ConstDiagonalReturnType(derived()); +} + +/** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this + * + * \c *this is not required to be square. + * + * The template parameter \a DiagIndex represent a super diagonal if \a DiagIndex > 0 + * and a sub diagonal otherwise. \a DiagIndex == 0 is equivalent to the main diagonal. + * + * Example: \include MatrixBase_diagonal_int.cpp + * Output: \verbinclude MatrixBase_diagonal_int.out + * + * \sa MatrixBase::diagonal(), class Diagonal */ +template +EIGEN_DEVICE_FUNC inline Diagonal MatrixBase::diagonal(Index index) { + return Diagonal(derived(), index); +} + +/** This is the const version of diagonal(Index). */ +template +EIGEN_DEVICE_FUNC inline const Diagonal MatrixBase::diagonal(Index index) const { + return Diagonal(derived(), index); +} + +/** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this + * + * \c *this is not required to be square. + * + * The template parameter \a DiagIndex represent a super diagonal if \a DiagIndex > 0 + * and a sub diagonal otherwise. \a DiagIndex == 0 is equivalent to the main diagonal. + * + * Example: \include MatrixBase_diagonal_template_int.cpp + * Output: \verbinclude MatrixBase_diagonal_template_int.out + * + * \sa MatrixBase::diagonal(), class Diagonal */ +template +template +EIGEN_DEVICE_FUNC inline Diagonal MatrixBase::diagonal() { + return Diagonal(derived()); +} + +/** This is the const version of diagonal(). */ +template +template +EIGEN_DEVICE_FUNC inline const Diagonal MatrixBase::diagonal() const { + return Diagonal(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_DIAGONAL_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h new file mode 100644 index 00000000..fd61bb79 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h @@ -0,0 +1,414 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud +// Copyright (C) 2007-2009 Benoit Jacob +// +// 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_DIAGONALMATRIX_H +#define EIGEN_DIAGONALMATRIX_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class DiagonalBase + * \ingroup Core_Module + * + * \brief Base class for diagonal matrices and expressions + * + * This is the base class that is inherited by diagonal matrix and related expression + * types, which internally use a vector for storing the diagonal entries. Diagonal + * types always represent square matrices. + * + * \tparam Derived is the derived type, a DiagonalMatrix or DiagonalWrapper. + * + * \sa class DiagonalMatrix, class DiagonalWrapper + */ +template +class DiagonalBase : public EigenBase { + public: + typedef typename internal::traits::DiagonalVectorType DiagonalVectorType; + typedef typename DiagonalVectorType::Scalar Scalar; + typedef typename DiagonalVectorType::RealScalar RealScalar; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::StorageIndex StorageIndex; + + enum { + RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, + ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, + MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, + MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, + IsVectorAtCompileTime = 0, + Flags = NoPreferredStorageOrderBit + }; + + typedef Matrix + DenseMatrixType; + typedef DenseMatrixType DenseType; + typedef DiagonalMatrix + PlainObject; + + /** \returns a reference to the derived object. */ + EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast(this); } + /** \returns a const reference to the derived object. */ + EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast(this); } + + /** + * Constructs a dense matrix from \c *this. Note, this directly returns a dense matrix type, + * not an expression. + * \returns A dense matrix, with its diagonal entries set from the the derived object. */ + EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const { return derived(); } + + /** \returns a reference to the derived object's vector of diagonal coefficients. */ + EIGEN_DEVICE_FUNC inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); } + /** \returns a const reference to the derived object's vector of diagonal coefficients. */ + EIGEN_DEVICE_FUNC inline DiagonalVectorType& diagonal() { return derived().diagonal(); } + + /** \returns the value of the coefficient as if \c *this was a dense matrix. */ + EIGEN_DEVICE_FUNC inline Scalar coeff(Index row, Index col) const { + eigen_assert(row >= 0 && col >= 0 && row < rows() && col <= cols()); + return row == col ? diagonal().coeff(row) : Scalar(0); + } + + /** \returns the number of rows. */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const { return diagonal().size(); } + /** \returns the number of columns. */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const { return diagonal().size(); } + + /** \returns the diagonal matrix product of \c *this by the dense matrix, \a matrix */ + template + EIGEN_DEVICE_FUNC const Product operator*( + const MatrixBase& matrix) const { + return Product(derived(), matrix.derived()); + } + + template + using DiagonalProductReturnType = DiagonalWrapper; + + /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a other */ + template + EIGEN_DEVICE_FUNC const DiagonalProductReturnType operator*( + const DiagonalBase& other) const { + return diagonal().cwiseProduct(other.diagonal()).asDiagonal(); + } + + using DiagonalInverseReturnType = + DiagonalWrapper, const DiagonalVectorType>>; + + /** \returns the inverse \c *this. Computed as the coefficient-wise inverse of the diagonal. */ + EIGEN_DEVICE_FUNC inline const DiagonalInverseReturnType inverse() const { + return diagonal().cwiseInverse().asDiagonal(); + } + + using DiagonalScaleReturnType = + DiagonalWrapper; + + /** \returns the product of \c *this by the scalar \a scalar */ + EIGEN_DEVICE_FUNC inline const DiagonalScaleReturnType operator*(const Scalar& scalar) const { + return (diagonal() * scalar).asDiagonal(); + } + + using ScaleDiagonalReturnType = + DiagonalWrapper; + + /** \returns the product of a scalar and the diagonal matrix \a other */ + EIGEN_DEVICE_FUNC friend inline const ScaleDiagonalReturnType operator*(const Scalar& scalar, + const DiagonalBase& other) { + return (scalar * other.diagonal()).asDiagonal(); + } + + template + using DiagonalSumReturnType = DiagonalWrapper; + + /** \returns the sum of \c *this and the diagonal matrix \a other */ + template + EIGEN_DEVICE_FUNC inline const DiagonalSumReturnType operator+( + const DiagonalBase& other) const { + return (diagonal() + other.diagonal()).asDiagonal(); + } + + template + using DiagonalDifferenceReturnType = DiagonalWrapper; + + /** \returns the difference of \c *this and the diagonal matrix \a other */ + template + EIGEN_DEVICE_FUNC inline const DiagonalDifferenceReturnType operator-( + const DiagonalBase& other) const { + return (diagonal() - other.diagonal()).asDiagonal(); + } +}; + +/** \class DiagonalMatrix + * \ingroup Core_Module + * + * \brief Represents a diagonal matrix with its storage + * + * \tparam Scalar_ the type of coefficients + * \tparam SizeAtCompileTime the dimension of the matrix, or Dynamic + * \tparam MaxSizeAtCompileTime the dimension of the matrix, or Dynamic. This parameter is optional and defaults + * to SizeAtCompileTime. Most of the time, you do not need to specify it. + * + * \sa class DiagonalBase, class DiagonalWrapper + */ + +namespace internal { +template +struct traits> + : traits> { + typedef Matrix DiagonalVectorType; + typedef DiagonalShape StorageKind; + enum { Flags = LvalueBit | NoPreferredStorageOrderBit | NestByRefBit }; +}; +} // namespace internal +template +class DiagonalMatrix : public DiagonalBase> { + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename internal::traits::DiagonalVectorType DiagonalVectorType; + typedef const DiagonalMatrix& Nested; + typedef Scalar_ Scalar; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::StorageIndex StorageIndex; +#endif + + protected: + DiagonalVectorType m_diagonal; + + public: + /** const version of diagonal(). */ + EIGEN_DEVICE_FUNC inline const DiagonalVectorType& diagonal() const { return m_diagonal; } + /** \returns a reference to the stored vector of diagonal coefficients. */ + EIGEN_DEVICE_FUNC inline DiagonalVectorType& diagonal() { return m_diagonal; } + + /** Default constructor without initialization */ + EIGEN_DEVICE_FUNC inline DiagonalMatrix() {} + + /** Constructs a diagonal matrix with given dimension */ + EIGEN_DEVICE_FUNC explicit inline DiagonalMatrix(Index dim) : m_diagonal(dim) {} + + /** 2D constructor. */ + EIGEN_DEVICE_FUNC inline DiagonalMatrix(const Scalar& x, const Scalar& y) : m_diagonal(x, y) {} + + /** 3D constructor. */ + EIGEN_DEVICE_FUNC inline DiagonalMatrix(const Scalar& x, const Scalar& y, const Scalar& z) : m_diagonal(x, y, z) {} + + /** \brief Construct a diagonal matrix with fixed size from an arbitrary number of coefficients. + * + * \warning To construct a diagonal matrix of fixed size, the number of values passed to this + * constructor must match the fixed dimension of \c *this. + * + * \sa DiagonalMatrix(const Scalar&, const Scalar&) + * \sa DiagonalMatrix(const Scalar&, const Scalar&, const Scalar&) + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DiagonalMatrix(const Scalar& a0, const Scalar& a1, const Scalar& a2, + const ArgTypes&... args) + : m_diagonal(a0, a1, a2, args...) {} + + /** \brief Constructs a DiagonalMatrix and initializes it by elements given by an initializer list of initializer + * lists \cpp11 + */ + EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE DiagonalMatrix( + const std::initializer_list>& list) + : m_diagonal(list) {} + + /** \brief Constructs a DiagonalMatrix from an r-value diagonal vector type */ + EIGEN_DEVICE_FUNC explicit inline DiagonalMatrix(DiagonalVectorType&& diag) : m_diagonal(std::move(diag)) {} + + /** Copy constructor. */ + template + EIGEN_DEVICE_FUNC inline DiagonalMatrix(const DiagonalBase& other) : m_diagonal(other.diagonal()) {} + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** copy constructor. prevent a default copy constructor from hiding the other templated constructor */ + inline DiagonalMatrix(const DiagonalMatrix& other) : m_diagonal(other.diagonal()) {} +#endif + + /** generic constructor from expression of the diagonal coefficients */ + template + EIGEN_DEVICE_FUNC explicit inline DiagonalMatrix(const MatrixBase& other) : m_diagonal(other) {} + + /** Copy operator. */ + template + EIGEN_DEVICE_FUNC DiagonalMatrix& operator=(const DiagonalBase& other) { + m_diagonal = other.diagonal(); + return *this; + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + EIGEN_DEVICE_FUNC DiagonalMatrix& operator=(const DiagonalMatrix& other) { + m_diagonal = other.diagonal(); + return *this; + } +#endif + + typedef DiagonalWrapper, DiagonalVectorType>> + InitializeReturnType; + + /** Initializes a diagonal matrix of size SizeAtCompileTime with coefficients set to zero */ + EIGEN_DEVICE_FUNC static const InitializeReturnType Zero() { return DiagonalVectorType::Zero().asDiagonal(); } + /** Initializes a diagonal matrix of size dim with coefficients set to zero */ + EIGEN_DEVICE_FUNC static const InitializeReturnType Zero(Index size) { + return DiagonalVectorType::Zero(size).asDiagonal(); + } + /** Initializes a identity matrix of size SizeAtCompileTime */ + EIGEN_DEVICE_FUNC static const InitializeReturnType Identity() { return DiagonalVectorType::Ones().asDiagonal(); } + /** Initializes a identity matrix of size dim */ + EIGEN_DEVICE_FUNC static const InitializeReturnType Identity(Index size) { + return DiagonalVectorType::Ones(size).asDiagonal(); + } + + /** Resizes to given size. */ + EIGEN_DEVICE_FUNC inline void resize(Index size) { m_diagonal.resize(size); } + /** Sets all coefficients to zero. */ + EIGEN_DEVICE_FUNC inline void setZero() { m_diagonal.setZero(); } + /** Resizes and sets all coefficients to zero. */ + EIGEN_DEVICE_FUNC inline void setZero(Index size) { m_diagonal.setZero(size); } + /** Sets this matrix to be the identity matrix of the current size. */ + EIGEN_DEVICE_FUNC inline void setIdentity() { m_diagonal.setOnes(); } + /** Sets this matrix to be the identity matrix of the given size. */ + EIGEN_DEVICE_FUNC inline void setIdentity(Index size) { m_diagonal.setOnes(size); } +}; + +/** \class DiagonalWrapper + * \ingroup Core_Module + * + * \brief Expression of a diagonal matrix + * + * \tparam DiagonalVectorType_ the type of the vector of diagonal coefficients + * + * This class is an expression of a diagonal matrix, but not storing its own vector of diagonal coefficients, + * instead wrapping an existing vector expression. It is the return type of MatrixBase::asDiagonal() + * and most of the time this is the only way that it is used. + * + * \sa class DiagonalMatrix, class DiagonalBase, MatrixBase::asDiagonal() + */ + +namespace internal { +template +struct traits> { + typedef DiagonalVectorType_ DiagonalVectorType; + typedef typename DiagonalVectorType::Scalar Scalar; + typedef typename DiagonalVectorType::StorageIndex StorageIndex; + typedef DiagonalShape StorageKind; + typedef typename traits::XprKind XprKind; + enum { + RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, + ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, + MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, + MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, + Flags = (traits::Flags & LvalueBit) | NoPreferredStorageOrderBit + }; +}; +} // namespace internal + +template +class DiagonalWrapper : public DiagonalBase>, internal::no_assignment_operator { + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef DiagonalVectorType_ DiagonalVectorType; + typedef DiagonalWrapper Nested; +#endif + + /** Constructor from expression of diagonal coefficients to wrap. */ + EIGEN_DEVICE_FUNC explicit inline DiagonalWrapper(DiagonalVectorType& a_diagonal) : m_diagonal(a_diagonal) {} + + /** \returns a const reference to the wrapped expression of diagonal coefficients. */ + EIGEN_DEVICE_FUNC const DiagonalVectorType& diagonal() const { return m_diagonal; } + + protected: + typename DiagonalVectorType::Nested m_diagonal; +}; + +/** \returns a pseudo-expression of a diagonal matrix with *this as vector of diagonal coefficients + * + * \only_for_vectors + * + * Example: \include MatrixBase_asDiagonal.cpp + * Output: \verbinclude MatrixBase_asDiagonal.out + * + * \sa class DiagonalWrapper, class DiagonalMatrix, diagonal(), isDiagonal() + **/ +template +EIGEN_DEVICE_FUNC inline const DiagonalWrapper MatrixBase::asDiagonal() const { + return DiagonalWrapper(derived()); +} + +/** \returns true if *this is approximately equal to a diagonal matrix, + * within the precision given by \a prec. + * + * Example: \include MatrixBase_isDiagonal.cpp + * Output: \verbinclude MatrixBase_isDiagonal.out + * + * \sa asDiagonal() + */ +template +bool MatrixBase::isDiagonal(const RealScalar& prec) const { + if (cols() != rows()) return false; + RealScalar maxAbsOnDiagonal = static_cast(-1); + for (Index j = 0; j < cols(); ++j) { + RealScalar absOnDiagonal = numext::abs(coeff(j, j)); + if (absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal; + } + for (Index j = 0; j < cols(); ++j) + for (Index i = 0; i < j; ++i) { + if (!internal::isMuchSmallerThan(coeff(i, j), maxAbsOnDiagonal, prec)) return false; + if (!internal::isMuchSmallerThan(coeff(j, i), maxAbsOnDiagonal, prec)) return false; + } + return true; +} + +namespace internal { + +template <> +struct storage_kind_to_shape { + typedef DiagonalShape Shape; +}; + +struct Diagonal2Dense {}; + +template <> +struct AssignmentKind { + typedef Diagonal2Dense Kind; +}; + +// Diagonal matrix to Dense assignment +template +struct Assignment { + static void run(DstXprType& dst, const SrcXprType& src, + const internal::assign_op& /*func*/) { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols); + + dst.setZero(); + dst.diagonal() = src.diagonal(); + } + + static void run(DstXprType& dst, const SrcXprType& src, + const internal::add_assign_op& /*func*/) { + dst.diagonal() += src.diagonal(); + } + + static void run(DstXprType& dst, const SrcXprType& src, + const internal::sub_assign_op& /*func*/) { + dst.diagonal() -= src.diagonal(); + } +}; + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_DIAGONALMATRIX_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h new file mode 100644 index 00000000..bd0feeac --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h @@ -0,0 +1,30 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2007-2009 Benoit Jacob +// +// 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_DIAGONALPRODUCT_H +#define EIGEN_DIAGONALPRODUCT_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal. + */ +template +template +EIGEN_DEVICE_FUNC inline const Product MatrixBase::operator*( + const DiagonalBase &a_diagonal) const { + return Product(derived(), a_diagonal.derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_DIAGONALPRODUCT_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Dot.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Dot.h new file mode 100644 index 00000000..82eb9c70 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Dot.h @@ -0,0 +1,289 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2008, 2010 Benoit Jacob +// +// 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_DOT_H +#define EIGEN_DOT_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +// helper function for dot(). The problem is that if we put that in the body of dot(), then upon calling dot +// with mismatched types, the compiler emits errors about failing to instantiate cwiseProduct BEFORE +// looking at the static assertions. Thus this is a trick to get better compile errors. +template +struct dot_nocheck { + typedef scalar_conj_product_op::Scalar, typename traits::Scalar> conj_prod; + typedef typename conj_prod::result_type ResScalar; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static ResScalar run(const MatrixBase& a, const MatrixBase& b) { + return a.template binaryExpr(b).sum(); + } +}; + +template +struct dot_nocheck { + typedef scalar_conj_product_op::Scalar, typename traits::Scalar> conj_prod; + typedef typename conj_prod::result_type ResScalar; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static ResScalar run(const MatrixBase& a, const MatrixBase& b) { + return a.transpose().template binaryExpr(b).sum(); + } +}; + +} // end namespace internal + +/** \fn MatrixBase::dot + * \returns the dot product of *this with other. + * + * \only_for_vectors + * + * \note If the scalar type is complex numbers, then this function returns the hermitian + * (sesquilinear) dot product, conjugate-linear in the first variable and linear in the + * second variable. + * + * \sa squaredNorm(), norm() + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename ScalarBinaryOpTraits::Scalar, + typename internal::traits::Scalar>::ReturnType + MatrixBase::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) +#if !(defined(EIGEN_NO_STATIC_ASSERT) && defined(EIGEN_NO_DEBUG)) + EIGEN_CHECK_BINARY_COMPATIBILIY( + Eigen::internal::scalar_conj_product_op, Scalar, + typename OtherDerived::Scalar); +#endif + + eigen_assert(size() == other.size()); + + return internal::dot_nocheck::run(*this, other); +} + +//---------- implementation of L2 norm and related functions ---------- + +/** \returns, for vectors, the squared \em l2 norm of \c *this, and for matrices the squared Frobenius norm. + * In both cases, it consists in the sum of the square of all the matrix entries. + * For vectors, this is also equals to the dot product of \c *this with itself. + * + * \sa dot(), norm(), lpNorm() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits::Scalar>::Real +MatrixBase::squaredNorm() const { + return numext::real((*this).cwiseAbs2().sum()); +} + +/** \returns, for vectors, the \em l2 norm of \c *this, and for matrices the Frobenius norm. + * In both cases, it consists in the square root of the sum of the square of all the matrix entries. + * For vectors, this is also equals to the square root of the dot product of \c *this with itself. + * + * \sa lpNorm(), dot(), squaredNorm() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits::Scalar>::Real +MatrixBase::norm() const { + return numext::sqrt(squaredNorm()); +} + +/** \returns an expression of the quotient of \c *this by its own norm. + * + * \warning If the input vector is too small (i.e., this->norm()==0), + * then this function returns a copy of the input. + * + * \only_for_vectors + * + * \sa norm(), normalize() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::PlainObject MatrixBase::normalized() + const { + typedef typename internal::nested_eval::type Nested_; + Nested_ n(derived()); + RealScalar z = n.squaredNorm(); + // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU + if (z > RealScalar(0)) + return n / numext::sqrt(z); + else + return n; +} + +/** Normalizes the vector, i.e. divides it by its own norm. + * + * \only_for_vectors + * + * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged. + * + * \sa norm(), normalized() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase::normalize() { + RealScalar z = squaredNorm(); + // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU + if (z > RealScalar(0)) derived() /= numext::sqrt(z); +} + +/** \returns an expression of the quotient of \c *this by its own norm while avoiding underflow and overflow. + * + * \only_for_vectors + * + * This method is analogue to the normalized() method, but it reduces the risk of + * underflow and overflow when computing the norm. + * + * \warning If the input vector is too small (i.e., this->norm()==0), + * then this function returns a copy of the input. + * + * \sa stableNorm(), stableNormalize(), normalized() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase::PlainObject +MatrixBase::stableNormalized() const { + typedef typename internal::nested_eval::type Nested_; + Nested_ n(derived()); + RealScalar w = n.cwiseAbs().maxCoeff(); + RealScalar z = (n / w).squaredNorm(); + if (z > RealScalar(0)) + return n / (numext::sqrt(z) * w); + else + return n; +} + +/** Normalizes the vector while avoid underflow and overflow + * + * \only_for_vectors + * + * This method is analogue to the normalize() method, but it reduces the risk of + * underflow and overflow when computing the norm. + * + * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged. + * + * \sa stableNorm(), stableNormalized(), normalize() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase::stableNormalize() { + RealScalar w = cwiseAbs().maxCoeff(); + RealScalar z = (derived() / w).squaredNorm(); + if (z > RealScalar(0)) derived() /= numext::sqrt(z) * w; +} + +//---------- implementation of other norms ---------- + +namespace internal { + +template +struct lpNorm_selector { + typedef typename NumTraits::Scalar>::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const MatrixBase& m) { + EIGEN_USING_STD(pow) + return pow(m.cwiseAbs().array().pow(p).sum(), RealScalar(1) / p); + } +}; + +template +struct lpNorm_selector { + EIGEN_DEVICE_FUNC static inline typename NumTraits::Scalar>::Real run( + const MatrixBase& m) { + return m.cwiseAbs().sum(); + } +}; + +template +struct lpNorm_selector { + EIGEN_DEVICE_FUNC static inline typename NumTraits::Scalar>::Real run( + const MatrixBase& m) { + return m.norm(); + } +}; + +template +struct lpNorm_selector { + typedef typename NumTraits::Scalar>::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const MatrixBase& m) { + if (Derived::SizeAtCompileTime == 0 || (Derived::SizeAtCompileTime == Dynamic && m.size() == 0)) + return RealScalar(0); + return m.cwiseAbs().maxCoeff(); + } +}; + +} // end namespace internal + +/** \returns the \b coefficient-wise \f$ \ell^p \f$ norm of \c *this, that is, returns the p-th root of the sum of the + * p-th powers of the absolute values of the coefficients of \c *this. If \a p is the special value \a Eigen::Infinity, + * this function returns the \f$ \ell^\infty \f$ norm, that is the maximum of the absolute values of the coefficients of + * \c *this. + * + * In all cases, if \c *this is empty, then the value 0 is returned. + * + * \note For matrices, this function does not compute the operator-norm. That is, if \c *this is a matrix, then its + * coefficients are interpreted as a 1D vector. Nonetheless, you can easily compute the 1-norm and \f$\infty\f$-norm + * matrix operator norms using \link TutorialReductionsVisitorsBroadcastingReductionsNorm partial reductions \endlink. + * + * \sa norm() + */ +template +template +#ifndef EIGEN_PARSED_BY_DOXYGEN +EIGEN_DEVICE_FUNC inline typename NumTraits::Scalar>::Real +#else +EIGEN_DEVICE_FUNC MatrixBase::RealScalar +#endif +MatrixBase::lpNorm() const { + return internal::lpNorm_selector::run(*this); +} + +//---------- implementation of isOrthogonal / isUnitary ---------- + +/** \returns true if *this is approximately orthogonal to \a other, + * within the precision given by \a prec. + * + * Example: \include MatrixBase_isOrthogonal.cpp + * Output: \verbinclude MatrixBase_isOrthogonal.out + */ +template +template +bool MatrixBase::isOrthogonal(const MatrixBase& other, const RealScalar& prec) const { + typename internal::nested_eval::type nested(derived()); + typename internal::nested_eval::type otherNested(other.derived()); + return numext::abs2(nested.dot(otherNested)) <= prec * prec * nested.squaredNorm() * otherNested.squaredNorm(); +} + +/** \returns true if *this is approximately an unitary matrix, + * within the precision given by \a prec. In the case where the \a Scalar + * type is real numbers, a unitary matrix is an orthogonal matrix, whence the name. + * + * \note This can be used to check whether a family of vectors forms an orthonormal basis. + * Indeed, \c m.isUnitary() returns true if and only if the columns (equivalently, the rows) of m form an + * orthonormal basis. + * + * Example: \include MatrixBase_isUnitary.cpp + * Output: \verbinclude MatrixBase_isUnitary.out + */ +template +bool MatrixBase::isUnitary(const RealScalar& prec) const { + typename internal::nested_eval::type self(derived()); + for (Index i = 0; i < cols(); ++i) { + if (!internal::isApprox(self.col(i).squaredNorm(), static_cast(1), prec)) return false; + for (Index j = 0; j < i; ++j) + if (!internal::isMuchSmallerThan(self.col(i).dot(self.col(j)), static_cast(1), prec)) return false; + } + return true; +} + +} // end namespace Eigen + +#endif // EIGEN_DOT_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h new file mode 100644 index 00000000..f485016a --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h @@ -0,0 +1,144 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Benoit Jacob +// Copyright (C) 2009 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_EIGENBASE_H +#define EIGEN_EIGENBASE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class EigenBase + * \ingroup Core_Module + * + * Common base class for all classes T such that MatrixBase has an operator=(T) and a constructor MatrixBase(T). + * + * In other words, an EigenBase object is an object that can be copied into a MatrixBase. + * + * Besides MatrixBase-derived classes, this also includes special matrix classes such as diagonal matrices, etc. + * + * Notice that this class is trivial, it is only used to disambiguate overloaded functions. + * + * \sa \blank \ref TopicClassHierarchy + */ +template +struct EigenBase { + // typedef typename internal::plain_matrix_type::type PlainObject; + + /** \brief The interface type of indices + * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE. + * \sa StorageIndex, \ref TopicPreprocessorDirectives. + * DEPRECATED: Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. + * Deprecation is not marked with a doxygen comment because there are too many existing usages to add the deprecation + * attribute. + */ + typedef Eigen::Index Index; + + // FIXME is it needed? + typedef typename internal::traits::StorageKind StorageKind; + + /** \returns a reference to the derived object */ + EIGEN_DEVICE_FUNC Derived& derived() { return *static_cast(this); } + /** \returns a const reference to the derived object */ + EIGEN_DEVICE_FUNC const Derived& derived() const { return *static_cast(this); } + + EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const { + return *static_cast(const_cast(this)); + } + EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(this); } + + /** \returns the number of rows. \sa cols(), RowsAtCompileTime */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); } + /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); } + /** \returns the number of coefficients, which is rows()*cols(). + * \sa rows(), cols(), SizeAtCompileTime. */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index size() const EIGEN_NOEXCEPT { return rows() * cols(); } + + /** \internal Don't use it, but do the equivalent: \code dst = *this; \endcode */ + template + EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const { + derived().evalTo(dst); + } + + /** \internal Don't use it, but do the equivalent: \code dst += *this; \endcode */ + template + EIGEN_DEVICE_FUNC inline void addTo(Dest& dst) const { + // This is the default implementation, + // derived class can reimplement it in a more optimized way. + typename Dest::PlainObject res(rows(), cols()); + evalTo(res); + dst += res; + } + + /** \internal Don't use it, but do the equivalent: \code dst -= *this; \endcode */ + template + EIGEN_DEVICE_FUNC inline void subTo(Dest& dst) const { + // This is the default implementation, + // derived class can reimplement it in a more optimized way. + typename Dest::PlainObject res(rows(), cols()); + evalTo(res); + dst -= res; + } + + /** \internal Don't use it, but do the equivalent: \code dst.applyOnTheRight(*this); \endcode */ + template + EIGEN_DEVICE_FUNC inline void applyThisOnTheRight(Dest& dst) const { + // This is the default implementation, + // derived class can reimplement it in a more optimized way. + dst = dst * this->derived(); + } + + /** \internal Don't use it, but do the equivalent: \code dst.applyOnTheLeft(*this); \endcode */ + template + EIGEN_DEVICE_FUNC inline void applyThisOnTheLeft(Dest& dst) const { + // This is the default implementation, + // derived class can reimplement it in a more optimized way. + dst = this->derived() * dst; + } +}; + +/*************************************************************************** + * Implementation of matrix base methods + ***************************************************************************/ + +/** \brief Copies the generic expression \a other into *this. + * + * \details The expression must provide a (templated) evalTo(Derived& dst) const + * function which does the actual job. In practice, this allows any user to write + * its own special matrix without having to modify MatrixBase + * + * \returns a reference to *this. + */ +template +template +EIGEN_DEVICE_FUNC Derived& DenseBase::operator=(const EigenBase& other) { + call_assignment(derived(), other.derived()); + return derived(); +} + +template +template +EIGEN_DEVICE_FUNC Derived& DenseBase::operator+=(const EigenBase& other) { + call_assignment(derived(), other.derived(), internal::add_assign_op()); + return derived(); +} + +template +template +EIGEN_DEVICE_FUNC Derived& DenseBase::operator-=(const EigenBase& other) { + call_assignment(derived(), other.derived(), internal::sub_assign_op()); + return derived(); +} + +} // end namespace Eigen + +#endif // EIGEN_EIGENBASE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h new file mode 100644 index 00000000..a91b0da6 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h @@ -0,0 +1,131 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-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/. + +#ifndef EIGEN_FORCEALIGNEDACCESS_H +#define EIGEN_FORCEALIGNEDACCESS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class ForceAlignedAccess + * \ingroup Core_Module + * + * \brief Enforce aligned packet loads and stores regardless of what is requested + * + * \param ExpressionType the type of the object of which we are forcing aligned packet access + * + * This class is the return type of MatrixBase::forceAlignedAccess() + * and most of the time this is the only way it is used. + * + * \sa MatrixBase::forceAlignedAccess() + */ + +namespace internal { +template +struct traits> : public traits {}; +} // namespace internal + +template +class ForceAlignedAccess : public internal::dense_xpr_base>::type { + public: + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(ForceAlignedAccess) + + EIGEN_DEVICE_FUNC explicit inline ForceAlignedAccess(const ExpressionType& matrix) : m_expression(matrix) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { + return m_expression.outerStride(); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { + return m_expression.innerStride(); + } + + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const { + return m_expression.coeff(row, col); + } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) { + return m_expression.const_cast_derived().coeffRef(row, col); + } + + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const { return m_expression.coeff(index); } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) { return m_expression.const_cast_derived().coeffRef(index); } + + template + inline const PacketScalar packet(Index row, Index col) const { + return m_expression.template packet(row, col); + } + + template + inline void writePacket(Index row, Index col, const PacketScalar& x) { + m_expression.const_cast_derived().template writePacket(row, col, x); + } + + template + inline const PacketScalar packet(Index index) const { + return m_expression.template packet(index); + } + + template + inline void writePacket(Index index, const PacketScalar& x) { + m_expression.const_cast_derived().template writePacket(index, x); + } + + EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; } + + protected: + const ExpressionType& m_expression; + + private: + ForceAlignedAccess& operator=(const ForceAlignedAccess&); +}; + +/** \returns an expression of *this with forced aligned access + * \sa forceAlignedAccessIf(),class ForceAlignedAccess + */ +template +inline const ForceAlignedAccess MatrixBase::forceAlignedAccess() const { + return ForceAlignedAccess(derived()); +} + +/** \returns an expression of *this with forced aligned access + * \sa forceAlignedAccessIf(), class ForceAlignedAccess + */ +template +inline ForceAlignedAccess MatrixBase::forceAlignedAccess() { + return ForceAlignedAccess(derived()); +} + +/** \returns an expression of *this with forced aligned access if \a Enable is true. + * \sa forceAlignedAccess(), class ForceAlignedAccess + */ +template +template +inline add_const_on_value_type_t, Derived&>> +MatrixBase::forceAlignedAccessIf() const { + return derived(); // FIXME This should not work but apparently is never used +} + +/** \returns an expression of *this with forced aligned access if \a Enable is true. + * \sa forceAlignedAccess(), class ForceAlignedAccess + */ +template +template +inline std::conditional_t, Derived&> MatrixBase::forceAlignedAccessIf() { + return derived(); // FIXME This should not work but apparently is never used +} + +} // end namespace Eigen + +#endif // EIGEN_FORCEALIGNEDACCESS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h new file mode 100644 index 00000000..ed6b4ffe --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h @@ -0,0 +1,132 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2008 Benoit Jacob +// 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_FUZZY_H +#define EIGEN_FUZZY_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template ::IsInteger> +struct isApprox_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) { + typename internal::nested_eval::type nested(x); + typename internal::nested_eval::type otherNested(y); + return (nested.matrix() - otherNested.matrix()).cwiseAbs2().sum() <= + prec * prec * numext::mini(nested.cwiseAbs2().sum(), otherNested.cwiseAbs2().sum()); + } +}; + +template +struct isApprox_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar&) { + return x.matrix() == y.matrix(); + } +}; + +template ::IsInteger> +struct isMuchSmallerThan_object_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) { + return x.cwiseAbs2().sum() <= numext::abs2(prec) * y.cwiseAbs2().sum(); + } +}; + +template +struct isMuchSmallerThan_object_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived&, const typename Derived::RealScalar&) { + return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix(); + } +}; + +template ::IsInteger> +struct isMuchSmallerThan_scalar_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const typename Derived::RealScalar& y, + const typename Derived::RealScalar& prec) { + return x.cwiseAbs2().sum() <= numext::abs2(prec * y); + } +}; + +template +struct isMuchSmallerThan_scalar_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const typename Derived::RealScalar&, + const typename Derived::RealScalar&) { + return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix(); + } +}; + +} // end namespace internal + +/** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \note The fuzzy compares are done multiplicatively. Two vectors \f$ v \f$ and \f$ w \f$ + * are considered to be approximately equal within precision \f$ p \f$ if + * \f[ \Vert v - w \Vert \leqslant p\,\min(\Vert v\Vert, \Vert w\Vert). \f] + * For matrices, the comparison is done using the Hilbert-Schmidt norm (aka Frobenius norm + * L2 norm). + * + * \note Because of the multiplicativeness of this comparison, one can't use this function + * to check whether \c *this is approximately equal to the zero matrix or vector. + * Indeed, \c isApprox(zero) returns false unless \c *this itself is exactly the zero matrix + * or vector. If you want to test whether \c *this is zero, use internal::isMuchSmallerThan(const + * RealScalar&, RealScalar) instead. + * + * \sa internal::isMuchSmallerThan(const RealScalar&, RealScalar) const + */ +template +template +EIGEN_DEVICE_FUNC bool DenseBase::isApprox(const DenseBase& other, + const RealScalar& prec) const { + return internal::isApprox_selector::run(derived(), other.derived(), prec); +} + +/** \returns \c true if the norm of \c *this is much smaller than \a other, + * within the precision determined by \a prec. + * + * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is + * considered to be much smaller than \f$ x \f$ within precision \f$ p \f$ if + * \f[ \Vert v \Vert \leqslant p\,\vert x\vert. \f] + * + * For matrices, the comparison is done using the Hilbert-Schmidt norm. For this reason, + * the value of the reference scalar \a other should come from the Hilbert-Schmidt norm + * of a reference matrix of same dimensions. + * + * \sa isApprox(), isMuchSmallerThan(const DenseBase&, RealScalar) const + */ +template +EIGEN_DEVICE_FUNC bool DenseBase::isMuchSmallerThan(const typename NumTraits::Real& other, + const RealScalar& prec) const { + return internal::isMuchSmallerThan_scalar_selector::run(derived(), other, prec); +} + +/** \returns \c true if the norm of \c *this is much smaller than the norm of \a other, + * within the precision determined by \a prec. + * + * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is + * considered to be much smaller than a vector \f$ w \f$ within precision \f$ p \f$ if + * \f[ \Vert v \Vert \leqslant p\,\Vert w\Vert. \f] + * For matrices, the comparison is done using the Hilbert-Schmidt norm. + * + * \sa isApprox(), isMuchSmallerThan(const RealScalar&, RealScalar) const + */ +template +template +EIGEN_DEVICE_FUNC bool DenseBase::isMuchSmallerThan(const DenseBase& other, + const RealScalar& prec) const { + return internal::isMuchSmallerThan_object_selector::run(derived(), other.derived(), prec); +} + +} // end namespace Eigen + +#endif // EIGEN_FUZZY_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h new file mode 100644 index 00000000..3ec68527 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h @@ -0,0 +1,527 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2008 Benoit Jacob +// Copyright (C) 2008-2011 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_GENERAL_PRODUCT_H +#define EIGEN_GENERAL_PRODUCT_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +enum { Large = 2, Small = 3 }; + +// Define the threshold value to fallback from the generic matrix-matrix product +// implementation (heavy) to the lightweight coeff-based product one. +// See generic_product_impl +// in products/GeneralMatrixMatrix.h for more details. +// TODO This threshold should also be used in the compile-time selector below. +#ifndef EIGEN_GEMM_TO_COEFFBASED_THRESHOLD +// This default value has been obtained on a Haswell architecture. +#define EIGEN_GEMM_TO_COEFFBASED_THRESHOLD 20 +#endif + +namespace internal { + +template +struct product_type_selector; + +template +struct product_size_category { + enum { +#ifndef EIGEN_GPU_COMPILE_PHASE + is_large = MaxSize == Dynamic || Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD || + (Size == Dynamic && MaxSize >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD), +#else + is_large = 0, +#endif + value = is_large ? Large + : Size == 1 ? 1 + : Small + }; +}; + +template +struct product_type { + typedef remove_all_t Lhs_; + typedef remove_all_t Rhs_; + enum { + MaxRows = traits::MaxRowsAtCompileTime, + Rows = traits::RowsAtCompileTime, + MaxCols = traits::MaxColsAtCompileTime, + Cols = traits::ColsAtCompileTime, + MaxDepth = min_size_prefer_fixed(traits::MaxColsAtCompileTime, traits::MaxRowsAtCompileTime), + Depth = min_size_prefer_fixed(traits::ColsAtCompileTime, traits::RowsAtCompileTime) + }; + + // the splitting into different lines of code here, introducing the _select enums and the typedef below, + // is to work around an internal compiler error with gcc 4.1 and 4.2. + private: + enum { + rows_select = product_size_category::value, + cols_select = product_size_category::value, + depth_select = product_size_category::value + }; + typedef product_type_selector selector; + + public: + enum { value = selector::ret, ret = selector::ret }; +#ifdef EIGEN_DEBUG_PRODUCT + static void debug() { + EIGEN_DEBUG_VAR(Rows); + EIGEN_DEBUG_VAR(Cols); + EIGEN_DEBUG_VAR(Depth); + EIGEN_DEBUG_VAR(rows_select); + EIGEN_DEBUG_VAR(cols_select); + EIGEN_DEBUG_VAR(depth_select); + EIGEN_DEBUG_VAR(value); + } +#endif +}; + +/* The following allows to select the kind of product at compile time + * based on the three dimensions of the product. + * This is a compile time mapping from {1,Small,Large}^3 -> {product types} */ +// FIXME I'm not sure the current mapping is the ideal one. +template +struct product_type_selector { + enum { ret = OuterProduct }; +}; +template +struct product_type_selector { + enum { ret = LazyCoeffBasedProductMode }; +}; +template +struct product_type_selector<1, N, 1> { + enum { ret = LazyCoeffBasedProductMode }; +}; +template +struct product_type_selector<1, 1, Depth> { + enum { ret = InnerProduct }; +}; +template <> +struct product_type_selector<1, 1, 1> { + enum { ret = InnerProduct }; +}; +template <> +struct product_type_selector { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector<1, Small, Small> { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = LazyCoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = LazyCoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = LazyCoeffBasedProductMode }; +}; +template <> +struct product_type_selector<1, Large, Small> { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector<1, Large, Large> { + enum { ret = GemvProduct }; +}; +template <> +struct product_type_selector<1, Small, Large> { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = GemvProduct }; +}; +template <> +struct product_type_selector { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = GemmProduct }; +}; +template <> +struct product_type_selector { + enum { ret = GemmProduct }; +}; +template <> +struct product_type_selector { + enum { ret = GemmProduct }; +}; +template <> +struct product_type_selector { + enum { ret = GemmProduct }; +}; +template <> +struct product_type_selector { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = CoeffBasedProductMode }; +}; +template <> +struct product_type_selector { + enum { ret = GemmProduct }; +}; + +} // end namespace internal + +/*********************************************************************** + * Implementation of Inner Vector Vector Product + ***********************************************************************/ + +// FIXME : maybe the "inner product" could return a Scalar +// instead of a 1x1 matrix ?? +// Pro: more natural for the user +// Cons: this could be a problem if in a meta unrolled algorithm a matrix-matrix +// product ends up to a row-vector times col-vector product... To tackle this use +// case, we could have a specialization for Block with: operator=(Scalar x); + +/*********************************************************************** + * Implementation of Outer Vector Vector Product + ***********************************************************************/ + +/*********************************************************************** + * Implementation of General Matrix Vector Product + ***********************************************************************/ + +/* According to the shape/flags of the matrix we have to distinghish 3 different cases: + * 1 - the matrix is col-major, BLAS compatible and M is large => call fast BLAS-like colmajor routine + * 2 - the matrix is row-major, BLAS compatible and N is large => call fast BLAS-like rowmajor routine + * 3 - all other cases are handled using a simple loop along the outer-storage direction. + * Therefore we need a lower level meta selector. + * Furthermore, if the matrix is the rhs, then the product has to be transposed. + */ +namespace internal { + +template +struct gemv_dense_selector; + +} // end namespace internal + +namespace internal { + +template +struct gemv_static_vector_if; + +template +struct gemv_static_vector_if { + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Scalar* data() { + eigen_internal_assert(false && "should never be called"); + return 0; + } +}; + +template +struct gemv_static_vector_if { + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Scalar* data() { return 0; } +}; + +template +struct gemv_static_vector_if { + enum { + ForceAlignment = internal::packet_traits::Vectorizable, + PacketSize = internal::packet_traits::size + }; +#if EIGEN_MAX_STATIC_ALIGN_BYTES != 0 + internal::plain_array + m_data; + EIGEN_STRONG_INLINE Scalar* data() { return m_data.array; } +#else + // Some architectures cannot align on the stack, + // => let's manually enforce alignment by allocating more data and return the address of the first aligned element. + internal::plain_array< + Scalar, internal::min_size_prefer_fixed(Size, MaxSize) + (ForceAlignment ? EIGEN_MAX_ALIGN_BYTES : 0), 0> + m_data; + EIGEN_STRONG_INLINE Scalar* data() { + return ForceAlignment + ? reinterpret_cast((std::uintptr_t(m_data.array) & ~(std::size_t(EIGEN_MAX_ALIGN_BYTES - 1))) + + EIGEN_MAX_ALIGN_BYTES) + : m_data.array; + } +#endif +}; + +// The vector is on the left => transposition +template +struct gemv_dense_selector { + template + static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) { + Transpose destT(dest); + enum { OtherStorageOrder = StorageOrder == RowMajor ? ColMajor : RowMajor }; + gemv_dense_selector::run(rhs.transpose(), lhs.transpose(), destT, + alpha); + } +}; + +template <> +struct gemv_dense_selector { + template + static inline void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) { + typedef typename Lhs::Scalar LhsScalar; + typedef typename Rhs::Scalar RhsScalar; + typedef typename Dest::Scalar ResScalar; + + typedef internal::blas_traits LhsBlasTraits; + typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType; + typedef internal::blas_traits RhsBlasTraits; + typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType; + + typedef Map, plain_enum_min(AlignedMax, internal::packet_traits::size)> + MappedDest; + + ActualLhsType actualLhs = LhsBlasTraits::extract(lhs); + ActualRhsType actualRhs = RhsBlasTraits::extract(rhs); + + ResScalar actualAlpha = combine_scalar_factors(alpha, lhs, rhs); + + // make sure Dest is a compile-time vector type (bug 1166) + typedef std::conditional_t ActualDest; + + enum { + // FIXME find a way to allow an inner stride on the result if packet_traits::size==1 + // on, the other hand it is good for the cache to pack the vector anyways... + EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime == 1), + ComplexByReal = (NumTraits::IsComplex) && (!NumTraits::IsComplex), + MightCannotUseDest = ((!EvalToDestAtCompileTime) || ComplexByReal) && (ActualDest::MaxSizeAtCompileTime != 0) + }; + + typedef const_blas_data_mapper LhsMapper; + typedef const_blas_data_mapper RhsMapper; + RhsScalar compatibleAlpha = get_factor::run(actualAlpha); + + if (!MightCannotUseDest) { + // shortcut if we are sure to be able to use dest directly, + // this ease the compiler to generate cleaner and more optimzized code for most common cases + general_matrix_vector_product::run(actualLhs.rows(), actualLhs.cols(), + LhsMapper(actualLhs.data(), + actualLhs.outerStride()), + RhsMapper(actualRhs.data(), + actualRhs.innerStride()), + dest.data(), 1, compatibleAlpha); + } else { + gemv_static_vector_if + static_dest; + + const bool alphaIsCompatible = (!ComplexByReal) || (numext::is_exactly_zero(numext::imag(actualAlpha))); + const bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible; + + ei_declare_aligned_stack_constructed_variable(ResScalar, actualDestPtr, dest.size(), + evalToDest ? dest.data() : static_dest.data()); + + if (!evalToDest) { +#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN + Index size = dest.size(); + EIGEN_DENSE_STORAGE_CTOR_PLUGIN +#endif + if (!alphaIsCompatible) { + MappedDest(actualDestPtr, dest.size()).setZero(); + compatibleAlpha = RhsScalar(1); + } else + MappedDest(actualDestPtr, dest.size()) = dest; + } + + general_matrix_vector_product::run(actualLhs.rows(), actualLhs.cols(), + LhsMapper(actualLhs.data(), + actualLhs.outerStride()), + RhsMapper(actualRhs.data(), + actualRhs.innerStride()), + actualDestPtr, 1, compatibleAlpha); + + if (!evalToDest) { + if (!alphaIsCompatible) + dest.matrix() += actualAlpha * MappedDest(actualDestPtr, dest.size()); + else + dest = MappedDest(actualDestPtr, dest.size()); + } + } + } +}; + +template <> +struct gemv_dense_selector { + template + static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) { + typedef typename Lhs::Scalar LhsScalar; + typedef typename Rhs::Scalar RhsScalar; + typedef typename Dest::Scalar ResScalar; + + typedef internal::blas_traits LhsBlasTraits; + typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType; + typedef internal::blas_traits RhsBlasTraits; + typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType; + typedef internal::remove_all_t ActualRhsTypeCleaned; + + std::add_const_t actualLhs = LhsBlasTraits::extract(lhs); + std::add_const_t actualRhs = RhsBlasTraits::extract(rhs); + + ResScalar actualAlpha = combine_scalar_factors(alpha, lhs, rhs); + + enum { + // FIXME find a way to allow an inner stride on the result if packet_traits::size==1 + // on, the other hand it is good for the cache to pack the vector anyways... + DirectlyUseRhs = + ActualRhsTypeCleaned::InnerStrideAtCompileTime == 1 || ActualRhsTypeCleaned::MaxSizeAtCompileTime == 0 + }; + + gemv_static_vector_if + static_rhs; + + ei_declare_aligned_stack_constructed_variable( + RhsScalar, actualRhsPtr, actualRhs.size(), + DirectlyUseRhs ? const_cast(actualRhs.data()) : static_rhs.data()); + + if (!DirectlyUseRhs) { +#ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN + Index size = actualRhs.size(); + EIGEN_DENSE_STORAGE_CTOR_PLUGIN +#endif + Map(actualRhsPtr, actualRhs.size()) = actualRhs; + } + + typedef const_blas_data_mapper LhsMapper; + typedef const_blas_data_mapper RhsMapper; + general_matrix_vector_product:: + run(actualLhs.rows(), actualLhs.cols(), LhsMapper(actualLhs.data(), actualLhs.outerStride()), + RhsMapper(actualRhsPtr, 1), dest.data(), + dest.col(0).innerStride(), // NOTE if dest is not a vector at compile-time, then dest.innerStride() might + // be wrong. (bug 1166) + actualAlpha); + } +}; + +template <> +struct gemv_dense_selector { + template + static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) { + EIGEN_STATIC_ASSERT((!nested_eval::Evaluate), + EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE); + // TODO if rhs is large enough it might be beneficial to make sure that dest is sequentially stored in memory, + // otherwise use a temp + typename nested_eval::type actual_rhs(rhs); + const Index size = rhs.rows(); + for (Index k = 0; k < size; ++k) dest += (alpha * actual_rhs.coeff(k)) * lhs.col(k); + } +}; + +template <> +struct gemv_dense_selector { + template + static void run(const Lhs& lhs, const Rhs& rhs, Dest& dest, const typename Dest::Scalar& alpha) { + EIGEN_STATIC_ASSERT((!nested_eval::Evaluate), + EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE); + typename nested_eval::type actual_rhs(rhs); + const Index rows = dest.rows(); + for (Index i = 0; i < rows; ++i) + dest.coeffRef(i) += alpha * (lhs.row(i).cwiseProduct(actual_rhs.transpose())).sum(); + } +}; + +} // end namespace internal + +/*************************************************************************** + * Implementation of matrix base methods + ***************************************************************************/ + +/** \returns the matrix product of \c *this and \a other. + * + * \note If instead of the matrix product you want the coefficient-wise product, see Cwise::operator*(). + * + * \sa lazyProduct(), operator*=(const MatrixBase&), Cwise::operator*() + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Product MatrixBase::operator*( + const MatrixBase& other) const { + // A note regarding the function declaration: In MSVC, this function will sometimes + // not be inlined since DenseStorage is an unwindable object for dynamic + // matrices and product types are holding a member to store the result. + // Thus it does not help tagging this function with EIGEN_STRONG_INLINE. + enum { + ProductIsValid = Derived::ColsAtCompileTime == Dynamic || OtherDerived::RowsAtCompileTime == Dynamic || + int(Derived::ColsAtCompileTime) == int(OtherDerived::RowsAtCompileTime), + AreVectors = Derived::IsVectorAtCompileTime && OtherDerived::IsVectorAtCompileTime, + SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived, OtherDerived) + }; + // note to the lost user: + // * for a dot product use: v1.dot(v2) + // * for a coeff-wise product use: v1.cwiseProduct(v2) + EIGEN_STATIC_ASSERT( + ProductIsValid || !(AreVectors && SameSizes), + INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS) + EIGEN_STATIC_ASSERT(ProductIsValid || !(SameSizes && !AreVectors), + INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION) + EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT) +#ifdef EIGEN_DEBUG_PRODUCT + internal::product_type::debug(); +#endif + + return Product(derived(), other.derived()); +} + +/** \returns an expression of the matrix product of \c *this and \a other without implicit evaluation. + * + * The returned product will behave like any other expressions: the coefficients of the product will be + * computed once at a time as requested. This might be useful in some extremely rare cases when only + * a small and no coherent fraction of the result's coefficients have to be computed. + * + * \warning This version of the matrix product can be much much slower. So use it only if you know + * what you are doing and that you measured a true speed improvement. + * + * \sa operator*(const MatrixBase&) + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Product +MatrixBase::lazyProduct(const MatrixBase& other) const { + enum { + ProductIsValid = Derived::ColsAtCompileTime == Dynamic || OtherDerived::RowsAtCompileTime == Dynamic || + int(Derived::ColsAtCompileTime) == int(OtherDerived::RowsAtCompileTime), + AreVectors = Derived::IsVectorAtCompileTime && OtherDerived::IsVectorAtCompileTime, + SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived, OtherDerived) + }; + // note to the lost user: + // * for a dot product use: v1.dot(v2) + // * for a coeff-wise product use: v1.cwiseProduct(v2) + EIGEN_STATIC_ASSERT( + ProductIsValid || !(AreVectors && SameSizes), + INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS) + EIGEN_STATIC_ASSERT(ProductIsValid || !(SameSizes && !AreVectors), + INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION) + EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT) + + return Product(derived(), other.derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_PRODUCT_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h new file mode 100644 index 00000000..59363366 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h @@ -0,0 +1,1487 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// 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_GENERIC_PACKET_MATH_H +#define EIGEN_GENERIC_PACKET_MATH_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +/** \internal + * \file GenericPacketMath.h + * + * Default implementation for types not supported by the vectorization. + * In practice these functions are provided to make easier the writing + * of generic vectorized code. + */ + +#ifndef EIGEN_DEBUG_ALIGNED_LOAD +#define EIGEN_DEBUG_ALIGNED_LOAD +#endif + +#ifndef EIGEN_DEBUG_UNALIGNED_LOAD +#define EIGEN_DEBUG_UNALIGNED_LOAD +#endif + +#ifndef EIGEN_DEBUG_ALIGNED_STORE +#define EIGEN_DEBUG_ALIGNED_STORE +#endif + +#ifndef EIGEN_DEBUG_UNALIGNED_STORE +#define EIGEN_DEBUG_UNALIGNED_STORE +#endif + +struct default_packet_traits { + enum { + HasAdd = 1, + HasSub = 1, + HasShift = 1, + HasMul = 1, + HasNegate = 1, + HasAbs = 1, + HasArg = 0, + HasAbs2 = 1, + HasAbsDiff = 0, + HasMin = 1, + HasMax = 1, + HasConj = 1, + HasSetLinear = 1, + HasSign = 1, + HasBlend = 0, + // This flag is used to indicate whether packet comparison is supported. + // pcmp_eq, pcmp_lt and pcmp_le should be defined for it to be true. + HasCmp = 0, + + HasDiv = 0, + HasReciprocal = 0, + HasSqrt = 0, + HasRsqrt = 0, + HasExp = 0, + HasExpm1 = 0, + HasLog = 0, + HasLog1p = 0, + HasLog10 = 0, + HasPow = 0, + + HasSin = 0, + HasCos = 0, + HasTan = 0, + HasASin = 0, + HasACos = 0, + HasATan = 0, + HasATanh = 0, + HasSinh = 0, + HasCosh = 0, + HasTanh = 0, + HasLGamma = 0, + HasDiGamma = 0, + HasZeta = 0, + HasPolygamma = 0, + HasErf = 0, + HasErfc = 0, + HasNdtri = 0, + HasBessel = 0, + HasIGamma = 0, + HasIGammaDerA = 0, + HasGammaSampleDerAlpha = 0, + HasIGammac = 0, + HasBetaInc = 0, + + HasRound = 0, + HasRint = 0, + HasFloor = 0, + HasCeil = 0 + }; +}; + +template +struct packet_traits : default_packet_traits { + typedef T type; + typedef T half; + enum { + Vectorizable = 0, + size = 1, + AlignedOnScalar = 0, + }; + enum { + HasAdd = 0, + HasSub = 0, + HasMul = 0, + HasNegate = 0, + HasAbs = 0, + HasAbs2 = 0, + HasMin = 0, + HasMax = 0, + HasConj = 0, + HasSetLinear = 0 + }; +}; + +template +struct packet_traits : packet_traits {}; + +template +struct unpacket_traits { + typedef T type; + typedef T half; + enum { size = 1, alignment = 1, vectorizable = false, masked_load_available = false, masked_store_available = false }; +}; + +template +struct unpacket_traits : unpacket_traits {}; + +/** \internal A convenience utility for determining if the type is a scalar. + * This is used to enable some generic packet implementations. + */ +template +struct is_scalar { + using Scalar = typename unpacket_traits::type; + enum { value = internal::is_same::value }; +}; + +// automatically and succinctly define combinations of pcast when +// 1) the packets are the same type, or +// 2) the packets differ only in sign. +// In both of these cases, preinterpret (bit_cast) is equivalent to pcast (static_cast) +template ::value && is_scalar::value> +struct is_degenerate_helper : is_same {}; +template <> +struct is_degenerate_helper : std::true_type {}; +template <> +struct is_degenerate_helper : std::true_type {}; +template <> +struct is_degenerate_helper : std::true_type {}; +template <> +struct is_degenerate_helper : std::true_type {}; + +template +struct is_degenerate_helper { + using SrcScalar = typename unpacket_traits::type; + static constexpr int SrcSize = unpacket_traits::size; + using TgtScalar = typename unpacket_traits::type; + static constexpr int TgtSize = unpacket_traits::size; + static constexpr bool value = is_degenerate_helper::value && (SrcSize == TgtSize); +}; + +// is_degenerate::value == is_degenerate::value +template +struct is_degenerate { + static constexpr bool value = + is_degenerate_helper::value || is_degenerate_helper::value; +}; + +template +struct is_half { + using Scalar = typename unpacket_traits::type; + static constexpr int Size = unpacket_traits::size; + using DefaultPacket = typename packet_traits::type; + static constexpr int DefaultSize = unpacket_traits::size; + static constexpr bool value = Size < DefaultSize; +}; + +template +struct type_casting_traits { + enum { + VectorizedCast = + is_degenerate::value && packet_traits::Vectorizable && packet_traits::Vectorizable, + SrcCoeffRatio = 1, + TgtCoeffRatio = 1 + }; +}; + +// provides a succint template to define vectorized casting traits with respect to the largest accessible packet types +template +struct vectorized_type_casting_traits { + enum : int { + DefaultSrcPacketSize = packet_traits::size, + DefaultTgtPacketSize = packet_traits::size, + VectorizedCast = 1, + SrcCoeffRatio = plain_enum_max(DefaultTgtPacketSize / DefaultSrcPacketSize, 1), + TgtCoeffRatio = plain_enum_max(DefaultSrcPacketSize / DefaultTgtPacketSize, 1) + }; +}; + +/** \internal Wrapper to ensure that multiple packet types can map to the same + same underlying vector type. */ +template +struct eigen_packet_wrapper { + EIGEN_ALWAYS_INLINE operator T&() { return m_val; } + EIGEN_ALWAYS_INLINE operator const T&() const { return m_val; } + EIGEN_ALWAYS_INLINE eigen_packet_wrapper() = default; + EIGEN_ALWAYS_INLINE eigen_packet_wrapper(const T& v) : m_val(v) {} + EIGEN_ALWAYS_INLINE eigen_packet_wrapper& operator=(const T& v) { + m_val = v; + return *this; + } + + T m_val; +}; + +template ::value> +struct preinterpret_generic; + +template +struct preinterpret_generic { + // the packets are not the same, attempt scalar bit_cast + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Target run(const Packet& a) { + return numext::bit_cast(a); + } +}; + +template +struct preinterpret_generic { + // the packets are the same type: do nothing + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& a) { return a; } +}; + +/** \internal \returns reinterpret_cast(a) */ +template +EIGEN_DEVICE_FUNC inline Target preinterpret(const Packet& a) { + return preinterpret_generic::run(a); +} + +template ::value, + bool TgtIsHalf = is_half::value> +struct pcast_generic; + +template +struct pcast_generic { + // the packets are not degenerate: attempt scalar static_cast + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket run(const SrcPacket& a) { + return cast_impl::run(a); + } +}; + +template +struct pcast_generic { + // the packets are the same: do nothing + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& a) { return a; } +}; + +template +struct pcast_generic { + // the packets are degenerate: preinterpret is equivalent to pcast + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket run(const SrcPacket& a) { return preinterpret(a); } +}; + +/** \internal \returns static_cast(a) (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a) { + return pcast_generic::run(a); +} +template +EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a, const SrcPacket& b) { + return pcast_generic::run(a, b); +} +template +EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a, const SrcPacket& b, const SrcPacket& c, + const SrcPacket& d) { + return pcast_generic::run(a, b, c, d); +} +template +EIGEN_DEVICE_FUNC inline TgtPacket pcast(const SrcPacket& a, const SrcPacket& b, const SrcPacket& c, const SrcPacket& d, + const SrcPacket& e, const SrcPacket& f, const SrcPacket& g, + const SrcPacket& h) { + return pcast_generic::run(a, b, c, d, e, f, g, h); +} + +template +struct pcast_generic { + // TgtPacket is a half packet of some other type + // perform cast and truncate result + using DefaultTgtPacket = typename is_half::DefaultPacket; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket run(const SrcPacket& a) { + return preinterpret(pcast(a)); + } +}; + +/** \internal \returns a + b (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet padd(const Packet& a, const Packet& b) { + return a + b; +} +// Avoid compiler warning for boolean algebra. +template <> +EIGEN_DEVICE_FUNC inline bool padd(const bool& a, const bool& b) { + return a || b; +} + +/** \internal \returns a packet version of \a *from, (un-aligned masked add) + * There is no generic implementation. We only have implementations for specialized + * cases. Generic case should not be called. + */ +template +EIGEN_DEVICE_FUNC inline std::enable_if_t::masked_fpops_available, Packet> padd( + const Packet& a, const Packet& b, typename unpacket_traits::mask_t umask); + +/** \internal \returns a - b (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet psub(const Packet& a, const Packet& b) { + return a - b; +} + +/** \internal \returns -a (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pnegate(const Packet& a) { + return -a; +} + +template <> +EIGEN_DEVICE_FUNC inline bool pnegate(const bool& a) { + return !a; +} + +/** \internal \returns conj(a) (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pconj(const Packet& a) { + return numext::conj(a); +} + +/** \internal \returns a * b (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pmul(const Packet& a, const Packet& b) { + return a * b; +} +// Avoid compiler warning for boolean algebra. +template <> +EIGEN_DEVICE_FUNC inline bool pmul(const bool& a, const bool& b) { + return a && b; +} + +/** \internal \returns a / b (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pdiv(const Packet& a, const Packet& b) { + return a / b; +} + +// In the generic case, memset to all one bits. +template +struct ptrue_impl { + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/) { + Packet b; + memset(static_cast(&b), 0xff, sizeof(Packet)); + return b; + } +}; + +// For non-trivial scalars, set to Scalar(1) (i.e. a non-zero value). +// Although this is technically not a valid bitmask, the scalar path for pselect +// uses a comparison to zero, so this should still work in most cases. We don't +// have another option, since the scalar type requires initialization. +template +struct ptrue_impl::value && NumTraits::RequireInitialization>> { + static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/) { return T(1); } +}; + +/** \internal \returns one bits. */ +template +EIGEN_DEVICE_FUNC inline Packet ptrue(const Packet& a) { + return ptrue_impl::run(a); +} + +// In the general case, memset to zero. +template +struct pzero_impl { + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/) { + Packet b; + memset(static_cast(&b), 0x00, sizeof(Packet)); + return b; + } +}; + +// For scalars, explicitly set to Scalar(0), since the underlying representation +// for zero may not consist of all-zero bits. +template +struct pzero_impl::value>> { + static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/) { return T(0); } +}; + +/** \internal \returns packet of zeros */ +template +EIGEN_DEVICE_FUNC inline Packet pzero(const Packet& a) { + return pzero_impl::run(a); +} + +/** \internal \returns a <= b as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_le(const Packet& a, const Packet& b) { + return a <= b ? ptrue(a) : pzero(a); +} + +/** \internal \returns a < b as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_lt(const Packet& a, const Packet& b) { + return a < b ? ptrue(a) : pzero(a); +} + +/** \internal \returns a == b as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_eq(const Packet& a, const Packet& b) { + return a == b ? ptrue(a) : pzero(a); +} + +/** \internal \returns a < b or a==NaN or b==NaN as a bit mask */ +template +EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) { + return a >= b ? pzero(a) : ptrue(a); +} + +template +struct bit_and { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a & b; } +}; + +template +struct bit_or { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a | b; } +}; + +template +struct bit_xor { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a ^ b; } +}; + +template +struct bit_not { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a) const { return ~a; } +}; + +// Use operators &, |, ^, ~. +template +struct operator_bitwise_helper { + EIGEN_DEVICE_FUNC static inline T bitwise_and(const T& a, const T& b) { return bit_and()(a, b); } + EIGEN_DEVICE_FUNC static inline T bitwise_or(const T& a, const T& b) { return bit_or()(a, b); } + EIGEN_DEVICE_FUNC static inline T bitwise_xor(const T& a, const T& b) { return bit_xor()(a, b); } + EIGEN_DEVICE_FUNC static inline T bitwise_not(const T& a) { return bit_not()(a); } +}; + +// Apply binary operations byte-by-byte +template +struct bytewise_bitwise_helper { + EIGEN_DEVICE_FUNC static inline T bitwise_and(const T& a, const T& b) { + return binary(a, b, bit_and()); + } + EIGEN_DEVICE_FUNC static inline T bitwise_or(const T& a, const T& b) { return binary(a, b, bit_or()); } + EIGEN_DEVICE_FUNC static inline T bitwise_xor(const T& a, const T& b) { + return binary(a, b, bit_xor()); + } + EIGEN_DEVICE_FUNC static inline T bitwise_not(const T& a) { return unary(a, bit_not()); } + + private: + template + EIGEN_DEVICE_FUNC static inline T unary(const T& a, Op op) { + const unsigned char* a_ptr = reinterpret_cast(&a); + T c; + unsigned char* c_ptr = reinterpret_cast(&c); + for (size_t i = 0; i < sizeof(T); ++i) { + *c_ptr++ = op(*a_ptr++); + } + return c; + } + + template + EIGEN_DEVICE_FUNC static inline T binary(const T& a, const T& b, Op op) { + const unsigned char* a_ptr = reinterpret_cast(&a); + const unsigned char* b_ptr = reinterpret_cast(&b); + T c; + unsigned char* c_ptr = reinterpret_cast(&c); + for (size_t i = 0; i < sizeof(T); ++i) { + *c_ptr++ = op(*a_ptr++, *b_ptr++); + } + return c; + } +}; + +// In the general case, use byte-by-byte manipulation. +template +struct bitwise_helper : public bytewise_bitwise_helper {}; + +// For integers or non-trivial scalars, use binary operators. +template +struct bitwise_helper::value && + (NumTraits::IsInteger || NumTraits::RequireInitialization)>> + : public operator_bitwise_helper {}; + +/** \internal \returns the bitwise and of \a a and \a b */ +template +EIGEN_DEVICE_FUNC inline Packet pand(const Packet& a, const Packet& b) { + return bitwise_helper::bitwise_and(a, b); +} + +/** \internal \returns the bitwise or of \a a and \a b */ +template +EIGEN_DEVICE_FUNC inline Packet por(const Packet& a, const Packet& b) { + return bitwise_helper::bitwise_or(a, b); +} + +/** \internal \returns the bitwise xor of \a a and \a b */ +template +EIGEN_DEVICE_FUNC inline Packet pxor(const Packet& a, const Packet& b) { + return bitwise_helper::bitwise_xor(a, b); +} + +/** \internal \returns the bitwise not of \a a */ +template +EIGEN_DEVICE_FUNC inline Packet pnot(const Packet& a) { + return bitwise_helper::bitwise_not(a); +} + +/** \internal \returns the bitwise and of \a a and not \a b */ +template +EIGEN_DEVICE_FUNC inline Packet pandnot(const Packet& a, const Packet& b) { + return pand(a, pnot(b)); +} + +/** \internal \returns isnan(a) */ +template +EIGEN_DEVICE_FUNC inline Packet pisnan(const Packet& a) { + return pandnot(ptrue(a), pcmp_eq(a, a)); +} + +// In the general case, use bitwise select. +template +struct pselect_impl { + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) { + return por(pand(a, mask), pandnot(b, mask)); + } +}; + +// For scalars, use ternary select. +template +struct pselect_impl::value>> { + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) { + return numext::equal_strict(mask, Packet(0)) ? b : a; + } +}; + +/** \internal \returns \a or \b for each field in packet according to \mask */ +template +EIGEN_DEVICE_FUNC inline Packet pselect(const Packet& mask, const Packet& a, const Packet& b) { + return pselect_impl::run(mask, a, b); +} + +template <> +EIGEN_DEVICE_FUNC inline bool pselect(const bool& cond, const bool& a, const bool& b) { + return cond ? a : b; +} + +/** \internal \returns the min or of \a a and \a b (coeff-wise) + If either \a a or \a b are NaN, the result is implementation defined. */ +template +struct pminmax_impl { + template + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) { + return op(a, b); + } +}; + +/** \internal \returns the min or max of \a a and \a b (coeff-wise) + If either \a a or \a b are NaN, NaN is returned. */ +template <> +struct pminmax_impl { + template + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) { + Packet not_nan_mask_a = pcmp_eq(a, a); + Packet not_nan_mask_b = pcmp_eq(b, b); + return pselect(not_nan_mask_a, pselect(not_nan_mask_b, op(a, b), b), a); + } +}; + +/** \internal \returns the min or max of \a a and \a b (coeff-wise) + If both \a a and \a b are NaN, NaN is returned. + Equivalent to std::fmin(a, b). */ +template <> +struct pminmax_impl { + template + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) { + Packet not_nan_mask_a = pcmp_eq(a, a); + Packet not_nan_mask_b = pcmp_eq(b, b); + return pselect(not_nan_mask_a, pselect(not_nan_mask_b, op(a, b), a), b); + } +}; + +#ifndef SYCL_DEVICE_ONLY +#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) Func +#else +#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) [](const Type& a, const Type& b) { return Func(a, b); } +#endif + +/** \internal \returns the min of \a a and \a b (coeff-wise). + If \a a or \b b is NaN, the return value is implementation defined. */ +template +EIGEN_DEVICE_FUNC inline Packet pmin(const Packet& a, const Packet& b) { + return numext::mini(a, b); +} + +/** \internal \returns the min of \a a and \a b (coeff-wise). + NaNPropagation determines the NaN propagation semantics. */ +template +EIGEN_DEVICE_FUNC inline Packet pmin(const Packet& a, const Packet& b) { + return pminmax_impl::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmin))); +} + +/** \internal \returns the max of \a a and \a b (coeff-wise) + If \a a or \b b is NaN, the return value is implementation defined. */ +template +EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, const Packet& b) { + return numext::maxi(a, b); +} + +/** \internal \returns the max of \a a and \a b (coeff-wise). + NaNPropagation determines the NaN propagation semantics. */ +template +EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, const Packet& b) { + return pminmax_impl::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmax))); +} + +/** \internal \returns the absolute value of \a a */ +template +EIGEN_DEVICE_FUNC inline Packet pabs(const Packet& a) { + return numext::abs(a); +} +template <> +EIGEN_DEVICE_FUNC inline unsigned int pabs(const unsigned int& a) { + return a; +} +template <> +EIGEN_DEVICE_FUNC inline unsigned long pabs(const unsigned long& a) { + return a; +} +template <> +EIGEN_DEVICE_FUNC inline unsigned long long pabs(const unsigned long long& a) { + return a; +} + +/** \internal \returns the addsub value of \a a,b */ +template +EIGEN_DEVICE_FUNC inline Packet paddsub(const Packet& a, const Packet& b) { + return pselect(peven_mask(a), padd(a, b), psub(a, b)); +} + +/** \internal \returns the phase angle of \a a */ +template +EIGEN_DEVICE_FUNC inline Packet parg(const Packet& a) { + using numext::arg; + return arg(a); +} + +/** \internal \returns \a a arithmetically shifted by N bits to the right */ +template +EIGEN_DEVICE_FUNC inline int parithmetic_shift_right(const int& a) { + return a >> N; +} +template +EIGEN_DEVICE_FUNC inline long int parithmetic_shift_right(const long int& a) { + return a >> N; +} + +/** \internal \returns \a a logically shifted by N bits to the right */ +template +EIGEN_DEVICE_FUNC inline int plogical_shift_right(const int& a) { + return static_cast(static_cast(a) >> N); +} +template +EIGEN_DEVICE_FUNC inline long int plogical_shift_right(const long int& a) { + return static_cast(static_cast(a) >> N); +} + +/** \internal \returns \a a shifted by N bits to the left */ +template +EIGEN_DEVICE_FUNC inline int plogical_shift_left(const int& a) { + return a << N; +} +template +EIGEN_DEVICE_FUNC inline long int plogical_shift_left(const long int& a) { + return a << N; +} + +/** \internal \returns the significant and exponent of the underlying floating point numbers + * See https://en.cppreference.com/w/cpp/numeric/math/frexp + */ +template +EIGEN_DEVICE_FUNC inline Packet pfrexp(const Packet& a, Packet& exponent) { + int exp; + EIGEN_USING_STD(frexp); + Packet result = static_cast(frexp(a, &exp)); + exponent = static_cast(exp); + return result; +} + +/** \internal \returns a * 2^((int)exponent) + * See https://en.cppreference.com/w/cpp/numeric/math/ldexp + */ +template +EIGEN_DEVICE_FUNC inline Packet pldexp(const Packet& a, const Packet& exponent) { + EIGEN_USING_STD(ldexp) + return static_cast(ldexp(a, static_cast(exponent))); +} + +/** \internal \returns the min of \a a and \a b (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pabsdiff(const Packet& a, const Packet& b) { + return pselect(pcmp_lt(a, b), psub(b, a), psub(a, b)); +} + +/** \internal \returns a packet version of \a *from, from must be properly aligned */ +template +EIGEN_DEVICE_FUNC inline Packet pload(const typename unpacket_traits::type* from) { + return *from; +} + +/** \internal \returns n elements of a packet version of \a *from, from must be properly aligned + * offset indicates the starting element in which to load and + * offset + n <= unpacket_traits::size + * All elements before offset and after the last element loaded will initialized with zero */ +template +EIGEN_DEVICE_FUNC inline Packet pload_partial(const typename unpacket_traits::type* from, const Index n, + const Index offset = 0) { + const Index packet_size = unpacket_traits::size; + eigen_assert(n + offset <= packet_size && "number of elements plus offset will read past end of packet"); + typedef typename unpacket_traits::type Scalar; + EIGEN_ALIGN_MAX Scalar elements[packet_size] = {Scalar(0)}; + for (Index i = offset; i < numext::mini(n + offset, packet_size); i++) { + elements[i] = from[i - offset]; + } + return pload(elements); +} + +/** \internal \returns a packet version of \a *from, (un-aligned load) */ +template +EIGEN_DEVICE_FUNC inline Packet ploadu(const typename unpacket_traits::type* from) { + return *from; +} + +/** \internal \returns n elements of a packet version of \a *from, (un-aligned load) + * All elements after the last element loaded will initialized with zero */ +template +EIGEN_DEVICE_FUNC inline Packet ploadu_partial(const typename unpacket_traits::type* from, const Index n, + const Index offset = 0) { + const Index packet_size = unpacket_traits::size; + eigen_assert(n + offset <= packet_size && "number of elements plus offset will read past end of packet"); + typedef typename unpacket_traits::type Scalar; + EIGEN_ALIGN_MAX Scalar elements[packet_size] = {Scalar(0)}; + for (Index i = offset; i < numext::mini(n + offset, packet_size); i++) { + elements[i] = from[i - offset]; + } + return pload(elements); +} + +/** \internal \returns a packet version of \a *from, (un-aligned masked load) + * There is no generic implementation. We only have implementations for specialized + * cases. Generic case should not be called. + */ +template +EIGEN_DEVICE_FUNC inline std::enable_if_t::masked_load_available, Packet> ploadu( + const typename unpacket_traits::type* from, typename unpacket_traits::mask_t umask); + +/** \internal \returns a packet with constant coefficients \a a, e.g.: (a,a,a,a) */ +template +EIGEN_DEVICE_FUNC inline Packet pset1(const typename unpacket_traits::type& a) { + return a; +} + +/** \internal \returns a packet with constant coefficients set from bits */ +template +EIGEN_DEVICE_FUNC inline Packet pset1frombits(BitsType a); + +/** \internal \returns a packet with constant coefficients \a a[0], e.g.: (a[0],a[0],a[0],a[0]) */ +template +EIGEN_DEVICE_FUNC inline Packet pload1(const typename unpacket_traits::type* a) { + return pset1(*a); +} + +/** \internal \returns a packet with elements of \a *from duplicated. + * For instance, for a packet of 8 elements, 4 scalars will be read from \a *from and + * duplicated to form: {from[0],from[0],from[1],from[1],from[2],from[2],from[3],from[3]} + * Currently, this function is only used for scalar * complex products. + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet ploaddup(const typename unpacket_traits::type* from) { + return *from; +} + +/** \internal \returns a packet with elements of \a *from quadrupled. + * For instance, for a packet of 8 elements, 2 scalars will be read from \a *from and + * replicated to form: {from[0],from[0],from[0],from[0],from[1],from[1],from[1],from[1]} + * Currently, this function is only used in matrix products. + * For packet-size smaller or equal to 4, this function is equivalent to pload1 + */ +template +EIGEN_DEVICE_FUNC inline Packet ploadquad(const typename unpacket_traits::type* from) { + return pload1(from); +} + +/** \internal equivalent to + * \code + * a0 = pload1(a+0); + * a1 = pload1(a+1); + * a2 = pload1(a+2); + * a3 = pload1(a+3); + * \endcode + * \sa pset1, pload1, ploaddup, pbroadcast2 + */ +template +EIGEN_DEVICE_FUNC inline void pbroadcast4(const typename unpacket_traits::type* a, Packet& a0, Packet& a1, + Packet& a2, Packet& a3) { + a0 = pload1(a + 0); + a1 = pload1(a + 1); + a2 = pload1(a + 2); + a3 = pload1(a + 3); +} + +/** \internal equivalent to + * \code + * a0 = pload1(a+0); + * a1 = pload1(a+1); + * \endcode + * \sa pset1, pload1, ploaddup, pbroadcast4 + */ +template +EIGEN_DEVICE_FUNC inline void pbroadcast2(const typename unpacket_traits::type* a, Packet& a0, Packet& a1) { + a0 = pload1(a + 0); + a1 = pload1(a + 1); +} + +/** \internal \brief Returns a packet with coefficients (a,a+1,...,a+packet_size-1). */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet plset(const typename unpacket_traits::type& a) { + return a; +} + +/** \internal \returns a packet with constant coefficients \a a, e.g.: (x, 0, x, 0), + where x is the value of all 1-bits. */ +template +EIGEN_DEVICE_FUNC inline Packet peven_mask(const Packet& /*a*/) { + typedef typename unpacket_traits::type Scalar; + const size_t n = unpacket_traits::size; + EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n]; + for (size_t i = 0; i < n; ++i) { + memset(elements + i, ((i & 1) == 0 ? 0xff : 0), sizeof(Scalar)); + } + return ploadu(elements); +} + +/** \internal copy the packet \a from to \a *to, \a to must be properly aligned */ +template +EIGEN_DEVICE_FUNC inline void pstore(Scalar* to, const Packet& from) { + (*to) = from; +} + +/** \internal copy n elements of the packet \a from to \a *to, \a to must be properly aligned + * offset indicates the starting element in which to store and + * offset + n <= unpacket_traits::size */ +template +EIGEN_DEVICE_FUNC inline void pstore_partial(Scalar* to, const Packet& from, const Index n, const Index offset = 0) { + const Index packet_size = unpacket_traits::size; + eigen_assert(n + offset <= packet_size && "number of elements plus offset will write past end of packet"); + EIGEN_ALIGN_MAX Scalar elements[packet_size]; + pstore(elements, from); + for (Index i = 0; i < numext::mini(n, packet_size - offset); i++) { + to[i] = elements[i + offset]; + } +} + +/** \internal copy the packet \a from to \a *to, (un-aligned store) */ +template +EIGEN_DEVICE_FUNC inline void pstoreu(Scalar* to, const Packet& from) { + (*to) = from; +} + +/** \internal copy n elements of the packet \a from to \a *to, (un-aligned store) */ +template +EIGEN_DEVICE_FUNC inline void pstoreu_partial(Scalar* to, const Packet& from, const Index n, const Index offset = 0) { + const Index packet_size = unpacket_traits::size; + eigen_assert(n + offset <= packet_size && "number of elements plus offset will write past end of packet"); + EIGEN_ALIGN_MAX Scalar elements[packet_size]; + pstore(elements, from); + for (Index i = 0; i < numext::mini(n, packet_size - offset); i++) { + to[i] = elements[i + offset]; + } +} + +/** \internal copy the packet \a from to \a *to, (un-aligned store with a mask) + * There is no generic implementation. We only have implementations for specialized + * cases. Generic case should not be called. + */ +template +EIGEN_DEVICE_FUNC inline std::enable_if_t::masked_store_available, void> pstoreu( + Scalar* to, const Packet& from, typename unpacket_traits::mask_t umask); + +template +EIGEN_DEVICE_FUNC inline Packet pgather(const Scalar* from, Index /*stride*/) { + return ploadu(from); +} + +template +EIGEN_DEVICE_FUNC inline Packet pgather_partial(const Scalar* from, Index stride, const Index n) { + const Index packet_size = unpacket_traits::size; + EIGEN_ALIGN_MAX Scalar elements[packet_size] = {Scalar(0)}; + for (Index i = 0; i < numext::mini(n, packet_size); i++) { + elements[i] = from[i * stride]; + } + return pload(elements); +} + +template +EIGEN_DEVICE_FUNC inline void pscatter(Scalar* to, const Packet& from, Index /*stride*/) { + pstore(to, from); +} + +template +EIGEN_DEVICE_FUNC inline void pscatter_partial(Scalar* to, const Packet& from, Index stride, const Index n) { + const Index packet_size = unpacket_traits::size; + EIGEN_ALIGN_MAX Scalar elements[packet_size]; + pstore(elements, from); + for (Index i = 0; i < numext::mini(n, packet_size); i++) { + to[i * stride] = elements[i]; + } +} + +/** \internal tries to do cache prefetching of \a addr */ +template +EIGEN_DEVICE_FUNC inline void prefetch(const Scalar* addr) { +#if defined(EIGEN_HIP_DEVICE_COMPILE) + // do nothing +#elif defined(EIGEN_CUDA_ARCH) +#if defined(__LP64__) || EIGEN_OS_WIN64 + // 64-bit pointer operand constraint for inlined asm + asm(" prefetch.L1 [ %1 ];" : "=l"(addr) : "l"(addr)); +#else + // 32-bit pointer operand constraint for inlined asm + asm(" prefetch.L1 [ %1 ];" : "=r"(addr) : "r"(addr)); +#endif +#elif (!EIGEN_COMP_MSVC) && (EIGEN_COMP_GNUC || EIGEN_COMP_CLANG || EIGEN_COMP_ICC) + __builtin_prefetch(addr); +#endif +} + +/** \internal \returns the reversed elements of \a a*/ +template +EIGEN_DEVICE_FUNC inline Packet preverse(const Packet& a) { + return a; +} + +/** \internal \returns \a a with real and imaginary part flipped (for complex type only) */ +template +EIGEN_DEVICE_FUNC inline Packet pcplxflip(const Packet& a) { + return Packet(numext::imag(a), numext::real(a)); +} + +/************************** + * Special math functions + ***************************/ + +/** \internal \returns the sine of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psin(const Packet& a) { + EIGEN_USING_STD(sin); + return sin(a); +} + +/** \internal \returns the cosine of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcos(const Packet& a) { + EIGEN_USING_STD(cos); + return cos(a); +} + +/** \internal \returns the tan of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet ptan(const Packet& a) { + EIGEN_USING_STD(tan); + return tan(a); +} + +/** \internal \returns the arc sine of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pasin(const Packet& a) { + EIGEN_USING_STD(asin); + return asin(a); +} + +/** \internal \returns the arc cosine of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pacos(const Packet& a) { + EIGEN_USING_STD(acos); + return acos(a); +} + +/** \internal \returns the hyperbolic sine of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psinh(const Packet& a) { + EIGEN_USING_STD(sinh); + return sinh(a); +} + +/** \internal \returns the hyperbolic cosine of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcosh(const Packet& a) { + EIGEN_USING_STD(cosh); + return cosh(a); +} + +/** \internal \returns the arc tangent of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patan(const Packet& a) { + EIGEN_USING_STD(atan); + return atan(a); +} + +/** \internal \returns the hyperbolic tan of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet ptanh(const Packet& a) { + EIGEN_USING_STD(tanh); + return tanh(a); +} + +/** \internal \returns the arc tangent of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet patanh(const Packet& a) { + EIGEN_USING_STD(atanh); + return atanh(a); +} + +/** \internal \returns the exp of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp(const Packet& a) { + EIGEN_USING_STD(exp); + return exp(a); +} + +/** \internal \returns the expm1 of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexpm1(const Packet& a) { + return numext::expm1(a); +} + +/** \internal \returns the log of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog(const Packet& a) { + EIGEN_USING_STD(log); + return log(a); +} + +/** \internal \returns the log1p of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog1p(const Packet& a) { + return numext::log1p(a); +} + +/** \internal \returns the log10 of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog10(const Packet& a) { + EIGEN_USING_STD(log10); + return log10(a); +} + +/** \internal \returns the log10 of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog2(const Packet& a) { + typedef typename internal::unpacket_traits::type Scalar; + return pmul(pset1(Scalar(EIGEN_LOG2E)), plog(a)); +} + +/** \internal \returns the square-root of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psqrt(const Packet& a) { + return numext::sqrt(a); +} + +/** \internal \returns the cube-root of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcbrt(const Packet& a) { + return numext::cbrt(a); +} + +/** \internal \returns the rounded value of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pround(const Packet& a) { + using numext::round; + return round(a); +} + +/** \internal \returns the floor of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pfloor(const Packet& a) { + using numext::floor; + return floor(a); +} + +/** \internal \returns the rounded value of \a a (coeff-wise) with current + * rounding mode */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet print(const Packet& a) { + using numext::rint; + return rint(a); +} + +/** \internal \returns the ceil of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pceil(const Packet& a) { + using numext::ceil; + return ceil(a); +} + +template +struct psign_impl { + static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a) { return numext::sign(a); } +}; + +/** \internal \returns the sign of \a a (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet psign(const Packet& a) { + return psign_impl::run(a); +} + +template <> +EIGEN_DEVICE_FUNC inline bool psign(const bool& a) { + return a; +} + +/** \internal \returns the first element of a packet */ +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type pfirst(const Packet& a) { + return a; +} + +/** \internal \returns the sum of the elements of upper and lower half of \a a if \a a is larger than 4. + * For a packet {a0, a1, a2, a3, a4, a5, a6, a7}, it returns a half packet {a0+a4, a1+a5, a2+a6, a3+a7} + * For packet-size smaller or equal to 4, this boils down to a noop. + */ +template +EIGEN_DEVICE_FUNC inline std::conditional_t<(unpacket_traits::size % 8) == 0, + typename unpacket_traits::half, Packet> +predux_half_dowto4(const Packet& a) { + return a; +} + +// Slow generic implementation of Packet reduction. +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_helper(const Packet& a, Op op) { + typedef typename unpacket_traits::type Scalar; + const size_t n = unpacket_traits::size; + EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n]; + pstoreu(elements, a); + for (size_t k = n / 2; k > 0; k /= 2) { + for (size_t i = 0; i < k; ++i) { + elements[i] = op(elements[i], elements[i + k]); + } + } + return elements[0]; +} + +/** \internal \returns the sum of the elements of \a a*/ +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux(const Packet& a) { + return a; +} + +/** \internal \returns the product of the elements of \a a */ +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_mul(const Packet& a) { + typedef typename unpacket_traits::type Scalar; + return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmul))); +} + +/** \internal \returns the min of the elements of \a a */ +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_min(const Packet& a) { + typedef typename unpacket_traits::type Scalar; + return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin))); +} + +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_min(const Packet& a) { + typedef typename unpacket_traits::type Scalar; + return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin))); +} + +/** \internal \returns the min of the elements of \a a */ +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_max(const Packet& a) { + typedef typename unpacket_traits::type Scalar; + return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax))); +} + +template +EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_max(const Packet& a) { + typedef typename unpacket_traits::type Scalar; + return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax))); +} + +#undef EIGEN_BINARY_OP_NAN_PROPAGATION + +/** \internal \returns true if all coeffs of \a a means "true" + * It is supposed to be called on values returned by pcmp_*. + */ +// not needed yet +// template EIGEN_DEVICE_FUNC inline bool predux_all(const Packet& a) +// { return bool(a); } + +/** \internal \returns true if any coeffs of \a a means "true" + * It is supposed to be called on values returned by pcmp_*. + */ +template +EIGEN_DEVICE_FUNC inline bool predux_any(const Packet& a) { + // Dirty but generic implementation where "true" is assumed to be non 0 and all the sames. + // It is expected that "true" is either: + // - Scalar(1) + // - bits full of ones (NaN for floats), + // - or first bit equals to 1 (1 for ints, smallest denormal for floats). + // For all these cases, taking the sum is just fine, and this boils down to a no-op for scalars. + typedef typename unpacket_traits::type Scalar; + return numext::not_equal_strict(predux(a), Scalar(0)); +} + +/*************************************************************************** + * The following functions might not have to be overwritten for vectorized types + ***************************************************************************/ + +// FMA instructions. +/** \internal \returns a * b + c (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pmadd(const Packet& a, const Packet& b, const Packet& c) { + return padd(pmul(a, b), c); +} + +/** \internal \returns a * b - c (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pmsub(const Packet& a, const Packet& b, const Packet& c) { + return psub(pmul(a, b), c); +} + +/** \internal \returns -(a * b) + c (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pnmadd(const Packet& a, const Packet& b, const Packet& c) { + return padd(pnegate(pmul(a, b)), c); +} + +/** \internal \returns -(a * b) - c (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet pnmsub(const Packet& a, const Packet& b, const Packet& c) { + return psub(pnegate(pmul(a, b)), c); +} + +/** \internal copy a packet with constant coefficient \a a (e.g., [a,a,a,a]) to \a *to. \a to must be 16 bytes aligned + */ +// NOTE: this function must really be templated on the packet type (think about different packet types for the same +// scalar type) +template +inline void pstore1(typename unpacket_traits::type* to, const typename unpacket_traits::type& a) { + pstore(to, pset1(a)); +} + +/** \internal \returns a packet version of \a *from. + * The pointer \a from must be aligned on a \a Alignment bytes boundary. */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt(const typename unpacket_traits::type* from) { + if (Alignment >= unpacket_traits::alignment) + return pload(from); + else + return ploadu(from); +} + +/** \internal \returns n elements of a packet version of \a *from. + * The pointer \a from must be aligned on a \a Alignment bytes boundary. */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt_partial(const typename unpacket_traits::type* from, + const Index n, const Index offset = 0) { + if (Alignment >= unpacket_traits::alignment) + return pload_partial(from, n, offset); + else + return ploadu_partial(from, n, offset); +} + +/** \internal copy the packet \a from to \a *to. + * The pointer \a from must be aligned on a \a Alignment bytes boundary. */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void pstoret(Scalar* to, const Packet& from) { + if (Alignment >= unpacket_traits::alignment) + pstore(to, from); + else + pstoreu(to, from); +} + +/** \internal copy n elements of the packet \a from to \a *to. + * The pointer \a from must be aligned on a \a Alignment bytes boundary. */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void pstoret_partial(Scalar* to, const Packet& from, const Index n, + const Index offset = 0) { + if (Alignment >= unpacket_traits::alignment) + pstore_partial(to, from, n, offset); + else + pstoreu_partial(to, from, n, offset); +} + +/** \internal \returns a packet version of \a *from. + * Unlike ploadt, ploadt_ro takes advantage of the read-only memory path on the + * hardware if available to speedup the loading of data that won't be modified + * by the current computation. + */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt_ro(const typename unpacket_traits::type* from) { + return ploadt(from); +} + +/*************************************************************************** + * Fast complex products (GCC generates a function call which is very slow) + ***************************************************************************/ + +// Eigen+CUDA does not support complexes. +#if !defined(EIGEN_GPUCC) + +template <> +inline std::complex pmul(const std::complex& a, const std::complex& b) { + return std::complex(a.real() * b.real() - a.imag() * b.imag(), a.imag() * b.real() + a.real() * b.imag()); +} + +template <> +inline std::complex pmul(const std::complex& a, const std::complex& b) { + return std::complex(a.real() * b.real() - a.imag() * b.imag(), a.imag() * b.real() + a.real() * b.imag()); +} + +#endif + +/*************************************************************************** + * PacketBlock, that is a collection of N packets where the number of words + * in the packet is a multiple of N. + ***************************************************************************/ +template ::size> +struct PacketBlock { + Packet packet[N]; +}; + +template +EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock& /*kernel*/) { + // Nothing to do in the scalar case, i.e. a 1x1 matrix. +} + +/*************************************************************************** + * Selector, i.e. vector of N boolean values used to select (i.e. blend) + * words from 2 packets. + ***************************************************************************/ +template +struct Selector { + bool select[N]; +}; + +template +EIGEN_DEVICE_FUNC inline Packet pblend(const Selector::size>& ifPacket, + const Packet& thenPacket, const Packet& elsePacket) { + return ifPacket.select[0] ? thenPacket : elsePacket; +} + +/** \internal \returns 1 / a (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline Packet preciprocal(const Packet& a) { + using Scalar = typename unpacket_traits::type; + return pdiv(pset1(Scalar(1)), a); +} + +/** \internal \returns the reciprocal square-root of \a a (coeff-wise) */ +template +EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet prsqrt(const Packet& a) { + return preciprocal(psqrt(a)); +} + +template ::value, + bool IsInteger = NumTraits::type>::IsInteger> +struct psignbit_impl; +template +struct psignbit_impl { + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Packet run(const Packet& a) { return numext::signbit(a); } +}; +template +struct psignbit_impl { + // generic implementation if not specialized in PacketMath.h + // slower than arithmetic shift + typedef typename unpacket_traits::type Scalar; + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static Packet run(const Packet& a) { + const Packet cst_pos_one = pset1(Scalar(1)); + const Packet cst_neg_one = pset1(Scalar(-1)); + return pcmp_eq(por(pand(a, cst_neg_one), cst_pos_one), cst_neg_one); + } +}; +template +struct psignbit_impl { + // generic implementation for integer packets + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Packet run(const Packet& a) { return pcmp_lt(a, pzero(a)); } +}; +/** \internal \returns the sign bit of \a a as a bitmask*/ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE constexpr Packet psignbit(const Packet& a) { + return psignbit_impl::run(a); +} + +/** \internal \returns the 2-argument arc tangent of \a y and \a x (coeff-wise) */ +template ::value, int> = 0> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet patan2(const Packet& y, const Packet& x) { + return numext::atan2(y, x); +} + +/** \internal \returns the 2-argument arc tangent of \a y and \a x (coeff-wise) */ +template ::value, int> = 0> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet patan2(const Packet& y, const Packet& x) { + typedef typename internal::unpacket_traits::type Scalar; + + // See https://en.cppreference.com/w/cpp/numeric/math/atan2 + // for how corner cases are supposed to be handled according to the + // IEEE floating-point standard (IEC 60559). + const Packet kSignMask = pset1(-Scalar(0)); + const Packet kZero = pzero(x); + const Packet kOne = pset1(Scalar(1)); + const Packet kPi = pset1(Scalar(EIGEN_PI)); + + const Packet x_has_signbit = psignbit(x); + const Packet y_signmask = pand(y, kSignMask); + const Packet x_signmask = pand(x, kSignMask); + const Packet result_signmask = pxor(y_signmask, x_signmask); + const Packet shift = por(pand(x_has_signbit, kPi), y_signmask); + + const Packet x_and_y_are_same = pcmp_eq(pabs(x), pabs(y)); + const Packet x_and_y_are_zero = pcmp_eq(por(x, y), kZero); + + Packet arg = pdiv(y, x); + arg = pselect(x_and_y_are_same, por(kOne, result_signmask), arg); + arg = pselect(x_and_y_are_zero, result_signmask, arg); + + Packet result = patan(arg); + result = padd(result, shift); + return result; +} + +/** \internal \returns the argument of \a a as a complex number */ +template ::value, int> = 0> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pcarg(const Packet& a) { + return Packet(numext::arg(a)); +} + +/** \internal \returns the argument of \a a as a complex number */ +template ::value, int> = 0> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pcarg(const Packet& a) { + EIGEN_STATIC_ASSERT(NumTraits::type>::IsComplex, + THIS METHOD IS FOR COMPLEX TYPES ONLY) + using RealPacket = typename unpacket_traits::as_real; + // a // r i r i ... + RealPacket aflip = pcplxflip(a).v; // i r i r ... + RealPacket result = patan2(aflip, a.v); // atan2 crap atan2 crap ... + return (Packet)pand(result, peven_mask(result)); // atan2 0 atan2 0 ... +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_GENERIC_PACKET_MATH_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h new file mode 100644 index 00000000..f0ae5a85 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h @@ -0,0 +1,226 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010-2016 Gael Guennebaud +// Copyright (C) 2010 Benoit Jacob +// +// 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_GLOBAL_FUNCTIONS_H +#define EIGEN_GLOBAL_FUNCTIONS_H + +#ifdef EIGEN_PARSED_BY_DOXYGEN + +#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME, FUNCTOR, DOC_OP, DOC_DETAILS) \ + /** \returns an expression of the coefficient-wise DOC_OP of \a x \ + \ \ + DOC_DETAILS \ + \ \ + \sa Math functions, class CwiseUnaryOp \ + */ \ + template \ + inline const Eigen::CwiseUnaryOp, const Derived> NAME( \ + const Eigen::ArrayBase& x); + +#else + +#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME, FUNCTOR, DOC_OP, DOC_DETAILS) \ + template \ + inline const Eigen::CwiseUnaryOp, const Derived>(NAME)( \ + const Eigen::ArrayBase& x) { \ + return Eigen::CwiseUnaryOp, const Derived>(x.derived()); \ + } + +#endif // EIGEN_PARSED_BY_DOXYGEN + +#define EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(NAME, FUNCTOR) \ + \ + template \ + struct NAME##_retval > { \ + typedef const Eigen::CwiseUnaryOp, const Derived> type; \ + }; \ + template \ + struct NAME##_impl > { \ + static inline typename NAME##_retval >::type run(const Eigen::ArrayBase& x) { \ + return typename NAME##_retval >::type(x.derived()); \ + } \ + }; + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(real, scalar_real_op, real part,\sa ArrayBase::real) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(imag, scalar_imag_op, imaginary part,\sa ArrayBase::imag) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(conj, scalar_conjugate_op, complex conjugate,\sa ArrayBase::conjugate) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(inverse, scalar_inverse_op, inverse,\sa ArrayBase::inverse) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sin, scalar_sin_op, sine,\sa ArrayBase::sin) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cos, scalar_cos_op, cosine,\sa ArrayBase::cos) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tan, scalar_tan_op, tangent,\sa ArrayBase::tan) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atan, scalar_atan_op, arc - tangent,\sa ArrayBase::atan) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asin, scalar_asin_op, arc - sine,\sa ArrayBase::asin) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acos, scalar_acos_op, arc - consine,\sa ArrayBase::acos) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sinh, scalar_sinh_op, hyperbolic sine,\sa ArrayBase::sinh) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cosh, scalar_cosh_op, hyperbolic cosine,\sa ArrayBase::cosh) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tanh, scalar_tanh_op, hyperbolic tangent,\sa ArrayBase::tanh) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asinh, scalar_asinh_op, inverse hyperbolic sine,\sa ArrayBase::asinh) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acosh, scalar_acosh_op, inverse hyperbolic cosine,\sa ArrayBase::acosh) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atanh, scalar_atanh_op, inverse hyperbolic tangent,\sa ArrayBase::atanh) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(logistic, scalar_logistic_op, logistic function,\sa ArrayBase::logistic) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(lgamma, scalar_lgamma_op, + natural logarithm of the gamma function,\sa ArrayBase::lgamma) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(digamma, scalar_digamma_op, derivative of lgamma,\sa ArrayBase::digamma) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erf, scalar_erf_op, error function,\sa ArrayBase::erf) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erfc, scalar_erfc_op, complement error function,\sa ArrayBase::erfc) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(ndtri, scalar_ndtri_op, inverse normal distribution function,\sa ArrayBase::ndtri) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(exp, scalar_exp_op, exponential,\sa ArrayBase::exp) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(expm1, scalar_expm1_op, exponential of a value minus 1,\sa ArrayBase::expm1) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log, scalar_log_op, natural logarithm,\sa Eigen::log10 DOXCOMMA ArrayBase::log) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log1p, scalar_log1p_op, natural logarithm of 1 plus the value,\sa ArrayBase::log1p) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log10, scalar_log10_op, base 10 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log10) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log2, scalar_log2_op, base 2 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log2) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs, scalar_abs_op, absolute value,\sa ArrayBase::abs DOXCOMMA MatrixBase::cwiseAbs) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs2, scalar_abs2_op, + squared absolute value,\sa ArrayBase::abs2 DOXCOMMA MatrixBase::cwiseAbs2) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(arg, scalar_arg_op, complex argument,\sa ArrayBase::arg DOXCOMMA MatrixBase::cwiseArg) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(carg, scalar_carg_op, + complex argument, \sa ArrayBase::carg DOXCOMMA MatrixBase::cwiseCArg) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt, scalar_sqrt_op, square root,\sa ArrayBase::sqrt DOXCOMMA MatrixBase::cwiseSqrt) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cbrt, scalar_cbrt_op, cube root,\sa ArrayBase::cbrt DOXCOMMA MatrixBase::cwiseCbrt) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rsqrt, scalar_rsqrt_op, reciprocal square root,\sa ArrayBase::rsqrt) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(square, scalar_square_op, + square(power 2),\sa Eigen::abs2 DOXCOMMA Eigen::pow DOXCOMMA ArrayBase::square) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cube, scalar_cube_op, cube(power 3),\sa Eigen::pow DOXCOMMA ArrayBase::cube) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rint, scalar_rint_op, + nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(round, scalar_round_op, + nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY( + floor, scalar_floor_op, nearest integer not greater than the giben value,\sa Eigen::ceil DOXCOMMA ArrayBase::floor) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY( + ceil, scalar_ceil_op, nearest integer not less than the giben value,\sa Eigen::floor DOXCOMMA ArrayBase::ceil) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY( + isnan, scalar_isnan_op, not -a - number test,\sa Eigen::isinf DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isnan) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY( + isinf, scalar_isinf_op, infinite value test,\sa Eigen::isnan DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isinf) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isfinite, scalar_isfinite_op, + finite value test,\sa Eigen::isinf DOXCOMMA Eigen::isnan DOXCOMMA ArrayBase::isfinite) +EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sign, scalar_sign_op, sign(or 0),\sa ArrayBase::sign) + +template +using GlobalUnaryPowReturnType = std::enable_if_t< + !internal::is_arithmetic::Real>::value && + internal::is_arithmetic::Real>::value, + CwiseUnaryOp, const Derived> >; + +/** \returns an expression of the coefficient-wise power of \a x to the given constant \a exponent. + * + * \tparam ScalarExponent is the scalar type of \a exponent. It must be compatible with the scalar type of the given + * expression (\c Derived::Scalar). + * + * \sa ArrayBase::pow() + * + * \relates ArrayBase + */ +#ifdef EIGEN_PARSED_BY_DOXYGEN +template +EIGEN_DEVICE_FUNC inline const GlobalUnaryPowReturnType pow(const Eigen::ArrayBase& x, + const ScalarExponent& exponent); +#else +template +EIGEN_DEVICE_FUNC inline const GlobalUnaryPowReturnType pow(const Eigen::ArrayBase& x, + const ScalarExponent& exponent) { + return GlobalUnaryPowReturnType( + x.derived(), internal::scalar_unary_pow_op(exponent)); +} +#endif + +/** \returns an expression of the coefficient-wise power of \a x to the given array of \a exponents. + * + * This function computes the coefficient-wise power. + * + * Example: \include Cwise_array_power_array.cpp + * Output: \verbinclude Cwise_array_power_array.out + * + * \sa ArrayBase::pow() + * + * \relates ArrayBase + */ +template +inline const Eigen::CwiseBinaryOp< + Eigen::internal::scalar_pow_op, const Derived, + const ExponentDerived> +pow(const Eigen::ArrayBase& x, const Eigen::ArrayBase& exponents) { + return Eigen::CwiseBinaryOp< + Eigen::internal::scalar_pow_op, const Derived, + const ExponentDerived>(x.derived(), exponents.derived()); +} + +/** \returns an expression of the coefficient-wise power of the scalar \a x to the given array of \a exponents. + * + * This function computes the coefficient-wise power between a scalar and an array of exponents. + * + * \tparam Scalar is the scalar type of \a x. It must be compatible with the scalar type of the given array expression + * (\c Derived::Scalar). + * + * Example: \include Cwise_scalar_power_array.cpp + * Output: \verbinclude Cwise_scalar_power_array.out + * + * \sa ArrayBase::pow() + * + * \relates ArrayBase + */ +#ifdef EIGEN_PARSED_BY_DOXYGEN +template +inline const CwiseBinaryOp, Constant, Derived> pow( + const Scalar& x, const Eigen::ArrayBase& x); +#else +template +EIGEN_DEVICE_FUNC inline const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE( + typename internal::promote_scalar_arg::type, + Derived, pow) pow(const Scalar& x, const Eigen::ArrayBase& exponents) { + typedef + typename internal::promote_scalar_arg::type + PromotedScalar; + return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedScalar, Derived, pow)( + typename internal::plain_constant_type::type( + exponents.derived().rows(), exponents.derived().cols(), internal::scalar_constant_op(x)), + exponents.derived()); +} +#endif + +/** \returns an expression of the coefficient-wise atan2(\a x, \a y). \a x and \a y must be of the same type. + * + * This function computes the coefficient-wise atan2(). + * + * \sa ArrayBase::atan2() + * + * \relates ArrayBase + */ +template +inline const std::enable_if_t< + std::is_same::value, + Eigen::CwiseBinaryOp, + const LhsDerived, const RhsDerived> > +atan2(const Eigen::ArrayBase& x, const Eigen::ArrayBase& exponents) { + return Eigen::CwiseBinaryOp< + Eigen::internal::scalar_atan2_op, const LhsDerived, + const RhsDerived>(x.derived(), exponents.derived()); +} + +namespace internal { +EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(real, scalar_real_op) +EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(imag, scalar_imag_op) +EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(abs2, scalar_abs2_op) +} // namespace internal +} // namespace Eigen + +// TODO: cleanly disable those functions that are not supported on Array (numext::real_ref, internal::random, +// internal::isApprox...) + +#endif // EIGEN_GLOBAL_FUNCTIONS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/IO.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/IO.h new file mode 100644 index 00000000..ca5f247e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/IO.h @@ -0,0 +1,233 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2008 Benoit Jacob +// 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_IO_H +#define EIGEN_IO_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +enum { DontAlignCols = 1 }; +enum { StreamPrecision = -1, FullPrecision = -2 }; + +namespace internal { +template +std::ostream& print_matrix(std::ostream& s, const Derived& _m, const IOFormat& fmt); +} + +/** \class IOFormat + * \ingroup Core_Module + * + * \brief Stores a set of parameters controlling the way matrices are printed + * + * List of available parameters: + * - \b precision number of digits for floating point values, or one of the special constants \c StreamPrecision and \c + * FullPrecision. The default is the special value \c StreamPrecision which means to use the stream's own precision + * setting, as set for instance using \c cout.precision(3). The other special value \c FullPrecision means that the + * number of digits will be computed to match the full precision of each floating-point type. + * - \b flags an OR-ed combination of flags, the default value is 0, the only currently available flag is \c + * DontAlignCols which allows to disable the alignment of columns, resulting in faster code. + * - \b coeffSeparator string printed between two coefficients of the same row + * - \b rowSeparator string printed between two rows + * - \b rowPrefix string printed at the beginning of each row + * - \b rowSuffix string printed at the end of each row + * - \b matPrefix string printed at the beginning of the matrix + * - \b matSuffix string printed at the end of the matrix + * - \b fill character printed to fill the empty space in aligned columns + * + * Example: \include IOFormat.cpp + * Output: \verbinclude IOFormat.out + * + * \sa DenseBase::format(), class WithFormat + */ +struct IOFormat { + /** Default constructor, see class IOFormat for the meaning of the parameters */ + IOFormat(int _precision = StreamPrecision, int _flags = 0, const std::string& _coeffSeparator = " ", + const std::string& _rowSeparator = "\n", const std::string& _rowPrefix = "", + const std::string& _rowSuffix = "", const std::string& _matPrefix = "", const std::string& _matSuffix = "", + const char _fill = ' ') + : matPrefix(_matPrefix), + matSuffix(_matSuffix), + rowPrefix(_rowPrefix), + rowSuffix(_rowSuffix), + rowSeparator(_rowSeparator), + rowSpacer(""), + coeffSeparator(_coeffSeparator), + fill(_fill), + precision(_precision), + flags(_flags) { + // TODO check if rowPrefix, rowSuffix or rowSeparator contains a newline + // don't add rowSpacer if columns are not to be aligned + if ((flags & DontAlignCols)) return; + int i = int(matSuffix.length()) - 1; + while (i >= 0 && matSuffix[i] != '\n') { + rowSpacer += ' '; + i--; + } + } + std::string matPrefix, matSuffix; + std::string rowPrefix, rowSuffix, rowSeparator, rowSpacer; + std::string coeffSeparator; + char fill; + int precision; + int flags; +}; + +/** \class WithFormat + * \ingroup Core_Module + * + * \brief Pseudo expression providing matrix output with given format + * + * \tparam ExpressionType the type of the object on which IO stream operations are performed + * + * This class represents an expression with stream operators controlled by a given IOFormat. + * It is the return type of DenseBase::format() + * and most of the time this is the only way it is used. + * + * See class IOFormat for some examples. + * + * \sa DenseBase::format(), class IOFormat + */ +template +class WithFormat { + public: + WithFormat(const ExpressionType& matrix, const IOFormat& format) : m_matrix(matrix), m_format(format) {} + + friend std::ostream& operator<<(std::ostream& s, const WithFormat& wf) { + return internal::print_matrix(s, wf.m_matrix.eval(), wf.m_format); + } + + protected: + typename ExpressionType::Nested m_matrix; + IOFormat m_format; +}; + +namespace internal { + +// NOTE: This helper is kept for backward compatibility with previous code specializing +// this internal::significant_decimals_impl structure. In the future we should directly +// call max_digits10(). +template +struct significant_decimals_impl { + static inline int run() { return NumTraits::max_digits10(); } +}; + +/** \internal + * print the matrix \a _m to the output stream \a s using the output format \a fmt */ +template +std::ostream& print_matrix(std::ostream& s, const Derived& _m, const IOFormat& fmt) { + using internal::is_same; + + if (_m.size() == 0) { + s << fmt.matPrefix << fmt.matSuffix; + return s; + } + + typename Derived::Nested m = _m; + typedef typename Derived::Scalar Scalar; + typedef std::conditional_t::value || is_same::value || + is_same::value || is_same::value, + int, + std::conditional_t >::value || + is_same >::value || + is_same >::value || + is_same >::value, + std::complex, const Scalar&> > + PrintType; + + Index width = 0; + + std::streamsize explicit_precision; + if (fmt.precision == StreamPrecision) { + explicit_precision = 0; + } else if (fmt.precision == FullPrecision) { + if (NumTraits::IsInteger) { + explicit_precision = 0; + } else { + explicit_precision = significant_decimals_impl::run(); + } + } else { + explicit_precision = fmt.precision; + } + + std::streamsize old_precision = 0; + if (explicit_precision) old_precision = s.precision(explicit_precision); + + bool align_cols = !(fmt.flags & DontAlignCols); + if (align_cols) { + // compute the largest width + for (Index j = 0; j < m.cols(); ++j) + for (Index i = 0; i < m.rows(); ++i) { + std::stringstream sstr; + sstr.copyfmt(s); + sstr << static_cast(m.coeff(i, j)); + width = std::max(width, Index(sstr.str().length())); + } + } + std::streamsize old_width = s.width(); + char old_fill_character = s.fill(); + s << fmt.matPrefix; + for (Index i = 0; i < m.rows(); ++i) { + if (i) s << fmt.rowSpacer; + s << fmt.rowPrefix; + if (width) { + s.fill(fmt.fill); + s.width(width); + } + s << static_cast(m.coeff(i, 0)); + for (Index j = 1; j < m.cols(); ++j) { + s << fmt.coeffSeparator; + if (width) { + s.fill(fmt.fill); + s.width(width); + } + s << static_cast(m.coeff(i, j)); + } + s << fmt.rowSuffix; + if (i < m.rows() - 1) s << fmt.rowSeparator; + } + s << fmt.matSuffix; + if (explicit_precision) s.precision(old_precision); + if (width) { + s.fill(old_fill_character); + s.width(old_width); + } + return s; +} + +} // end namespace internal + +/** \relates DenseBase + * + * Outputs the matrix, to the given stream. + * + * If you wish to print the matrix with a format different than the default, use DenseBase::format(). + * + * It is also possible to change the default format by defining EIGEN_DEFAULT_IO_FORMAT before including Eigen headers. + * If not defined, this will automatically be defined to Eigen::IOFormat(), that is the Eigen::IOFormat with default + * parameters. + * + * \sa DenseBase::format() + */ +template +std::ostream& operator<<(std::ostream& s, const DenseBase& m) { + return internal::print_matrix(s, m.eval(), EIGEN_DEFAULT_IO_FORMAT); +} + +template +std::ostream& operator<<(std::ostream& s, const DiagonalBase& m) { + return internal::print_matrix(s, m.derived(), EIGEN_DEFAULT_IO_FORMAT); +} + +} // end namespace Eigen + +#endif // EIGEN_IO_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h new file mode 100644 index 00000000..0a024170 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h @@ -0,0 +1,243 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2017 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_INDEXED_VIEW_H +#define EIGEN_INDEXED_VIEW_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct traits> : traits { + enum { + RowsAtCompileTime = int(array_size::value), + ColsAtCompileTime = int(array_size::value), + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + + XprTypeIsRowMajor = (int(traits::Flags) & RowMajorBit) != 0, + IsRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1 + : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0 + : XprTypeIsRowMajor, + + RowIncr = int(get_compile_time_incr::value), + ColIncr = int(get_compile_time_incr::value), + InnerIncr = IsRowMajor ? ColIncr : RowIncr, + OuterIncr = IsRowMajor ? RowIncr : ColIncr, + + HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor), + XprInnerStride = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time::ret) + : int(outer_stride_at_compile_time::ret), + XprOuterstride = HasSameStorageOrderAsXprType ? int(outer_stride_at_compile_time::ret) + : int(inner_stride_at_compile_time::ret), + + InnerSize = XprTypeIsRowMajor ? ColsAtCompileTime : RowsAtCompileTime, + IsBlockAlike = InnerIncr == 1 && OuterIncr == 1, + IsInnerPannel = HasSameStorageOrderAsXprType && + is_same, std::conditional_t>::value, + + InnerStrideAtCompileTime = + InnerIncr < 0 || InnerIncr == DynamicIndex || XprInnerStride == Dynamic || InnerIncr == UndefinedIncr + ? Dynamic + : XprInnerStride * InnerIncr, + OuterStrideAtCompileTime = + OuterIncr < 0 || OuterIncr == DynamicIndex || XprOuterstride == Dynamic || OuterIncr == UndefinedIncr + ? Dynamic + : XprOuterstride * OuterIncr, + + ReturnAsScalar = is_same::value && is_same::value, + ReturnAsBlock = (!ReturnAsScalar) && IsBlockAlike, + ReturnAsIndexedView = (!ReturnAsScalar) && (!ReturnAsBlock), + + // FIXME we deal with compile-time strides if and only if we have DirectAccessBit flag, + // but this is too strict regarding negative strides... + DirectAccessMask = + (int(InnerIncr) != UndefinedIncr && int(OuterIncr) != UndefinedIncr && InnerIncr >= 0 && OuterIncr >= 0) + ? DirectAccessBit + : 0, + FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0, + FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, + FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0, + Flags = (traits::Flags & (HereditaryBits | DirectAccessMask)) | FlagsLvalueBit | FlagsRowMajorBit | + FlagsLinearAccessBit + }; + + typedef Block BlockType; +}; + +} // namespace internal + +template +class IndexedViewImpl; + +/** \class IndexedView + * \ingroup Core_Module + * + * \brief Expression of a non-sequential sub-matrix defined by arbitrary sequences of row and column indices + * + * \tparam XprType the type of the expression in which we are taking the intersections of sub-rows and sub-columns + * \tparam RowIndices the type of the object defining the sequence of row indices + * \tparam ColIndices the type of the object defining the sequence of column indices + * + * This class represents an expression of a sub-matrix (or sub-vector) defined as the intersection + * of sub-sets of rows and columns, that are themself defined by generic sequences of row indices \f$ + * \{r_0,r_1,..r_{m-1}\} \f$ and column indices \f$ \{c_0,c_1,..c_{n-1} \}\f$. Let \f$ A \f$ be the nested matrix, then + * the resulting matrix \f$ B \f$ has \c m rows and \c n columns, and its entries are given by: \f$ B(i,j) = A(r_i,c_j) + * \f$. + * + * The \c RowIndices and \c ColIndices types must be compatible with the following API: + * \code + * operator[](Index) const; + * Index size() const; + * \endcode + * + * Typical supported types thus include: + * - std::vector + * - std::valarray + * - std::array + * - Eigen::ArrayXi + * - decltype(ArrayXi::LinSpaced(...)) + * - Any view/expressions of the previous types + * - Eigen::ArithmeticSequence + * - Eigen::internal::AllRange (helper for Eigen::placeholders::all) + * - Eigen::internal::SingleRange (helper for single index) + * - etc. + * + * In typical usages of %Eigen, this class should never be used directly. It is the return type of + * DenseBase::operator()(const RowIndices&, const ColIndices&). + * + * \sa class Block + */ +template +class IndexedView + : public IndexedViewImpl::StorageKind> { + public: + typedef + typename IndexedViewImpl::StorageKind>::Base + Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(IndexedView) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(IndexedView) + + typedef typename internal::ref_selector::non_const_type MatrixTypeNested; + typedef internal::remove_all_t NestedExpression; + + template + IndexedView(XprType& xpr, const T0& rowIndices, const T1& colIndices) + : m_xpr(xpr), m_rowIndices(rowIndices), m_colIndices(colIndices) {} + + /** \returns number of rows */ + Index rows() const { return internal::index_list_size(m_rowIndices); } + + /** \returns number of columns */ + Index cols() const { return internal::index_list_size(m_colIndices); } + + /** \returns the nested expression */ + const internal::remove_all_t& nestedExpression() const { return m_xpr; } + + /** \returns the nested expression */ + std::remove_reference_t& nestedExpression() { return m_xpr; } + + /** \returns a const reference to the object storing/generating the row indices */ + const RowIndices& rowIndices() const { return m_rowIndices; } + + /** \returns a const reference to the object storing/generating the column indices */ + const ColIndices& colIndices() const { return m_colIndices; } + + protected: + MatrixTypeNested m_xpr; + RowIndices m_rowIndices; + ColIndices m_colIndices; +}; + +// Generic API dispatcher +template +class IndexedViewImpl : public internal::generic_xpr_base>::type { + public: + typedef typename internal::generic_xpr_base>::type Base; +}; + +namespace internal { + +template +struct unary_evaluator, IndexBased> + : evaluator_base> { + typedef IndexedView XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost /* TODO + cost of row/col index */, + + FlagsLinearAccessBit = + (traits::RowsAtCompileTime == 1 || traits::ColsAtCompileTime == 1) ? LinearAccessBit : 0, + + FlagsRowMajorBit = traits::FlagsRowMajorBit, + + Flags = (evaluator::Flags & (HereditaryBits & ~RowMajorBit /*| LinearAccessBit | DirectAccessBit*/)) | + FlagsLinearAccessBit | FlagsRowMajorBit, + + Alignment = 0 + }; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr) { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { + eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() && + m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols()); + return m_argImpl.coeff(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) { + eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() && + m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols()); + return m_argImpl.coeffRef(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { + EIGEN_STATIC_ASSERT_LVALUE(XprType) + Index row = XprType::RowsAtCompileTime == 1 ? 0 : index; + Index col = XprType::RowsAtCompileTime == 1 ? index : 0; + eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() && + m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols()); + return m_argImpl.coeffRef(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeffRef(Index index) const { + Index row = XprType::RowsAtCompileTime == 1 ? 0 : index; + Index col = XprType::RowsAtCompileTime == 1 ? index : 0; + eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() && + m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols()); + return m_argImpl.coeffRef(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index index) const { + Index row = XprType::RowsAtCompileTime == 1 ? 0 : index; + Index col = XprType::RowsAtCompileTime == 1 ? index : 0; + eigen_assert(m_xpr.rowIndices()[row] >= 0 && m_xpr.rowIndices()[row] < m_xpr.nestedExpression().rows() && + m_xpr.colIndices()[col] >= 0 && m_xpr.colIndices()[col] < m_xpr.nestedExpression().cols()); + return m_argImpl.coeff(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]); + } + + protected: + evaluator m_argImpl; + const XprType& m_xpr; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_INDEXED_VIEW_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/InternalHeaderCheck.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/InternalHeaderCheck.h new file mode 100644 index 00000000..1cea572d --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/InternalHeaderCheck.h @@ -0,0 +1,3 @@ +#ifndef EIGEN_CORE_MODULE_H +#error "Please include Eigen/Core instead of including headers inside the src directory directly." +#endif diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Inverse.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Inverse.h new file mode 100644 index 00000000..cfb3b20e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Inverse.h @@ -0,0 +1,108 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014-2019 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_INVERSE_H +#define EIGEN_INVERSE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +template +class InverseImpl; + +namespace internal { + +template +struct traits > : traits { + typedef typename XprType::PlainObject PlainObject; + typedef traits BaseTraits; + enum { Flags = BaseTraits::Flags & RowMajorBit }; +}; + +} // end namespace internal + +/** \class Inverse + * + * \brief Expression of the inverse of another expression + * + * \tparam XprType the type of the expression we are taking the inverse + * + * This class represents an abstract expression of A.inverse() + * and most of the time this is the only way it is used. + * + */ +template +class Inverse : public InverseImpl::StorageKind> { + public: + typedef typename XprType::StorageIndex StorageIndex; + typedef typename XprType::Scalar Scalar; + typedef typename internal::ref_selector::type XprTypeNested; + typedef internal::remove_all_t XprTypeNestedCleaned; + typedef typename internal::ref_selector::type Nested; + typedef internal::remove_all_t NestedExpression; + + explicit EIGEN_DEVICE_FUNC Inverse(const XprType& xpr) : m_xpr(xpr) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.cols(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.rows(); } + + EIGEN_DEVICE_FUNC const XprTypeNestedCleaned& nestedExpression() const { return m_xpr; } + + protected: + XprTypeNested m_xpr; +}; + +// Generic API dispatcher +template +class InverseImpl : public internal::generic_xpr_base >::type { + public: + typedef typename internal::generic_xpr_base >::type Base; + typedef typename XprType::Scalar Scalar; + + private: + Scalar coeff(Index row, Index col) const; + Scalar coeff(Index i) const; +}; + +namespace internal { + +/** \internal + * \brief Default evaluator for Inverse expression. + * + * This default evaluator for Inverse expression simply evaluate the inverse into a temporary + * by a call to internal::call_assignment_no_alias. + * Therefore, inverse implementers only have to specialize Assignment, ...> for + * there own nested expression. + * + * \sa class Inverse + */ +template +struct unary_evaluator > : public evaluator::PlainObject> { + typedef Inverse InverseType; + typedef typename InverseType::PlainObject PlainObject; + typedef evaluator Base; + + enum { Flags = Base::Flags | EvalBeforeNestingBit }; + + unary_evaluator(const InverseType& inv_xpr) : m_result(inv_xpr.rows(), inv_xpr.cols()) { + internal::construct_at(this, m_result); + internal::call_assignment_no_alias(m_result, inv_xpr); + } + + protected: + PlainObject m_result; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_INVERSE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Map.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Map.h new file mode 100644 index 00000000..df7b7ca7 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Map.h @@ -0,0 +1,153 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2007-2010 Benoit Jacob +// 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_MAP_H +#define EIGEN_MAP_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits > : public traits { + typedef traits TraitsBase; + enum { + PlainObjectTypeInnerSize = ((traits::Flags & RowMajorBit) == RowMajorBit) + ? PlainObjectType::ColsAtCompileTime + : PlainObjectType::RowsAtCompileTime, + + InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0 + ? int(PlainObjectType::InnerStrideAtCompileTime) + : int(StrideType::InnerStrideAtCompileTime), + OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0 + ? (InnerStrideAtCompileTime == Dynamic || PlainObjectTypeInnerSize == Dynamic + ? Dynamic + : int(InnerStrideAtCompileTime) * int(PlainObjectTypeInnerSize)) + : int(StrideType::OuterStrideAtCompileTime), + Alignment = int(MapOptions) & int(AlignedMask), + Flags0 = TraitsBase::Flags & (~NestByRefBit), + Flags = is_lvalue::value ? int(Flags0) : (int(Flags0) & ~LvalueBit) + }; + + private: + enum { Options }; // Expressions don't have Options +}; +} // namespace internal + +/** \class Map + * \ingroup Core_Module + * + * \brief A matrix or vector expression mapping an existing array of data. + * + * \tparam PlainObjectType the equivalent matrix type of the mapped data + * \tparam MapOptions specifies the pointer alignment in bytes. It can be: \c #Aligned128, \c #Aligned64, \c #Aligned32, + * \c #Aligned16, \c #Aligned8 or \c #Unaligned. The default is \c #Unaligned. \tparam StrideType optionally specifies + * strides. By default, Map assumes the memory layout of an ordinary, contiguous array. This can be overridden by + * specifying strides. The type passed here must be a specialization of the Stride template, see examples below. + * + * This class represents a matrix or vector expression mapping an existing array of data. + * It can be used to let Eigen interface without any overhead with non-Eigen data structures, + * such as plain C arrays or structures from other libraries. By default, it assumes that the + * data is laid out contiguously in memory. You can however override this by explicitly specifying + * inner and outer strides. + * + * Here's an example of simply mapping a contiguous array as a \ref TopicStorageOrders "column-major" matrix: + * \include Map_simple.cpp + * Output: \verbinclude Map_simple.out + * + * If you need to map non-contiguous arrays, you can do so by specifying strides: + * + * Here's an example of mapping an array as a vector, specifying an inner stride, that is, the pointer + * increment between two consecutive coefficients. Here, we're specifying the inner stride as a compile-time + * fixed value. + * \include Map_inner_stride.cpp + * Output: \verbinclude Map_inner_stride.out + * + * Here's an example of mapping an array while specifying an outer stride. Here, since we're mapping + * as a column-major matrix, 'outer stride' means the pointer increment between two consecutive columns. + * Here, we're specifying the outer stride as a runtime parameter. Note that here \c OuterStride<> is + * a short version of \c OuterStride because the default template parameter of OuterStride + * is \c Dynamic + * \include Map_outer_stride.cpp + * Output: \verbinclude Map_outer_stride.out + * + * For more details and for an example of specifying both an inner and an outer stride, see class Stride. + * + * \b Tip: to change the array of data mapped by a Map object, you can use the C++ + * placement new syntax: + * + * Example: \include Map_placement_new.cpp + * Output: \verbinclude Map_placement_new.out + * + * This class is the return type of PlainObjectBase::Map() but can also be used directly. + * + * \sa PlainObjectBase::Map(), \ref TopicStorageOrders + */ +template +class Map : public MapBase > { + public: + typedef MapBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Map) + + typedef typename Base::PointerType PointerType; + typedef PointerType PointerArgType; + EIGEN_DEVICE_FUNC inline PointerType cast_to_pointer_type(PointerArgType ptr) { return ptr; } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { + return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { + return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer() + : internal::traits::OuterStrideAtCompileTime != Dynamic + ? Index(internal::traits::OuterStrideAtCompileTime) + : IsVectorAtCompileTime ? (this->size() * innerStride()) + : int(Flags) & RowMajorBit ? (this->cols() * innerStride()) + : (this->rows() * innerStride()); + } + + /** Constructor in the fixed-size case. + * + * \param dataPtr pointer to the array to map + * \param stride optional Stride object, passing the strides. + */ + EIGEN_DEVICE_FUNC explicit inline Map(PointerArgType dataPtr, const StrideType& stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr)), m_stride(stride) {} + + /** Constructor in the dynamic-size vector case. + * + * \param dataPtr pointer to the array to map + * \param size the size of the vector expression + * \param stride optional Stride object, passing the strides. + */ + EIGEN_DEVICE_FUNC inline Map(PointerArgType dataPtr, Index size, const StrideType& stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr), size), m_stride(stride) {} + + /** Constructor in the dynamic-size matrix case. + * + * \param dataPtr pointer to the array to map + * \param rows the number of rows of the matrix expression + * \param cols the number of columns of the matrix expression + * \param stride optional Stride object, passing the strides. + */ + EIGEN_DEVICE_FUNC inline Map(PointerArgType dataPtr, Index rows, Index cols, const StrideType& stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr), rows, cols), m_stride(stride) {} + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + + protected: + StrideType m_stride; +}; + +} // end namespace Eigen + +#endif // EIGEN_MAP_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/MapBase.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MapBase.h new file mode 100644 index 00000000..da95b5c4 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MapBase.h @@ -0,0 +1,283 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2007-2010 Benoit Jacob +// 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_MAPBASE_H +#define EIGEN_MAPBASE_H + +#define EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) \ + EIGEN_STATIC_ASSERT((int(internal::evaluator::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \ + YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT) + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \ingroup Core_Module + * + * \brief Base class for dense Map and Block expression with direct access + * + * This base class provides the const low-level accessors (e.g. coeff, coeffRef) of dense + * Map and Block objects with direct access. + * Typical users do not have to directly deal with this class. + * + * This class can be extended by through the macro plugin \c EIGEN_MAPBASE_PLUGIN. + * See \link TopicCustomizing_Plugins customizing Eigen \endlink for details. + * + * The \c Derived class has to provide the following two methods describing the memory layout: + * \code Index innerStride() const; \endcode + * \code Index outerStride() const; \endcode + * + * \sa class Map, class Block + */ +template +class MapBase : public internal::dense_xpr_base::type { + public: + typedef typename internal::dense_xpr_base::type Base; + enum { + RowsAtCompileTime = internal::traits::RowsAtCompileTime, + ColsAtCompileTime = internal::traits::ColsAtCompileTime, + InnerStrideAtCompileTime = internal::traits::InnerStrideAtCompileTime, + SizeAtCompileTime = Base::SizeAtCompileTime + }; + + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::packet_traits::type PacketScalar; + typedef typename NumTraits::Real RealScalar; + typedef std::conditional_t::value), Scalar*, const Scalar*> PointerType; + + using Base::derived; + // using Base::RowsAtCompileTime; + // using Base::ColsAtCompileTime; + // using Base::SizeAtCompileTime; + using Base::Flags; + using Base::IsRowMajor; + using Base::IsVectorAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxSizeAtCompileTime; + + using Base::coeff; + using Base::coeffRef; + using Base::cols; + using Base::eval; + using Base::lazyAssign; + using Base::rows; + using Base::size; + + using Base::colStride; + using Base::innerStride; + using Base::outerStride; + using Base::rowStride; + + // bug 217 - compile error on ICC 11.1 + using Base::operator=; + + typedef typename Base::CoeffReturnType CoeffReturnType; + + /** \copydoc DenseBase::rows() */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_rows.value(); } + /** \copydoc DenseBase::cols() */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_cols.value(); } + + /** Returns a pointer to the first coefficient of the matrix or vector. + * + * \note When addressing this data, make sure to honor the strides returned by innerStride() and outerStride(). + * + * \sa innerStride(), outerStride() + */ + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_data; } + + /** \copydoc PlainObjectBase::coeff(Index,Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeff(Index rowId, Index colId) const { + return m_data[colId * colStride() + rowId * rowStride()]; + } + + /** \copydoc PlainObjectBase::coeff(Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeff(Index index) const { + EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) + return m_data[index * innerStride()]; + } + + /** \copydoc PlainObjectBase::coeffRef(Index,Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { + return this->m_data[colId * colStride() + rowId * rowStride()]; + } + + /** \copydoc PlainObjectBase::coeffRef(Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { + EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) + return this->m_data[index * innerStride()]; + } + + /** \internal */ + template + inline PacketScalar packet(Index rowId, Index colId) const { + return internal::ploadt(m_data + (colId * colStride() + rowId * rowStride())); + } + + /** \internal */ + template + inline PacketScalar packet(Index index) const { + EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) + return internal::ploadt(m_data + index * innerStride()); + } + + /** \internal Constructor for fixed size matrices or vectors */ + EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) + : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) + checkSanity(); + } + + /** \internal Constructor for dynamically sized vectors */ + EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) + : m_data(dataPtr), + m_rows(RowsAtCompileTime == Dynamic ? vecSize : Index(RowsAtCompileTime)), + m_cols(ColsAtCompileTime == Dynamic ? vecSize : Index(ColsAtCompileTime)) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) + eigen_assert(vecSize >= 0); + eigen_assert(dataPtr == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == vecSize); + checkSanity(); + } + + /** \internal Constructor for dynamically sized matrices */ + EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols) + : m_data(dataPtr), m_rows(rows), m_cols(cols) { + eigen_assert((dataPtr == 0) || (rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) && + cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols))); + checkSanity(); + } + +#ifdef EIGEN_MAPBASE_PLUGIN +#include EIGEN_MAPBASE_PLUGIN +#endif + + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase) + + template + EIGEN_DEVICE_FUNC void checkSanity(std::enable_if_t<(internal::traits::Alignment > 0), void*> = 0) const { +// Temporary macro to allow scalars to not be properly aligned. This is while we sort out failures +// in TensorFlow Lite that are currently relying on this UB. +#ifndef EIGEN_ALLOW_UNALIGNED_SCALARS + // Pointer must be aligned to the Scalar type, otherwise we get UB. + eigen_assert((std::uintptr_t(m_data) % alignof(Scalar) == 0) && "data is not scalar-aligned"); +#endif +#if EIGEN_MAX_ALIGN_BYTES > 0 + // innerStride() is not set yet when this function is called, so we optimistically assume the lowest plausible + // value: + const Index minInnerStride = InnerStrideAtCompileTime == Dynamic ? 1 : Index(InnerStrideAtCompileTime); + EIGEN_ONLY_USED_FOR_DEBUG(minInnerStride); + eigen_assert((((std::uintptr_t(m_data) % internal::traits::Alignment) == 0) || + (cols() * rows() * minInnerStride * sizeof(Scalar)) < internal::traits::Alignment) && + "data is not aligned"); +#endif + } + + template + EIGEN_DEVICE_FUNC void checkSanity(std::enable_if_t::Alignment == 0, void*> = 0) const { +#ifndef EIGEN_ALLOW_UNALIGNED_SCALARS + // Pointer must be aligned to the Scalar type, otherwise we get UB. + eigen_assert((std::uintptr_t(m_data) % alignof(Scalar) == 0) && "data is not scalar-aligned"); +#endif + } + + PointerType m_data; + const internal::variable_if_dynamic m_rows; + const internal::variable_if_dynamic m_cols; +}; + +/** \ingroup Core_Module + * + * \brief Base class for non-const dense Map and Block expression with direct access + * + * This base class provides the non-const low-level accessors (e.g. coeff and coeffRef) of + * dense Map and Block objects with direct access. + * It inherits MapBase which defines the const variant for reading specific entries. + * + * \sa class Map, class Block + */ +template +class MapBase : public MapBase { + typedef MapBase ReadOnlyMapBase; + + public: + typedef MapBase Base; + + typedef typename Base::Scalar Scalar; + typedef typename Base::PacketScalar PacketScalar; + typedef typename Base::StorageIndex StorageIndex; + typedef typename Base::PointerType PointerType; + + using Base::coeff; + using Base::coeffRef; + using Base::cols; + using Base::derived; + using Base::rows; + using Base::size; + + using Base::colStride; + using Base::innerStride; + using Base::outerStride; + using Base::rowStride; + + typedef std::conditional_t::value, Scalar, const Scalar> ScalarWithConstIfNotLvalue; + + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return this->m_data; } + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { + return this->m_data; + } // no const-cast here so non-const-correct code will give a compile error + + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue& coeffRef(Index row, Index col) { + return this->m_data[col * colStride() + row * rowStride()]; + } + + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue& coeffRef(Index index) { + EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) + return this->m_data[index * innerStride()]; + } + + template + inline void writePacket(Index row, Index col, const PacketScalar& val) { + internal::pstoret(this->m_data + (col * colStride() + row * rowStride()), val); + } + + template + inline void writePacket(Index index, const PacketScalar& val) { + EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) + internal::pstoret(this->m_data + index * innerStride(), val); + } + + EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {} + EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {} + EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols) : Base(dataPtr, rows, cols) {} + + EIGEN_DEVICE_FUNC Derived& operator=(const MapBase& other) { + ReadOnlyMapBase::Base::operator=(other); + return derived(); + } + + // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base, + // see bugs 821 and 920. + using ReadOnlyMapBase::Base::operator=; + + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase) +}; + +#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS + +} // end namespace Eigen + +#endif // EIGEN_MAPBASE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h new file mode 100644 index 00000000..95f9b972 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h @@ -0,0 +1,1798 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2010 Benoit Jacob +// Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. +// +// 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_MATHFUNCTIONS_H +#define EIGEN_MATHFUNCTIONS_H + +// TODO this should better be moved to NumTraits +// Source: WolframAlpha +#define EIGEN_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406L +#define EIGEN_LOG2E 1.442695040888963407359924681001892137426645954152985934135449406931109219L +#define EIGEN_LN2 0.693147180559945309417232121458176568075500134360255254120680009493393621L + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +/** \internal \class global_math_functions_filtering_base + * + * What it does: + * Defines a typedef 'type' as follows: + * - if type T has a member typedef Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl, then + * global_math_functions_filtering_base::type is a typedef for it. + * - otherwise, global_math_functions_filtering_base::type is a typedef for T. + * + * How it's used: + * To allow to defined the global math functions (like sin...) in certain cases, like the Array expressions. + * When you do sin(array1+array2), the object array1+array2 has a complicated expression type, all what you want to know + * is that it inherits ArrayBase. So we implement a partial specialization of sin_impl for ArrayBase. + * So we must make sure to use sin_impl > and not sin_impl, otherwise our partial + * specialization won't be used. How does sin know that? That's exactly what global_math_functions_filtering_base tells + * it. + * + * How it's implemented: + * SFINAE in the style of enable_if. Highly susceptible of breaking compilers. With GCC, it sure does work, but if you + * replace the typename dummy by an integer template parameter, it doesn't work anymore! + */ + +template +struct global_math_functions_filtering_base { + typedef T type; +}; + +template +struct always_void { + typedef void type; +}; + +template +struct global_math_functions_filtering_base< + T, typename always_void::type> { + typedef typename T::Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl type; +}; + +#define EIGEN_MATHFUNC_IMPL(func, scalar) \ + Eigen::internal::func##_impl::type> +#define EIGEN_MATHFUNC_RETVAL(func, scalar) \ + typename Eigen::internal::func##_retval< \ + typename Eigen::internal::global_math_functions_filtering_base::type>::type + +/**************************************************************************** + * Implementation of real * + ****************************************************************************/ + +template ::IsComplex> +struct real_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x; } +}; + +template +struct real_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + using std::real; + return real(x); + } +}; + +template +struct real_impl : real_default_impl {}; + +#if defined(EIGEN_GPU_COMPILE_PHASE) +template +struct real_impl> { + typedef T RealScalar; + EIGEN_DEVICE_FUNC static inline T run(const std::complex& x) { return x.real(); } +}; +#endif + +template +struct real_retval { + typedef typename NumTraits::Real type; +}; + +/**************************************************************************** + * Implementation of imag * + ****************************************************************************/ + +template ::IsComplex> +struct imag_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar&) { return RealScalar(0); } +}; + +template +struct imag_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + using std::imag; + return imag(x); + } +}; + +template +struct imag_impl : imag_default_impl {}; + +#if defined(EIGEN_GPU_COMPILE_PHASE) +template +struct imag_impl> { + typedef T RealScalar; + EIGEN_DEVICE_FUNC static inline T run(const std::complex& x) { return x.imag(); } +}; +#endif + +template +struct imag_retval { + typedef typename NumTraits::Real type; +}; + +/**************************************************************************** + * Implementation of real_ref * + ****************************************************************************/ + +template +struct real_ref_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar& run(Scalar& x) { return reinterpret_cast(&x)[0]; } + EIGEN_DEVICE_FUNC static inline const RealScalar& run(const Scalar& x) { + return reinterpret_cast(&x)[0]; + } +}; + +template +struct real_ref_retval { + typedef typename NumTraits::Real& type; +}; + +/**************************************************************************** + * Implementation of imag_ref * + ****************************************************************************/ + +template +struct imag_ref_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar& run(Scalar& x) { return reinterpret_cast(&x)[1]; } + EIGEN_DEVICE_FUNC static inline const RealScalar& run(const Scalar& x) { + return reinterpret_cast(&x)[1]; + } +}; + +template +struct imag_ref_default_impl { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Scalar run(Scalar&) { return Scalar(0); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline const Scalar run(const Scalar&) { return Scalar(0); } +}; + +template +struct imag_ref_impl : imag_ref_default_impl::IsComplex> {}; + +template +struct imag_ref_retval { + typedef typename NumTraits::Real& type; +}; + +/**************************************************************************** + * Implementation of conj * + ****************************************************************************/ + +template ::IsComplex> +struct conj_default_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { return x; } +}; + +template +struct conj_default_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { + using std::conj; + return conj(x); + } +}; + +template ::IsComplex> +struct conj_impl : conj_default_impl {}; + +template +struct conj_retval { + typedef Scalar type; +}; + +/**************************************************************************** + * Implementation of abs2 * + ****************************************************************************/ + +template +struct abs2_impl_default { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x * x; } +}; + +template +struct abs2_impl_default // IsComplex +{ + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x.real() * x.real() + x.imag() * x.imag(); } +}; + +template +struct abs2_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + return abs2_impl_default::IsComplex>::run(x); + } +}; + +template +struct abs2_retval { + typedef typename NumTraits::Real type; +}; + +/**************************************************************************** + * Implementation of sqrt/rsqrt * + ****************************************************************************/ + +template +struct sqrt_impl { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE Scalar run(const Scalar& x) { + EIGEN_USING_STD(sqrt); + return sqrt(x); + } +}; + +// Complex sqrt defined in MathFunctionsImpl.h. +template +EIGEN_DEVICE_FUNC std::complex complex_sqrt(const std::complex& a_x); + +// Custom implementation is faster than `std::sqrt`, works on +// GPU, and correctly handles special cases (unlike MSVC). +template +struct sqrt_impl> { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE std::complex run(const std::complex& x) { + return complex_sqrt(x); + } +}; + +template +struct sqrt_retval { + typedef Scalar type; +}; + +// Default implementation relies on numext::sqrt, at bottom of file. +template +struct rsqrt_impl; + +// Complex rsqrt defined in MathFunctionsImpl.h. +template +EIGEN_DEVICE_FUNC std::complex complex_rsqrt(const std::complex& a_x); + +template +struct rsqrt_impl> { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE std::complex run(const std::complex& x) { + return complex_rsqrt(x); + } +}; + +template +struct rsqrt_retval { + typedef Scalar type; +}; + +/**************************************************************************** + * Implementation of norm1 * + ****************************************************************************/ + +template +struct norm1_default_impl; + +template +struct norm1_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + EIGEN_USING_STD(abs); + return abs(x.real()) + abs(x.imag()); + } +}; + +template +struct norm1_default_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { + EIGEN_USING_STD(abs); + return abs(x); + } +}; + +template +struct norm1_impl : norm1_default_impl::IsComplex> {}; + +template +struct norm1_retval { + typedef typename NumTraits::Real type; +}; + +/**************************************************************************** + * Implementation of hypot * + ****************************************************************************/ + +template +struct hypot_impl; + +template +struct hypot_retval { + typedef typename NumTraits::Real type; +}; + +/**************************************************************************** + * Implementation of cast * + ****************************************************************************/ + +template +struct cast_impl { + EIGEN_DEVICE_FUNC static inline NewType run(const OldType& x) { return static_cast(x); } +}; + +template +struct cast_impl { + EIGEN_DEVICE_FUNC static inline bool run(const OldType& x) { return x != OldType(0); } +}; + +// Casting from S -> Complex leads to an implicit conversion from S to T, +// generating warnings on clang. Here we explicitly cast the real component. +template +struct cast_impl::IsComplex && NumTraits::IsComplex>> { + EIGEN_DEVICE_FUNC static inline NewType run(const OldType& x) { + typedef typename NumTraits::Real NewReal; + return static_cast(static_cast(x)); + } +}; + +// here, for once, we're plainly returning NewType: we don't want cast to do weird things. + +template +EIGEN_DEVICE_FUNC inline NewType cast(const OldType& x) { + return cast_impl::run(x); +} + +/**************************************************************************** + * Implementation of arg * + ****************************************************************************/ + +// Visual Studio 2017 has a bug where arg(float) returns 0 for negative inputs. +// This seems to be fixed in VS 2019. +#if (!EIGEN_COMP_MSVC || EIGEN_COMP_MSVC >= 1920) +// std::arg is only defined for types of std::complex, or integer types or float/double/long double +template ::IsComplex || is_integral::value || + is_same::value || is_same::value || + is_same::value> +struct arg_default_impl; + +template +struct arg_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + // There is no official ::arg on device in CUDA/HIP, so we always need to use std::arg. + using std::arg; + return static_cast(arg(x)); + } +}; + +// Must be non-complex floating-point type (e.g. half/bfloat16). +template +struct arg_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + return (x < Scalar(0)) ? RealScalar(EIGEN_PI) : RealScalar(0); + } +}; +#else +template ::IsComplex> +struct arg_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + return (x < RealScalar(0)) ? RealScalar(EIGEN_PI) : RealScalar(0); + } +}; + +template +struct arg_default_impl { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + EIGEN_USING_STD(arg); + return arg(x); + } +}; +#endif +template +struct arg_impl : arg_default_impl {}; + +template +struct arg_retval { + typedef typename NumTraits::Real type; +}; + +/**************************************************************************** + * Implementation of expm1 * + ****************************************************************************/ + +// This implementation is based on GSL Math's expm1. +namespace std_fallback { +// fallback expm1 implementation in case there is no expm1(Scalar) function in namespace of Scalar, +// or that there is no suitable std::expm1 function available. Implementation +// attributed to Kahan. See: http://www.plunk.org/~hatch/rightway.php. +template +EIGEN_DEVICE_FUNC inline Scalar expm1(const Scalar& x) { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + typedef typename NumTraits::Real RealScalar; + + EIGEN_USING_STD(exp); + Scalar u = exp(x); + if (numext::equal_strict(u, Scalar(1))) { + return x; + } + Scalar um1 = u - RealScalar(1); + if (numext::equal_strict(um1, Scalar(-1))) { + return RealScalar(-1); + } + + EIGEN_USING_STD(log); + Scalar logu = log(u); + return numext::equal_strict(u, logu) ? u : (u - RealScalar(1)) * x / logu; +} +} // namespace std_fallback + +template +struct expm1_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + EIGEN_USING_STD(expm1); + return expm1(x); + } +}; + +template +struct expm1_retval { + typedef Scalar type; +}; + +/**************************************************************************** + * Implementation of log * + ****************************************************************************/ + +// Complex log defined in MathFunctionsImpl.h. +template +EIGEN_DEVICE_FUNC std::complex complex_log(const std::complex& z); + +template +struct log_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { + EIGEN_USING_STD(log); + return static_cast(log(x)); + } +}; + +template +struct log_impl> { + EIGEN_DEVICE_FUNC static inline std::complex run(const std::complex& z) { return complex_log(z); } +}; + +/**************************************************************************** + * Implementation of log1p * + ****************************************************************************/ + +namespace std_fallback { +// fallback log1p implementation in case there is no log1p(Scalar) function in namespace of Scalar, +// or that there is no suitable std::log1p function available +template +EIGEN_DEVICE_FUNC inline Scalar log1p(const Scalar& x) { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + typedef typename NumTraits::Real RealScalar; + EIGEN_USING_STD(log); + Scalar x1p = RealScalar(1) + x; + Scalar log_1p = log_impl::run(x1p); + const bool is_small = numext::equal_strict(x1p, Scalar(1)); + const bool is_inf = numext::equal_strict(x1p, log_1p); + return (is_small || is_inf) ? x : x * (log_1p / (x1p - RealScalar(1))); +} +} // namespace std_fallback + +template +struct log1p_impl { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { + EIGEN_USING_STD(log1p); + return log1p(x); + } +}; + +// Specialization for complex types that are not supported by std::log1p. +template +struct log1p_impl> { + EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar) + + EIGEN_DEVICE_FUNC static inline std::complex run(const std::complex& x) { + return std_fallback::log1p(x); + } +}; + +template +struct log1p_retval { + typedef Scalar type; +}; + +/**************************************************************************** + * Implementation of pow * + ****************************************************************************/ + +template ::IsInteger && NumTraits::IsInteger> +struct pow_impl { + // typedef Scalar retval; + typedef typename ScalarBinaryOpTraits>::ReturnType + result_type; + static EIGEN_DEVICE_FUNC inline result_type run(const ScalarX& x, const ScalarY& y) { + EIGEN_USING_STD(pow); + return pow(x, y); + } +}; + +template +struct pow_impl { + typedef ScalarX result_type; + static EIGEN_DEVICE_FUNC inline ScalarX run(ScalarX x, ScalarY y) { + ScalarX res(1); + eigen_assert(!NumTraits::IsSigned || y >= 0); + if (y & 1) res *= x; + y >>= 1; + while (y) { + x *= x; + if (y & 1) res *= x; + y >>= 1; + } + return res; + } +}; + +/**************************************************************************** + * Implementation of random * + ****************************************************************************/ + +template +struct random_default_impl {}; + +template +struct random_impl : random_default_impl::IsComplex, NumTraits::IsInteger> {}; + +template +struct random_retval { + typedef Scalar type; +}; + +template +inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random(const Scalar& x, const Scalar& y); +template +inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random(); + +template +struct random_default_impl { + static inline Scalar run(const Scalar& x, const Scalar& y) { + return x + (y - x) * Scalar(std::rand()) / Scalar(RAND_MAX); + } + static inline Scalar run() { return run(Scalar(NumTraits::IsSigned ? -1 : 0), Scalar(1)); } +}; + +enum { meta_floor_log2_terminate, meta_floor_log2_move_up, meta_floor_log2_move_down, meta_floor_log2_bogus }; + +template +struct meta_floor_log2_selector { + enum { + middle = (lower + upper) / 2, + value = (upper <= lower + 1) ? int(meta_floor_log2_terminate) + : (n < (1 << middle)) ? int(meta_floor_log2_move_down) + : (n == 0) ? int(meta_floor_log2_bogus) + : int(meta_floor_log2_move_up) + }; +}; + +template ::value> +struct meta_floor_log2 {}; + +template +struct meta_floor_log2 { + enum { value = meta_floor_log2::middle>::value }; +}; + +template +struct meta_floor_log2 { + enum { value = meta_floor_log2::middle, upper>::value }; +}; + +template +struct meta_floor_log2 { + enum { value = (n >= ((unsigned int)(1) << (lower + 1))) ? lower + 1 : lower }; +}; + +template +struct meta_floor_log2 { + // no value, error at compile time +}; + +template +struct random_default_impl { + static inline Scalar run(const Scalar& x, const Scalar& y) { + if (y <= x) return x; + // ScalarU is the unsigned counterpart of Scalar, possibly Scalar itself. + typedef typename make_unsigned::type ScalarU; + // ScalarX is the widest of ScalarU and unsigned int. + // We'll deal only with ScalarX and unsigned int below thus avoiding signed + // types and arithmetic and signed overflows (which are undefined behavior). + typedef std::conditional_t<(ScalarU(-1) > unsigned(-1)), ScalarU, unsigned> ScalarX; + // The following difference doesn't overflow, provided our integer types are two's + // complement and have the same number of padding bits in signed and unsigned variants. + // This is the case in most modern implementations of C++. + ScalarX range = ScalarX(y) - ScalarX(x); + ScalarX offset = 0; + ScalarX divisor = 1; + ScalarX multiplier = 1; + const unsigned rand_max = RAND_MAX; + if (range <= rand_max) + divisor = (rand_max + 1) / (range + 1); + else + multiplier = 1 + range / (rand_max + 1); + // Rejection sampling. + do { + offset = (unsigned(std::rand()) * multiplier) / divisor; + } while (offset > range); + return Scalar(ScalarX(x) + offset); + } + + static inline Scalar run() { +#ifdef EIGEN_MAKING_DOCS + return run(Scalar(NumTraits::IsSigned ? -10 : 0), Scalar(10)); +#else + enum { + rand_bits = meta_floor_log2<(unsigned int)(RAND_MAX) + 1>::value, + scalar_bits = sizeof(Scalar) * CHAR_BIT, + shift = plain_enum_max(0, int(rand_bits) - int(scalar_bits)), + offset = NumTraits::IsSigned ? (1 << (plain_enum_min(rand_bits, scalar_bits) - 1)) : 0 + }; + return Scalar((std::rand() >> shift) - offset); +#endif + } +}; + +template +struct random_default_impl { + static inline Scalar run(const Scalar& x, const Scalar& y) { + return Scalar(random(x.real(), y.real()), random(x.imag(), y.imag())); + } + static inline Scalar run() { + typedef typename NumTraits::Real RealScalar; + return Scalar(random(), random()); + } +}; + +template +inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random(const Scalar& x, const Scalar& y) { + return EIGEN_MATHFUNC_IMPL(random, Scalar)::run(x, y); +} + +template +inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random() { + return EIGEN_MATHFUNC_IMPL(random, Scalar)::run(); +} + +// Implementation of is* functions + +template +EIGEN_DEVICE_FUNC std::enable_if_t::has_infinity || std::numeric_limits::has_quiet_NaN || + std::numeric_limits::has_signaling_NaN), + bool> +isfinite_impl(const T&) { + return true; +} + +template +EIGEN_DEVICE_FUNC std::enable_if_t<(std::numeric_limits::has_infinity || std::numeric_limits::has_quiet_NaN || + std::numeric_limits::has_signaling_NaN) && + (!NumTraits::IsComplex), + bool> +isfinite_impl(const T& x) { + EIGEN_USING_STD(isfinite); + return isfinite EIGEN_NOT_A_MACRO(x); +} + +template +EIGEN_DEVICE_FUNC std::enable_if_t::has_infinity, bool> isinf_impl(const T&) { + return false; +} + +template +EIGEN_DEVICE_FUNC std::enable_if_t<(std::numeric_limits::has_infinity && !NumTraits::IsComplex), bool> isinf_impl( + const T& x) { + EIGEN_USING_STD(isinf); + return isinf EIGEN_NOT_A_MACRO(x); +} + +template +EIGEN_DEVICE_FUNC + std::enable_if_t::has_quiet_NaN || std::numeric_limits::has_signaling_NaN), bool> + isnan_impl(const T&) { + return false; +} + +template +EIGEN_DEVICE_FUNC std::enable_if_t< + (std::numeric_limits::has_quiet_NaN || std::numeric_limits::has_signaling_NaN) && (!NumTraits::IsComplex), + bool> +isnan_impl(const T& x) { + EIGEN_USING_STD(isnan); + return isnan EIGEN_NOT_A_MACRO(x); +} + +// The following overload are defined at the end of this file +template +EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex& x); +template +EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex& x); +template +EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex& x); +template +T generic_fast_tanh_float(const T& a_x); + +/**************************************************************************** + * Implementation of sign * + ****************************************************************************/ +template ::IsComplex != 0), + bool IsInteger = (NumTraits::IsInteger != 0)> +struct sign_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& a) { return Scalar((a > Scalar(0)) - (a < Scalar(0))); } +}; + +template +struct sign_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& a) { + return (isnan_impl)(a) ? a : Scalar((a > Scalar(0)) - (a < Scalar(0))); + } +}; + +template +struct sign_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& a) { + using real_type = typename NumTraits::Real; + EIGEN_USING_STD(abs); + real_type aa = abs(a); + if (aa == real_type(0)) return Scalar(0); + aa = real_type(1) / aa; + return Scalar(a.real() * aa, a.imag() * aa); + } +}; + +// The sign function for bool is the identity. +template <> +struct sign_impl { + EIGEN_DEVICE_FUNC static inline bool run(const bool& a) { return a; } +}; + +template +struct sign_retval { + typedef Scalar type; +}; + +template ::type>::IsInteger> +struct nearest_integer_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_floor(const Scalar& x) { + EIGEN_USING_STD(floor) return floor(x); + } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_ceil(const Scalar& x) { + EIGEN_USING_STD(ceil) return ceil(x); + } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_rint(const Scalar& x) { + EIGEN_USING_STD(rint) return rint(x); + } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_round(const Scalar& x) { + EIGEN_USING_STD(round) return round(x); + } +}; +template +struct nearest_integer_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_floor(const Scalar& x) { return x; } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_ceil(const Scalar& x) { return x; } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_rint(const Scalar& x) { return x; } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_round(const Scalar& x) { return x; } +}; + +} // end namespace internal + +/**************************************************************************** + * Generic math functions * + ****************************************************************************/ + +namespace numext { + +#if (!defined(EIGEN_GPUCC) || defined(EIGEN_CONSTEXPR_ARE_DEVICE_FUNC)) +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) { + EIGEN_USING_STD(min) + return min EIGEN_NOT_A_MACRO(x, y); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y) { + EIGEN_USING_STD(max) + return max EIGEN_NOT_A_MACRO(x, y); +} +#else +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) { + return y < x ? y : x; +} +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float mini(const float& x, const float& y) { + return fminf(x, y); +} +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double mini(const double& x, const double& y) { + return fmin(x, y); +} + +#ifndef EIGEN_GPU_COMPILE_PHASE +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE long double mini(const long double& x, const long double& y) { +#if defined(EIGEN_HIPCC) + // no "fminl" on HIP yet + return (x < y) ? x : y; +#else + return fminl(x, y); +#endif +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y) { + return x < y ? y : x; +} +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float maxi(const float& x, const float& y) { + return fmaxf(x, y); +} +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double maxi(const double& x, const double& y) { + return fmax(x, y); +} +#ifndef EIGEN_GPU_COMPILE_PHASE +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE long double maxi(const long double& x, const long double& y) { +#if defined(EIGEN_HIPCC) + // no "fmaxl" on HIP yet + return (x > y) ? x : y; +#else + return fmaxl(x, y); +#endif +} +#endif +#endif + +#if defined(SYCL_DEVICE_ONLY) + +#define SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_char) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_short) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_int) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_long) +#define SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_char) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_short) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_int) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_long) +#define SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uint) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_ulong) +#define SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uint) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_ulong) +#define SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(NAME, FUNC) \ + SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \ + SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) +#define SYCL_SPECIALIZE_INTEGER_TYPES_UNARY(NAME, FUNC) \ + SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \ + SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) +#define SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(NAME, FUNC) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_float) \ + SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_double) +#define SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(NAME, FUNC) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_float) \ + SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_double) +#define SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(NAME, FUNC, RET_TYPE) \ + SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, cl::sycl::cl_float) \ + SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, cl::sycl::cl_double) + +#define SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE) \ + template <> \ + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE& x) { \ + return cl::sycl::FUNC(x); \ + } + +#define SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, TYPE) SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, TYPE, TYPE) + +#define SYCL_SPECIALIZE_GEN1_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE1, ARG_TYPE2) \ + template <> \ + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE1& x, const ARG_TYPE2& y) { \ + return cl::sycl::FUNC(x, y); \ + } + +#define SYCL_SPECIALIZE_GEN2_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE) \ + SYCL_SPECIALIZE_GEN1_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE, ARG_TYPE) + +#define SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, TYPE) SYCL_SPECIALIZE_GEN2_BINARY_FUNC(NAME, FUNC, TYPE, TYPE) + +SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(mini, min) +SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(mini, fmin) +SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(maxi, max) +SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(maxi, fmax) + +#endif + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(real, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline internal::add_const_on_value_type_t real_ref( + const Scalar& x) { + return internal::real_ref_impl::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x) { + return EIGEN_MATHFUNC_IMPL(real_ref, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(arg, Scalar) arg(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(arg, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline internal::add_const_on_value_type_t imag_ref( + const Scalar& x) { + return internal::imag_ref_impl::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) imag_ref(Scalar& x) { + return EIGEN_MATHFUNC_IMPL(imag_ref, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(conj, Scalar) conj(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(conj, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(sign, Scalar) sign(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(sign, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x); +} + +EIGEN_DEVICE_FUNC inline bool abs2(bool x) { return x; } + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T absdiff(const T& x, const T& y) { + return x > y ? x - y : y - x; +} +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float absdiff(const float& x, const float& y) { + return fabsf(x - y); +} +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double absdiff(const double& x, const double& y) { + return fabs(x - y); +} + +// HIP and CUDA do not support long double. +#ifndef EIGEN_GPU_COMPILE_PHASE +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE long double absdiff(const long double& x, const long double& y) { + return fabsl(x - y); +} +#endif + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(norm1, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(hypot, Scalar) hypot(const Scalar& x, const Scalar& y) { + return EIGEN_MATHFUNC_IMPL(hypot, Scalar)::run(x, y); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(hypot, hypot) +#endif + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(log1p, Scalar) log1p(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(log1p, Scalar)::run(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(log1p, log1p) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float log1p(const float& x) { + return ::log1pf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double log1p(const double& x) { + return ::log1p(x); +} +#endif + +template +EIGEN_DEVICE_FUNC inline typename internal::pow_impl::result_type pow(const ScalarX& x, + const ScalarY& y) { + return internal::pow_impl::run(x, y); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(pow, pow) +#endif + +template +EIGEN_DEVICE_FUNC bool(isnan)(const T& x) { + return internal::isnan_impl(x); +} +template +EIGEN_DEVICE_FUNC bool(isinf)(const T& x) { + return internal::isinf_impl(x); +} +template +EIGEN_DEVICE_FUNC bool(isfinite)(const T& x) { + return internal::isfinite_impl(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isnan, isnan, bool) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isinf, isinf, bool) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isfinite, isfinite, bool) +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar rint(const Scalar& x) { + return internal::nearest_integer_impl::run_rint(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar round(const Scalar& x) { + return internal::nearest_integer_impl::run_round(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(round, round) +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar(floor)(const Scalar& x) { + return internal::nearest_integer_impl::run_floor(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(floor, floor) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float floor(const float& x) { + return ::floorf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double floor(const double& x) { + return ::floor(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar(ceil)(const Scalar& x) { + return internal::nearest_integer_impl::run_ceil(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(ceil, ceil) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float ceil(const float& x) { + return ::ceilf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double ceil(const double& x) { + return ::ceil(x); +} +#endif + +// Integer division with rounding up. +// T is assumed to be an integer type with a>=0, and b>0 +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_CONSTEXPR T div_ceil(T a, T b) { + EIGEN_STATIC_ASSERT((NumTraits::IsInteger), THIS FUNCTION IS FOR INTEGER TYPES) + eigen_assert(a >= 0); + eigen_assert(b > 0); + // Note: This form is used because it cannot overflow. + return a == 0 ? 0 : (a - 1) / b + 1; +} + +/** Log base 2 for 32 bits positive integers. + * Conveniently returns 0 for x==0. */ +inline int log2(int x) { + eigen_assert(x >= 0); + unsigned int v(x); + static const int table[32] = {0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30, + 8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31}; + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + return table[(v * 0x07C4ACDDU) >> 27]; +} + +/** \returns the square root of \a x. + * + * It is essentially equivalent to + * \code using std::sqrt; return sqrt(x); \endcode + * but slightly faster for float/double and some compilers (e.g., gcc), thanks to + * specializations when SSE is enabled. + * + * It's usage is justified in performance critical functions, like norm/normalize. + */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_MATHFUNC_RETVAL(sqrt, Scalar) sqrt(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(sqrt, Scalar)::run(x); +} + +// Boolean specialization, avoids implicit float to bool conversion (-Wimplicit-conversion-floating-point-to-bool). +template <> +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_DEVICE_FUNC bool sqrt(const bool& x) { + return x; +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sqrt, sqrt) +#endif + +/** \returns the cube root of \a x. **/ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cbrt(const T& x) { + EIGEN_USING_STD(cbrt); + return static_cast(cbrt(x)); +} + +/** \returns the reciprocal square root of \a x. **/ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T rsqrt(const T& x) { + return internal::rsqrt_impl::run(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T log(const T& x) { + return internal::log_impl::run(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(log, log) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float log(const float& x) { + return ::logf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double log(const double& x) { + return ::log(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE + std::enable_if_t::IsSigned || NumTraits::IsComplex, typename NumTraits::Real> + abs(const T& x) { + EIGEN_USING_STD(abs); + return abs(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE + std::enable_if_t::IsSigned || NumTraits::IsComplex), typename NumTraits::Real> + abs(const T& x) { + return x; +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_INTEGER_TYPES_UNARY(abs, abs) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(abs, fabs) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float abs(const float& x) { + return ::fabsf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double abs(const double& x) { + return ::fabs(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float abs(const std::complex& x) { + return ::hypotf(x.real(), x.imag()); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double abs(const std::complex& x) { + return ::hypot(x.real(), x.imag()); +} +#endif + +template ::IsInteger, bool IsSigned = NumTraits::IsSigned> +struct signbit_impl; +template +struct signbit_impl { + static constexpr size_t Size = sizeof(Scalar); + static constexpr size_t Shift = (CHAR_BIT * Size) - 1; + using intSize_t = typename get_integer_by_size::signed_type; + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static Scalar run(const Scalar& x) { + intSize_t a = bit_cast(x); + a = a >> Shift; + Scalar result = bit_cast(a); + return result; + } +}; +template +struct signbit_impl { + static constexpr size_t Size = sizeof(Scalar); + static constexpr size_t Shift = (CHAR_BIT * Size) - 1; + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Scalar run(const Scalar& x) { return x >> Shift; } +}; +template +struct signbit_impl { + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Scalar run(const Scalar&) { return Scalar(0); } +}; +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static constexpr Scalar signbit(const Scalar& x) { + return signbit_impl::run(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T exp(const T& x) { + EIGEN_USING_STD(exp); + return exp(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(exp, exp) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float exp(const float& x) { + return ::expf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double exp(const double& x) { + return ::exp(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE std::complex exp(const std::complex& x) { + float com = ::expf(x.real()); + float res_real = com * ::cosf(x.imag()); + float res_imag = com * ::sinf(x.imag()); + return std::complex(res_real, res_imag); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE std::complex exp(const std::complex& x) { + double com = ::exp(x.real()); + double res_real = com * ::cos(x.imag()); + double res_imag = com * ::sin(x.imag()); + return std::complex(res_real, res_imag); +} +#endif + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(expm1, Scalar) expm1(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(expm1, Scalar)::run(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(expm1, expm1) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float expm1(const float& x) { + return ::expm1f(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double expm1(const double& x) { + return ::expm1(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cos(const T& x) { + EIGEN_USING_STD(cos); + return cos(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(cos, cos) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float cos(const float& x) { + return ::cosf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double cos(const double& x) { + return ::cos(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T sin(const T& x) { + EIGEN_USING_STD(sin); + return sin(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sin, sin) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float sin(const float& x) { + return ::sinf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double sin(const double& x) { + return ::sin(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T tan(const T& x) { + EIGEN_USING_STD(tan); + return tan(x); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(tan, tan) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float tan(const float& x) { + return ::tanf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double tan(const double& x) { + return ::tan(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T acos(const T& x) { + EIGEN_USING_STD(acos); + return acos(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T acosh(const T& x) { + EIGEN_USING_STD(acosh); + return static_cast(acosh(x)); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(acos, acos) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(acosh, acosh) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float acos(const float& x) { + return ::acosf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double acos(const double& x) { + return ::acos(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T asin(const T& x) { + EIGEN_USING_STD(asin); + return asin(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T asinh(const T& x) { + EIGEN_USING_STD(asinh); + return static_cast(asinh(x)); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(asin, asin) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(asinh, asinh) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float asin(const float& x) { + return ::asinf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double asin(const double& x) { + return ::asin(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T atan(const T& x) { + EIGEN_USING_STD(atan); + return static_cast(atan(x)); +} + +template ::IsComplex, int> = 0> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T atan2(const T& y, const T& x) { + EIGEN_USING_STD(atan2); + return static_cast(atan2(y, x)); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T atanh(const T& x) { + EIGEN_USING_STD(atanh); + return static_cast(atanh(x)); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(atan, atan) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(atanh, atanh) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float atan(const float& x) { + return ::atanf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double atan(const double& x) { + return ::atan(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cosh(const T& x) { + EIGEN_USING_STD(cosh); + return static_cast(cosh(x)); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(cosh, cosh) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float cosh(const float& x) { + return ::coshf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double cosh(const double& x) { + return ::cosh(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T sinh(const T& x) { + EIGEN_USING_STD(sinh); + return static_cast(sinh(x)); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sinh, sinh) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float sinh(const float& x) { + return ::sinhf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double sinh(const double& x) { + return ::sinh(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T tanh(const T& x) { + EIGEN_USING_STD(tanh); + return tanh(x); +} + +#if (!defined(EIGEN_GPUCC)) && EIGEN_FAST_MATH && !defined(SYCL_DEVICE_ONLY) +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float tanh(float x) { return internal::generic_fast_tanh_float(x); } +#endif + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(tanh, tanh) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float tanh(const float& x) { + return ::tanhf(x); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double tanh(const double& x) { + return ::tanh(x); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T fmod(const T& a, const T& b) { + EIGEN_USING_STD(fmod); + return fmod(a, b); +} + +#if defined(SYCL_DEVICE_ONLY) +SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(fmod, fmod) +#endif + +#if defined(EIGEN_GPUCC) +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE float fmod(const float& a, const float& b) { + return ::fmodf(a, b); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double fmod(const double& a, const double& b) { + return ::fmod(a, b); +} +#endif + +#if defined(SYCL_DEVICE_ONLY) +#undef SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY +#undef SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY +#undef SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY +#undef SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY +#undef SYCL_SPECIALIZE_INTEGER_TYPES_BINARY +#undef SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY +#undef SYCL_SPECIALIZE_FLOATING_TYPES_BINARY +#undef SYCL_SPECIALIZE_FLOATING_TYPES_UNARY +#undef SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE +#undef SYCL_SPECIALIZE_GEN_UNARY_FUNC +#undef SYCL_SPECIALIZE_UNARY_FUNC +#undef SYCL_SPECIALIZE_GEN1_BINARY_FUNC +#undef SYCL_SPECIALIZE_GEN2_BINARY_FUNC +#undef SYCL_SPECIALIZE_BINARY_FUNC +#endif + +} // end namespace numext + +namespace internal { + +template +EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex& x) { + return (numext::isfinite)(numext::real(x)) && (numext::isfinite)(numext::imag(x)); +} + +template +EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex& x) { + return (numext::isnan)(numext::real(x)) || (numext::isnan)(numext::imag(x)); +} + +template +EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex& x) { + return ((numext::isinf)(numext::real(x)) || (numext::isinf)(numext::imag(x))) && (!(numext::isnan)(x)); +} + +/**************************************************************************** + * Implementation of fuzzy comparisons * + ****************************************************************************/ + +template +struct scalar_fuzzy_default_impl {}; + +template +struct scalar_fuzzy_default_impl { + typedef typename NumTraits::Real RealScalar; + template + EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, + const RealScalar& prec) { + return numext::abs(x) <= numext::abs(y) * prec; + } + EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) { + return numext::abs(x - y) <= numext::mini(numext::abs(x), numext::abs(y)) * prec; + } + EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar& prec) { + return x <= y || isApprox(x, y, prec); + } +}; + +template +struct scalar_fuzzy_default_impl { + typedef typename NumTraits::Real RealScalar; + template + EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const Scalar&, const RealScalar&) { + return x == Scalar(0); + } + EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar&) { return x == y; } + EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar&) { + return x <= y; + } +}; + +template +struct scalar_fuzzy_default_impl { + typedef typename NumTraits::Real RealScalar; + template + EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, + const RealScalar& prec) { + return numext::abs2(x) <= numext::abs2(y) * prec * prec; + } + EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) { + return numext::abs2(x - y) <= numext::mini(numext::abs2(x), numext::abs2(y)) * prec * prec; + } +}; + +template +struct scalar_fuzzy_impl + : scalar_fuzzy_default_impl::IsComplex, NumTraits::IsInteger> {}; + +template +EIGEN_DEVICE_FUNC inline bool isMuchSmallerThan( + const Scalar& x, const OtherScalar& y, + const typename NumTraits::Real& precision = NumTraits::dummy_precision()) { + return scalar_fuzzy_impl::template isMuchSmallerThan(x, y, precision); +} + +template +EIGEN_DEVICE_FUNC inline bool isApprox( + const Scalar& x, const Scalar& y, + const typename NumTraits::Real& precision = NumTraits::dummy_precision()) { + return scalar_fuzzy_impl::isApprox(x, y, precision); +} + +template +EIGEN_DEVICE_FUNC inline bool isApproxOrLessThan( + const Scalar& x, const Scalar& y, + const typename NumTraits::Real& precision = NumTraits::dummy_precision()) { + return scalar_fuzzy_impl::isApproxOrLessThan(x, y, precision); +} + +/****************************************** +*** The special case of the bool type *** +******************************************/ + +template <> +struct random_impl { + static inline bool run() { return random(0, 1) == 0 ? false : true; } + + static inline bool run(const bool& a, const bool& b) { return random(a, b) == 0 ? false : true; } +}; + +template <> +struct scalar_fuzzy_impl { + typedef bool RealScalar; + + template + EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const bool& x, const bool&, const bool&) { + return !x; + } + + EIGEN_DEVICE_FUNC static inline bool isApprox(bool x, bool y, bool) { return x == y; } + + EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const bool& x, const bool& y, const bool&) { + return (!x) || y; + } +}; + +} // end namespace internal + +// Default implementations that rely on other numext implementations +namespace internal { + +// Specialization for complex types that are not supported by std::expm1. +template +struct expm1_impl> { + EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar) + + EIGEN_DEVICE_FUNC static inline std::complex run(const std::complex& x) { + RealScalar xr = x.real(); + RealScalar xi = x.imag(); + // expm1(z) = exp(z) - 1 + // = exp(x + i * y) - 1 + // = exp(x) * (cos(y) + i * sin(y)) - 1 + // = exp(x) * cos(y) - 1 + i * exp(x) * sin(y) + // Imag(expm1(z)) = exp(x) * sin(y) + // Real(expm1(z)) = exp(x) * cos(y) - 1 + // = exp(x) * cos(y) - 1. + // = expm1(x) + exp(x) * (cos(y) - 1) + // = expm1(x) + exp(x) * (2 * sin(y / 2) ** 2) + RealScalar erm1 = numext::expm1(xr); + RealScalar er = erm1 + RealScalar(1.); + RealScalar sin2 = numext::sin(xi / RealScalar(2.)); + sin2 = sin2 * sin2; + RealScalar s = numext::sin(xi); + RealScalar real_part = erm1 - RealScalar(2.) * er * sin2; + return std::complex(real_part, er * s); + } +}; + +template +struct rsqrt_impl { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE T run(const T& x) { return T(1) / numext::sqrt(x); } +}; + +#if defined(EIGEN_GPU_COMPILE_PHASE) +template +struct conj_impl, true> { + EIGEN_DEVICE_FUNC static inline std::complex run(const std::complex& x) { + return std::complex(numext::real(x), -numext::imag(x)); + } +}; +#endif + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_MATHFUNCTIONS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h new file mode 100644 index 00000000..ed44089e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h @@ -0,0 +1,321 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Pedro Gonnet (pedro.gonnet@gmail.com) +// Copyright (C) 2016 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_MATHFUNCTIONSIMPL_H +#define EIGEN_MATHFUNCTIONSIMPL_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +/** \internal Fast reciprocal using Newton-Raphson's method. + + Preconditions: + 1. The starting guess provided in approx_a_recip must have at least half + the leading mantissa bits in the correct result, such that a single + Newton-Raphson step is sufficient to get within 1-2 ulps of the currect + result. + 2. If a is zero, approx_a_recip must be infinite with the same sign as a. + 3. If a is infinite, approx_a_recip must be zero with the same sign as a. + + If the preconditions are satisfied, which they are for for the _*_rcp_ps + instructions on x86, the result has a maximum relative error of 2 ulps, + and correctly handles reciprocals of zero, infinity, and NaN. +*/ +template +struct generic_reciprocal_newton_step { + static_assert(Steps > 0, "Steps must be at least 1."); + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& a, const Packet& approx_a_recip) { + using Scalar = typename unpacket_traits::type; + const Packet two = pset1(Scalar(2)); + // Refine the approximation using one Newton-Raphson step: + // x_{i} = x_{i-1} * (2 - a * x_{i-1}) + const Packet x = generic_reciprocal_newton_step::run(a, approx_a_recip); + const Packet tmp = pnmadd(a, x, two); + // If tmp is NaN, it means that a is either +/-0 or +/-Inf. + // In this case return the approximation directly. + const Packet is_not_nan = pcmp_eq(tmp, tmp); + return pselect(is_not_nan, pmul(x, tmp), x); + } +}; + +template +struct generic_reciprocal_newton_step { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& /*unused*/, const Packet& approx_rsqrt) { + return approx_rsqrt; + } +}; + +/** \internal Fast reciprocal sqrt using Newton-Raphson's method. + + Preconditions: + 1. The starting guess provided in approx_a_recip must have at least half + the leading mantissa bits in the correct result, such that a single + Newton-Raphson step is sufficient to get within 1-2 ulps of the currect + result. + 2. If a is zero, approx_a_recip must be infinite with the same sign as a. + 3. If a is infinite, approx_a_recip must be zero with the same sign as a. + + If the preconditions are satisfied, which they are for for the _*_rcp_ps + instructions on x86, the result has a maximum relative error of 2 ulps, + and correctly handles zero, infinity, and NaN. Positive denormals are + treated as zero. +*/ +template +struct generic_rsqrt_newton_step { + static_assert(Steps > 0, "Steps must be at least 1."); + using Scalar = typename unpacket_traits::type; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& a, const Packet& approx_rsqrt) { + constexpr Scalar kMinusHalf = Scalar(-1) / Scalar(2); + const Packet cst_minus_half = pset1(kMinusHalf); + const Packet cst_minus_one = pset1(Scalar(-1)); + + Packet inv_sqrt = approx_rsqrt; + for (int step = 0; step < Steps; ++step) { + // Refine the approximation using one Newton-Raphson step: + // h_n = (x * inv_sqrt) * inv_sqrt - 1 (so that h_n is nearly 0). + // inv_sqrt = inv_sqrt - 0.5 * inv_sqrt * h_n + Packet r2 = pmul(a, inv_sqrt); + Packet half_r = pmul(inv_sqrt, cst_minus_half); + Packet h_n = pmadd(r2, inv_sqrt, cst_minus_one); + inv_sqrt = pmadd(half_r, h_n, inv_sqrt); + } + + // If x is NaN, then either: + // 1) the input is NaN + // 2) zero and infinity were multiplied + // In either of these cases, return approx_rsqrt + return pselect(pisnan(inv_sqrt), approx_rsqrt, inv_sqrt); + } +}; + +template +struct generic_rsqrt_newton_step { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& /*unused*/, const Packet& approx_rsqrt) { + return approx_rsqrt; + } +}; + +/** \internal Fast sqrt using Newton-Raphson's method. + + Preconditions: + 1. The starting guess for the reciprocal sqrt provided in approx_rsqrt must + have at least half the leading mantissa bits in the correct result, such + that a single Newton-Raphson step is sufficient to get within 1-2 ulps of + the currect result. + 2. If a is zero, approx_rsqrt must be infinite. + 3. If a is infinite, approx_rsqrt must be zero. + + If the preconditions are satisfied, which they are for for the _*_rsqrt_ps + instructions on x86, the result has a maximum relative error of 2 ulps, + and correctly handles zero and infinity, and NaN. Positive denormal inputs + are treated as zero. +*/ +template +struct generic_sqrt_newton_step { + static_assert(Steps > 0, "Steps must be at least 1."); + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& a, const Packet& approx_rsqrt) { + using Scalar = typename unpacket_traits::type; + const Packet one_point_five = pset1(Scalar(1.5)); + const Packet minus_half = pset1(Scalar(-0.5)); + // If a is inf or zero, return a directly. + const Packet inf_mask = pcmp_eq(a, pset1(NumTraits::infinity())); + const Packet return_a = por(pcmp_eq(a, pzero(a)), inf_mask); + // Do a single step of Newton's iteration for reciprocal square root: + // x_{n+1} = x_n * (1.5 + (-0.5 * x_n) * (a * x_n))). + // The Newton's step is computed this way to avoid over/under-flows. + Packet rsqrt = pmul(approx_rsqrt, pmadd(pmul(minus_half, approx_rsqrt), pmul(a, approx_rsqrt), one_point_five)); + for (int step = 1; step < Steps; ++step) { + rsqrt = pmul(rsqrt, pmadd(pmul(minus_half, rsqrt), pmul(a, rsqrt), one_point_five)); + } + + // Return sqrt(x) = x * rsqrt(x) for non-zero finite positive arguments. + // Return a itself for 0 or +inf, NaN for negative arguments. + return pselect(return_a, a, pmul(a, rsqrt)); + } +}; + +/** \internal \returns the hyperbolic tan of \a a (coeff-wise) + Doesn't do anything fancy, just a 13/6-degree rational interpolant which + is accurate up to a couple of ulps in the (approximate) range [-8, 8], + outside of which tanh(x) = +/-1 in single precision. The input is clamped + to the range [-c, c]. The value c is chosen as the smallest value where + the approximation evaluates to exactly 1. In the reange [-0.0004, 0.0004] + the approximation tanh(x) ~= x is used for better accuracy as x tends to zero. + + This implementation works on both scalars and packets. +*/ +template +T generic_fast_tanh_float(const T& a_x) { + // Clamp the inputs to the range [-c, c] +#ifdef EIGEN_VECTORIZE_FMA + const T plus_clamp = pset1(7.99881172180175781f); + const T minus_clamp = pset1(-7.99881172180175781f); +#else + const T plus_clamp = pset1(7.90531110763549805f); + const T minus_clamp = pset1(-7.90531110763549805f); +#endif + const T tiny = pset1(0.0004f); + const T x = pmax(pmin(a_x, plus_clamp), minus_clamp); + const T tiny_mask = pcmp_lt(pabs(a_x), tiny); + // The monomial coefficients of the numerator polynomial (odd). + const T alpha_1 = pset1(4.89352455891786e-03f); + const T alpha_3 = pset1(6.37261928875436e-04f); + const T alpha_5 = pset1(1.48572235717979e-05f); + const T alpha_7 = pset1(5.12229709037114e-08f); + const T alpha_9 = pset1(-8.60467152213735e-11f); + const T alpha_11 = pset1(2.00018790482477e-13f); + const T alpha_13 = pset1(-2.76076847742355e-16f); + + // The monomial coefficients of the denominator polynomial (even). + const T beta_0 = pset1(4.89352518554385e-03f); + const T beta_2 = pset1(2.26843463243900e-03f); + const T beta_4 = pset1(1.18534705686654e-04f); + const T beta_6 = pset1(1.19825839466702e-06f); + + // Since the polynomials are odd/even, we need x^2. + const T x2 = pmul(x, x); + + // Evaluate the numerator polynomial p. + T p = pmadd(x2, alpha_13, alpha_11); + p = pmadd(x2, p, alpha_9); + p = pmadd(x2, p, alpha_7); + p = pmadd(x2, p, alpha_5); + p = pmadd(x2, p, alpha_3); + p = pmadd(x2, p, alpha_1); + p = pmul(x, p); + + // Evaluate the denominator polynomial q. + T q = pmadd(x2, beta_6, beta_4); + q = pmadd(x2, q, beta_2); + q = pmadd(x2, q, beta_0); + + // Divide the numerator by the denominator. + return pselect(tiny_mask, x, pdiv(p, q)); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE RealScalar positive_real_hypot(const RealScalar& x, const RealScalar& y) { + // IEEE IEC 6059 special cases. + if ((numext::isinf)(x) || (numext::isinf)(y)) return NumTraits::infinity(); + if ((numext::isnan)(x) || (numext::isnan)(y)) return NumTraits::quiet_NaN(); + + EIGEN_USING_STD(sqrt); + RealScalar p, qp; + p = numext::maxi(x, y); + if (numext::is_exactly_zero(p)) return RealScalar(0); + qp = numext::mini(y, x) / p; + return p * sqrt(RealScalar(1) + qp * qp); +} + +template +struct hypot_impl { + typedef typename NumTraits::Real RealScalar; + static EIGEN_DEVICE_FUNC inline RealScalar run(const Scalar& x, const Scalar& y) { + EIGEN_USING_STD(abs); + return positive_real_hypot(abs(x), abs(y)); + } +}; + +// Generic complex sqrt implementation that correctly handles corner cases +// according to https://en.cppreference.com/w/cpp/numeric/complex/sqrt +template +EIGEN_DEVICE_FUNC std::complex complex_sqrt(const std::complex& z) { + // Computes the principal sqrt of the input. + // + // For a complex square root of the number x + i*y. We want to find real + // numbers u and v such that + // (u + i*v)^2 = x + i*y <=> + // u^2 - v^2 + i*2*u*v = x + i*v. + // By equating the real and imaginary parts we get: + // u^2 - v^2 = x + // 2*u*v = y. + // + // For x >= 0, this has the numerically stable solution + // u = sqrt(0.5 * (x + sqrt(x^2 + y^2))) + // v = y / (2 * u) + // and for x < 0, + // v = sign(y) * sqrt(0.5 * (-x + sqrt(x^2 + y^2))) + // u = y / (2 * v) + // + // Letting w = sqrt(0.5 * (|x| + |z|)), + // if x == 0: u = w, v = sign(y) * w + // if x > 0: u = w, v = y / (2 * w) + // if x < 0: u = |y| / (2 * w), v = sign(y) * w + + const T x = numext::real(z); + const T y = numext::imag(z); + const T zero = T(0); + const T w = numext::sqrt(T(0.5) * (numext::abs(x) + numext::hypot(x, y))); + + return (numext::isinf)(y) ? std::complex(NumTraits::infinity(), y) + : numext::is_exactly_zero(x) ? std::complex(w, y < zero ? -w : w) + : x > zero ? std::complex(w, y / (2 * w)) + : std::complex(numext::abs(y) / (2 * w), y < zero ? -w : w); +} + +// Generic complex rsqrt implementation. +template +EIGEN_DEVICE_FUNC std::complex complex_rsqrt(const std::complex& z) { + // Computes the principal reciprocal sqrt of the input. + // + // For a complex reciprocal square root of the number z = x + i*y. We want to + // find real numbers u and v such that + // (u + i*v)^2 = 1 / (x + i*y) <=> + // u^2 - v^2 + i*2*u*v = x/|z|^2 - i*v/|z|^2. + // By equating the real and imaginary parts we get: + // u^2 - v^2 = x/|z|^2 + // 2*u*v = y/|z|^2. + // + // For x >= 0, this has the numerically stable solution + // u = sqrt(0.5 * (x + |z|)) / |z| + // v = -y / (2 * u * |z|) + // and for x < 0, + // v = -sign(y) * sqrt(0.5 * (-x + |z|)) / |z| + // u = -y / (2 * v * |z|) + // + // Letting w = sqrt(0.5 * (|x| + |z|)), + // if x == 0: u = w / |z|, v = -sign(y) * w / |z| + // if x > 0: u = w / |z|, v = -y / (2 * w * |z|) + // if x < 0: u = |y| / (2 * w * |z|), v = -sign(y) * w / |z| + + const T x = numext::real(z); + const T y = numext::imag(z); + const T zero = T(0); + + const T abs_z = numext::hypot(x, y); + const T w = numext::sqrt(T(0.5) * (numext::abs(x) + abs_z)); + const T woz = w / abs_z; + // Corner cases consistent with 1/sqrt(z) on gcc/clang. + return numext::is_exactly_zero(abs_z) ? std::complex(NumTraits::infinity(), NumTraits::quiet_NaN()) + : ((numext::isinf)(x) || (numext::isinf)(y)) ? std::complex(zero, zero) + : numext::is_exactly_zero(x) ? std::complex(woz, y < zero ? woz : -woz) + : x > zero ? std::complex(woz, -y / (2 * w * abs_z)) + : std::complex(numext::abs(y) / (2 * w * abs_z), y < zero ? woz : -woz); +} + +template +EIGEN_DEVICE_FUNC std::complex complex_log(const std::complex& z) { + // Computes complex log. + T a = numext::abs(z); + EIGEN_USING_STD(atan2); + T b = atan2(z.imag(), z.real()); + return std::complex(numext::log(a), b); +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_MATHFUNCTIONSIMPL_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Matrix.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Matrix.h new file mode 100644 index 00000000..ce0e4e6a --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Matrix.h @@ -0,0 +1,527 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2010 Benoit Jacob +// Copyright (C) 2008-2009 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_MATRIX_H +#define EIGEN_MATRIX_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits> { + private: + constexpr static int size = internal::size_at_compile_time(Rows_, Cols_); + typedef typename find_best_packet::type PacketScalar; + enum { + row_major_bit = Options_ & RowMajor ? RowMajorBit : 0, + is_dynamic_size_storage = MaxRows_ == Dynamic || MaxCols_ == Dynamic, + max_size = is_dynamic_size_storage ? Dynamic : MaxRows_ * MaxCols_, + default_alignment = compute_default_alignment::value, + actual_alignment = ((Options_ & DontAlign) == 0) ? default_alignment : 0, + required_alignment = unpacket_traits::alignment, + packet_access_bit = (packet_traits::Vectorizable && + (EIGEN_UNALIGNED_VECTORIZE || (actual_alignment >= required_alignment))) + ? PacketAccessBit + : 0 + }; + + public: + typedef Scalar_ Scalar; + typedef Dense StorageKind; + typedef Eigen::Index StorageIndex; + typedef MatrixXpr XprKind; + enum { + RowsAtCompileTime = Rows_, + ColsAtCompileTime = Cols_, + MaxRowsAtCompileTime = MaxRows_, + MaxColsAtCompileTime = MaxCols_, + Flags = compute_matrix_flags(Options_), + Options = Options_, + InnerStrideAtCompileTime = 1, + OuterStrideAtCompileTime = (Options & RowMajor) ? ColsAtCompileTime : RowsAtCompileTime, + + // FIXME, the following flag in only used to define NeedsToAlign in PlainObjectBase + EvaluatorFlags = LinearAccessBit | DirectAccessBit | packet_access_bit | row_major_bit, + Alignment = actual_alignment + }; +}; +} // namespace internal + +/** \class Matrix + * \ingroup Core_Module + * + * \brief The matrix class, also used for vectors and row-vectors + * + * The %Matrix class is the work-horse for all \em dense (\ref dense "note") matrices and vectors within Eigen. + * Vectors are matrices with one column, and row-vectors are matrices with one row. + * + * The %Matrix class encompasses \em both fixed-size and dynamic-size objects (\ref fixedsize "note"). + * + * The first three template parameters are required: + * \tparam Scalar_ Numeric type, e.g. float, double, int or std::complex. + * User defined scalar types are supported as well (see \ref user_defined_scalars "here"). + * \tparam Rows_ Number of rows, or \b Dynamic + * \tparam Cols_ Number of columns, or \b Dynamic + * + * The remaining template parameters are optional -- in most cases you don't have to worry about them. + * \tparam Options_ A combination of either \b #RowMajor or \b #ColMajor, and of either + * \b #AutoAlign or \b #DontAlign. + * The former controls \ref TopicStorageOrders "storage order", and defaults to column-major. The latter + * controls alignment, which is required for vectorization. It defaults to aligning matrices except for fixed sizes that + * aren't a multiple of the packet size. \tparam MaxRows_ Maximum number of rows. Defaults to \a Rows_ (\ref maxrows + * "note"). \tparam MaxCols_ Maximum number of columns. Defaults to \a Cols_ (\ref maxrows "note"). + * + * Eigen provides a number of typedefs covering the usual cases. Here are some examples: + * + * \li \c Matrix2d is a 2x2 square matrix of doubles (\c Matrix) + * \li \c Vector4f is a vector of 4 floats (\c Matrix) + * \li \c RowVector3i is a row-vector of 3 ints (\c Matrix) + * + * \li \c MatrixXf is a dynamic-size matrix of floats (\c Matrix) + * \li \c VectorXf is a dynamic-size vector of floats (\c Matrix) + * + * \li \c Matrix2Xf is a partially fixed-size (dynamic-size) matrix of floats (\c Matrix) + * \li \c MatrixX3d is a partially dynamic-size (fixed-size) matrix of double (\c Matrix) + * + * See \link matrixtypedefs this page \endlink for a complete list of predefined \em %Matrix and \em Vector typedefs. + * + * You can access elements of vectors and matrices using normal subscripting: + * + * \code + * Eigen::VectorXd v(10); + * v[0] = 0.1; + * v[1] = 0.2; + * v(0) = 0.3; + * v(1) = 0.4; + * + * Eigen::MatrixXi m(10, 10); + * m(0, 1) = 1; + * m(0, 2) = 2; + * m(0, 3) = 3; + * \endcode + * + * 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_MATRIX_PLUGIN. + * + * Some notes: + * + *

+ *
\anchor dense Dense versus sparse:
+ *
This %Matrix class handles dense, not sparse matrices and vectors. For sparse matrices and vectors, see the + * Sparse module. + * + * Dense matrices and vectors are plain usual arrays of coefficients. All the coefficients are stored, in an ordinary + * contiguous array. This is unlike Sparse matrices and vectors where the coefficients are stored as a list of nonzero + * coefficients.
+ * + *
\anchor fixedsize Fixed-size versus dynamic-size:
+ *
Fixed-size means that the numbers of rows and columns are known are compile-time. In this case, Eigen allocates + * the array of coefficients as a fixed-size array, as a class member. This makes sense for very small matrices, + * typically up to 4x4, sometimes up to 16x16. Larger matrices should be declared as dynamic-size even if one happens to + * know their size at compile-time. + * + * Dynamic-size means that the numbers of rows or columns are not necessarily known at compile-time. In this case they + * are runtime variables, and the array of coefficients is allocated dynamically on the heap. + * + * Note that \em dense matrices, be they Fixed-size or Dynamic-size, do not expand dynamically in the sense of + * a std::map. If you want this behavior, see the Sparse module.
+ * + *
\anchor maxrows MaxRows_ and MaxCols_:
+ *
In most cases, one just leaves these parameters to the default values. + * These parameters mean the maximum size of rows and columns that the matrix may have. They are useful in cases + * when the exact numbers of rows and columns are not known are compile-time, but it is known at compile-time that they + * cannot exceed a certain value. This happens when taking dynamic-size blocks inside fixed-size matrices: in this case + * MaxRows_ and MaxCols_ are the dimensions of the original matrix, while Rows_ and Cols_ are Dynamic.
+ *
+ * + * ABI and storage layout + * + * The table below summarizes the ABI of some possible Matrix instances which is fixed thorough the lifetime of Eigen 3. + * + * + * + * + * + * + *
Matrix typeEquivalent C structure
\code Matrix \endcode\code + * struct { + * T *data; // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0 + * Eigen::Index rows, cols; + * }; + * \endcode
\code + * Matrix + * Matrix \endcode\code + * struct { + * T *data; // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0 + * Eigen::Index size; + * }; + * \endcode
\code Matrix \endcode\code + * struct { + * T data[Rows*Cols]; // with (size_t(data)%A(Rows*Cols*sizeof(T)))==0 + * }; + * \endcode
\code Matrix \endcode\code + * struct { + * T data[MaxRows*MaxCols]; // with (size_t(data)%A(MaxRows*MaxCols*sizeof(T)))==0 + * Eigen::Index rows, cols; + * }; + * \endcode
+ * Note that in this table Rows, Cols, MaxRows and MaxCols are all positive integers. A(S) is defined to the largest + * possible power-of-two smaller to EIGEN_MAX_STATIC_ALIGN_BYTES. + * + * \see MatrixBase for the majority of the API methods for matrices, \ref TopicClassHierarchy, + * \ref TopicStorageOrders + */ + +template +class Matrix : public PlainObjectBase> { + public: + /** \brief Base class typedef. + * \sa PlainObjectBase + */ + typedef PlainObjectBase Base; + + enum { Options = Options_ }; + + EIGEN_DENSE_PUBLIC_INTERFACE(Matrix) + + typedef typename Base::PlainObject PlainObject; + + using Base::base; + using Base::coeffRef; + + /** + * \brief Assigns matrices to each other. + * + * \note This is a special case of the templated operator=. Its purpose is + * to prevent a default operator= from hiding the templated operator=. + * + * \callgraph + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const Matrix& other) { return Base::_set(other); } + + /** \internal + * \brief Copies the value of the expression \a other into \c *this with automatic resizing. + * + * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized), + * it will be initialized. + * + * Note that copying a row-vector into a vector (and conversely) is allowed. + * The resizing, if any, is then done in the appropriate way so that row-vectors + * remain row-vectors and vectors remain vectors. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const DenseBase& other) { + return Base::_set(other); + } + + /* Here, doxygen failed to copy the brief information when using \copydoc */ + + /** + * \brief Copies the generic expression \a other into *this. + * \copydetails DenseBase::operator=(const EigenBase &other) + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const EigenBase& other) { + return Base::operator=(other); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const ReturnByValue& func) { + return Base::operator=(func); + } + + /** \brief Default constructor. + * + * For fixed-size matrices, does nothing. + * + * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix + * is called a null matrix. This constructor is the unique way to create null matrices: resizing + * a matrix to 0 is not supported. + * + * \sa resize(Index,Index) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix() + : Base(){EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED} + + // FIXME is it still needed + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Matrix(internal::constructor_without_unaligned_array_assert) + : Base(internal::constructor_without_unaligned_array_assert()){EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(Matrix && other) + EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) + : Base(std::move(other)) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(Matrix&& other) + EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) { + Base::operator=(std::move(other)); + return *this; + } + + /** \copydoc PlainObjectBase(const Scalar&, const Scalar&, const Scalar&, const Scalar&, const ArgTypes&... args) + * + * Example: \include Matrix_variadic_ctor_cxx11.cpp + * Output: \verbinclude Matrix_variadic_ctor_cxx11.out + * + * \sa Matrix(const std::initializer_list>&) + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, + const ArgTypes&... args) + : Base(a0, a1, a2, a3, args...) {} + + /** \brief Constructs a Matrix and initializes it from the coefficients given as initializer-lists grouped by row. + * \cpp11 + * + * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients: + * + * Example: \include Matrix_initializer_list_23_cxx11.cpp + * Output: \verbinclude Matrix_initializer_list_23_cxx11.out + * + * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is + * triggered. + * + * In the case of a compile-time column vector, implicit transposition from a single row is allowed. + * Therefore VectorXd{{1,2,3,4,5}} is legal and the more verbose syntax + * RowVectorXd{{1},{2},{3},{4},{5}} can be avoided: + * + * Example: \include Matrix_initializer_list_vector_cxx11.cpp + * Output: \verbinclude Matrix_initializer_list_vector_cxx11.out + * + * In the case of fixed-sized matrices, the initializer list sizes must exactly match the matrix sizes, + * and implicit transposition is allowed for compile-time vectors only. + * + * \sa Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) + */ + EIGEN_DEVICE_FUNC explicit constexpr EIGEN_STRONG_INLINE Matrix( + const std::initializer_list>& list) + : Base(list) {} + +#ifndef EIGEN_PARSED_BY_DOXYGEN + + // This constructor is for both 1x1 matrices and dynamic vectors + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Matrix(const T& x) { + Base::template _init1(x); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const T0& x, const T1& y) { + Base::template _init2(x, y); + } + +#else + /** \brief Constructs a fixed-sized matrix initialized with coefficients starting at \a data */ + EIGEN_DEVICE_FUNC explicit Matrix(const Scalar* data); + + /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors + * + * This is useful for dynamic-size vectors. For fixed-size vectors, + * it is redundant to pass these parameters, so one should use the default constructor + * Matrix() instead. + * + * \warning This constructor is disabled for fixed-size \c 1x1 matrices. For instance, + * calling Matrix(1) will call the initialization constructor: Matrix(const Scalar&). + * For fixed-size \c 1x1 matrices it is therefore recommended to use the default + * constructor Matrix() instead, especially when using one of the non standard + * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives). + */ + EIGEN_STRONG_INLINE explicit Matrix(Index dim); + /** \brief Constructs an initialized 1x1 matrix with the given coefficient + * \sa Matrix(const Scalar&, const Scalar&, const Scalar&, const Scalar&, const ArgTypes&...) */ + Matrix(const Scalar& x); + /** \brief Constructs an uninitialized matrix with \a rows rows and \a cols columns. + * + * This is useful for dynamic-size matrices. For fixed-size matrices, + * it is redundant to pass these parameters, so one should use the default constructor + * Matrix() instead. + * + * \warning This constructor is disabled for fixed-size \c 1x2 and \c 2x1 vectors. For instance, + * calling Matrix2f(2,1) will call the initialization constructor: Matrix(const Scalar& x, const Scalar& y). + * For fixed-size \c 1x2 or \c 2x1 vectors it is therefore recommended to use the default + * constructor Matrix() instead, especially when using one of the non standard + * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives). + */ + EIGEN_DEVICE_FUNC Matrix(Index rows, Index cols); + + /** \brief Constructs an initialized 2D vector with given coefficients + * \sa Matrix(const Scalar&, const Scalar&, const Scalar&, const Scalar&, const ArgTypes&...) */ + Matrix(const Scalar& x, const Scalar& y); +#endif // end EIGEN_PARSED_BY_DOXYGEN + + /** \brief Constructs an initialized 3D vector with given coefficients + * \sa Matrix(const Scalar&, const Scalar&, const Scalar&, const Scalar&, const ArgTypes&...) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 3) + m_storage.data()[0] = x; + m_storage.data()[1] = y; + m_storage.data()[2] = z; + } + /** \brief Constructs an initialized 4D vector with given coefficients + * \sa Matrix(const Scalar&, const Scalar&, const Scalar&, const Scalar&, const ArgTypes&...) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 4) + m_storage.data()[0] = x; + m_storage.data()[1] = y; + m_storage.data()[2] = z; + m_storage.data()[3] = w; + } + + /** \brief Copy constructor */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Matrix& other) : Base(other) {} + + /** \brief Copy constructor for generic expressions. + * \sa MatrixBase::operator=(const EigenBase&) + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const EigenBase& other) : Base(other.derived()) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return 1; } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); } + + /////////// Geometry module /////////// + + template + EIGEN_DEVICE_FUNC explicit Matrix(const RotationBase& r); + template + EIGEN_DEVICE_FUNC Matrix& operator=(const RotationBase& r); + +// allow to extend Matrix outside Eigen +#ifdef EIGEN_MATRIX_PLUGIN +#include EIGEN_MATRIX_PLUGIN +#endif + + protected: + template + friend struct internal::conservative_resize_like_impl; + + using Base::m_storage; +}; + +/** \defgroup matrixtypedefs Global matrix typedefs + * + * \ingroup Core_Module + * + * %Eigen defines several typedef shortcuts for most common matrix and vector types. + * + * The general patterns are the following: + * + * \c MatrixSizeType where \c Size can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size, + * and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c cd + * for complex double. + * + * For example, \c Matrix3d is a fixed-size 3x3 matrix type of doubles, and \c MatrixXf is a dynamic-size matrix of + * floats. + * + * There are also \c VectorSizeType and \c RowVectorSizeType which are self-explanatory. For example, \c Vector4cf is + * a fixed-size vector of 4 complex floats. + * + * With \cpp11, template alias are also defined for common sizes. + * They follow the same pattern as above except that the scalar type suffix is replaced by a + * template parameter, i.e.: + * - `MatrixSize` where `Size` can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size. + * - `MatrixXSize` and `MatrixSizeX` where `Size` can be \c 2,\c 3,\c 4 for hybrid dynamic/fixed matrices. + * - `VectorSize` and `RowVectorSize` for column and row vectors. + * + * With \cpp11, you can also use fully generic column and row vector types: `Vector` and + * `RowVector`. + * + * \sa class Matrix + */ + +#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ + /** \ingroup matrixtypedefs */ \ + /** \brief `Size`×`Size` matrix of type `Type`. */ \ + typedef Matrix Matrix##SizeSuffix##TypeSuffix; \ + /** \ingroup matrixtypedefs */ \ + /** \brief `Size`×`1` vector of type `Type`. */ \ + typedef Matrix Vector##SizeSuffix##TypeSuffix; \ + /** \ingroup matrixtypedefs */ \ + /** \brief `1`×`Size` vector of type `Type`. */ \ + typedef Matrix RowVector##SizeSuffix##TypeSuffix; + +#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \ + /** \ingroup matrixtypedefs */ \ + /** \brief `Size`×`Dynamic` matrix of type `Type`. */ \ + typedef Matrix Matrix##Size##X##TypeSuffix; \ + /** \ingroup matrixtypedefs */ \ + /** \brief `Dynamic`×`Size` matrix of type `Type`. */ \ + typedef Matrix Matrix##X##Size##TypeSuffix; + +#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X) \ + EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 2) \ + EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 3) \ + EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 4) + +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i) +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f) +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d) +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex, cf) +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex, cd) + +#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES +#undef EIGEN_MAKE_TYPEDEFS +#undef EIGEN_MAKE_FIXED_TYPEDEFS + +#define EIGEN_MAKE_TYPEDEFS(Size, SizeSuffix) \ + /** \ingroup matrixtypedefs */ \ + /** \brief \cpp11 `Size`×`Size` matrix of type `Type`.*/ \ + template \ + using Matrix##SizeSuffix = Matrix; \ + /** \ingroup matrixtypedefs */ \ + /** \brief \cpp11 `Size`×`1` vector of type `Type`.*/ \ + template \ + using Vector##SizeSuffix = Matrix; \ + /** \ingroup matrixtypedefs */ \ + /** \brief \cpp11 `1`×`Size` vector of type `Type`.*/ \ + template \ + using RowVector##SizeSuffix = Matrix; + +#define EIGEN_MAKE_FIXED_TYPEDEFS(Size) \ + /** \ingroup matrixtypedefs */ \ + /** \brief \cpp11 `Size`×`Dynamic` matrix of type `Type` */ \ + template \ + using Matrix##Size##X = Matrix; \ + /** \ingroup matrixtypedefs */ \ + /** \brief \cpp11 `Dynamic`×`Size` matrix of type `Type`. */ \ + template \ + using Matrix##X##Size = Matrix; + +EIGEN_MAKE_TYPEDEFS(2, 2) +EIGEN_MAKE_TYPEDEFS(3, 3) +EIGEN_MAKE_TYPEDEFS(4, 4) +EIGEN_MAKE_TYPEDEFS(Dynamic, X) +EIGEN_MAKE_FIXED_TYPEDEFS(2) +EIGEN_MAKE_FIXED_TYPEDEFS(3) +EIGEN_MAKE_FIXED_TYPEDEFS(4) + +/** \ingroup matrixtypedefs + * \brief \cpp11 `Size`×`1` vector of type `Type`. */ +template +using Vector = Matrix; + +/** \ingroup matrixtypedefs + * \brief \cpp11 `1`×`Size` vector of type `Type`. */ +template +using RowVector = Matrix; + +#undef EIGEN_MAKE_TYPEDEFS +#undef EIGEN_MAKE_FIXED_TYPEDEFS + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h new file mode 100644 index 00000000..81d5a97e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h @@ -0,0 +1,542 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2009 Benoit Jacob +// 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_MATRIXBASE_H +#define EIGEN_MATRIXBASE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class MatrixBase + * \ingroup Core_Module + * + * \brief Base class for all dense matrices, vectors, and expressions + * + * This class is the base that is inherited by all matrix, vector, and related expression + * types. Most of the Eigen API is contained in this class, and its base classes. Other important + * classes for the Eigen API are Matrix, and VectorwiseOp. + * + * Note that some methods are defined in other modules such as the \ref LU_Module LU module + * for all functions related to matrix inversions. + * + * \tparam Derived is the derived type, e.g. a matrix type, or an expression, etc. + * + * When writing a function taking Eigen objects as argument, if you want your function + * to take as argument any matrix, vector, or expression, just let it take a + * MatrixBase argument. As an example, here is a function printFirstRow which, given + * a matrix, vector, or expression \a x, prints the first row of \a x. + * + * \code + template + void printFirstRow(const Eigen::MatrixBase& x) + { + cout << x.row(0) << endl; + } + * \endcode + * + * 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_MATRIXBASE_PLUGIN. + * + * \sa \blank \ref TopicClassHierarchy + */ +template +class MatrixBase : public DenseBase { + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef MatrixBase StorageBaseType; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::StorageIndex StorageIndex; + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::packet_traits::type PacketScalar; + typedef typename NumTraits::Real RealScalar; + + typedef DenseBase Base; + using Base::ColsAtCompileTime; + using Base::Flags; + using Base::IsVectorAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxSizeAtCompileTime; + using Base::RowsAtCompileTime; + using Base::SizeAtCompileTime; + + using Base::coeff; + using Base::coeffRef; + using Base::cols; + using Base::const_cast_derived; + using Base::derived; + using Base::eval; + using Base::lazyAssign; + using Base::rows; + using Base::size; + using Base::operator-; + using Base::operator+=; + using Base::operator-=; + using Base::operator*=; + using Base::operator/=; + + typedef typename Base::CoeffReturnType CoeffReturnType; + typedef typename Base::ConstTransposeReturnType ConstTransposeReturnType; + typedef typename Base::RowXpr RowXpr; + typedef typename Base::ColXpr ColXpr; +#endif // not EIGEN_PARSED_BY_DOXYGEN + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** type of the equivalent square matrix */ + typedef Matrix + SquareMatrixType; +#endif // not EIGEN_PARSED_BY_DOXYGEN + + /** \returns the size of the main diagonal, which is min(rows(),cols()). + * \sa rows(), cols(), SizeAtCompileTime. */ + EIGEN_DEVICE_FUNC inline Index diagonalSize() const { return (numext::mini)(rows(), cols()); } + + typedef typename Base::PlainObject PlainObject; + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal Represents a matrix with all coefficients equal to one another*/ + typedef CwiseNullaryOp, PlainObject> ConstantReturnType; + /** \internal the return type of MatrixBase::adjoint() */ + typedef std::conditional_t::IsComplex, + CwiseUnaryOp, ConstTransposeReturnType>, + ConstTransposeReturnType> + AdjointReturnType; + /** \internal Return type of eigenvalues() */ + typedef Matrix, internal::traits::ColsAtCompileTime, 1, ColMajor> + EigenvaluesReturnType; + /** \internal the return type of identity */ + typedef CwiseNullaryOp, PlainObject> IdentityReturnType; + /** \internal the return type of unit vectors */ + typedef Block, SquareMatrixType>, + internal::traits::RowsAtCompileTime, internal::traits::ColsAtCompileTime> + BasisReturnType; +#endif // not EIGEN_PARSED_BY_DOXYGEN + +#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::MatrixBase +#define EIGEN_DOC_UNARY_ADDONS(X, Y) +#include "../plugins/CommonCwiseBinaryOps.inc" +#include "../plugins/MatrixCwiseUnaryOps.inc" +#include "../plugins/MatrixCwiseBinaryOps.inc" +#ifdef EIGEN_MATRIXBASE_PLUGIN +#include EIGEN_MATRIXBASE_PLUGIN +#endif +#undef EIGEN_CURRENT_STORAGE_BASE_CLASS +#undef EIGEN_DOC_UNARY_ADDONS + + /** Special case of the template operator=, in order to prevent the compiler + * from generating a default operator= (issue hit with g++ 4.1) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const MatrixBase& other); + + // We cannot inherit here via Base::operator= since it is causing + // trouble with MSVC. + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& other); + + template + EIGEN_DEVICE_FUNC Derived& operator=(const EigenBase& other); + + template + EIGEN_DEVICE_FUNC Derived& operator=(const ReturnByValue& other); + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const MatrixBase& other); + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const MatrixBase& other); + + template + EIGEN_DEVICE_FUNC const Product operator*(const MatrixBase& other) const; + + template + EIGEN_DEVICE_FUNC const Product lazyProduct( + const MatrixBase& other) const; + + template + Derived& operator*=(const EigenBase& other); + + template + void applyOnTheLeft(const EigenBase& other); + + template + void applyOnTheRight(const EigenBase& other); + + template + EIGEN_DEVICE_FUNC const Product operator*( + const DiagonalBase& diagonal) const; + + template + EIGEN_DEVICE_FUNC const Product operator*( + const SkewSymmetricBase& skew) const; + + template + EIGEN_DEVICE_FUNC typename ScalarBinaryOpTraits::Scalar, + typename internal::traits::Scalar>::ReturnType + dot(const MatrixBase& other) const; + + EIGEN_DEVICE_FUNC RealScalar squaredNorm() const; + EIGEN_DEVICE_FUNC RealScalar norm() const; + RealScalar stableNorm() const; + RealScalar blueNorm() const; + RealScalar hypotNorm() const; + EIGEN_DEVICE_FUNC const PlainObject normalized() const; + EIGEN_DEVICE_FUNC const PlainObject stableNormalized() const; + EIGEN_DEVICE_FUNC void normalize(); + EIGEN_DEVICE_FUNC void stableNormalize(); + + EIGEN_DEVICE_FUNC const AdjointReturnType adjoint() const; + EIGEN_DEVICE_FUNC void adjointInPlace(); + + typedef Diagonal DiagonalReturnType; + EIGEN_DEVICE_FUNC DiagonalReturnType diagonal(); + + typedef Diagonal ConstDiagonalReturnType; + EIGEN_DEVICE_FUNC const ConstDiagonalReturnType diagonal() const; + + template + EIGEN_DEVICE_FUNC Diagonal diagonal(); + + template + EIGEN_DEVICE_FUNC const Diagonal diagonal() const; + + EIGEN_DEVICE_FUNC Diagonal diagonal(Index index); + EIGEN_DEVICE_FUNC const Diagonal diagonal(Index index) const; + + template + struct TriangularViewReturnType { + typedef TriangularView Type; + }; + template + struct ConstTriangularViewReturnType { + typedef const TriangularView Type; + }; + + template + EIGEN_DEVICE_FUNC typename TriangularViewReturnType::Type triangularView(); + template + EIGEN_DEVICE_FUNC typename ConstTriangularViewReturnType::Type triangularView() const; + + template + struct SelfAdjointViewReturnType { + typedef SelfAdjointView Type; + }; + template + struct ConstSelfAdjointViewReturnType { + typedef const SelfAdjointView Type; + }; + + template + EIGEN_DEVICE_FUNC typename SelfAdjointViewReturnType::Type selfadjointView(); + template + EIGEN_DEVICE_FUNC typename ConstSelfAdjointViewReturnType::Type selfadjointView() const; + + const SparseView sparseView( + const Scalar& m_reference = Scalar(0), + const typename NumTraits::Real& m_epsilon = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(); + EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(Index rows, Index cols); + EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index size, Index i); + EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index i); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitX(); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitY(); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitZ(); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitW(); + + EIGEN_DEVICE_FUNC const DiagonalWrapper asDiagonal() const; + const PermutationWrapper asPermutation() const; + EIGEN_DEVICE_FUNC const SkewSymmetricWrapper asSkewSymmetric() const; + + EIGEN_DEVICE_FUNC Derived& setIdentity(); + EIGEN_DEVICE_FUNC Derived& setIdentity(Index rows, Index cols); + EIGEN_DEVICE_FUNC Derived& setUnit(Index i); + EIGEN_DEVICE_FUNC Derived& setUnit(Index newSize, Index i); + + bool isIdentity(const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isDiagonal(const RealScalar& prec = NumTraits::dummy_precision()) const; + + bool isUpperTriangular(const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isLowerTriangular(const RealScalar& prec = NumTraits::dummy_precision()) const; + + bool isSkewSymmetric(const RealScalar& prec = NumTraits::dummy_precision()) const; + + template + bool isOrthogonal(const MatrixBase& other, + const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isUnitary(const RealScalar& prec = NumTraits::dummy_precision()) const; + + /** \returns true if each coefficients of \c *this and \a other are all exactly equal. + * \warning When using floating point scalar values you probably should rather use a + * fuzzy comparison such as isApprox() + * \sa isApprox(), operator!= */ + template + EIGEN_DEVICE_FUNC inline bool operator==(const MatrixBase& other) const { + return cwiseEqual(other).all(); + } + + /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other. + * \warning When using floating point scalar values you probably should rather use a + * fuzzy comparison such as isApprox() + * \sa isApprox(), operator== */ + template + EIGEN_DEVICE_FUNC inline bool operator!=(const MatrixBase& other) const { + return cwiseNotEqual(other).any(); + } + + NoAlias EIGEN_DEVICE_FUNC noalias(); + + // TODO forceAlignedAccess is temporarily disabled + // Need to find a nicer workaround. + inline const Derived& forceAlignedAccess() const { return derived(); } + inline Derived& forceAlignedAccess() { return derived(); } + template + inline const Derived& forceAlignedAccessIf() const { + return derived(); + } + template + inline Derived& forceAlignedAccessIf() { + return derived(); + } + + EIGEN_DEVICE_FUNC Scalar trace() const; + + template + EIGEN_DEVICE_FUNC RealScalar lpNorm() const; + + EIGEN_DEVICE_FUNC MatrixBase& matrix() { return *this; } + EIGEN_DEVICE_FUNC const MatrixBase& matrix() const { return *this; } + + /** \returns an \link Eigen::ArrayBase Array \endlink expression of this matrix + * \sa ArrayBase::matrix() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ArrayWrapper array() { return ArrayWrapper(derived()); } + /** \returns a const \link Eigen::ArrayBase Array \endlink expression of this matrix + * \sa ArrayBase::matrix() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ArrayWrapper array() const { + return ArrayWrapper(derived()); + } + + /////////// LU module /////////// + + template + inline const FullPivLU fullPivLu() const; + template + inline const PartialPivLU partialPivLu() const; + + template + inline const PartialPivLU lu() const; + + EIGEN_DEVICE_FUNC inline const Inverse inverse() const; + + template + inline void computeInverseAndDetWithCheck( + ResultType& inverse, typename ResultType::Scalar& determinant, bool& invertible, + const RealScalar& absDeterminantThreshold = NumTraits::dummy_precision()) const; + + template + inline void computeInverseWithCheck( + ResultType& inverse, bool& invertible, + const RealScalar& absDeterminantThreshold = NumTraits::dummy_precision()) const; + + EIGEN_DEVICE_FUNC Scalar determinant() const; + + /////////// Cholesky module /////////// + + inline const LLT llt() const; + inline const LDLT ldlt() const; + + /////////// QR module /////////// + + inline const HouseholderQR householderQr() const; + template + inline const ColPivHouseholderQR colPivHouseholderQr() const; + template + inline const FullPivHouseholderQR fullPivHouseholderQr() const; + template + inline const CompleteOrthogonalDecomposition completeOrthogonalDecomposition() const; + + /////////// Eigenvalues module /////////// + + inline EigenvaluesReturnType eigenvalues() const; + inline RealScalar operatorNorm() const; + + /////////// SVD module /////////// + + template + inline JacobiSVD jacobiSvd() const; + template + EIGEN_DEPRECATED inline JacobiSVD jacobiSvd(unsigned int computationOptions) const; + + template + inline BDCSVD bdcSvd() const; + template + EIGEN_DEPRECATED inline BDCSVD bdcSvd(unsigned int computationOptions) const; + + /////////// Geometry module /////////// + + template + EIGEN_DEVICE_FUNC inline typename internal::cross_impl::return_type cross( + const MatrixBase& other) const; + + template + EIGEN_DEVICE_FUNC inline PlainObject cross3(const MatrixBase& other) const; + + EIGEN_DEVICE_FUNC inline PlainObject unitOrthogonal(void) const; + + EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix eulerAngles(Index a0, Index a1, Index a2) const; + + EIGEN_DEVICE_FUNC inline Matrix canonicalEulerAngles(Index a0, Index a1, Index a2) const; + + // put this as separate enum value to work around possible GCC 4.3 bug (?) + enum { + HomogeneousReturnTypeDirection = + ColsAtCompileTime == 1 && RowsAtCompileTime == 1 + ? ((internal::traits::Flags & RowMajorBit) == RowMajorBit ? Horizontal : Vertical) + : ColsAtCompileTime == 1 ? Vertical + : Horizontal + }; + typedef Homogeneous HomogeneousReturnType; + EIGEN_DEVICE_FUNC inline HomogeneousReturnType homogeneous() const; + + enum { SizeMinusOne = SizeAtCompileTime == Dynamic ? Dynamic : SizeAtCompileTime - 1 }; + typedef Block::ColsAtCompileTime == 1 ? SizeMinusOne : 1, + internal::traits::ColsAtCompileTime == 1 ? 1 : SizeMinusOne> + ConstStartMinusOne; + typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(ConstStartMinusOne, Scalar, quotient) HNormalizedReturnType; + EIGEN_DEVICE_FUNC inline const HNormalizedReturnType hnormalized() const; + + ////////// Householder module /////////// + + EIGEN_DEVICE_FUNC void makeHouseholderInPlace(Scalar& tau, RealScalar& beta); + template + EIGEN_DEVICE_FUNC void makeHouseholder(EssentialPart& essential, Scalar& tau, RealScalar& beta) const; + template + EIGEN_DEVICE_FUNC void applyHouseholderOnTheLeft(const EssentialPart& essential, const Scalar& tau, + Scalar* workspace); + template + EIGEN_DEVICE_FUNC void applyHouseholderOnTheRight(const EssentialPart& essential, const Scalar& tau, + Scalar* workspace); + + ///////// Jacobi module ///////// + + template + EIGEN_DEVICE_FUNC void applyOnTheLeft(Index p, Index q, const JacobiRotation& j); + template + EIGEN_DEVICE_FUNC void applyOnTheRight(Index p, Index q, const JacobiRotation& j); + + ///////// SparseCore module ///////// + + template + EIGEN_STRONG_INLINE const typename SparseMatrixBase::template CwiseProductDenseReturnType::Type + cwiseProduct(const SparseMatrixBase& other) const { + return other.cwiseProduct(derived()); + } + + ///////// MatrixFunctions module ///////// + + typedef typename internal::stem_function::type StemFunction; +#define EIGEN_MATRIX_FUNCTION(ReturnType, Name, Description) \ + /** \returns an expression of the matrix Description of \c *this. \brief This function requires the unsupported MatrixFunctions module. To compute the \ + * coefficient-wise Description use ArrayBase::##Name . */ \ + const ReturnType Name() const; +#define EIGEN_MATRIX_FUNCTION_1(ReturnType, Name, Description, Argument) \ + /** \returns an expression of the matrix Description of \c *this. \brief This function requires the unsupported MatrixFunctions module. To compute the \ + * coefficient-wise Description use ArrayBase::##Name . */ \ + const ReturnType Name(Argument) const; + + EIGEN_MATRIX_FUNCTION(MatrixExponentialReturnValue, exp, exponential) + /** \brief Helper function for the unsupported + * MatrixFunctions module.*/ + const MatrixFunctionReturnValue matrixFunction(StemFunction f) const; + EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cosh, hyperbolic cosine) + EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sinh, hyperbolic sine) + EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, atanh, inverse hyperbolic cosine) + EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, acosh, inverse hyperbolic cosine) + EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, asinh, inverse hyperbolic sine) + EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cos, cosine) + EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sin, sine) + EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root) + EIGEN_MATRIX_FUNCTION(MatrixLogarithmReturnValue, log, logarithm) + EIGEN_MATRIX_FUNCTION_1(MatrixPowerReturnValue, pow, power to \c p, const RealScalar& p) + EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex& p) + + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(MatrixBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MatrixBase) + + private: + EIGEN_DEVICE_FUNC explicit MatrixBase(int); + EIGEN_DEVICE_FUNC MatrixBase(int, int); + template + EIGEN_DEVICE_FUNC explicit MatrixBase(const MatrixBase&); + + protected: + // mixing arrays and matrices is not legal + template + Derived& operator+=(const ArrayBase&) { + EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1, + YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); + return *this; + } + // mixing arrays and matrices is not legal + template + Derived& operator-=(const ArrayBase&) { + EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1, + YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); + return *this; + } +}; + +/*************************************************************************** + * Implementation of matrix base methods + ***************************************************************************/ + +/** replaces \c *this by \c *this * \a other. + * + * \returns a reference to \c *this + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline Derived& MatrixBase::operator*=(const EigenBase& other) { + other.derived().applyThisOnTheRight(derived()); + return derived(); +} + +/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline void MatrixBase::applyOnTheRight(const EigenBase& other) { + other.derived().applyThisOnTheRight(derived()); +} + +/** replaces \c *this by \a other * \c *this. + * + * Example: \include MatrixBase_applyOnTheLeft.cpp + * Output: \verbinclude MatrixBase_applyOnTheLeft.out + */ +template +template +inline void MatrixBase::applyOnTheLeft(const EigenBase& other) { + other.derived().applyThisOnTheLeft(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_MATRIXBASE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h new file mode 100644 index 00000000..ec360ebd --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h @@ -0,0 +1,91 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// 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_NESTBYVALUE_H +#define EIGEN_NESTBYVALUE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits > : public traits { + enum { Flags = traits::Flags & ~NestByRefBit }; +}; +} // namespace internal + +/** \class NestByValue + * \ingroup Core_Module + * + * \brief Expression which must be nested by value + * + * \tparam ExpressionType the type of the object of which we are requiring nesting-by-value + * + * This class is the return type of MatrixBase::nestByValue() + * and most of the time this is the only way it is used. + * + * \sa MatrixBase::nestByValue() + */ +template +class NestByValue : public internal::dense_xpr_base >::type { + public: + typedef typename internal::dense_xpr_base::type Base; + static constexpr bool HasDirectAccess = internal::has_direct_access::ret; + + EIGEN_DENSE_PUBLIC_INTERFACE(NestByValue) + + EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } + + EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; } + + EIGEN_DEVICE_FUNC const ExpressionType& nestedExpression() const { return m_expression; } + + EIGEN_DEVICE_FUNC typename std::enable_if::type data() const { + return m_expression.data(); + } + + EIGEN_DEVICE_FUNC typename std::enable_if::type innerStride() const { + return m_expression.innerStride(); + } + + EIGEN_DEVICE_FUNC typename std::enable_if::type outerStride() const { + return m_expression.outerStride(); + } + + protected: + const ExpressionType m_expression; +}; + +/** \returns an expression of the temporary version of *this. + */ +template +EIGEN_DEVICE_FUNC inline const NestByValue DenseBase::nestByValue() const { + return NestByValue(derived()); +} + +namespace internal { + +// Evaluator of Solve -> eval into a temporary +template +struct evaluator > : public evaluator { + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const NestByValue& xpr) : Base(xpr.nestedExpression()) {} +}; +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_NESTBYVALUE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h new file mode 100644 index 00000000..b6c72091 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h @@ -0,0 +1,102 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 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_NOALIAS_H +#define EIGEN_NOALIAS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class NoAlias + * \ingroup Core_Module + * + * \brief Pseudo expression providing an operator = assuming no aliasing + * + * \tparam ExpressionType the type of the object on which to do the lazy assignment + * + * This class represents an expression with special assignment operators + * assuming no aliasing between the target expression and the source expression. + * More precisely it alloas to bypass the EvalBeforeAssignBit flag of the source expression. + * It is the return type of MatrixBase::noalias() + * and most of the time this is the only way it is used. + * + * \sa MatrixBase::noalias() + */ +template class StorageBase> +class NoAlias { + public: + typedef typename ExpressionType::Scalar Scalar; + + EIGEN_DEVICE_FUNC explicit NoAlias(ExpressionType& expression) : m_expression(expression) {} + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator=(const StorageBase& other) { + call_assignment_no_alias(m_expression, other.derived(), + internal::assign_op()); + return m_expression; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator+=(const StorageBase& other) { + call_assignment_no_alias(m_expression, other.derived(), + internal::add_assign_op()); + return m_expression; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator-=(const StorageBase& other) { + call_assignment_no_alias(m_expression, other.derived(), + internal::sub_assign_op()); + return m_expression; + } + + EIGEN_DEVICE_FUNC ExpressionType& expression() const { return m_expression; } + + protected: + ExpressionType& m_expression; +}; + +/** \returns a pseudo expression of \c *this with an operator= assuming + * no aliasing between \c *this and the source expression. + * + * More precisely, noalias() allows to bypass the EvalBeforeAssignBit flag. + * Currently, even though several expressions may alias, only product + * expressions have this flag. Therefore, noalias() is only useful when + * the source expression contains a matrix product. + * + * Here are some examples where noalias is useful: + * \code + * D.noalias() = A * B; + * D.noalias() += A.transpose() * B; + * D.noalias() -= 2 * A * B.adjoint(); + * \endcode + * + * On the other hand the following example will lead to a \b wrong result: + * \code + * A.noalias() = A * B; + * \endcode + * because the result matrix A is also an operand of the matrix product. Therefore, + * there is no alternative than evaluating A * B in a temporary, that is the default + * behavior when you write: + * \code + * A = A * B; + * \endcode + * + * \sa class NoAlias + */ +template +NoAlias EIGEN_DEVICE_FUNC MatrixBase::noalias() { + return NoAlias(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_NOALIAS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h new file mode 100644 index 00000000..80f74e92 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h @@ -0,0 +1,329 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2010 Benoit Jacob +// +// 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_NUMTRAITS_H +#define EIGEN_NUMTRAITS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +// default implementation of digits(), based on numeric_limits if specialized, +// 0 for integer types, and log2(epsilon()) otherwise. +template ::is_specialized, + bool is_integer = NumTraits::IsInteger> +struct default_digits_impl { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits::digits; } +}; + +template +struct default_digits_impl // Floating point +{ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { + using std::ceil; + using std::log2; + typedef typename NumTraits::Real Real; + return int(ceil(-log2(NumTraits::epsilon()))); + } +}; + +template +struct default_digits_impl // Integer +{ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; } +}; + +// default implementation of digits10(), based on numeric_limits if specialized, +// 0 for integer types, and floor((digits()-1)*log10(2)) otherwise. +template ::is_specialized, + bool is_integer = NumTraits::IsInteger> +struct default_digits10_impl { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits::digits10; } +}; + +template +struct default_digits10_impl // Floating point +{ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { + using std::floor; + using std::log10; + typedef typename NumTraits::Real Real; + return int(floor((internal::default_digits_impl::run() - 1) * log10(2))); + } +}; + +template +struct default_digits10_impl // Integer +{ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; } +}; + +// default implementation of max_digits10(), based on numeric_limits if specialized, +// 0 for integer types, and log10(2) * digits() + 1 otherwise. +template ::is_specialized, + bool is_integer = NumTraits::IsInteger> +struct default_max_digits10_impl { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits::max_digits10; } +}; + +template +struct default_max_digits10_impl // Floating point +{ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { + using std::ceil; + using std::log10; + typedef typename NumTraits::Real Real; + return int(ceil(internal::default_digits_impl::run() * log10(2) + 1)); + } +}; + +template +struct default_max_digits10_impl // Integer +{ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; } +}; + +} // end namespace internal + +namespace numext { +/** \internal bit-wise cast without changing the underlying bit representation. */ + +// TODO: Replace by std::bit_cast (available in C++20) +template +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Tgt bit_cast(const Src& src) { + // The behaviour of memcpy is not specified for non-trivially copyable types + EIGEN_STATIC_ASSERT(std::is_trivially_copyable::value, THIS_TYPE_IS_NOT_SUPPORTED); + EIGEN_STATIC_ASSERT(std::is_trivially_copyable::value && std::is_default_constructible::value, + THIS_TYPE_IS_NOT_SUPPORTED); + EIGEN_STATIC_ASSERT(sizeof(Src) == sizeof(Tgt), THIS_TYPE_IS_NOT_SUPPORTED); + + Tgt tgt; + // Load src into registers first. This allows the memcpy to be elided by CUDA. + const Src staged = src; + EIGEN_USING_STD(memcpy) + memcpy(static_cast(&tgt), static_cast(&staged), sizeof(Tgt)); + return tgt; +} +} // namespace numext + +/** \class NumTraits + * \ingroup Core_Module + * + * \brief Holds information about the various numeric (i.e. scalar) types allowed by Eigen. + * + * \tparam T the numeric type at hand + * + * This class stores enums, typedefs and static methods giving information about a numeric type. + * + * The provided data consists of: + * \li A typedef \c Real, giving the "real part" type of \a T. If \a T is already real, + * then \c Real is just a typedef to \a T. If \a T is \c std::complex then \c Real + * is a typedef to \a U. + * \li A typedef \c NonInteger, giving the type that should be used for operations producing non-integral values, + * such as quotients, square roots, etc. If \a T is a floating-point type, then this typedef just gives + * \a T again. Note however that many Eigen functions such as internal::sqrt simply refuse to + * take integers. Outside of a few cases, Eigen doesn't do automatic type promotion. Thus, this typedef is + * only intended as a helper for code that needs to explicitly promote types. + * \li A typedef \c Literal giving the type to use for numeric literals such as "2" or "0.5". For instance, for \c + * std::complex, Literal is defined as \c U. Of course, this type must be fully compatible with \a T. In doubt, just + * use \a T here. \li A typedef \a Nested giving the type to use to nest a value inside of the expression tree. If you + * don't know what this means, just use \a T here. \li An enum value \a IsComplex. It is equal to 1 if \a T is a \c + * std::complex type, and to 0 otherwise. \li An enum value \a IsInteger. It is equal to \c 1 if \a T is an integer type + * such as \c int, and to \c 0 otherwise. \li Enum values ReadCost, AddCost and MulCost representing a rough estimate of + * the number of CPU cycles needed to by move / add / mul instructions respectively, assuming the data is already stored + * in CPU registers. Stay vague here. No need to do architecture-specific stuff. If you don't know what this means, just + * use \c Eigen::HugeCost. \li An enum value \a IsSigned. It is equal to \c 1 if \a T is a signed type and to 0 if \a T + * is unsigned. \li An enum value \a RequireInitialization. It is equal to \c 1 if the constructor of the numeric type + * \a T must be called, and to 0 if it is safe not to call it. Default is 0 if \a T is an arithmetic type, and 1 + * otherwise. \li An epsilon() function which, unlike std::numeric_limits::epsilon(), it returns a + * \a Real instead of a \a T. \li A dummy_precision() function returning a weak epsilon value. It is mainly used as a + * default value by the fuzzy comparison operators. \li highest() and lowest() functions returning the highest and + * lowest possible values respectively. \li digits() function returning the number of radix digits (non-sign digits for + * integers, mantissa for floating-point). This is the analogue of std::numeric_limits::digits which is used + * as the default implementation if specialized. \li digits10() function returning the number of decimal digits that can + * be represented without change. This is the analogue of std::numeric_limits::digits10 which is + * used as the default implementation if specialized. \li max_digits10() function returning the number of decimal digits + * required to uniquely represent all distinct values of the type. This is the analogue of std::numeric_limits::max_digits10 + * which is used as the default implementation if specialized. + * \li min_exponent() and max_exponent() functions returning the highest and lowest possible values, respectively, + * such that the radix raised to the power exponent-1 is a normalized floating-point number. These are equivalent + * to std::numeric_limits::min_exponent/ + * std::numeric_limits::max_exponent. + * \li infinity() function returning a representation of positive infinity, if available. + * \li quiet_NaN function returning a non-signaling "not-a-number", if available. + */ + +template +struct GenericNumTraits { + enum { + IsInteger = std::numeric_limits::is_integer, + IsSigned = std::numeric_limits::is_signed, + IsComplex = 0, + RequireInitialization = internal::is_arithmetic::value ? 0 : 1, + ReadCost = 1, + AddCost = 1, + MulCost = 1 + }; + + typedef T Real; + typedef std::conditional_t, T> NonInteger; + typedef T Nested; + typedef T Literal; + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real epsilon() { return numext::numeric_limits::epsilon(); } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits10() { return internal::default_digits10_impl::run(); } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_digits10() { + return internal::default_max_digits10_impl::run(); + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits() { return internal::default_digits_impl::run(); } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int min_exponent() { return numext::numeric_limits::min_exponent; } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_exponent() { return numext::numeric_limits::max_exponent; } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real dummy_precision() { + // make sure to override this for floating-point types + return Real(0); + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T highest() { return (numext::numeric_limits::max)(); } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T lowest() { + return IsInteger ? (numext::numeric_limits::min)() : static_cast(-(numext::numeric_limits::max)()); + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T infinity() { return numext::numeric_limits::infinity(); } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T quiet_NaN() { return numext::numeric_limits::quiet_NaN(); } +}; + +template +struct NumTraits : GenericNumTraits {}; + +template <> +struct NumTraits : GenericNumTraits { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline float dummy_precision() { return 1e-5f; } +}; + +template <> +struct NumTraits : GenericNumTraits { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline double dummy_precision() { return 1e-12; } +}; + +// GPU devices treat `long double` as `double`. +#ifndef EIGEN_GPU_COMPILE_PHASE +template <> +struct NumTraits : GenericNumTraits { + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline long double dummy_precision() { + return static_cast(1e-15l); + } + +#if defined(EIGEN_ARCH_PPC) && (__LDBL_MANT_DIG__ == 106) + // PowerPC double double causes issues with some values + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline long double epsilon() { + // 2^(-(__LDBL_MANT_DIG__)+1) + return static_cast(2.4651903288156618919116517665087e-32l); + } +#endif +}; +#endif + +template +struct NumTraits > : GenericNumTraits > { + typedef Real_ Real; + typedef typename NumTraits::Literal Literal; + enum { + IsComplex = 1, + RequireInitialization = NumTraits::RequireInitialization, + ReadCost = 2 * NumTraits::ReadCost, + AddCost = 2 * NumTraits::AddCost, + MulCost = 4 * NumTraits::MulCost + 2 * NumTraits::AddCost + }; + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real dummy_precision() { return NumTraits::dummy_precision(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits10() { return NumTraits::digits10(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_digits10() { return NumTraits::max_digits10(); } +}; + +template +struct NumTraits > { + typedef Array ArrayType; + typedef typename NumTraits::Real RealScalar; + typedef Array Real; + typedef typename NumTraits::NonInteger NonIntegerScalar; + typedef Array NonInteger; + typedef ArrayType& Nested; + typedef typename NumTraits::Literal Literal; + + enum { + IsComplex = NumTraits::IsComplex, + IsInteger = NumTraits::IsInteger, + IsSigned = NumTraits::IsSigned, + RequireInitialization = 1, + ReadCost = ArrayType::SizeAtCompileTime == Dynamic + ? HugeCost + : ArrayType::SizeAtCompileTime * int(NumTraits::ReadCost), + AddCost = ArrayType::SizeAtCompileTime == Dynamic ? HugeCost + : ArrayType::SizeAtCompileTime * int(NumTraits::AddCost), + MulCost = ArrayType::SizeAtCompileTime == Dynamic ? HugeCost + : ArrayType::SizeAtCompileTime * int(NumTraits::MulCost) + }; + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline RealScalar epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline RealScalar dummy_precision() { + return NumTraits::dummy_precision(); + } + + EIGEN_CONSTEXPR + static inline int digits10() { return NumTraits::digits10(); } + EIGEN_CONSTEXPR + static inline int max_digits10() { return NumTraits::max_digits10(); } +}; + +template <> +struct NumTraits : GenericNumTraits { + enum { RequireInitialization = 1, ReadCost = HugeCost, AddCost = HugeCost, MulCost = HugeCost }; + + EIGEN_CONSTEXPR + static inline int digits10() { return 0; } + EIGEN_CONSTEXPR + static inline int max_digits10() { return 0; } + + private: + static inline std::string epsilon(); + static inline std::string dummy_precision(); + static inline std::string lowest(); + static inline std::string highest(); + static inline std::string infinity(); + static inline std::string quiet_NaN(); +}; + +// Empty specialization for void to allow template specialization based on NumTraits::Real with T==void and SFINAE. +template <> +struct NumTraits {}; + +template <> +struct NumTraits : GenericNumTraits {}; + +} // end namespace Eigen + +#endif // EIGEN_NUMTRAITS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h new file mode 100644 index 00000000..7b2c8dca --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h @@ -0,0 +1,209 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011-2018 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_PARTIALREDUX_H +#define EIGEN_PARTIALREDUX_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +/*************************************************************************** + * + * This file provides evaluators for partial reductions. + * There are two modes: + * + * - scalar path: simply calls the respective function on the column or row. + * -> nothing special here, all the tricky part is handled by the return + * types of VectorwiseOp's members. They embed the functor calling the + * respective DenseBase's member function. + * + * - vectorized path: implements a packet-wise reductions followed by + * some (optional) processing of the outcome, e.g., division by n for mean. + * + * For the vectorized path let's observe that the packet-size and outer-unrolling + * are both decided by the assignment logic. So all we have to do is to decide + * on the inner unrolling. + * + * For the unrolling, we can reuse "internal::redux_vec_unroller" from Redux.h, + * but be need to be careful to specify correct increment. + * + ***************************************************************************/ + +/* logic deciding a strategy for unrolling of vectorized paths */ +template +struct packetwise_redux_traits { + enum { + OuterSize = int(Evaluator::IsRowMajor) ? Evaluator::RowsAtCompileTime : Evaluator::ColsAtCompileTime, + Cost = OuterSize == Dynamic ? HugeCost + : OuterSize * Evaluator::CoeffReadCost + (OuterSize - 1) * functor_traits::Cost, + Unrolling = Cost <= EIGEN_UNROLLING_LIMIT ? CompleteUnrolling : NoUnrolling + }; +}; + +/* Value to be returned when size==0 , by default let's return 0 */ +template +EIGEN_DEVICE_FUNC PacketType packetwise_redux_empty_value(const Func&) { + const typename unpacket_traits::type zero(0); + return pset1(zero); +} + +/* For products the default is 1 */ +template +EIGEN_DEVICE_FUNC PacketType packetwise_redux_empty_value(const scalar_product_op&) { + return pset1(Scalar(1)); +} + +/* Perform the actual reduction */ +template ::Unrolling> +struct packetwise_redux_impl; + +/* Perform the actual reduction with unrolling */ +template +struct packetwise_redux_impl { + typedef redux_novec_unroller Base; + typedef typename Evaluator::Scalar Scalar; + + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func& func, Index /*size*/) { + return redux_vec_unroller::OuterSize>::template run(eval, + func); + } +}; + +/* Add a specialization of redux_vec_unroller for size==0 at compiletime. + * This specialization is not required for general reductions, which is + * why it is defined here. + */ +template +struct redux_vec_unroller { + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator&, const Func& f) { + return packetwise_redux_empty_value(f); + } +}; + +/* Perform the actual reduction for dynamic sizes */ +template +struct packetwise_redux_impl { + typedef typename Evaluator::Scalar Scalar; + typedef typename redux_traits::PacketType PacketScalar; + + template + EIGEN_DEVICE_FUNC static PacketType run(const Evaluator& eval, const Func& func, Index size) { + if (size == 0) return packetwise_redux_empty_value(func); + + const Index size4 = (size - 1) & (~3); + PacketType p = eval.template packetByOuterInner(0, 0); + Index i = 1; + // This loop is optimized for instruction pipelining: + // - each iteration generates two independent instructions + // - thanks to branch prediction and out-of-order execution we have independent instructions across loops + for (; i < size4; i += 4) + p = func.packetOp( + p, func.packetOp(func.packetOp(eval.template packetByOuterInner(i + 0, 0), + eval.template packetByOuterInner(i + 1, 0)), + func.packetOp(eval.template packetByOuterInner(i + 2, 0), + eval.template packetByOuterInner(i + 3, 0)))); + for (; i < size; ++i) p = func.packetOp(p, eval.template packetByOuterInner(i, 0)); + return p; + } +}; + +template +struct evaluator > + : evaluator_base > { + typedef PartialReduxExpr XprType; + typedef typename internal::nested_eval::type ArgTypeNested; + typedef add_const_on_value_type_t ConstArgTypeNested; + typedef internal::remove_all_t ArgTypeNestedCleaned; + typedef typename ArgType::Scalar InputScalar; + typedef typename XprType::Scalar Scalar; + enum { + TraversalSize = Direction == int(Vertical) ? int(ArgType::RowsAtCompileTime) : int(ArgType::ColsAtCompileTime) + }; + typedef typename MemberOp::template Cost CostOpType; + enum { + CoeffReadCost = TraversalSize == Dynamic ? HugeCost + : TraversalSize == 0 + ? 1 + : int(TraversalSize) * int(evaluator::CoeffReadCost) + int(CostOpType::value), + + ArgFlags_ = evaluator::Flags, + + Vectorizable_ = bool(int(ArgFlags_) & PacketAccessBit) && bool(MemberOp::Vectorizable) && + (Direction == int(Vertical) ? bool(ArgFlags_ & RowMajorBit) : (ArgFlags_ & RowMajorBit) == 0) && + (TraversalSize != 0), + + Flags = (traits::Flags & RowMajorBit) | (evaluator::Flags & (HereditaryBits & (~RowMajorBit))) | + (Vectorizable_ ? PacketAccessBit : 0) | LinearAccessBit, + + Alignment = 0 // FIXME this will need to be improved once PartialReduxExpr is vectorized + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType xpr) : m_arg(xpr.nestedExpression()), m_functor(xpr.functor()) { + EIGEN_INTERNAL_CHECK_COST_VALUE(TraversalSize == Dynamic ? HugeCost + : (TraversalSize == 0 ? 1 : int(CostOpType::value))); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index i, Index j) const { + return coeff(Direction == Vertical ? j : i); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index index) const { + return m_functor(m_arg.template subVector(index)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index i, Index j) const { + return packet(Direction == Vertical ? j : i); + } + + template + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC PacketType packet(Index idx) const { + enum { PacketSize = internal::unpacket_traits::size }; + typedef Block + PanelType; + + PanelType panel(m_arg, Direction == Vertical ? 0 : idx, Direction == Vertical ? idx : 0, + Direction == Vertical ? m_arg.rows() : Index(PacketSize), + Direction == Vertical ? Index(PacketSize) : m_arg.cols()); + + // FIXME + // See bug 1612, currently if PacketSize==1 (i.e. complex with 128bits registers) then the storage-order of + // panel get reversed and methods like packetByOuterInner do not make sense anymore in this context. So let's just + // by pass "vectorization" in this case: + if (PacketSize == 1) return internal::pset1(coeff(idx)); + + typedef typename internal::redux_evaluator PanelEvaluator; + PanelEvaluator panel_eval(panel); + typedef typename MemberOp::BinaryOp BinaryOp; + PacketType p = internal::packetwise_redux_impl::template run( + panel_eval, m_functor.binaryFunc(), m_arg.outerSize()); + return p; + } + + protected: + ConstArgTypeNested m_arg; + const MemberOp m_functor; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_PARTIALREDUX_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h new file mode 100644 index 00000000..6945964a --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h @@ -0,0 +1,552 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Benoit Jacob +// 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_PERMUTATIONMATRIX_H +#define EIGEN_PERMUTATIONMATRIX_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +enum PermPermProduct_t { PermPermProduct }; + +} // end namespace internal + +/** \class PermutationBase + * \ingroup Core_Module + * + * \brief Base class for permutations + * + * \tparam Derived the derived class + * + * This class is the base class for all expressions representing a permutation matrix, + * internally stored as a vector of integers. + * The convention followed here is that if \f$ \sigma \f$ is a permutation, the corresponding permutation matrix + * \f$ P_\sigma \f$ is such that if \f$ (e_1,\ldots,e_p) \f$ is the canonical basis, we have: + * \f[ P_\sigma(e_i) = e_{\sigma(i)}. \f] + * This convention ensures that for any two permutations \f$ \sigma, \tau \f$, we have: + * \f[ P_{\sigma\circ\tau} = P_\sigma P_\tau. \f] + * + * Permutation matrices are square and invertible. + * + * Notice that in addition to the member functions and operators listed here, there also are non-member + * operator* to multiply any kind of permutation object with any kind of matrix expression (MatrixBase) + * on either side. + * + * \sa class PermutationMatrix, class PermutationWrapper + */ +template +class PermutationBase : public EigenBase { + typedef internal::traits Traits; + typedef EigenBase Base; + + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename Traits::IndicesType IndicesType; + enum { + Flags = Traits::Flags, + RowsAtCompileTime = Traits::RowsAtCompileTime, + ColsAtCompileTime = Traits::ColsAtCompileTime, + MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = Traits::MaxColsAtCompileTime + }; + typedef typename Traits::StorageIndex StorageIndex; + typedef Matrix + DenseMatrixType; + typedef PermutationMatrix + PlainPermutationType; + typedef PlainPermutationType PlainObject; + using Base::derived; + typedef Inverse InverseReturnType; + typedef void Scalar; +#endif + + /** Copies the other permutation into *this */ + template + Derived& operator=(const PermutationBase& other) { + indices() = other.indices(); + return derived(); + } + + /** Assignment from the Transpositions \a tr */ + template + Derived& operator=(const TranspositionsBase& tr) { + setIdentity(tr.size()); + for (Index k = size() - 1; k >= 0; --k) applyTranspositionOnTheRight(k, tr.coeff(k)); + return derived(); + } + + /** \returns the number of rows */ + inline EIGEN_DEVICE_FUNC Index rows() const { return Index(indices().size()); } + + /** \returns the number of columns */ + inline EIGEN_DEVICE_FUNC Index cols() const { return Index(indices().size()); } + + /** \returns the size of a side of the respective square matrix, i.e., the number of indices */ + inline EIGEN_DEVICE_FUNC Index size() const { return Index(indices().size()); } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + void evalTo(MatrixBase& other) const { + other.setZero(); + for (Index i = 0; i < rows(); ++i) other.coeffRef(indices().coeff(i), i) = typename DenseDerived::Scalar(1); + } +#endif + + /** \returns a Matrix object initialized from this permutation matrix. Notice that it + * is inefficient to return this Matrix object by value. For efficiency, favor using + * the Matrix constructor taking EigenBase objects. + */ + DenseMatrixType toDenseMatrix() const { return derived(); } + + /** const version of indices(). */ + const IndicesType& indices() const { return derived().indices(); } + /** \returns a reference to the stored array representing the permutation. */ + IndicesType& indices() { return derived().indices(); } + + /** Resizes to given size. + */ + inline void resize(Index newSize) { indices().resize(newSize); } + + /** Sets *this to be the identity permutation matrix */ + void setIdentity() { + StorageIndex n = StorageIndex(size()); + for (StorageIndex i = 0; i < n; ++i) indices().coeffRef(i) = i; + } + + /** Sets *this to be the identity permutation matrix of given size. + */ + void setIdentity(Index newSize) { + resize(newSize); + setIdentity(); + } + + /** Multiplies *this by the transposition \f$(ij)\f$ on the left. + * + * \returns a reference to *this. + * + * \warning This is much slower than applyTranspositionOnTheRight(Index,Index): + * this has linear complexity and requires a lot of branching. + * + * \sa applyTranspositionOnTheRight(Index,Index) + */ + Derived& applyTranspositionOnTheLeft(Index i, Index j) { + eigen_assert(i >= 0 && j >= 0 && i < size() && j < size()); + for (Index k = 0; k < size(); ++k) { + if (indices().coeff(k) == i) + indices().coeffRef(k) = StorageIndex(j); + else if (indices().coeff(k) == j) + indices().coeffRef(k) = StorageIndex(i); + } + return derived(); + } + + /** Multiplies *this by the transposition \f$(ij)\f$ on the right. + * + * \returns a reference to *this. + * + * This is a fast operation, it only consists in swapping two indices. + * + * \sa applyTranspositionOnTheLeft(Index,Index) + */ + Derived& applyTranspositionOnTheRight(Index i, Index j) { + eigen_assert(i >= 0 && j >= 0 && i < size() && j < size()); + std::swap(indices().coeffRef(i), indices().coeffRef(j)); + return derived(); + } + + /** \returns the inverse permutation matrix. + * + * \note \blank \note_try_to_help_rvo + */ + inline InverseReturnType inverse() const { return InverseReturnType(derived()); } + /** \returns the tranpose permutation matrix. + * + * \note \blank \note_try_to_help_rvo + */ + inline InverseReturnType transpose() const { return InverseReturnType(derived()); } + + /**** multiplication helpers to hopefully get RVO ****/ + +#ifndef EIGEN_PARSED_BY_DOXYGEN + protected: + template + void assignTranspose(const PermutationBase& other) { + for (Index i = 0; i < rows(); ++i) indices().coeffRef(other.indices().coeff(i)) = i; + } + template + void assignProduct(const Lhs& lhs, const Rhs& rhs) { + eigen_assert(lhs.cols() == rhs.rows()); + for (Index i = 0; i < rows(); ++i) indices().coeffRef(i) = lhs.indices().coeff(rhs.indices().coeff(i)); + } +#endif + + public: + /** \returns the product permutation matrix. + * + * \note \blank \note_try_to_help_rvo + */ + template + inline PlainPermutationType operator*(const PermutationBase& other) const { + return PlainPermutationType(internal::PermPermProduct, derived(), other.derived()); + } + + /** \returns the product of a permutation with another inverse permutation. + * + * \note \blank \note_try_to_help_rvo + */ + template + inline PlainPermutationType operator*(const InverseImpl& other) const { + return PlainPermutationType(internal::PermPermProduct, *this, other.eval()); + } + + /** \returns the product of an inverse permutation with another permutation. + * + * \note \blank \note_try_to_help_rvo + */ + template + friend inline PlainPermutationType operator*(const InverseImpl& other, + const PermutationBase& perm) { + return PlainPermutationType(internal::PermPermProduct, other.eval(), perm); + } + + /** \returns the determinant of the permutation matrix, which is either 1 or -1 depending on the parity of the + * permutation. + * + * This function is O(\c n) procedure allocating a buffer of \c n booleans. + */ + Index determinant() const { + Index res = 1; + Index n = size(); + Matrix mask(n); + mask.fill(false); + Index r = 0; + while (r < n) { + // search for the next seed + while (r < n && mask[r]) r++; + if (r >= n) break; + // we got one, let's follow it until we are back to the seed + Index k0 = r++; + mask.coeffRef(k0) = true; + for (Index k = indices().coeff(k0); k != k0; k = indices().coeff(k)) { + mask.coeffRef(k) = true; + res = -res; + } + } + return res; + } + + protected: +}; + +namespace internal { +template +struct traits > + : traits< + Matrix > { + typedef PermutationStorage StorageKind; + typedef Matrix IndicesType; + typedef StorageIndex_ StorageIndex; + typedef void Scalar; +}; +} // namespace internal + +/** \class PermutationMatrix + * \ingroup Core_Module + * + * \brief Permutation matrix + * + * \tparam SizeAtCompileTime the number of rows/cols, or Dynamic + * \tparam MaxSizeAtCompileTime the maximum number of rows/cols, or Dynamic. This optional parameter defaults to + * SizeAtCompileTime. Most of the time, you should not have to specify it. \tparam StorageIndex_ the integer type of the + * indices + * + * This class represents a permutation matrix, internally stored as a vector of integers. + * + * \sa class PermutationBase, class PermutationWrapper, class DiagonalMatrix + */ +template +class PermutationMatrix + : public PermutationBase > { + typedef PermutationBase Base; + typedef internal::traits Traits; + + public: + typedef const PermutationMatrix& Nested; + +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename Traits::IndicesType IndicesType; + typedef typename Traits::StorageIndex StorageIndex; +#endif + + inline PermutationMatrix() {} + + /** Constructs an uninitialized permutation matrix of given size. + */ + explicit inline PermutationMatrix(Index size) : m_indices(size) { + eigen_internal_assert(size <= NumTraits::highest()); + } + + /** Copy constructor. */ + template + inline PermutationMatrix(const PermutationBase& other) : m_indices(other.indices()) {} + + /** Generic constructor from expression of the indices. The indices + * array has the meaning that the permutations sends each integer i to indices[i]. + * + * \warning It is your responsibility to check that the indices array that you passes actually + * describes a permutation, i.e., each value between 0 and n-1 occurs exactly once, where n is the + * array's size. + */ + template + explicit inline PermutationMatrix(const MatrixBase& indices) : m_indices(indices) {} + + /** Convert the Transpositions \a tr to a permutation matrix */ + template + explicit PermutationMatrix(const TranspositionsBase& tr) : m_indices(tr.size()) { + *this = tr; + } + + /** Copies the other permutation into *this */ + template + PermutationMatrix& operator=(const PermutationBase& other) { + m_indices = other.indices(); + return *this; + } + + /** Assignment from the Transpositions \a tr */ + template + PermutationMatrix& operator=(const TranspositionsBase& tr) { + return Base::operator=(tr.derived()); + } + + /** const version of indices(). */ + const IndicesType& indices() const { return m_indices; } + /** \returns a reference to the stored array representing the permutation. */ + IndicesType& indices() { return m_indices; } + + /**** multiplication helpers to hopefully get RVO ****/ + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + PermutationMatrix(const InverseImpl& other) + : m_indices(other.derived().nestedExpression().size()) { + eigen_internal_assert(m_indices.size() <= NumTraits::highest()); + StorageIndex end = StorageIndex(m_indices.size()); + for (StorageIndex i = 0; i < end; ++i) + m_indices.coeffRef(other.derived().nestedExpression().indices().coeff(i)) = i; + } + template + PermutationMatrix(internal::PermPermProduct_t, const Lhs& lhs, const Rhs& rhs) : m_indices(lhs.indices().size()) { + Base::assignProduct(lhs, rhs); + } +#endif + + protected: + IndicesType m_indices; +}; + +namespace internal { +template +struct traits, PacketAccess_> > + : traits< + Matrix > { + typedef PermutationStorage StorageKind; + typedef Map, PacketAccess_> IndicesType; + typedef StorageIndex_ StorageIndex; + typedef void Scalar; +}; +} // namespace internal + +template +class Map, PacketAccess_> + : public PermutationBase< + Map, PacketAccess_> > { + typedef PermutationBase Base; + typedef internal::traits Traits; + + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename Traits::IndicesType IndicesType; + typedef typename IndicesType::Scalar StorageIndex; +#endif + + inline Map(const StorageIndex* indicesPtr) : m_indices(indicesPtr) {} + + inline Map(const StorageIndex* indicesPtr, Index size) : m_indices(indicesPtr, size) {} + + /** Copies the other permutation into *this */ + template + Map& operator=(const PermutationBase& other) { + return Base::operator=(other.derived()); + } + + /** Assignment from the Transpositions \a tr */ + template + Map& operator=(const TranspositionsBase& tr) { + return Base::operator=(tr.derived()); + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + Map& operator=(const Map& other) { + m_indices = other.m_indices; + return *this; + } +#endif + + /** const version of indices(). */ + const IndicesType& indices() const { return m_indices; } + /** \returns a reference to the stored array representing the permutation. */ + IndicesType& indices() { return m_indices; } + + protected: + IndicesType m_indices; +}; + +template +class TranspositionsWrapper; +namespace internal { +template +struct traits > { + typedef PermutationStorage StorageKind; + typedef void Scalar; + typedef typename IndicesType_::Scalar StorageIndex; + typedef IndicesType_ IndicesType; + enum { + RowsAtCompileTime = IndicesType_::SizeAtCompileTime, + ColsAtCompileTime = IndicesType_::SizeAtCompileTime, + MaxRowsAtCompileTime = IndicesType::MaxSizeAtCompileTime, + MaxColsAtCompileTime = IndicesType::MaxSizeAtCompileTime, + Flags = 0 + }; +}; +} // namespace internal + +/** \class PermutationWrapper + * \ingroup Core_Module + * + * \brief Class to view a vector of integers as a permutation matrix + * + * \tparam IndicesType_ the type of the vector of integer (can be any compatible expression) + * + * This class allows to view any vector expression of integers as a permutation matrix. + * + * \sa class PermutationBase, class PermutationMatrix + */ +template +class PermutationWrapper : public PermutationBase > { + typedef PermutationBase Base; + typedef internal::traits Traits; + + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename Traits::IndicesType IndicesType; +#endif + + inline PermutationWrapper(const IndicesType& indices) : m_indices(indices) {} + + /** const version of indices(). */ + const internal::remove_all_t& indices() const { return m_indices; } + + protected: + typename IndicesType::Nested m_indices; +}; + +/** \returns the matrix with the permutation applied to the columns. + */ +template +EIGEN_DEVICE_FUNC const Product operator*( + const MatrixBase& matrix, const PermutationBase& permutation) { + return Product(matrix.derived(), permutation.derived()); +} + +/** \returns the matrix with the permutation applied to the rows. + */ +template +EIGEN_DEVICE_FUNC const Product operator*( + const PermutationBase& permutation, const MatrixBase& matrix) { + return Product(permutation.derived(), matrix.derived()); +} + +template +class InverseImpl : public EigenBase > { + typedef typename PermutationType::PlainPermutationType PlainPermutationType; + typedef internal::traits PermTraits; + + protected: + InverseImpl() {} + + public: + typedef Inverse InverseType; + using EigenBase >::derived; + +#ifndef EIGEN_PARSED_BY_DOXYGEN + typedef typename PermutationType::DenseMatrixType DenseMatrixType; + enum { + RowsAtCompileTime = PermTraits::RowsAtCompileTime, + ColsAtCompileTime = PermTraits::ColsAtCompileTime, + MaxRowsAtCompileTime = PermTraits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = PermTraits::MaxColsAtCompileTime + }; +#endif + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + void evalTo(MatrixBase& other) const { + other.setZero(); + for (Index i = 0; i < derived().rows(); ++i) + other.coeffRef(i, derived().nestedExpression().indices().coeff(i)) = typename DenseDerived::Scalar(1); + } +#endif + + /** \return the equivalent permutation matrix */ + PlainPermutationType eval() const { return derived(); } + + DenseMatrixType toDenseMatrix() const { return derived(); } + + /** \returns the matrix with the inverse permutation applied to the columns. + */ + template + friend const Product operator*(const MatrixBase& matrix, + const InverseType& trPerm) { + return Product(matrix.derived(), trPerm.derived()); + } + + /** \returns the matrix with the inverse permutation applied to the rows. + */ + template + const Product operator*(const MatrixBase& matrix) const { + return Product(derived(), matrix.derived()); + } +}; + +template +const PermutationWrapper MatrixBase::asPermutation() const { + return derived(); +} + +namespace internal { + +template <> +struct AssignmentKind { + typedef EigenBase2EigenBase Kind; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_PERMUTATIONMATRIX_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h new file mode 100644 index 00000000..a8307c7a --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h @@ -0,0 +1,1039 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// 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_DENSESTORAGEBASE_H +#define EIGEN_DENSESTORAGEBASE_H + +#if defined(EIGEN_INITIALIZE_MATRICES_BY_ZERO) +#define EIGEN_INITIALIZE_COEFFS +#define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED \ + for (Index i = 0; i < base().size(); ++i) coeffRef(i) = Scalar(0); +#elif defined(EIGEN_INITIALIZE_MATRICES_BY_NAN) +#define EIGEN_INITIALIZE_COEFFS +#define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED \ + for (Index i = 0; i < base().size(); ++i) coeffRef(i) = std::numeric_limits::quiet_NaN(); +#else +#undef EIGEN_INITIALIZE_COEFFS +#define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED +#endif + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct check_rows_cols_for_overflow { + EIGEN_STATIC_ASSERT(MaxRowsAtCompileTime* MaxColsAtCompileTime == MaxSizeAtCompileTime, + YOU MADE A PROGRAMMING MISTAKE) + template + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index, Index) {} +}; + +template +struct check_rows_cols_for_overflow { + template + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index, Index cols) { + constexpr Index MaxIndex = NumTraits::highest(); + bool error = cols > MaxIndex / MaxRowsAtCompileTime; + if (error) throw_std_bad_alloc(); + } +}; + +template +struct check_rows_cols_for_overflow { + template + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index rows, Index) { + constexpr Index MaxIndex = NumTraits::highest(); + bool error = rows > MaxIndex / MaxColsAtCompileTime; + if (error) throw_std_bad_alloc(); + } +}; + +template <> +struct check_rows_cols_for_overflow { + template + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE constexpr void run(Index rows, Index cols) { + constexpr Index MaxIndex = NumTraits::highest(); + bool error = cols == 0 ? false : (rows > MaxIndex / cols); + if (error) throw_std_bad_alloc(); + } +}; + +template +struct conservative_resize_like_impl; + +template +struct matrix_swap_impl; + +} // end namespace internal + +#ifdef EIGEN_PARSED_BY_DOXYGEN +namespace doxygen { + +// This is a workaround to doxygen not being able to understand the inheritance logic +// when it is hidden by the dense_xpr_base helper struct. +// Moreover, doxygen fails to include members that are not documented in the declaration body of +// MatrixBase if we inherits MatrixBase >, +// this is why we simply inherits MatrixBase, though this does not make sense. + +/** This class is just a workaround for Doxygen and it does not not actually exist. */ +template +struct dense_xpr_base_dispatcher; +/** This class is just a workaround for Doxygen and it does not not actually exist. */ +template +struct dense_xpr_base_dispatcher> : public MatrixBase {}; +/** This class is just a workaround for Doxygen and it does not not actually exist. */ +template +struct dense_xpr_base_dispatcher> : public ArrayBase {}; + +} // namespace doxygen + +/** \class PlainObjectBase + * \ingroup Core_Module + * \brief %Dense storage base class for matrices and arrays. + * + * 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_PLAINOBJECTBASE_PLUGIN. + * + * \tparam Derived is the derived type, e.g., a Matrix or Array + * + * \sa \ref TopicClassHierarchy + */ +template +class PlainObjectBase : public doxygen::dense_xpr_base_dispatcher +#else +template +class PlainObjectBase : public internal::dense_xpr_base::type +#endif +{ + public: + enum { Options = internal::traits::Options }; + typedef typename internal::dense_xpr_base::type Base; + + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Scalar Scalar; + + typedef typename internal::packet_traits::type PacketScalar; + typedef typename NumTraits::Real RealScalar; + typedef Derived DenseType; + + using Base::ColsAtCompileTime; + using Base::Flags; + using Base::IsVectorAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxSizeAtCompileTime; + using Base::RowsAtCompileTime; + using Base::SizeAtCompileTime; + + typedef Eigen::Map MapType; + typedef const Eigen::Map ConstMapType; + typedef Eigen::Map AlignedMapType; + typedef const Eigen::Map ConstAlignedMapType; + template + struct StridedMapType { + typedef Eigen::Map type; + }; + template + struct StridedConstMapType { + typedef Eigen::Map type; + }; + template + struct StridedAlignedMapType { + typedef Eigen::Map type; + }; + template + struct StridedConstAlignedMapType { + typedef Eigen::Map type; + }; + + protected: + DenseStorage m_storage; + + public: + enum { NeedsToAlign = (SizeAtCompileTime != Dynamic) && (internal::traits::Alignment > 0) }; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + + EIGEN_STATIC_ASSERT(internal::check_implication(MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1, + (int(Options) & RowMajor) == RowMajor), + INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT(internal::check_implication(MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1, + (int(Options) & RowMajor) == 0), + INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT((RowsAtCompileTime == Dynamic) || (RowsAtCompileTime >= 0), INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT((ColsAtCompileTime == Dynamic) || (ColsAtCompileTime >= 0), INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT((MaxRowsAtCompileTime == Dynamic) || (MaxRowsAtCompileTime >= 0), + INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT((MaxColsAtCompileTime == Dynamic) || (MaxColsAtCompileTime >= 0), + INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT((MaxRowsAtCompileTime == RowsAtCompileTime || RowsAtCompileTime == Dynamic), + INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT((MaxColsAtCompileTime == ColsAtCompileTime || ColsAtCompileTime == Dynamic), + INVALID_MATRIX_TEMPLATE_PARAMETERS) + EIGEN_STATIC_ASSERT(((Options & (DontAlign | RowMajor)) == Options), INVALID_MATRIX_TEMPLATE_PARAMETERS) + + EIGEN_DEVICE_FUNC Base& base() { return *static_cast(this); } + EIGEN_DEVICE_FUNC const Base& base() const { return *static_cast(this); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_storage.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_storage.cols(); } + + /** This is an overloaded version of DenseCoeffsBase::coeff(Index,Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeff(Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr const Scalar& coeff(Index rowId, Index colId) const { + if (Flags & RowMajorBit) + return m_storage.data()[colId + rowId * m_storage.cols()]; + else // column-major + return m_storage.data()[rowId + colId * m_storage.rows()]; + } + + /** This is an overloaded version of DenseCoeffsBase::coeff(Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeff(Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const { return m_storage.data()[index]; } + + /** This is an overloaded version of DenseCoeffsBase::coeffRef(Index,Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeffRef(Index,Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& coeffRef(Index rowId, Index colId) { + if (Flags & RowMajorBit) + return m_storage.data()[colId + rowId * m_storage.cols()]; + else // column-major + return m_storage.data()[rowId + colId * m_storage.rows()]; + } + + /** This is an overloaded version of DenseCoeffsBase::coeffRef(Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeffRef(Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& coeffRef(Index index) { return m_storage.data()[index]; } + + /** This is the const version of coeffRef(Index,Index) which is thus synonym of coeff(Index,Index). + * It is provided for convenience. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr const Scalar& coeffRef(Index rowId, Index colId) const { + if (Flags & RowMajorBit) + return m_storage.data()[colId + rowId * m_storage.cols()]; + else // column-major + return m_storage.data()[rowId + colId * m_storage.rows()]; + } + + /** This is the const version of coeffRef(Index) which is thus synonym of coeff(Index). + * It is provided for convenience. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr const Scalar& coeffRef(Index index) const { + return m_storage.data()[index]; + } + + /** \internal */ + template + EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const { + return internal::ploadt( + m_storage.data() + (Flags & RowMajorBit ? colId + rowId * m_storage.cols() : rowId + colId * m_storage.rows())); + } + + /** \internal */ + template + EIGEN_STRONG_INLINE PacketScalar packet(Index index) const { + return internal::ploadt(m_storage.data() + index); + } + + /** \internal */ + template + EIGEN_STRONG_INLINE void writePacket(Index rowId, Index colId, const PacketScalar& val) { + internal::pstoret( + m_storage.data() + (Flags & RowMajorBit ? colId + rowId * m_storage.cols() : rowId + colId * m_storage.rows()), + val); + } + + /** \internal */ + template + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketScalar& val) { + internal::pstoret(m_storage.data() + index, val); + } + + /** \returns a const pointer to the data array of this matrix */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar* data() const { return m_storage.data(); } + + /** \returns a pointer to the data array of this matrix */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar* data() { return m_storage.data(); } + + /** Resizes \c *this to a \a rows x \a cols matrix. + * + * This method is intended for dynamic-size matrices, although it is legal to call it on any + * matrix as long as fixed dimensions are left unchanged. If you only want to change the number + * of rows and/or of columns, you can use resize(NoChange_t, Index), resize(Index, NoChange_t). + * + * If the current number of coefficients of \c *this exactly matches the + * product \a rows * \a cols, then no memory allocation is performed and + * the current values are left unchanged. In all other cases, including + * shrinking, the data is reallocated and all previous values are lost. + * + * Example: \include Matrix_resize_int_int.cpp + * Output: \verbinclude Matrix_resize_int_int.out + * + * \sa resize(Index) for vectors, resize(NoChange_t, Index), resize(Index, NoChange_t) + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize(Index rows, Index cols) { + eigen_assert(internal::check_implication(RowsAtCompileTime != Dynamic, rows == RowsAtCompileTime) && + internal::check_implication(ColsAtCompileTime != Dynamic, cols == ColsAtCompileTime) && + internal::check_implication(RowsAtCompileTime == Dynamic && MaxRowsAtCompileTime != Dynamic, + rows <= MaxRowsAtCompileTime) && + internal::check_implication(ColsAtCompileTime == Dynamic && MaxColsAtCompileTime != Dynamic, + cols <= MaxColsAtCompileTime) && + rows >= 0 && cols >= 0 && "Invalid sizes when resizing a matrix or array."); + internal::check_rows_cols_for_overflow::run(rows, + cols); +#ifdef EIGEN_INITIALIZE_COEFFS + Index size = rows * cols; + bool size_changed = size != this->size(); + m_storage.resize(size, rows, cols); + if (size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED +#else + m_storage.resize(rows * cols, rows, cols); +#endif + } + + /** Resizes \c *this to a vector of length \a size + * + * \only_for_vectors. This method does not work for + * partially dynamic matrices when the static dimension is anything other + * than 1. For example it will not work with Matrix. + * + * Example: \include Matrix_resize_int.cpp + * Output: \verbinclude Matrix_resize_int.out + * + * \sa resize(Index,Index), resize(NoChange_t, Index), resize(Index, NoChange_t) + */ + EIGEN_DEVICE_FUNC inline constexpr void resize(Index size) { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(PlainObjectBase) + eigen_assert(((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime == Dynamic || size <= MaxSizeAtCompileTime)) || + SizeAtCompileTime == size) && + size >= 0); +#ifdef EIGEN_INITIALIZE_COEFFS + bool size_changed = size != this->size(); +#endif + if (RowsAtCompileTime == 1) + m_storage.resize(size, 1, size); + else + m_storage.resize(size, size, 1); +#ifdef EIGEN_INITIALIZE_COEFFS + if (size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED +#endif + } + + /** Resizes the matrix, changing only the number of columns. For the parameter of type NoChange_t, just pass the + * special value \c NoChange as in the example below. + * + * Example: \include Matrix_resize_NoChange_int.cpp + * Output: \verbinclude Matrix_resize_NoChange_int.out + * + * \sa resize(Index,Index) + */ + EIGEN_DEVICE_FUNC inline constexpr void resize(NoChange_t, Index cols) { resize(rows(), cols); } + + /** Resizes the matrix, changing only the number of rows. For the parameter of type NoChange_t, just pass the special + * value \c NoChange as in the example below. + * + * Example: \include Matrix_resize_int_NoChange.cpp + * Output: \verbinclude Matrix_resize_int_NoChange.out + * + * \sa resize(Index,Index) + */ + EIGEN_DEVICE_FUNC inline constexpr void resize(Index rows, NoChange_t) { resize(rows, cols()); } + + /** Resizes \c *this to have the same dimensions as \a other. + * Takes care of doing all the checking that's needed. + * + * Note that copying a row-vector into a vector (and conversely) is allowed. + * The resizing, if any, is then done in the appropriate way so that row-vectors + * remain row-vectors and vectors remain vectors. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resizeLike(const EigenBase& _other) { + const OtherDerived& other = _other.derived(); + internal::check_rows_cols_for_overflow::run( + other.rows(), other.cols()); + const Index othersize = other.rows() * other.cols(); + if (RowsAtCompileTime == 1) { + eigen_assert(other.rows() == 1 || other.cols() == 1); + resize(1, othersize); + } else if (ColsAtCompileTime == 1) { + eigen_assert(other.rows() == 1 || other.cols() == 1); + resize(othersize, 1); + } else + resize(other.rows(), other.cols()); + } + + /** Resizes the matrix to \a rows x \a cols while leaving old values untouched. + * + * The method is intended for matrices of dynamic size. If you only want to change the number + * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or + * conservativeResize(Index, NoChange_t). + * + * Matrices are resized relative to the top-left element. In case values need to be + * appended to the matrix they will be uninitialized. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(Index rows, Index cols) { + internal::conservative_resize_like_impl::run(*this, rows, cols); + } + + /** Resizes the matrix to \a rows x \a cols while leaving old values untouched. + * + * As opposed to conservativeResize(Index rows, Index cols), this version leaves + * the number of columns unchanged. + * + * In case the matrix is growing, new rows will be uninitialized. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(Index rows, NoChange_t) { + // Note: see the comment in conservativeResize(Index,Index) + conservativeResize(rows, cols()); + } + + /** Resizes the matrix to \a rows x \a cols while leaving old values untouched. + * + * As opposed to conservativeResize(Index rows, Index cols), this version leaves + * the number of rows unchanged. + * + * In case the matrix is growing, new columns will be uninitialized. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(NoChange_t, Index cols) { + // Note: see the comment in conservativeResize(Index,Index) + conservativeResize(rows(), cols); + } + + /** Resizes the vector to \a size while retaining old values. + * + * \only_for_vectors. This method does not work for + * partially dynamic matrices when the static dimension is anything other + * than 1. For example it will not work with Matrix. + * + * When values are appended, they will be uninitialized. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(Index size) { + internal::conservative_resize_like_impl::run(*this, size); + } + + /** Resizes the matrix to \a rows x \a cols of \c other, while leaving old values untouched. + * + * The method is intended for matrices of dynamic size. If you only want to change the number + * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or + * conservativeResize(Index, NoChange_t). + * + * Matrices are resized relative to the top-left element. In case values need to be + * appended to the matrix they will copied from \c other. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResizeLike(const DenseBase& other) { + internal::conservative_resize_like_impl::run(*this, other); + } + + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const PlainObjectBase& other) { return _set(other); } + + /** \sa MatrixBase::lazyAssign() */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& lazyAssign(const DenseBase& other) { + _resize_to_match(other); + return Base::lazyAssign(other.derived()); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const ReturnByValue& func) { + resize(func.rows(), func.cols()); + return Base::operator=(func); + } + + // Prevent user from trying to instantiate PlainObjectBase objects + // by making all its constructor protected. See bug 1074. + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase() : m_storage() { + // EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN + // FIXME is it still needed ? + /** \internal */ + EIGEN_DEVICE_FUNC explicit PlainObjectBase(internal::constructor_without_unaligned_array_assert) + : m_storage(internal::constructor_without_unaligned_array_assert()) { + // EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } +#endif + + EIGEN_DEVICE_FUNC PlainObjectBase(PlainObjectBase&& other) EIGEN_NOEXCEPT : m_storage(std::move(other.m_storage)) {} + + EIGEN_DEVICE_FUNC PlainObjectBase& operator=(PlainObjectBase&& other) EIGEN_NOEXCEPT { + m_storage = std::move(other.m_storage); + return *this; + } + + /** Copy constructor */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const PlainObjectBase& other) + : Base(), m_storage(other.m_storage) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(Index size, Index rows, Index cols) + : m_storage(size, rows, cols) { + // EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } + + /** \brief Construct a row of column vector with fixed size from an arbitrary number of coefficients. + * + * \only_for_vectors + * + * This constructor is for 1D array or vectors with more than 4 coefficients. + * + * \warning To construct a column (resp. row) vector of fixed length, the number of values passed to this + * constructor must match the the fixed number of rows (resp. columns) of \c *this. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2, + const Scalar& a3, const ArgTypes&... args) + : m_storage() { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, sizeof...(args) + 4); + m_storage.data()[0] = a0; + m_storage.data()[1] = a1; + m_storage.data()[2] = a2; + m_storage.data()[3] = a3; + Index i = 4; + auto x = {(m_storage.data()[i++] = args, 0)...}; + static_cast(x); + } + + /** \brief Constructs a Matrix or Array and initializes it by elements given by an initializer list of initializer + * lists + */ + EIGEN_DEVICE_FUNC explicit constexpr EIGEN_STRONG_INLINE PlainObjectBase( + const std::initializer_list>& list) + : m_storage() { + size_t list_size = 0; + if (list.begin() != list.end()) { + list_size = list.begin()->size(); + } + + // This is to allow syntax like VectorXi {{1, 2, 3, 4}} + if (ColsAtCompileTime == 1 && list.size() == 1) { + eigen_assert(list_size == static_cast(RowsAtCompileTime) || RowsAtCompileTime == Dynamic); + resize(list_size, ColsAtCompileTime); + if (list.begin()->begin() != nullptr) { + std::copy(list.begin()->begin(), list.begin()->end(), m_storage.data()); + } + } else { + eigen_assert(list.size() == static_cast(RowsAtCompileTime) || RowsAtCompileTime == Dynamic); + eigen_assert(list_size == static_cast(ColsAtCompileTime) || ColsAtCompileTime == Dynamic); + resize(list.size(), list_size); + + Index row_index = 0; + for (const std::initializer_list& row : list) { + eigen_assert(list_size == row.size()); + Index col_index = 0; + for (const Scalar& e : row) { + coeffRef(row_index, col_index) = e; + ++col_index; + } + ++row_index; + } + } + } + + /** \sa PlainObjectBase::operator=(const EigenBase&) */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase& other) : m_storage() { + resizeLike(other); + _set_noalias(other); + } + + /** \sa PlainObjectBase::operator=(const EigenBase&) */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase& other) : m_storage() { + resizeLike(other); + *this = other.derived(); + } + /** \brief Copy constructor with in-place evaluation */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const ReturnByValue& other) { + // FIXME this does not automatically transpose vectors if necessary + resize(other.rows(), other.cols()); + other.evalTo(this->derived()); + } + + public: + /** \brief Copies the generic expression \a other into *this. + * \copydetails DenseBase::operator=(const EigenBase &other) + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const EigenBase& other) { + _resize_to_match(other); + Base::operator=(other.derived()); + return this->derived(); + } + + /** \name Map + * These are convenience functions returning Map objects. The Map() static functions return unaligned Map objects, + * while the AlignedMap() functions return aligned Map objects and thus should be called only with 16-byte-aligned + * \a data pointers. + * + * Here is an example using strides: + * \include Matrix_Map_stride.cpp + * Output: \verbinclude Matrix_Map_stride.out + * + * \see class Map + */ + ///@{ + static inline ConstMapType Map(const Scalar* data) { return ConstMapType(data); } + static inline MapType Map(Scalar* data) { return MapType(data); } + static inline ConstMapType Map(const Scalar* data, Index size) { return ConstMapType(data, size); } + static inline MapType Map(Scalar* data, Index size) { return MapType(data, size); } + static inline ConstMapType Map(const Scalar* data, Index rows, Index cols) { return ConstMapType(data, rows, cols); } + static inline MapType Map(Scalar* data, Index rows, Index cols) { return MapType(data, rows, cols); } + + static inline ConstAlignedMapType MapAligned(const Scalar* data) { return ConstAlignedMapType(data); } + static inline AlignedMapType MapAligned(Scalar* data) { return AlignedMapType(data); } + static inline ConstAlignedMapType MapAligned(const Scalar* data, Index size) { + return ConstAlignedMapType(data, size); + } + static inline AlignedMapType MapAligned(Scalar* data, Index size) { return AlignedMapType(data, size); } + static inline ConstAlignedMapType MapAligned(const Scalar* data, Index rows, Index cols) { + return ConstAlignedMapType(data, rows, cols); + } + static inline AlignedMapType MapAligned(Scalar* data, Index rows, Index cols) { + return AlignedMapType(data, rows, cols); + } + + template + static inline typename StridedConstMapType>::type Map(const Scalar* data, + const Stride& stride) { + return typename StridedConstMapType>::type(data, stride); + } + template + static inline typename StridedMapType>::type Map(Scalar* data, + const Stride& stride) { + return typename StridedMapType>::type(data, stride); + } + template + static inline typename StridedConstMapType>::type Map(const Scalar* data, Index size, + const Stride& stride) { + return typename StridedConstMapType>::type(data, size, stride); + } + template + static inline typename StridedMapType>::type Map(Scalar* data, Index size, + const Stride& stride) { + return typename StridedMapType>::type(data, size, stride); + } + template + static inline typename StridedConstMapType>::type Map(const Scalar* data, Index rows, Index cols, + const Stride& stride) { + return typename StridedConstMapType>::type(data, rows, cols, stride); + } + template + static inline typename StridedMapType>::type Map(Scalar* data, Index rows, Index cols, + const Stride& stride) { + return typename StridedMapType>::type(data, rows, cols, stride); + } + + template + static inline typename StridedConstAlignedMapType>::type MapAligned( + const Scalar* data, const Stride& stride) { + return typename StridedConstAlignedMapType>::type(data, stride); + } + template + static inline typename StridedAlignedMapType>::type MapAligned( + Scalar* data, const Stride& stride) { + return typename StridedAlignedMapType>::type(data, stride); + } + template + static inline typename StridedConstAlignedMapType>::type MapAligned( + const Scalar* data, Index size, const Stride& stride) { + return typename StridedConstAlignedMapType>::type(data, size, stride); + } + template + static inline typename StridedAlignedMapType>::type MapAligned( + Scalar* data, Index size, const Stride& stride) { + return typename StridedAlignedMapType>::type(data, size, stride); + } + template + static inline typename StridedConstAlignedMapType>::type MapAligned( + const Scalar* data, Index rows, Index cols, const Stride& stride) { + return typename StridedConstAlignedMapType>::type(data, rows, cols, stride); + } + template + static inline typename StridedAlignedMapType>::type MapAligned( + Scalar* data, Index rows, Index cols, const Stride& stride) { + return typename StridedAlignedMapType>::type(data, rows, cols, stride); + } + ///@} + + using Base::setConstant; + EIGEN_DEVICE_FUNC Derived& setConstant(Index size, const Scalar& val); + EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, Index cols, const Scalar& val); + EIGEN_DEVICE_FUNC Derived& setConstant(NoChange_t, Index cols, const Scalar& val); + EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, NoChange_t, const Scalar& val); + + using Base::setZero; + EIGEN_DEVICE_FUNC Derived& setZero(Index size); + EIGEN_DEVICE_FUNC Derived& setZero(Index rows, Index cols); + EIGEN_DEVICE_FUNC Derived& setZero(NoChange_t, Index cols); + EIGEN_DEVICE_FUNC Derived& setZero(Index rows, NoChange_t); + + using Base::setOnes; + EIGEN_DEVICE_FUNC Derived& setOnes(Index size); + EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, Index cols); + EIGEN_DEVICE_FUNC Derived& setOnes(NoChange_t, Index cols); + EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, NoChange_t); + + using Base::setRandom; + Derived& setRandom(Index size); + Derived& setRandom(Index rows, Index cols); + Derived& setRandom(NoChange_t, Index cols); + Derived& setRandom(Index rows, NoChange_t); + +#ifdef EIGEN_PLAINOBJECTBASE_PLUGIN +#include EIGEN_PLAINOBJECTBASE_PLUGIN +#endif + + protected: + /** \internal Resizes *this in preparation for assigning \a other to it. + * Takes care of doing all the checking that's needed. + * + * Note that copying a row-vector into a vector (and conversely) is allowed. + * The resizing, if any, is then done in the appropriate way so that row-vectors + * remain row-vectors and vectors remain vectors. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _resize_to_match(const EigenBase& other) { +#ifdef EIGEN_NO_AUTOMATIC_RESIZING + eigen_assert((this->size() == 0 || (IsVectorAtCompileTime ? (this->size() == other.size()) + : (rows() == other.rows() && cols() == other.cols()))) && + "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined"); + EIGEN_ONLY_USED_FOR_DEBUG(other); +#else + resizeLike(other); +#endif + } + + /** + * \brief Copies the value of the expression \a other into \c *this with automatic resizing. + * + * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized), + * it will be initialized. + * + * Note that copying a row-vector into a vector (and conversely) is allowed. + * The resizing, if any, is then done in the appropriate way so that row-vectors + * remain row-vectors and vectors remain vectors. + * + * \sa operator=(const MatrixBase&), _set_noalias() + * + * \internal + */ + // aliasing is dealt once in internal::call_assignment + // so at this stage we have to assume aliasing... and resising has to be done later. + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& _set(const DenseBase& other) { + internal::call_assignment(this->derived(), other.derived()); + return this->derived(); + } + + /** \internal Like _set() but additionally makes the assumption that no aliasing effect can happen (which + * is the case when creating a new matrix) so one can enforce lazy evaluation. + * + * \sa operator=(const MatrixBase&), _set() + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& _set_noalias(const DenseBase& other) { + // I don't think we need this resize call since the lazyAssign will anyways resize + // and lazyAssign will be called by the assign selector. + //_resize_to_match(other); + // the 'false' below means to enforce lazy evaluation. We don't use lazyAssign() because + // it wouldn't allow to copy a row-vector into a column-vector. + internal::call_assignment_no_alias(this->derived(), other.derived(), + internal::assign_op()); + return this->derived(); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2(Index rows, Index cols, + std::enable_if_t* = 0) { + EIGEN_STATIC_ASSERT(internal::is_valid_index_type::value && internal::is_valid_index_type::value, + T0 AND T1 MUST BE INTEGER TYPES) + resize(rows, cols); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2(const T0& val0, const T1& val1, + std::enable_if_t* = 0) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2) + m_storage.data()[0] = Scalar(val0); + m_storage.data()[1] = Scalar(val1); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2( + const Index& val0, const Index& val1, + std::enable_if_t<(!internal::is_same::value) && (internal::is_same::value) && + (internal::is_same::value) && Base::SizeAtCompileTime == 2, + T1>* = 0) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2) + m_storage.data()[0] = Scalar(val0); + m_storage.data()[1] = Scalar(val1); + } + + // The argument is convertible to the Index type and we either have a non 1x1 Matrix, or a dynamic-sized Array, + // then the argument is meant to be the size of the object. + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1( + Index size, + std::enable_if_t<(Base::SizeAtCompileTime != 1 || !internal::is_convertible::value) && + ((!internal::is_same::XprKind, ArrayXpr>::value || + Base::SizeAtCompileTime == Dynamic)), + T>* = 0) { + // NOTE MSVC 2008 complains if we directly put bool(NumTraits::IsInteger) as the EIGEN_STATIC_ASSERT argument. + const bool is_integer_alike = internal::is_valid_index_type::value; + EIGEN_UNUSED_VARIABLE(is_integer_alike); + EIGEN_STATIC_ASSERT(is_integer_alike, FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) + resize(size); + } + + // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar + // type can be implicitly converted) + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1( + const Scalar& val0, + std::enable_if_t::value, T>* = 0) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1) + m_storage.data()[0] = val0; + } + + // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar + // type match the index type) + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1( + const Index& val0, + std::enable_if_t<(!internal::is_same::value) && (internal::is_same::value) && + Base::SizeAtCompileTime == 1 && internal::is_convertible::value, + T*>* = 0) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1) + m_storage.data()[0] = Scalar(val0); + } + + // Initialize a fixed size matrix from a pointer to raw data + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const Scalar* data) { + this->_set_noalias(ConstMapType(data)); + } + + // Initialize an arbitrary matrix from a dense expression + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const DenseBase& other) { + this->_set_noalias(other); + } + + // Initialize an arbitrary matrix from an object convertible to the Derived type. + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const Derived& other) { + this->_set_noalias(other); + } + + // Initialize an arbitrary matrix from a generic Eigen expression + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const EigenBase& other) { + this->derived() = other; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const ReturnByValue& other) { + resize(other.rows(), other.cols()); + other.evalTo(this->derived()); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1(const RotationBase& r) { + this->derived() = r; + } + + // For fixed-size Array + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1( + const Scalar& val0, + std::enable_if_t::value && + internal::is_same::XprKind, ArrayXpr>::value, + T>* = 0) { + Base::setConstant(val0); + } + + // For fixed-size Array + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init1( + const Index& val0, + std::enable_if_t<(!internal::is_same::value) && (internal::is_same::value) && + Base::SizeAtCompileTime != Dynamic && Base::SizeAtCompileTime != 1 && + internal::is_convertible::value && + internal::is_same::XprKind, ArrayXpr>::value, + T*>* = 0) { + Base::setConstant(val0); + } + + template + friend struct internal::matrix_swap_impl; + + public: +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal + * \brief Override DenseBase::swap() since for dynamic-sized matrices + * of same type it is enough to swap the data pointers. + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(DenseBase& other) { + enum {SwapPointers = internal::is_same::value && Base::SizeAtCompileTime == Dynamic}; + internal::matrix_swap_impl::run(this->derived(), other.derived()); + } + + /** \internal + * \brief const version forwarded to DenseBase::swap + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void swap(DenseBase const& other) { + Base::swap(other.derived()); + } + + enum {IsPlainObjectBase = 1}; +#endif + public: + // These apparently need to be down here for nvcc+icc to prevent duplicate + // Map symbol. + template + friend class Eigen::Map; + friend class Eigen::Map; + friend class Eigen::Map; +#if EIGEN_MAX_ALIGN_BYTES > 0 + // for EIGEN_MAX_ALIGN_BYTES==0, AlignedMax==Unaligned, and many compilers generate warnings for friend-ing a class + // twice. + friend class Eigen::Map; + friend class Eigen::Map; +#endif +}; + +namespace internal { + +template +struct conservative_resize_like_impl { + static constexpr bool IsRelocatable = std::is_trivially_copyable::value; + static void run(DenseBase& _this, Index rows, Index cols) { + if (_this.rows() == rows && _this.cols() == cols) return; + EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(Derived) + + if (IsRelocatable && + ((Derived::IsRowMajor && _this.cols() == cols) || // row-major and we change only the number of rows + (!Derived::IsRowMajor && _this.rows() == rows))) // column-major and we change only the number of columns + { + internal::check_rows_cols_for_overflow::run(rows, cols); + _this.derived().m_storage.conservativeResize(rows * cols, rows, cols); + } else { + // The storage order does not allow us to use reallocation. + Derived tmp(rows, cols); + const Index common_rows = numext::mini(rows, _this.rows()); + const Index common_cols = numext::mini(cols, _this.cols()); + tmp.block(0, 0, common_rows, common_cols) = _this.block(0, 0, common_rows, common_cols); + _this.derived().swap(tmp); + } + } + + static void run(DenseBase& _this, const DenseBase& other) { + if (_this.rows() == other.rows() && _this.cols() == other.cols()) return; + + // Note: Here is space for improvement. Basically, for conservativeResize(Index,Index), + // neither RowsAtCompileTime or ColsAtCompileTime must be Dynamic. If only one of the + // dimensions is dynamic, one could use either conservativeResize(Index rows, NoChange_t) or + // conservativeResize(NoChange_t, Index cols). For these methods new static asserts like + // EIGEN_STATIC_ASSERT_DYNAMIC_ROWS and EIGEN_STATIC_ASSERT_DYNAMIC_COLS would be good. + EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(Derived) + EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(OtherDerived) + + if (IsRelocatable && + ((Derived::IsRowMajor && _this.cols() == other.cols()) || // row-major and we change only the number of rows + (!Derived::IsRowMajor && + _this.rows() == other.rows()))) // column-major and we change only the number of columns + { + const Index new_rows = other.rows() - _this.rows(); + const Index new_cols = other.cols() - _this.cols(); + _this.derived().m_storage.conservativeResize(other.size(), other.rows(), other.cols()); + if (new_rows > 0) + _this.bottomRightCorner(new_rows, other.cols()) = other.bottomRows(new_rows); + else if (new_cols > 0) + _this.bottomRightCorner(other.rows(), new_cols) = other.rightCols(new_cols); + } else { + // The storage order does not allow us to use reallocation. + Derived tmp(other); + const Index common_rows = numext::mini(tmp.rows(), _this.rows()); + const Index common_cols = numext::mini(tmp.cols(), _this.cols()); + tmp.block(0, 0, common_rows, common_cols) = _this.block(0, 0, common_rows, common_cols); + _this.derived().swap(tmp); + } + } +}; + +// Here, the specialization for vectors inherits from the general matrix case +// to allow calling .conservativeResize(rows,cols) on vectors. +template +struct conservative_resize_like_impl + : conservative_resize_like_impl { + typedef conservative_resize_like_impl Base; + using Base::IsRelocatable; + using Base::run; + + static void run(DenseBase& _this, Index size) { + const Index new_rows = Derived::RowsAtCompileTime == 1 ? 1 : size; + const Index new_cols = Derived::RowsAtCompileTime == 1 ? size : 1; + if (IsRelocatable) + _this.derived().m_storage.conservativeResize(size, new_rows, new_cols); + else + Base::run(_this.derived(), new_rows, new_cols); + } + + static void run(DenseBase& _this, const DenseBase& other) { + if (_this.rows() == other.rows() && _this.cols() == other.cols()) return; + + const Index num_new_elements = other.size() - _this.size(); + + const Index new_rows = Derived::RowsAtCompileTime == 1 ? 1 : other.rows(); + const Index new_cols = Derived::RowsAtCompileTime == 1 ? other.cols() : 1; + if (IsRelocatable) + _this.derived().m_storage.conservativeResize(other.size(), new_rows, new_cols); + else + Base::run(_this.derived(), new_rows, new_cols); + + if (num_new_elements > 0) _this.tail(num_new_elements) = other.tail(num_new_elements); + } +}; + +template +struct matrix_swap_impl { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(MatrixTypeA& a, MatrixTypeB& b) { a.base().swap(b); } +}; + +template +struct matrix_swap_impl { + EIGEN_DEVICE_FUNC static inline void run(MatrixTypeA& a, MatrixTypeB& b) { + static_cast(a).m_storage.swap(static_cast(b).m_storage); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_DENSESTORAGEBASE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Product.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Product.h new file mode 100644 index 00000000..6bad832e --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Product.h @@ -0,0 +1,174 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2011 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_PRODUCT_H +#define EIGEN_PRODUCT_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +template +class ProductImpl; + +namespace internal { + +template +struct traits > { + typedef remove_all_t LhsCleaned; + typedef remove_all_t RhsCleaned; + typedef traits LhsTraits; + typedef traits RhsTraits; + + typedef MatrixXpr XprKind; + + typedef typename ScalarBinaryOpTraits::Scalar, + typename traits::Scalar>::ReturnType Scalar; + typedef typename product_promote_storage_type::ret>::ret StorageKind; + typedef typename promote_index_type::type + StorageIndex; + + enum { + RowsAtCompileTime = LhsTraits::RowsAtCompileTime, + ColsAtCompileTime = RhsTraits::ColsAtCompileTime, + MaxRowsAtCompileTime = LhsTraits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = RhsTraits::MaxColsAtCompileTime, + + // FIXME: only needed by GeneralMatrixMatrixTriangular + InnerSize = min_size_prefer_fixed(LhsTraits::ColsAtCompileTime, RhsTraits::RowsAtCompileTime), + + // The storage order is somewhat arbitrary here. The correct one will be determined through the evaluator. + Flags = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? RowMajorBit + : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0 + : (((LhsTraits::Flags & NoPreferredStorageOrderBit) && (RhsTraits::Flags & RowMajorBit)) || + ((RhsTraits::Flags & NoPreferredStorageOrderBit) && (LhsTraits::Flags & RowMajorBit))) + ? RowMajorBit + : NoPreferredStorageOrderBit + }; +}; + +} // end namespace internal + +/** \class Product + * \ingroup Core_Module + * + * \brief Expression of the product of two arbitrary matrices or vectors + * + * \tparam Lhs_ the type of the left-hand side expression + * \tparam Rhs_ the type of the right-hand side expression + * + * This class represents an expression of the product of two arbitrary matrices. + * + * The other template parameters are: + * \tparam Option can be DefaultProduct, AliasFreeProduct, or LazyProduct + * + */ +template +class Product + : public ProductImpl::StorageKind, typename internal::traits::StorageKind, + internal::product_type::ret>::ret> { + public: + typedef Lhs_ Lhs; + typedef Rhs_ Rhs; + + typedef + typename ProductImpl::StorageKind, typename internal::traits::StorageKind, + internal::product_type::ret>::ret>::Base Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(Product) + + typedef typename internal::ref_selector::type LhsNested; + typedef typename internal::ref_selector::type RhsNested; + typedef internal::remove_all_t LhsNestedCleaned; + typedef internal::remove_all_t RhsNestedCleaned; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Product(const Lhs& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) { + eigen_assert(lhs.cols() == rhs.rows() && "invalid matrix product" && + "if you wanted a coeff-wise or a dot product use the respective explicit functions"); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const LhsNestedCleaned& lhs() const { return m_lhs; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const RhsNestedCleaned& rhs() const { return m_rhs; } + + protected: + LhsNested m_lhs; + RhsNested m_rhs; +}; + +namespace internal { + +template ::ret> +class dense_product_base : public internal::dense_xpr_base >::type {}; + +/** Conversion to scalar for inner-products */ +template +class dense_product_base + : public internal::dense_xpr_base >::type { + typedef Product ProductXpr; + typedef typename internal::dense_xpr_base::type Base; + + public: + using Base::derived; + typedef typename Base::Scalar Scalar; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator const Scalar() const { + return internal::evaluator(derived()).coeff(0, 0); + } +}; + +} // namespace internal + +// Generic API dispatcher +template +class ProductImpl : public internal::generic_xpr_base, MatrixXpr, StorageKind>::type { + public: + typedef typename internal::generic_xpr_base, MatrixXpr, StorageKind>::type Base; +}; + +template +class ProductImpl : public internal::dense_product_base { + typedef Product Derived; + + public: + typedef typename internal::dense_product_base Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Derived) + protected: + enum { + IsOneByOne = (RowsAtCompileTime == 1 || RowsAtCompileTime == Dynamic) && + (ColsAtCompileTime == 1 || ColsAtCompileTime == Dynamic), + EnableCoeff = IsOneByOne || Option == LazyProduct + }; + + public: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index row, Index col) const { + EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS); + eigen_assert((Option == LazyProduct) || (this->rows() == 1 && this->cols() == 1)); + + return internal::evaluator(derived()).coeff(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index i) const { + EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS); + eigen_assert((Option == LazyProduct) || (this->rows() == 1 && this->cols() == 1)); + + return internal::evaluator(derived()).coeff(i); + } +}; + +} // end namespace Eigen + +#endif // EIGEN_PRODUCT_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h new file mode 100644 index 00000000..19c25604 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h @@ -0,0 +1,1155 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2008 Benoit Jacob +// Copyright (C) 2008-2010 Gael Guennebaud +// Copyright (C) 2011 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_PRODUCTEVALUATORS_H +#define EIGEN_PRODUCTEVALUATORS_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +/** \internal + * Evaluator of a product expression. + * Since products require special treatments to handle all possible cases, + * we simply defer the evaluation logic to a product_evaluator class + * which offers more partial specialization possibilities. + * + * \sa class product_evaluator + */ +template +struct evaluator> : public product_evaluator> { + typedef Product XprType; + typedef product_evaluator Base; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +// Catch "scalar * ( A * B )" and transform it to "(A*scalar) * B" +// TODO we should apply that rule only if that's really helpful +template +struct evaluator_assume_aliasing, + const CwiseNullaryOp, Plain1>, + const Product>> { + static const bool value = true; +}; +template +struct evaluator, + const CwiseNullaryOp, Plain1>, + const Product>> + : public evaluator> { + typedef CwiseBinaryOp, + const CwiseNullaryOp, Plain1>, + const Product> + XprType; + typedef evaluator> Base; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr) + : Base(xpr.lhs().functor().m_other * xpr.rhs().lhs() * xpr.rhs().rhs()) {} +}; + +template +struct evaluator, DiagIndex>> + : public evaluator, DiagIndex>> { + typedef Diagonal, DiagIndex> XprType; + typedef evaluator, DiagIndex>> Base; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr) + : Base(Diagonal, DiagIndex>( + Product(xpr.nestedExpression().lhs(), xpr.nestedExpression().rhs()), xpr.index())) {} +}; + +// Helper class to perform a matrix product with the destination at hand. +// Depending on the sizes of the factors, there are different evaluation strategies +// as controlled by internal::product_type. +template ::Shape, + typename RhsShape = typename evaluator_traits::Shape, + int ProductType = internal::product_type::value> +struct generic_product_impl; + +template +struct evaluator_assume_aliasing> { + static const bool value = true; +}; + +// This is the default evaluator implementation for products: +// It creates a temporary and call generic_product_impl +template +struct product_evaluator, ProductTag, LhsShape, RhsShape> + : public evaluator::PlainObject> { + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + typedef evaluator Base; + enum { Flags = Base::Flags | EvalBeforeNestingBit }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit product_evaluator(const XprType& xpr) + : m_result(xpr.rows(), xpr.cols()) { + internal::construct_at(this, m_result); + + // FIXME shall we handle nested_eval here?, + // if so, then we must take care at removing the call to nested_eval in the specializations (e.g., in + // permutation_matrix_product, transposition_matrix_product, etc.) + // typedef typename internal::nested_eval::type LhsNested; + // typedef typename internal::nested_eval::type RhsNested; + // typedef internal::remove_all_t LhsNestedCleaned; + // typedef internal::remove_all_t RhsNestedCleaned; + // + // const LhsNested lhs(xpr.lhs()); + // const RhsNested rhs(xpr.rhs()); + // + // generic_product_impl::evalTo(m_result, lhs, rhs); + + generic_product_impl::evalTo(m_result, xpr.lhs(), xpr.rhs()); + } + + protected: + PlainObject m_result; +}; + +// The following three shortcuts are enabled only if the scalar types match exactly. +// TODO: we could enable them for different scalar types when the product is not vectorized. + +// Dense = Product +template +struct Assignment, internal::assign_op, Dense2Dense, + std::enable_if_t<(Options == DefaultProduct || Options == AliasFreeProduct)>> { + typedef Product SrcXprType; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 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); + // FIXME shall we handle nested_eval here? + generic_product_impl::evalTo(dst, src.lhs(), src.rhs()); + } +}; + +// Dense += Product +template +struct Assignment, internal::add_assign_op, Dense2Dense, + std::enable_if_t<(Options == DefaultProduct || Options == AliasFreeProduct)>> { + typedef Product SrcXprType; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, + const internal::add_assign_op&) { + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + // FIXME shall we handle nested_eval here? + generic_product_impl::addTo(dst, src.lhs(), src.rhs()); + } +}; + +// Dense -= Product +template +struct Assignment, internal::sub_assign_op, Dense2Dense, + std::enable_if_t<(Options == DefaultProduct || Options == AliasFreeProduct)>> { + typedef Product SrcXprType; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, + const internal::sub_assign_op&) { + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + // FIXME shall we handle nested_eval here? + generic_product_impl::subTo(dst, src.lhs(), src.rhs()); + } +}; + +// Dense ?= scalar * Product +// TODO we should apply that rule if that's really helpful +// for instance, this is not good for inner products +template +struct Assignment, + const CwiseNullaryOp, Plain>, + const Product>, + AssignFunc, Dense2Dense> { + typedef CwiseBinaryOp, + const CwiseNullaryOp, Plain>, + const Product> + SrcXprType; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, + const AssignFunc& func) { + call_assignment_no_alias(dst, (src.lhs().functor().m_other * src.rhs().lhs()) * src.rhs().rhs(), func); + } +}; + +//---------------------------------------- +// Catch "Dense ?= xpr + Product<>" expression to save one temporary +// FIXME we could probably enable these rules for any product, i.e., not only Dense and DefaultProduct + +template +struct evaluator_assume_aliasing< + CwiseBinaryOp< + internal::scalar_sum_op::Scalar>, + const OtherXpr, const Product>, + DenseShape> { + static const bool value = true; +}; + +template +struct evaluator_assume_aliasing< + CwiseBinaryOp< + internal::scalar_difference_op::Scalar>, + const OtherXpr, const Product>, + DenseShape> { + static const bool value = true; +}; + +template +struct assignment_from_xpr_op_product { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, + const InitialFunc& /*func*/) { + call_assignment_no_alias(dst, src.lhs(), Func1()); + call_assignment_no_alias(dst, src.rhs(), Func2()); + } +}; + +#define EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(ASSIGN_OP, BINOP, ASSIGN_OP2) \ + template \ + struct Assignment, const OtherXpr, \ + const Product>, \ + internal::ASSIGN_OP, Dense2Dense> \ + : assignment_from_xpr_op_product, \ + internal::ASSIGN_OP, \ + internal::ASSIGN_OP2> {} + +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op, scalar_sum_op, add_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op, scalar_sum_op, add_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op, scalar_sum_op, sub_assign_op); + +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op, scalar_difference_op, sub_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op, scalar_difference_op, sub_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op, scalar_difference_op, add_assign_op); + +//---------------------------------------- + +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + dst.coeffRef(0, 0) = (lhs.transpose().cwiseProduct(rhs)).sum(); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + dst.coeffRef(0, 0) += (lhs.transpose().cwiseProduct(rhs)).sum(); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + dst.coeffRef(0, 0) -= (lhs.transpose().cwiseProduct(rhs)).sum(); + } +}; + +/*********************************************************************** + * Implementation of outer dense * dense vector product + ***********************************************************************/ + +// Column major result +template +void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Func& func, + const false_type&) { + evaluator rhsEval(rhs); + ei_declare_local_nested_eval(Lhs, lhs, Rhs::SizeAtCompileTime, actual_lhs); + // FIXME if cols is large enough, then it might be useful to make sure that lhs is sequentially stored + // FIXME not very good if rhs is real and lhs complex while alpha is real too + const Index cols = dst.cols(); + for (Index j = 0; j < cols; ++j) func(dst.col(j), rhsEval.coeff(Index(0), j) * actual_lhs); +} + +// Row major result +template +void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Func& func, + const true_type&) { + evaluator lhsEval(lhs); + ei_declare_local_nested_eval(Rhs, rhs, Lhs::SizeAtCompileTime, actual_rhs); + // FIXME if rows is large enough, then it might be useful to make sure that rhs is sequentially stored + // FIXME not very good if lhs is real and rhs complex while alpha is real too + const Index rows = dst.rows(); + for (Index i = 0; i < rows; ++i) func(dst.row(i), lhsEval.coeff(i, Index(0)) * actual_rhs); +} + +template +struct generic_product_impl { + template + struct is_row_major : std::conditional_t<(int(T::Flags) & RowMajorBit), internal::true_type, internal::false_type> {}; + typedef typename Product::Scalar Scalar; + + // TODO it would be nice to be able to exploit our *_assign_op functors for that purpose + struct set { + template + EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { + dst.const_cast_derived() = src; + } + }; + struct add { + template + EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { + dst.const_cast_derived() += src; + } + }; + struct sub { + template + EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { + dst.const_cast_derived() -= src; + } + }; + struct adds { + Scalar m_scale; + explicit adds(const Scalar& s) : m_scale(s) {} + template + void EIGEN_DEVICE_FUNC operator()(const Dst& dst, const Src& src) const { + dst.const_cast_derived() += m_scale * src; + } + }; + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + internal::outer_product_selector_run(dst, lhs, rhs, set(), is_row_major()); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + internal::outer_product_selector_run(dst, lhs, rhs, add(), is_row_major()); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + internal::outer_product_selector_run(dst, lhs, rhs, sub(), is_row_major()); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, + const Scalar& alpha) { + internal::outer_product_selector_run(dst, lhs, rhs, adds(alpha), is_row_major()); + } +}; + +// This base class provides default implementations for evalTo, addTo, subTo, in terms of scaleAndAddTo +template +struct generic_product_impl_base { + typedef typename Product::Scalar Scalar; + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + dst.setZero(); + scaleAndAddTo(dst, lhs, rhs, Scalar(1)); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + scaleAndAddTo(dst, lhs, rhs, Scalar(1)); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + scaleAndAddTo(dst, lhs, rhs, Scalar(-1)); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, + const Scalar& alpha) { + Derived::scaleAndAddTo(dst, lhs, rhs, alpha); + } +}; + +template +struct generic_product_impl + : generic_product_impl_base> { + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + typedef typename Product::Scalar Scalar; + enum { Side = Lhs::IsVectorAtCompileTime ? OnTheLeft : OnTheRight }; + typedef internal::remove_all_t> MatrixType; + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, + const Scalar& alpha) { + // Fallback to inner product if both the lhs and rhs is a runtime vector. + if (lhs.rows() == 1 && rhs.cols() == 1) { + dst.coeffRef(0, 0) += alpha * lhs.row(0).conjugate().dot(rhs.col(0)); + return; + } + LhsNested actual_lhs(lhs); + RhsNested actual_rhs(rhs); + internal::gemv_dense_selector::HasUsableDirectAccess)>::run(actual_lhs, + actual_rhs, dst, + alpha); + } +}; + +template +struct generic_product_impl { + typedef typename Product::Scalar Scalar; + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + // Same as: dst.noalias() = lhs.lazyProduct(rhs); + // but easier on the compiler side + call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::assign_op()); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + // dst.noalias() += lhs.lazyProduct(rhs); + call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::add_assign_op()); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { + // dst.noalias() -= lhs.lazyProduct(rhs); + call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::sub_assign_op()); + } + + // This is a special evaluation path called from generic_product_impl<...,GemmProduct> in file GeneralMatrixMatrix.h + // This variant tries to extract scalar multiples from both the LHS and RHS and factor them out. For instance: + // dst {,+,-}= (s1*A)*(B*s2) + // will be rewritten as: + // dst {,+,-}= (s1*s2) * (A.lazyProduct(B)) + // There are at least four benefits of doing so: + // 1 - huge performance gain for heap-allocated matrix types as it save costly allocations. + // 2 - it is faster than simply by-passing the heap allocation through stack allocation. + // 3 - it makes this fallback consistent with the heavy GEMM routine. + // 4 - it fully by-passes huge stack allocation attempts when multiplying huge fixed-size matrices. + // (see https://stackoverflow.com/questions/54738495) + // For small fixed sizes matrices, however, the gains are less obvious, it is sometimes x2 faster, but sometimes x3 + // slower, and the behavior depends also a lot on the compiler... This is why this re-writing strategy is currently + // enabled only when falling back from the main GEMM. + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void eval_dynamic(Dst& dst, const Lhs& lhs, const Rhs& rhs, + const Func& func) { + enum { + HasScalarFactor = blas_traits::HasScalarFactor || blas_traits::HasScalarFactor, + ConjLhs = blas_traits::NeedToConjugate, + ConjRhs = blas_traits::NeedToConjugate + }; + // FIXME: in c++11 this should be auto, and extractScalarFactor should also return auto + // this is important for real*complex_mat + Scalar actualAlpha = combine_scalar_factors(lhs, rhs); + + eval_dynamic_impl(dst, blas_traits::extract(lhs).template conjugateIf(), + blas_traits::extract(rhs).template conjugateIf(), func, actualAlpha, + std::conditional_t()); + } + + protected: + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs, + const Func& func, const Scalar& s /* == 1 */, + false_type) { + EIGEN_UNUSED_VARIABLE(s); + eigen_internal_assert(numext::is_exactly_one(s)); + call_restricted_packet_assignment_no_alias(dst, lhs.lazyProduct(rhs), func); + } + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs, + const Func& func, const Scalar& s, true_type) { + call_restricted_packet_assignment_no_alias(dst, s * lhs.lazyProduct(rhs), func); + } +}; + +// This specialization enforces the use of a coefficient-based evaluation strategy +template +struct generic_product_impl + : generic_product_impl {}; + +// Case 2: Evaluate coeff by coeff +// +// This is mostly taken from CoeffBasedProduct.h +// The main difference is that we add an extra argument to the etor_product_*_impl::run() function +// for the inner dimension of the product, because evaluator object do not know their size. + +template +struct etor_product_coeff_impl; + +template +struct etor_product_packet_impl; + +template +struct product_evaluator, ProductTag, DenseShape, DenseShape> + : evaluator_base> { + typedef Product XprType; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit product_evaluator(const XprType& xpr) + : m_lhs(xpr.lhs()), + m_rhs(xpr.rhs()), + m_lhsImpl(m_lhs), // FIXME the creation of the evaluator objects should result in a no-op, but check that! + m_rhsImpl(m_rhs), // Moreover, they are only useful for the packet path, so we could completely disable + // them when not needed, or perhaps declare them on the fly on the packet method... We + // have experiment to check what's best. + m_innerDim(xpr.lhs().cols()) { + EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits::MulCost); + EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits::AddCost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); +#if 0 + std::cerr << "LhsOuterStrideBytes= " << LhsOuterStrideBytes << "\n"; + std::cerr << "RhsOuterStrideBytes= " << RhsOuterStrideBytes << "\n"; + std::cerr << "LhsAlignment= " << LhsAlignment << "\n"; + std::cerr << "RhsAlignment= " << RhsAlignment << "\n"; + std::cerr << "CanVectorizeLhs= " << CanVectorizeLhs << "\n"; + std::cerr << "CanVectorizeRhs= " << CanVectorizeRhs << "\n"; + std::cerr << "CanVectorizeInner= " << CanVectorizeInner << "\n"; + std::cerr << "EvalToRowMajor= " << EvalToRowMajor << "\n"; + std::cerr << "Alignment= " << Alignment << "\n"; + std::cerr << "Flags= " << Flags << "\n"; +#endif + } + + // Everything below here is taken from CoeffBasedProduct.h + + typedef typename internal::nested_eval::type LhsNested; + typedef typename internal::nested_eval::type RhsNested; + + typedef internal::remove_all_t LhsNestedCleaned; + typedef internal::remove_all_t RhsNestedCleaned; + + typedef evaluator LhsEtorType; + typedef evaluator RhsEtorType; + + enum { + RowsAtCompileTime = LhsNestedCleaned::RowsAtCompileTime, + ColsAtCompileTime = RhsNestedCleaned::ColsAtCompileTime, + InnerSize = min_size_prefer_fixed(LhsNestedCleaned::ColsAtCompileTime, RhsNestedCleaned::RowsAtCompileTime), + MaxRowsAtCompileTime = LhsNestedCleaned::MaxRowsAtCompileTime, + MaxColsAtCompileTime = RhsNestedCleaned::MaxColsAtCompileTime + }; + + typedef typename find_best_packet::type LhsVecPacketType; + typedef typename find_best_packet::type RhsVecPacketType; + + enum { + + LhsCoeffReadCost = LhsEtorType::CoeffReadCost, + RhsCoeffReadCost = RhsEtorType::CoeffReadCost, + CoeffReadCost = InnerSize == 0 ? NumTraits::ReadCost + : InnerSize == Dynamic + ? HugeCost + : InnerSize * (NumTraits::MulCost + int(LhsCoeffReadCost) + int(RhsCoeffReadCost)) + + (InnerSize - 1) * NumTraits::AddCost, + + Unroll = CoeffReadCost <= EIGEN_UNROLLING_LIMIT, + + LhsFlags = LhsEtorType::Flags, + RhsFlags = RhsEtorType::Flags, + + LhsRowMajor = LhsFlags & RowMajorBit, + RhsRowMajor = RhsFlags & RowMajorBit, + + LhsVecPacketSize = unpacket_traits::size, + RhsVecPacketSize = unpacket_traits::size, + + // Here, we don't care about alignment larger than the usable packet size. + LhsAlignment = + plain_enum_min(LhsEtorType::Alignment, LhsVecPacketSize* int(sizeof(typename LhsNestedCleaned::Scalar))), + RhsAlignment = + plain_enum_min(RhsEtorType::Alignment, RhsVecPacketSize* int(sizeof(typename RhsNestedCleaned::Scalar))), + + SameType = is_same::value, + + CanVectorizeRhs = bool(RhsRowMajor) && (RhsFlags & PacketAccessBit) && (ColsAtCompileTime != 1), + CanVectorizeLhs = (!LhsRowMajor) && (LhsFlags & PacketAccessBit) && (RowsAtCompileTime != 1), + + EvalToRowMajor = (MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1 + : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) + ? 0 + : (bool(RhsRowMajor) && !CanVectorizeLhs), + + Flags = ((int(LhsFlags) | int(RhsFlags)) & HereditaryBits & ~RowMajorBit) | + (EvalToRowMajor ? RowMajorBit : 0) + // TODO enable vectorization for mixed types + | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0) | + (XprType::IsVectorAtCompileTime ? LinearAccessBit : 0), + + LhsOuterStrideBytes = + int(LhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename LhsNestedCleaned::Scalar)), + RhsOuterStrideBytes = + int(RhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename RhsNestedCleaned::Scalar)), + + Alignment = bool(CanVectorizeLhs) + ? (LhsOuterStrideBytes <= 0 || (int(LhsOuterStrideBytes) % plain_enum_max(1, LhsAlignment)) != 0 + ? 0 + : LhsAlignment) + : bool(CanVectorizeRhs) + ? (RhsOuterStrideBytes <= 0 || (int(RhsOuterStrideBytes) % plain_enum_max(1, RhsAlignment)) != 0 + ? 0 + : RhsAlignment) + : 0, + + /* CanVectorizeInner deserves special explanation. It does not affect the product flags. It is not used outside + * of Product. If the Product itself is not a packet-access expression, there is still a chance that the inner + * loop of the product might be vectorized. This is the meaning of CanVectorizeInner. Since it doesn't affect + * the Flags, it is safe to make this value depend on ActualPacketAccessBit, that doesn't affect the ABI. + */ + CanVectorizeInner = SameType && LhsRowMajor && (!RhsRowMajor) && + (int(LhsFlags) & int(RhsFlags) & ActualPacketAccessBit) && + (int(InnerSize) % packet_traits::size == 0) + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index row, Index col) const { + return (m_lhs.row(row).transpose().cwiseProduct(m_rhs.col(col))).sum(); + } + + /* Allow index-based non-packet access. It is impossible though to allow index-based packed access, + * which is why we don't set the LinearAccessBit. + * TODO: this seems possible when the result is a vector + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index index) const { + const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? 0 : index; + const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? index : 0; + return (m_lhs.row(row).transpose().cwiseProduct(m_rhs.col(col))).sum(); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packet(Index row, Index col) const { + PacketType res; + typedef etor_product_packet_impl + PacketImpl; + PacketImpl::run(row, col, m_lhsImpl, m_rhsImpl, m_innerDim, res); + return res; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packet(Index index) const { + const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? 0 : index; + const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? index : 0; + return packet(row, col); + } + + protected: + add_const_on_value_type_t m_lhs; + add_const_on_value_type_t m_rhs; + + LhsEtorType m_lhsImpl; + RhsEtorType m_rhsImpl; + + // TODO: Get rid of m_innerDim if known at compile time + Index m_innerDim; +}; + +template +struct product_evaluator, LazyCoeffBasedProductMode, DenseShape, DenseShape> + : product_evaluator, CoeffBasedProductMode, DenseShape, DenseShape> { + typedef Product XprType; + typedef Product BaseProduct; + typedef product_evaluator Base; + enum { Flags = Base::Flags | EvalBeforeNestingBit }; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit product_evaluator(const XprType& xpr) + : Base(BaseProduct(xpr.lhs(), xpr.rhs())) {} +}; + +/**************************************** +*** Coeff based product, Packet path *** +****************************************/ + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res) { + etor_product_packet_impl::run(row, col, lhs, rhs, + innerDim, res); + res = pmadd(pset1(lhs.coeff(row, Index(UnrollingIndex - 1))), + rhs.template packet(Index(UnrollingIndex - 1), col), res); + } +}; + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res) { + etor_product_packet_impl::run(row, col, lhs, rhs, + innerDim, res); + res = pmadd(lhs.template packet(row, Index(UnrollingIndex - 1)), + pset1(rhs.coeff(Index(UnrollingIndex - 1), col)), res); + } +}; + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index /*innerDim*/, Packet& res) { + res = pmul(pset1(lhs.coeff(row, Index(0))), rhs.template packet(Index(0), col)); + } +}; + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index /*innerDim*/, Packet& res) { + res = pmul(lhs.template packet(row, Index(0)), pset1(rhs.coeff(Index(0), col))); + } +}; + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, + const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res) { + res = pset1(typename unpacket_traits::type(0)); + } +}; + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, + const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res) { + res = pset1(typename unpacket_traits::type(0)); + } +}; + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res) { + res = pset1(typename unpacket_traits::type(0)); + for (Index i = 0; i < innerDim; ++i) + res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packet(i, col), res); + } +}; + +template +struct etor_product_packet_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res) { + res = pset1(typename unpacket_traits::type(0)); + for (Index i = 0; i < innerDim; ++i) + res = pmadd(lhs.template packet(row, i), pset1(rhs.coeff(i, col)), res); + } +}; + +/*************************************************************************** + * Triangular products + ***************************************************************************/ +template +struct triangular_product_impl; + +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) { + triangular_product_impl::run( + dst, lhs.nestedExpression(), rhs, alpha); + } +}; + +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) { + triangular_product_impl::run( + dst, lhs, rhs.nestedExpression(), alpha); + } +}; + +/*************************************************************************** + * SelfAdjoint products + ***************************************************************************/ +template +struct selfadjoint_product_impl; + +template +struct generic_product_impl + : generic_product_impl_base> { + typedef typename Product::Scalar Scalar; + + template + static EIGEN_DEVICE_FUNC void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) { + selfadjoint_product_impl::run( + dst, lhs.nestedExpression(), rhs, alpha); + } +}; + +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) { + selfadjoint_product_impl::run( + dst, lhs, rhs.nestedExpression(), alpha); + } +}; + +/*************************************************************************** + * Diagonal products + ***************************************************************************/ + +template +struct diagonal_product_evaluator_base : evaluator_base { + typedef typename ScalarBinaryOpTraits::ReturnType Scalar; + + public: + enum { + CoeffReadCost = int(NumTraits::MulCost) + int(evaluator::CoeffReadCost) + + int(evaluator::CoeffReadCost), + + MatrixFlags = evaluator::Flags, + DiagFlags = evaluator::Flags, + + StorageOrder_ = (Derived::MaxRowsAtCompileTime == 1 && Derived::MaxColsAtCompileTime != 1) ? RowMajor + : (Derived::MaxColsAtCompileTime == 1 && Derived::MaxRowsAtCompileTime != 1) ? ColMajor + : MatrixFlags & RowMajorBit ? RowMajor + : ColMajor, + SameStorageOrder_ = StorageOrder_ == (MatrixFlags & RowMajorBit ? RowMajor : ColMajor), + + ScalarAccessOnDiag_ = !((int(StorageOrder_) == ColMajor && int(ProductOrder) == OnTheLeft) || + (int(StorageOrder_) == RowMajor && int(ProductOrder) == OnTheRight)), + SameTypes_ = is_same::value, + // FIXME currently we need same types, but in the future the next rule should be the one + // Vectorizable_ = bool(int(MatrixFlags)&PacketAccessBit) && ((!_PacketOnDiag) || (SameTypes_ && + // bool(int(DiagFlags)&PacketAccessBit))), + Vectorizable_ = bool(int(MatrixFlags) & PacketAccessBit) && SameTypes_ && + (SameStorageOrder_ || (MatrixFlags & LinearAccessBit) == LinearAccessBit) && + (ScalarAccessOnDiag_ || (bool(int(DiagFlags) & PacketAccessBit))), + LinearAccessMask_ = + (MatrixType::RowsAtCompileTime == 1 || MatrixType::ColsAtCompileTime == 1) ? LinearAccessBit : 0, + Flags = + ((HereditaryBits | LinearAccessMask_) & (unsigned int)(MatrixFlags)) | (Vectorizable_ ? PacketAccessBit : 0), + Alignment = evaluator::Alignment, + + AsScalarProduct = + (DiagonalType::SizeAtCompileTime == 1) || + (DiagonalType::SizeAtCompileTime == Dynamic && MatrixType::RowsAtCompileTime == 1 && + ProductOrder == OnTheLeft) || + (DiagonalType::SizeAtCompileTime == Dynamic && MatrixType::ColsAtCompileTime == 1 && ProductOrder == OnTheRight) + }; + + EIGEN_DEVICE_FUNC diagonal_product_evaluator_base(const MatrixType& mat, const DiagonalType& diag) + : m_diagImpl(diag), m_matImpl(mat) { + EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits::MulCost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const { + if (AsScalarProduct) + return m_diagImpl.coeff(0) * m_matImpl.coeff(idx); + else + return m_diagImpl.coeff(idx) * m_matImpl.coeff(idx); + } + + protected: + template + EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::true_type) const { + return internal::pmul(m_matImpl.template packet(row, col), + internal::pset1(m_diagImpl.coeff(id))); + } + + template + EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::false_type) const { + enum { + InnerSize = (MatrixType::Flags & RowMajorBit) ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime, + DiagonalPacketLoadMode = plain_enum_min( + LoadMode, + ((InnerSize % 16) == 0) ? int(Aligned16) : int(evaluator::Alignment)) // FIXME hardcoded 16!! + }; + return internal::pmul(m_matImpl.template packet(row, col), + m_diagImpl.template packet(id)); + } + + evaluator m_diagImpl; + evaluator m_matImpl; +}; + +// diagonal * dense +template +struct product_evaluator, ProductTag, DiagonalShape, DenseShape> + : diagonal_product_evaluator_base, + OnTheLeft> { + typedef diagonal_product_evaluator_base, + OnTheLeft> + Base; + using Base::coeff; + using Base::m_diagImpl; + using Base::m_matImpl; + typedef typename Base::Scalar Scalar; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + typedef typename Lhs::DiagonalVectorType DiagonalType; + + enum { StorageOrder = Base::StorageOrder_ }; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const { + return m_diagImpl.coeff(row) * m_matImpl.coeff(row, col); + } + +#ifndef EIGEN_GPUCC + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + // FIXME: NVCC used to complain about the template keyword, but we have to check whether this is still the case. + // See also similar calls below. + return this->template packet_impl( + row, col, row, std::conditional_t()); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index idx) const { + return packet(int(StorageOrder) == ColMajor ? idx : 0, + int(StorageOrder) == ColMajor ? 0 : idx); + } +#endif +}; + +// dense * diagonal +template +struct product_evaluator, ProductTag, DenseShape, DiagonalShape> + : diagonal_product_evaluator_base, + OnTheRight> { + typedef diagonal_product_evaluator_base, + OnTheRight> + Base; + using Base::coeff; + using Base::m_diagImpl; + using Base::m_matImpl; + typedef typename Base::Scalar Scalar; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + + enum { StorageOrder = Base::StorageOrder_ }; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs().diagonal()) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const { + return m_matImpl.coeff(row, col) * m_diagImpl.coeff(col); + } + +#ifndef EIGEN_GPUCC + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { + return this->template packet_impl( + row, col, col, std::conditional_t()); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index idx) const { + return packet(int(StorageOrder) == ColMajor ? idx : 0, + int(StorageOrder) == ColMajor ? 0 : idx); + } +#endif +}; + +/*************************************************************************** + * Products with permutation matrices + ***************************************************************************/ + +/** \internal + * \class permutation_matrix_product + * Internal helper class implementing the product between a permutation matrix and a matrix. + * This class is specialized for DenseShape below and for SparseShape in SparseCore/SparsePermutation.h + */ +template +struct permutation_matrix_product; + +template +struct permutation_matrix_product { + typedef typename nested_eval::type MatrixType; + typedef remove_all_t MatrixTypeCleaned; + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const PermutationType& perm, + const ExpressionType& xpr) { + MatrixType mat(xpr); + const Index n = Side == OnTheLeft ? mat.rows() : mat.cols(); + // FIXME we need an is_same for expression that is not sensitive to constness. For instance + // is_same_xpr, Block >::value should be true. + // if(is_same::value && extract_data(dst) == extract_data(mat)) + if (is_same_dense(dst, mat)) { + // apply the permutation inplace + Matrix mask(perm.size()); + mask.fill(false); + Index r = 0; + while (r < perm.size()) { + // search for the next seed + while (r < perm.size() && mask[r]) r++; + if (r >= perm.size()) break; + // we got one, let's follow it until we are back to the seed + Index k0 = r++; + Index kPrev = k0; + mask.coeffRef(k0) = true; + for (Index k = perm.indices().coeff(k0); k != k0; k = perm.indices().coeff(k)) { + Block(dst, k) + .swap(Block < Dest, Side == OnTheLeft ? 1 : Dest::RowsAtCompileTime, + Side == OnTheRight + ? 1 + : Dest::ColsAtCompileTime > (dst, ((Side == OnTheLeft) ^ Transposed) ? k0 : kPrev)); + + mask.coeffRef(k) = true; + kPrev = k; + } + } + } else { + for (Index i = 0; i < n; ++i) { + Block( + dst, ((Side == OnTheLeft) ^ Transposed) ? perm.indices().coeff(i) : i) + + = + + Block < const MatrixTypeCleaned, + Side == OnTheLeft ? 1 : MatrixTypeCleaned::RowsAtCompileTime, + Side == OnTheRight ? 1 + : MatrixTypeCleaned::ColsAtCompileTime > + (mat, ((Side == OnTheRight) ^ Transposed) ? perm.indices().coeff(i) : i); + } + } + } +}; + +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) { + permutation_matrix_product::run(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) { + permutation_matrix_product::run(dst, rhs, lhs); + } +}; + +template +struct generic_product_impl, Rhs, PermutationShape, MatrixShape, ProductTag> { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Inverse& lhs, const Rhs& rhs) { + permutation_matrix_product::run(dst, lhs.nestedExpression(), rhs); + } +}; + +template +struct generic_product_impl, MatrixShape, PermutationShape, ProductTag> { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Inverse& rhs) { + permutation_matrix_product::run(dst, rhs.nestedExpression(), lhs); + } +}; + +/*************************************************************************** + * Products with transpositions matrices + ***************************************************************************/ + +// FIXME could we unify Transpositions and Permutation into a single "shape"?? + +/** \internal + * \class transposition_matrix_product + * Internal helper class implementing the product between a permutation matrix and a matrix. + */ +template +struct transposition_matrix_product { + typedef typename nested_eval::type MatrixType; + typedef remove_all_t MatrixTypeCleaned; + + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const TranspositionType& tr, + const ExpressionType& xpr) { + MatrixType mat(xpr); + typedef typename TranspositionType::StorageIndex StorageIndex; + const Index size = tr.size(); + StorageIndex j = 0; + + if (!is_same_dense(dst, mat)) dst = mat; + + for (Index k = (Transposed ? size - 1 : 0); Transposed ? k >= 0 : k < size; Transposed ? --k : ++k) + if (Index(j = tr.coeff(k)) != k) { + if (Side == OnTheLeft) + dst.row(k).swap(dst.row(j)); + else if (Side == OnTheRight) + dst.col(k).swap(dst.col(j)); + } + } +}; + +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) { + transposition_matrix_product::run(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) { + transposition_matrix_product::run(dst, rhs, lhs); + } +}; + +template +struct generic_product_impl, Rhs, TranspositionsShape, MatrixShape, ProductTag> { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Transpose& lhs, const Rhs& rhs) { + transposition_matrix_product::run(dst, lhs.nestedExpression(), rhs); + } +}; + +template +struct generic_product_impl, MatrixShape, TranspositionsShape, ProductTag> { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Transpose& rhs) { + transposition_matrix_product::run(dst, rhs.nestedExpression(), lhs); + } +}; + +/*************************************************************************** + * skew symmetric products + * for now we just call the generic implementation + ***************************************************************************/ +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) { + generic_product_impl::evalTo(dst, lhs, + rhs); + } +}; + +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) { + generic_product_impl::evalTo(dst, lhs, + rhs); + } +}; + +template +struct generic_product_impl { + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) { + generic_product_impl::evalTo(dst, lhs, rhs); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_PRODUCT_EVALUATORS_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Random.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Random.h new file mode 100644 index 00000000..f8a54356 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Random.h @@ -0,0 +1,207 @@ +// 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_RANDOM_H +#define EIGEN_RANDOM_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct scalar_random_op { + inline const Scalar operator()() const { return random(); } +}; + +template +struct functor_traits > { + enum { Cost = 5 * NumTraits::MulCost, PacketAccess = false, IsRepeatable = false }; +}; + +} // end namespace internal + +/** \returns a random matrix expression + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * The parameters \a rows and \a cols are the number of rows and of columns of + * the returned matrix. Must be compatible with this MatrixBase type. + * + * \not_reentrant + * + * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, + * it is redundant to pass \a rows and \a cols as arguments, so Random() should be used + * instead. + * + * + * Example: \include MatrixBase_random_int_int.cpp + * Output: \verbinclude MatrixBase_random_int_int.out + * + * This expression has the "evaluate before nesting" flag so that it will be evaluated into + * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected + * behavior with expressions involving random matrices. + * + * See DenseBase::NullaryExpr(Index, const CustomNullaryOp&) for an example using C++11 random generators. + * + * \sa DenseBase::setRandom(), DenseBase::Random(Index), DenseBase::Random() + */ +template +inline const typename DenseBase::RandomReturnType DenseBase::Random(Index rows, Index cols) { + return NullaryExpr(rows, cols, internal::scalar_random_op()); +} + +/** \returns a random vector expression + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * The parameter \a size is the size of the returned vector. + * Must be compatible with this MatrixBase type. + * + * \only_for_vectors + * \not_reentrant + * + * This variant is meant to be used for dynamic-size vector types. For fixed-size types, + * it is redundant to pass \a size as argument, so Random() should be used + * instead. + * + * Example: \include MatrixBase_random_int.cpp + * Output: \verbinclude MatrixBase_random_int.out + * + * This expression has the "evaluate before nesting" flag so that it will be evaluated into + * a temporary vector whenever it is nested in a larger expression. This prevents unexpected + * behavior with expressions involving random matrices. + * + * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random() + */ +template +inline const typename DenseBase::RandomReturnType DenseBase::Random(Index size) { + return NullaryExpr(size, internal::scalar_random_op()); +} + +/** \returns a fixed-size random matrix or vector expression + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you + * need to use the variants taking size arguments. + * + * Example: \include MatrixBase_random.cpp + * Output: \verbinclude MatrixBase_random.out + * + * This expression has the "evaluate before nesting" flag so that it will be evaluated into + * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected + * behavior with expressions involving random matrices. + * + * \not_reentrant + * + * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random(Index) + */ +template +inline const typename DenseBase::RandomReturnType DenseBase::Random() { + return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_random_op()); +} + +/** Sets all coefficients in this expression to random values. + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * \not_reentrant + * + * Example: \include MatrixBase_setRandom.cpp + * Output: \verbinclude MatrixBase_setRandom.out + * + * \sa class CwiseNullaryOp, setRandom(Index), setRandom(Index,Index) + */ +template +EIGEN_DEVICE_FUNC inline Derived& DenseBase::setRandom() { + return *this = Random(rows(), cols()); +} + +/** Resizes to the given \a newSize, and sets all coefficients in this expression to random values. + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * \only_for_vectors + * \not_reentrant + * + * Example: \include Matrix_setRandom_int.cpp + * Output: \verbinclude Matrix_setRandom_int.out + * + * \sa DenseBase::setRandom(), setRandom(Index,Index), class CwiseNullaryOp, DenseBase::Random() + */ +template +EIGEN_STRONG_INLINE Derived& PlainObjectBase::setRandom(Index newSize) { + resize(newSize); + return setRandom(); +} + +/** Resizes to the given size, and sets all coefficients in this expression to random values. + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * \not_reentrant + * + * \param rows the new number of rows + * \param cols the new number of columns + * + * Example: \include Matrix_setRandom_int_int.cpp + * Output: \verbinclude Matrix_setRandom_int_int.out + * + * \sa DenseBase::setRandom(), setRandom(Index), class CwiseNullaryOp, DenseBase::Random() + */ +template +EIGEN_STRONG_INLINE Derived& PlainObjectBase::setRandom(Index rows, Index cols) { + resize(rows, cols); + return setRandom(); +} + +/** Resizes to the given size, changing only the number of columns, and sets all + * coefficients in this expression to random values. For the parameter of type + * NoChange_t, just pass the special value \c NoChange. + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * \not_reentrant + * + * \sa DenseBase::setRandom(), setRandom(Index), setRandom(Index, NoChange_t), class CwiseNullaryOp, DenseBase::Random() + */ +template +EIGEN_STRONG_INLINE Derived& PlainObjectBase::setRandom(NoChange_t, Index cols) { + return setRandom(rows(), cols); +} + +/** Resizes to the given size, changing only the number of rows, and sets all + * coefficients in this expression to random values. For the parameter of type + * NoChange_t, just pass the special value \c NoChange. + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * \not_reentrant + * + * \sa DenseBase::setRandom(), setRandom(Index), setRandom(NoChange_t, Index), class CwiseNullaryOp, DenseBase::Random() + */ +template +EIGEN_STRONG_INLINE Derived& PlainObjectBase::setRandom(Index rows, NoChange_t) { + return setRandom(rows, cols()); +} + +} // end namespace Eigen + +#endif // EIGEN_RANDOM_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Redux.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Redux.h new file mode 100644 index 00000000..0c5f2d9f --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Redux.h @@ -0,0 +1,528 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// 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_REDUX_H +#define EIGEN_REDUX_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +// TODO +// * implement other kind of vectorization +// * factorize code + +/*************************************************************************** + * Part 1 : the logic deciding a strategy for vectorization and unrolling + ***************************************************************************/ + +template +struct redux_traits { + public: + typedef typename find_best_packet::type PacketType; + enum { + PacketSize = unpacket_traits::size, + InnerMaxSize = int(Evaluator::IsRowMajor) ? Evaluator::MaxColsAtCompileTime : Evaluator::MaxRowsAtCompileTime, + OuterMaxSize = int(Evaluator::IsRowMajor) ? Evaluator::MaxRowsAtCompileTime : Evaluator::MaxColsAtCompileTime, + SliceVectorizedWork = int(InnerMaxSize) == Dynamic ? Dynamic + : int(OuterMaxSize) == Dynamic ? (int(InnerMaxSize) >= int(PacketSize) ? Dynamic : 0) + : (int(InnerMaxSize) / int(PacketSize)) * int(OuterMaxSize) + }; + + enum { + MayLinearize = (int(Evaluator::Flags) & LinearAccessBit), + MightVectorize = (int(Evaluator::Flags) & ActualPacketAccessBit) && (functor_traits::PacketAccess), + MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize), + MaySliceVectorize = bool(MightVectorize) && (int(SliceVectorizedWork) == Dynamic || int(SliceVectorizedWork) >= 3) + }; + + public: + enum { + Traversal = int(MayLinearVectorize) ? int(LinearVectorizedTraversal) + : int(MaySliceVectorize) ? int(SliceVectorizedTraversal) + : int(MayLinearize) ? int(LinearTraversal) + : int(DefaultTraversal) + }; + + public: + enum { + Cost = Evaluator::SizeAtCompileTime == Dynamic + ? HugeCost + : int(Evaluator::SizeAtCompileTime) * int(Evaluator::CoeffReadCost) + + (Evaluator::SizeAtCompileTime - 1) * functor_traits::Cost, + UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Traversal) == int(DefaultTraversal) ? 1 : int(PacketSize)) + }; + + public: + enum { Unrolling = Cost <= UnrollingLimit ? CompleteUnrolling : NoUnrolling }; + +#ifdef EIGEN_DEBUG_ASSIGN + static void debug() { + std::cerr << "Xpr: " << typeid(typename Evaluator::XprType).name() << std::endl; + std::cerr.setf(std::ios::hex, std::ios::basefield); + EIGEN_DEBUG_VAR(Evaluator::Flags) + std::cerr.unsetf(std::ios::hex); + EIGEN_DEBUG_VAR(InnerMaxSize) + EIGEN_DEBUG_VAR(OuterMaxSize) + EIGEN_DEBUG_VAR(SliceVectorizedWork) + EIGEN_DEBUG_VAR(PacketSize) + EIGEN_DEBUG_VAR(MightVectorize) + EIGEN_DEBUG_VAR(MayLinearVectorize) + EIGEN_DEBUG_VAR(MaySliceVectorize) + std::cerr << "Traversal" + << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl; + EIGEN_DEBUG_VAR(UnrollingLimit) + std::cerr << "Unrolling" + << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl; + std::cerr << std::endl; + } +#endif +}; + +/*************************************************************************** + * Part 2 : unrollers + ***************************************************************************/ + +/*** no vectorization ***/ + +template +struct redux_novec_unroller { + static constexpr Index HalfLength = Length / 2; + + typedef typename Evaluator::Scalar Scalar; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func) { + return func(redux_novec_unroller::run(eval, func), + redux_novec_unroller::run(eval, func)); + } +}; + +template +struct redux_novec_unroller { + static constexpr Index outer = Start / Evaluator::InnerSizeAtCompileTime; + static constexpr Index inner = Start % Evaluator::InnerSizeAtCompileTime; + + typedef typename Evaluator::Scalar Scalar; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func&) { + return eval.coeffByOuterInner(outer, inner); + } +}; + +// This is actually dead code and will never be called. It is required +// to prevent false warnings regarding failed inlining though +// for 0 length run() will never be called at all. +template +struct redux_novec_unroller { + typedef typename Evaluator::Scalar Scalar; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator&, const Func&) { return Scalar(); } +}; + +template +struct redux_novec_linear_unroller { + static constexpr Index HalfLength = Length / 2; + + typedef typename Evaluator::Scalar Scalar; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func) { + return func(redux_novec_linear_unroller::run(eval, func), + redux_novec_linear_unroller::run(eval, func)); + } +}; + +template +struct redux_novec_linear_unroller { + typedef typename Evaluator::Scalar Scalar; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func&) { + return eval.coeff(Start); + } +}; + +// This is actually dead code and will never be called. It is required +// to prevent false warnings regarding failed inlining though +// for 0 length run() will never be called at all. +template +struct redux_novec_linear_unroller { + typedef typename Evaluator::Scalar Scalar; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator&, const Func&) { return Scalar(); } +}; + +/*** vectorization ***/ + +template +struct redux_vec_unroller { + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func& func) { + constexpr Index HalfLength = Length / 2; + + return func.packetOp( + redux_vec_unroller::template run(eval, func), + redux_vec_unroller::template run(eval, + func)); + } +}; + +template +struct redux_vec_unroller { + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func&) { + constexpr Index PacketSize = unpacket_traits::size; + constexpr Index index = Start * PacketSize; + constexpr Index outer = index / int(Evaluator::InnerSizeAtCompileTime); + constexpr Index inner = index % int(Evaluator::InnerSizeAtCompileTime); + constexpr int alignment = Evaluator::Alignment; + + return eval.template packetByOuterInner(outer, inner); + } +}; + +template +struct redux_vec_linear_unroller { + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func& func) { + constexpr Index HalfLength = Length / 2; + + return func.packetOp( + redux_vec_linear_unroller::template run(eval, func), + redux_vec_linear_unroller::template run( + eval, func)); + } +}; + +template +struct redux_vec_linear_unroller { + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE PacketType run(const Evaluator& eval, const Func&) { + constexpr Index PacketSize = unpacket_traits::size; + constexpr Index index = (Start * PacketSize); + constexpr int alignment = Evaluator::Alignment; + return eval.template packet(index); + } +}; + +/*************************************************************************** + * Part 3 : implementation of all cases + ***************************************************************************/ + +template ::Traversal, + int Unrolling = redux_traits::Unrolling> +struct redux_impl; + +template +struct redux_impl { + typedef typename Evaluator::Scalar Scalar; + + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) { + eigen_assert(xpr.rows() > 0 && xpr.cols() > 0 && "you are using an empty matrix"); + Scalar res = eval.coeffByOuterInner(0, 0); + for (Index i = 1; i < xpr.innerSize(); ++i) res = func(res, eval.coeffByOuterInner(0, i)); + for (Index i = 1; i < xpr.outerSize(); ++i) + for (Index j = 0; j < xpr.innerSize(); ++j) res = func(res, eval.coeffByOuterInner(i, j)); + return res; + } +}; + +template +struct redux_impl { + typedef typename Evaluator::Scalar Scalar; + + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) { + eigen_assert(xpr.size() > 0 && "you are using an empty matrix"); + Scalar res = eval.coeff(0); + for (Index k = 1; k < xpr.size(); ++k) res = func(res, eval.coeff(k)); + return res; + } +}; + +template +struct redux_impl + : redux_novec_unroller { + typedef redux_novec_unroller Base; + typedef typename Evaluator::Scalar Scalar; + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, + const XprType& /*xpr*/) { + return Base::run(eval, func); + } +}; + +template +struct redux_impl + : redux_novec_linear_unroller { + typedef redux_novec_linear_unroller Base; + typedef typename Evaluator::Scalar Scalar; + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, + const XprType& /*xpr*/) { + return Base::run(eval, func); + } +}; + +template +struct redux_impl { + typedef typename Evaluator::Scalar Scalar; + typedef typename redux_traits::PacketType PacketScalar; + + template + static Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) { + const Index size = xpr.size(); + + constexpr Index packetSize = redux_traits::PacketSize; + constexpr int packetAlignment = unpacket_traits::alignment; + constexpr int alignment0 = + (bool(Evaluator::Flags & DirectAccessBit) && bool(packet_traits::AlignedOnScalar)) + ? int(packetAlignment) + : int(Unaligned); + constexpr int alignment = plain_enum_max(alignment0, Evaluator::Alignment); + const Index alignedStart = internal::first_default_aligned(xpr); + const Index alignedSize2 = ((size - alignedStart) / (2 * packetSize)) * (2 * packetSize); + const Index alignedSize = ((size - alignedStart) / (packetSize)) * (packetSize); + const Index alignedEnd2 = alignedStart + alignedSize2; + const Index alignedEnd = alignedStart + alignedSize; + Scalar res; + if (alignedSize) { + PacketScalar packet_res0 = eval.template packet(alignedStart); + if (alignedSize > packetSize) // we have at least two packets to partly unroll the loop + { + PacketScalar packet_res1 = eval.template packet(alignedStart + packetSize); + for (Index index = alignedStart + 2 * packetSize; index < alignedEnd2; index += 2 * packetSize) { + packet_res0 = func.packetOp(packet_res0, eval.template packet(index)); + packet_res1 = func.packetOp(packet_res1, eval.template packet(index + packetSize)); + } + + packet_res0 = func.packetOp(packet_res0, packet_res1); + if (alignedEnd > alignedEnd2) + packet_res0 = func.packetOp(packet_res0, eval.template packet(alignedEnd2)); + } + res = func.predux(packet_res0); + + for (Index index = 0; index < alignedStart; ++index) res = func(res, eval.coeff(index)); + + for (Index index = alignedEnd; index < size; ++index) res = func(res, eval.coeff(index)); + } else // too small to vectorize anything. + // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize. + { + res = eval.coeff(0); + for (Index index = 1; index < size; ++index) res = func(res, eval.coeff(index)); + } + + return res; + } +}; + +// NOTE: for SliceVectorizedTraversal we simply bypass unrolling +template +struct redux_impl { + typedef typename Evaluator::Scalar Scalar; + typedef typename redux_traits::PacketType PacketType; + + template + EIGEN_DEVICE_FUNC static Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) { + eigen_assert(xpr.rows() > 0 && xpr.cols() > 0 && "you are using an empty matrix"); + constexpr Index packetSize = redux_traits::PacketSize; + const Index innerSize = xpr.innerSize(); + const Index outerSize = xpr.outerSize(); + const Index packetedInnerSize = ((innerSize) / packetSize) * packetSize; + Scalar res; + if (packetedInnerSize) { + PacketType packet_res = eval.template packet(0, 0); + for (Index j = 0; j < outerSize; ++j) + for (Index i = (j == 0 ? packetSize : 0); i < packetedInnerSize; i += Index(packetSize)) + packet_res = func.packetOp(packet_res, eval.template packetByOuterInner(j, i)); + + res = func.predux(packet_res); + for (Index j = 0; j < outerSize; ++j) + for (Index i = packetedInnerSize; i < innerSize; ++i) res = func(res, eval.coeffByOuterInner(j, i)); + } else // too small to vectorize anything. + // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize. + { + res = redux_impl::run(eval, func, xpr); + } + + return res; + } +}; + +template +struct redux_impl { + typedef typename Evaluator::Scalar Scalar; + + typedef typename redux_traits::PacketType PacketType; + static constexpr Index PacketSize = redux_traits::PacketSize; + static constexpr Index Size = Evaluator::SizeAtCompileTime; + static constexpr Index VectorizedSize = (int(Size) / int(PacketSize)) * int(PacketSize); + + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Evaluator& eval, const Func& func, const XprType& xpr) { + EIGEN_ONLY_USED_FOR_DEBUG(xpr) + eigen_assert(xpr.rows() > 0 && xpr.cols() > 0 && "you are using an empty matrix"); + if (VectorizedSize > 0) { + Scalar res = func.predux( + redux_vec_linear_unroller::template run(eval, func)); + if (VectorizedSize != Size) + res = func( + res, redux_novec_linear_unroller::run(eval, func)); + return res; + } else { + return redux_novec_linear_unroller::run(eval, func); + } + } +}; + +// evaluator adaptor +template +class redux_evaluator : public internal::evaluator { + typedef internal::evaluator Base; + + public: + typedef XprType_ XprType; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit redux_evaluator(const XprType& xpr) : Base(xpr) {} + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename XprType::PacketScalar PacketScalar; + + enum { + MaxRowsAtCompileTime = XprType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = XprType::MaxColsAtCompileTime, + // TODO we should not remove DirectAccessBit and rather find an elegant way to query the alignment offset at runtime + // from the evaluator + Flags = Base::Flags & ~DirectAccessBit, + IsRowMajor = XprType::IsRowMajor, + SizeAtCompileTime = XprType::SizeAtCompileTime, + InnerSizeAtCompileTime = XprType::InnerSizeAtCompileTime + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeffByOuterInner(Index outer, Index inner) const { + return Base::coeff(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetByOuterInner(Index outer, Index inner) const { + return Base::template packet(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); + } +}; + +} // end namespace internal + +/*************************************************************************** + * Part 4 : public API + ***************************************************************************/ + +/** \returns the result of a full redux operation on the whole matrix or vector using \a func + * + * The template parameter \a BinaryOp is the type of the functor \a func which must be + * an associative operator. Both current C++98 and C++11 functor styles are handled. + * + * \warning the matrix must be not empty, otherwise an assertion is triggered. + * + * \sa DenseBase::sum(), DenseBase::minCoeff(), DenseBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise() + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::redux( + const Func& func) const { + eigen_assert(this->rows() > 0 && this->cols() > 0 && "you are using an empty matrix"); + + typedef typename internal::redux_evaluator ThisEvaluator; + ThisEvaluator thisEval(derived()); + + // The initial expression is passed to the reducer as an additional argument instead of + // passing it as a member of redux_evaluator to help + return internal::redux_impl::run(thisEval, func, derived()); +} + +/** \returns the minimum of all coefficients of \c *this. + * In case \c *this contains NaN, NaNPropagation determines the behavior: + * NaNPropagation == PropagateFast : undefined + * NaNPropagation == PropagateNaN : result is NaN + * NaNPropagation == PropagateNumbers : result is minimum of elements that are not NaN + * \warning the matrix must be not empty, otherwise an assertion is triggered. + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::minCoeff() const { + return derived().redux(Eigen::internal::scalar_min_op()); +} + +/** \returns the maximum of all coefficients of \c *this. + * In case \c *this contains NaN, NaNPropagation determines the behavior: + * NaNPropagation == PropagateFast : undefined + * NaNPropagation == PropagateNaN : result is NaN + * NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN + * \warning the matrix must be not empty, otherwise an assertion is triggered. + */ +template +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::maxCoeff() const { + return derived().redux(Eigen::internal::scalar_max_op()); +} + +/** \returns the sum of all coefficients of \c *this + * + * If \c *this is empty, then the value 0 is returned. + * + * \sa trace(), prod(), mean() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::sum() const { + if (SizeAtCompileTime == 0 || (SizeAtCompileTime == Dynamic && size() == 0)) return Scalar(0); + return derived().redux(Eigen::internal::scalar_sum_op()); +} + +/** \returns the mean of all coefficients of *this + * + * \sa trace(), prod(), sum() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::mean() const { +#ifdef __INTEL_COMPILER +#pragma warning push +#pragma warning(disable : 2259) +#endif + return Scalar(derived().redux(Eigen::internal::scalar_sum_op())) / Scalar(this->size()); +#ifdef __INTEL_COMPILER +#pragma warning pop +#endif +} + +/** \returns the product of all coefficients of *this + * + * Example: \include MatrixBase_prod.cpp + * Output: \verbinclude MatrixBase_prod.out + * + * \sa sum(), mean(), trace() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::prod() const { + if (SizeAtCompileTime == 0 || (SizeAtCompileTime == Dynamic && size() == 0)) return Scalar(1); + return derived().redux(Eigen::internal::scalar_product_op()); +} + +/** \returns the trace of \c *this, i.e. the sum of the coefficients on the main diagonal. + * + * \c *this can be any matrix, not necessarily square. + * + * \sa diagonal(), sum() + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits::Scalar MatrixBase::trace() const { + return derived().diagonal().sum(); +} + +} // end namespace Eigen + +#endif // EIGEN_REDUX_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Ref.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Ref.h new file mode 100644 index 00000000..129bc85f --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Ref.h @@ -0,0 +1,383 @@ +// 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_REF_H +#define EIGEN_REF_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct traits > + : public traits > { + typedef PlainObjectType_ PlainObjectType; + typedef StrideType_ StrideType; + enum { + Options = Options_, + Flags = traits >::Flags | NestByRefBit, + Alignment = traits >::Alignment, + InnerStrideAtCompileTime = traits >::InnerStrideAtCompileTime, + OuterStrideAtCompileTime = traits >::OuterStrideAtCompileTime + }; + + template + struct match { + enum { + IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime, + HasDirectAccess = internal::has_direct_access::ret, + StorageOrderMatch = + IsVectorAtCompileTime || ((PlainObjectType::Flags & RowMajorBit) == (Derived::Flags & RowMajorBit)), + InnerStrideMatch = int(InnerStrideAtCompileTime) == int(Dynamic) || + int(InnerStrideAtCompileTime) == int(Derived::InnerStrideAtCompileTime) || + (int(InnerStrideAtCompileTime) == 0 && int(Derived::InnerStrideAtCompileTime) == 1), + OuterStrideMatch = IsVectorAtCompileTime || int(OuterStrideAtCompileTime) == int(Dynamic) || + int(OuterStrideAtCompileTime) == int(Derived::OuterStrideAtCompileTime), + // NOTE, this indirection of evaluator::Alignment is needed + // to workaround a very strange bug in MSVC related to the instantiation + // of has_*ary_operator in evaluator. + // This line is surprisingly very sensitive. For instance, simply adding parenthesis + // as "DerivedAlignment = (int(evaluator::Alignment))," will make MSVC fail... + DerivedAlignment = int(evaluator::Alignment), + AlignmentMatch = (int(traits::Alignment) == int(Unaligned)) || + (DerivedAlignment >= int(Alignment)), // FIXME the first condition is not very clear, it should + // be replaced by the required alignment + ScalarTypeMatch = internal::is_same::value, + MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && + AlignmentMatch && ScalarTypeMatch + }; + typedef std::conditional_t type; + }; +}; + +template +struct traits > : public traits {}; + +} // namespace internal + +template +class RefBase : public MapBase { + typedef typename internal::traits::PlainObjectType PlainObjectType; + typedef typename internal::traits::StrideType StrideType; + + public: + typedef MapBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(RefBase) + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { + return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; + } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { + return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer() + : IsVectorAtCompileTime ? this->size() + : int(Flags) & RowMajorBit ? this->cols() + : this->rows(); + } + + EIGEN_DEVICE_FUNC RefBase() + : Base(0, RowsAtCompileTime == Dynamic ? 0 : RowsAtCompileTime, + ColsAtCompileTime == Dynamic ? 0 : ColsAtCompileTime), + // Stride<> does not allow default ctor for Dynamic strides, so let' initialize it with dummy values: + m_stride(StrideType::OuterStrideAtCompileTime == Dynamic ? 0 : StrideType::OuterStrideAtCompileTime, + StrideType::InnerStrideAtCompileTime == Dynamic ? 0 : StrideType::InnerStrideAtCompileTime) {} + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(RefBase) + + protected: + typedef Stride StrideBase; + + // Resolves inner stride if default 0. + static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveInnerStride(Index inner) { return inner == 0 ? 1 : inner; } + + // Resolves outer stride if default 0. + static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveOuterStride(Index inner, Index outer, Index rows, Index cols, + bool isVectorAtCompileTime, bool isRowMajor) { + return outer == 0 ? isVectorAtCompileTime ? inner * rows * cols : isRowMajor ? inner * cols : inner * rows : outer; + } + + // Returns true if construction is valid, false if there is a stride mismatch, + // and fails if there is a size mismatch. + template + EIGEN_DEVICE_FUNC bool construct(Expression& expr) { + // Check matrix sizes. If this is a compile-time vector, we do allow + // implicitly transposing. + EIGEN_STATIC_ASSERT(EIGEN_PREDICATE_SAME_MATRIX_SIZE(PlainObjectType, Expression) + // If it is a vector, the transpose sizes might match. + || (PlainObjectType::IsVectorAtCompileTime && + ((int(PlainObjectType::RowsAtCompileTime) == Eigen::Dynamic || + int(Expression::ColsAtCompileTime) == Eigen::Dynamic || + int(PlainObjectType::RowsAtCompileTime) == int(Expression::ColsAtCompileTime)) && + (int(PlainObjectType::ColsAtCompileTime) == Eigen::Dynamic || + int(Expression::RowsAtCompileTime) == Eigen::Dynamic || + int(PlainObjectType::ColsAtCompileTime) == int(Expression::RowsAtCompileTime)))), + YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES) + + // Determine runtime rows and columns. + Index rows = expr.rows(); + Index cols = expr.cols(); + if (PlainObjectType::RowsAtCompileTime == 1) { + eigen_assert(expr.rows() == 1 || expr.cols() == 1); + rows = 1; + cols = expr.size(); + } else if (PlainObjectType::ColsAtCompileTime == 1) { + eigen_assert(expr.rows() == 1 || expr.cols() == 1); + rows = expr.size(); + cols = 1; + } + // Verify that the sizes are valid. + eigen_assert((PlainObjectType::RowsAtCompileTime == Dynamic) || (PlainObjectType::RowsAtCompileTime == rows)); + eigen_assert((PlainObjectType::ColsAtCompileTime == Dynamic) || (PlainObjectType::ColsAtCompileTime == cols)); + + // If this is a vector, we might be transposing, which means that stride should swap. + const bool transpose = PlainObjectType::IsVectorAtCompileTime && (rows != expr.rows()); + // If the storage format differs, we also need to swap the stride. + const bool row_major = ((PlainObjectType::Flags)&RowMajorBit) != 0; + const bool expr_row_major = (Expression::Flags & RowMajorBit) != 0; + const bool storage_differs = (row_major != expr_row_major); + + const bool swap_stride = (transpose != storage_differs); + + // Determine expr's actual strides, resolving any defaults if zero. + const Index expr_inner_actual = resolveInnerStride(expr.innerStride()); + const Index expr_outer_actual = resolveOuterStride(expr_inner_actual, expr.outerStride(), expr.rows(), expr.cols(), + Expression::IsVectorAtCompileTime != 0, expr_row_major); + + // If this is a column-major row vector or row-major column vector, the inner-stride + // is arbitrary, so set it to either the compile-time inner stride or 1. + const bool row_vector = (rows == 1); + const bool col_vector = (cols == 1); + const Index inner_stride = + ((!row_major && row_vector) || (row_major && col_vector)) + ? (StrideType::InnerStrideAtCompileTime > 0 ? Index(StrideType::InnerStrideAtCompileTime) : 1) + : swap_stride ? expr_outer_actual + : expr_inner_actual; + + // If this is a column-major column vector or row-major row vector, the outer-stride + // is arbitrary, so set it to either the compile-time outer stride or vector size. + const Index outer_stride = + ((!row_major && col_vector) || (row_major && row_vector)) + ? (StrideType::OuterStrideAtCompileTime > 0 ? Index(StrideType::OuterStrideAtCompileTime) + : rows * cols * inner_stride) + : swap_stride ? expr_inner_actual + : expr_outer_actual; + + // Check if given inner/outer strides are compatible with compile-time strides. + const bool inner_valid = (StrideType::InnerStrideAtCompileTime == Dynamic) || + (resolveInnerStride(Index(StrideType::InnerStrideAtCompileTime)) == inner_stride); + if (!inner_valid) { + return false; + } + + const bool outer_valid = + (StrideType::OuterStrideAtCompileTime == Dynamic) || + (resolveOuterStride(inner_stride, Index(StrideType::OuterStrideAtCompileTime), rows, cols, + PlainObjectType::IsVectorAtCompileTime != 0, row_major) == outer_stride); + if (!outer_valid) { + return false; + } + + internal::construct_at(this, expr.data(), rows, cols); + internal::construct_at(&m_stride, (StrideType::OuterStrideAtCompileTime == 0) ? 0 : outer_stride, + (StrideType::InnerStrideAtCompileTime == 0) ? 0 : inner_stride); + return true; + } + + StrideBase m_stride; +}; + +/** \class Ref + * \ingroup Core_Module + * + * \brief A matrix or vector expression mapping an existing expression + * + * \tparam PlainObjectType the equivalent matrix type of the mapped data + * \tparam Options specifies the pointer alignment in bytes. It can be: \c #Aligned128, , \c #Aligned64, \c #Aligned32, + * \c #Aligned16, \c #Aligned8 or \c #Unaligned. The default is \c #Unaligned. \tparam StrideType optionally specifies + * strides. By default, Ref implies a contiguous storage along the inner dimension (inner stride==1), but accepts a + * variable outer stride (leading dimension). This can be overridden by specifying strides. The type passed here must be + * a specialization of the Stride template, see examples below. + * + * This class provides a way to write non-template functions taking Eigen objects as parameters while limiting the + * number of copies. A Ref<> object can represent either a const expression or a l-value: \code + * // in-out argument: + * void foo1(Ref x); + * + * // read-only const argument: + * void foo2(const Ref& x); + * \endcode + * + * In the in-out case, the input argument must satisfy the constraints of the actual Ref<> type, otherwise a compilation + * issue will be triggered. By default, a Ref can reference any dense vector expression of float having a + * contiguous memory layout. Likewise, a Ref can reference any column-major dense matrix expression of float + * whose column's elements are contiguously stored with the possibility to have a constant space in-between each column, + * i.e. the inner stride must be equal to 1, but the outer stride (or leading dimension) can be greater than the number + * of rows. + * + * In the const case, if the input expression does not match the above requirement, then it is evaluated into a + * temporary before being passed to the function. Here are some examples: \code MatrixXf A; VectorXf a; foo1(a.head()); + * // OK foo1(A.col()); // OK foo1(A.row()); // Compilation error because here innerstride!=1 + * foo2(A.row()); // Compilation error because A.row() is a 1xN object while foo2 is expecting a Nx1 object + * foo2(A.row().transpose()); // The row is copied into a contiguous temporary + * foo2(2*a); // The expression is evaluated into a temporary + * foo2(A.col().segment(2,4)); // No temporary + * \endcode + * + * The range of inputs that can be referenced without temporary can be enlarged using the last two template parameters. + * Here is an example accepting an innerstride!=1: + * \code + * // in-out argument: + * void foo3(Ref > x); + * foo3(A.row()); // OK + * \endcode + * The downside here is that the function foo3 might be significantly slower than foo1 because it won't be able to + * exploit vectorization, and will involve more expensive address computations even if the input is contiguously stored + * in memory. To overcome this issue, one might propose to overload internally calling a template function, e.g.: \code + * // in the .h: + * void foo(const Ref& A); + * void foo(const Ref >& A); + * + * // in the .cpp: + * template void foo_impl(const TypeOfA& A) { + * ... // crazy code goes here + * } + * void foo(const Ref& A) { foo_impl(A); } + * void foo(const Ref >& A) { foo_impl(A); } + * \endcode + * + * See also the following stackoverflow questions for further references: + * - Correct usage of the + * Eigen::Ref<> class + * + * \sa PlainObjectBase::Map(), \ref TopicStorageOrders + */ +template +class Ref : public RefBase > { + private: + typedef internal::traits Traits; + template + EIGEN_DEVICE_FUNC inline Ref( + const PlainObjectBase& expr, + std::enable_if_t::MatchAtCompileTime), Derived>* = 0); + + public: + typedef RefBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Ref) + +#ifndef EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC inline Ref( + PlainObjectBase& expr, + std::enable_if_t::MatchAtCompileTime), Derived>* = 0) { + EIGEN_STATIC_ASSERT(bool(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + // Construction must pass since we will not create temporary storage in the non-const case. + const bool success = Base::construct(expr.derived()); + EIGEN_UNUSED_VARIABLE(success) + eigen_assert(success); + } + template + EIGEN_DEVICE_FUNC inline Ref( + const DenseBase& expr, + std::enable_if_t::MatchAtCompileTime), Derived>* = 0) +#else + /** Implicit constructor from any dense expression */ + template + inline Ref(DenseBase& 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_STATIC_ASSERT(!Derived::IsPlainObjectBase, THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + // Construction must pass since we will not create temporary storage in the non-const case. + const bool success = Base::construct(expr.const_cast_derived()); + EIGEN_UNUSED_VARIABLE(success) + eigen_assert(success); + } + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Ref) +}; + +// this is the const ref version +template +class Ref + : public RefBase > { + typedef internal::traits Traits; + + static constexpr bool may_map_m_object_successfully = + (static_cast(StrideType::InnerStrideAtCompileTime) == 0 || + static_cast(StrideType::InnerStrideAtCompileTime) == 1 || + static_cast(StrideType::InnerStrideAtCompileTime) == Dynamic) && + (TPlainObjectType::IsVectorAtCompileTime || static_cast(StrideType::OuterStrideAtCompileTime) == 0 || + static_cast(StrideType::OuterStrideAtCompileTime) == Dynamic || + static_cast(StrideType::OuterStrideAtCompileTime) == + static_cast(TPlainObjectType::InnerSizeAtCompileTime) || + static_cast(TPlainObjectType::InnerSizeAtCompileTime) == Dynamic); + + public: + typedef RefBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Ref) + + template + EIGEN_DEVICE_FUNC inline Ref(const DenseBase& expr, + std::enable_if_t::ScalarTypeMatch), Derived>* = 0) { + // std::cout << match_helper::HasDirectAccess << "," << match_helper::OuterStrideMatch << "," + // << match_helper::InnerStrideMatch << "\n"; std::cout << int(StrideType::OuterStrideAtCompileTime) + // << " - " << int(Derived::OuterStrideAtCompileTime) << "\n"; std::cout << + // int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n"; + EIGEN_STATIC_ASSERT(Traits::template match::type::value || may_map_m_object_successfully, + STORAGE_LAYOUT_DOES_NOT_MATCH); + construct(expr.derived(), typename Traits::template match::type()); + } + + EIGEN_DEVICE_FUNC inline Ref(const Ref& other) : Base(other) { + // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy + } + + EIGEN_DEVICE_FUNC inline Ref(Ref&& other) { + if (other.data() == other.m_object.data()) { + m_object = std::move(other.m_object); + Base::construct(m_object); + } else + Base::construct(other); + } + + template + EIGEN_DEVICE_FUNC inline Ref(const RefBase& other) { + EIGEN_STATIC_ASSERT(Traits::template match::type::value || may_map_m_object_successfully, + STORAGE_LAYOUT_DOES_NOT_MATCH); + construct(other.derived(), typename Traits::template match::type()); + } + + protected: + template + EIGEN_DEVICE_FUNC void construct(const Expression& expr, internal::true_type) { + // Check if we can use the underlying expr's storage directly, otherwise call the copy version. + if (!Base::construct(expr)) { + construct(expr, internal::false_type()); + } + } + + template + EIGEN_DEVICE_FUNC void construct(const Expression& expr, internal::false_type) { + internal::call_assignment_no_alias(m_object, expr, internal::assign_op()); + const bool success = Base::construct(m_object); + EIGEN_ONLY_USED_FOR_DEBUG(success) + eigen_assert(success); + } + + protected: + TPlainObjectType m_object; +}; + +} // end namespace Eigen + +#endif // EIGEN_REF_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Replicate.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Replicate.h new file mode 100644 index 00000000..c01c6273 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Replicate.h @@ -0,0 +1,133 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-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/. + +#ifndef EIGEN_REPLICATE_H +#define EIGEN_REPLICATE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { +template +struct traits > : traits { + typedef typename MatrixType::Scalar Scalar; + typedef typename traits::StorageKind StorageKind; + typedef typename traits::XprKind XprKind; + typedef typename ref_selector::type MatrixTypeNested; + typedef std::remove_reference_t MatrixTypeNested_; + enum { + RowsAtCompileTime = RowFactor == Dynamic || int(MatrixType::RowsAtCompileTime) == Dynamic + ? Dynamic + : RowFactor * MatrixType::RowsAtCompileTime, + ColsAtCompileTime = ColFactor == Dynamic || int(MatrixType::ColsAtCompileTime) == Dynamic + ? Dynamic + : ColFactor * MatrixType::ColsAtCompileTime, + // FIXME we don't propagate the max sizes !!! + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + IsRowMajor = MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1 ? 1 + : MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1 ? 0 + : (MatrixType::Flags & RowMajorBit) ? 1 + : 0, + + // FIXME enable DirectAccess with negative strides? + Flags = IsRowMajor ? RowMajorBit : 0 + }; +}; +} // namespace internal + +/** + * \class Replicate + * \ingroup Core_Module + * + * \brief Expression of the multiple replication of a matrix or vector + * + * \tparam MatrixType the type of the object we are replicating + * \tparam RowFactor number of repetitions at compile time along the vertical direction, can be Dynamic. + * \tparam ColFactor number of repetitions at compile time along the horizontal direction, can be Dynamic. + * + * This class represents an expression of the multiple replication of a matrix or vector. + * It is the return type of DenseBase::replicate() and most of the time + * this is the only way it is used. + * + * \sa DenseBase::replicate() + */ +template +class Replicate : public internal::dense_xpr_base >::type { + typedef typename internal::traits::MatrixTypeNested MatrixTypeNested; + typedef typename internal::traits::MatrixTypeNested_ MatrixTypeNested_; + + public: + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Replicate) + typedef internal::remove_all_t NestedExpression; + + template + EIGEN_DEVICE_FUNC inline explicit Replicate(const OriginalMatrixType& matrix) + : m_matrix(matrix), m_rowFactor(RowFactor), m_colFactor(ColFactor) { + EIGEN_STATIC_ASSERT((internal::is_same, OriginalMatrixType>::value), + THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE) + eigen_assert(RowFactor != Dynamic && ColFactor != Dynamic); + } + + template + EIGEN_DEVICE_FUNC inline Replicate(const OriginalMatrixType& matrix, Index rowFactor, Index colFactor) + : m_matrix(matrix), + m_rowFactor(rowFactor), + m_colFactor(colFactor){ + EIGEN_STATIC_ASSERT((internal::is_same, OriginalMatrixType>::value), + THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE)} + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const { + return m_matrix.rows() * m_rowFactor.value(); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const { return m_matrix.cols() * m_colFactor.value(); } + + EIGEN_DEVICE_FUNC const MatrixTypeNested_& nestedExpression() const { return m_matrix; } + + protected: + MatrixTypeNested m_matrix; + const internal::variable_if_dynamic m_rowFactor; + const internal::variable_if_dynamic m_colFactor; +}; + +/** + * \return an expression of the replication of \c *this + * + * Example: \include MatrixBase_replicate.cpp + * Output: \verbinclude MatrixBase_replicate.out + * + * \sa VectorwiseOp::replicate(), DenseBase::replicate(Index,Index), class Replicate + */ +template +template +EIGEN_DEVICE_FUNC const Replicate DenseBase::replicate() const { + return Replicate(derived()); +} + +/** + * \return an expression of the replication of each column (or row) of \c *this + * + * Example: \include DirectionWise_replicate_int.cpp + * Output: \verbinclude DirectionWise_replicate_int.out + * + * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate + */ +template +EIGEN_DEVICE_FUNC const typename VectorwiseOp::ReplicateReturnType +VectorwiseOp::replicate(Index factor) const { + return typename VectorwiseOp::ReplicateReturnType( + _expression(), Direction == Vertical ? factor : 1, Direction == Horizontal ? factor : 1); +} + +} // end namespace Eigen + +#endif // EIGEN_REPLICATE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h new file mode 100644 index 00000000..b881dd6c --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h @@ -0,0 +1,398 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2017 Gael Guennebaud +// Copyright (C) 2014 yoco +// +// 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_RESHAPED_H +#define EIGEN_RESHAPED_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class Reshaped + * \ingroup Core_Module + * + * \brief Expression of a fixed-size or dynamic-size reshape + * + * \tparam XprType the type of the expression in which we are taking a reshape + * \tparam Rows the number of rows of the reshape we are taking at compile time (optional) + * \tparam Cols the number of columns of the reshape we are taking at compile time (optional) + * \tparam Order can be ColMajor or RowMajor, default is ColMajor. + * + * This class represents an expression of either a fixed-size or dynamic-size reshape. + * It is the return type of DenseBase::reshaped(NRowsType,NColsType) and + * most of the time this is the only way it is used. + * + * If you want to directly manipulate reshaped expressions, + * for instance if you want to write a function returning such an expression, + * it is advised to use the \em auto keyword for such use cases. + * + * Here is an example illustrating the dynamic case: + * \include class_Reshaped.cpp + * Output: \verbinclude class_Reshaped.out + * + * Here is an example illustrating the fixed-size case: + * \include class_FixedReshaped.cpp + * Output: \verbinclude class_FixedReshaped.out + * + * \sa DenseBase::reshaped(NRowsType,NColsType) + */ + +namespace internal { + +template +struct traits > : traits { + typedef typename traits::Scalar Scalar; + typedef typename traits::StorageKind StorageKind; + typedef typename traits::XprKind XprKind; + enum { + MatrixRows = traits::RowsAtCompileTime, + MatrixCols = traits::ColsAtCompileTime, + RowsAtCompileTime = Rows, + ColsAtCompileTime = Cols, + MaxRowsAtCompileTime = Rows, + MaxColsAtCompileTime = Cols, + XpxStorageOrder = ((int(traits::Flags) & RowMajorBit) == RowMajorBit) ? RowMajor : ColMajor, + ReshapedStorageOrder = (RowsAtCompileTime == 1 && ColsAtCompileTime != 1) ? RowMajor + : (ColsAtCompileTime == 1 && RowsAtCompileTime != 1) ? ColMajor + : XpxStorageOrder, + HasSameStorageOrderAsXprType = (ReshapedStorageOrder == XpxStorageOrder), + InnerSize = (ReshapedStorageOrder == int(RowMajor)) ? int(ColsAtCompileTime) : int(RowsAtCompileTime), + InnerStrideAtCompileTime = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time::ret) : Dynamic, + OuterStrideAtCompileTime = Dynamic, + + HasDirectAccess = internal::has_direct_access::ret && (Order == int(XpxStorageOrder)) && + ((evaluator::Flags & LinearAccessBit) == LinearAccessBit), + + MaskPacketAccessBit = + (InnerSize == Dynamic || (InnerSize % packet_traits::size) == 0) && (InnerStrideAtCompileTime == 1) + ? PacketAccessBit + : 0, + // MaskAlignedBit = ((OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) + // == 0)) ? AlignedBit : 0, + FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0, + FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, + FlagsRowMajorBit = (ReshapedStorageOrder == int(RowMajor)) ? RowMajorBit : 0, + FlagsDirectAccessBit = HasDirectAccess ? DirectAccessBit : 0, + Flags0 = traits::Flags & ((HereditaryBits & ~RowMajorBit) | MaskPacketAccessBit), + + Flags = (Flags0 | FlagsLinearAccessBit | FlagsLvalueBit | FlagsRowMajorBit | FlagsDirectAccessBit) + }; +}; + +template +class ReshapedImpl_dense; + +} // end namespace internal + +template +class ReshapedImpl; + +template +class Reshaped : public ReshapedImpl::StorageKind> { + typedef ReshapedImpl::StorageKind> Impl; + + public: + // typedef typename Impl::Base Base; + typedef Impl Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(Reshaped) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reshaped) + + /** Fixed-size constructor + */ + EIGEN_DEVICE_FUNC inline Reshaped(XprType& xpr) : Impl(xpr) { + EIGEN_STATIC_ASSERT(RowsAtCompileTime != Dynamic && ColsAtCompileTime != Dynamic, + THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE) + eigen_assert(Rows * Cols == xpr.rows() * xpr.cols()); + } + + /** Dynamic-size constructor + */ + EIGEN_DEVICE_FUNC inline Reshaped(XprType& xpr, Index reshapeRows, Index reshapeCols) + : Impl(xpr, reshapeRows, reshapeCols) { + eigen_assert((RowsAtCompileTime == Dynamic || RowsAtCompileTime == reshapeRows) && + (ColsAtCompileTime == Dynamic || ColsAtCompileTime == reshapeCols)); + eigen_assert(reshapeRows * reshapeCols == xpr.rows() * xpr.cols()); + } +}; + +// The generic default implementation for dense reshape simply forward to the internal::ReshapedImpl_dense +// that must be specialized for direct and non-direct access... +template +class ReshapedImpl + : public internal::ReshapedImpl_dense >::HasDirectAccess> { + typedef internal::ReshapedImpl_dense >::HasDirectAccess> + Impl; + + public: + typedef Impl Base; + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl) + EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr) : Impl(xpr) {} + EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr, Index reshapeRows, Index reshapeCols) + : Impl(xpr, reshapeRows, reshapeCols) {} +}; + +namespace internal { + +/** \internal Internal implementation of dense Reshaped in the general case. */ +template +class ReshapedImpl_dense + : public internal::dense_xpr_base >::type { + typedef Reshaped ReshapedType; + + public: + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense) + + typedef typename internal::ref_selector::non_const_type MatrixTypeNested; + typedef internal::remove_all_t NestedExpression; + + class InnerIterator; + + /** Fixed-size constructor + */ + EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr) : m_xpr(xpr), m_rows(Rows), m_cols(Cols) {} + + /** Dynamic-size constructor + */ + EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols) + : m_xpr(xpr), m_rows(nRows), m_cols(nCols) {} + + EIGEN_DEVICE_FUNC Index rows() const { return m_rows; } + EIGEN_DEVICE_FUNC Index cols() const { return m_cols; } + +#ifdef EIGEN_PARSED_BY_DOXYGEN + /** \sa MapBase::data() */ + EIGEN_DEVICE_FUNC inline const Scalar* data() const; + EIGEN_DEVICE_FUNC inline Index innerStride() const; + EIGEN_DEVICE_FUNC inline Index outerStride() const; +#endif + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC const internal::remove_all_t& nestedExpression() const { return m_xpr; } + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC std::remove_reference_t& nestedExpression() { return m_xpr; } + + protected: + MatrixTypeNested m_xpr; + const internal::variable_if_dynamic m_rows; + const internal::variable_if_dynamic m_cols; +}; + +/** \internal Internal implementation of dense Reshaped in the direct access case. */ +template +class ReshapedImpl_dense : public MapBase > { + typedef Reshaped ReshapedType; + typedef typename internal::ref_selector::non_const_type XprTypeNested; + + public: + typedef MapBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense) + + /** Fixed-size constructor + */ + EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr) : Base(xpr.data()), m_xpr(xpr) {} + + /** Dynamic-size constructor + */ + EIGEN_DEVICE_FUNC inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols) + : Base(xpr.data(), nRows, nCols), m_xpr(xpr) {} + + EIGEN_DEVICE_FUNC const internal::remove_all_t& nestedExpression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC XprType& nestedExpression() { return m_xpr; } + + /** \sa MapBase::innerStride() */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { return m_xpr.innerStride(); } + + /** \sa MapBase::outerStride() */ + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { + return (((Flags & RowMajorBit) == RowMajorBit) ? this->cols() : this->rows()) * m_xpr.innerStride(); + } + + protected: + XprTypeNested m_xpr; +}; + +// Evaluators +template +struct reshaped_evaluator; + +template +struct evaluator > + : reshaped_evaluator >::HasDirectAccess> { + typedef Reshaped XprType; + typedef typename XprType::Scalar Scalar; + // TODO: should check for smaller packet types + typedef typename packet_traits::type PacketScalar; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + HasDirectAccess = traits::HasDirectAccess, + + // RowsAtCompileTime = traits::RowsAtCompileTime, + // ColsAtCompileTime = traits::ColsAtCompileTime, + // MaxRowsAtCompileTime = traits::MaxRowsAtCompileTime, + // MaxColsAtCompileTime = traits::MaxColsAtCompileTime, + // + // InnerStrideAtCompileTime = traits::HasSameStorageOrderAsXprType + // ? int(inner_stride_at_compile_time::ret) + // : Dynamic, + // OuterStrideAtCompileTime = Dynamic, + + FlagsLinearAccessBit = + (traits::RowsAtCompileTime == 1 || traits::ColsAtCompileTime == 1 || HasDirectAccess) + ? LinearAccessBit + : 0, + FlagsRowMajorBit = (traits::ReshapedStorageOrder == int(RowMajor)) ? RowMajorBit : 0, + FlagsDirectAccessBit = HasDirectAccess ? DirectAccessBit : 0, + Flags0 = evaluator::Flags & (HereditaryBits & ~RowMajorBit), + Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit | FlagsDirectAccessBit, + + PacketAlignment = unpacket_traits::alignment, + Alignment = evaluator::Alignment + }; + typedef reshaped_evaluator reshaped_evaluator_type; + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : reshaped_evaluator_type(xpr) { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } +}; + +template +struct reshaped_evaluator + : evaluator_base > { + typedef Reshaped XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost /* TODO + cost of index computations */, + + Flags = (evaluator::Flags & (HereditaryBits /*| LinearAccessBit | DirectAccessBit*/)), + + Alignment = 0 + }; + + EIGEN_DEVICE_FUNC explicit reshaped_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr) { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + typedef std::pair RowCol; + + EIGEN_DEVICE_FUNC inline RowCol index_remap(Index rowId, Index colId) const { + if (Order == ColMajor) { + const Index nth_elem_idx = colId * m_xpr.rows() + rowId; + return RowCol(nth_elem_idx % m_xpr.nestedExpression().rows(), nth_elem_idx / m_xpr.nestedExpression().rows()); + } else { + const Index nth_elem_idx = colId + rowId * m_xpr.cols(); + return RowCol(nth_elem_idx / m_xpr.nestedExpression().cols(), nth_elem_idx % m_xpr.nestedExpression().cols()); + } + } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index rowId, Index colId) { + EIGEN_STATIC_ASSERT_LVALUE(XprType) + const RowCol row_col = index_remap(rowId, colId); + return m_argImpl.coeffRef(row_col.first, row_col.second); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { + const RowCol row_col = index_remap(rowId, colId); + return m_argImpl.coeffRef(row_col.first, row_col.second); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const { + const RowCol row_col = index_remap(rowId, colId); + return m_argImpl.coeff(row_col.first, row_col.second); + } + + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) { + EIGEN_STATIC_ASSERT_LVALUE(XprType) + const RowCol row_col = index_remap(Rows == 1 ? 0 : index, Rows == 1 ? index : 0); + return m_argImpl.coeffRef(row_col.first, row_col.second); + } + + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { + const RowCol row_col = index_remap(Rows == 1 ? 0 : index, Rows == 1 ? index : 0); + return m_argImpl.coeffRef(row_col.first, row_col.second); + } + + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const { + const RowCol row_col = index_remap(Rows == 1 ? 0 : index, Rows == 1 ? index : 0); + return m_argImpl.coeff(row_col.first, row_col.second); + } +#if 0 + EIGEN_DEVICE_FUNC + template + inline PacketScalar packet(Index rowId, Index colId) const + { + const RowCol row_col = index_remap(rowId, colId); + return m_argImpl.template packet(row_col.first, row_col.second); + + } + + template + EIGEN_DEVICE_FUNC + inline void writePacket(Index rowId, Index colId, const PacketScalar& val) + { + const RowCol row_col = index_remap(rowId, colId); + m_argImpl.const_cast_derived().template writePacket + (row_col.first, row_col.second, val); + } + + template + EIGEN_DEVICE_FUNC + inline PacketScalar packet(Index index) const + { + const RowCol row_col = index_remap(RowsAtCompileTime == 1 ? 0 : index, + RowsAtCompileTime == 1 ? index : 0); + return m_argImpl.template packet(row_col.first, row_col.second); + } + + template + EIGEN_DEVICE_FUNC + inline void writePacket(Index index, const PacketScalar& val) + { + const RowCol row_col = index_remap(RowsAtCompileTime == 1 ? 0 : index, + RowsAtCompileTime == 1 ? index : 0); + return m_argImpl.template packet(row_col.first, row_col.second, val); + } +#endif + protected: + evaluator m_argImpl; + const XprType& m_xpr; +}; + +template +struct reshaped_evaluator + : mapbase_evaluator, + typename Reshaped::PlainObject> { + typedef Reshaped XprType; + typedef typename XprType::Scalar Scalar; + + EIGEN_DEVICE_FUNC explicit reshaped_evaluator(const XprType& xpr) + : mapbase_evaluator(xpr) { + // TODO: for the 3.4 release, this should be turned to an internal assertion, but let's keep it as is for the beta + // lifetime + eigen_assert(((std::uintptr_t(xpr.data()) % plain_enum_max(1, evaluator::Alignment)) == 0) && + "data is not aligned"); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_RESHAPED_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h new file mode 100644 index 00000000..3b5e470c --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h @@ -0,0 +1,115 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-2010 Gael Guennebaud +// Copyright (C) 2009-2010 Benoit Jacob +// +// 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_RETURNBYVALUE_H +#define EIGEN_RETURNBYVALUE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct traits > : public traits::ReturnType> { + enum { + // We're disabling the DirectAccess because e.g. the constructor of + // the Block-with-DirectAccess expression requires to have a coeffRef method. + // Also, we don't want to have to implement the stride stuff. + Flags = (traits::ReturnType>::Flags | EvalBeforeNestingBit) & ~DirectAccessBit + }; +}; + +/* The ReturnByValue object doesn't even have a coeff() method. + * So the only way that nesting it in an expression can work, is by evaluating it into a plain matrix. + * So internal::nested always gives the plain return matrix type. + * + * FIXME: I don't understand why we need this specialization: isn't this taken care of by the EvalBeforeNestingBit ?? + * Answer: EvalBeforeNestingBit should be deprecated since we have the evaluators + */ +template +struct nested_eval, n, PlainObject> { + typedef typename traits::ReturnType type; +}; + +} // end namespace internal + +/** \class ReturnByValue + * \ingroup Core_Module + * + */ +template +class ReturnByValue : public internal::dense_xpr_base >::type, internal::no_assignment_operator { + public: + typedef typename internal::traits::ReturnType ReturnType; + + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(ReturnByValue) + + template + EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const { + static_cast(this)->evalTo(dst); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { + return static_cast(this)->rows(); + } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { + return static_cast(this)->cols(); + } + +#ifndef EIGEN_PARSED_BY_DOXYGEN +#define Unusable \ + YOU_ARE_TRYING_TO_ACCESS_A_SINGLE_COEFFICIENT_IN_A_SPECIAL_EXPRESSION_WHERE_THAT_IS_NOT_ALLOWED_BECAUSE_THAT_WOULD_BE_INEFFICIENT + class Unusable { + Unusable(const Unusable&) {} + Unusable& operator=(const Unusable&) { return *this; } + }; + const Unusable& coeff(Index) const { return *reinterpret_cast(this); } + const Unusable& coeff(Index, Index) const { return *reinterpret_cast(this); } + Unusable& coeffRef(Index) { return *reinterpret_cast(this); } + Unusable& coeffRef(Index, Index) { return *reinterpret_cast(this); } +#undef Unusable +#endif +}; + +template +template +EIGEN_DEVICE_FUNC Derived& DenseBase::operator=(const ReturnByValue& other) { + other.evalTo(derived()); + return derived(); +} + +namespace internal { + +// Expression is evaluated in a temporary; default implementation of Assignment is bypassed so that +// when a ReturnByValue expression is assigned, the evaluator is not constructed. +// TODO: Finalize port to new regime; ReturnByValue should not exist in the expression world + +template +struct evaluator > : public evaluator::ReturnType> { + typedef ReturnByValue XprType; + typedef typename internal::traits::ReturnType PlainObject; + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : m_result(xpr.rows(), xpr.cols()) { + internal::construct_at(this, m_result); + xpr.evalTo(m_result); + } + + protected: + PlainObject m_result; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_RETURNBYVALUE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Reverse.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Reverse.h new file mode 100644 index 00000000..66116aa4 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Reverse.h @@ -0,0 +1,196 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2008 Benoit Jacob +// Copyright (C) 2009 Ricard Marxer +// Copyright (C) 2009-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/. + +#ifndef EIGEN_REVERSE_H +#define EIGEN_REVERSE_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct traits > : traits { + typedef typename MatrixType::Scalar Scalar; + typedef typename traits::StorageKind StorageKind; + typedef typename traits::XprKind XprKind; + typedef typename ref_selector::type MatrixTypeNested; + typedef std::remove_reference_t MatrixTypeNested_; + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, + Flags = MatrixTypeNested_::Flags & (RowMajorBit | LvalueBit) + }; +}; + +template +struct reverse_packet_cond { + static inline PacketType run(const PacketType& x) { return preverse(x); } +}; + +template +struct reverse_packet_cond { + static inline PacketType run(const PacketType& x) { return x; } +}; + +} // end namespace internal + +/** \class Reverse + * \ingroup Core_Module + * + * \brief Expression of the reverse of a vector or matrix + * + * \tparam MatrixType the type of the object of which we are taking the reverse + * \tparam Direction defines the direction of the reverse operation, can be Vertical, Horizontal, or BothDirections + * + * This class represents an expression of the reverse of a vector. + * It is the return type of MatrixBase::reverse() and VectorwiseOp::reverse() + * and most of the time this is the only way it is used. + * + * \sa MatrixBase::reverse(), VectorwiseOp::reverse() + */ +template +class Reverse : public internal::dense_xpr_base >::type { + public: + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Reverse) + typedef internal::remove_all_t NestedExpression; + using Base::IsRowMajor; + + protected: + enum { + PacketSize = internal::packet_traits::size, + IsColMajor = !IsRowMajor, + ReverseRow = (Direction == Vertical) || (Direction == BothDirections), + ReverseCol = (Direction == Horizontal) || (Direction == BothDirections), + OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1, + OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1, + ReversePacket = (Direction == BothDirections) || ((Direction == Vertical) && IsColMajor) || + ((Direction == Horizontal) && IsRowMajor) + }; + typedef internal::reverse_packet_cond reverse_packet; + + public: + EIGEN_DEVICE_FUNC explicit inline Reverse(const MatrixType& matrix) : m_matrix(matrix) {} + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reverse) + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + + EIGEN_DEVICE_FUNC inline Index innerStride() const { return -m_matrix.innerStride(); } + + EIGEN_DEVICE_FUNC const internal::remove_all_t& nestedExpression() const { + return m_matrix; + } + + protected: + typename MatrixType::Nested m_matrix; +}; + +/** \returns an expression of the reverse of *this. + * + * Example: \include MatrixBase_reverse.cpp + * Output: \verbinclude MatrixBase_reverse.out + * + */ +template +EIGEN_DEVICE_FUNC inline typename DenseBase::ReverseReturnType DenseBase::reverse() { + return ReverseReturnType(derived()); +} + +// reverse const overload moved DenseBase.h due to a CUDA compiler bug + +/** This is the "in place" version of reverse: it reverses \c *this. + * + * In most cases it is probably better to simply use the reversed expression + * of a matrix. However, when reversing the matrix data itself is really needed, + * then this "in-place" version is probably the right choice because it provides + * the following additional benefits: + * - less error prone: doing the same operation with .reverse() requires special care: + * \code m = m.reverse().eval(); \endcode + * - this API enables reverse operations without the need for a temporary + * - it allows future optimizations (cache friendliness, etc.) + * + * \sa VectorwiseOp::reverseInPlace(), reverse() */ +template +EIGEN_DEVICE_FUNC inline void DenseBase::reverseInPlace() { + if (cols() > rows()) { + Index half = cols() / 2; + leftCols(half).swap(rightCols(half).reverse()); + if ((cols() % 2) == 1) { + Index half2 = rows() / 2; + col(half).head(half2).swap(col(half).tail(half2).reverse()); + } + } else { + Index half = rows() / 2; + topRows(half).swap(bottomRows(half).reverse()); + if ((rows() % 2) == 1) { + Index half2 = cols() / 2; + row(half).head(half2).swap(row(half).tail(half2).reverse()); + } + } +} + +namespace internal { + +template +struct vectorwise_reverse_inplace_impl; + +template <> +struct vectorwise_reverse_inplace_impl { + template + static void run(ExpressionType& xpr) { + constexpr Index HalfAtCompileTime = + ExpressionType::RowsAtCompileTime == Dynamic ? Dynamic : ExpressionType::RowsAtCompileTime / 2; + Index half = xpr.rows() / 2; + xpr.template topRows(half).swap( + xpr.template bottomRows(half).colwise().reverse()); + } +}; + +template <> +struct vectorwise_reverse_inplace_impl { + template + static void run(ExpressionType& xpr) { + constexpr Index HalfAtCompileTime = + ExpressionType::ColsAtCompileTime == Dynamic ? Dynamic : ExpressionType::ColsAtCompileTime / 2; + Index half = xpr.cols() / 2; + xpr.template leftCols(half).swap( + xpr.template rightCols(half).rowwise().reverse()); + } +}; + +} // end namespace internal + +/** This is the "in place" version of VectorwiseOp::reverse: it reverses each column or row of \c *this. + * + * In most cases it is probably better to simply use the reversed expression + * of a matrix. However, when reversing the matrix data itself is really needed, + * then this "in-place" version is probably the right choice because it provides + * the following additional benefits: + * - less error prone: doing the same operation with .reverse() requires special care: + * \code m = m.reverse().eval(); \endcode + * - this API enables reverse operations without the need for a temporary + * + * \sa DenseBase::reverseInPlace(), reverse() */ +template +EIGEN_DEVICE_FUNC void VectorwiseOp::reverseInPlace() { + internal::vectorwise_reverse_inplace_impl::run(m_matrix); +} + +} // end namespace Eigen + +#endif // EIGEN_REVERSE_H diff --git a/include/frc/thirdparty/eigen/include/Eigen/src/Core/Select.h b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Select.h new file mode 100644 index 00000000..9f461204 --- /dev/null +++ b/include/frc/thirdparty/eigen/include/Eigen/src/Core/Select.h @@ -0,0 +1,156 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-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/. + +#ifndef EIGEN_SELECT_H +#define EIGEN_SELECT_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +/** \class Select + * \ingroup Core_Module + * + * \brief Expression of a coefficient wise version of the C++ ternary operator ?: + * + * \tparam ConditionMatrixType the type of the \em condition expression which must be a boolean matrix + * \tparam ThenMatrixType the type of the \em then expression + * \tparam ElseMatrixType the type of the \em else expression + * + * This class represents an expression of a coefficient wise version of the C++ ternary operator ?:. + * It is the return type of DenseBase::select() and most of the time this is the only way it is used. + * + * \sa DenseBase::select(const DenseBase&, const DenseBase&) const + */ + +namespace internal { +template +struct traits > : traits { + typedef typename traits::Scalar Scalar; + typedef Dense StorageKind; + typedef typename traits::XprKind XprKind; + typedef typename ConditionMatrixType::Nested ConditionMatrixNested; + typedef typename ThenMatrixType::Nested ThenMatrixNested; + typedef typename ElseMatrixType::Nested ElseMatrixNested; + enum { + RowsAtCompileTime = ConditionMatrixType::RowsAtCompileTime, + ColsAtCompileTime = ConditionMatrixType::ColsAtCompileTime, + MaxRowsAtCompileTime = ConditionMatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = ConditionMatrixType::MaxColsAtCompileTime, + Flags = (unsigned int)ThenMatrixType::Flags & ElseMatrixType::Flags & RowMajorBit + }; +}; +} // namespace internal + +template +class Select : public internal::dense_xpr_base >::type, + internal::no_assignment_operator { + public: + typedef typename internal::dense_xpr_base