From 80684f44eab23372a92071be93d56d6819a254ee Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 4 Jan 2019 10:10:20 -0800 Subject: [PATCH 1/3] fix issue 76 (should be harmless) --- src/BulletCollision/BroadphaseCollision/btDbvt.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 37156fd58..5ab3f9b60 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -37,7 +37,7 @@ static DBVT_INLINE int indexof(const btDbvtNode* node) static DBVT_INLINE btDbvtVolume merge(const btDbvtVolume& a, const btDbvtVolume& b) { -#if (DBVT_MERGE_IMPL == DBVT_IMPL_SSE) +#ifdef BT_USE_SSE ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); btDbvtVolume* ptr = (btDbvtVolume*)locals; btDbvtVolume& res = *ptr; @@ -298,7 +298,7 @@ static int split(btDbvtNode** leaves, static btDbvtVolume bounds(btDbvtNode** leaves, int count) { -#if DBVT_MERGE_IMPL == DBVT_IMPL_SSE +#ifdef BT_USE_SSE ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); btDbvtVolume* ptr = (btDbvtVolume*)locals; btDbvtVolume& volume = *ptr; From ae8e83988b77d55b3013561f589a0f5164795249 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 22 Jan 2019 21:08:37 -0800 Subject: [PATCH 2/3] Add preliminary PhysX 4.0 backend for PyBullet Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py) Fix related to TinyRenderer object transforms not updating when using collision filtering --- .../btMultiBodyTreeCreator.cpp | 5 +- examples/ExampleBrowser/premake4.lua | 14 +- .../ImportURDFDemo/BulletUrdfImporter.cpp | 4 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 112 +- .../Importers/ImportURDFDemo/URDF2Bullet.h | 23 +- .../ImportURDFDemo/URDFImporterInterface.h | 5 + .../Importers/ImportURDFDemo/URDFJointTypes.h | 25 + .../ImportURDFDemo/UrdfRenderingInterface.h | 2 +- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 59 +- examples/OpenGLWindow/Win32Window.cpp | 12 +- examples/SharedMemory/PhysicsClientC_API.cpp | 105 +- examples/SharedMemory/PhysicsClientC_API.h | 8 +- .../PhysicsServerCommandProcessor.cpp | 413 +- examples/SharedMemory/SharedMemoryCommands.h | 3 + examples/SharedMemory/SharedMemoryPublic.h | 2 + examples/SharedMemory/physx/PhysXC_API.cpp | 15 + examples/SharedMemory/physx/PhysXC_API.h | 20 + examples/SharedMemory/physx/PhysXClient.cpp | 1409 ++ examples/SharedMemory/physx/PhysXClient.h | 135 + .../physx/PhysXServerCommandProcessor.cpp | 1639 ++ .../physx/PhysXServerCommandProcessor.h | 47 + .../SharedMemory/physx/PhysXUrdfImporter.cpp | 1434 ++ .../SharedMemory/physx/PhysXUrdfImporter.h | 116 + examples/SharedMemory/physx/PhysXUserData.h | 8 + examples/SharedMemory/physx/URDF2PhysX.cpp | 908 + examples/SharedMemory/physx/URDF2PhysX.h | 25 + .../eglRendererVisualShapeConverter.cpp | 407 +- .../eglRendererVisualShapeConverter.h | 6 +- .../stablePDPlugin/BulletConversion.cpp | 391 + .../plugins/stablePDPlugin/BulletConversion.h | 9 + .../plugins/stablePDPlugin/KinTree.cpp | 1982 ++ .../plugins/stablePDPlugin/KinTree.h | 271 + .../plugins/stablePDPlugin/LICENSE.txt | 21 + .../plugins/stablePDPlugin/MathUtil.cpp | 904 + .../plugins/stablePDPlugin/MathUtil.h | 155 + .../plugins/stablePDPlugin/RBDModel.cpp | 245 + .../plugins/stablePDPlugin/RBDModel.h | 71 + .../plugins/stablePDPlugin/RBDUtil.cpp | 1050 + .../plugins/stablePDPlugin/RBDUtil.h | 85 + .../plugins/stablePDPlugin/Rand.cpp | 140 + .../plugins/stablePDPlugin/Rand.h | 31 + .../plugins/stablePDPlugin/Shape.cpp | 37 + .../plugins/stablePDPlugin/Shape.h | 20 + .../plugins/stablePDPlugin/SpAlg.cpp | 356 + .../plugins/stablePDPlugin/SpAlg.h | 68 + .../TinyRendererVisualShapeConverter.cpp | 15 +- .../TinyRendererVisualShapeConverter.h | 2 +- examples/ThirdPartyLibs/Eigen/CMakeLists.txt | 19 + examples/ThirdPartyLibs/Eigen/Cholesky | 46 + examples/ThirdPartyLibs/Eigen/CholmodSupport | 48 + examples/ThirdPartyLibs/Eigen/Core | 562 + examples/ThirdPartyLibs/Eigen/Dense | 7 + examples/ThirdPartyLibs/Eigen/Eigen | 2 + examples/ThirdPartyLibs/Eigen/Eigenvalues | 61 + examples/ThirdPartyLibs/Eigen/Geometry | 61 + examples/ThirdPartyLibs/Eigen/Householder | 30 + .../Eigen/IterativeLinearSolvers | 48 + examples/ThirdPartyLibs/Eigen/Jacobi | 33 + examples/ThirdPartyLibs/Eigen/LU | 50 + examples/ThirdPartyLibs/Eigen/MetisSupport | 35 + examples/ThirdPartyLibs/Eigen/OrderingMethods | 73 + examples/ThirdPartyLibs/Eigen/PaStiXSupport | 48 + examples/ThirdPartyLibs/Eigen/PardisoSupport | 35 + examples/ThirdPartyLibs/Eigen/QR | 51 + examples/ThirdPartyLibs/Eigen/QtAlignedMalloc | 40 + examples/ThirdPartyLibs/Eigen/SPQRSupport | 34 + examples/ThirdPartyLibs/Eigen/SVD | 51 + examples/ThirdPartyLibs/Eigen/Sparse | 36 + examples/ThirdPartyLibs/Eigen/SparseCholesky | 45 + examples/ThirdPartyLibs/Eigen/SparseCore | 69 + examples/ThirdPartyLibs/Eigen/SparseLU | 46 + examples/ThirdPartyLibs/Eigen/SparseQR | 37 + examples/ThirdPartyLibs/Eigen/StdDeque | 27 + examples/ThirdPartyLibs/Eigen/StdList | 26 + examples/ThirdPartyLibs/Eigen/StdVector | 27 + examples/ThirdPartyLibs/Eigen/SuperLUSupport | 64 + examples/ThirdPartyLibs/Eigen/UmfPackSupport | 40 + .../ThirdPartyLibs/Eigen/src/Cholesky/LDLT.h | 668 + .../ThirdPartyLibs/Eigen/src/Cholesky/LLT.h | 541 + .../Eigen/src/Cholesky/LLT_LAPACKE.h | 99 + .../Eigen/src/CholmodSupport/CholmodSupport.h | 682 + .../Eigen/src/Core/ArithmeticSequence.h | 350 + .../ThirdPartyLibs/Eigen/src/Core/Array.h | 331 + .../ThirdPartyLibs/Eigen/src/Core/ArrayBase.h | 226 + .../Eigen/src/Core/ArrayWrapper.h | 209 + .../ThirdPartyLibs/Eigen/src/Core/Assign.h | 90 + .../Eigen/src/Core/AssignEvaluator.h | 935 + .../Eigen/src/Core/Assign_MKL.h | 176 + .../Eigen/src/Core/BandMatrix.h | 353 + .../ThirdPartyLibs/Eigen/src/Core/Block.h | 452 + .../Eigen/src/Core/BooleanRedux.h | 162 + .../Eigen/src/Core/CommaInitializer.h | 160 + .../Eigen/src/Core/ConditionEstimator.h | 175 + .../Eigen/src/Core/CoreEvaluators.h | 1728 ++ .../Eigen/src/Core/CoreIterators.h | 132 + .../Eigen/src/Core/CwiseBinaryOp.h | 183 + .../Eigen/src/Core/CwiseNullaryOp.h | 902 + .../Eigen/src/Core/CwiseTernaryOp.h | 197 + .../Eigen/src/Core/CwiseUnaryOp.h | 103 + .../Eigen/src/Core/CwiseUnaryView.h | 128 + .../ThirdPartyLibs/Eigen/src/Core/DenseBase.h | 615 + .../Eigen/src/Core/DenseCoeffsBase.h | 681 + .../Eigen/src/Core/DenseStorage.h | 570 + .../ThirdPartyLibs/Eigen/src/Core/Diagonal.h | 259 + .../Eigen/src/Core/DiagonalMatrix.h | 343 + .../Eigen/src/Core/DiagonalProduct.h | 28 + examples/ThirdPartyLibs/Eigen/src/Core/Dot.h | 315 + .../ThirdPartyLibs/Eigen/src/Core/EigenBase.h | 159 + .../Eigen/src/Core/ForceAlignedAccess.h | 146 + .../ThirdPartyLibs/Eigen/src/Core/Fuzzy.h | 155 + .../Eigen/src/Core/GeneralProduct.h | 466 + .../Eigen/src/Core/GenericPacketMath.h | 598 + .../Eigen/src/Core/GlobalFunctions.h | 185 + examples/ThirdPartyLibs/Eigen/src/Core/IO.h | 225 + .../Eigen/src/Core/IndexedView.h | 207 + .../ThirdPartyLibs/Eigen/src/Core/Inverse.h | 118 + examples/ThirdPartyLibs/Eigen/src/Core/Map.h | 171 + .../ThirdPartyLibs/Eigen/src/Core/MapBase.h | 299 + .../Eigen/src/Core/MathFunctions.h | 1711 ++ .../Eigen/src/Core/MathFunctionsImpl.h | 73 + .../ThirdPartyLibs/Eigen/src/Core/Matrix.h | 461 + .../Eigen/src/Core/MatrixBase.h | 525 + .../Eigen/src/Core/NestByValue.h | 110 + .../ThirdPartyLibs/Eigen/src/Core/NoAlias.h | 109 + .../ThirdPartyLibs/Eigen/src/Core/NumTraits.h | 248 + .../Eigen/src/Core/PermutationMatrix.h | 633 + .../Eigen/src/Core/PlainObjectBase.h | 1035 + .../ThirdPartyLibs/Eigen/src/Core/Product.h | 186 + .../Eigen/src/Core/ProductEvaluators.h | 1105 ++ .../ThirdPartyLibs/Eigen/src/Core/Random.h | 182 + .../ThirdPartyLibs/Eigen/src/Core/Redux.h | 505 + examples/ThirdPartyLibs/Eigen/src/Core/Ref.h | 283 + .../ThirdPartyLibs/Eigen/src/Core/Replicate.h | 142 + .../Eigen/src/Core/ReturnByValue.h | 117 + .../ThirdPartyLibs/Eigen/src/Core/Reverse.h | 211 + .../ThirdPartyLibs/Eigen/src/Core/Select.h | 162 + .../Eigen/src/Core/SelfAdjointView.h | 350 + .../Eigen/src/Core/SelfCwiseBinaryOp.h | 51 + .../ThirdPartyLibs/Eigen/src/Core/Solve.h | 188 + .../Eigen/src/Core/SolveTriangular.h | 232 + .../Eigen/src/Core/SolverBase.h | 130 + .../Eigen/src/Core/StableNorm.h | 221 + .../ThirdPartyLibs/Eigen/src/Core/Stride.h | 111 + examples/ThirdPartyLibs/Eigen/src/Core/Swap.h | 67 + .../ThirdPartyLibs/Eigen/src/Core/Transpose.h | 403 + .../Eigen/src/Core/Transpositions.h | 407 + .../Eigen/src/Core/TriangularMatrix.h | 984 + .../Eigen/src/Core/VectorBlock.h | 96 + .../Eigen/src/Core/VectorwiseOp.h | 695 + .../ThirdPartyLibs/Eigen/src/Core/Visitor.h | 273 + .../Eigen/src/Core/arch/AVX/Complex.h | 451 + .../Eigen/src/Core/arch/AVX/MathFunctions.h | 439 + .../Eigen/src/Core/arch/AVX/PacketMath.h | 643 + .../Eigen/src/Core/arch/AVX/TypeCasting.h | 51 + .../src/Core/arch/AVX512/MathFunctions.h | 396 + .../Eigen/src/Core/arch/AVX512/PacketMath.h | 1286 ++ .../Eigen/src/Core/arch/AltiVec/Complex.h | 430 + .../src/Core/arch/AltiVec/MathFunctions.h | 322 + .../Eigen/src/Core/arch/AltiVec/PacketMath.h | 1033 + .../Eigen/src/Core/arch/CUDA/Complex.h | 103 + .../Eigen/src/Core/arch/CUDA/Half.h | 666 + .../Eigen/src/Core/arch/CUDA/MathFunctions.h | 103 + .../Eigen/src/Core/arch/CUDA/PacketMath.h | 333 + .../Eigen/src/Core/arch/CUDA/PacketMathHalf.h | 1133 ++ .../Eigen/src/Core/arch/CUDA/TypeCasting.h | 212 + .../Eigen/src/Core/arch/Default/ConjHelper.h | 29 + .../Eigen/src/Core/arch/Default/Settings.h | 49 + .../Eigen/src/Core/arch/NEON/Complex.h | 490 + .../Eigen/src/Core/arch/NEON/MathFunctions.h | 91 + .../Eigen/src/Core/arch/NEON/PacketMath.h | 729 + .../Eigen/src/Core/arch/SSE/Complex.h | 471 + .../Eigen/src/Core/arch/SSE/MathFunctions.h | 562 + .../Eigen/src/Core/arch/SSE/PacketMath.h | 931 + .../Eigen/src/Core/arch/SSE/TypeCasting.h | 77 + .../Eigen/src/Core/arch/ZVector/Complex.h | 397 + .../src/Core/arch/ZVector/MathFunctions.h | 137 + .../Eigen/src/Core/arch/ZVector/PacketMath.h | 945 + .../src/Core/functors/AssignmentFunctors.h | 168 + .../Eigen/src/Core/functors/BinaryFunctors.h | 482 + .../Eigen/src/Core/functors/NullaryFunctors.h | 188 + .../Eigen/src/Core/functors/StlFunctors.h | 132 + .../Eigen/src/Core/functors/TernaryFunctors.h | 25 + .../Eigen/src/Core/functors/UnaryFunctors.h | 830 + .../Core/products/GeneralBlockPanelKernel.h | 2149 ++ .../src/Core/products/GeneralMatrixMatrix.h | 498 + .../products/GeneralMatrixMatrixTriangular.h | 311 + .../GeneralMatrixMatrixTriangular_BLAS.h | 145 + .../Core/products/GeneralMatrixMatrix_BLAS.h | 122 + .../src/Core/products/GeneralMatrixVector.h | 405 + .../Core/products/GeneralMatrixVector_BLAS.h | 136 + .../Eigen/src/Core/products/Parallelizer.h | 163 + .../Core/products/SelfadjointMatrixMatrix.h | 521 + .../products/SelfadjointMatrixMatrix_BLAS.h | 287 + .../Core/products/SelfadjointMatrixVector.h | 260 + .../products/SelfadjointMatrixVector_BLAS.h | 118 + .../src/Core/products/SelfadjointProduct.h | 133 + .../Core/products/SelfadjointRank2Update.h | 93 + .../Core/products/TriangularMatrixMatrix.h | 448 + .../products/TriangularMatrixMatrix_BLAS.h | 315 + .../Core/products/TriangularMatrixVector.h | 336 + .../products/TriangularMatrixVector_BLAS.h | 255 + .../Core/products/TriangularSolverMatrix.h | 335 + .../products/TriangularSolverMatrix_BLAS.h | 163 + .../Core/products/TriangularSolverVector.h | 145 + .../Eigen/src/Core/util/BlasUtil.h | 403 + .../Eigen/src/Core/util/Constants.h | 551 + .../src/Core/util/DisableStupidWarnings.h | 79 + .../Eigen/src/Core/util/ForwardDeclarations.h | 303 + .../Eigen/src/Core/util/IndexedViewHelper.h | 187 + .../Eigen/src/Core/util/IntegralConstant.h | 270 + .../Eigen/src/Core/util/MKL_support.h | 130 + .../Eigen/src/Core/util/Macros.h | 1029 + .../Eigen/src/Core/util/Memory.h | 985 + .../ThirdPartyLibs/Eigen/src/Core/util/Meta.h | 550 + .../Eigen/src/Core/util/NonMPL2.h | 3 + .../src/Core/util/ReenableStupidWarnings.h | 27 + .../Eigen/src/Core/util/StaticAssert.h | 216 + .../Eigen/src/Core/util/SymbolicIndex.h | 300 + .../Eigen/src/Core/util/XprHelper.h | 835 + .../src/Eigenvalues/ComplexEigenSolver.h | 346 + .../Eigen/src/Eigenvalues/ComplexSchur.h | 459 + .../src/Eigenvalues/ComplexSchur_LAPACKE.h | 91 + .../Eigen/src/Eigenvalues/EigenSolver.h | 622 + .../src/Eigenvalues/GeneralizedEigenSolver.h | 419 + .../GeneralizedSelfAdjointEigenSolver.h | 226 + .../src/Eigenvalues/HessenbergDecomposition.h | 374 + .../src/Eigenvalues/MatrixBaseEigenvalues.h | 160 + .../Eigen/src/Eigenvalues/RealQZ.h | 654 + .../Eigen/src/Eigenvalues/RealSchur.h | 546 + .../Eigen/src/Eigenvalues/RealSchur_LAPACKE.h | 77 + .../src/Eigenvalues/SelfAdjointEigenSolver.h | 870 + .../SelfAdjointEigenSolver_LAPACKE.h | 90 + .../src/Eigenvalues/Tridiagonalization.h | 556 + .../Eigen/src/Geometry/AlignedBox.h | 392 + .../Eigen/src/Geometry/AngleAxis.h | 247 + .../Eigen/src/Geometry/EulerAngles.h | 114 + .../Eigen/src/Geometry/Homogeneous.h | 497 + .../Eigen/src/Geometry/Hyperplane.h | 282 + .../Eigen/src/Geometry/OrthoMethods.h | 234 + .../Eigen/src/Geometry/ParametrizedLine.h | 232 + .../Eigen/src/Geometry/Quaternion.h | 814 + .../Eigen/src/Geometry/Rotation2D.h | 199 + .../Eigen/src/Geometry/RotationBase.h | 206 + .../Eigen/src/Geometry/Scaling.h | 170 + .../Eigen/src/Geometry/Transform.h | 1542 ++ .../Eigen/src/Geometry/Translation.h | 208 + .../Eigen/src/Geometry/Umeyama.h | 166 + .../Eigen/src/Geometry/arch/Geometry_SSE.h | 161 + .../Eigen/src/Householder/BlockHouseholder.h | 103 + .../Eigen/src/Householder/Householder.h | 172 + .../src/Householder/HouseholderSequence.h | 470 + .../BasicPreconditioners.h | 226 + .../src/IterativeLinearSolvers/BiCGSTAB.h | 228 + .../ConjugateGradient.h | 245 + .../IncompleteCholesky.h | 400 + .../IterativeLinearSolvers/IncompleteLUT.h | 462 + .../IterativeSolverBase.h | 394 + .../LeastSquareConjugateGradient.h | 216 + .../IterativeLinearSolvers/SolveWithGuess.h | 115 + .../ThirdPartyLibs/Eigen/src/Jacobi/Jacobi.h | 474 + .../ThirdPartyLibs/Eigen/src/LU/Determinant.h | 101 + .../ThirdPartyLibs/Eigen/src/LU/FullPivLU.h | 889 + .../ThirdPartyLibs/Eigen/src/LU/InverseImpl.h | 415 + .../Eigen/src/LU/PartialPivLU.h | 611 + .../Eigen/src/LU/PartialPivLU_LAPACKE.h | 83 + .../Eigen/src/LU/arch/Inverse_SSE.h | 338 + .../Eigen/src/MetisSupport/MetisSupport.h | 137 + .../Eigen/src/OrderingMethods/Amd.h | 445 + .../Eigen/src/OrderingMethods/Eigen_Colamd.h | 1843 ++ .../Eigen/src/OrderingMethods/Ordering.h | 157 + .../Eigen/src/PaStiXSupport/PaStiXSupport.h | 678 + .../Eigen/src/PardisoSupport/PardisoSupport.h | 543 + .../Eigen/src/QR/ColPivHouseholderQR.h | 652 + .../src/QR/ColPivHouseholderQR_LAPACKE.h | 97 + .../src/QR/CompleteOrthogonalDecomposition.h | 562 + .../Eigen/src/QR/FullPivHouseholderQR.h | 675 + .../Eigen/src/QR/HouseholderQR.h | 408 + .../Eigen/src/QR/HouseholderQR_LAPACKE.h | 68 + .../src/SPQRSupport/SuiteSparseQRSupport.h | 313 + .../ThirdPartyLibs/Eigen/src/SVD/BDCSVD.h | 1231 ++ .../ThirdPartyLibs/Eigen/src/SVD/JacobiSVD.h | 804 + .../Eigen/src/SVD/JacobiSVD_LAPACKE.h | 90 + .../ThirdPartyLibs/Eigen/src/SVD/SVDBase.h | 312 + .../Eigen/src/SVD/UpperBidiagonalization.h | 414 + .../src/SparseCholesky/SimplicialCholesky.h | 689 + .../SparseCholesky/SimplicialCholesky_impl.h | 199 + .../Eigen/src/SparseCore/AmbiVector.h | 377 + .../Eigen/src/SparseCore/CompressedStorage.h | 258 + .../ConservativeSparseSparseProduct.h | 352 + .../Eigen/src/SparseCore/MappedSparseMatrix.h | 67 + .../Eigen/src/SparseCore/SparseAssign.h | 216 + .../Eigen/src/SparseCore/SparseBlock.h | 603 + .../Eigen/src/SparseCore/SparseColEtree.h | 206 + .../src/SparseCore/SparseCompressedBase.h | 357 + .../src/SparseCore/SparseCwiseBinaryOp.h | 722 + .../Eigen/src/SparseCore/SparseCwiseUnaryOp.h | 148 + .../Eigen/src/SparseCore/SparseDenseProduct.h | 320 + .../src/SparseCore/SparseDiagonalProduct.h | 138 + .../Eigen/src/SparseCore/SparseDot.h | 98 + .../Eigen/src/SparseCore/SparseFuzzy.h | 29 + .../Eigen/src/SparseCore/SparseMap.h | 305 + .../Eigen/src/SparseCore/SparseMatrix.h | 1403 ++ .../Eigen/src/SparseCore/SparseMatrixBase.h | 405 + .../Eigen/src/SparseCore/SparsePermutation.h | 178 + .../Eigen/src/SparseCore/SparseProduct.h | 169 + .../Eigen/src/SparseCore/SparseRedux.h | 49 + .../Eigen/src/SparseCore/SparseRef.h | 397 + .../src/SparseCore/SparseSelfAdjointView.h | 656 + .../Eigen/src/SparseCore/SparseSolverBase.h | 124 + .../SparseSparseProductWithPruning.h | 198 + .../Eigen/src/SparseCore/SparseTranspose.h | 92 + .../src/SparseCore/SparseTriangularView.h | 189 + .../Eigen/src/SparseCore/SparseUtil.h | 178 + .../Eigen/src/SparseCore/SparseVector.h | 478 + .../Eigen/src/SparseCore/SparseView.h | 253 + .../Eigen/src/SparseCore/TriangularSolver.h | 315 + .../Eigen/src/SparseLU/SparseLU.h | 775 + .../Eigen/src/SparseLU/SparseLUImpl.h | 66 + .../Eigen/src/SparseLU/SparseLU_Memory.h | 226 + .../Eigen/src/SparseLU/SparseLU_Structs.h | 110 + .../src/SparseLU/SparseLU_SupernodalMatrix.h | 301 + .../Eigen/src/SparseLU/SparseLU_Utils.h | 80 + .../Eigen/src/SparseLU/SparseLU_column_bmod.h | 181 + .../Eigen/src/SparseLU/SparseLU_column_dfs.h | 179 + .../src/SparseLU/SparseLU_copy_to_ucol.h | 107 + .../Eigen/src/SparseLU/SparseLU_gemm_kernel.h | 280 + .../src/SparseLU/SparseLU_heap_relax_snode.h | 126 + .../Eigen/src/SparseLU/SparseLU_kernel_bmod.h | 130 + .../Eigen/src/SparseLU/SparseLU_panel_bmod.h | 223 + .../Eigen/src/SparseLU/SparseLU_panel_dfs.h | 258 + .../Eigen/src/SparseLU/SparseLU_pivotL.h | 137 + .../Eigen/src/SparseLU/SparseLU_pruneL.h | 136 + .../Eigen/src/SparseLU/SparseLU_relax_snode.h | 83 + .../Eigen/src/SparseQR/SparseQR.h | 739 + .../Eigen/src/StlSupport/StdDeque.h | 126 + .../Eigen/src/StlSupport/StdList.h | 106 + .../Eigen/src/StlSupport/StdVector.h | 131 + .../Eigen/src/StlSupport/details.h | 84 + .../Eigen/src/SuperLUSupport/SuperLUSupport.h | 1027 + .../Eigen/src/UmfPackSupport/UmfPackSupport.h | 506 + .../ThirdPartyLibs/Eigen/src/misc/Image.h | 82 + .../ThirdPartyLibs/Eigen/src/misc/Kernel.h | 79 + .../Eigen/src/misc/RealSvd2x2.h | 55 + examples/ThirdPartyLibs/Eigen/src/misc/blas.h | 440 + .../ThirdPartyLibs/Eigen/src/misc/lapack.h | 152 + .../ThirdPartyLibs/Eigen/src/misc/lapacke.h | 16292 ++++++++++++++++ .../Eigen/src/misc/lapacke_mangling.h | 17 + .../Eigen/src/plugins/ArrayCwiseBinaryOps.h | 332 + .../Eigen/src/plugins/ArrayCwiseUnaryOps.h | 567 + .../Eigen/src/plugins/BlockMethods.h | 1356 ++ .../Eigen/src/plugins/CommonCwiseBinaryOps.h | 115 + .../Eigen/src/plugins/CommonCwiseUnaryOps.h | 163 + .../Eigen/src/plugins/IndexedViewMethods.h | 267 + .../Eigen/src/plugins/MatrixCwiseBinaryOps.h | 152 + .../Eigen/src/plugins/MatrixCwiseUnaryOps.h | 85 + .../examples/humanoidMotionCapture.py | 329 +- examples/pybullet/examples/pdControl.py | 23 +- .../pybullet/examples/pdControllerExplicit.py | 102 +- .../pybullet/examples/pdControllerStable.py | 144 + examples/pybullet/pybullet.c | 167 +- .../CollisionDispatch/btCollisionObject.cpp | 1 + .../CollisionDispatch/btCollisionObject.h | 12 + src/BulletDynamics/Featherstone/btMultiBody.h | 5 +- src/BulletInverseDynamics/MultiBodyTree.cpp | 2 +- .../details/MultiBodyTreeImpl.cpp | 27 +- src/LinearMath/btMatrixX.h | 5 +- 366 files changed, 131855 insertions(+), 359 deletions(-) create mode 100644 examples/SharedMemory/physx/PhysXC_API.cpp create mode 100644 examples/SharedMemory/physx/PhysXC_API.h create mode 100644 examples/SharedMemory/physx/PhysXClient.cpp create mode 100644 examples/SharedMemory/physx/PhysXClient.h create mode 100644 examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp create mode 100644 examples/SharedMemory/physx/PhysXServerCommandProcessor.h create mode 100644 examples/SharedMemory/physx/PhysXUrdfImporter.cpp create mode 100644 examples/SharedMemory/physx/PhysXUrdfImporter.h create mode 100644 examples/SharedMemory/physx/PhysXUserData.h create mode 100644 examples/SharedMemory/physx/URDF2PhysX.cpp create mode 100644 examples/SharedMemory/physx/URDF2PhysX.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/KinTree.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/LICENSE.txt create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/Rand.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/Shape.h create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp create mode 100644 examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h create mode 100644 examples/ThirdPartyLibs/Eigen/CMakeLists.txt create mode 100644 examples/ThirdPartyLibs/Eigen/Cholesky create mode 100644 examples/ThirdPartyLibs/Eigen/CholmodSupport create mode 100644 examples/ThirdPartyLibs/Eigen/Core create mode 100644 examples/ThirdPartyLibs/Eigen/Dense create mode 100644 examples/ThirdPartyLibs/Eigen/Eigen create mode 100644 examples/ThirdPartyLibs/Eigen/Eigenvalues create mode 100644 examples/ThirdPartyLibs/Eigen/Geometry create mode 100644 examples/ThirdPartyLibs/Eigen/Householder create mode 100644 examples/ThirdPartyLibs/Eigen/IterativeLinearSolvers create mode 100644 examples/ThirdPartyLibs/Eigen/Jacobi create mode 100644 examples/ThirdPartyLibs/Eigen/LU create mode 100644 examples/ThirdPartyLibs/Eigen/MetisSupport create mode 100644 examples/ThirdPartyLibs/Eigen/OrderingMethods create mode 100644 examples/ThirdPartyLibs/Eigen/PaStiXSupport create mode 100644 examples/ThirdPartyLibs/Eigen/PardisoSupport create mode 100644 examples/ThirdPartyLibs/Eigen/QR create mode 100644 examples/ThirdPartyLibs/Eigen/QtAlignedMalloc create mode 100644 examples/ThirdPartyLibs/Eigen/SPQRSupport create mode 100644 examples/ThirdPartyLibs/Eigen/SVD create mode 100644 examples/ThirdPartyLibs/Eigen/Sparse create mode 100644 examples/ThirdPartyLibs/Eigen/SparseCholesky create mode 100644 examples/ThirdPartyLibs/Eigen/SparseCore create mode 100644 examples/ThirdPartyLibs/Eigen/SparseLU create mode 100644 examples/ThirdPartyLibs/Eigen/SparseQR create mode 100644 examples/ThirdPartyLibs/Eigen/StdDeque create mode 100644 examples/ThirdPartyLibs/Eigen/StdList create mode 100644 examples/ThirdPartyLibs/Eigen/StdVector create mode 100644 examples/ThirdPartyLibs/Eigen/SuperLUSupport create mode 100644 examples/ThirdPartyLibs/Eigen/UmfPackSupport create mode 100644 examples/ThirdPartyLibs/Eigen/src/Cholesky/LDLT.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/CholmodSupport/CholmodSupport.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/ArithmeticSequence.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Array.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/ArrayBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/ArrayWrapper.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Assign.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/AssignEvaluator.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Assign_MKL.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/BandMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Block.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/BooleanRedux.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CommaInitializer.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/ConditionEstimator.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CoreEvaluators.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CoreIterators.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CwiseBinaryOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CwiseNullaryOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CwiseTernaryOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryView.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/DenseBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/DenseCoeffsBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/DenseStorage.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Diagonal.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/DiagonalMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/DiagonalProduct.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Dot.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/EigenBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/ForceAlignedAccess.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Fuzzy.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/GeneralProduct.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/GenericPacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/GlobalFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/IO.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/IndexedView.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Inverse.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Map.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/MapBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/MathFunctionsImpl.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Matrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/MatrixBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/NestByValue.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/NoAlias.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/NumTraits.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/PermutationMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/PlainObjectBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Product.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/ProductEvaluators.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Random.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Redux.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Ref.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Replicate.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/ReturnByValue.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Reverse.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Select.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/SelfAdjointView.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/SelfCwiseBinaryOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Solve.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/SolveTriangular.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/SolverBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/StableNorm.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Stride.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Swap.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Transpose.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Transpositions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/TriangularMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/VectorBlock.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/VectorwiseOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/Visitor.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AVX/Complex.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AVX/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AVX/PacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AVX/TypeCasting.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AVX512/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AVX512/PacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AltiVec/Complex.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AltiVec/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/AltiVec/PacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/CUDA/Complex.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/CUDA/Half.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/CUDA/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/CUDA/PacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/CUDA/PacketMathHalf.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/CUDA/TypeCasting.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/Default/ConjHelper.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/Default/Settings.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/NEON/Complex.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/NEON/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/NEON/PacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/SSE/Complex.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/SSE/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/SSE/PacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/SSE/TypeCasting.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/ZVector/Complex.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/ZVector/MathFunctions.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/arch/ZVector/PacketMath.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/functors/AssignmentFunctors.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/functors/BinaryFunctors.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/functors/NullaryFunctors.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/functors/StlFunctors.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/functors/TernaryFunctors.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/functors/UnaryFunctors.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/GeneralBlockPanelKernel.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/GeneralMatrixMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/GeneralMatrixVector.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/Parallelizer.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/SelfadjointMatrixMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/SelfadjointMatrixVector.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/SelfadjointProduct.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/SelfadjointRank2Update.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/TriangularMatrixMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/TriangularMatrixVector.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/TriangularSolverMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/products/TriangularSolverVector.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/BlasUtil.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/Constants.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/DisableStupidWarnings.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/ForwardDeclarations.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/IndexedViewHelper.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/IntegralConstant.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/MKL_support.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/Macros.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/Memory.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/Meta.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/NonMPL2.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/ReenableStupidWarnings.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/StaticAssert.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/SymbolicIndex.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Core/util/XprHelper.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/ComplexEigenSolver.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/ComplexSchur.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/ComplexSchur_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/EigenSolver.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/HessenbergDecomposition.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/RealQZ.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/RealSchur.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Eigenvalues/Tridiagonalization.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/AlignedBox.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/AngleAxis.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/EulerAngles.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Homogeneous.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Hyperplane.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/OrthoMethods.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/ParametrizedLine.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Quaternion.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Rotation2D.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/RotationBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Scaling.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Transform.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Translation.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/Umeyama.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Geometry/arch/Geometry_SSE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Householder/BlockHouseholder.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Householder/Householder.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Householder/HouseholderSequence.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/Jacobi/Jacobi.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/LU/Determinant.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/LU/FullPivLU.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/LU/InverseImpl.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/LU/PartialPivLU.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/LU/PartialPivLU_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/LU/arch/Inverse_SSE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/MetisSupport/MetisSupport.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/OrderingMethods/Amd.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/OrderingMethods/Eigen_Colamd.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/OrderingMethods/Ordering.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/PaStiXSupport/PaStiXSupport.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/PardisoSupport/PardisoSupport.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/QR/ColPivHouseholderQR.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/QR/ColPivHouseholderQR_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/QR/CompleteOrthogonalDecomposition.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/QR/FullPivHouseholderQR.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/QR/HouseholderQR.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/QR/HouseholderQR_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SVD/BDCSVD.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SVD/JacobiSVD.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SVD/JacobiSVD_LAPACKE.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SVD/SVDBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SVD/UpperBidiagonalization.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCholesky/SimplicialCholesky.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/AmbiVector.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/CompressedStorage.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/MappedSparseMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseAssign.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseBlock.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseColEtree.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseCompressedBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseCwiseBinaryOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseCwiseUnaryOp.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseDenseProduct.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseDiagonalProduct.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseDot.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseFuzzy.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseMap.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseMatrixBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparsePermutation.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseProduct.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseRedux.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseRef.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseSelfAdjointView.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseSolverBase.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseSparseProductWithPruning.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseTranspose.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseTriangularView.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseUtil.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseVector.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/SparseView.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseCore/TriangularSolver.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLUImpl.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_Memory.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_Structs.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_Utils.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_column_bmod.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_column_dfs.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_gemm_kernel.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_kernel_bmod.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_panel_bmod.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_panel_dfs.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_pivotL.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_pruneL.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseLU/SparseLU_relax_snode.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SparseQR/SparseQR.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/StlSupport/StdDeque.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/StlSupport/StdList.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/StlSupport/StdVector.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/StlSupport/details.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/SuperLUSupport/SuperLUSupport.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/UmfPackSupport/UmfPackSupport.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/misc/Image.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/misc/Kernel.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/misc/RealSvd2x2.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/misc/blas.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/misc/lapack.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/misc/lapacke.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/misc/lapacke_mangling.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/ArrayCwiseBinaryOps.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/ArrayCwiseUnaryOps.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/BlockMethods.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/CommonCwiseBinaryOps.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/CommonCwiseUnaryOps.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/IndexedViewMethods.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/MatrixCwiseBinaryOps.h create mode 100644 examples/ThirdPartyLibs/Eigen/src/plugins/MatrixCwiseUnaryOps.h create mode 100644 examples/pybullet/examples/pdControllerStable.py diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index 02dcf3819..a6c2f4dc3 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -202,15 +202,14 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; break; case btMultibodyLink::eSpherical: - link.joint_type = SPHERICAL; link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0]; link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1]; link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; // random unit vector link.body_axis_of_motion(0) = 0; - link.body_axis_of_motion(1) = 0; - link.body_axis_of_motion(2) = 1; + link.body_axis_of_motion(1) = 1; + link.body_axis_of_motion(2) = 0; break; case btMultibodyLink::ePlanar: bt_id_error_message("planar joints not implemented\n"); diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index d5f1e02fe..b57b05a82 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -27,7 +27,7 @@ project "App_BulletExampleBrowser" initOpenCL("clew") end - links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"} + links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","rbdl_static","LinearMath","BussIK", "Bullet3Common"} initOpenGL() initGlew() @@ -133,6 +133,17 @@ project "App_BulletExampleBrowser" "../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", "../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", "../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", + "../SharedMemory/plugins/stablePDPlugin/SpAlg.cpp", + "../SharedMemory/plugins/stablePDPlugin/Shape.cpp", + "../SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp", + "../SharedMemory/plugins/stablePDPlugin/RBDModel.cpp", + "../SharedMemory/plugins/stablePDPlugin/Rand.cpp", + "../SharedMemory/plugins/stablePDPlugin/MathUtil.cpp", + "../SharedMemory/plugins/stablePDPlugin/KinTree.cpp", + "../SharedMemory/plugins/stablePDPlugin/FileUtil.cpp", + "../SharedMemory/plugins/stablePDPlugin/json/json_writer.cpp", + "../SharedMemory/plugins/stablePDPlugin/json/json_value.cpp", + "../SharedMemory/plugins/stablePDPlugin/json/json_reader.cpp", "../SharedMemory/SharedMemoryCommands.h", "../SharedMemory/SharedMemoryPublic.h", "../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", @@ -169,6 +180,7 @@ project "App_BulletExampleBrowser" "../VoronoiFracture/*", "../SoftDemo/*", "../RollingFrictionDemo/*", + "../rbdl/*", "../FractureDemo/*", "../DynamicControlDemo/*", "../Constraints/*", diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index bf2e74ed2..034ca3d6f 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1353,7 +1353,9 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, if (linkPtr) { m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO); + + int uid3 = m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO); + colObj->setUserIndex3(uid3); } } } diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index bde786ffe..8caf7320c 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -113,6 +113,7 @@ void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCac } } + void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex) { cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex; @@ -126,7 +127,7 @@ void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedDat } } -void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache) +void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int flags) { //compute the number of links, and compute parent indices array (and possibly other cached data?) cache.m_totalNumJoints1 = 0; @@ -143,7 +144,24 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase); cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1 - ComputeParentIndices(u2b, cache, rootLinkIndex, -2); + + bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER)!=0; + if (maintainLinkOrder) + { + URDF2BulletCachedData cache2 = cache; + + ComputeParentIndices(u2b, cache2, rootLinkIndex, -2); + + for (int j=0;j& allIndices) +{ + childParentIndex cp; + cp.m_index = urdfLinkIndex; + int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex); + cp.m_mbIndex = mbIndex; + cp.m_parentIndex = parentIndex; + int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1; + cp.m_parentMBIndex = parentMbIndex; + + allIndices.push_back(cp); + btAlignedObjectArray urdfChildIndices; + u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices); + int numChildren = urdfChildIndices.size(); for (int i = 0; i < numChildren; i++) { int urdfChildLinkIndex = urdfChildIndices[i]; - - ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut); + GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices); } } + + +bool MyIntCompareFunc(childParentIndex a, childParentIndex b) +{ + return (a.m_index < b.m_index); +} + + void ConvertURDF2Bullet( const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, @@ -695,13 +755,49 @@ void ConvertURDF2Bullet( bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes) { URDF2BulletCachedData cache; - InitURDF2BulletCache(u2b, cache); + InitURDF2BulletCache(u2b, cache, flags); int urdfLinkIndex = u2b.getRootLinkIndex(); + int rootIndex = u2b.getRootLinkIndex(); B3_PROFILE("ConvertURDF2Bullet"); UrdfVisualShapeCache cachedLinkGraphicsShapesOut; - ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut); + + bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER)==0; + if (recursive) + { + ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive); + } + else + { + + btAlignedObjectArray parentTransforms; + if (urdfLinkIndex >= parentTransforms.size()) + { + parentTransforms.resize(urdfLinkIndex + 1); + } + parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace; + btAlignedObjectArray allIndices; + + GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices); + allIndices.quickSort(MyIntCompareFunc); + + for (int i = 0; i < allIndices.size(); i++) + { + int urdfLinkIndex = allIndices[i].m_index; + int parentIndex = allIndices[i].m_parentIndex; + btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace; + btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr , world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive); + if ((urdfLinkIndex+1) >= parentTransforms.size()) + { + parentTransforms.resize(urdfLinkIndex + 1); + } + parentTransforms[urdfLinkIndex] = tr; + } + + + + } if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size()) { *cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index cbbd58b58..233091c1e 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -13,28 +13,7 @@ class btTransform; class URDFImporterInterface; class MultiBodyCreationInterface; -//manually sync with eURDF_Flags in SharedMemoryPublic.h! -enum ConvertURDFFlags -{ - CUF_USE_SDF = 1, - // Use inertia values in URDF instead of recomputing them from collision shape. - CUF_USE_URDF_INERTIA = 2, - CUF_USE_MJCF = 4, - CUF_USE_SELF_COLLISION = 8, - CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16, - CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32, - CUF_RESERVED = 64, - CUF_USE_IMPLICIT_CYLINDER = 128, - CUF_GLOBAL_VELOCITIES_MB = 256, - CUF_MJCF_COLORS_FROM_FILE = 512, - CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, - CUF_ENABLE_SLEEPING = 2048, - CUF_INITIALIZE_SAT_FEATURES = 4096, - CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, - CUF_PARSE_SENSORS = 16384, - CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768, - CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738, -}; + struct UrdfVisualShapeCache { diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 4b03c86af..23a69a6c6 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -81,6 +81,11 @@ public: return 0; } + virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const + { + return 0; + } + virtual int getNumAllocatedCollisionShapes() const { return 0; } virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/) { return 0; } virtual int getNumModels() const { return 0; } diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 67e5affca..ed3124272 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -2,6 +2,7 @@ #define URDF_JOINT_TYPES_H #include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" enum UrdfJointTypes { @@ -76,4 +77,28 @@ struct UrdfMaterialColor } }; +//manually sync with eURDF_Flags in SharedMemoryPublic.h! +enum ConvertURDFFlags +{ + CUF_USE_SDF = 1, + // Use inertia values in URDF instead of recomputing them from collision shape. + CUF_USE_URDF_INERTIA = 2, + CUF_USE_MJCF = 4, + CUF_USE_SELF_COLLISION = 8, + CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16, + CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32, + CUF_RESERVED = 64, + CUF_USE_IMPLICIT_CYLINDER = 128, + CUF_GLOBAL_VELOCITIES_MB = 256, + CUF_MJCF_COLORS_FROM_FILE = 512, + CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, + CUF_ENABLE_SLEEPING = 2048, + CUF_INITIALIZE_SAT_FEATURES = 4096, + CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, + CUF_PARSE_SENSORS = 16384, + CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768, + CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536, + CUF_MAINTAIN_LINK_ORDER = 131072, +}; + #endif //URDF_JOINT_TYPES_H diff --git a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h index f84e6f551..7e01cffc7 100644 --- a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h +++ b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h @@ -15,7 +15,7 @@ struct UrdfRenderingInterface virtual ~UrdfRenderingInterface() {} ///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc) ///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape. - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0; + virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0; ///remove a visual shapes, based on the shape unique id (shapeUid) virtual void removeVisualShape(int collisionObjectUid) = 0; diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index d3176cfe5..7293cc5b6 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -57,6 +57,7 @@ struct SimpleInternalData int m_droidRegular; int m_droidRegular2; + int m_textureId; const char* m_frameDumpPngFileName; FILE* m_ffmpegFile; @@ -74,6 +75,7 @@ struct SimpleInternalData m_renderCallbacks2(0), m_droidRegular(0), m_droidRegular2(0), + m_textureId(-1), m_frameDumpPngFileName(0), m_ffmpegFile(0), m_renderTexture(0), @@ -772,6 +774,51 @@ void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], f int SimpleOpenGL3App::registerGraphicsUnitSphereShape(EnumSphereLevelOfDetail lod, int textureId) { + + int red = 255; + int green = 0; + int blue = 128; + if (textureId<0) + { + if (m_data->m_textureId < 0) + { + int texWidth = 1024; + int texHeight = 1024; + b3AlignedObjectArray texels; + texels.resize(texWidth * texHeight * 3); + for (int i = 0; i < texWidth * texHeight * 3; i++) + texels[i] = 255; + + for (int i = 0; i < texWidth; i++) + { + for (int j = 0; j < texHeight; j++) + { + int a = i < texWidth / 2 ? 1 : 0; + int b = j < texWidth / 2 ? 1 : 0; + + if (a == b) + { + texels[(i + j * texWidth) * 3 + 0] = red; + texels[(i + j * texWidth) * 3 + 1] = green; + texels[(i + j * texWidth) * 3 + 2] = blue; + // texels[(i+j*texWidth)*4+3] = 255; + } + /*else + { + texels[i*3+0+j*texWidth] = 255; + texels[i*3+1+j*texWidth] = 255; + texels[i*3+2+j*texWidth] = 255; + } + */ + } + } + + m_data->m_textureId = m_instancingRenderer->registerTexture(&texels[0], texWidth, texHeight); + + } + textureId = m_data->m_textureId; + } + int strideInBytes = 9 * sizeof(float); int graphicsShapeIndex = -1; @@ -795,17 +842,17 @@ int SimpleOpenGL3App::registerGraphicsUnitSphereShape(EnumSphereLevelOfDetail lo } case SPHERE_LOD_MEDIUM: { - int numVertices = sizeof(medium_sphere_vertices) / strideInBytes; - int numIndices = sizeof(medium_sphere_indices) / sizeof(int); - graphicsShapeIndex = m_instancingRenderer->registerShape(&medium_sphere_vertices[0], numVertices, medium_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId); + int numVertices = sizeof(textured_detailed_sphere_vertices) / strideInBytes; + int numIndices = sizeof(textured_detailed_sphere_indices) / sizeof(int); + graphicsShapeIndex = m_instancingRenderer->registerShape(&textured_detailed_sphere_vertices[0], numVertices, textured_detailed_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId); break; } case SPHERE_LOD_HIGH: default: { - int numVertices = sizeof(detailed_sphere_vertices) / strideInBytes; - int numIndices = sizeof(detailed_sphere_indices) / sizeof(int); - graphicsShapeIndex = m_instancingRenderer->registerShape(&detailed_sphere_vertices[0], numVertices, detailed_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId); + int numVertices = sizeof(textured_detailed_sphere_vertices) / strideInBytes; + int numIndices = sizeof(textured_detailed_sphere_indices) / sizeof(int); + graphicsShapeIndex = m_instancingRenderer->registerShape(&textured_detailed_sphere_vertices[0], numVertices, textured_detailed_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId); break; } }; diff --git a/examples/OpenGLWindow/Win32Window.cpp b/examples/OpenGLWindow/Win32Window.cpp index 55d2c63ca..38f1d5e48 100644 --- a/examples/OpenGLWindow/Win32Window.cpp +++ b/examples/OpenGLWindow/Win32Window.cpp @@ -530,11 +530,19 @@ void Win32Window::setWindowTitle(const char* titleChar) { #ifdef _WIN64 SetWindowTextA(m_data->m_hWnd, titleChar); +#else +#ifdef UNICODE + DWORD dwResult; + SendMessageTimeoutA(m_data->m_hWnd, WM_SETTEXT, 0, + reinterpret_cast(titleChar), + SMTO_ABORTIFHUNG, 2000, &dwResult); #else DWORD dwResult; SendMessageTimeout(m_data->m_hWnd, WM_SETTEXT, 0, - reinterpret_cast(titleChar), - SMTO_ABORTIFHUNG, 2000, &dwResult); + reinterpret_cast(titleChar), + SMTO_ABORTIFHUNG, 2000, &dwResult); + +#endif #endif } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 902c543e4..f41d37254 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -903,6 +903,22 @@ B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommand return 0; } +B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4) + { + for (int dof = 0; dof < dofCount; dof++) + { + command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex+dof] = forces[dof]; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + } + } + return 0; +} + B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; @@ -3459,6 +3475,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsC { command->m_userDebugDrawArgs.m_text[0] = 0; } + command->m_userDebugDrawArgs.m_rangeMin = rangeMin; command->m_userDebugDrawArgs.m_rangeMax = rangeMax; command->m_userDebugDrawArgs.m_startValue = startValue; @@ -3477,6 +3494,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics command->m_type = CMD_USER_DEBUG_DRAW; command->m_updateFlags = USER_DEBUG_READ_PARAMETER; command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId; + command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1; return (b3SharedMemoryCommandHandle)command; } @@ -3505,6 +3523,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsCli command->m_type = CMD_USER_DEBUG_DRAW; command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM; command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId; + command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1; return (b3SharedMemoryCommandHandle)command; } @@ -3517,6 +3536,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3Physics b3Assert(command); command->m_type = CMD_USER_DEBUG_DRAW; command->m_updateFlags = USER_DEBUG_REMOVE_ALL; + command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1; return (b3SharedMemoryCommandHandle)command; } @@ -3539,6 +3559,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsCli b3Assert(command); command->m_type = CMD_USER_DEBUG_DRAW; command->m_updateFlags = 0; + command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1; return (b3SharedMemoryCommandHandle)command; } @@ -4377,7 +4398,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit( command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS; command->m_updateFlags = 0; command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId; - + command->m_calculateInverseDynamicsArguments.m_flags = 0; + int dofCount = b3ComputeDofCount(physClient, bodyUniqueId); for (int i = 0; i < dofCount; i++) @@ -4405,6 +4427,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2 command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS; command->m_updateFlags = 0; command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId; + command->m_calculateInverseDynamicsArguments.m_flags = 0; command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCountQ; for (int i = 0; i < dofCountQ; i++) @@ -4422,6 +4445,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2 return (b3SharedMemoryCommandHandle)command; } +B3_SHARED_API void b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + command->m_calculateInverseDynamicsArguments.m_flags = flags; +} + B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, @@ -4511,8 +4540,13 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i return true; } +B3_SHARED_API void b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + command->m_calculateMassMatrixArguments.m_flags = flags; +} -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ) +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -4522,11 +4556,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy command->m_type = CMD_CALCULATE_MASS_MATRIX; command->m_updateFlags = 0; - int numJoints = cl->getNumJoints(bodyUniqueId); - for (int i = 0; i < numJoints; i++) + + for (int i = 0; i < dofCountQ; i++) { command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; } + command->m_calculateMassMatrixArguments.m_bodyUniqueId = bodyUniqueId; + command->m_calculateMassMatrixArguments.m_dofCountQ = dofCountQ; + command->m_calculateMassMatrixArguments.m_flags = 0; return (b3SharedMemoryCommandHandle)command; } @@ -5360,3 +5397,63 @@ B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], cons outOrn[3] = dorn[3]; } +b3Scalar b3GetMatrixElem(const b3Matrix3x3& mat, int index) +{ + int i = index % 3; + int j = index / 3; + return mat[i][j]; +} + + +static bool MyMatrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz) +{ + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + + b3Scalar fi = b3GetMatrixElem(mat, 2); + if (fi < b3Scalar(1.0f)) + { + if (fi > b3Scalar(-1.0f)) + { + xyz[0] = b3Atan2(-b3GetMatrixElem(mat, 5), b3GetMatrixElem(mat, 8)); + xyz[1] = b3Asin(b3GetMatrixElem(mat, 2)); + xyz[2] = b3Atan2(-b3GetMatrixElem(mat, 1), b3GetMatrixElem(mat, 0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4)); + xyz[1] = -B3_HALF_PI; + xyz[2] = b3Scalar(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4)); + xyz[1] = B3_HALF_PI; + xyz[2] = 0.0; + } + return false; +} + + +B3_SHARED_API void b3GetAxisDifferenceQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double axisOut[/*3*/]) +{ + b3Quaternion currentQuat(startQuat[0], startQuat[1], startQuat[2], startQuat[3]); + b3Quaternion desiredQuat(endQuat[0], endQuat[1], endQuat[2], endQuat[3]); + + b3Quaternion relRot = currentQuat.inverse() * desiredQuat; + b3Vector3 angleDiff; + MyMatrixToEulerXYZ(b3Matrix3x3(relRot), angleDiff); + axisOut[0] = angleDiff[0]; + axisOut[1] = angleDiff[1]; + axisOut[2] = angleDiff[2]; + +} + + + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a09ebfcd1..28fb2d7c9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -386,6 +386,7 @@ extern "C" const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot); + B3_SHARED_API void b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, @@ -398,7 +399,8 @@ extern "C" double* linearJacobian, double* angularJacobian); - B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ); + B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ); + B3_SHARED_API void b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); ///the mass matrix is stored in column-major layout of size dofCount*dofCount B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix); @@ -452,7 +454,10 @@ extern "C" ///Only use when controlMode is CONTROL_MODE_VELOCITY B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount); + B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); + B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount); + ///Only use if when controlMode is CONTROL_MODE_TORQUE, B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); @@ -641,6 +646,7 @@ extern "C" B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/]); B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle); B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]); + B3_SHARED_API void b3GetAxisDifferenceQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double axisOut[/*3*/]); B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]); B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c29d6265c..68e3492b3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -67,6 +67,13 @@ #include "plugins/pdControlPlugin/pdControlPlugin.h" #endif //SKIP_STATIC_PD_CONTROL_PLUGIN + +#ifdef STATIC_LINK_SPD_PLUGIN +#include "plugins/stablePDPlugin/BulletConversion.h" +#include "plugins/stablePDPlugin/RBDModel.h" +#include "plugins/stablePDPlugin/RBDutil.h" +#endif + #ifdef STATIC_LINK_VR_PLUGIN #include "plugins/vrSyncPlugin/vrSyncPlugin.h" #endif @@ -1790,6 +1797,143 @@ struct PhysicsServerCommandProcessorInternalData m_userVisualShapeHandles.initHandles(); } +#ifdef STATIC_LINK_SPD_PLUGIN + b3HashMap m_rbdModels; + + cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot) + { + cRBDModel* rbdModel = 0; + cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody); + if (rbdModelPtr) + { + rbdModel = *rbdModelPtr; + } + else + { + rbdModel = new cRBDModel(); + Eigen::MatrixXd bodyDefs; + Eigen::MatrixXd jointMat; + btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat); + btVector3 grav = m_dynamicsWorld->getGravity(); + tVector3 gravity(grav[0], grav[1], grav[2], 0); + rbdModel->Init(jointMat, bodyDefs, gravity); + m_rbdModels.insert(multiBody, rbdModel); + } + + //sync pose and vel + + int baseDofQ = multiBody->hasFixedBase() ? 0 : 7; + int baseDofQdot = multiBody->hasFixedBase() ? 0 : 6; + + Eigen::VectorXd pose, vel; + pose.resize(7 + multiBody->getNumPosVars()); + vel.resize(7 + multiBody->getNumPosVars()); //?? + + btTransform tr = multiBody->getBaseWorldTransform(); + int dofsrc = 0; + int velsrcdof = 0; + if (baseDofQ == 7) + { + pose[0] = jointPositionsQ[dofsrc++]; + pose[1] = jointPositionsQ[dofsrc++]; + pose[2] = jointPositionsQ[dofsrc++]; + + double quatXYZW[4]; + quatXYZW[0] = jointPositionsQ[dofsrc++]; + quatXYZW[1] = jointPositionsQ[dofsrc++]; + quatXYZW[2] = jointPositionsQ[dofsrc++]; + quatXYZW[3] = jointPositionsQ[dofsrc++]; + + pose[3] = quatXYZW[3]; + pose[4] = quatXYZW[0]; + pose[5] = quatXYZW[1]; + pose[6] = quatXYZW[2]; + } + else + { + pose[0] = tr.getOrigin()[0]; + pose[1] = tr.getOrigin()[1]; + pose[2] = tr.getOrigin()[2]; + pose[3] = tr.getRotation()[3]; + pose[4] = tr.getRotation()[0]; + pose[5] = tr.getRotation()[1]; + pose[6] = tr.getRotation()[2]; + } + if (baseDofQdot == 6) + { + vel[0] = jointVelocitiesQdot[velsrcdof++]; + vel[1] = jointVelocitiesQdot[velsrcdof++]; + vel[2] = jointVelocitiesQdot[velsrcdof++]; + vel[3] = jointVelocitiesQdot[velsrcdof++]; + vel[4] = jointVelocitiesQdot[velsrcdof++]; + vel[5] = jointVelocitiesQdot[velsrcdof++]; + vel[6] = jointVelocitiesQdot[velsrcdof++]; + vel[6] = 0; + } + else + { + vel[0] = multiBody->getBaseVel()[0]; + vel[1] = multiBody->getBaseVel()[1]; + vel[2] = multiBody->getBaseVel()[2]; + vel[3] = multiBody->getBaseOmega()[0]; + vel[4] = multiBody->getBaseOmega()[1]; + vel[5] = multiBody->getBaseOmega()[2]; + vel[6] = 0; + } + int dof = 7; + int veldof = 7; + + + for (int l = 0; l < multiBody->getNumLinks(); l++) + { + switch (multiBody->getLink(l).m_jointType) + { + case btMultibodyLink::eRevolute: + case btMultibodyLink::ePrismatic: + { + pose[dof++] = jointPositionsQ[dofsrc++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + break; + } + case btMultibodyLink::eSpherical: + { + double quatXYZW[4]; + quatXYZW[0] = jointPositionsQ[dofsrc++]; + quatXYZW[1] = jointPositionsQ[dofsrc++]; + quatXYZW[2] = jointPositionsQ[dofsrc++]; + quatXYZW[3] = jointPositionsQ[dofsrc++]; + + pose[dof++] = quatXYZW[3]; + pose[dof++] = quatXYZW[0]; + pose[dof++] = quatXYZW[1]; + pose[dof++] = quatXYZW[2]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + break; + } + case btMultibodyLink::eFixed: + { + break; + } + default: + { + assert(0); + } + } + } + + btVector3 gravOrg = m_dynamicsWorld->getGravity(); + tVector grav (gravOrg[0], gravOrg[1], gravOrg[2], 0); + rbdModel->SetGravity(grav); + rbdModel->Update(pose, vel); + + return rbdModel; + } + +#endif + btInverseDynamics::MultiBodyTree* findOrCreateTree(btMultiBody* multiBody) { btInverseDynamics::MultiBodyTree* tree = 0; @@ -3703,7 +3847,7 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc for (int i = 0; i < m_data->m_dynamicsWorld->getNumCollisionObjects(); i++) { const btCollisionObject* colObj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; - m_data->m_pluginManager.getRenderInterface()->syncTransform(colObj->getBroadphaseHandle()->getUid(), colObj->getWorldTransform(), colObj->getCollisionShape()->getLocalScaling()); + m_data->m_pluginManager.getRenderInterface()->syncTransform(colObj->getUserIndex3(), colObj->getWorldTransform(), colObj->getCollisionShape()->getLocalScaling()); } if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0) @@ -8142,9 +8286,10 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe int posVarCountIndex = 7; for (int i = 0; i < mb->getNumLinks(); i++) { - bool hasPosVar = true; - - for (int j = 0; j < mb->getLink(i).m_posVarCount; j++) + int posVarCount = mb->getLink(i).m_posVarCount; + bool hasPosVar = posVarCount > 0; + + for (int j = 0; j < posVarCount; j++) { if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0) { @@ -8154,7 +8299,13 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe } if (hasPosVar) { - mb->setJointPosMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]); + btQuaternion q( + clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex], + clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+1], + clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+2], + clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+3]); + q.normalize(); + mb->setJointPosMultiDof(i, &q[0]); double vel[6] = { 0, 0, 0, 0, 0, 0 }; mb->setJointVelMultiDof(i, vel); } @@ -8540,82 +8691,111 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; if (bodyHandle && bodyHandle->m_multiBody) { - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7; - int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); - - if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ+ num_dofs) && - clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot+ num_dofs)) + if (clientCmd.m_calculateInverseDynamicsArguments.m_flags & 1) { - - btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot); - - //for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order - //PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API - if (baseDofQ) +#ifdef STATIC_LINK_SPD_PLUGIN { - btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0], - clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1], - clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]); - - btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0], - clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1], - clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2], - clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]); - btScalar yawZ, pitchY, rollX; - orn.getEulerZYX(yawZ, pitchY, rollX); - q[0] = rollX; - q[1] = pitchY; - q[2] = yawZ; - q[3] = pos[0]; - q[4] = pos[1]; - q[5] = pos[2]; - } - else - { - for (int i = 0; i < num_dofs; i++) + cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody, + clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ, + clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot); + if (rbdModel) { - q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; + int posVal = bodyHandle->m_multiBody->getNumPosVars(); + + Eigen::VectorXd acc2 = Eigen::VectorXd::Zero(7 + posVal); + Eigen::VectorXd out_tau = Eigen::VectorXd::Zero(7 + posVal); + cRBDUtil::SolveInvDyna(*rbdModel, acc2, out_tau); + int dof = 7 + bodyHandle->m_multiBody->getNumPosVars(); + for (int i = 0; i < dof; i++) + { + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = out_tau[i]; + } + serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof; + + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; + } } - for (int i = 0; i < num_dofs + baseDofQdot; i++) +#endif + } else + { + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7; + int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); + + if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ + num_dofs) && + clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot + num_dofs)) { - qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; - nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; - } - // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot); - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs+ baseDofQdot; - - //inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API. - if (baseDofQdot) + //for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order + //PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API + if (baseDofQ) { - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3]; - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4]; - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5]; - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0]; - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1]; - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2]; + btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0], + clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1], + clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]); + + btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0], + clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1], + clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2], + clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]); + btScalar yawZ, pitchY, rollX; + orn.getEulerZYX(yawZ, pitchY, rollX); + q[0] = rollX; + q[1] = pitchY; + q[2] = yawZ; + q[3] = pos[0]; + q[4] = pos[1]; + q[5] = pos[2]; } - - for (int i = baseDofQdot; i < num_dofs+ baseDofQdot; i++) + else { - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i]; + for (int i = 0; i < num_dofs; i++) + { + q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; + } + } + for (int i = 0; i < num_dofs + baseDofQdot; i++) + { + qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; + nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; + } + + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs + baseDofQdot; + + //inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API. + if (baseDofQdot) + { + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3]; + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4]; + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5]; + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0]; + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1]; + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2]; + } + + for (int i = baseDofQdot; i < num_dofs + baseDofQdot; i++) + { + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i]; + } + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; } - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; - } - else - { - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; } } } @@ -8732,37 +8912,82 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) + if (clientCmd.m_calculateMassMatrixArguments.m_flags & 1) { - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - const int totDofs = numDofs + baseDofs; - btInverseDynamics::vecx q(totDofs); - btInverseDynamics::matxx massMatrix(totDofs, totDofs); - for (int i = 0; i < numDofs; i++) +#ifdef STATIC_LINK_SPD_PLUGIN { - q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i]; - } - if (-1 != tree->calculateMassMatrix(q, &massMatrix)) - { - serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs; - // Fill in the result into the shared memory. - double* sharedBuf = (double*)bufferServerToClient; - int sizeInBytes = totDofs * totDofs * sizeof(double); - if (sizeInBytes < bufferSizeInBytes) + int posVal = bodyHandle->m_multiBody->getNumPosVars(); + btAlignedObjectArray zeroVel; + int dof = 7 + posVal; + zeroVel.resize(dof); + cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody, clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ, + &zeroVel[0]); + if (rbdModel) { - for (int i = 0; i < (totDofs); ++i) + Eigen::MatrixXd out_mass; + cRBDUtil::BuildMassMat(*rbdModel, out_mass); + int skipDofs = 0;// 7 - baseDofQ; + int totDofs = dof; + serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs-skipDofs; + // Fill in the result into the shared memory. + double* sharedBuf = (double*)bufferServerToClient; + int sizeInBytes = totDofs * totDofs * sizeof(double); + if (sizeInBytes < bufferSizeInBytes) { - for (int j = 0; j < (totDofs); ++j) + for (int i = skipDofs; i < (totDofs); ++i) { - int element = (totDofs)*i + j; - - sharedBuf[element] = massMatrix(i, j); + for (int j = skipDofs; j < (totDofs); ++j) + { + int element = (totDofs- skipDofs)*(i- skipDofs) + (j- skipDofs); + double v = out_mass(i, j); + if (i == j && v == 0) + { + v = 1; + } + sharedBuf[element] = v; + } } + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; + } + } + } +#endif + } + else + { + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + const int totDofs = numDofs + baseDofs; + btInverseDynamics::vecx q(totDofs); + btInverseDynamics::matxx massMatrix(totDofs, totDofs); + for (int i = 0; i < numDofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i]; + } + if (-1 != tree->calculateMassMatrix(q, &massMatrix)) + { + serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs; + // Fill in the result into the shared memory. + double* sharedBuf = (double*)bufferServerToClient; + int sizeInBytes = totDofs * totDofs * sizeof(double); + if (sizeInBytes < bufferSizeInBytes) + { + for (int i = 0; i < (totDofs); ++i) + { + for (int j = 0; j < (totDofs); ++j) + { + int element = (totDofs)*i + j; + + sharedBuf[element] = massMatrix(i, j); + } + } + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; } - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; } } } @@ -9800,7 +10025,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff); } - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId; + serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; for (int i = 0; i < numDofs; i++) { serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i]; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index eacc049a6..2737ac30f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -663,6 +663,7 @@ struct CalculateInverseDynamicsArgs double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM]; double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM]; + int m_flags; }; struct CalculateInverseDynamicsResultArgs @@ -693,6 +694,8 @@ struct CalculateMassMatrixArgs { int m_bodyUniqueId; double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; + int m_dofCountQ; + int m_flags; }; struct CalculateMassMatrixResultArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 17cef03ce..8d59ddc91 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -817,6 +817,7 @@ enum eCONNECT_METHOD eCONNECT_DART = 10, eCONNECT_MUJOCO = 11, eCONNECT_GRPC = 12, + eCONNECT_PHYSX=13, }; enum eURDF_Flags @@ -836,6 +837,7 @@ enum eURDF_Flags URDF_PARSE_SENSORS = 16384, URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768, URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536, + URDF_MAINTAIN_LINK_ORDER = 131072, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/SharedMemory/physx/PhysXC_API.cpp b/examples/SharedMemory/physx/PhysXC_API.cpp new file mode 100644 index 000000000..bd32027cf --- /dev/null +++ b/examples/SharedMemory/physx/PhysXC_API.cpp @@ -0,0 +1,15 @@ +#ifdef BT_ENABLE_PHYSX +#include "PhysXC_API.h" +#include "PhysXServerCommandProcessor.h" +#include "PhysXClient.h" + +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX() +{ + PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor; + + PhysXClient* direct = new PhysXClient(sdk, true); + bool connected; + connected = direct->connect(); + return (b3PhysicsClientHandle)direct; +} +#endif //BT_ENABLE_PHYSX \ No newline at end of file diff --git a/examples/SharedMemory/physx/PhysXC_API.h b/examples/SharedMemory/physx/PhysXC_API.h new file mode 100644 index 000000000..54ad3e1a6 --- /dev/null +++ b/examples/SharedMemory/physx/PhysXC_API.h @@ -0,0 +1,20 @@ +#ifndef PHYSX_C_API_H +#define PHYSX_C_API_H + +#ifdef BT_ENABLE_PHYSX + +#include "../PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + + B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX(); + +#ifdef __cplusplus +} +#endif + +#endif //BT_ENABLE_PHYSX +#endif //PHYSX_C_API_H diff --git a/examples/SharedMemory/physx/PhysXClient.cpp b/examples/SharedMemory/physx/PhysXClient.cpp new file mode 100644 index 000000000..611c53125 --- /dev/null +++ b/examples/SharedMemory/physx/PhysXClient.cpp @@ -0,0 +1,1409 @@ +#ifdef BT_ENABLE_PHYSX +#include "PhysXClient.h" + +#include "../PhysicsClientSharedMemory.h" +#include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemoryCommands.h" +#include "../PhysicsCommandProcessorInterface.h" +#include "../../Utils/b3Clock.h" + +#include "LinearMath/btHashMap.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../../../Extras/Serialize/BulletFileLoader/btBulletFile.h" +#include "../../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +#include "../BodyJointInfoUtility.h" +#include + +#include "../SharedMemoryUserData.h" +#include "LinearMath/btQuickprof.h" + +struct PhysXUserDataCache +{ + btHashMap m_userDataMap; + btHashMap m_keyToUserDataIdMap; + + ~PhysXUserDataCache() + { + } +}; + +struct BodyJointInfoCache2 +{ + std::string m_baseName; + btAlignedObjectArray m_jointInfo; + std::string m_bodyName; + + // Joint index -> user data. + btHashMap m_jointToUserDataMap; + + ~BodyJointInfoCache2() + { + } +}; + +struct PhysXDirectInternalData +{ + DummyGUIHelper m_noGfx; + + btAlignedObjectArray m_serverDNA; + SharedMemoryCommand m_command; + SharedMemoryStatus m_serverStatus; + + SharedMemoryCommand m_tmpInfoRequestCommand; + SharedMemoryStatus m_tmpInfoStatus; + bool m_hasStatus; + bool m_verboseOutput; + + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + btHashMap m_bodyJointMap; + btHashMap m_userConstraintInfoMap; + + btAlignedObjectArray m_profileTimings; + btHashMap m_profileTimingStringArray; + + char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + btAlignedObjectArray m_cachedMassMatrix; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMask; + + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_cachedOverlappingObjects; + + btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedCollisionShapes; + + btAlignedObjectArray m_cachedVREvents; + + btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + + btAlignedObjectArray m_raycastHits; + + PhysicsCommandProcessorInterface* m_commandProcessor; + bool m_ownsCommandProcessor; + double m_timeOutInSeconds; + + PhysXDirectInternalData() + : m_hasStatus(false), + m_verboseOutput(false), + m_cachedCameraPixelsWidth(0), + m_cachedCameraPixelsHeight(0), + m_commandProcessor(NULL), + m_ownsCommandProcessor(false), + m_timeOutInSeconds(1e30) + { + memset(&m_command, 0, sizeof(m_command)); + memset(&m_serverStatus, 0, sizeof(m_serverStatus)); + memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); + } +}; + +PhysXClient::PhysXClient(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) +{ + int sz = sizeof(SharedMemoryCommand); + int sz2 = sizeof(SharedMemoryStatus); + + m_data = new PhysXDirectInternalData; + m_data->m_commandProcessor = physSdk; + m_data->m_ownsCommandProcessor = passSdkOwnership; +} + +PhysXClient::~PhysXClient() +{ + for (int i = 0; i < m_data->m_profileTimingStringArray.size(); i++) + { + std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i); + if (str) + { + delete *str; + } + } + m_data->m_profileTimingStringArray.clear(); + + if (m_data->m_commandProcessor->isConnected()) + { + m_data->m_commandProcessor->disconnect(); + } + if (m_data->m_ownsCommandProcessor) + { + delete m_data->m_commandProcessor; + } + + resetData(); + + delete m_data; +} + +void PhysXClient::resetData() +{ + m_data->m_debugLinesFrom.clear(); + m_data->m_debugLinesTo.clear(); + m_data->m_debugLinesColor.clear(); + for (int i = 0; i < m_data->m_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + } + } + m_data->m_bodyJointMap.clear(); + m_data->m_userConstraintInfoMap.clear(); +} + +// return true if connection succesfull, can also check 'isConnected' +bool PhysXClient::connect() +{ + bool connected = m_data->m_commandProcessor->connect(); + m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + + if (connected) + //also request serialization data + { + SharedMemoryCommand command; + command.m_type = CMD_REQUEST_INTERNAL_DATA; + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + } + else + { + b3Clock clock; + double timeSec = clock.getTimeInSeconds(); + + while ((!hasStatus) && (clock.getTimeInSeconds() - timeSec < 10)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + } + } + + return connected; +} + +// return true if connection succesfull, can also check 'isConnected' +bool PhysXClient::connect(struct GUIHelperInterface* guiHelper) +{ + bool connected = m_data->m_commandProcessor->connect(); + + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return connected; +} + +void PhysXClient::renderScene() +{ + int renderFlags = 0; + m_data->m_commandProcessor->renderScene(renderFlags); +} + +void PhysXClient::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + +////todo: rename to 'disconnect' +void PhysXClient::disconnectSharedMemory() +{ + m_data->m_commandProcessor->disconnect(); + m_data->m_commandProcessor->setGuiHelper(0); +} + +bool PhysXClient::isConnected() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* PhysXClient::processServerStatus() +{ + if (!m_data->m_hasStatus) + { + m_data->m_hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + SharedMemoryStatus* stat = 0; + if (m_data->m_hasStatus) + { + stat = &m_data->m_serverStatus; + + postProcessStatus(m_data->m_serverStatus); + + m_data->m_hasStatus = false; + } + return stat; +} + +SharedMemoryCommand* PhysXClient::getAvailableSharedMemoryCommand() +{ + return &m_data->m_command; +} + +bool PhysXClient::canSubmitCommand() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +bool PhysXClient::processDebugLines(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_DEBUG_LINES_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Success receiving %d debug lines", + serverCmd.m_sendDebugLinesArgs.m_numDebugLines); + } + + int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; + float* linesFrom = + (float*)&m_data->m_bulletStreamDataServerToClient[0]; + float* linesTo = + (float*)(&m_data->m_bulletStreamDataServerToClient[0] + + numLines * 3 * sizeof(float)); + float* linesColor = + (float*)(&m_data->m_bulletStreamDataServerToClient[0] + + 2 * numLines * 3 * sizeof(float)); + + m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesColor.resize( + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); + + for (int i = 0; i < numLines; i++) + { + TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1], + linesFrom[i * 3 + 2]); + TmpFloat3 to = + CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]); + TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1], + linesColor[i * 3 + 2]); + + m_data + ->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + from; + m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + to; + m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + i] = color; + } + + if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_startingLineIndex = + serverCmd.m_sendDebugLinesArgs.m_numDebugLines + + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; + } + } + + } while (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0); + + return m_data->m_hasStatus; +} + +bool PhysXClient::processVisualShapeData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Visual Shape Information Request OK\n"); + } + int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; + int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); + b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numVisualShapesCopied; i++) + { + m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; + } + + if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; + } + } + } while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied); + + return m_data->m_hasStatus; +} + +bool PhysXClient::processOverlappingObjects(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Overlapping Objects Request OK\n"); + } + + int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied); + b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i = 0; i < numOverlapCopied; i++) + { + m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i]; + } + + if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied) + { + m_data->m_hasStatus = false; + command.m_type = CMD_REQUEST_AABB_OVERLAP; + command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + } + } + } while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied); + + return m_data->m_hasStatus; +} + +bool PhysXClient::processContactPointData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex + numContactsCopied); + + b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i = 0; i < numContactsCopied; i++) + { + m_data->m_cachedContactPoints[startContactIndex + i] = contactData[i]; + } + + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + } + } + } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); + + return m_data->m_hasStatus; +} + +bool PhysXClient::processCamera(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Camera image OK\n"); + } + + int numBytesPerPixel = 4; //RGBA + int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied + + serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; + + m_data->m_cachedCameraPixelsWidth = 0; + m_data->m_cachedCameraPixelsHeight = 0; + + int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth * serverCmd.m_sendPixelDataArguments.m_imageHeight; + + m_data->m_cachedCameraPixelsRGBA.reserve(numPixels * numBytesPerPixel); + m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedSegmentationMask.resize(numTotalPixels); + m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels * numBytesPerPixel); + + unsigned char* rgbaPixelsReceived = + (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; + + float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 4]); + int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 8]); + + // printf("pixel = %d\n", rgbaPixelsReceived[0]); + + for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++) + { + m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++) + { + m_data->m_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; + } + for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * numBytesPerPixel; i++) + { + m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex * numBytesPerPixel] = rgbaPixelsReceived[i]; + } + + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) + { + m_data->m_hasStatus = false; + + // continue requesting remaining pixels + command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; + command.m_requestPixelDataArguments.m_startPixelIndex = + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; + } + else + { + m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; + m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; + } + } + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); + + return m_data->m_hasStatus; +} + +void PhysXClient::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + BodyJointInfoCache2** cachePtr = m_data->m_bodyJointMap[bodyUniqueId]; + //don't process same bodyUniqueId multiple times + if (cachePtr) + { + return; + } + + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_numDataStreamBytes); + if (m_data->m_serverDNA.size()) + { + bf.setFileDNA(false, &m_data->m_serverDNA[0], m_data->m_serverDNA.size()); + } + else + { + bf.setFileDNAisMemoryDNA(); + } + bf.parse(false); + + if (bf.ok()) + { + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); + } + else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); + } + } + + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } + else + { + b3Warning("Robot description not received"); + } +} + +void PhysXClient::processAddUserData(const struct SharedMemoryStatus& serverCmd) +{ + +} + +void PhysXClient::postProcessStatus(const struct SharedMemoryStatus& serverCmd) +{ + switch (serverCmd.m_type) + { + case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Raycast completed"); + } + m_data->m_raycastHits.clear(); + b3RayHitInfo* rayHits = (b3RayHitInfo*)m_data->m_bulletStreamDataServerToClient; + for (int i = 0; i < serverCmd.m_raycastHits.m_numRaycastHits; i++) + { + m_data->m_raycastHits.push_back(rayHits[i]); + } + break; + } + case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Request VR Events completed"); + } + m_data->m_cachedVREvents.resize(serverCmd.m_sendVREvents.m_numVRControllerEvents); + for (int i = 0; i < serverCmd.m_sendVREvents.m_numVRControllerEvents; i++) + { + m_data->m_cachedVREvents[i] = serverCmd.m_sendVREvents.m_controllerEvents[i]; + } + break; + } + case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Request keyboard events completed"); + } + m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents); + for (int i = 0; i < serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents; i++) + { + m_data->m_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i]; + } + break; + } + + case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: + { + B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Request mouse events completed"); + } + m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); + for (int i = 0; i < serverCmd.m_sendMouseEvents.m_numMouseEvents; i++) + { + m_data->m_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; + } + break; + } + + case CMD_REQUEST_INTERNAL_DATA_COMPLETED: + { + if (serverCmd.m_numDataStreamBytes) + { + int numStreamBytes = serverCmd.m_numDataStreamBytes; + m_data->m_serverDNA.resize(numStreamBytes); + for (int i = 0; i < numStreamBytes; i++) + { + m_data->m_serverDNA[i] = m_data->m_bulletStreamDataServerToClient[i]; + } + } + break; + } + case CMD_RESET_SIMULATION_COMPLETED: + { + resetData(); + break; + } + + case CMD_USER_CONSTRAINT_INFO_COMPLETED: + case CMD_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid, serverCmd.m_userConstraintResultArgs); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.remove(cid); + break; + } + case CMD_REMOVE_BODY_FAILED: + { + b3Warning("Remove body failed\n"); + break; + } + case CMD_REMOVE_BODY_COMPLETED: + { + for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numBodies; i++) + { + int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i]; + removeCachedBody(bodyUniqueId); + } + for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numUserConstraints; i++) + { + int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i]; + m_data->m_userConstraintInfoMap.remove(key); + } + + break; + } + case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid]; + if (userConstraintPtr) + { + const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs; + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0]; + userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1]; + userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3]; + userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4]; + userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5]; + userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_erp = serverConstraint->m_erp; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink; + } + } + break; + } + case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: + { + break; + } + case CMD_SYNC_BODY_INFO_COMPLETED: + case CMD_MJCF_LOADING_COMPLETED: + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints; + for (int i = 0; i < numConstraints; i++) + { + int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i]; + + m_data->m_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; + m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; + m_data->m_tmpInfoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + int cid = m_data->m_tmpInfoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid, m_data->m_tmpInfoStatus.m_userConstraintResultArgs); + } + } + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i = 0; i < numBodies; i++) + { + int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]; + + m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; + m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus); + } + } + break; + } + case CMD_CREATE_MULTI_BODY_COMPLETED: + case CMD_URDF_LOADING_COMPLETED: + { + if (serverCmd.m_numDataStreamBytes > 0) + { + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex, serverCmd); + } + break; + } + case CMD_BULLET_LOADING_FAILED: + { + b3Warning("Couldn't load .bullet file"); + break; + } + case CMD_BULLET_LOADING_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: + { + b3Warning("requestOpenGLVisualizeCamera failed"); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_FAILED: + { + b3Warning("removeConstraint failed"); + break; + } + case CMD_CHANGE_USER_CONSTRAINT_FAILED: + { + //b3Warning("changeConstraint failed"); + break; + } + + case CMD_USER_CONSTRAINT_FAILED: + { + b3Warning("createConstraint failed"); + break; + } + + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("createVisualShape failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("createMultiBody failed"); + break; + } + case CMD_REQUEST_COLLISION_INFO_COMPLETED: + { + break; + } + case CMD_REQUEST_COLLISION_INFO_FAILED: + { + b3Warning("Request getCollisionInfo failed"); + break; + } + + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } + case CMD_CUSTOM_COMMAND_FAILED: + { + b3Warning("custom plugin command failed"); + break; + } + case CMD_CLIENT_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_FAILED: + { + b3Warning("calculate mass matrix failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_COMPLETED: + { + double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0]; + m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount * serverCmd.m_massMatrixResultArgs.m_dofCount); + for (int i = 0; i < serverCmd.m_massMatrixResultArgs.m_dofCount * serverCmd.m_massMatrixResultArgs.m_dofCount; i++) + { + m_data->m_cachedMassMatrix[i] = matrixData[i]; + } + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } + case CMD_SAVE_STATE_COMPLETED: + { + break; + } + case CMD_COLLISION_SHAPE_INFO_FAILED: + { + b3Warning("getCollisionShapeData failed"); + break; + } + case CMD_COLLISION_SHAPE_INFO_COMPLETED: + { + B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Collision Shape Information Request OK\n"); + } + int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes; + m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied); + b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numCollisionShapesCopied; i++) + { + m_data->m_cachedCollisionShapes[i] = shapeData[i]; + } + break; + } + case CMD_RESTORE_STATE_FAILED: + { + b3Warning("restoreState failed"); + break; + } + case CMD_RESTORE_STATE_COMPLETED: + { + break; + } + case CMD_BULLET_SAVING_COMPLETED: + { + break; + } + case CMD_LOAD_SOFT_BODY_FAILED: + { + b3Warning("loadSoftBody failed"); + break; + } + case CMD_LOAD_SOFT_BODY_COMPLETED: + { + break; + } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_ADD_USER_DATA_FAILED: + { + b3Warning("Adding user data failed (do the specified body and link exist?)"); + break; + } + case CMD_REMOVE_USER_DATA_FAILED: + { + b3Warning("Removing user data failed"); + break; + } + case CMD_ADD_USER_DATA_COMPLETED: + { + processAddUserData(serverCmd); + break; + } + case CMD_SYNC_USER_DATA_COMPLETED: + { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + break; + } + case CMD_REMOVE_USER_DATA_COMPLETED: + { + break; + } + default: + { + //b3Warning("Unknown server status type"); + } + }; +} +bool PhysXClient::submitClientCommand(const struct SharedMemoryCommand& command) +{ + if (command.m_type == CMD_REQUEST_DEBUG_LINES) + { + return processDebugLines(command); + } + + if (command.m_type == CMD_REQUEST_CAMERA_IMAGE_DATA) + { + return processCamera(command); + } + if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) + { + return processContactPointData(command); + } + + if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) + { + return processVisualShapeData(command); + } + if (command.m_type == CMD_REQUEST_AABB_OVERLAP) + { + return processOverlappingObjects(command); + } + + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + /*if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + m_data->m_hasStatus = false; + } + */ + return hasStatus; +} + +int PhysXClient::getNumBodies() const +{ + return m_data->m_bodyJointMap.size(); +} + +void PhysXClient::removeCachedBody(int bodyUniqueId) +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + m_data->m_bodyJointMap.remove(bodyUniqueId); + } +} + +int PhysXClient::getNumUserConstraints() const +{ + return m_data->m_userConstraintInfoMap.size(); +} + +int PhysXClient::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const +{ + b3UserConstraint* constraintPtr = m_data->m_userConstraintInfoMap[constraintUniqueId]; + if (constraintPtr) + { + info = *constraintPtr; + return 1; + } + return 0; +} + +int PhysXClient::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +int PhysXClient::getBodyUniqueId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumBodies())) + { + return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +bool PhysXClient::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + strcpy(info.m_baseName, bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName, bodyJoints->m_bodyName.c_str()); + return true; + } + + return false; +} + +int PhysXClient::getNumJoints(int bodyIndex) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + return bodyJoints->m_jointInfo.size(); + } + btAssert(0); + return 0; +} + +bool PhysXClient::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size())) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } + } + return false; +} + +void PhysXClient::setSharedMemoryKey(int key) +{ +} + +void PhysXClient::uploadBulletFileToSharedMemory(const char* data, int len) +{ + if (len > SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + } + for (int i = 0; i < len; i++) + { + m_data->m_bulletStreamDataServerToClient[i] = data[i]; + } + //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +void PhysXClient::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) +{ + int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays; + int newNumRays = curNumStreamingRays + numRays; + btAssert(newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING); + + if (newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING) + { + for (int i = 0; i < numRays; i++) + { + b3RayData* rayDataStream = (b3RayData*)m_data->m_bulletStreamDataServerToClient; + rayDataStream[curNumStreamingRays + i].m_rayFromPosition[0] = rayFromWorldArray[i * 3 + 0]; + rayDataStream[curNumStreamingRays + i].m_rayFromPosition[1] = rayFromWorldArray[i * 3 + 1]; + rayDataStream[curNumStreamingRays + i].m_rayFromPosition[2] = rayFromWorldArray[i * 3 + 2]; + rayDataStream[curNumStreamingRays + i].m_rayToPosition[0] = rayToWorldArray[i * 3 + 0]; + rayDataStream[curNumStreamingRays + i].m_rayToPosition[1] = rayToWorldArray[i * 3 + 1]; + rayDataStream[curNumStreamingRays + i].m_rayToPosition[2] = rayToWorldArray[i * 3 + 2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; + } + } +} + +int PhysXClient::getNumDebugLines() const +{ + return m_data->m_debugLinesFrom.size(); +} + +const float* PhysXClient::getDebugLinesFrom() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* PhysXClient::getDebugLinesTo() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* PhysXClient::getDebugLinesColor() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} + +void PhysXClient::getCachedCameraImage(b3CameraImageData* cameraData) +{ + if (cameraData) + { + cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; + cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; + cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size() ? &m_data->m_cachedSegmentationMask[0] : 0; + } +} + +void PhysXClient::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactPointData = contactPointData->m_numContactPoints ? &m_data->m_cachedContactPoints[0] : 0; +} + +void PhysXClient::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) +{ + overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size(); + overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? &m_data->m_cachedOverlappingObjects[0] : 0; +} + +void PhysXClient::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); + visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; +} + +void PhysXClient::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) +{ + collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size(); + collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; +} + +void PhysXClient::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); + vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents ? &m_data->m_cachedVREvents[0] : 0; +} + +void PhysXClient::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) +{ + keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size(); + keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents ? &m_data->m_cachedKeyboardEvents[0] : 0; +} + +void PhysXClient::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); + mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents ? &m_data->m_cachedMouseEvents[0] : 0; +} + +void PhysXClient::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + raycastHits->m_numRayHits = m_data->m_raycastHits.size(); + raycastHits->m_rayHits = raycastHits->m_numRayHits ? &m_data->m_raycastHits[0] : 0; +} + +void PhysXClient::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + int sz = dofCountCheck * dofCountCheck; + if (sz == m_data->m_cachedMassMatrix.size()) + { + for (int i = 0; i < sz; i++) + { + massMatrix[i] = m_data->m_cachedMassMatrix[i]; + } + } +} + +void PhysXClient::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} + +double PhysXClient::getTimeOut() const +{ + return m_data->m_timeOutInSeconds; +} + + +void PhysXClient::pushProfileTiming(const char* timingName) +{ + std::string** strPtr = m_data->m_profileTimingStringArray[timingName]; + std::string* str = 0; + if (strPtr) + { + str = *strPtr; + } + else + { + str = new std::string(timingName); + m_data->m_profileTimingStringArray.insert(timingName, str); + } + m_data->m_profileTimings.push_back(new CProfileSample(str->c_str())); +} + +void PhysXClient::popProfileTiming() +{ + if (m_data->m_profileTimings.size()) + { + CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size() - 1]; + m_data->m_profileTimings.pop_back(); + delete sample; + } +} + + + +#endif //BT_ENABLE_PHYSX diff --git a/examples/SharedMemory/physx/PhysXClient.h b/examples/SharedMemory/physx/PhysXClient.h new file mode 100644 index 000000000..c5bd9f6ba --- /dev/null +++ b/examples/SharedMemory/physx/PhysXClient.h @@ -0,0 +1,135 @@ +#ifndef PHYSX_CLIENT_H +#define PHYSX_CLIENT_H + +#include "../PhysicsClient.h" + +///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands +class PhysXClient : public PhysicsClient +{ +protected: + struct PhysXDirectInternalData* m_data; + + bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + + bool processCamera(const struct SharedMemoryCommand& orgCommand); + + bool processContactPointData(const struct SharedMemoryCommand& orgCommand); + + bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand); + + bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); + + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + + void processAddUserData(const struct SharedMemoryStatus& serverCmd); + + void postProcessStatus(const struct SharedMemoryStatus& serverCmd); + + void resetData(); + + void removeCachedBody(int bodyUniqueId); + +public: + PhysXClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); + + virtual ~PhysXClient(); + + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper + virtual bool connect(); + + ////todo: rename to 'disconnect' + virtual void disconnectSharedMemory(); + + virtual bool isConnected() const; + + // return non-null if there is a status, nullptr otherwise + virtual const SharedMemoryStatus* processServerStatus(); + + virtual SharedMemoryCommand* getAvailableSharedMemoryCommand(); + + virtual bool canSubmitCommand() const; + + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + + virtual int getNumBodies() const; + + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + + virtual int getNumJoints(int bodyIndex) const; + + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + + virtual int getNumUserConstraints() const; + + virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; + + ///todo: move this out of the + virtual void setSharedMemoryKey(int key); + + void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; + + virtual void getCachedCameraImage(b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + + virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); + + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + + virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo); + + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + + //the following APIs are for internal use for visualization: + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; + + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const + { + return false; + } + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const + { + return -1; + } + virtual int getNumUserData(int bodyUniqueId) const + { + return 0; + } + virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const + { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + + virtual void pushProfileTiming(const char* timingName); + virtual void popProfileTiming(); +}; + +#endif //PHYSX_CLIENT_H diff --git a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp new file mode 100644 index 000000000..821f17216 --- /dev/null +++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp @@ -0,0 +1,1639 @@ +#ifdef BT_ENABLE_PHYSX +#include "PhysXServerCommandProcessor.h" + +#include "../../Utils/ChromeTraceUtil.h" + +#include +#include "../SharedMemoryCommands.h" +#include "LinearMath/btQuickprof.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "LinearMath/btMinMax.h" +#include "Bullet3Common/b3FileUtils.h" +#include "../../Utils/b3ResourcePath.h" +#include "Bullet3Common/b3ResizablePool.h" +#include "PxPhysicsAPI.h" +#include "../Utils/b3BulletDefaultFileIO.h" +#include "PhysXUrdfImporter.h" +#include "PxTolerancesScale.h" +#include "PxDefaultCpuDispatcher.h" +#include "PxDefaultSimulationFilterShader.h" +#include "URDF2PhysX.h" +#include "../b3PluginManager.h" + +#define STATIC_EGLRENDERER_PLUGIN +#ifdef STATIC_EGLRENDERER_PLUGIN +#include "../plugins/eglPlugin/eglRendererPlugin.h" +#endif //STATIC_EGLRENDERER_PLUGIN + +//for serialization of data to client +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h" +#include "LinearMath/btSerializer.h" +#include "PhysXUserData.h" + +class MyPhysXErrorCallback : public physx::PxErrorCallback +{ +public: + MyPhysXErrorCallback() + { + } + ~MyPhysXErrorCallback() + { + } + + virtual void reportError(physx::PxErrorCode::Enum code, const char* message, const char* file, int line) + { + b3Printf("%s in file:%s line:%d\n", message, file, line); + } +}; + +struct InternalPhysXBodyData +{ + physx::PxArticulationReducedCoordinate* mArticulation; + std::string m_bodyName; + //physx::PxArticulationJointReducedCoordinate* gDriveJoint; + void clear() + { + m_bodyName = ""; + } +}; + + + +typedef b3PoolBodyHandle InternalPhysXBodyHandle; + + +struct PhysXServerCommandProcessorInternalData +{ + bool m_isConnected; + bool m_verboseOutput; + double m_physicsDeltaTime; + int m_numSimulationSubSteps; + + + + b3PluginManager m_pluginManager; + + + physx::PxDefaultAllocator m_allocator; + MyPhysXErrorCallback m_errorCallback; + physx::PxFoundation* m_foundation; + physx::PxPhysics* m_physics; + physx::PxCooking* m_cooking; + physx::PxDefaultCpuDispatcher* m_dispatcher; + physx::PxScene* m_scene; + physx::PxMaterial* m_material; + //physx::PxPvd* m_pvd; + + b3ResizablePool m_bodyHandles; + + + + + b3AlignedObjectArray m_mjcfRecentLoadedBodies; + + int m_profileTimingLoggingUid; + int m_stateLoggersUniqueId; + std::string m_profileTimingFileName; + + PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk) + : m_isConnected(false), + m_verboseOutput(false), + m_physicsDeltaTime(1. / 240.), + m_numSimulationSubSteps(0), + m_pluginManager(sdk), + m_profileTimingLoggingUid(-1), + m_stateLoggersUniqueId(1) + { + m_foundation = NULL; + m_physics = NULL; + m_cooking = NULL; + m_dispatcher = NULL; + m_scene = NULL; + m_material = NULL; + //m_pvd = NULL; + +#ifdef STATIC_EGLRENDERER_PLUGIN + { + bool initPlugin = false; + b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin); + funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin; + int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin); + m_pluginManager.selectPluginRenderer(renderPluginId); + } +#endif //STATIC_EGLRENDERER_PLUGIN + } +}; + +PhysXServerCommandProcessor::PhysXServerCommandProcessor() +{ + m_data = new PhysXServerCommandProcessorInternalData(this); +} + +PhysXServerCommandProcessor::~PhysXServerCommandProcessor() +{ + delete m_data; +} + + + +physx::PxFilterFlags MyPhysXFilter(physx::PxFilterObjectAttributes attributes0, physx::PxFilterData filterData0, + physx::PxFilterObjectAttributes attributes1, physx::PxFilterData filterData1, + physx::PxPairFlags& pairFlags, const void* constantBlock, physx::PxU32 constantBlockSize) +{ + PX_UNUSED(attributes0); + PX_UNUSED(attributes1); + PX_UNUSED(constantBlock); + PX_UNUSED(constantBlockSize); + if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2) + return physx::PxFilterFlag::eKILL; + pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT; + return physx::PxFilterFlag::eDEFAULT; +} + + + +bool PhysXServerCommandProcessor::connect() +{ + if (m_data->m_isConnected) + { + printf("already connected\n"); + return true; + } + + int result = 0; + { + + m_data->m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_data->m_allocator, m_data->m_errorCallback); + m_data->m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxTolerancesScale(), true, 0); + m_data->m_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxCookingParams(physx::PxTolerancesScale())); + + physx::PxU32 numCores = 1;// + m_data->m_dispatcher = physx::PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1); + + physx::PxSceneDesc sceneDesc(m_data->m_physics->getTolerancesScale()); + sceneDesc.gravity = physx::PxVec3(0.0f, -9.81f, 0.0f); + sceneDesc.solverType = physx::PxSolverType::eTGS; + //sceneDesc.solverType = physx::PxSolverType::ePGS; + sceneDesc.cpuDispatcher = m_data->m_dispatcher; + //sceneDesc.filterShader = MyPhysXFilter; + sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader; + + + m_data->m_scene = m_data->m_physics->createScene(sceneDesc); + + m_data->m_material = m_data->m_physics->createMaterial(0.5f, 0.5f, 0.f); + + PxInitExtensions(*m_data->m_physics, 0); + + + //PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0, 1, 0, 0), *gMaterial); + //gScene->addActor(*groundPlane); + + result = 1; + } + if (result == 1) + { + m_data->m_isConnected = true; + return true; + } + + return false; +} + +void PhysXServerCommandProcessor::resetSimulation() +{ + //gArticulation->release(); + m_data->m_scene->release(); + m_data->m_dispatcher->release(); + m_data->m_cooking->release(); + m_data->m_physics->release(); + //PxPvdTransport* transport = gPvd->getTransport(); + //gPvd->release(); + //transport->release(); + PxCloseExtensions(); + + m_data->m_foundation->release(); +} + +void PhysXServerCommandProcessor::disconnect() +{ + resetSimulation(); + + m_data->m_isConnected = false; +} + +bool PhysXServerCommandProcessor::isConnected() const +{ + return m_data->m_isConnected; +} + + +bool PhysXServerCommandProcessor::processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED; + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1; + + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN) + { + //pluginPath could be registered or load from disk + const char* postFix = ""; + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX) + { + postFix = clientCmd.m_customCommandArgs.m_postFix; + } + + int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix); + if (pluginUniqueId >= 0) + { + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN) + { + m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId); + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND) + { + int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments); + serverCmd.m_customCommandResultArgs.m_executeCommandResult = result; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + return hasStatus; +} + + +bool PhysXServerCommandProcessor::processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + BT_PROFILE("CMD_STATE_LOGGING"); + + serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED; + bool hasStatus = true; + + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS) + { + if (m_data->m_profileTimingLoggingUid < 0) + { + b3ChromeUtilsStartTimings(); + m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + m_data->m_profileTimingLoggingUid = loggerUid; + } + } + + if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId >= 0) + { + if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid) + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str()); + m_data->m_profileTimingLoggingUid = -1; + } + } + +#if 0 + if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG) + { + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_ALL_COMMANDS) + { + if (m_data->m_commandLogger == 0) + { + enableCommandLogging(true, clientCmd.m_stateLoggingArguments.m_fileName); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + int loggerUid = m_data->m_stateLoggersUniqueId++; + m_data->m_commandLoggingUid = loggerUid; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_REPLAY_ALL_COMMANDS) + { + if (m_data->m_logPlayback == 0) + { + replayFromLogFile(clientCmd.m_stateLoggingArguments.m_fileName); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + int loggerUid = m_data->m_stateLoggersUniqueId++; + m_data->m_logPlaybackUid = loggerUid; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + + + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4) + { + //if (clientCmd.m_stateLoggingArguments.m_fileName) + { + int loggerUid = m_data->m_stateLoggersUniqueId++; + VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid, clientCmd.m_stateLoggingArguments.m_fileName, this->m_data->m_guiHelper); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + //either provide the minitaur by object unique Id, or search for first multibody with 8 motors... + + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds > 0)) + { + int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body) + { + if (body->m_multiBody) + { + btAlignedObjectArray motorNames; + motorNames.push_back("motor_front_leftR_joint"); + motorNames.push_back("motor_front_leftL_joint"); + motorNames.push_back("motor_back_leftR_joint"); + motorNames.push_back("motor_back_leftL_joint"); + motorNames.push_back("motor_front_rightL_joint"); + motorNames.push_back("motor_front_rightR_joint"); + motorNames.push_back("motor_back_rightL_joint"); + motorNames.push_back("motor_back_rightR_joint"); + + btAlignedObjectArray motorIdList; + for (int m = 0; m < motorNames.size(); m++) + { + for (int i = 0; i < body->m_multiBody->getNumLinks(); i++) + { + std::string jointName; + if (body->m_multiBody->getLink(i).m_jointName) + { + jointName = body->m_multiBody->getLink(i).m_jointName; + } + if (motorNames[m] == jointName) + { + motorIdList.push_back(i); + } + } + } + + if (motorIdList.size() == 8) + { + int loggerUid = m_data->m_stateLoggersUniqueId++; + MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid, fileName, body->m_multiBody, motorIdList); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + } + } + } + + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + + int loggerUid = m_data->m_stateLoggersUniqueId++; + int maxLogDof = 12; + if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF)) + { + maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof; + } + + int logFlags = 0; + if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS) + { + logFlags = clientCmd.m_stateLoggingArguments.m_logFlags; + } + GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid, fileName, m_data->m_dynamicsWorld, maxLogDof, logFlags); + + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds > 0)) + { + logger->m_filterObjectUniqueId = true; + for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i) + { + int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]; + logger->m_bodyIdList.push_back(objectUniqueId); + } + } + + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid, fileName, m_data->m_dynamicsWorld); + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1) + { + logger->m_filterLinkA = true; + logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1) + { + logger->m_filterLinkB = true; + logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1) + { + logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1) + { + logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB; + } + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VR_CONTROLLERS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + int deviceFilterType = VR_DEVICE_CONTROLLER; + if (clientCmd.m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE) + { + deviceFilterType = clientCmd.m_stateLoggingArguments.m_deviceFilterType; + } + VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid, deviceFilterType, fileName); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId >= 0) + { + if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_logPlaybackUid) + { + if (m_data->m_logPlayback) + { + delete m_data->m_logPlayback; + m_data->m_logPlayback = 0; + m_data->m_logPlaybackUid = -1; + } + } + + if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_commandLoggingUid) + { + if (m_data->m_commandLogger) + { + enableCommandLogging(false, 0); + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + m_data->m_commandLoggingUid = -1; + } + } + + if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid) + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str()); + m_data->m_profileTimingLoggingUid = -1; + } + else + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + for (int i = 0; i < m_data->m_stateLoggers.size(); i++) + { + if (m_data->m_stateLoggers[i]->m_loggingUniqueId == clientCmd.m_stateLoggingArguments.m_loggingUniqueId) + { + m_data->m_stateLoggers[i]->stop(); + delete m_data->m_stateLoggers[i]; + m_data->m_stateLoggers.removeAtIndex(i); + } + } + } + } +#endif + return hasStatus; +} + + +bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + // BT_PROFILE("processCommand"); + + int sz = sizeof(SharedMemoryStatus); + int sz2 = sizeof(SharedMemoryCommand); + + bool hasStatus = false; + + serverStatusOut.m_type = CMD_INVALID_STATUS; + serverStatusOut.m_numDataStreamBytes = 0; + serverStatusOut.m_dataStream = 0; + + //consume the command + switch (clientCmd.m_type) + { + case CMD_REQUEST_INTERNAL_DATA: + { + hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_SYNC_BODY_INFO: + { + hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SYNC_USER_DATA: + { + hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_STEP_FORWARD_SIMULATION: + { + hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_REQUEST_ACTUAL_STATE: + { + hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_RESET_SIMULATION: + { + hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + default: + { + BT_PROFILE("CMD_UNKNOWN"); + printf("Unknown command encountered: %d", clientCmd.m_type); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + hasStatus = true; + } + + case CMD_LOAD_URDF: + { + hasStatus = processLoadURDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CUSTOM_COMMAND: + { + hasStatus = processCustomCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_STATE_LOGGING: + { + hasStatus = processStateLoggingCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + +#if 0 + case CMD_SET_VR_CAMERA_STATE: + { + hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VR_EVENTS_DATA: + { + hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_MOUSE_EVENTS_DATA: + { + hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_KEYBOARD_EVENTS_DATA: + { + hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS: + { + + hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_DEBUG_LINES: + { + hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_CAMERA_IMAGE_DATA: + { + hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_WORLD: + { + hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_SDF: + { + hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_VISUAL_SHAPE: + { + hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_MULTI_BODY: + { + hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_ADDITIONAL_SEARCH_PATH: + { + hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_LOAD_MJCF: + { + hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_LOAD_SOFT_BODY: + { + hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_SENSOR: + { + hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PROFILE_TIMING: + { + hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_DESIRED_STATE: + { + hasStatus = processSendDesiredStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_COLLISION_INFO: + { + hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + + + case CMD_CHANGE_DYNAMICS_INFO: + { + hasStatus = processChangeDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_GET_DYNAMICS_INFO: + { + hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + + case CMD_INIT_POSE: + { + hasStatus = processInitPoseCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_CREATE_RIGID_BODY: + { + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_BOX_COLLISION_SHAPE: + { + //for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PICK_BODY: + { + hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_MOVE_PICKED_BODY: + { + hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_AABB_OVERLAP: + { + hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: + { + hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CONFIGURE_OPENGL_VISUALIZER: + { + hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_JACOBIAN: + { + hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_MASS_MATRIX: + { + hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_APPLY_EXTERNAL_FORCE: + { + hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_BODY: + { + hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_CONSTRAINT: + { + hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_KINEMATICS: + { + hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VISUAL_SHAPE_INFO: + { + hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_COLLISION_SHAPE_INFO: + { + hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_UPDATE_VISUAL_SHAPE: + { + hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CHANGE_TEXTURE: + { + hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_TEXTURE: + { + hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_RESTORE_STATE: + { + hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SAVE_STATE: + { + hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_LOAD_BULLET: + { + hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_BULLET: + { + hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_MJCF: + { + hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_DEBUG_DRAW: + { + hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_USER_DATA: + { + hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_ADD_USER_DATA: + { + hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_USER_DATA: + { + hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } +#endif + }; + + return hasStatus; +} + +bool PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; + serverCmd.m_numDataStreamBytes = 0; + return hasStatus; +} + +bool PhysXServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_BODY_INFO"); + int actualNumBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; + return hasStatus; +} + +bool PhysXServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_USER_DATA"); + int numIdentifiers = 0; + serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers; + serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; + return hasStatus; +} + +struct MyPhysXURDFImporter : public PhysXURDFImporter +{ + b3PluginManager& m_pluginManager; + + MyPhysXURDFImporter(struct CommonFileIOInterface* fileIO, double globalScaling, int flags, b3PluginManager& pluginManager) + :PhysXURDFImporter(fileIO, globalScaling, flags), + m_pluginManager(pluginManager) + { + + } + + int convertLinkVisualShapes3( + int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + const UrdfLink* linkPtr, const UrdfModel* model, + int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const + { + + if (m_pluginManager.getRenderInterface()) + { + int graphicsUniqueId = m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, linkPtr, model, collisionObjectUniqueId, bodyUniqueId, fileIO); + return graphicsUniqueId; + } + return 0; + } + +}; + + +bool PhysXServerCommandProcessor::processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + BT_PROFILE("CMD_LOAD_URDF"); + serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; + serverStatusOut.m_numDataStreamBytes = 0; + + const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; + + btAssert(m_data->m_foundation); + if (!m_data->m_foundation) + { + b3Error("loadUrdf: No valid m_dynamicsWorld"); + return false; + } + + bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody != 0) : true; + bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase != 0) : false; + + btScalar globalScaling = 1.f; + if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) + { + globalScaling = urdfArgs.m_globalScaling; + } + + b3BulletDefaultFileIO fileIO; + + btVector3 initialPos(0, 0, 0); + btQuaternion initialOrn(0, 0, 0, 1); + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) + { + initialPos[0] = urdfArgs.m_initialPosition[0]; + initialPos[1] = urdfArgs.m_initialPosition[1]; + initialPos[2] = urdfArgs.m_initialPosition[2]; + } + int urdfFlags = 0; + if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + urdfFlags = urdfArgs.m_urdfFlags; + } + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) + { + initialOrn[0] = urdfArgs.m_initialOrientation[0]; + initialOrn[1] = urdfArgs.m_initialOrientation[1]; + initialOrn[2] = urdfArgs.m_initialOrientation[2]; + initialOrn[3] = urdfArgs.m_initialOrientation[3]; + } + + + MyPhysXURDFImporter u2p(&fileIO, globalScaling, urdfArgs.m_urdfFlags, m_data->m_pluginManager); + + + + bool loadOk = u2p.loadURDF(urdfArgs.m_urdfFileName, useFixedBase); + + + if (loadOk) + { + + + for (int m = 0; m < u2p.getNumModels(); m++) + { + u2p.activateModel(m); + + btTransform rootTrans; + rootTrans.setOrigin(initialPos); + rootTrans.setRotation(initialOrn); + u2p.setRootTransformInWorld(rootTrans); + + //get a body index + int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + + InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + //sd.m_bodyUniqueIds.push_back(bodyUniqueId); + + + + u2p.setBodyUniqueId(bodyUniqueId); + { + btScalar mass = 0; + //bodyHandle->m_rootLocalInertialFrame.setIdentity(); + bodyHandle->m_bodyName = u2p.getBodyName(); + btVector3 localInertiaDiagonal(0, 0, 0); + int urdfLinkIndex = u2p.getRootLinkIndex(); + //u2p.getMassAndInertia2((urdfLinkIndex, mass, localInertiaDiagonal, bodyHandle->m_rootLocalInertialFrame, flags); + } + + physx::PxArticulationReducedCoordinate* articulation = URDF2PhysX(m_data->m_foundation,m_data->m_physics, m_data->m_cooking, m_data->m_scene, u2p, urdfArgs.m_urdfFlags, u2p.getPathPrefix(), rootTrans, &fileIO); + + if (articulation) + { + bodyHandle->mArticulation = articulation; + + serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + sprintf(serverStatusOut.m_dataStreamArguments.m_bodyName, "%s", bodyHandle->m_bodyName.c_str()); + + btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient); + + ser.startSerialization(); + + int len = sizeof(btMultiBodyData); + btChunk* chunk = ser.allocate(len, 1); + + + btMultiBodyData *mbd = (btMultiBodyData *)chunk->m_oldPtr; + btTransform rootTrans; + u2p.getRootTransformInWorld(rootTrans); + rootTrans.getOrigin().serialize(mbd->m_baseWorldPosition); + rootTrans.getRotation().serialize(mbd->m_baseWorldOrientation); + btVector3 zero(0, 0, 0); + + zero.serialize(mbd->m_baseLinearVelocity); + zero.serialize(mbd->m_baseAngularVelocity); + + ser.registerNameForPointer(bodyHandle->m_bodyName.c_str(), bodyHandle->m_bodyName.c_str()); + { + char *name = (char *)ser.findNameForPointer(bodyHandle->m_bodyName.c_str()); + mbd->m_baseName = (char *)ser.getUniquePointer(name); + if (mbd->m_baseName) + { + ser.serializeName(name); + } + } + mbd->m_numLinks = articulation->getNbLinks()-1; + + if (mbd->m_numLinks) + { + int sz = sizeof(btMultiBodyLinkData); + int numElem = mbd->m_numLinks; + btChunk *chunk = ser.allocate(sz, numElem); + + physx::PxArticulationLink* physxLinks[64]; + physx::PxU32 bufferSize = 64; + physx::PxU32 startIndex = 0; + int numLinks2 = articulation->getLinks(physxLinks, bufferSize, startIndex); + + btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr; + for (int j = 0; j < numElem; j++, memPtr++) + { + int i = j + 1; + + + memPtr->m_jointType = 0;//todo + memPtr->m_dofCount = physxLinks[i]->getInboundJointDof(); + memPtr->m_posVarCount = physxLinks[i]->getInboundJointDof(); //?? + + physx::PxVec3 li = physxLinks[i]->getMassSpaceInertiaTensor(); + btVector3 localInertia(li[0], li[1], li[2]); + localInertia.serialize(memPtr->m_linkInertia); + + memPtr->m_linkMass = physxLinks[i]->getMass(); + memPtr->m_parentIndex = i>0? physxLinks[i]->getInboundJoint()->getParentArticulationLink().getLinkIndex(): -1; + memPtr->m_jointDamping = 0;//todophysxLinks[i]->getLinearDamping();//?? + memPtr->m_jointFriction = 0;//todo + memPtr->m_jointLowerLimit = 0;//todogetLink(i).m_jointLowerLimit; + memPtr->m_jointUpperLimit = 0;//todogetLink(i).m_jointUpperLimit; + memPtr->m_jointMaxForce = 0;//todogetLink(i).m_jointMaxForce; + memPtr->m_jointMaxVelocity = 0;//todogetLink(i).m_jointMaxVelocity; + + //getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset); + //getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); + //getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); + + + { + char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName()); + memPtr->m_linkName = (char *)ser.getUniquePointer(name); + if (memPtr->m_linkName) + { + ser.serializeName(name); + } + } + { + char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName()); + memPtr->m_jointName = (char *)ser.getUniquePointer(name); + if (memPtr->m_jointName) + { + ser.serializeName(name); + } + } + memPtr->m_linkCollider = (btCollisionObjectData *)ser.getUniquePointer(0); + } + ser.finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)articulation); + } + mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)ser.getUniquePointer((void *)articulation) : 0; + + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); +#endif + + const char* structType = btMultiBodyDataName; + ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE,0); + int streamSizeInBytes = ser.getCurrentBufferSize(); + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + + } + } +#if 0 + btTransform rootTrans; + rootTrans.setOrigin(pos); + rootTrans.setRotation(orn); + u2b.setRootTransformInWorld(rootTrans); + bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + if (ok) + { + if (m_data->m_sdfRecentLoadedBodies.size() == 1) + { + *bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0]; + } + m_data->m_sdfRecentLoadedBodies.clear(); + } +#endif + return true; + } + return false; +} + +bool PhysXServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_BODY_INFO"); + + const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; + //stream info into memory + int streamSizeInBytes = 0; //createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; + + + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + + return hasStatus; +} + +bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); + + int numArt = m_data->m_scene->getNbArticulations(); + + { + B3_PROFILE("PhysX_simulate_fetchResults"); + m_data->m_scene->simulate(m_data->m_physicsDeltaTime); + m_data->m_scene->fetchResults(true); + } + { + B3_PROFILE("syncTransform"); + if (m_data->m_pluginManager.getRenderInterface()) + { + + //sync transforms... + + b3AlignedObjectArray usedHandles; + m_data->m_bodyHandles.getUsedHandles(usedHandles); + + for (int i = 0; i < usedHandles.size(); i++) + { + InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(usedHandles[i]); + physx::PxArticulationLink* physxLinks[64]; + physx::PxU32 bufferSize = 64; + physx::PxU32 startIndex = 0; + int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex); + + for (int l = 0; l < numLinks2; l++) + { + MyPhysXUserData* ud = (MyPhysXUserData*)physxLinks[l]->userData; + if (ud) + { + btTransform tr; + tr.setIdentity(); + physx::PxTransform pt = physxLinks[l]->getGlobalPose(); + tr.setOrigin(btVector3(pt.p[0], pt.p[1], pt.p[2])); + tr.setRotation(btQuaternion(pt.q.x, pt.q.y, pt.q.z, pt.q.w)); + btVector3 localScaling(1, 1, 1);//?? + m_data->m_pluginManager.getRenderInterface()->syncTransform(ud->m_graphicsUniqueId, tr, localScaling); + } + } + } + } + + { + B3_PROFILE("render"); + //m_data->m_pluginManager.getRenderInterface()->render(); + unsigned char* pixelRGBA = 0; + int numRequestedPixels = 0; + float* depthBuffer = 0; + int* segmentationMaskBuffer = 0; + int startPixelIndex = 0; + int width = 1024; + int height = 768; + int numPixelsCopied = 0; + + m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels, + depthBuffer, numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex, &width, &height, &numPixelsCopied); + } + } + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; + return hasStatus; +} + +bool PhysXServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME) + { + m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) + { + btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); + + m_data->m_scene->setGravity(physx::PxVec3(grav[0], grav[1], grav[2])); + + if (m_data->m_verboseOutput) + { + b3Printf("Updated Gravity: %f,%f,%f", grav[0], grav[1], grav[2]); + } + + } + +#if 0 + if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION) + { + if (clientCmd.m_physSimParamArgs.m_enableConeFriction) + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } else + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0); + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE) + { + gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0; + gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0; + } + + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) + { + m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); + } + + //see + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) + { + //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk + gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) + { + gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) + { + m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; + } + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; + } + if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) + { + m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM) + { + m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; + } + + + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) + { + b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); + } + +#endif + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysXServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + + int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; + InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + if (bodyHandle->mArticulation) + { + BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); + if (m_data->m_verboseOutput) + { + b3Printf("Sending the actual state (Q,U)"); + } + + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = bodyHandle->mArticulation->getNbLinks()-1; //skip base! + + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; + + if (serverCmd.m_sendActualStateArgs.m_numLinks >= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + return hasStatus; + } + + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0); + bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0); + + if (computeForwardKinematics || computeLinkVelocities) + { + //todo:check this + } + + physx::PxArticulationLink* physxLinks[64]; + physx::PxU32 bufferSize = 64; + physx::PxU32 startIndex = 0; + int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex); + + + //always add the base, even for static (non-moving objects) + //so that we can easily move the 'fixed' base when needed + //do we don't use this conditional "if (!mb->hasFixedBase())" + { + int rootLink = 0; //todo check + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 1; + + physx::PxArticulationLink* l = physxLinks[0]; + physx::PxVec3 pos = l->getGlobalPose().p; + physx::PxQuat orn = l->getGlobalPose().q; + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = pos[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = pos[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = pos[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = orn.x; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn.y; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn.z; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn.w; + totalDegreeOfFreedomQ += 7; //pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = 0;//cvel[3]; //mb->getBaseVel()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = 0;//cvel[4]; //mb->getBaseVel()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = 0;//cvel[5]; //mb->getBaseVel()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = 0;//cvel[0]; //mb->getBaseOmega()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = 0;//cvel[1]; //mb->getBaseOmega()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = 0;//cvel[2]; //mb->getBaseOmega()[2]; + totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF + } + + //btAlignedObjectArray omega; + //btAlignedObjectArray linVel; + + int numLinks = 0;// m_data->m_mujocoModel->body_jntnum[bodyUniqueId]; + for (int l = 0; l < numLinks; l++) + { + //int type = (m_data->m_mujocoModel->jnt_type + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l]; + //int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l]; + +#if 0 + physx::PxArticulationCache* c = bodyHandle->mArticulation->createCache(); + if (c) + { + c->jointVelocity[0] = 1; + bodyHandle->mArticulation->applyCache(*c, physx::PxArticulationCache::eVELOCITY); + bodyHandle->mArticulation->releaseCache(*c); + } +#endif +#if 0 + mjtNum* xpos = + for (int d=0;dgetLink(l).m_posVarCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = 0; + } + for (int d=0;dgetLink(l).m_dofCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = 0; + } + + if (0 == mb->getLink(l).m_jointFeedback) + { + for (int d=0;d<6;d++) + { + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; + } + } else + { + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = 0; + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = 0; + } + + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; +#if 0 + if (supportsJointMotor(mb,l)) + { + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) + { + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + } + } +#endif + //btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); + //btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); + + //btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); + //btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); + + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = 0;//linkCOMOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = 0;//linkCOMOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = 0;//linkCOMOrigin.getZ(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = 0;//linkCOMRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = 0;//linkCOMRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = 0;//linkCOMRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = 1;//linkCOMRotation.w(); + +#if 0 + btVector3 worldLinVel(0,0,0); + btVector3 worldAngVel(0,0,0); + + if (computeLinkVelocities) + { + const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis(); + worldLinVel = linkRotMat * linVel[l+1]; + worldAngVel = linkRotMat * omega[l+1]; + } +#endif + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = 0;//worldLinVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = 0;//worldLinVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = 0;//worldLinVel[2]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = 0;//worldAngVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = 0;//worldAngVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = 0;//worldAngVel[2]; + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = 0;//linkLocalInertialOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = 0;//linkLocalInertialOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = 0;//linkLocalInertialOrigin.getZ(); + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = 0;//linkLocalInertialRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = 0;//linkLocalInertialRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = 0;//linkLocalInertialRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = 1;//linkLocalInertialRotation.w(); +#endif + } + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + } + } + + + return hasStatus; +} + +bool PhysXServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_RESET_SIMULATION"); + + resetSimulation(); + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; + return hasStatus; +} + +bool PhysXServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + +#endif //BT_ENABLE_PHYSX \ No newline at end of file diff --git a/examples/SharedMemory/physx/PhysXServerCommandProcessor.h b/examples/SharedMemory/physx/PhysXServerCommandProcessor.h new file mode 100644 index 000000000..36fc62116 --- /dev/null +++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.h @@ -0,0 +1,47 @@ +#ifndef PHYSX_SERVER_COMMAND_PROCESSOR_H +#define PHYSX_SERVER_COMMAND_PROCESSOR_H + +#include "../PhysicsCommandProcessorInterface.h" + +class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface +{ + struct PhysXServerCommandProcessorInternalData* m_data; + + bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + void resetSimulation(); + bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); +public: + PhysXServerCommandProcessor(); + + virtual ~PhysXServerCommandProcessor(); + + virtual bool connect(); + + virtual void disconnect(); + + virtual bool isConnected() const; + + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual void renderScene(int renderFlags) {} + virtual void physicsDebugDraw(int debugDrawFlags) {} + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) {} + virtual void setTimeOut(double timeOutInSeconds) {} + + virtual void reportNotifications() {} +}; + +#endif //PHYSX_SERVER_COMMAND_PROCESSOR_H diff --git a/examples/SharedMemory/physx/PhysXUrdfImporter.cpp b/examples/SharedMemory/physx/PhysXUrdfImporter.cpp new file mode 100644 index 000000000..d86e31068 --- /dev/null +++ b/examples/SharedMemory/physx/PhysXUrdfImporter.cpp @@ -0,0 +1,1434 @@ +/* Copyright (C) 2015 Google + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "PhysXUrdfImporter.h" +#include "../../CommonInterfaces/CommonRenderInterface.h" +#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" +#include "../../Importers/ImportURDFDemo/URDFImporterInterface.h" + +#include "../../Importers/ImportObjDemo/LoadMeshFromObj.h" +#include "../../Importers/ImportSTLDemo/LoadMeshFromSTL.h" +#include "../../Importers/ImportColladaDemo/LoadMeshFromCollada.h" +//#include "BulletCollision/CollisionShapes/btShapeHull.h" //to create a tesselation of a generic btConvexShape +#include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../../CommonInterfaces/CommonFileIOInterface.h" +#include "Bullet3Common/b3FileUtils.h" +#include +#include "../../Utils/b3ResourcePath.h" +#include "../../Utils/b3BulletDefaultFileIO.h" + +#include "../OpenGLWindow/ShapeData.h" + + +#include "../../Importers/ImportMeshUtility/b3ImportMeshUtility.h" + +static btScalar gUrdfDefaultCollisionMargin = 0.001; + +#include +#include +#include +#include "../../Importers/ImportURDFDemo/URDFJointTypes.h" +#include "../../Importers/ImportURDFDemo/UrdfParser.h" + + +ATTRIBUTE_ALIGNED16(struct) +PhysXURDFInternalData +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + b3BulletDefaultFileIO m_defaultFileIO; + UrdfParser m_urdfParser; + struct GUIHelperInterface* m_guiHelper; + struct CommonFileIOInterface* m_fileIO; + std::string m_sourceFile; + char m_pathPrefix[1024]; + int m_bodyId; + btHashMap m_linkColors; + btAlignedObjectArray m_allocatedCollisionShapes; + btAlignedObjectArray m_allocatedTextures; + //mutable btAlignedObjectArray m_allocatedMeshInterfaces; + btHashMap m_bulletCollisionShape2UrdfCollision; + + UrdfRenderingInterface* m_customVisualShapesConverter; + bool m_enableTinyRenderer; + int m_flags; + + void setSourceFile(const std::string& relativeFileName, const std::string& prefix) + { + m_sourceFile = relativeFileName; + m_urdfParser.setSourceFile(relativeFileName); + strncpy(m_pathPrefix, prefix.c_str(), sizeof(m_pathPrefix)); + m_pathPrefix[sizeof(m_pathPrefix) - 1] = 0; // required, strncpy doesn't write zero on overflow + } + + PhysXURDFInternalData(CommonFileIOInterface* fileIO) + :m_urdfParser(fileIO? fileIO : &m_defaultFileIO), + m_fileIO(fileIO? fileIO : &m_defaultFileIO) + { + m_enableTinyRenderer = true; + m_pathPrefix[0] = 0; + m_flags = 0; + } + + void setGlobalScaling(btScalar scaling) + { + m_urdfParser.setGlobalScaling(scaling); + } + + +}; + +void PhysXURDFImporter::printTree() +{ + // btAssert(0); +} + + + + +PhysXURDFImporter::PhysXURDFImporter(struct CommonFileIOInterface* fileIO,double globalScaling, int flags) +{ + m_data = new PhysXURDFInternalData(fileIO); + m_data->setGlobalScaling(globalScaling); + m_data->m_flags = flags; +} + +struct PhysXErrorLogger : public ErrorLogger +{ + int m_numErrors; + int m_numWarnings; + + PhysXErrorLogger() + : m_numErrors(0), + m_numWarnings(0) + { + } + virtual void reportError(const char* error) + { + m_numErrors++; + b3Error(error); + } + virtual void reportWarning(const char* warning) + { + m_numWarnings++; + b3Warning(warning); + } + + virtual void printMessage(const char* msg) + { + b3Printf(msg); + } +}; + +bool PhysXURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) +{ + if (strlen(fileName) == 0) + return false; + + //int argc=0; + char relativeFileName[1024]; + + b3FileUtils fu; + + //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); + bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024); + + std::string xml_string; + + if (!fileFound) + { + b3Warning("URDF file '%s' not found\n", fileName); + return false; + } + else + { + char path[1024]; + fu.extractPath(relativeFileName, path, sizeof(path)); + m_data->setSourceFile(relativeFileName, path); + + //read file + int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); + + + char destBuffer[8192]; + char* line = 0; + do + { + line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); + if (line) + { + xml_string += (std::string(destBuffer) + "\n"); + } + } + while (line); + m_data->m_fileIO->fileClose(fileId); +#if 0 + std::fstream xml_file(relativeFileName, std::fstream::in); + while (xml_file.good()) + { + std::string line; + std::getline(xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); +#endif + + } + + PhysXErrorLogger loggie; + m_data->m_urdfParser.setParseSDF(false); + bool result = false; + + if (xml_string.length()) + { + result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS)); + } + + return result; +} + +int PhysXURDFImporter::getNumModels() const +{ + return m_data->m_urdfParser.getNumModels(); +} + +void PhysXURDFImporter::activateModel(int modelIndex) +{ + m_data->m_urdfParser.activateModel(modelIndex); +} + +bool PhysXURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) +{ + //int argc=0; + char relativeFileName[1024]; + + b3FileUtils fu; + + //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); + bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)); + + std::string xml_string; + + if (!fileFound) + { + b3Warning("SDF file '%s' not found\n", fileName); + return false; + } + else + { + + char path[1024]; + fu.extractPath(relativeFileName, path, sizeof(path)); + m_data->setSourceFile(relativeFileName, path); + + //read file + int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); + + char destBuffer[8192]; + char* line = 0; + do + { + line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); + if (line) + { + xml_string += (std::string(destBuffer) + "\n"); + } + } + while (line); + m_data->m_fileIO->fileClose(fileId); + } + + PhysXErrorLogger loggie; + //todo: quick test to see if we can re-use the URDF parser for SDF or not + m_data->m_urdfParser.setParseSDF(true); + bool result = false; + if (xml_string.length()) + { + result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie); + } + + return result; +} + +const char* PhysXURDFImporter::getPathPrefix() +{ + return m_data->m_pathPrefix; +} + +void PhysXURDFImporter::setBodyUniqueId(int bodyId) +{ + m_data->m_bodyId = bodyId; +} + +int PhysXURDFImporter::getBodyUniqueId() const +{ + return m_data->m_bodyId; +} + +PhysXURDFImporter::~PhysXURDFImporter() +{ + delete m_data; +} + +int PhysXURDFImporter::getRootLinkIndex() const +{ + if (m_data->m_urdfParser.getModel().m_rootLinks.size() == 1) + { + return m_data->m_urdfParser.getModel().m_rootLinks[0]->m_linkIndex; + } + return -1; +}; + +void PhysXURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const +{ + childLinkIndices.resize(0); + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + if (linkPtr) + { + const UrdfLink* link = *linkPtr; + //int numChildren = m_data->m_urdfParser->getModel().m_links.getAtIndex(linkIndex)-> + + for (int i = 0; i < link->m_childLinks.size(); i++) + { + int childIndex = link->m_childLinks[i]->m_linkIndex; + childLinkIndices.push_back(childIndex); + } + } +} + +std::string PhysXURDFImporter::getLinkName(int linkIndex) const +{ + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + return link->m_name; + } + return ""; +} + +std::string PhysXURDFImporter::getBodyName() const +{ + return m_data->m_urdfParser.getModel().m_name; +} + +std::string PhysXURDFImporter::getJointName(int linkIndex) const +{ + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + if (link->m_parentJoint) + { + return link->m_parentJoint->m_name; + } + } + return ""; +} + +void PhysXURDFImporter::getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const +{ + if (flags & CUF_USE_URDF_INERTIA) + { + getMassAndInertia(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrame); + } + else + { + //the link->m_inertia is NOT necessarily aligned with the inertial frame + //so an additional transform might need to be computed + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex); + + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + btScalar linkMass; + if (link->m_parentJoint == 0 && m_data->m_urdfParser.getModel().m_overrideFixedBase) + { + linkMass = 0.f; + } + else + { + linkMass = link->m_inertia.m_mass; + } + mass = linkMass; + localInertiaDiagonal.setValue(0, 0, 0); + inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin()); + inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()); + } + else + { + mass = 1.f; + localInertiaDiagonal.setValue(1, 1, 1); + inertialFrame.setIdentity(); + } + } +} + +void PhysXURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const +{ + //the link->m_inertia is NOT necessarily aligned with the inertial frame + //so an additional transform might need to be computed + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + btMatrix3x3 linkInertiaBasis; + btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ; + if (link->m_parentJoint == 0 && m_data->m_urdfParser.getModel().m_overrideFixedBase) + { + linkMass = 0.f; + principalInertiaX = 0.f; + principalInertiaY = 0.f; + principalInertiaZ = 0.f; + linkInertiaBasis.setIdentity(); + } + else + { + linkMass = link->m_inertia.m_mass; + if (link->m_inertia.m_ixy == 0.0 && + link->m_inertia.m_ixz == 0.0 && + link->m_inertia.m_iyz == 0.0) + { + principalInertiaX = link->m_inertia.m_ixx; + principalInertiaY = link->m_inertia.m_iyy; + principalInertiaZ = link->m_inertia.m_izz; + linkInertiaBasis.setIdentity(); + } + else + { + principalInertiaX = link->m_inertia.m_ixx; + btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz, + link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz, + link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz); + btScalar threshold = 1.0e-6; + int numIterations = 30; + inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations); + principalInertiaX = inertiaTensor[0][0]; + principalInertiaY = inertiaTensor[1][1]; + principalInertiaZ = inertiaTensor[2][2]; + } + } + mass = linkMass; + if (principalInertiaX < 0 || + principalInertiaX > (principalInertiaY + principalInertiaZ) || + principalInertiaY < 0 || + principalInertiaY > (principalInertiaX + principalInertiaZ) || + principalInertiaZ < 0 || + principalInertiaZ > (principalInertiaX + principalInertiaY)) + { + b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str()); + principalInertiaX = 0.f; + principalInertiaY = 0.f; + principalInertiaZ = 0.f; + linkInertiaBasis.setIdentity(); + } + localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ); + inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin()); + inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis() * linkInertiaBasis); + } + else + { + mass = 1.f; + localInertiaDiagonal.setValue(1, 1, 1); + inertialFrame.setIdentity(); + } +} + +bool PhysXURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const +{ + jointLowerLimit = 0.f; + jointUpperLimit = 0.f; + jointDamping = 0.f; + jointFriction = 0.f; + jointMaxForce = 0.f; + jointMaxVelocity = 0.f; + + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex); + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + linkTransformInWorld = link->m_linkTransformInWorld; + + if (link->m_parentJoint) + { + UrdfJoint* pj = link->m_parentJoint; + parent2joint = pj->m_parentLinkToJointTransform; + jointType = pj->m_type; + jointAxisInJointSpace = pj->m_localJointAxis; + jointLowerLimit = pj->m_lowerLimit; + jointUpperLimit = pj->m_upperLimit; + jointDamping = pj->m_jointDamping; + jointFriction = pj->m_jointFriction; + jointMaxForce = pj->m_effortLimit; + jointMaxVelocity = pj->m_velocityLimit; + return true; + } + else + { + parent2joint.setIdentity(); + return false; + } + } + + return false; +}; + +bool PhysXURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const +{ + btScalar jointMaxForce; + btScalar jointMaxVelocity; + return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity); +} + +void PhysXURDFImporter::setRootTransformInWorld(const btTransform& rootTransformInWorld) +{ + m_data->m_urdfParser.getModel().m_rootTransformInWorld = rootTransformInWorld; +} + +bool PhysXURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const +{ + rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld; + return true; +} + + +const struct UrdfLink* PhysXURDFImporter::getUrdfLink(int urdfLinkIndex) const +{ + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex); + btAssert(linkPtr); + if (linkPtr) + { + const UrdfLink* link = *linkPtr; + return link; + } + return 0; +} + +const struct UrdfModel* PhysXURDFImporter::getUrdfModel() const +{ + return &m_data->m_urdfParser.getModel(); +} + + +int PhysXURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const +{ + UrdfCollision* col = m_data->m_bulletCollisionShape2UrdfCollision.find(collisionShape); + if (col) + { + collision = *col; + return 1; + } + return 0; +} + +#if 0 +btCollisionShape* PhysXURDFImporter::convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) const +{ + + B3_PROFILE("convertURDFToCollisionShape"); + + btCollisionShape* shape = 0; + + switch (collision->m_geometry.m_type) + { + case URDF_GEOM_PLANE: + { + btVector3 planeNormal = collision->m_geometry.m_planeNormal; + btScalar planeConstant = 0; //not available? + btStaticPlaneShape* plane = new btStaticPlaneShape(planeNormal, planeConstant); + shape = plane; + shape->setMargin(gUrdfDefaultCollisionMargin); + break; + } + case URDF_GEOM_CAPSULE: + { + btScalar radius = collision->m_geometry.m_capsuleRadius; + btScalar height = collision->m_geometry.m_capsuleHeight; + btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height); + shape = capsuleShape; + shape->setMargin(gUrdfDefaultCollisionMargin); + break; + } + + case URDF_GEOM_CYLINDER: + { + btScalar cylRadius = collision->m_geometry.m_capsuleRadius; + btScalar cylHalfLength = 0.5 * collision->m_geometry.m_capsuleHeight; + if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER) + { + btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength); + btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + shape = cylZShape; + } + else + { + btAlignedObjectArray vertices; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i = 0; i < numSteps; i++) + { + btVector3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylHalfLength); + vertices.push_back(vert); + vert[2] = -cylHalfLength; + vertices.push_back(vert); + } + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(gUrdfDefaultCollisionMargin); + cylZShape->recalcLocalAabb(); + if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES) + { + cylZShape->initializePolyhedralFeatures(); + } + cylZShape->optimizeConvexHull(); + shape = cylZShape; + } + + break; + } + case URDF_GEOM_BOX: + { + btVector3 extents = collision->m_geometry.m_boxSize; + btBoxShape* boxShape = new btBoxShape(extents * 0.5f); + //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); + if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES) + { + boxShape->initializePolyhedralFeatures(); + } + shape = boxShape; + shape->setMargin(gUrdfDefaultCollisionMargin); + break; + } + case URDF_GEOM_SPHERE: + { + btScalar radius = collision->m_geometry.m_sphereRadius; + btSphereShape* sphereShape = new btSphereShape(radius); + shape = sphereShape; + shape->setMargin(gUrdfDefaultCollisionMargin); + break; + } + case URDF_GEOM_CDF: + { + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + + btAlignedObjectArray sdfData; + { + std::streampos fsize = 0; + std::ifstream file(relativeFileName, std::ios::binary); + if (file.good()) + { + fsize = file.tellg(); + file.seekg(0, std::ios::end); + fsize = file.tellg() - fsize; + file.seekg(0, std::ios::beg); + sdfData.resize(fsize); + int bytesRead = file.rdbuf()->sgetn(&sdfData[0], fsize); + btAssert(bytesRead == fsize); + file.close(); + } + } + + if (sdfData.size()) + { + btSdfCollisionShape* sdfShape = new btSdfCollisionShape(); + bool valid = sdfShape->initializeSDF(&sdfData[0], sdfData.size()); + btAssert(valid); + + if (valid) + { + shape = sdfShape; + } + else + { + delete sdfShape; + } + } + } + break; + } + case URDF_GEOM_MESH: + { + GLInstanceGraphicsShape* glmesh = 0; + switch (collision->m_geometry.m_meshFileType) + { + case UrdfGeometry::FILE_OBJ: + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix,m_data->m_fileIO); + } + else + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO); + //create a convex hull for each shape, and store it in a btCompoundShape + + btAssert(0); + + return shape; + } + break; + + case UrdfGeometry::FILE_STL: + glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO); + break; + + case UrdfGeometry::FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; + upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2, m_data->m_fileIO); + + glmesh = new GLInstanceGraphicsShape; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; i < visualShapeInstances.size(); i++) + { + ColladaGraphicsInstance* instance = &visualShapeInstances[i]; + GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; i < gfxShape->m_vertices->size(); i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; k < additionalIndices; k++) + { + glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); + //upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat * instance->m_worldTransform * upAxisMat; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + + for (int v = 0; v < verts.size(); v++) + { + btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]); + pos = worldMat * pos; + verts[v].xyzw[0] = float(pos[0]); + verts[v].xyzw[1] = float(pos[1]); + verts[v].xyzw[2] = float(pos[2]); + glmesh->m_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(success.c_str()); + break; + } + } + + if (!glmesh || glmesh->m_numvertices <= 0) + { + b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, collision->m_geometry.m_meshFileName.c_str()); + delete glmesh; + break; + } + + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i = 0; i < glmesh->m_numvertices; i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0] * collision->m_geometry.m_meshScale[0], + glmesh->m_vertices->at(i).xyzw[1] * collision->m_geometry.m_meshScale[1], + glmesh->m_vertices->at(i).xyzw[2] * collision->m_geometry.m_meshScale[2])); + } + + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + BT_PROFILE("convert trimesh"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + m_data->m_allocatedMeshInterfaces.push_back(meshInterface); + { + BT_PROFILE("convert vertices"); + + for (int i = 0; i < glmesh->m_numIndices / 3; i++) + { + const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i * 3)]; + const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i * 3 + 1)]; + const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i * 3 + 2)]; + meshInterface->addTriangle(v0, v1, v2); + } + } + { + BT_PROFILE("create btBvhTriangleMeshShape"); + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true); + //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; + } + } + else + { + BT_PROFILE("convert btConvexHullShape"); + btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + convexHull->optimizeConvexHull(); + if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES) + { + convexHull->initializePolyhedralFeatures(); + } + convexHull->setMargin(gUrdfDefaultCollisionMargin); + convexHull->recalcLocalAabb(); + //convexHull->setLocalScaling(collision->m_geometry.m_meshScale); + shape = convexHull; + } + + delete glmesh; + break; + } // mesh case + + default: + b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type); + } + if (shape && collision->m_geometry.m_type == URDF_GEOM_MESH) + { + m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision); + } + return shape; +} + + +void PhysXURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const +{ + BT_PROFILE("convertURDFToVisualShapeInternal"); + + GLInstanceGraphicsShape* glmesh = 0; + + btConvexShape* convexColShape = 0; + + switch (visual->m_geometry.m_type) + { + case URDF_GEOM_CAPSULE: + { + btScalar radius = visual->m_geometry.m_capsuleRadius; + btScalar height = visual->m_geometry.m_capsuleHeight; + btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height); + convexColShape = capsuleShape; + convexColShape->setMargin(gUrdfDefaultCollisionMargin); + break; + } + case URDF_GEOM_CYLINDER: + { + btAlignedObjectArray vertices; + + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i = 0; i < numSteps; i++) + { + btScalar cylRadius = visual->m_geometry.m_capsuleRadius; + btScalar cylLength = visual->m_geometry.m_capsuleHeight; + + btVector3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylLength / 2.); + vertices.push_back(vert); + vert[2] = -cylLength / 2.; + vertices.push_back(vert); + } + + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(gUrdfDefaultCollisionMargin); + cylZShape->recalcLocalAabb(); + convexColShape = cylZShape; + break; + } + + case URDF_GEOM_BOX: + { + btVector3 extents = visual->m_geometry.m_boxSize; + int strideInBytes = 9 * sizeof(float); + int numVertices = sizeof(cube_vertices_textured) / strideInBytes; + int numIndices = sizeof(cube_indices) / sizeof(int); + glmesh = new GLInstanceGraphicsShape; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_indices->resize(numIndices); + for (int k = 0; k < numIndices; k++) + { + glmesh->m_indices->at(k) = cube_indices[k]; + } + glmesh->m_vertices->resize(numVertices); + + btScalar halfExtentsX = extents[0] * 0.5; + btScalar halfExtentsY = extents[1] * 0.5; + btScalar halfExtentsZ = extents[2] * 0.5; + GLInstanceVertex* verts = &glmesh->m_vertices->at(0); + btScalar textureScaling = 1; + + for (int i = 0; i < numVertices; i++) + { + verts[i].xyzw[0] = halfExtentsX * cube_vertices_textured[i * 9]; + verts[i].xyzw[1] = halfExtentsY * cube_vertices_textured[i * 9 + 1]; + verts[i].xyzw[2] = halfExtentsZ * cube_vertices_textured[i * 9 + 2]; + verts[i].xyzw[3] = cube_vertices_textured[i * 9 + 3]; + verts[i].normal[0] = cube_vertices_textured[i * 9 + 4]; + verts[i].normal[1] = cube_vertices_textured[i * 9 + 5]; + verts[i].normal[2] = cube_vertices_textured[i * 9 + 6]; + verts[i].uv[0] = cube_vertices_textured[i * 9 + 7] * textureScaling; + verts[i].uv[1] = cube_vertices_textured[i * 9 + 8] * textureScaling; + } + + glmesh->m_numIndices = numIndices; + glmesh->m_numvertices = numVertices; + break; + } + + case URDF_GEOM_SPHERE: + { + btScalar radius = visual->m_geometry.m_sphereRadius; + btSphereShape* sphereShape = new btSphereShape(radius); + convexColShape = sphereShape; + convexColShape->setMargin(gUrdfDefaultCollisionMargin); + break; + } + + case URDF_GEOM_MESH: + { + switch (visual->m_geometry.m_meshFileType) + { + case UrdfGeometry::FILE_OBJ: + { + + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) + { + if (meshData.m_textureImage1) + { + BulletURDFTexture texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData1 = meshData.m_textureImage1; + texData.m_isCached = meshData.m_isCached; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; + } + break; + } + + case UrdfGeometry::FILE_STL: + { + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(),m_data->m_fileIO); + break; + } + + case UrdfGeometry::FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; + upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis, + m_data->m_fileIO); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; i < visualShapeInstances.size(); i++) + { + ColladaGraphicsInstance* instance = &visualShapeInstances[i]; + GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; i < gfxShape->m_vertices->size(); i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; k < additionalIndices; k++) + { + glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); + // upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat * upAxisMat * instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + + for (int v = 0; v < verts.size(); v++) + { + btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]); + pos = worldMat * pos; + verts[v].xyzw[0] = float(pos[0]); + verts[v].xyzw[1] = float(pos[1]); + verts[v].xyzw[2] = float(pos[2]); + glmesh->m_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName); + + break; + } + } // switch file type + + if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices <= 0) + { + b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, visual->m_geometry.m_meshFileName.c_str()); + break; + } + + //apply the geometry scaling + for (int i = 0; i < glmesh->m_vertices->size(); i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; + } + break; + } + case URDF_GEOM_PLANE: + { + b3Warning("No default visual for URDF_GEOM_PLANE"); + break; + } + default: + { + b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type); + } + } + + //if we have a convex, tesselate into localVertices/localIndices + if ((glmesh == 0) && convexColShape) + { + BT_PROFILE("convexColShape"); + + btShapeHull* hull = new btShapeHull(convexColShape); + hull->buildHull(0.0); + { + // int strideInBytes = 9*sizeof(float); + int numVertices = hull->numVertices(); + int numIndices = hull->numIndices(); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; i < numVertices; i++) + { + GLInstanceVertex vtx; + btVector3 pos = hull->getVertexPointer()[i]; + vtx.xyzw[0] = pos.x(); + vtx.xyzw[1] = pos.y(); + vtx.xyzw[2] = pos.z(); + vtx.xyzw[3] = 1.f; + pos.normalize(); + vtx.normal[0] = pos.x(); + vtx.normal[1] = pos.y(); + vtx.normal[2] = pos.z(); + btScalar u = btAtan2(vtx.normal[0], vtx.normal[2]) / (2 * SIMD_PI) + 0.5; + btScalar v = vtx.normal[1] * 0.5 + 0.5; + vtx.uv[0] = u; + vtx.uv[1] = v; + glmesh->m_vertices->push_back(vtx); + } + + btAlignedObjectArray indices; + for (int i = 0; i < numIndices; i++) + { + glmesh->m_indices->push_back(hull->getIndexPointer()[i]); + } + + glmesh->m_numvertices = glmesh->m_vertices->size(); + glmesh->m_numIndices = glmesh->m_indices->size(); + } + delete hull; + delete convexColShape; + convexColShape = 0; + } + + if (glmesh && glmesh->m_numIndices > 0 && glmesh->m_numvertices > 0) + { + BT_PROFILE("glmesh"); + int baseIndex = verticesOut.size(); + + for (int i = 0; i < glmesh->m_indices->size(); i++) + { + indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); + } + + for (int i = 0; i < glmesh->m_vertices->size(); i++) + { + GLInstanceVertex& v = glmesh->m_vertices->at(i); + btVector3 vert(v.xyzw[0], v.xyzw[1], v.xyzw[2]); + btVector3 vt = visualTransform * vert; + v.xyzw[0] = vt[0]; + v.xyzw[1] = vt[1]; + v.xyzw[2] = vt[2]; + btVector3 triNormal(v.normal[0], v.normal[1], v.normal[2]); + triNormal = visualTransform.getBasis() * triNormal; + v.normal[0] = triNormal[0]; + v.normal[1] = triNormal[1]; + v.normal[2] = triNormal[2]; + verticesOut.push_back(v); + } + } + delete glmesh; +} + +int PhysXURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const +{ + int graphicsIndex = -1; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; + startTrans.setIdentity(); + btAlignedObjectArray textures; + + const UrdfModel& model = m_data->m_urdfParser.getModel(); + UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); + if (linkPtr) + { + const UrdfLink* link = *linkPtr; + + for (int v = 0; v < link->m_visualArray.size(); v++) + { + const UrdfVisual& vis = link->m_visualArray[v]; + btTransform childTrans = vis.m_linkLocalFrame; + btHashString matName(vis.m_materialName.c_str()); + UrdfMaterial* const* matPtr = model.m_materials[matName]; + b3ImportMeshData meshData; + + convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures,meshData); + + if (m_data->m_flags&CUF_USE_MATERIAL_COLORS_FROM_MTL) + { + if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) && + (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) + { + UrdfMaterialColor matCol; + + if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) + { + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + meshData.m_rgbaColor[3]); + } else + { + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + 1); + } + + matCol.m_specularColor.setValue(meshData.m_specularColor[0], + meshData.m_specularColor[1], + meshData.m_specularColor[2]); + m_data->m_linkColors.insert(linkIndex, matCol); + } + } else + { + if (matPtr) + { + UrdfMaterial* const mat = *matPtr; + //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); + UrdfMaterialColor matCol; + matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; + matCol.m_specularColor = mat->m_matColor.m_specularColor; + m_data->m_linkColors.insert(linkIndex, matCol); + } + } + + } + } + if (vertices.size() && indices.size()) + { + // graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); + //graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); + + //CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface(); + + if (1) + { + int textureIndex = -1; + if (textures.size()) + { + textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height); + if (textureIndex >= 0) + { + m_data->m_allocatedTextures.push_back(textureIndex); + } + } + { + B3_PROFILE("registerGraphicsShape"); + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); + } + } + } + + //delete textures + for (int i = 0; i < textures.size(); i++) + { + B3_PROFILE("free textureData"); + if (!textures[i].m_isCached) + { + free(textures[i].textureData1); + } + } + return graphicsIndex; +} +#endif +bool PhysXURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const +{ + const UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex]; + if (matColPtr) + { + colorRGBA = matColPtr->m_rgbaColor; + return true; + } + return false; +} + +bool PhysXURDFImporter::getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const +{ + UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex]; + if (matColPtr) + { + matCol = *matColPtr; + return true; + } + return false; +} + +void PhysXURDFImporter::setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const +{ + m_data->m_linkColors.insert(linkIndex, matCol); +} + +bool PhysXURDFImporter::getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const +{ + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdflinkIndex); + if (linkPtr) + { + const UrdfLink* link = *linkPtr; + contactInfo = link->m_contactInfo; + return true; + } + return false; +} + +bool PhysXURDFImporter::getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const +{ + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + if (linkPtr) + { + const UrdfLink* link = *linkPtr; + if (link->m_audioSource.m_flags & SDFAudioSource::SDFAudioSourceValid) + { + audioSource = link->m_audioSource; + return true; + } + } + return false; +} + +void PhysXURDFImporter::setEnableTinyRenderer(bool enable) +{ + m_data->m_enableTinyRenderer = enable; +} + + + +int PhysXURDFImporter::convertLinkVisualShapes3( + int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + const UrdfLink* linkPtr, const UrdfModel* model, + int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const +{ + return 0; +} + +void PhysXURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const +{ + if (m_data->m_enableTinyRenderer && m_data->m_customVisualShapesConverter) + { + const UrdfModel& model = m_data->m_urdfParser.getModel(); + UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex); + if (linkPtr) + { + m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, 0, bodyUniqueId, m_data->m_fileIO); + } + } +} + +int PhysXURDFImporter::getNumAllocatedCollisionShapes() const +{ + return m_data->m_allocatedCollisionShapes.size(); +} + +btCollisionShape* PhysXURDFImporter::getAllocatedCollisionShape(int index) +{ + return m_data->m_allocatedCollisionShapes[index]; +} + +int PhysXURDFImporter::getNumAllocatedMeshInterfaces() const +{ + return 0;// m_data->m_allocatedMeshInterfaces.size(); +} + +btStridingMeshInterface* PhysXURDFImporter::getAllocatedMeshInterface(int index) +{ + return 0;// m_data->m_allocatedMeshInterfaces[index]; +} + +int PhysXURDFImporter::getNumAllocatedTextures() const +{ + return m_data->m_allocatedTextures.size(); +} + +int PhysXURDFImporter::getAllocatedTexture(int index) const +{ + return m_data->m_allocatedTextures[index]; +} + +int PhysXURDFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const +{ + int result = 0; + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + for (int v = 0; v < link->m_collisionArray.size(); v++) + { + const UrdfCollision& col = link->m_collisionArray[v]; + if (col.m_flags & URDF_HAS_COLLISION_GROUP) + { + colGroup = col.m_collisionGroup; + result |= URDF_HAS_COLLISION_GROUP; + } + if (col.m_flags & URDF_HAS_COLLISION_MASK) + { + colMask = col.m_collisionMask; + result |= URDF_HAS_COLLISION_MASK; + } + } + } + return result; +} + +#if 0 +class btCompoundShape* PhysXURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const +{ + + btCompoundShape* compoundShape = new btCompoundShape(); + m_data->m_allocatedCollisionShapes.push_back(compoundShape); + + compoundShape->setMargin(gUrdfDefaultCollisionMargin); + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + + for (int v = 0; v < link->m_collisionArray.size(); v++) + { + const UrdfCollision& col = link->m_collisionArray[v]; + btCollisionShape* childShape = convertURDFToCollisionShape(&col, pathPrefix); + if (childShape) + { + m_data->m_allocatedCollisionShapes.push_back(childShape); + if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) + { + btCompoundShape* compound = (btCompoundShape*)childShape; + for (int i = 0; i < compound->getNumChildShapes(); i++) + { + m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i)); + } + } + + btTransform childTrans = col.m_linkLocalFrame; + + compoundShape->addChildShape(localInertiaFrame.inverse() * childTrans, childShape); + } + } + } + + return compoundShape; +} +#endif diff --git a/examples/SharedMemory/physx/PhysXUrdfImporter.h b/examples/SharedMemory/physx/PhysXUrdfImporter.h new file mode 100644 index 000000000..2caef42cb --- /dev/null +++ b/examples/SharedMemory/physx/PhysXUrdfImporter.h @@ -0,0 +1,116 @@ +#ifndef PHYSX_URDF_IMPORTER_H +#define PHYSX_URDF_IMPORTER_H + +#include "../../Importers/ImportURDFDemo/URDFImporterInterface.h" +#include "../../Importers/ImportURDFDemo/UrdfRenderingInterface.h" + +struct PhysXURDFTexture +{ + int m_width; + int m_height; + unsigned char* textureData1; + bool m_isCached; +}; + + + + + +///PhysXURDFImporter can deal with URDF and SDF files +class PhysXURDFImporter : public URDFImporterInterface +{ + struct PhysXURDFInternalData* m_data; + +public: + PhysXURDFImporter(struct CommonFileIOInterface* fileIO=0,double globalScaling=1, int flags=0); + + virtual ~PhysXURDFImporter(); + + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); + + //warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path + virtual bool loadSDF(const char* fileName, bool forceFixedBase = false); + virtual int getNumModels() const; + virtual void activateModel(int modelIndex); + virtual void setBodyUniqueId(int bodyId); + virtual int getBodyUniqueId() const; + const char* getPathPrefix(); + + void printTree(); //for debugging + + virtual int getRootLinkIndex() const; + + virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; + + virtual std::string getBodyName() const; + + virtual std::string getLinkName(int linkIndex) const; + + virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; + + virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const; + + virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const; + + virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const; + + virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const; + + virtual std::string getJointName(int linkIndex) const; + + virtual void getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; + virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const; + + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const; + virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; + virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld); + + + + virtual int convertLinkVisualShapes3( + int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + const struct UrdfLink* linkPtr, const UrdfModel* model, + int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const; + + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; + + class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const + { + return 0; + } + + virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const; + + ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation + + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const + { + return 0; + } + + virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const; + + virtual const struct UrdfModel* getUrdfModel() const; + + virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const; + + virtual int getNumAllocatedCollisionShapes() const; + virtual class btCollisionShape* getAllocatedCollisionShape(int index); + + virtual int getNumAllocatedMeshInterfaces() const; + virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index); + + virtual int getNumAllocatedTextures() const; + virtual int getAllocatedTexture(int index) const; + + virtual void setEnableTinyRenderer(bool enable); + //void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const; + + + int getNumPhysXLinks() const; + +}; + +#endif //PHYSX_URDF_IMPORTER_H diff --git a/examples/SharedMemory/physx/PhysXUserData.h b/examples/SharedMemory/physx/PhysXUserData.h new file mode 100644 index 000000000..8f4642a81 --- /dev/null +++ b/examples/SharedMemory/physx/PhysXUserData.h @@ -0,0 +1,8 @@ +#ifndef PHYSX_USER_DATA_H +#define PHYSX_USER_DATA_H + +struct MyPhysXUserData +{ + int m_graphicsUniqueId; +}; +#endif //PHYSX_USER_DATA_H \ No newline at end of file diff --git a/examples/SharedMemory/physx/URDF2PhysX.cpp b/examples/SharedMemory/physx/URDF2PhysX.cpp new file mode 100644 index 000000000..51a115605 --- /dev/null +++ b/examples/SharedMemory/physx/URDF2PhysX.cpp @@ -0,0 +1,908 @@ + +#include "URDF2PhysX.h" + +#include "PhysXUrdfImporter.h" +#include "Bullet3Common/b3Logging.h" +#include "PxArticulationReducedCoordinate.h" +#include "PxArticulationJointReducedCoordinate.h" +#include "PxRigidActorExt.h" +#include "PxArticulation.h" +#include "PxRigidBodyExt.h" +#include "PxRigidBody.h" +#include "LinearMath/btThreads.h" +#include "PxRigidActorExt.h" +#include "PxArticulationBase.h" +#include "PxArticulationLink.h" +#include "PxMaterial.h" +#include "PxCooking.h" +#include "PxScene.h" +#include "PxAggregate.h" + +#include "PhysXUserData.h" +#include "../../CommonInterfaces/CommonFileIOInterface.h" + +#include "Importers/ImportURDFDemo/UrdfParser.h" + + + + +struct URDF2PhysXCachedData +{ + URDF2PhysXCachedData() + : m_currentMultiBodyLinkIndex(-1), + m_articulation(0), + m_totalNumJoints1(0) + { + } + //these arrays will be initialized in the 'InitURDF2BulletCache' + + btAlignedObjectArray m_urdfLinkParentIndices; + btAlignedObjectArray m_urdfLinkIndices2BulletLinkIndices; + btAlignedObjectArray m_urdfLink2physxLink; + btAlignedObjectArray m_urdfLinkLocalInertialFrames; + + int m_currentMultiBodyLinkIndex; + + physx::PxArticulationReducedCoordinate* m_articulation; + + btAlignedObjectArray m_linkTransWorldSpace; + btAlignedObjectArray m_urdfLinkIndex; + btAlignedObjectArray m_parentUrdfLinkIndex; + btAlignedObjectArray m_linkMasses; + btAlignedObjectArray m_jointTypes; + + btAlignedObjectArray m_parentLocalPoses; + btAlignedObjectArray m_childLocalPoses; + btAlignedObjectArray m_geomTypes; + btAlignedObjectArray m_geomDimensions; + btAlignedObjectArray m_linkMaterials; + btAlignedObjectArray m_geomLocalPoses; + + + + //this will be initialized in the constructor + int m_totalNumJoints1; + int getParentUrdfIndex(int linkIndex) const + { + return m_urdfLinkParentIndices[linkIndex]; + } + int getMbIndexFromUrdfIndex(int urdfIndex) const + { + if (urdfIndex == -2) + return -2; + return m_urdfLinkIndices2BulletLinkIndices[urdfIndex]; + } + + void registerMultiBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& localInertialFrame) + { + m_urdfLink2physxLink[urdfLinkIndex] = body; + m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; + } + + class physx::PxArticulationLink* getPhysxLinkFromLink(int urdfLinkIndex) + { + return m_urdfLink2physxLink[urdfLinkIndex]; + } + + void registerRigidBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame) + { + btAssert(m_urdfLink2physxLink[urdfLinkIndex] == 0); + + m_urdfLink2physxLink[urdfLinkIndex] = body; + m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; + } +}; + + +void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int urdfLinkIndex, int urdfParentIndex) +{ + cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex; + cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++; + + btAlignedObjectArray childIndices; + u2b.getLinkChildIndices(urdfLinkIndex, childIndices); + for (int i = 0; i < childIndices.size(); i++) + { + ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex); + } +} + +void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int linkIndex) +{ + btAlignedObjectArray childIndices; + u2b.getLinkChildIndices(linkIndex, childIndices); + cache.m_totalNumJoints1 += childIndices.size(); + for (int i = 0; i < childIndices.size(); i++) + { + int childIndex = childIndices[i]; + ComputeTotalNumberOfJoints(u2b, cache, childIndex); + } +} + + +void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int flags) +{ + //compute the number of links, and compute parent indices array (and possibly other cached data?) + cache.m_totalNumJoints1 = 0; + + int rootLinkIndex = u2b.getRootLinkIndex(); + if (rootLinkIndex >= 0) + { + ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex); + int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1; + + cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase); + cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase); + cache.m_urdfLink2physxLink.resize(numTotalLinksIncludingBase); + cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase); + + cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1 + + bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER) != 0; + if (maintainLinkOrder) + { + URDF2PhysXCachedData cache2 = cache; + + ComputeParentIndices(u2b, cache2, rootLinkIndex, -2); + + for (int j = 0; j m_mb2urdfLink; + void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) + { + if (m_mb2urdfLink.size() < (mbLinkIndex + 1)) + { + m_mb2urdfLink.resize((mbLinkIndex + 1), -2); + } + // m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex; + m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex; + } +}; + +static btVector4 colors4[4] = +{ + btVector4(1, 0, 0, 1), + btVector4(0, 1, 0, 1), + btVector4(0, 1, 1, 1), + btVector4(1, 1, 0, 1), +}; + +static btVector4 selectColor4() +{ + static btSpinMutex sMutex; + sMutex.lock(); + static int curColor = 0; + btVector4 color = colors4[curColor]; + curColor++; + curColor &= 3; + sMutex.unlock(); + return color; +} + + + + +static physx::PxConvexMesh* createPhysXConvex(physx::PxU32 numVerts, const physx::PxVec3* verts, ArticulationCreationInterface& creation) +{ + physx::PxCookingParams params = creation.m_cooking->getParams(); + + // Use the new (default) PxConvexMeshCookingType::eQUICKHULL + params.convexMeshCookingType = physx::PxConvexMeshCookingType::eQUICKHULL; + + // If the gaussMapLimit is chosen higher than the number of output vertices, no gauss map is added to the convex mesh data (here 256). + // If the gaussMapLimit is chosen lower than the number of output vertices, a gauss map is added to the convex mesh data (here 16). + int gaussMapLimit = 256; + + params.gaussMapLimit = gaussMapLimit; + creation.m_cooking->setParams(params); + + // Setup the convex mesh descriptor + physx::PxConvexMeshDesc desc; + + // We provide points only, therefore the PxConvexFlag::eCOMPUTE_CONVEX flag must be specified + desc.points.data = verts; + desc.points.count = numVerts; + desc.points.stride = sizeof(physx::PxVec3); + desc.flags = physx::PxConvexFlag::eCOMPUTE_CONVEX; + + physx::PxU32 meshSize = 0; + + // Directly insert mesh into PhysX + physx::PxConvexMesh* convex = creation.m_cooking->createConvexMesh(desc, creation.m_physics->getPhysicsInsertionCallback()); + PX_ASSERT(convex); + + return convex; +} + + +int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, ArticulationCreationInterface& creation, int urdfLinkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + physx::PxArticulationReducedCoordinate* articulation, int mbLinkIndex, physx::PxArticulationLink* linkPtr) +{ + int numShapes = 0; + + URDFLinkContactInfo contactInfo; + u2b.getLinkContactInfo(urdfLinkIndex, contactInfo); + + //static friction, dynamic frictoin, restitution + cache.m_linkMaterials.push_back(physx::PxVec3(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution)); + physx::PxMaterial* material = creation.m_physics->createMaterial(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution); + + const UrdfLink* link = u2b.getUrdfLink(urdfLinkIndex);//.convertLinkCollisionShapes m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + btAssert(linkPtr); + if (linkPtr) + { + + for (int v = 0; v < link->m_collisionArray.size(); v++) + { + const UrdfCollision& col = link->m_collisionArray[v]; + const UrdfCollision* collision = &col; + btTransform childTrans = col.m_linkLocalFrame; + btTransform localTrans = localInertiaFrame.inverse()*childTrans; + physx::PxTransform tr; + tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]); + tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]); + + physx::PxShape* shape = 0; + cache.m_geomTypes.push_back(col.m_geometry.m_type); + + + switch (col.m_geometry.m_type) + { + case URDF_GEOM_PLANE: + { + btVector3 planeNormal = col.m_geometry.m_planeNormal; + btScalar planeConstant = 0; //not available? + //PxPlane(1, 0, 0, 0). + shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material); + //todo: compensate for plane axis + //physx::PxTransform localPose = tr*physx::PxTransform + //shape->setLocalPose(localPose); + numShapes++; + break; + } + case URDF_GEOM_CAPSULE: + { + btScalar radius = collision->m_geometry.m_capsuleRadius; + btScalar height = collision->m_geometry.m_capsuleHeight; + + shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxCapsuleGeometry(radius, 0.5*height), *material); + + btTransform childTrans = col.m_linkLocalFrame; + btTransform x2z; + x2z.setIdentity(); + x2z.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI)); + btTransform localTrans = localInertiaFrame.inverse()*childTrans*x2z; + physx::PxTransform tr; + tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]); + tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]); + + shape->setLocalPose(tr); + cache.m_geomLocalPoses.push_back(tr); + numShapes++; + //btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height); + //shape = capsuleShape; + //shape->setMargin(gUrdfDefaultCollisionMargin); + break; + } + + case URDF_GEOM_CYLINDER: + { + btScalar cylRadius = collision->m_geometry.m_capsuleRadius; + btScalar cylHalfLength = 0.5 * collision->m_geometry.m_capsuleHeight; + cache.m_geomDimensions.push_back(physx::PxVec3(cylRadius, cylHalfLength,0)); + //if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER) + //{ + // btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength); + // btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + // shape = cylZShape; + //} + //else + { + btAlignedObjectArray vertices; + + int numSteps = 32; + for (int i = 0; i < numSteps; i++) + { + physx::PxVec3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylHalfLength); + vertices.push_back(vert); + vert[2] = -cylHalfLength; + vertices.push_back(vert); + } + + physx::PxConvexMesh* convexMesh = createPhysXConvex(vertices.size(), &vertices[0], creation); + + shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, + physx::PxConvexMeshGeometry(convexMesh), *material); + + shape->setLocalPose(tr); + cache.m_geomLocalPoses.push_back(tr); + numShapes++; + + } + break; + } + case URDF_GEOM_BOX: + { + btVector3 extents = collision->m_geometry.m_boxSize; + shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxBoxGeometry(extents[0] * 0.5, extents[1] * 0.5, extents[2] * 0.5), *material); + cache.m_geomDimensions.push_back(physx::PxVec3(extents[0]*0.5, extents[1] * 0.5, extents[2] * 0.5)); + shape->setLocalPose(tr); + cache.m_geomLocalPoses.push_back(tr); + numShapes++; + break; + } + case URDF_GEOM_SPHERE: + { + btScalar radius = collision->m_geometry.m_sphereRadius; + shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxSphereGeometry(radius), *material); + cache.m_geomDimensions.push_back(physx::PxVec3(radius,0,0)); + shape->setLocalPose(tr); + cache.m_geomLocalPoses.push_back(tr); + numShapes++; + break; + } + + default: + { + } + } + + if (shape) + { + //see https://github.com/NVIDIAGameWorks/PhysX/issues/21 + physx::PxReal contactOffset = shape->getContactOffset(); + physx::PxReal restOffset = shape->getContactOffset(); + + //shape->setContactOffset(physx::PxReal(.03)); + //shape->setRestOffset(physx::PxReal(.01)); // + } + } + + } + + + return numShapes; +} + +btTransform ConvertURDF2PhysXInternal( + const PhysXURDFImporter& u2b, + ArticulationCreationInterface& creation, + URDF2PhysXCachedData& cache, int urdfLinkIndex, + const btTransform& parentTransformInWorldSpace, + bool createMultiBody, const char* pathPrefix, + int flags, + UrdfVisualShapeCache2* cachedLinkGraphicsShapesIn, + UrdfVisualShapeCache2* cachedLinkGraphicsShapesOut, + bool recursive) +{ + B3_PROFILE("ConvertURDF2PhysXInternal"); + //b3Printf("start converting/extracting data from URDF interface\n"); + + btTransform linkTransformInWorldSpace; + linkTransformInWorldSpace.setIdentity(); + + int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex); + + int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex); + int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); + + + + //b3Printf("mb link index = %d\n",mbLinkIndex); + + btTransform parentLocalInertialFrame; + parentLocalInertialFrame.setIdentity(); + btScalar parentMass(1); + btVector3 parentLocalInertiaDiagonal(1, 1, 1); + + if (urdfParentIndex == -2) + { + //b3Printf("root link has no parent\n"); + } + else + { + //b3Printf("urdf parent index = %d\n",urdfParentIndex); + //b3Printf("mb parent index = %d\n",mbParentIndex); + //parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); + u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags); + } + + btScalar mass = 0; + btTransform localInertialFrame; + localInertialFrame.setIdentity(); + btVector3 localInertiaDiagonal(0, 0, 0); + u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags); + + btTransform parent2joint; + parent2joint.setIdentity(); + + int jointType; + btVector3 jointAxisInJointSpace; + btScalar jointLowerLimit; + btScalar jointUpperLimit; + btScalar jointDamping; + btScalar jointFriction; + btScalar jointMaxForce; + btScalar jointMaxVelocity; + + bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity); + + btTransform axis2Reference; + axis2Reference.setIdentity(); + + switch (jointType) + { + case URDFContinuousJoint: + case URDFPrismaticJoint: + case URDFRevoluteJoint: + { + //rotate from revolute 'axis' to standard X axis + btVector3 refAxis(1, 0, 0); + btVector3 axis = jointAxisInJointSpace; + //btQuaternion axis2ReferenceRot(btVector3(0, 1, 0), SIMD_HALF_PI);// = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2])); + btQuaternion axis2ReferenceRot = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2])); + axis2Reference.setRotation(axis2ReferenceRot); + break; + } + default: + { + } + }; + + parent2joint = parent2joint*axis2Reference; + //localInertialFrame = axis2Reference.inverse()*localInertialFrame; + + std::string linkName = u2b.getLinkName(urdfLinkIndex); + + if (flags & CUF_USE_SDF) + { + parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace; + } + else + { + if (flags & CUF_USE_MJCF) + { + linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace; + } + else + { + linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint; + } + } + + + int graphicsIndex; + { + B3_PROFILE("convertLinkVisualShapes"); + + + + graphicsIndex = u2b.convertLinkVisualShapes3(urdfLinkIndex, pathPrefix, localInertialFrame, u2b.getUrdfLink(urdfLinkIndex), u2b.getUrdfModel(), -1, u2b.getBodyUniqueId(), creation.m_fileIO); + + +#if 0 + if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1)) + { + graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1]; + UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1]; + u2b.setLinkColor2(urdfLinkIndex, matColor); + } + else + { + graphicsIndex = + if (cachedLinkGraphicsShapesOut) + { + cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex); + UrdfMaterialColor matColor; + u2b.getLinkColor2(urdfLinkIndex, matColor); + cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor); + } + } +#endif + } + + if (1) + { + UrdfMaterialColor matColor; + btVector4 color2 = selectColor4(); + btVector3 specular(0.5, 0.5, 0.5); + if (u2b.getLinkColor2(urdfLinkIndex, matColor)) + { + color2 = matColor.m_rgbaColor; + specular = matColor.m_specularColor; + } + + /* + if (visual->material.get()) + { + color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); + } + */ + if (mass) + { + if (!(flags & CUF_USE_URDF_INERTIA)) + { + //b3Assert(0); + //compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + btAssert(localInertiaDiagonal[0] < 1e10); + btAssert(localInertiaDiagonal[1] < 1e10); + btAssert(localInertiaDiagonal[2] < 1e10); + } + URDFLinkContactInfo contactInfo; + u2b.getLinkContactInfo(urdfLinkIndex, contactInfo); + //temporary inertia scaling until we load inertia from URDF + if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING) + { + localInertiaDiagonal *= contactInfo.m_inertiaScaling; + } + } + + + btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame; + bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0; + + physx::PxArticulationLink* linkPtr = 0; + + if (!createMultiBody) + { + + } + else + { + physx::PxTransform tr; + tr.p = physx::PxVec3(linkTransformInWorldSpace.getOrigin().x(), linkTransformInWorldSpace.getOrigin().y(), linkTransformInWorldSpace.getOrigin().z()); + tr.q = physx::PxQuat(linkTransformInWorldSpace.getRotation().x(), linkTransformInWorldSpace.getRotation().y(), linkTransformInWorldSpace.getRotation().z(), linkTransformInWorldSpace.getRotation().w()); + + cache.m_linkTransWorldSpace.push_back(tr); + cache.m_urdfLinkIndex.push_back(urdfLinkIndex); + cache.m_parentUrdfLinkIndex.push_back(urdfParentIndex); + cache.m_linkMasses.push_back(mass); + + if (cache.m_articulation == 0) + { + + bool isFixedBase = (mass == 0); //todo: figure out when base is fixed + + cache.m_articulation = creation.m_physics->createArticulationReducedCoordinate(); + + if (isFixedBase) + { + cache.m_articulation->setArticulationFlags(physx::PxArticulationFlag::eFIX_BASE); + } + + + + physx::PxArticulationLink* base = cache.m_articulation->createLink(NULL,tr); + + linkPtr = base; + + + //physx::PxRigidActorExt::createExclusiveShape(*base, PxBoxGeometry(0.5f, 0.25f, 1.5f), *gMaterial); + physx::PxRigidBody& body = *base; + + //Now create the slider and fixed joints... + + cache.m_articulation->setSolverIterationCounts(32);//todo: API? + + cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED); + cache.m_parentLocalPoses.push_back(physx::PxTransform()); + cache.m_childLocalPoses.push_back(physx::PxTransform()); + + // Stabilization can create artefacts on jointed objects so we just disable it + //cache.m_articulation->setStabilizationThreshold(0.0f); + //cache.m_articulation->setMaxProjectionIterations(16); + //cache.m_articulation->setSeparationTolerance(0.001f); + +#if 0 + int totalNumJoints = cache.m_totalNumJoints1; + cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep); + if (flags & CUF_GLOBAL_VELOCITIES_MB) + { + cache.m_bulletMultiBody->useGlobalVelocities(true); + } + if (flags & CUF_USE_MJCF) + { + cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace); + } + + +#endif + } + else + { + + physx::PxArticulationLink* parentLink = cache.getPhysxLinkFromLink(urdfParentIndex); + linkPtr = cache.m_articulation->createLink(parentLink, tr); + + physx::PxArticulationJointReducedCoordinate* joint = static_cast(linkPtr->getInboundJoint()); + + switch (jointType) + { + case URDFFixedJoint: + { + joint->setJointType(physx::PxArticulationJointType::eFIX); + break; + } + case URDFSphericalJoint: + { + joint->setJointType(physx::PxArticulationJointType::eSPHERICAL); + joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE); + joint->setMotion(physx::PxArticulationAxis::eSWING2, physx::PxArticulationMotion::eFREE); + joint->setMotion(physx::PxArticulationAxis::eSWING1, physx::PxArticulationMotion::eFREE); + break; + } + case URDFContinuousJoint: + case URDFRevoluteJoint: + { + joint->setJointType(physx::PxArticulationJointType::eREVOLUTE); + joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE); + + break; + } + case URDFPrismaticJoint: + { + joint->setJointType(physx::PxArticulationJointType::ePRISMATIC); + joint->setMotion(physx::PxArticulationAxis::eX, physx::PxArticulationMotion::eFREE); + break; + } + default: + { + joint->setJointType(physx::PxArticulationJointType::eFIX); + btAssert(0); + } + }; + + cache.m_jointTypes.push_back(joint->getJointType()); + btTransform offsetInA, offsetInB; + + offsetInA = parentLocalInertialFrame.inverse() * parent2joint; + offsetInB = (axis2Reference.inverse()*localInertialFrame).inverse(); + + physx::PxTransform parentPose(physx::PxVec3(offsetInA.getOrigin()[0], offsetInA.getOrigin()[1], offsetInA.getOrigin()[2]), + physx::PxQuat(offsetInA.getRotation()[0], offsetInA.getRotation()[1], offsetInA.getRotation()[2], offsetInA.getRotation()[3])); + + physx::PxTransform childPose(physx::PxVec3(offsetInB.getOrigin()[0], offsetInB.getOrigin()[1], offsetInB.getOrigin()[2]), + physx::PxQuat(offsetInB.getRotation()[0], offsetInB.getRotation()[1], offsetInB.getRotation()[2], offsetInB.getRotation()[3])); + + cache.m_parentLocalPoses.push_back(parentPose); + cache.m_childLocalPoses.push_back(childPose); + + joint->setParentPose(parentPose); + joint->setChildPose(childPose); + + } + cache.registerMultiBody(urdfLinkIndex, linkPtr, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame); + if (linkPtr) + { + //todo: mem leaks + MyPhysXUserData* userData = new MyPhysXUserData(); + userData->m_graphicsUniqueId = graphicsIndex; + linkPtr->userData = userData; + } + } + + + //create collision shapes + + //physx::PxRigidActorExt::createExclusiveShape + convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr); + + + physx::PxRigidBodyExt::updateMassAndInertia(*linkPtr, mass); + + + //base->setMass(massOut); + //base->setMassSpaceInertiaTensor(diagTensor); + //base->setCMassLocalPose(PxTransform(com, orient)); + + + //create a joint if necessary + if (hasParentJoint) + { + btTransform offsetInA, offsetInB; + offsetInA = parentLocalInertialFrame.inverse() * parent2joint; + offsetInB = localInertialFrame.inverse(); + btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation(); + + bool disableParentCollision = true; + + if (createMultiBody && cache.m_articulation) + { +#if 0 + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction; + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit; + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit; + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce; + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity; +#endif + } + } + + + if (createMultiBody) + { + + } + else + { + + } + }//was if 'shape/compountShape' + + btAlignedObjectArray urdfChildIndices; + u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices); + + int numChildren = urdfChildIndices.size(); + + if (recursive) + { + for (int i = 0; i < numChildren; i++) + { + int urdfChildLinkIndex = urdfChildIndices[i]; + + ConvertURDF2PhysXInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive); + } + } + return linkTransformInWorldSpace; +} + + + + + + + +physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const btTransform& rootTransformInWorldSpace, struct CommonFileIOInterface* fileIO) +{ + URDF2PhysXCachedData cache; + InitURDF2BulletCache(u2p, cache, flags); + int urdfLinkIndex = u2p.getRootLinkIndex(); + int rootIndex = u2p.getRootLinkIndex(); + B3_PROFILE("ConvertURDF2Bullet"); + + UrdfVisualShapeCache2 cachedLinkGraphicsShapesOut; + UrdfVisualShapeCache2 cachedLinkGraphicsShapes; + + ArticulationCreationInterface creation; + creation.m_foundation = foundation; + creation.m_physics = physics; + creation.m_cooking = cooking; + creation.m_scene = scene; + creation.m_fileIO = fileIO; + + bool createMultiBody = true; + + bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER) == 0; + + if (recursive) + { + ConvertURDF2PhysXInternal(u2p, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, createMultiBody, pathPrefix, flags, &cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive); + + } + else + { +#if 0 + btAlignedObjectArray parentTransforms; + if (urdfLinkIndex >= parentTransforms.size()) + { + parentTransforms.resize(urdfLinkIndex + 1); + } + parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace; + btAlignedObjectArray allIndices; + + GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices); + allIndices.quickSort(MyIntCompareFunc); + + for (int i = 0; i < allIndices.size(); i++) + { + int urdfLinkIndex = allIndices[i].m_index; + int parentIndex = allIndices[i].m_parentIndex; + btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace; + btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive); + if ((urdfLinkIndex + 1) >= parentTransforms.size()) + { + parentTransforms.resize(urdfLinkIndex + 1); + } + parentTransforms[urdfLinkIndex] = tr; + } +#endif + + } +#if 0 + if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size()) + { + *cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut; + } +#endif + + + if (scene && cache.m_articulation) + { +#ifdef DEBUG_ARTICULATIONS + printf("\n-----------------\n"); + printf("m_linkTransWorldSpace\n"); + for (int i = 0; i < cache.m_linkTransWorldSpace.size(); i++) + { + printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n", + cache.m_linkTransWorldSpace[i].p.x, cache.m_linkTransWorldSpace[i].p.y, cache.m_linkTransWorldSpace[i].p.z, + cache.m_linkTransWorldSpace[i].q.x, cache.m_linkTransWorldSpace[i].q.y, cache.m_linkTransWorldSpace[i].q.z, cache.m_linkTransWorldSpace[i].q.w); + } + printf("m_parentLocalPoses\n"); + for (int i = 0; i < cache.m_parentLocalPoses.size(); i++) + { + printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n", + cache.m_parentLocalPoses[i].p.x, cache.m_parentLocalPoses[i].p.y, cache.m_parentLocalPoses[i].p.z, + cache.m_parentLocalPoses[i].q.x, cache.m_parentLocalPoses[i].q.y, cache.m_parentLocalPoses[i].q.z, cache.m_parentLocalPoses[i].q.w); + } + + printf("m_childLocalPoses\n"); + for (int i = 0; i < cache.m_childLocalPoses.size(); i++) + { + printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n", + cache.m_childLocalPoses[i].p.x, cache.m_childLocalPoses[i].p.y, cache.m_childLocalPoses[i].p.z, + cache.m_childLocalPoses[i].q.x, cache.m_childLocalPoses[i].q.y, cache.m_childLocalPoses[i].q.z, cache.m_childLocalPoses[i].q.w); + } + + printf("m_geomDimensions\n"); + for (int i = 0; i < cache.m_geomDimensions.size(); i++) + { + printf("PxVec3(%f,%f,%f),\n", + cache.m_geomDimensions[i].x, cache.m_geomDimensions[i].y, cache.m_geomDimensions[i].z); + } + + printf("m_geomLocalPoses\n"); + for (int i = 0; i < cache.m_geomLocalPoses.size(); i++) + { + printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n", + cache.m_geomLocalPoses[i].p.x, cache.m_geomLocalPoses[i].p.y, cache.m_geomLocalPoses[i].p.z, + cache.m_geomLocalPoses[i].q.x, cache.m_geomLocalPoses[i].q.y, cache.m_geomLocalPoses[i].q.z, cache.m_geomLocalPoses[i].q.w); + } + + printf("m_linkMaterials\n"); + for (int i = 0; i < cache.m_linkMaterials.size(); i++) + { + printf("PxVec3(%f,%f,%f),\n", + cache.m_linkMaterials[i].x, cache.m_linkMaterials[i].y, cache.m_linkMaterials[i].z); + } +#endif //DEBUG_ARTICULATIONS + + //see also https://github.com/NVIDIAGameWorks/PhysX/issues/43 + if ((flags & CUF_USE_SELF_COLLISION) == 0) + { + physx::PxU32 nbActors = cache.m_articulation->getNbLinks();; // Max number of actors expected in the aggregate + bool selfCollisions = false; + physx::PxAggregate* aggregate = physics->createAggregate(nbActors, selfCollisions); + aggregate->addArticulation(*cache.m_articulation); + scene->addAggregate(*aggregate); + } + else + { + scene->addArticulation(*cache.m_articulation); + } + + } + + return cache.m_articulation; +} diff --git a/examples/SharedMemory/physx/URDF2PhysX.h b/examples/SharedMemory/physx/URDF2PhysX.h new file mode 100644 index 000000000..f00d18edb --- /dev/null +++ b/examples/SharedMemory/physx/URDF2PhysX.h @@ -0,0 +1,25 @@ +#ifndef URDF2PHYSX_H +#define URDF2PHYSX_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Importers/ImportURDFDemo/URDFJointTypes.h" + +namespace physx +{ + class PxFoundation; + class PxPhysics; + class PxDefaultCpuDispatcher; + class PxScene; + class PxCooking; + class PxArticulationReducedCoordinate; +}; + +struct UrdfVisualShapeCache2 +{ + b3AlignedObjectArray m_cachedUrdfLinkColors; + b3AlignedObjectArray m_cachedUrdfLinkVisualShapeIndices; +}; + +physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const class btTransform& rootTransformInWorldSpace,struct CommonFileIOInterface* fileIO); + +#endif //URDF2PHYSX_H \ No newline at end of file diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index 76910fe14..5c8a3dcf1 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -73,9 +73,9 @@ struct MyTexture3 struct EGLRendererObjectArray { btAlignedObjectArray m_renderObjects; + btAlignedObjectArray m_graphicsInstanceIds; int m_objectUniqueId; int m_linkIndex; - int m_graphicsInstanceId; btTransform m_worldTransform; btVector3 m_localScaling; @@ -83,12 +83,15 @@ struct EGLRendererObjectArray { m_worldTransform.setIdentity(); m_localScaling.setValue(1, 1, 1); - m_graphicsInstanceId = -1; } }; -#define START_WIDTH 2560 -#define START_HEIGHT 2048 +//#define START_WIDTH 2560 +//#define START_HEIGHT 2048 + +#define START_WIDTH 1024 +#define START_HEIGHT 768 + struct EGLRendererVisualShapeConverterInternalData { @@ -105,6 +108,7 @@ struct EGLRendererVisualShapeConverterInternalData btAlignedObjectArray m_visualShapes; + int m_upAxis; int m_swWidth; int m_swHeight; @@ -132,12 +136,22 @@ struct EGLRendererVisualShapeConverterInternalData int m_flags; SimpleCamera m_camera; + bool m_leftMouseButton; + bool m_middleMouseButton; + bool m_rightMouseButton; + float m_wheelMultiplier; + float m_mouseMoveMultiplier; + float m_mouseXpos; + float m_mouseYpos; + bool m_mouseInitialized; + int m_graphicsUniqueIdGenerator; + EGLRendererVisualShapeConverterInternalData() : m_upAxis(2), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB), - m_lightDirection(btVector3(-5, 200, -40)), + m_lightDirection(btVector3(-5, -40, 200 )), m_hasLightDirection(false), m_lightColor(btVector3(1.0, 1.0, 1.0)), m_hasLightColor(false), @@ -150,7 +164,16 @@ struct EGLRendererVisualShapeConverterInternalData m_lightSpecularCoeff(0.05), m_hasLightSpecularCoeff(false), m_hasShadow(false), - m_flags(0) + m_flags(0), + m_leftMouseButton(false), + m_middleMouseButton(false), + m_rightMouseButton(false), + m_wheelMultiplier(0.01f), + m_mouseMoveMultiplier(0.4f), + m_mouseXpos(0.f), + m_mouseYpos(0.f), + m_mouseInitialized(false), + m_graphicsUniqueIdGenerator(15) { m_depthBuffer.resize(m_swWidth * m_swHeight); m_shadowBuffer.resize(m_swWidth * m_swHeight); @@ -161,7 +184,7 @@ struct EGLRendererVisualShapeConverterInternalData m_window = new DefaultOpenGLWindow(); m_window->setAllowRetina(allowRetina); b3gWindowConstructionInfo ci; - ci.m_title = "Title"; + ci.m_title = "PyBullet"; ci.m_width = m_swWidth; ci.m_height = m_swHeight; ci.m_renderDevice = 0; @@ -207,21 +230,234 @@ struct EGLRendererVisualShapeConverterInternalData } }; + + +static EGLRendererVisualShapeConverter* gWindow = 0; + +static void SimpleResizeCallback(float widthf, float heightf) +{ + int width = (int)widthf; + int height = (int)heightf; + if (gWindow && gWindow->m_data->m_instancingRenderer) + { + gWindow->m_data->m_instancingRenderer->resize(width, height); + } + + //if (gApp && gApp->m_instancingRenderer) + // gApp->m_instancingRenderer->resize(width, height); + // + //if (gApp && gApp->m_primRenderer) + // gApp->m_primRenderer->setScreenSize(width, height); +} + +static void SimpleKeyboardCallback(int key, int state) +{ + if (key == B3G_ESCAPE) //&& gApp && gApp->m_window) + { + //gApp->m_window->setRequestExit(); + } + else + { + //gApp->defaultKeyboardCallback(key,state); + } +} + +static void SimpleMouseButtonCallback(int button, int state, float x, float y) +{ + if (gWindow) + { + gWindow->mouseButtonCallback(button, state, x, y); + } +} + +static void SimpleMouseMoveCallback(float x, float y) +{ + if (gWindow) + { + gWindow->mouseMoveCallback(x, y); + } +} + +static void SimpleWheelCallback(float deltax, float deltay) +{ + float wheelMultiplier = 0.01f; + if (gWindow && gWindow->m_data->m_instancingRenderer) + { + class GLInstancingRenderer* renderer = gWindow->m_data->m_instancingRenderer; + b3Vector3 cameraTargetPosition, cameraPosition, cameraUp = b3MakeVector3(0, 0, 0); + int upAxis = renderer->getActiveCamera()->getCameraUpAxis(); + cameraUp[upAxis] = 1; + CommonCameraInterface* camera = renderer->getActiveCamera(); + + camera->getCameraPosition(cameraPosition); + camera->getCameraTargetPosition(cameraTargetPosition); + + bool m_leftMouseButton = false; + + if (!m_leftMouseButton) + { + float cameraDistance = camera->getCameraDistance(); + if (deltay < 0 || cameraDistance > 1) + { + cameraDistance -= deltay * 0.01f; + if (cameraDistance < 1) + cameraDistance = 1; + camera->setCameraDistance(cameraDistance); + } + else + { + b3Vector3 fwd = cameraTargetPosition - cameraPosition; + fwd.normalize(); + cameraTargetPosition += fwd * deltay * wheelMultiplier; //todo: expose it in the GUI? + } + } + else + { + if (b3Fabs(deltax) > b3Fabs(deltay)) + { + b3Vector3 fwd = cameraTargetPosition - cameraPosition; + b3Vector3 side = cameraUp.cross(fwd); + side.normalize(); + cameraTargetPosition += side * deltax * wheelMultiplier; + } + else + { + cameraTargetPosition -= cameraUp * deltay * wheelMultiplier; + } + } + + camera->setCameraTargetPosition(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]); + } +} + +void defaultMouseButtonCallback(int button, int state, float x, float y) +{ + if (gWindow) + { + gWindow->mouseButtonCallback(button, state, x, y); + } +} +void defaultMouseMoveCallback(float x, float y) +{ + if (gWindow) + { + gWindow->mouseMoveCallback(x, y); + } //m_window && m_renderer + + +} + +void EGLRendererVisualShapeConverter::mouseButtonCallback(int button, int state, float x, float y) +{ + if (button == 0) + m_data->m_leftMouseButton = (state == 1); + if (button == 1) + m_data->m_middleMouseButton = (state == 1); + + if (button == 2) + m_data->m_rightMouseButton = (state == 1); + + m_data->m_mouseXpos = x; + m_data->m_mouseYpos = y; + m_data->m_mouseInitialized = true; +} +void EGLRendererVisualShapeConverter::mouseMoveCallback(float x, float y) +{ + class GLInstancingRenderer* renderer = m_data->m_instancingRenderer; + if (renderer == 0) + return; + + CommonCameraInterface* camera = renderer->getActiveCamera(); + + bool isAltPressed = m_data->m_window->isModifierKeyPressed(B3G_ALT); + bool isControlPressed = m_data->m_window->isModifierKeyPressed(B3G_CONTROL); + + if (isAltPressed || isControlPressed) + { + float xDelta = x - m_data->m_mouseXpos; + float yDelta = y - m_data->m_mouseYpos; + float cameraDistance = camera->getCameraDistance(); + float pitch = camera->getCameraPitch(); + float yaw = camera->getCameraYaw(); + + float targPos[3]; + float camPos[3]; + + camera->getCameraTargetPosition(targPos); + camera->getCameraPosition(camPos); + + b3Vector3 cameraPosition = b3MakeVector3(b3Scalar(camPos[0]), + b3Scalar(camPos[1]), + b3Scalar(camPos[2])); + + b3Vector3 cameraTargetPosition = b3MakeVector3(b3Scalar(targPos[0]), + b3Scalar(targPos[1]), + b3Scalar(targPos[2])); + b3Vector3 cameraUp = b3MakeVector3(0, 0, 0); + cameraUp[camera->getCameraUpAxis()] = 1.f; + + if (m_data->m_leftMouseButton) + { + // if (b3Fabs(xDelta)>b3Fabs(yDelta)) + // { + pitch -= yDelta * m_data->m_mouseMoveMultiplier; + // } else + // { + yaw -= xDelta * m_data->m_mouseMoveMultiplier; + // } + } + + if (m_data->m_middleMouseButton) + { + cameraTargetPosition += cameraUp * yDelta * 0.01; + + b3Vector3 fwd = cameraTargetPosition - cameraPosition; + b3Vector3 side = cameraUp.cross(fwd); + side.normalize(); + cameraTargetPosition += side * xDelta * 0.01; + } + if (m_data->m_rightMouseButton) + { + cameraDistance -= xDelta * 0.01f; + cameraDistance -= yDelta * 0.01f; + if (cameraDistance < 1) + cameraDistance = 1; + if (cameraDistance > 1000) + cameraDistance = 1000; + } + camera->setCameraDistance(cameraDistance); + camera->setCameraPitch(pitch); + camera->setCameraYaw(yaw); + camera->setCameraTargetPosition(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]); + } + m_data->m_mouseXpos = x; + m_data->m_mouseYpos = y; + m_data->m_mouseInitialized = true; +} EGLRendererVisualShapeConverter::EGLRendererVisualShapeConverter() { + m_data = new EGLRendererVisualShapeConverterInternalData(); - + + float dist = 1.5; float pitch = -10; float yaw = -80; float targetPos[3] = {0, 0, 0}; m_data->m_camera.setCameraUpAxis(m_data->m_upAxis); resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + gWindow = this; + m_data->m_window->setResizeCallback(SimpleResizeCallback); + m_data->m_window->setWheelCallback(SimpleWheelCallback); + m_data->m_window->setMouseButtonCallback(SimpleMouseButtonCallback); + m_data->m_window->setMouseMoveCallback(SimpleMouseMoveCallback); } EGLRendererVisualShapeConverter::~EGLRendererVisualShapeConverter() { + gWindow = 0; resetAll(); delete m_data; + } void EGLRendererVisualShapeConverter::setLightDirection(float x, float y, float z) @@ -649,11 +885,15 @@ static btVector4 sColors[4] = // If you are getting segfaults in this function it may be ecause you are // compliling the plugin with differently from pybullet, try complining the // plugin with distutils too. -void EGLRendererVisualShapeConverter::convertVisualShapes( +int EGLRendererVisualShapeConverter::convertVisualShapes( int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, - int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) + int orgGraphicsUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) { + if (orgGraphicsUniqueId< 0) + { + orgGraphicsUniqueId = m_data->m_graphicsUniqueIdGenerator++; + } btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) if (linkPtr) { @@ -743,12 +983,12 @@ void EGLRendererVisualShapeConverter::convertVisualShapes( } } - EGLRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + EGLRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[orgGraphicsUniqueId]; if (visualsPtr == 0) { - m_data->m_swRenderInstances.insert(collisionObjectUniqueId, new EGLRendererObjectArray); + m_data->m_swRenderInstances.insert(orgGraphicsUniqueId, new EGLRendererObjectArray); } - visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + visualsPtr = m_data->m_swRenderInstances[orgGraphicsUniqueId]; btAssert(visualsPtr); EGLRendererObjectArray* visuals = *visualsPtr; @@ -808,13 +1048,14 @@ void EGLRendererVisualShapeConverter::convertVisualShapes( int shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); double scaling[3] = {1, 1, 1}; - visuals->m_graphicsInstanceId = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling); + int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling); int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24); { - int graphicsIndex = visuals->m_graphicsInstanceId; if (graphicsIndex >= 0) { + visuals->m_graphicsInstanceIds.push_back(graphicsIndex); + if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1)) { m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1); @@ -835,6 +1076,7 @@ void EGLRendererVisualShapeConverter::convertVisualShapes( } } } + return orgGraphicsUniqueId; } int EGLRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId) @@ -1032,7 +1274,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL( int numRemainingPixels = numTotalPixels - startPixelIndex; int numBytesPerPixel = 4; //RGBA int numRequestedPixels = btMin(rgbaBufferSizeInPixels, numRemainingPixels); - if (numRequestedPixels) + if (1) { if (startPixelIndex == 0) { @@ -1049,9 +1291,13 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL( m_data->m_instancingRenderer->updateCamera(m_data->m_upAxis); m_data->m_instancingRenderer->renderScene(); + m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(1, 0, 0), b3MakeVector3(1, 0, 0), 3); + m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 1, 0), b3MakeVector3(0, 1, 0), 3); + m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 0, 1), b3MakeVector3(0, 0, 1), 3); int numBytesPerPixel = 4; //RGBA + if (pixelsRGBA || depthBuffer) { { BT_PROFILE("copy pixels"); @@ -1083,40 +1329,40 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL( } } } - } - - m_data->m_rgbaPixelBuffer1.resize((*widthPtr) * (*heightPtr) * numBytesPerPixel); - m_data->m_depthBuffer1.resize((*widthPtr) * (*heightPtr)); - //rescale and flip - { - BT_PROFILE("resize and flip"); - for (int j = 0; j < *heightPtr; j++) + + m_data->m_rgbaPixelBuffer1.resize((*widthPtr) * (*heightPtr) * numBytesPerPixel); + m_data->m_depthBuffer1.resize((*widthPtr) * (*heightPtr)); + //rescale and flip { - for (int i = 0; i < *widthPtr; i++) + BT_PROFILE("resize and flip"); + for (int j = 0; j < *heightPtr; j++) { - int xIndex = int(float(i) * (float(sourceWidth) / float(*widthPtr))); - int yIndex = int(float(*heightPtr - 1 - j) * (float(sourceHeight) / float(*heightPtr))); - btClamp(xIndex, 0, sourceWidth); - btClamp(yIndex, 0, sourceHeight); - int bytesPerPixel = 4; //RGBA - - int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel; - int sourceDepthIndex = xIndex + yIndex * sourceWidth; -#define COPY4PIXELS 1 -#ifdef COPY4PIXELS - int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i + j * (*widthPtr)) * 4 + 0]; - int* src = (int*)&m_data->m_sourceRgbaPixelBuffer[sourcePixelIndex + 0]; - *dst = *src; - -#else - m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 0] = sourceRgbaPixelBuffer[sourcePixelIndex + 0]; - m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 1] = sourceRgbaPixelBuffer[sourcePixelIndex + 1]; - m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 2] = sourceRgbaPixelBuffer[sourcePixelIndex + 2]; - m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 3] = 255; -#endif - if (depthBuffer) + for (int i = 0; i < *widthPtr; i++) { - m_data->m_depthBuffer1[i + j * (*widthPtr)] = m_data->m_sourceDepthBuffer[sourceDepthIndex]; + int xIndex = int(float(i) * (float(sourceWidth) / float(*widthPtr))); + int yIndex = int(float(*heightPtr - 1 - j) * (float(sourceHeight) / float(*heightPtr))); + btClamp(xIndex, 0, sourceWidth); + btClamp(yIndex, 0, sourceHeight); + int bytesPerPixel = 4; //RGBA + + int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel; + int sourceDepthIndex = xIndex + yIndex * sourceWidth; + #define COPY4PIXELS 1 + #ifdef COPY4PIXELS + int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i + j * (*widthPtr)) * 4 + 0]; + int* src = (int*)&m_data->m_sourceRgbaPixelBuffer[sourcePixelIndex + 0]; + *dst = *src; + + #else + m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 0] = sourceRgbaPixelBuffer[sourcePixelIndex + 0]; + m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 1] = sourceRgbaPixelBuffer[sourcePixelIndex + 1]; + m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 2] = sourceRgbaPixelBuffer[sourcePixelIndex + 2]; + m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 3] = 255; + #endif + if (depthBuffer) + { + m_data->m_depthBuffer1[i + j * (*widthPtr)] = m_data->m_sourceDepthBuffer[sourceDepthIndex]; + } } } } @@ -1153,36 +1399,35 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL( } } } - } + m_data->m_segmentationMaskBuffer.resize(destinationWidth * destinationHeight, -1); - m_data->m_segmentationMaskBuffer.resize(destinationWidth * destinationHeight, -1); - - //rescale and flip - { - BT_PROFILE("resize and flip"); - for (int j = 0; j < destinationHeight; j++) + //rescale and flip { - for (int i = 0; i < destinationWidth; i++) + BT_PROFILE("resize and flip"); + for (int j = 0; j < destinationHeight; j++) { - int xIndex = int(float(i) * (float(sourceWidth) / float(destinationWidth))); - int yIndex = int(float(destinationHeight - 1 - j) * (float(sourceHeight) / float(destinationHeight))); - btClamp(xIndex, 0, sourceWidth); - btClamp(yIndex, 0, sourceHeight); - int bytesPerPixel = 4; //RGBA - int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel; - int sourceDepthIndex = xIndex + yIndex * sourceWidth; - - if (segmentationMaskBuffer) + for (int i = 0; i < destinationWidth; i++) { - float depth = m_data->m_segmentationMaskSourceDepthBuffer[sourceDepthIndex]; - if (depth < 1) + int xIndex = int(float(i) * (float(sourceWidth) / float(destinationWidth))); + int yIndex = int(float(destinationHeight - 1 - j) * (float(sourceHeight) / float(destinationHeight))); + btClamp(xIndex, 0, sourceWidth); + btClamp(yIndex, 0, sourceHeight); + int bytesPerPixel = 4; //RGBA + int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel; + int sourceDepthIndex = xIndex + yIndex * sourceWidth; + + if (segmentationMaskBuffer) { - int segMask = m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 0] + 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 1]) + 256 * 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 2]); - m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = segMask; - } - else - { - m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = -1; + float depth = m_data->m_segmentationMaskSourceDepthBuffer[sourceDepthIndex]; + if (depth < 1) + { + int segMask = m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 0] + 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 1]) + 256 * 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 2]); + m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = segMask; + } + else + { + m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = -1; + } } } } @@ -1257,7 +1502,11 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu { for (int o = 0; o < ptr->m_renderObjects.size(); o++) { - m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceId); + for (int i = 0; i < ptr->m_graphicsInstanceIds.size(); i++) + { + m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceIds[i]); + } + delete ptr->m_renderObjects[o]; } } @@ -1387,11 +1636,15 @@ void EGLRendererVisualShapeConverter::syncTransform(int collisionObjectUniqueId, EGLRendererObjectArray* renderObj = *renderObjPtr; renderObj->m_worldTransform = worldTransform; renderObj->m_localScaling = localScaling; - if (renderObj->m_graphicsInstanceId >= 0) + for (int i = 0; i < renderObj->m_graphicsInstanceIds.size(); i++) { - btVector3 pos = worldTransform.getOrigin(); - btQuaternion orn = worldTransform.getRotation(); - m_data->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, renderObj->m_graphicsInstanceId); + int graphicsInstanceId = renderObj->m_graphicsInstanceIds[i]; + if (graphicsInstanceId >= 0) + { + btVector3 pos = worldTransform.getOrigin(); + btQuaternion orn = worldTransform.getRotation(); + m_data->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, graphicsInstanceId); + } } } } diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h index a6caea855..dfde7b22d 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h @@ -11,7 +11,7 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface virtual ~EGLRendererVisualShapeConverter(); - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO); + virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO); virtual int getNumVisualShapes(int bodyUniqueId); @@ -55,6 +55,10 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface virtual void setProjectiveTexture(bool useProjectiveTexture); virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling); + + virtual void mouseMoveCallback(float x, float y); + virtual void mouseButtonCallback(int button, int state, float x, float y); + }; #endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H diff --git a/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp new file mode 100644 index 000000000..224afaf9a --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp @@ -0,0 +1,391 @@ +#include "btBulletDynamicsCommon.h" + +//for inverse dynamics, DeepMimic implementation +#include "RBDModel.h" +#include "RBDUtil.h" +#include "KinTree.h" + +//for BulletInverseDynamics +//#include "BulletInverseDynamics/IDConfig.hpp" +//#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" + +//#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +//#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h" +//#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" + +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" + + +struct TempLink +{ + int m_parentIndex; + const btCollisionObject* m_collider; + double m_mass; + int m_jointType; + int m_dofOffset; + int m_dofCount; + btVector3 m_dVector; + btVector3 m_eVector; + btQuaternion m_zeroRotParentToThis; + btQuaternion m_this_to_body1; +}; + +bool btExtractJointBodyFromTempLinks(btAlignedObjectArray& links, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat) +{ + + bool result = true; + + int num_joints = links.size(); + + btAlignedObjectArray bodyToLinkPositions; + btAlignedObjectArray bodyToLinkRotations; + btAlignedObjectArray dVectorRot; + bodyToLinkRotations.resize(num_joints); + bodyToLinkPositions.resize(num_joints); + dVectorRot.resize(num_joints); + + jointMat.resize(num_joints, 19); + bodyDefs.resize(num_joints, 17); + for (int i = 0; i < num_joints * 19; i++) + { + jointMat(i) = SIMD_INFINITY; + } + for (int i = 0; i < num_joints * 17; i++) + { + bodyDefs(i) = SIMD_INFINITY; + } + + for (int i = 0; i < num_joints * 17; i++) + { + bodyDefs(i) = SIMD_INFINITY; + } + + btScalar unk = -12345; + + int totalDofs = 0; + + for (int j = 0; j < num_joints; ++j) + { + + int i = j; + int parentIndex = links[j].m_parentIndex; + + cShape::eShape shapeType = cShape::eShapeNull; + double param0 = 0, param1 = 0, param2 = 0; + if (links[j].m_collider) + { + const btCollisionShape* collisionShape = links[j].m_collider->getCollisionShape(); + if (collisionShape->isCompound()) + { + const btCompoundShape* compound = (const btCompoundShape*)collisionShape; + if (compound->getNumChildShapes() > 0) + { + collisionShape = compound->getChildShape(0); + } + } + switch (collisionShape->getShapeType()) + { + case BOX_SHAPE_PROXYTYPE: + { + shapeType = cShape::eShapeBox; + btBoxShape* box = (btBoxShape*)collisionShape; + param0 = box->getHalfExtentsWithMargin()[0] * 2; + param1 = box->getHalfExtentsWithMargin()[1] * 2; + param2 = box->getHalfExtentsWithMargin()[2] * 2; + break; + } + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphere = (btSphereShape*)collisionShape; + param0 = sphere->getRadius() * 2; + param1 = sphere->getRadius() * 2; + param2 = sphere->getRadius() * 2; + shapeType = cShape::eShapeSphere; + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* caps = (btCapsuleShape*)collisionShape; + param0 = caps->getRadius() * 2; + param1 = caps->getHalfHeight() * 2; + param2 = caps->getRadius() * 2; + shapeType = cShape::eShapeCapsule; + break; + } + default: + { + btAssert(0); + } + } + } + + + btVector3 body_attach_pt1 = links[j].m_dVector; + + //tQuaternion body_to_parent_body = parent_to_parent_body * this_to_parent * body_to_this; + //tQuaternion parent_to_parent_body = parent_body_to_parent.inverse(); + + + bodyDefs(i, cKinTree::eBodyParam0) = param0; + bodyDefs(i, cKinTree::eBodyParam1) = param1; + bodyDefs(i, cKinTree::eBodyParam2) = param2; + + bodyDefs(i, cKinTree::eBodyParamShape) = shapeType; + bodyDefs(i, cKinTree::eBodyParamMass) = links[j].m_mass; + + + bodyDefs(i, cKinTree::eBodyParamColGroup) = unk; + bodyDefs(i, cKinTree::eBodyParamEnableFallContact) = unk; + + bodyDefs(i, cKinTree::eBodyColorR) = unk; + bodyDefs(i, cKinTree::eBodyColorG) = unk; + bodyDefs(i, cKinTree::eBodyColorB) = unk; + bodyDefs(i, cKinTree::eBodyColorA) = unk; + + dVectorRot[j] = links[j].m_this_to_body1; + + btVector3 body_attach_pt2 = quatRotate(links[j].m_this_to_body1.inverse(), body_attach_pt1); + bodyToLinkPositions[i] = body_attach_pt2; + bodyDefs(i, cKinTree::eBodyParamAttachX) = body_attach_pt2[0]; + bodyDefs(i, cKinTree::eBodyParamAttachY) = body_attach_pt2[1]; + bodyDefs(i, cKinTree::eBodyParamAttachZ) = body_attach_pt2[2]; + btScalar bodyAttachThetaX = 0; + btScalar bodyAttachThetaY = 0; + btScalar bodyAttachThetaZ = 0; + btQuaternion body_to_this1 = links[j].m_this_to_body1.inverse(); + + + + body_to_this1.getEulerZYX(bodyAttachThetaZ, bodyAttachThetaY, bodyAttachThetaX); + bodyDefs(i, cKinTree::eBodyParamAttachThetaX) = bodyAttachThetaX; + bodyDefs(i, cKinTree::eBodyParamAttachThetaY) = bodyAttachThetaY; + bodyDefs(i, cKinTree::eBodyParamAttachThetaZ) = bodyAttachThetaZ; + + jointMat(i, cKinTree::eJointDescType) = links[j].m_jointType; + jointMat(i, cKinTree::eJointDescParent) = parentIndex; + + + btVector3 jointAttachPointMy = links[j].m_eVector; + btVector3 jointAttachPointMyv0 = jointAttachPointMy; + btVector3 parentBodyAttachPtMy(0, 0, 0); + btQuaternion parentBodyToLink; + parentBodyToLink = btQuaternion::getIdentity(); + btQuaternion linkToParentBody = btQuaternion::getIdentity(); + int parent_joint = links[j].m_parentIndex; + + if (parent_joint != gInvalidIdx) + { + parentBodyAttachPtMy = bodyToLinkPositions[parent_joint]; + parentBodyToLink = bodyToLinkRotations[parent_joint]; + linkToParentBody = parentBodyToLink.inverse(); + } + parentBodyAttachPtMy = quatRotate(linkToParentBody, parentBodyAttachPtMy); + //bodyToLinkRotations + jointAttachPointMy += parentBodyAttachPtMy; + jointAttachPointMy = quatRotate(linkToParentBody.inverse(), jointAttachPointMy); + + btVector3 parent_body_attach_pt1(0, 0, 0); + if (parentIndex >= 0) + { + parent_body_attach_pt1 = links[parentIndex].m_dVector; + } + btQuaternion myparent_body_to_body(0, 0, 0, 1); + btQuaternion mybody_to_parent_body(0, 0, 0, 1); + btQuaternion parent_body_to_body1 = links[i].m_zeroRotParentToThis; + btQuaternion body_to_parent_body1 = parent_body_to_body1.inverse(); + + bodyToLinkRotations[i] = body_to_this1; + + jointMat(i, cKinTree::eJointDescAttachX) = jointAttachPointMy[0]; + jointMat(i, cKinTree::eJointDescAttachY) = jointAttachPointMy[1]; + jointMat(i, cKinTree::eJointDescAttachZ) = jointAttachPointMy[2]; + + + btQuaternion parent2parent_body2(0, 0, 0, 1); + + if (parent_joint >= 0) + { + //parent2parent_body2 = bulletMB->getLink(parent_joint).m_dVectorRot; + parent2parent_body2 = dVectorRot[parent_joint]; + } + ///btQuaternion this2bodyA = bulletMB->getLink(j).m_dVectorRot; + btQuaternion this2bodyA = dVectorRot[j]; + + btQuaternion parent_body_2_body = links[j].m_zeroRotParentToThis; + btQuaternion combined2 = parent_body_2_body.inverse(); + btQuaternion recoverthis2parent = parent2parent_body2.inverse()*combined2*this2bodyA;// body2this.inverse(); + btScalar eulZ, eulY, eulX; + recoverthis2parent.getEulerZYX(eulZ, eulY, eulX); + + + jointMat(i, cKinTree::eJointDescAttachThetaX) = eulX; + jointMat(i, cKinTree::eJointDescAttachThetaY) = eulY; + jointMat(i, cKinTree::eJointDescAttachThetaZ) = eulZ; + + + jointMat(i, cKinTree::eJointDescLimLow0) = unk; + jointMat(i, cKinTree::eJointDescLimLow1) = unk; + jointMat(i, cKinTree::eJointDescLimLow2) = unk; + jointMat(i, cKinTree::eJointDescLimHigh0) = unk; + jointMat(i, cKinTree::eJointDescLimHigh1) = unk; + jointMat(i, cKinTree::eJointDescLimHigh2) = unk; + jointMat(i, cKinTree::eJointDescTorqueLim) = unk; + + jointMat(i, cKinTree::eJointDescForceLim) = unk; + jointMat(i, cKinTree::eJointDescIsEndEffector) = unk; + jointMat(i, cKinTree::eJointDescDiffWeight) = unk; + jointMat(i, cKinTree::eJointDescParamOffset) = totalDofs; + totalDofs += links[j].m_dofCount; + + } + return result; +} + + +void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat) +{ + btAlignedObjectArray links; + + int numBaseShapes = 0; + if (bulletMB->getBaseCollider()) + { + switch (bulletMB->getBaseCollider()->getCollisionShape()->getShapeType()) + { + case CAPSULE_SHAPE_PROXYTYPE: + case SPHERE_SHAPE_PROXYTYPE: + case BOX_SHAPE_PROXYTYPE: + { + numBaseShapes++; + break; + } + case COMPOUND_SHAPE_PROXYTYPE: + { + btCompoundShape* compound = (btCompoundShape*)bulletMB->getBaseCollider()->getCollisionShape(); + numBaseShapes += compound->getNumChildShapes(); + break; + } + default: + { + } + } + + } + + //links include the 'base' and its childlinks + int baseLink = numBaseShapes? 1 : 0; + links.resize(bulletMB->getNumLinks() + baseLink); + for (int i = 0; i < links.size(); i++) + { + memset(&links[i], 0xffffffff, sizeof(TempLink)); + } + + int totalDofs = 0; + if (numBaseShapes) + { + //links[0] is the root/base + links[0].m_parentIndex = -1; + links[0].m_collider = bulletMB->getBaseCollider(); + links[0].m_mass = bulletMB->getBaseMass(); + links[0].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone; + links[0].m_dofOffset = 0; + links[0].m_dofCount = 7; + links[0].m_dVector.setValue(0, 0, 0); + links[0].m_eVector.setValue(0, 0, 0); + links[0].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1); + links[0].m_this_to_body1 = btQuaternion(0, 0, 0, 1); + totalDofs = 7; + } + + + for (int j = 0; j < bulletMB->getNumLinks(); ++j) + { + int parentIndex = bulletMB->getLink(j).m_parent; + links[j + baseLink].m_parentIndex = parentIndex + baseLink; + links[j + baseLink].m_collider = bulletMB->getLinkCollider(j); + links[j + baseLink].m_mass = bulletMB->getLink(j).m_mass; + + int jointType = 0; + btQuaternion this_to_body1(0, 0, 0, 1); + int dofCount = 0; + + if ((baseLink)==0 &&j == 0)//for 'root' either use fixed or none + { + dofCount = 7; + links[j].m_parentIndex = -1; + links[j].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone; + links[j].m_dofOffset = 0; + links[j].m_dofCount = dofCount; + + + links[j].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1); + //links[j].m_dVector.setValue(0, 0, 0); + links[j].m_dVector = bulletMB->getLink(j).m_dVector; + links[j].m_eVector.setValue(0, 0, 0); + //links[j].m_eVector = bulletMB->getLink(j).m_eVector; + links[j].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis; + + links[j].m_this_to_body1 = btQuaternion(0, 0, 0, 1); + totalDofs = 7; + } + else + { + + switch (bulletMB->getLink(j).m_jointType) + { + case btMultibodyLink::eFixed: + { + jointType = cKinTree::eJointTypeFixed; + break; + } + case btMultibodyLink::ePrismatic: + { + dofCount = 1; + btVector3 refAxis(1, 0, 0); + btVector3 axis = bulletMB->getLink(j).getAxisTop(0); + this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2])); + jointType = cKinTree::eJointTypePrismatic; + break; + } + case btMultibodyLink::eSpherical: + { + dofCount = 4;//?? + jointType = cKinTree::eJointTypeSpherical; + break; + } + case btMultibodyLink::eRevolute: + { + dofCount = 1; + btVector3 refAxis(0, 0, 1); + btVector3 axis = bulletMB->getLink(j).getAxisTop(0); + this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2])); + jointType = cKinTree::eJointTypeRevolute; + break; + } + default: + { + } + } + links[j + baseLink].m_jointType = jointType; + links[j + baseLink].m_dofOffset = totalDofs; + links[j + baseLink].m_dofCount = dofCount; + links[j + baseLink].m_dVector = bulletMB->getLink(j).m_dVector; + links[j + baseLink].m_eVector = bulletMB->getLink(j).m_eVector; + links[j + baseLink].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis; + links[j + baseLink].m_this_to_body1 = this_to_body1; + } + + totalDofs += dofCount; + } + + btExtractJointBodyFromTempLinks(links, bodyDefs, jointMat); +} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h new file mode 100644 index 000000000..0ae62c9ef --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h @@ -0,0 +1,9 @@ +#ifndef BULLET_CONVERSION_H +#define BULLET_CONVERSION_H + +class btMultiBody; +#include "MathUtil.h" +void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat); + + +#endif //BULLET_CONVERSION_H \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp new file mode 100644 index 000000000..1b326f02d --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp @@ -0,0 +1,1982 @@ +#include "KinTree.h" + +#include + +#include "RBDUtil.h" + +#ifdef USE_JSON +#include "FileUtil.h" +#endif + + +const int cKinTree::gPosDim = 3; +const int cKinTree::gRotDim = 4; +const int cKinTree::gRootDim = gPosDim + gRotDim; +const int cKinTree::gInvalidJointID = -1; + +// Json keys +const std::string gJointTypeNames[cKinTree::eJointTypeMax] = +{ + "revolute", + "planar", + "prismatic", + "fixed", + "spherical", + "none" +}; + +const std::string gJointsKey = "Joints"; +const std::string gJointDescKeys[cKinTree::eJointDescMax] = +{ + "Type", + "Parent", + "AttachX", + "AttachY", + "AttachZ", + "AttachThetaX", + "AttachThetaY", + "AttachThetaZ", + "LimLow0", + "LimLow1", + "LimLow2", + "LimHigh0", + "LimHigh1", + "LimHigh2", + "TorqueLim", + "ForceLim", + "IsEndEffector", + "DiffWeight", + "Offset" +}; + +const std::string gBodyDefsKey = "BodyDefs"; +const std::string gBodyDescKeys[cKinTree::eBodyParamMax] = +{ + "Shape", + "Mass", + "ColGroup", + "EnableFallContact", + "AttachX", + "AttachY", + "AttachZ", + "AttachThetaX", + "AttachThetaY", + "AttachThetaZ", + "Param0", + "Param1", + "Param2", + "ColorR", + "ColorG", + "ColorB", + "ColorA" +}; + +const std::string gDrawShapeDefsKey = "DrawShapeDefs"; +const std::string gDrawShapeDescKeys[cKinTree::eDrawShapeParamMax] = +{ + "Shape", + "ParentJoint", + "AttachX", + "AttachY", + "AttachZ", + "AttachThetaX", + "AttachThetaY", + "AttachThetaZ", + "Param0", + "Param1", + "Param2", + "ColorR", + "ColorG", + "ColorB", + "ColorA", + "MeshID" +}; + +int cKinTree::GetRoot(const Eigen::MatrixXd& joint_desc) +{ + // this should always be true right? + return 0; +} + +void cKinTree::FindChildren(const Eigen::MatrixXd& joint_desc, int joint_id, Eigen::VectorXi& out_children) +{ + const int max_size = 128; + int children_buffer[max_size]; + int num_children = 0; + int num_joints = GetNumJoints(joint_desc); + + for (int i = 0; i < num_joints; ++i) + { + int parent = GetParent(joint_desc, i); + if (parent == joint_id) + { + children_buffer[num_children] = i; + ++num_children; + + if (num_children >= max_size) + { + printf("Too many children, max = %i", max_size); + assert(false); + return; + } + } + } + + out_children.resize(num_children); + for (int i = 0; i < num_children; ++i) + { + out_children[i] = children_buffer[i]; + } +} + +#ifdef USE_JSON +bool cKinTree::LoadBodyDefs(const std::string& char_file, Eigen::MatrixXd& out_body_defs) +{ + bool succ = true; + std::string str; + + std::ifstream f_stream(char_file.c_str()); + Json::Value root; + Json::Reader reader; + succ = reader.parse(f_stream, root); + f_stream.close(); + + if (succ) + { + succ = false; + if (!root[gBodyDefsKey].isNull()) + { + Json::Value body_defs = root.get(gBodyDefsKey, 0); + int num_bodies = body_defs.size(); + + succ = true; + out_body_defs.resize(num_bodies, eBodyParamMax); + for (int b = 0; b < num_bodies; ++b) + { + tBodyDef curr_def = BuildBodyDef(); + Json::Value body_json = body_defs.get(b, 0); + bool succ_def = ParseBodyDef(body_json, curr_def); + + if (succ) + { + out_body_defs.row(b) = curr_def; + } + else + { + succ = false; + break; + } + } + } + } + + if (!succ) + { + printf("Failed to load body definition from %s\n", char_file.c_str()); + succ = false; + } + + return succ; +} + +bool cKinTree::ParseBodyDef(const Json::Value& root, cKinTree::tBodyDef& out_def) +{ + std::string shape_str = root.get(gBodyDescKeys[eBodyParamShape], "").asString(); + cShape::eShape shape; + bool succ = cShape::ParseShape(shape_str, shape); + if (succ) + { + out_def(eBodyParamShape) = static_cast(static_cast(shape)); + } + + for (int i = 0; i < eBodyParamMax; ++i) + { + const std::string& curr_key = gBodyDescKeys[i]; + if (!root[curr_key].isNull() + && root[curr_key].isNumeric()) + { + Json::Value json_val = root[curr_key]; + double val = json_val.asDouble(); + out_def(i) = val; + } + } + + return succ; +} + +bool cKinTree::LoadDrawShapeDefs(const std::string& char_file, Eigen::MatrixXd& out_draw_defs) +{ + bool succ = true; + std::string str; + + std::ifstream f_stream(char_file.c_str()); + Json::Value root; + Json::Reader reader; + succ = reader.parse(f_stream, root); + f_stream.close(); + + if (succ) + { + if (!root[gDrawShapeDefsKey].isNull()) + { + Json::Value shape_defs = root.get(gDrawShapeDefsKey, 0); + int num_shapes = shape_defs.size(); + + succ = true; + out_draw_defs.resize(num_shapes, eDrawShapeParamMax); + for (int b = 0; b < num_shapes; ++b) + { + tDrawShapeDef curr_def = BuildDrawShapeDef(); + Json::Value shape_json = shape_defs.get(b, 0); + bool succ_def = ParseDrawShapeDef(shape_json, curr_def); + + if (succ) + { + out_draw_defs.row(b) = curr_def; + } + else + { + succ = false; + break; + } + } + } + } + + if (!succ) + { + printf("Failed to load draw shape definition from %s\n", char_file.c_str()); + assert(false); + } + + return succ; +} + +bool cKinTree::ParseDrawShapeDef(const Json::Value& root, tDrawShapeDef& out_def) +{ + std::string shape_str = root.get(gDrawShapeDescKeys[eDrawShapeShape], "").asString(); + cShape::eShape shape; + bool succ = cShape::ParseShape(shape_str, shape); + if (succ) + { + out_def(eDrawShapeShape) = static_cast(static_cast(shape)); + } + + for (int i = 0; i < eDrawShapeParamMax; ++i) + { + const std::string& curr_key = gDrawShapeDescKeys[i]; + if (!root[curr_key].isNull() + && root[curr_key].isNumeric()) + { + Json::Value json_val = root[curr_key]; + double val = json_val.asDouble(); + out_def(i) = val; + } + } + + return succ; +} + + + +bool cKinTree::Load(const Json::Value& root, Eigen::MatrixXd& out_joint_mat) +{ + bool succ = false; + + if (!root[gJointsKey].isNull()) + { + Json::Value joints = root[gJointsKey]; + int num_joints = joints.size(); + + out_joint_mat.resize(num_joints, eJointDescMax); + + for (int j = 0; j < num_joints; ++j) + { + tJointDesc curr_joint_desc = tJointDesc::Zero(); + + Json::Value joint_json = joints.get(j, 0); + succ = ParseJoint(joint_json, curr_joint_desc); + if (succ) + { + out_joint_mat.row(j) = curr_joint_desc; + } + else + { + printf("Failed to parse joint %i\n", j); + return false; + } + } + + for (int j = 0; j < num_joints; ++j) + { + const auto& curr_desc = out_joint_mat.row(j); + int parent_id = static_cast(curr_desc(eJointDescParent)); + if (parent_id >= j) + { + printf("Parent id must be < child id, parent id: %i, child id: %i\n", parent_id, j); + out_joint_mat.resize(0, 0); + assert(false); + + return false; + } + + out_joint_mat.row(j) = curr_desc; + } + + PostProcessJointMat(out_joint_mat); + } + + return succ; +} +bool cKinTree::ParseJoint(const Json::Value& root, tJointDesc& out_joint_desc) +{ + out_joint_desc = BuildJointDesc(); + eJointType joint_type = eJointTypeNone; + const Json::Value& type_json = root[gJointDescKeys[eJointDescType]]; + if (type_json.isNull()) + { + printf("No joint type specified\n"); + } + else + { + std::string type_str = type_json.asString(); + ParseJointType(type_str, joint_type); + out_joint_desc[eJointDescType] = static_cast(static_cast(joint_type)); + } + + for (int i = 0; i < eJointDescMax; ++i) + { + if (i != eJointDescType) + { + const std::string& key = gJointDescKeys[i]; + if (!root[key].isNull()) + { + out_joint_desc[i] = root[key].asDouble(); + } + } + } + return true; +} + + +std::string cKinTree::BuildJointMatJson(const Eigen::MatrixXd& joint_mat) +{ + std::string json = ""; + json += "{\n\"" + gJointsKey + "\":\n[\n"; + + int num_joints = GetNumJoints(joint_mat); + for (int j = 0; j < num_joints; ++j) + { + tJointDesc curr_desc = joint_mat.row(j); + std::string joint_json = BuildJointJson(j, curr_desc); + + if (j != 0) + { + json += ",\n"; + } + + json += joint_json; + } + + json += "\n]\n}"; + return json; +} + +std::string cKinTree::BuildJointJson(int id, const tJointDesc& joint_desc) +{ + std::string json = ""; + json += "{\n"; + json += "\"ID\": " + std::to_string(id); + for (int i = 0; i < eJointDescMax; ++i) + { + std::string param_name = gJointDescKeys[i]; + if (i == eJointDescType) + { + json += ",\n"; + int type_id = static_cast(joint_desc[i]); + std::string type_str = gJointTypeNames[type_id]; + json += "\"" + param_name + "\": \"" + type_str + "\""; + } + else if (i != eJointDescParamOffset) + { + double val = joint_desc[i]; + if (!std::isfinite(val) && (i == eJointDescTorqueLim || i == eJointDescForceLim)) + { + continue; + } + + if (i != 0) + { + json += ",\n"; + } + + json += "\"" + param_name + "\": " + std::to_string(val); + } + } + + json += "\n}"; + return json; +} +#endif //USE_JSON + +tVector cKinTree::CalcBodyPartPos(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id) +{ + assert(IsValidBody(body_defs, part_id)); + tMatrix body_joint_trans = BodyJointTrans(body_defs, part_id); + tMatrix joint_to_world_trans = JointWorldTrans(joint_mat, state, part_id); + + tVector attach_pt = joint_to_world_trans * body_joint_trans.col(3); + attach_pt[3] = 0; + return attach_pt; +} + +tVector cKinTree::CalcBodyPartVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int part_id) +{ + tVector attach_pt = cKinTree::GetBodyAttachPt(body_defs, part_id); + return CalcWorldVel(joint_mat, state, vel, part_id, attach_pt); +} + +cShape::eShape cKinTree::GetBodyShape(const Eigen::MatrixXd& body_defs, int part_id) +{ + cShape::eShape shape = static_cast(static_cast(body_defs(part_id, cKinTree::eBodyParamShape))); + return shape; +} + +tVector cKinTree::GetBodyAttachPt(const Eigen::MatrixXd& body_defs, int part_id) +{ + const cKinTree::tBodyDef& def = body_defs.row(part_id); + tVector attach_pt = tVector(def(eBodyParamAttachX), def(eBodyParamAttachY), def(eBodyParamAttachZ), 0); + return attach_pt; +} + +tVector cKinTree::GetBodyAttachTheta(const Eigen::MatrixXd& body_defs, int part_id) +{ + tVector attach_theta = tVector(body_defs(part_id, eBodyParamAttachThetaX), + body_defs(part_id, eBodyParamAttachThetaY), + body_defs(part_id, eBodyParamAttachThetaZ), 0); + return attach_theta; +} + +void cKinTree::GetBodyRotation(const Eigen::MatrixXd& body_defs, int part_id, tVector& out_axis, double& out_theta) +{ + tVector theta = GetBodyAttachTheta(body_defs, part_id); + cMathUtil::EulerToAxisAngle(theta, out_axis, out_theta); +} + +double cKinTree::GetBodyMass(const Eigen::MatrixXd& body_defs, int part_id) +{ + double mass = body_defs(part_id, eBodyParamMass); + return mass; +} + +int cKinTree::GetBodyColGroup(const Eigen::MatrixXd& body_defs, int part_id) +{ + int col_group = static_cast(body_defs(part_id, eBodyParamColGroup)); + return col_group; +} + +bool cKinTree::GetBodyEnableFallContact(const Eigen::MatrixXd& body_defs, int part_id) +{ + double fall_val = body_defs(part_id, eBodyParamEnableFallContact); + return fall_val != 0; +} + +void cKinTree::SetBodyEnableFallContact(int part_id, bool enable, Eigen::MatrixXd& out_body_defs) +{ + out_body_defs(part_id, eBodyParamEnableFallContact) = (enable) ? 1 : 0; +} + +tVector cKinTree::GetBodySize(const Eigen::MatrixXd& body_defs, int part_id) +{ + const tBodyDef& def = body_defs.row(part_id); + tVector size = tVector(def(eBodyParam0), def(eBodyParam1), def(eBodyParam2), 0); + return size; +} + +tVector cKinTree::GetBodyColor(const Eigen::MatrixXd& body_defs, int part_id) +{ + const tBodyDef& def = body_defs.row(part_id); + tVector col = tVector(def(eBodyColorR), def(eBodyColorG), def(eBodyColorB), def(eBodyColorA)); + return col; +} + +double cKinTree::CalcTotalMass(const Eigen::MatrixXd& body_defs) +{ + double total_mass = 0; + for (int i = 0; i < body_defs.rows(); ++i) + { + if (IsValidBody(body_defs, i)) + { + double mass = cKinTree::GetBodyMass(body_defs, i); + total_mass += mass; + } + } + return total_mass; +} + +bool cKinTree::IsValidBody(const Eigen::MatrixXd& body_defs, int part_id) +{ + cShape::eShape shape = GetBodyShape(body_defs, part_id); + if (shape != cShape::eShapeNull) + { + return true; + } + return false; +} + +tVector cKinTree::GetBodyLocalCoM(const Eigen::MatrixXd& body_defs, int part_id) +{ + cShape::eShape shape = GetBodyShape(body_defs, part_id); + tVector com = tVector::Zero(); + switch (shape) + { + case cShape::eShapeBox: + case cShape::eShapeCapsule: + case cShape::eShapeSphere: + case cShape::eShapeCylinder: + break; + default: + assert(false); // unsupported + break; + } + + return com; +} + +int cKinTree::GetDrawShapeParentJoint(const tDrawShapeDef& shape) +{ + return static_cast(shape[eDrawShapeParentJoint]); +} + +tVector cKinTree::GetDrawShapeAttachPt(const tDrawShapeDef& shape) +{ + return tVector(shape[eDrawShapeAttachX], shape[eDrawShapeAttachY], shape[cKinTree::eDrawShapeAttachZ], 0); +} + +tVector cKinTree::GetDrawShapeAttachTheta(const tDrawShapeDef& shape) +{ + return tVector(shape[eDrawShapeAttachThetaX], shape[eDrawShapeAttachThetaY], shape[eDrawShapeAttachThetaZ], 0); +} + +void cKinTree::GetDrawShapeRotation(const tDrawShapeDef& shape, tVector& out_axis, double& out_theta) +{ + tVector theta = GetDrawShapeAttachTheta(shape); + cMathUtil::EulerToAxisAngle(theta, out_axis, out_theta); +} + +tVector cKinTree::GetDrawShapeColor(const tDrawShapeDef& shape) +{ + return tVector(shape[eDrawShapeColorR], shape[eDrawShapeColorG], shape[eDrawShapeColorB], shape[cKinTree::eDrawShapeColorA]); +} + +int cKinTree::GetDrawShapeMeshID(const tDrawShapeDef& shape) +{ + return static_cast(shape[eDrawShapeMeshID]); +} + +void cKinTree::CalcBodyPartRotation(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id, tVector& out_axis, double& out_theta) +{ + tMatrix mat = BodyWorldTrans(joint_mat, body_defs, state, part_id); + cMathUtil::RotMatToAxisAngle(mat, out_axis, out_theta); +} + + + +bool cKinTree::HasValidRoot(const Eigen::MatrixXd& joint_desc) +{ + int root = GetRoot(joint_desc); + return root != gInvalidJointID; +} + +tVector cKinTree::GetRootPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state) +{ + int root_id = GetRoot(joint_mat); + tVector pos = tVector::Zero(); + int param_offset = GetParamOffset(joint_mat, root_id); + pos.segment(0, gPosDim) = state.segment(param_offset, gPosDim); + return pos; +} + +void cKinTree::SetRootPos(const Eigen::MatrixXd& joint_mat, const tVector& pos, Eigen::VectorXd& out_state) +{ + int root_id = GetRoot(joint_mat); + int param_offset = GetParamOffset(joint_mat, root_id); + out_state.segment(param_offset, gPosDim) = pos.segment(0, gPosDim); +} + +tQuaternion cKinTree::GetRootRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state) +{ + int root_id = GetRoot(joint_mat); + int param_offset = GetParamOffset(joint_mat, root_id); + tQuaternion rot = cMathUtil::VecToQuat(state.segment(param_offset + gPosDim, gRotDim)); + return rot; +} + +void cKinTree::SetRootRot(const Eigen::MatrixXd& joint_mat, const tQuaternion& rot, Eigen::VectorXd& out_state) +{ + int root_id = GetRoot(joint_mat); + int param_offset = GetParamOffset(joint_mat, root_id); + out_state.segment(param_offset + gPosDim, gRotDim) = cMathUtil::QuatToVec(rot).segment(0, gRotDim); +} + +tVector cKinTree::GetRootVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel) +{ + int root_id = GetRoot(joint_mat); + tVector pos = tVector::Zero(); + int param_offset = GetParamOffset(joint_mat, root_id); + pos.segment(0, gPosDim) = vel.segment(param_offset, gPosDim); + return pos; +} + +void cKinTree::SetRootVel(const Eigen::MatrixXd& joint_mat, const tVector& vel, Eigen::VectorXd& out_vel) +{ + int root_id = GetRoot(joint_mat); + int param_offset = GetParamOffset(joint_mat, root_id); + out_vel.segment(param_offset, gPosDim) = vel.segment(0, gPosDim); +} + +tVector cKinTree::GetRootAngVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel) +{ + int root_id = GetRoot(joint_mat); + tVector ang_vel = tVector::Zero(); + int param_offset = GetParamOffset(joint_mat, root_id); + ang_vel.segment(0, gRotDim) = vel.segment(param_offset + gPosDim, gRotDim); + return ang_vel; +} + +void cKinTree::SetRootAngVel(const Eigen::MatrixXd& joint_mat, const tVector& ang_vel, Eigen::VectorXd& out_vel) +{ + int root_id = GetRoot(joint_mat); + int param_offset = GetParamOffset(joint_mat, root_id); + out_vel.segment(param_offset + gPosDim, gRotDim) = ang_vel.segment(0, gRotDim); +} + +tVector cKinTree::CalcJointWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tVector pos = LocalToWorldPos(joint_mat, state, joint_id, tVector::Zero()); + return pos; +} + +tVector cKinTree::LocalToWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int parent_id, const tVector& attach_pt) +{ + tMatrix local_to_world_trans = JointWorldTrans(joint_mat, state, parent_id); + tVector pos = attach_pt; + pos[3] = 1; + pos = local_to_world_trans * pos; + pos[3] = 0; + + return pos; +} + +tQuaternion cKinTree::CalcJointWorldRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tMatrix mat = JointWorldTrans(joint_mat, state, joint_id); + return cMathUtil::RotMatToQuaternion(mat); +} + +void cKinTree::CalcJointWorldTheta(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id, + tVector& out_axis, double& out_theta) +{ + tMatrix mat = JointWorldTrans(joint_mat, state, joint_id); + cMathUtil::RotMatToAxisAngle(mat, out_axis, out_theta); +} + +tVector cKinTree::CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id) +{ + return CalcWorldVel(joint_mat, state, vel, joint_id, tVector::Zero()); +} + +tVector cKinTree::CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt) +{ + cSpAlg::tSpVec sv = cRBDUtil::CalcWorldVel(joint_mat, state, vel, parent_id); + tVector pos = cKinTree::LocalToWorldPos(joint_mat, state, parent_id, attach_pt); + cSpAlg::tSpTrans world_to_pt = cSpAlg::BuildTrans(pos); + sv = cSpAlg::ApplyTransM(world_to_pt, sv); + return cSpAlg::GetV(sv); +} + +tVector cKinTree::CalcJointWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id) +{ + return CalcWorldAngularVel(joint_mat, state, vel, joint_id, tVector::Zero()); +} + +tVector cKinTree::CalcWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt) +{ + cSpAlg::tSpVec sv = cRBDUtil::CalcWorldVel(joint_mat, state, vel, parent_id); + tVector pos = cKinTree::LocalToWorldPos(joint_mat, state, parent_id, attach_pt); + cSpAlg::tSpTrans world_to_pt = cSpAlg::BuildTrans(pos); + sv = cSpAlg::ApplyTransM(world_to_pt, sv); + return cSpAlg::GetOmega(sv); +} + +int cKinTree::GetNumJoints(const Eigen::MatrixXd& joint_mat) +{ + return static_cast(joint_mat.rows()); +} + +int cKinTree::GetNumDof(const Eigen::MatrixXd& joint_mat) +{ + int num_joints = GetNumJoints(joint_mat); + int num_dof = cKinTree::GetParamOffset(joint_mat, num_joints - 1) + cKinTree::GetParamSize(joint_mat, num_joints - 1); + return num_dof; +} + +void cKinTree::ApplyStep(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& step, Eigen::VectorXd& out_pose) +{ + int root_id = GetRoot(joint_mat); + int num_joints = cKinTree::GetNumJoints(joint_mat); + out_pose += step; +} + + +Eigen::VectorXi cKinTree::FindJointChain(const Eigen::MatrixXd& joint_mat, int joint_beg, int joint_end) +{ + Eigen::VectorXi chain; + + const int max_length = 128; + int chain_buffer[max_length]; + int buffer_idx = 0; + + if (joint_beg == joint_end) + { + Eigen::VectorXi chain(1); + chain[0] = joint_beg; + } + + int common_ancestor = gInvalidJointID; + int curr_id = joint_beg; + int end_len = 0; + while (curr_id != gInvalidJointID) + { + chain_buffer[buffer_idx] = curr_id; + ++buffer_idx; + + if (buffer_idx >= max_length) + { + printf("Exceeded maximum chain length %i\n", max_length); + assert(false); + return chain; + } + + bool is_ancestor = IsAncestor(joint_mat, joint_end, curr_id, end_len); + if (is_ancestor) + { + common_ancestor = curr_id; + break; + } + else + { + curr_id = GetParent(joint_mat, curr_id); + } + } + + bool found = common_ancestor != gInvalidJointID; + // tree should always connected? + assert(found); + + if (found) + { + chain.resize(buffer_idx + end_len); + for (int i = 0; i < buffer_idx; ++i) + { + chain[i] = chain_buffer[i]; + } + + int idx = buffer_idx; + curr_id = joint_end; + while (curr_id != common_ancestor) + { + chain[idx] = curr_id; + curr_id = GetParent(joint_mat, curr_id); + ++idx; + } + + int num_flips = static_cast(chain.size()) - buffer_idx; + chain.block(buffer_idx, 0, num_flips, 1).reverseInPlace(); + } + + return chain; +} + +bool cKinTree::IsAncestor(const Eigen::MatrixXd& joint_mat, int child_joint, int ancestor_joint, int& out_len) +{ + int curr_id = child_joint; + out_len = 0; + while (curr_id != gInvalidJointID) + { + if (curr_id == ancestor_joint) + { + return true; + } + else + { + curr_id = GetParent(joint_mat, curr_id); + ++out_len; + } + } + return false; +} + +double cKinTree::CalcChainLength(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXi& chain) +{ + double len = 0; + int num_joints = static_cast(chain.size()); + for (int i = 1; i < num_joints; ++i) + { + int curr_id = chain(i); + int prev_id = chain(i - 1); + + if (prev_id != gInvalidJointID) + { + int prev_parent = GetParent(joint_mat, prev_id); + bool is_parent = (prev_parent == curr_id); + if (is_parent) + { + double curr_len = CalcLinkLength(joint_mat, prev_id); + len += curr_len; + } + } + + if (curr_id != gInvalidJointID) + { + int curr_parent = GetParent(joint_mat, curr_id); + bool is_child = (curr_parent == prev_id); + if (is_child) + { + double curr_len = CalcLinkLength(joint_mat, curr_id); + len += curr_len; + } + } + } + + return len; +} + +void cKinTree::CalcAABB(const Eigen::MatrixXd& joint_desc, const Eigen::VectorXd& state, tVector& out_min, tVector& out_max) +{ + out_min[0] = std::numeric_limits::infinity(); + out_min[1] = std::numeric_limits::infinity(); + out_min[2] = std::numeric_limits::infinity(); + + out_max[0] = -std::numeric_limits::infinity(); + out_max[1] = -std::numeric_limits::infinity(); + out_max[2] = -std::numeric_limits::infinity(); + + for (int i = 0; i < GetNumJoints(joint_desc); ++i) + { + tVector pos = CalcJointWorldPos(joint_desc, state, i); + out_min = out_min.cwiseMin(pos); + out_max = out_max.cwiseMax(pos); + } +} + +int cKinTree::GetParamOffset(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + int offset = static_cast(joint_mat(joint_id, eJointDescParamOffset)); + return offset; +} + +int cKinTree::GetParamSize(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + eJointType joint_type = cKinTree::GetJointType(joint_mat, joint_id); + bool is_root = cKinTree::IsRoot(joint_mat, joint_id); + int size = (is_root) ? gRootDim : GetJointParamSize(joint_type); + return size; +} + +int cKinTree::GetJointParamSize(eJointType joint_type) +{ + int size = 0; + switch (joint_type) + { + case eJointTypeRevolute: + size = 1; + break; + case eJointTypePrismatic: + size = 1; + break; + case eJointTypePlanar: + size = 3; + break; + case eJointTypeFixed: + size = 0; + break; + case eJointTypeSpherical: + size = 4; + break; + default: + assert(false); // unsupported joint type + break; + } + + return size; +} + +void cKinTree::GetJointParams(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int j, Eigen::VectorXd& out_params) +{ + int offset = GetParamOffset(joint_mat, j); + int dim = GetParamSize(joint_mat, j); + if (dim > 0) + { + out_params = state.block(offset, 0, dim, 1); + } + else + { + out_params = Eigen::VectorXd::Zero(1); + } +} + +void cKinTree::SetJointParams(const Eigen::MatrixXd& joint_mat, int j, const Eigen::VectorXd& params, Eigen::VectorXd& out_state) +{ + int offset = GetParamOffset(joint_mat, j); + int dim = GetParamSize(joint_mat, j); + assert(dim == params.size()); + out_state.block(offset, 0, dim, 1) = params; +} + +cKinTree::eJointType cKinTree::GetJointType(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + eJointType type = static_cast(static_cast(joint_mat(joint_id, cKinTree::eJointDescType))); + return type; +} + +int cKinTree::GetParent(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + int parent = static_cast(joint_mat(joint_id, cKinTree::eJointDescParent)); + assert(parent < joint_id); // joints should always be ordered as such + // since some algorithms will assume this ordering + return parent; +} + +bool cKinTree::HasParent(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + int parent = GetParent(joint_mat, joint_id); + return parent != gInvalidJointID; +} + +bool cKinTree::IsRoot(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + return !HasParent(joint_mat, joint_id); +} + +bool cKinTree::IsJointActuated(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + return !IsRoot(joint_mat, joint_id); +} + +double cKinTree::GetTorqueLimit(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + double torque_lim = joint_mat(joint_id, cKinTree::eJointDescTorqueLim); + return torque_lim; +} + +double cKinTree::GetForceLimit(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + double force_lim = joint_mat(joint_id, cKinTree::eJointDescForceLim); + return force_lim; +} + +bool cKinTree::IsEndEffector(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + double end_eff_val = joint_mat(joint_id, cKinTree::eJointDescIsEndEffector); + return end_eff_val != 0; +} + +tVector cKinTree::GetJointLimLow(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + return tVector(joint_mat(joint_id, cKinTree::eJointDescLimLow0), + joint_mat(joint_id, cKinTree::eJointDescLimLow1), + joint_mat(joint_id, cKinTree::eJointDescLimLow2), 1); +} + +tVector cKinTree::GetJointLimHigh(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + return tVector(joint_mat(joint_id, cKinTree::eJointDescLimHigh0), + joint_mat(joint_id, cKinTree::eJointDescLimHigh1), + joint_mat(joint_id, cKinTree::eJointDescLimHigh2), 0); +} + +double cKinTree::GetJointDiffWeight(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + return joint_mat(joint_id, cKinTree::eJointDescDiffWeight); +} + +double cKinTree::CalcLinkLength(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + tVector attach_pt = GetAttachPt(joint_mat, joint_id); + bool is_root = IsRoot(joint_mat, joint_id); + double len = (is_root) ? 0 : attach_pt.norm(); + return len; +} + +tVector cKinTree::GetAttachPt(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + tVector attach_pt = tVector(joint_mat(joint_id, cKinTree::eJointDescAttachX), + joint_mat(joint_id, cKinTree::eJointDescAttachY), + joint_mat(joint_id, cKinTree::eJointDescAttachZ), 0); + return attach_pt; +} + +tVector cKinTree::GetAttachTheta(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + tVector attach_theta = tVector(joint_mat(joint_id, cKinTree::eJointDescAttachThetaX), + joint_mat(joint_id, cKinTree::eJointDescAttachThetaY), + joint_mat(joint_id, cKinTree::eJointDescAttachThetaZ), 0); + return attach_theta; +} + +void cKinTree::CalcMaxSubChainLengths(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_lengths) +{ + int num_joints = static_cast(joint_mat.rows()); + out_lengths = Eigen::VectorXd::Zero(num_joints); + + for (int j = num_joints - 1; j >= 0; --j) + { + int parent_id = GetParent(joint_mat, j); + if (parent_id != gInvalidJointID) + { + double curr_val = out_lengths(j); + double len = CalcLinkLength(joint_mat, j); + double& parent_val = out_lengths(parent_id); + + if (parent_val < len + curr_val) + { + parent_val = len + curr_val; + } + } + } +} + +void cKinTree::CalcSubTreeMasses(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, Eigen::VectorXd& out_masses) +{ + int num_joints = static_cast(joint_mat.rows()); + out_masses = Eigen::VectorXd::Zero(num_joints); + + for (int j = num_joints - 1; j >= 0; --j) + { + double& curr_val = out_masses(j); + double mass = GetBodyMass(body_defs, j); + curr_val += mass; + + int parent_id = GetParent(joint_mat, j); + if (parent_id != gInvalidJointID) + { + double& parent_val = out_masses(parent_id); + parent_val += curr_val; + } + } +} + +bool cKinTree::ParseJointType(const std::string& type_str, eJointType& out_joint_type) +{ + for (int i = 0; i < eJointTypeMax; ++i) + { + const std::string& name = gJointTypeNames[i]; + if (type_str == name) + { + out_joint_type = static_cast(i); + return true; + } + } + printf("Unsupported joint type: %s\n", type_str.c_str()); + assert(false); // unsupported joint type + return false; +} + +void cKinTree::PostProcessJointMat(Eigen::MatrixXd& out_joint_mat) +{ + int num_joints = GetNumJoints(out_joint_mat); + int offset = 0; + for (int j = 0; j < num_joints; ++j) + { + int curr_size = GetParamSize(out_joint_mat, j); + out_joint_mat(j, eJointDescParamOffset) = offset; + offset += curr_size; + } + int root_id = GetRoot(out_joint_mat); + + out_joint_mat(root_id, eJointDescAttachX) = 0; + out_joint_mat(root_id, eJointDescAttachY) = 0; + out_joint_mat(root_id, eJointDescAttachZ) = 0; +} + +tMatrix cKinTree::BuildAttachTrans(const Eigen::MatrixXd& joint_mat, int joint_id) +{ + // child to parent + tVector attach_pt = GetAttachPt(joint_mat, joint_id); + tVector attach_theta = GetAttachTheta(joint_mat, joint_id); + tMatrix mat = cMathUtil::RotateMat(attach_theta); + mat(0, 3) = attach_pt[0]; + mat(1, 3) = attach_pt[1]; + mat(2, 3) = attach_pt[2]; + return mat; +} + +tMatrix cKinTree::ChildParentTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tMatrix mat; + eJointType j_type = GetJointType(joint_mat, joint_id); + bool is_root = IsRoot(joint_mat, joint_id); + + if (is_root) + { + mat = ChildParentTransRoot(joint_mat, state, joint_id); + } + else + { + switch (j_type) + { + case eJointTypeRevolute: + mat = ChildParentTransRevolute(joint_mat, state, joint_id); + break; + case eJointTypePrismatic: + mat = ChildParentTransPrismatic(joint_mat, state, joint_id); + break; + case eJointTypePlanar: + mat = ChildParentTransPlanar(joint_mat, state, joint_id); + break; + case eJointTypeFixed: + mat = ChildParentTransFixed(joint_mat, state, joint_id); + break; + case eJointTypeSpherical: + mat = ChildParentTransSpherical(joint_mat, state, joint_id); + break; + default: + break; + } + } + + return mat; +} + +tMatrix cKinTree::ParentChildTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tMatrix mat = ChildParentTrans(joint_mat, state, joint_id); + mat = cMathUtil::InvRigidMat(mat); + return mat; +} + +tMatrix cKinTree::JointWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tMatrix m = tMatrix::Identity(); + int curr_id = joint_id; + assert(joint_id != gInvalidIdx); // invalid joint + while (curr_id != gInvalidJointID) + { + tMatrix child_parent_mat = ChildParentTrans(joint_mat, state, curr_id); + m = child_parent_mat * m; + curr_id = GetParent(joint_mat, curr_id); + } + + return m; +} + +tMatrix cKinTree::WorldJointTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tMatrix m = JointWorldTrans(joint_mat, state, joint_id); + m = cMathUtil::InvRigidMat(m); + return m; +} + +tMatrix cKinTree::BodyWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id) +{ + tMatrix body_trans = BodyJointTrans(body_defs, part_id); + tMatrix joint_trans = JointWorldTrans(joint_mat, state, part_id); + body_trans = joint_trans * body_trans; + return body_trans; +} + +tMatrix cKinTree::BodyJointTrans(const Eigen::MatrixXd& body_defs, int part_id) +{ + tVector attach_pt = GetBodyAttachPt(body_defs, part_id); + tVector euler = GetBodyAttachTheta(body_defs, part_id); + tVector com = GetBodyLocalCoM(body_defs, part_id); + + tMatrix rot = cMathUtil::RotateMat(euler); + tMatrix trans = cMathUtil::TranslateMat(attach_pt + com); + tMatrix m = trans * rot; + return m; +} + +cKinTree::tJointDesc cKinTree::BuildJointDesc(eJointType joint_type, int parent_id, const tVector& attach_pt) +{ + tJointDesc desc = BuildJointDesc(); + desc(eJointDescType) = static_cast(joint_type); + desc(eJointDescParent) = parent_id; + desc(eJointDescAttachX) = attach_pt[0]; + desc(eJointDescAttachY) = attach_pt[1]; + desc(eJointDescAttachZ) = attach_pt[2]; + + return desc; +} + +cKinTree::tJointDesc cKinTree::BuildJointDesc() +{ + tJointDesc desc; + desc(eJointDescType) = static_cast(eJointTypeRevolute); + desc(eJointDescParent) = gInvalidIdx; + desc(eJointDescAttachX) = 0; + desc(eJointDescAttachY) = 0; + desc(eJointDescAttachZ) = 0; + desc(eJointDescAttachThetaX) = 0; + desc(eJointDescAttachThetaY) = 0; + desc(eJointDescAttachThetaZ) = 0; + desc(eJointDescLimLow0) = 1; + desc(eJointDescLimLow1) = 1; + desc(eJointDescLimLow2) = 1; + desc(eJointDescLimHigh0) = 0; + desc(eJointDescLimHigh1) = 0; + desc(eJointDescLimHigh2) = 0; + desc(eJointDescIsEndEffector) = 0; + desc(eJointDescTorqueLim) = std::numeric_limits::infinity(); + desc(eJointDescForceLim) = std::numeric_limits::infinity(); + desc(eJointDescDiffWeight) = 1; + desc(eJointDescParamOffset) = 0; + + return desc; +} + +cKinTree::tBodyDef cKinTree::BuildBodyDef() +{ + tBodyDef def; + def(eBodyParamShape) = static_cast(cShape::eShapeNull); + def(eBodyParamMass) = 0; + def(eBodyParamColGroup) = gInvalidIdx; + def(eBodyParamEnableFallContact) = 0; + def(eBodyParamAttachX) = 0; + def(eBodyParamAttachY) = 0; + def(eBodyParamAttachZ) = 0; + def(eBodyParamAttachThetaX) = 0; + def(eBodyParamAttachThetaY) = 0; + def(eBodyParamAttachThetaZ) = 0; + def(eBodyParam0) = 0; + def(eBodyParam1) = 0; + def(eBodyParam2) = 0; + def(eBodyColorR) = 0; + def(eBodyColorG) = 0; + def(eBodyColorB) = 0; + def(eBodyColorA) = 1; + return def; +} + +cKinTree::tDrawShapeDef cKinTree::BuildDrawShapeDef() +{ + tDrawShapeDef def; + def(eDrawShapeShape) = static_cast(cShape::eShapeNull); + def(eDrawShapeParentJoint) = gInvalidIdx; + def(eDrawShapeAttachX) = 0; + def(eDrawShapeAttachY) = 0; + def(eDrawShapeAttachZ) = 0; + def(eDrawShapeAttachThetaX) = 0; + def(eDrawShapeAttachThetaY) = 0; + def(eDrawShapeAttachThetaZ) = 0; + def(eDrawShapeParam0) = 0; + def(eDrawShapeParam1) = 0; + def(eDrawShapeParam2) = 0; + def(eDrawShapeColorR) = 0; + def(eDrawShapeColorG) = 0; + def(eDrawShapeColorB) = 0; + def(eDrawShapeColorA) = 1; + def(eDrawShapeMeshID) = gInvalidIdx; + return def; +} + +void cKinTree::BuildDefaultPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose) +{ + int num_dof = GetNumDof(joint_mat); + out_pose = Eigen::VectorXd::Zero(num_dof); + + int num_joints = GetNumJoints(joint_mat); + + int root_id = GetRoot(joint_mat); + Eigen::VectorXd root_pose; + BuildDefaultPoseRoot(joint_mat, root_pose); + SetJointParams(joint_mat, root_id, root_pose, out_pose); + + for (int j = 1; j < num_joints; ++j) + { + eJointType joint_type = GetJointType(joint_mat, j); + Eigen::VectorXd joint_pose; + switch (joint_type) + { + case eJointTypeRevolute: + BuildDefaultPoseRevolute(joint_pose); + break; + case eJointTypePrismatic: + BuildDefaultPosePrismatic(joint_pose); + break; + case eJointTypePlanar: + BuildDefaultPosePlanar(joint_pose); + break; + case eJointTypeFixed: + BuildDefaultPoseFixed(joint_pose); + break; + case eJointTypeSpherical: + BuildDefaultPoseSpherical(joint_pose); + break; + default: + assert(false); // unsupported joint type + break; + } + + SetJointParams(joint_mat, j, joint_pose, out_pose); + } +} + +void cKinTree::BuildDefaultVel(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_vel) +{ + int num_dof = GetNumDof(joint_mat); + out_vel = Eigen::VectorXd::Zero(num_dof); + + int num_joints = GetNumJoints(joint_mat); + + int root_id = GetRoot(joint_mat); + Eigen::VectorXd root_pose; + BuildDefaultVelRoot(joint_mat, root_pose); + SetJointParams(joint_mat, root_id, root_pose, out_vel); + + for (int j = 1; j < num_joints; ++j) + { + eJointType joint_type = GetJointType(joint_mat, j); + Eigen::VectorXd joint_pose; + switch (joint_type) + { + case eJointTypeRevolute: + BuildDefaultVelRevolute(joint_pose); + break; + case eJointTypePrismatic: + BuildDefaultVelPrismatic(joint_pose); + break; + case eJointTypePlanar: + BuildDefaultVelPlanar(joint_pose); + break; + case eJointTypeFixed: + BuildDefaultVelFixed(joint_pose); + break; + case eJointTypeSpherical: + BuildDefaultVelSpherical(joint_pose); + break; + default: + assert(false); // unsupported joint type + break; + } + + SetJointParams(joint_mat, j, joint_pose, out_vel); + } +} + +void cKinTree::CalcPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff) +{ + int num_joints = GetNumJoints(joint_mat); + assert(out_diff.size() == pose0.size()); + assert(pose1.size() == pose0.size()); + + out_diff.resize(pose1.size()); + for (int j = 0; j < num_joints; ++j) + { + Eigen::VectorXd curr_diff; + CalcJointPoseDiff(joint_mat, j, pose0, pose1, curr_diff); + + int param_offset = GetParamOffset(joint_mat, j); + int param_size = GetParamSize(joint_mat, j); + out_diff.segment(param_offset, param_size) = curr_diff; + } +} + +tVector cKinTree::CalcRootPosDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1) +{ + tVector root_pos0 = GetRootPos(joint_mat, pose0); + tVector root_pos1 = GetRootPos(joint_mat, pose1); + return root_pos1 - root_pos0; +} + +tQuaternion cKinTree::CalcRootRotDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1) +{ + tQuaternion root_rot0 = GetRootRot(joint_mat, pose0); + tQuaternion root_rot1 = GetRootRot(joint_mat, pose1); + return cMathUtil::QuatDiff(root_rot0, root_rot1); +} + +double cKinTree::CalcPoseErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1) +{ + bool is_root = IsRoot(joint_mat, joint_id); + double err = 0; + if (is_root) + { + double root_pos_err = CalcRootPosErr(joint_mat, pose0, pose1); + double root_rot_err = CalcRootRotErr(joint_mat, pose0, pose1); + err = root_pos_err + root_rot_err; + } + else + { + Eigen::VectorXd diff; + CalcJointPoseDiff(joint_mat, joint_id, pose0, pose1, diff); + + eJointType joint_type = GetJointType(joint_mat, joint_id); + switch (joint_type) + { + case eJointTypeSpherical: + { + tQuaternion dq = cMathUtil::VecToQuat(diff); + double theta = cMathUtil::QuatTheta(dq); + err = theta * theta; + break; + } + default: + err = diff.squaredNorm(); + break; + } + } + return err; +} + +double cKinTree::CalcRootPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1) +{ + tVector diff = CalcRootPosDiff(joint_mat, pose0, pose1); + return diff.squaredNorm(); +} + +double cKinTree::CalcRootRotErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1) +{ + tQuaternion diff = CalcRootRotDiff(joint_mat, pose0, pose1); + double theta = cMathUtil::QuatTheta(diff); + return theta * theta; +} + + +void cKinTree::CalcVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff) +{ + int num_joints = GetNumJoints(joint_mat); + assert(vel0.size() == vel1.size()); + + out_diff.resize(vel0.size()); + for (int j = 0; j < num_joints; ++j) + { + Eigen::VectorXd curr_diff; + CalcJointVelDiff(joint_mat, j, vel0, vel1, curr_diff); + + int param_offset = GetParamOffset(joint_mat, j); + int param_size = GetParamSize(joint_mat, j); + out_diff.segment(param_offset, param_size) = curr_diff; + } +} + +tVector cKinTree::CalcRootVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1) +{ + tVector root_vel0 = GetRootVel(joint_mat, pose0); + tVector root_vel1 = GetRootVel(joint_mat, pose1); + return root_vel1 - root_vel0; +} + +tVector cKinTree::CalcRootAngVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1) +{ + tVector root_ang_vel0 = GetRootAngVel(joint_mat, pose0); + tVector root_ang_vel1 = GetRootAngVel(joint_mat, pose1); + return root_ang_vel1 - root_ang_vel0; +} + +double cKinTree::CalcVelErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1) +{ + bool is_root = IsRoot(joint_mat, joint_id); + double err = 0; + if (is_root) + { + double root_vel_err = CalcRootVelErr(joint_mat, vel0, vel1); + double root_ang_vel_err = CalcRootAngVelErr(joint_mat, vel0, vel1); + err = root_vel_err + root_ang_vel_err; + } + else + { + Eigen::VectorXd diff; + CalcJointVelDiff(joint_mat, joint_id, vel0, vel1, diff); + err = diff.squaredNorm(); + } + return err; +} + +double cKinTree::CalcRootVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1) +{ + tVector diff = CalcRootVelDiff(joint_mat, vel0, vel1); + return diff.squaredNorm(); +} + +double cKinTree::CalcRootAngVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1) +{ + tVector diff = CalcRootAngVelDiff(joint_mat, vel0, vel1); + return diff.squaredNorm(); +} + + +void cKinTree::CalcJointPoseDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff) +{ + int param_offset = GetParamOffset(joint_mat, joint_id); + int param_size = GetParamSize(joint_mat, joint_id); + bool is_root = cKinTree::IsRoot(joint_mat, joint_id); + + if (is_root) + { + tVector root_pos_diff = CalcRootPosDiff(joint_mat, pose0, pose1); + tQuaternion root_rot_diff = CalcRootRotDiff(joint_mat, pose0, pose1); + out_diff.resize(gRootDim); + out_diff.segment(0, gPosDim) = root_pos_diff.segment(0, gPosDim); + out_diff.segment(gPosDim, gRotDim) = cMathUtil::QuatToVec(root_rot_diff); + } + else + { + eJointType joint_type = GetJointType(joint_mat, joint_id); + switch (joint_type) + { + case eJointTypeSpherical: + { + tQuaternion q0 = cMathUtil::VecToQuat(pose0.segment(param_offset, param_size)); + tQuaternion q1 = cMathUtil::VecToQuat(pose1.segment(param_offset, param_size)); + tQuaternion q_diff = cMathUtil::QuatDiff(q0, q1); + out_diff = cMathUtil::QuatToVec(q_diff); + break; + } + default: + out_diff = pose1.segment(param_offset, param_size) - pose0.segment(param_offset, param_size); + break; + } + } +} + +void cKinTree::CalcJointVelDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff) +{ + int param_offset = GetParamOffset(joint_mat, joint_id); + int param_size = GetParamSize(joint_mat, joint_id); + out_diff = vel1.segment(param_offset, param_size) - vel0.segment(param_offset, param_size); +} + +void cKinTree::CalcVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double dt, Eigen::VectorXd& out_vel) +{ + assert(pose0.size() == pose1.size()); + out_vel.resize(pose0.size()); + + int num_joints = GetNumJoints(joint_mat); + tVector root_pos0 = GetRootPos(joint_mat, pose0); + tVector root_pos1 = GetRootPos(joint_mat, pose1); + tVector root_vel = (root_pos1 - root_pos0) / dt; + + tQuaternion root_rot0 = GetRootRot(joint_mat, pose0); + tQuaternion root_rot1 = GetRootRot(joint_mat, pose1); + tVector root_rot_vel = cMathUtil::CalcQuaternionVelRel(root_rot0, root_rot1, dt); + + cKinTree::SetRootVel(joint_mat, root_vel, out_vel); + cKinTree::SetRootAngVel(joint_mat, root_rot_vel, out_vel); + + for (int j = 1; j < num_joints; ++j) + { + int param_offset = GetParamOffset(joint_mat, j); + int param_size = GetParamSize(joint_mat, j); + eJointType joint_type = GetJointType(joint_mat, j); + + switch (joint_type) + { + case eJointTypeSpherical: + { + tQuaternion q0 = cMathUtil::VecToQuat(pose0.segment(param_offset, param_size)); + tQuaternion q1 = cMathUtil::VecToQuat(pose1.segment(param_offset, param_size)); + tVector rot_vel = cMathUtil::CalcQuaternionVelRel(q0, q1, dt); + out_vel.segment(param_offset, param_size) = rot_vel.segment(0, param_size); + break; + } + default: + out_vel.segment(param_offset, param_size) = (pose1.segment(param_offset, param_size) - pose0.segment(param_offset, param_size)) / dt; + break; + } + } +} + +void cKinTree::PostProcessPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose) +{ + // mostly to normalize quaternions + int num_joints = GetNumJoints(joint_mat); + int root_id = GetRoot(joint_mat); + int root_offset = GetParamOffset(joint_mat, root_id); + out_pose.segment(root_offset + gPosDim, gRotDim).normalize(); + + for (int j = 1; j < num_joints; ++j) + { + eJointType joint_type = GetJointType(joint_mat, j); + if (joint_type == eJointTypeSpherical) + { + int offset = GetParamOffset(joint_mat, j); + out_pose.segment(offset, gRotDim).normalize(); + } + } +} + +void cKinTree::LerpPoses(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double lerp, Eigen::VectorXd& out_pose) +{ + int num_joints = GetNumJoints(joint_mat); + int root_id = GetRoot(joint_mat); + int root_offset = GetParamOffset(joint_mat, root_id); + + out_pose.resize(pose0.size()); + assert(pose0.size() == pose1.size()); + + tVector root_pos0 = GetRootPos(joint_mat, pose0); + tVector root_pos1 = GetRootPos(joint_mat, pose1); + tVector root_pos_lerp = (1 - lerp) * root_pos0 + lerp * root_pos1; + + tQuaternion root_rot0 = GetRootRot(joint_mat, pose0); + tQuaternion root_rot1 = GetRootRot(joint_mat, pose1); + assert(std::abs(root_rot0.norm() - 1) < 0.000001); + assert(std::abs(root_rot1.norm() - 1) < 0.000001); + + tQuaternion root_rot_lerp = root_rot0.slerp(lerp, root_rot1); + root_rot_lerp.normalize(); + + cKinTree::SetRootPos(joint_mat, root_pos_lerp, out_pose); + cKinTree::SetRootRot(joint_mat, root_rot_lerp, out_pose); + + for (int j = 1; j < num_joints; ++j) + { + eJointType joint_type = GetJointType(joint_mat, j); + int offset = GetParamOffset(joint_mat, j); + int size = GetParamSize(joint_mat, j); + if (joint_type == eJointTypeSpherical) + { + tQuaternion rot0 = cMathUtil::VecToQuat(pose0.segment(offset, size)); + tQuaternion rot1 = cMathUtil::VecToQuat(pose1.segment(offset, size)); + assert(std::abs(rot0.norm() - 1) < 0.000001); + assert(std::abs(rot1.norm() - 1) < 0.000001); + tQuaternion rot_lerp = rot0.slerp(lerp, rot1); + out_pose.segment(offset, size) = cMathUtil::QuatToVec(rot_lerp).segment(0, size); + } + else + { + out_pose.segment(offset, size) = (1 - lerp) * pose0.segment(offset, size) + lerp * pose1.segment(offset, size); + } + } +} + +void cKinTree::VelToPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, Eigen::VectorXd& out_pose_diff) +{ + out_pose_diff = vel; + + int num_joints = GetNumJoints(joint_mat); + int root_id = GetRoot(joint_mat); + int root_offset = GetParamOffset(joint_mat, root_id); + + tVector root_rot_vel = GetRootAngVel(joint_mat, vel); + root_rot_vel[3] = 0; + tQuaternion root_rot = GetRootRot(joint_mat, pose); + tMatrix root_diff_mat = cMathUtil::BuildQuaternionDiffMat(root_rot); + tVector root_diff = root_diff_mat * root_rot_vel; + tQuaternion root_quat = cMathUtil::VecToQuat(root_diff); + + cKinTree::SetRootRot(joint_mat, root_quat, out_pose_diff); + + for (int j = 1; j < num_joints; ++j) + { + eJointType joint_type = GetJointType(joint_mat, j); + int offset = GetParamOffset(joint_mat, j); + int size = GetParamSize(joint_mat, j); + if (joint_type == eJointTypeSpherical) + { + tQuaternion rot = cMathUtil::VecToQuat(pose.segment(offset, size)); + tVector rot_vel = vel.segment(offset, size); + rot_vel[3] = 0; + tMatrix diff_mat = cMathUtil::BuildQuaternionDiffMat(rot); + tVector q_diff = diff_mat * rot_vel; + + out_pose_diff.segment(offset, size) = q_diff; + } + } +} + +double cKinTree::CalcHeading(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose) +{ + // heading is the direction of the root in the xz plane + tVector ref_dir = tVector(1, 0, 0, 0); + tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, pose); + printf("root_rot=%f,%f,%f,%f\n", root_rot.x(), root_rot.y(), root_rot.z(), root_rot.w()); + tVector rot_dir = cMathUtil::QuatRotVec(root_rot, ref_dir); + double heading = std::atan2(-rot_dir[2], rot_dir[0]); + return heading; +} + +tQuaternion cKinTree::CalcHeadingRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose) +{ + double heading = CalcHeading(joint_mat, pose); + return cMathUtil::AxisAngleToQuaternion(tVector(0, 1, 0, 0), heading); +} + +tMatrix cKinTree::BuildHeadingTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose) +{ + double heading = CalcHeading(joint_mat, pose); + tVector axis = tVector(0, 1, 0, 0); + printf("heading=%f\n", heading); + tMatrix mat = cMathUtil::RotateMat(axis, -heading); + return mat; +} + +tMatrix cKinTree::BuildOriginTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose) +{ + // world to origin transform + // origin is the point right under the root of the character on the xz plane with x-axis + // aligned along the character's heading + tVector origin = GetRootPos(joint_mat, pose); + printf("cKinTree::BuildOriginTrans: origin=%f,%f,%f\n", origin[0], origin[1], origin[2]); + origin[1] = 0; + tMatrix rot_mat = BuildHeadingTrans(joint_mat, pose); + printf("headingMat = \n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f", + rot_mat(0, 0), rot_mat(0, 1), rot_mat(0, 2), rot_mat(0, 3), + rot_mat(1, 0), rot_mat(1, 1), rot_mat(1, 2), rot_mat(1, 3), + rot_mat(2, 0), rot_mat(2, 1), rot_mat(2, 2), rot_mat(2, 3), + rot_mat(3, 0), rot_mat(3, 1), rot_mat(3, 2), rot_mat(3, 3)); + + tMatrix trans_mat = cMathUtil::TranslateMat(-origin); + tMatrix mat = rot_mat * trans_mat; + printf("mat = \n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f", + mat(0,0), mat(0, 1), mat(0, 2), mat(0, 3), + mat(1, 0), mat(1, 1), mat(1, 2), mat(1, 3), + mat(2, 0), mat(2, 1), mat(2, 2), mat(2, 3), + mat(3, 0), mat(3, 1), mat(3, 2), mat(3, 3)); + return mat; +} + +void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose) +{ + Eigen::VectorXd vel= Eigen::VectorXd::Zero(0); + NormalizePoseHeading(joint_mat, out_pose, vel); +} + +void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel) +{ + tVector root_pos = GetRootPos(joint_mat, out_pose); + tQuaternion root_rot = GetRootRot(joint_mat, out_pose); + + root_pos[0] = 0; + root_pos[2] = 0; + + double heading = CalcHeading(joint_mat, out_pose); + tQuaternion heading_q = cMathUtil::AxisAngleToQuaternion(tVector(0, 1, 0, 0), -heading); + root_rot = heading_q * root_rot; + + SetRootPos(joint_mat, root_pos, out_pose); + SetRootRot(joint_mat, root_rot, out_pose); + + if (out_vel.size() > 0) + { + tVector root_vel = GetRootVel(joint_mat, out_vel); + tVector root_ang_vel = GetRootAngVel(joint_mat, out_vel); + root_vel = cMathUtil::QuatRotVec(heading_q, root_vel); + root_ang_vel = cMathUtil::QuatRotVec(heading_q, root_ang_vel); + + SetRootVel(joint_mat, root_vel, out_vel); + SetRootAngVel(joint_mat, root_ang_vel, out_vel); + } +} + +void cKinTree::MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector mirror_joints0, const std::vector mirror_joints1, Eigen::VectorXd& out_pose) +{ + // mirrors along xy plane + assert(mirror_joints0.size() == mirror_joints1.size()); + int num_joints = cKinTree::GetNumJoints(joint_mat); + + tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, out_pose); + tVector root_pos = cKinTree::GetRootPos(joint_mat, out_pose); + root_pos[2] *= -1; + root_rot = cMathUtil::MirrorQuaternion(root_rot, cMathUtil::eAxisZ); + + cKinTree::SetRootRot(joint_mat, root_rot, out_pose); + cKinTree::SetRootPos(joint_mat, root_pos, out_pose); + + for (int j = 0; j < num_joints; ++j) + { + cKinTree::eJointType joint_type = cKinTree::GetJointType(joint_mat, j); + bool is_root = cKinTree::IsRoot(joint_mat, j); + if (!is_root) + { + int param_offset = cKinTree::GetParamOffset(joint_mat, j); + int param_size = cKinTree::GetParamSize(joint_mat, j); + auto joint_params = out_pose.segment(param_offset, param_size); + + if (joint_type == cKinTree::eJointTypeRevolute + || joint_type == cKinTree::eJointTypeFixed) + { + } + else if (joint_type == cKinTree::eJointTypeSpherical) + { + tQuaternion quat = cMathUtil::VecToQuat(joint_params); + quat = cMathUtil::MirrorQuaternion(quat, cMathUtil::eAxisZ); + joint_params = cMathUtil::QuatToVec(quat); + } + else + { + assert(false); // unsupported joint type + } + } + } + + for (size_t i = 0; i < mirror_joints0.size(); ++i) + { + int id0 = mirror_joints0[i]; + int id1 = mirror_joints1[i]; + + int offset0 = cKinTree::GetParamOffset(joint_mat, id0); + int size0 = cKinTree::GetParamSize(joint_mat, id0); + int offset1 = cKinTree::GetParamOffset(joint_mat, id1); + int size1 = cKinTree::GetParamSize(joint_mat, id1); + + Eigen::VectorXd params0 = out_pose.segment(offset0, size0); + Eigen::VectorXd params1 = out_pose.segment(offset1, size1); + out_pose.segment(offset0, size0) = params1; + out_pose.segment(offset1, size1) = params0; + } +} + + +tMatrix cKinTree::ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tVector offset = GetRootPos(joint_mat, state); + tQuaternion rot = GetRootRot(joint_mat, state); + + tMatrix A = BuildAttachTrans(joint_mat, joint_id); + tMatrix R = cMathUtil::RotateMat(rot); + tMatrix T = cMathUtil::TranslateMat(offset); + + tMatrix mat = A * T * R; + return mat; +} + +tMatrix cKinTree::ChildParentTransRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id); + double theta = state(param_offset); + + tMatrix A = BuildAttachTrans(joint_mat, joint_id); + tMatrix R = cMathUtil::RotateMat(tVector(0, 0, 1, 0), theta); + + tMatrix mat = A * R; + return mat; +} + +tMatrix cKinTree::ChildParentTransPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id); + double theta = state(param_offset + 2); + tVector offset = tVector::Zero(); + offset[0] = state(param_offset); + offset[1] = state(param_offset + 1); + + tMatrix A = BuildAttachTrans(joint_mat, joint_id); + tMatrix R = cMathUtil::RotateMat(tVector(0, 0, 1, 0), theta); + tMatrix T = cMathUtil::TranslateMat(offset); + + tMatrix mat = A * R * T; + return mat; +} + +tMatrix cKinTree::ChildParentTransFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + tMatrix A = BuildAttachTrans(joint_mat, joint_id); + tMatrix mat = A; + return mat; +} + +tMatrix cKinTree::ChildParentTransPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id); + tVector offset = tVector::Zero(); + offset[0] = state(param_offset); + + tMatrix A = BuildAttachTrans(joint_mat, joint_id); + tMatrix T = cMathUtil::TranslateMat(offset); + + tMatrix mat = A * T; + return mat; +} + +tMatrix cKinTree::ChildParentTransSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) +{ + int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id); + int param_size = cKinTree::GetParamSize(joint_mat, joint_id); + tQuaternion q = cMathUtil::VecToQuat(state.segment(param_offset, param_size)); + + tMatrix A = BuildAttachTrans(joint_mat, joint_id); + tMatrix R = cMathUtil::RotateMat(q); + + tMatrix mat = A * R; + return mat; +} + + + +void cKinTree::BuildDefaultPoseRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose) +{ + int dim = gRootDim; + out_pose = Eigen::VectorXd::Zero(dim); + out_pose(gPosDim) = 1; +} + +void cKinTree::BuildDefaultPoseRevolute(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypeRevolute); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultPosePrismatic(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypePrismatic); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultPosePlanar(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypePlanar); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultPoseFixed(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypeFixed); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultPoseSpherical(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypeSpherical); + out_pose = Eigen::VectorXd::Zero(dim); + out_pose(0) = 1; +} + + +void cKinTree::BuildDefaultVelRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose) +{ + int dim = gRootDim; + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultVelRevolute(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypeRevolute); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultVelPrismatic(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypePrismatic); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultVelPlanar(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypePlanar); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultVelFixed(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypeFixed); + out_pose = Eigen::VectorXd::Zero(dim); +} + +void cKinTree::BuildDefaultVelSpherical(Eigen::VectorXd& out_pose) +{ + int dim = GetJointParamSize(eJointTypeSpherical); + out_pose = Eigen::VectorXd::Zero(dim); +} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h new file mode 100644 index 000000000..6c2fcc31e --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h @@ -0,0 +1,271 @@ +#pragma once + +#include +#include +#include "Shape.h" +#ifdef USE_JSON +#include "json/json.h" +#endif +#include "MathUtil.h" + +class cKinTree +{ +public: + // description of the joint tree representing an articulated figure + enum eJointType + { + eJointTypeRevolute, + eJointTypePlanar, + eJointTypePrismatic, + eJointTypeFixed, + eJointTypeSpherical, + eJointTypeNone, + eJointTypeMax + }; + + enum eJointDesc + { + eJointDescType, + eJointDescParent, + eJointDescAttachX, + eJointDescAttachY, + eJointDescAttachZ, + eJointDescAttachThetaX, // euler angles order rot(Z) * rot(Y) * rot(X) + eJointDescAttachThetaY, + eJointDescAttachThetaZ, + eJointDescLimLow0, + eJointDescLimLow1, + eJointDescLimLow2, + eJointDescLimHigh0, + eJointDescLimHigh1, + eJointDescLimHigh2, + eJointDescTorqueLim, + eJointDescForceLim, + eJointDescIsEndEffector, + eJointDescDiffWeight, + eJointDescParamOffset, + eJointDescMax + }; + typedef Eigen::Matrix tJointDesc; + + enum eBodyParam + { + eBodyParamShape, + eBodyParamMass, + eBodyParamColGroup, // 0 collides with nothing and 1 collides with everything + eBodyParamEnableFallContact, + eBodyParamAttachX, + eBodyParamAttachY, + eBodyParamAttachZ, + eBodyParamAttachThetaX, // Euler angles order XYZ + eBodyParamAttachThetaY, + eBodyParamAttachThetaZ, + eBodyParam0, + eBodyParam1, + eBodyParam2, + eBodyColorR, + eBodyColorG, + eBodyColorB, + eBodyColorA, + eBodyParamMax + }; + typedef Eigen::Matrix tBodyDef; + + enum eDrawShape + { + eDrawShapeShape, + eDrawShapeParentJoint, + eDrawShapeAttachX, + eDrawShapeAttachY, + eDrawShapeAttachZ, + eDrawShapeAttachThetaX, // Euler angles order XYZ + eDrawShapeAttachThetaY, + eDrawShapeAttachThetaZ, + eDrawShapeParam0, + eDrawShapeParam1, + eDrawShapeParam2, + eDrawShapeColorR, + eDrawShapeColorG, + eDrawShapeColorB, + eDrawShapeColorA, + eDrawShapeMeshID, + eDrawShapeParamMax + }; + typedef Eigen::Matrix tDrawShapeDef; + + static const int gInvalidJointID; + static const int gPosDim; + static const int gRotDim; + static const int gRootDim; + + static bool HasValidRoot(const Eigen::MatrixXd& joint_mat); + static tVector GetRootPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state); + static void SetRootPos(const Eigen::MatrixXd& joint_mat, const tVector& pos, Eigen::VectorXd& out_state); + static tQuaternion GetRootRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state); + static void SetRootRot(const Eigen::MatrixXd& joint_mat, const tQuaternion& rot, Eigen::VectorXd& out_state); + static tVector GetRootVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel); + static void SetRootVel(const Eigen::MatrixXd& joint_mat, const tVector& vel, Eigen::VectorXd& out_vel); + static tVector GetRootAngVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel); + static void SetRootAngVel(const Eigen::MatrixXd& joint_mat, const tVector& ang_vel, Eigen::VectorXd& out_vel); + + static tVector CalcJointWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tVector LocalToWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int parent_id, const tVector& attach_pt); + static tQuaternion CalcJointWorldRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static void CalcJointWorldTheta(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id, tVector& out_axis, double& out_theta); + + static tVector CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id); + static tVector CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt); + static tVector CalcJointWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id); + static tVector CalcWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt); + + static int GetNumDof(const Eigen::MatrixXd& joint_mat); + static void ApplyStep(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& step, Eigen::VectorXd& out_pose); + + static Eigen::VectorXi FindJointChain(const Eigen::MatrixXd& joint_mat, int joint_beg, int joint_end); + static bool IsAncestor(const Eigen::MatrixXd& joint_mat, int child_joint, int ancestor_joint, int& out_len); + static double CalcChainLength(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXi& chain); + + static void CalcAABB(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, tVector& out_min, tVector& out_max); + + static int GetParamOffset(const Eigen::MatrixXd& joint_mat, int joint_id); + static int GetParamSize(const Eigen::MatrixXd& joint_mat, int joint_id); + static int GetJointParamSize(eJointType joint_type); + static void GetJointParams(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int j, Eigen::VectorXd& out_params); + static void SetJointParams(const Eigen::MatrixXd& joint_mat, int j, const Eigen::VectorXd& params, Eigen::VectorXd& out_state); + static eJointType GetJointType(const Eigen::MatrixXd& joint_mat, int joint_id); + static int GetParent(const Eigen::MatrixXd& joint_mat, int joint_id); + static bool HasParent(const Eigen::MatrixXd& joint_mat, int joint_id); + static bool IsRoot(const Eigen::MatrixXd& joint_mat, int joint_id); + static bool IsJointActuated(const Eigen::MatrixXd& joint_mat, int joint_id); + static double GetTorqueLimit(const Eigen::MatrixXd& joint_mat, int joint_id); + static double GetForceLimit(const Eigen::MatrixXd& joint_mat, int joint_id); + static bool IsEndEffector(const Eigen::MatrixXd& joint_mat, int joint_id); + + static tVector GetJointLimLow(const Eigen::MatrixXd& joint_mat, int joint_id); + static tVector GetJointLimHigh(const Eigen::MatrixXd& joint_mat, int joint_id); + static double GetJointDiffWeight(const Eigen::MatrixXd& joint_mat, int joint_id); + + static double CalcLinkLength(const Eigen::MatrixXd& joint_mat, int joint_id); + static tVector GetAttachPt(const Eigen::MatrixXd& joint_mat, int joint_id); + static tVector GetAttachTheta(const Eigen::MatrixXd& joint_mat, int joint_id); + + // calculates the longest chain in the subtree of each joint + static void CalcMaxSubChainLengths(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_lengths); + static void CalcSubTreeMasses(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, Eigen::VectorXd& out_masses); + + static tMatrix BuildAttachTrans(const Eigen::MatrixXd& joint_mat, int joint_id); + static tMatrix ChildParentTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix ParentChildTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix JointWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix WorldJointTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + +#ifdef USE_JSON + static bool Load(const Json::Value& root, Eigen::MatrixXd& out_joint_mat); + static bool ParseBodyDef(const Json::Value& root, tBodyDef& out_def); + static bool ParseDrawShapeDef(const Json::Value& root, tDrawShapeDef& out_def); + static std::string BuildJointMatJson(const Eigen::MatrixXd& joint_mat); + static std::string BuildJointJson(int id, const tJointDesc& joint_desc); + static bool ParseJoint(const Json::Value& root, tJointDesc& out_joint_desc); +#endif + + static int GetNumJoints(const Eigen::MatrixXd& joint_mat); + static int GetRoot(const Eigen::MatrixXd& joint_mat); + static void FindChildren(const Eigen::MatrixXd& joint_mat, int joint_id, Eigen::VectorXi& out_children); + + static bool LoadBodyDefs(const std::string& char_file, Eigen::MatrixXd& out_body_defs); + + static bool LoadDrawShapeDefs(const std::string& char_file, Eigen::MatrixXd& out_draw_defs); + + static cShape::eShape GetBodyShape(const Eigen::MatrixXd& body_defs, int part_id); + static tVector GetBodyAttachPt(const Eigen::MatrixXd& body_defs, int part_id); + static tVector GetBodyAttachTheta(const Eigen::MatrixXd& body_defs, int part_id); + static void GetBodyRotation(const Eigen::MatrixXd& body_defs, int part_id, tVector& out_axis, double& out_theta); + static double GetBodyMass(const Eigen::MatrixXd& body_defs, int part_id); + static int GetBodyColGroup(const Eigen::MatrixXd& body_defs, int part_id); + static bool GetBodyEnableFallContact(const Eigen::MatrixXd& body_defs, int part_id); + static void SetBodyEnableFallContact(int part_id, bool enable, Eigen::MatrixXd& out_body_defs); + static tVector GetBodySize(const Eigen::MatrixXd& body_defs, int part_id); + static tVector GetBodyColor(const Eigen::MatrixXd& body_defs, int part_id); + static double CalcTotalMass(const Eigen::MatrixXd& body_defs); + static bool IsValidBody(const Eigen::MatrixXd& body_defs, int part_id); + static tVector GetBodyLocalCoM(const Eigen::MatrixXd& body_defs, int part_id); + + static int GetDrawShapeParentJoint(const tDrawShapeDef& shape); + static tVector GetDrawShapeAttachPt(const tDrawShapeDef& shape); + static tVector GetDrawShapeAttachTheta(const tDrawShapeDef& shape); + static void GetDrawShapeRotation(const tDrawShapeDef& shape, tVector& out_axis, double& out_theta); + static tVector GetDrawShapeColor(const tDrawShapeDef& shape); + static int GetDrawShapeMeshID(const tDrawShapeDef& shape); + + static tVector CalcBodyPartPos(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id); + static tVector CalcBodyPartVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int part_id); + static void CalcBodyPartRotation(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id, tVector& out_axis, double& out_theta); + static tMatrix BodyWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id); + static tMatrix BodyJointTrans(const Eigen::MatrixXd& body_defs, int part_id); + + static tJointDesc BuildJointDesc(eJointType joint_type, int parent_id, const tVector& attach_pt); + static tJointDesc BuildJointDesc(); + static tBodyDef BuildBodyDef(); + static tDrawShapeDef BuildDrawShapeDef(); + + static void BuildDefaultPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose); + static void BuildDefaultVel(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_vel); + + static void CalcPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff); + static tVector CalcRootPosDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1); + static tQuaternion CalcRootRotDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1); + static double CalcPoseErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1); + static double CalcRootPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1); + static double CalcRootRotErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1); + + static void CalcVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff); + static tVector CalcRootVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1); + static tVector CalcRootAngVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1); + static double CalcVelErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1); + static double CalcRootVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1); + static double CalcRootAngVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1); + + static void CalcVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double dt, Eigen::VectorXd& out_vel); + + static void PostProcessPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose); + static void LerpPoses(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double lerp, Eigen::VectorXd& out_pose); + static void VelToPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, Eigen::VectorXd& out_pose_diff); + + static double CalcHeading(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose); + static tQuaternion CalcHeadingRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose); + static tMatrix BuildHeadingTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose); + static tMatrix BuildOriginTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose); + static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose); + static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel); + + static void MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector mirror_joints0, const std::vector mirror_joints1, Eigen::VectorXd& out_pose); + + +protected: + static bool ParseJointType(const std::string& type_str, eJointType& out_joint_type); + static void PostProcessJointMat(Eigen::MatrixXd& out_joint_mat); + + static tMatrix ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix ChildParentTransRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix ChildParentTransPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix ChildParentTransFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix ChildParentTransPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + static tMatrix ChildParentTransSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id); + + static void BuildDefaultPoseRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose); + static void BuildDefaultPoseRevolute(Eigen::VectorXd& out_pose); + static void BuildDefaultPosePrismatic(Eigen::VectorXd& out_pose); + static void BuildDefaultPosePlanar(Eigen::VectorXd& out_pose); + static void BuildDefaultPoseFixed(Eigen::VectorXd& out_pose); + static void BuildDefaultPoseSpherical(Eigen::VectorXd& out_pose); + + static void BuildDefaultVelRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose); + static void BuildDefaultVelRevolute(Eigen::VectorXd& out_pose); + static void BuildDefaultVelPrismatic(Eigen::VectorXd& out_pose); + static void BuildDefaultVelPlanar(Eigen::VectorXd& out_pose); + static void BuildDefaultVelFixed(Eigen::VectorXd& out_pose); + static void BuildDefaultVelSpherical(Eigen::VectorXd& out_pose); + + static void CalcJointPoseDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff); + static void CalcJointVelDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff); +}; diff --git a/examples/SharedMemory/plugins/stablePDPlugin/LICENSE.txt b/examples/SharedMemory/plugins/stablePDPlugin/LICENSE.txt new file mode 100644 index 000000000..3416f5117 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/LICENSE.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 Xue Bin Peng + +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. diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp new file mode 100644 index 000000000..dcdc07b60 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp @@ -0,0 +1,904 @@ +#include "MathUtil.h" +#include +#define _USE_MATH_DEFINES +#include + +cRand cMathUtil::gRand = cRand(); + +int cMathUtil::Clamp(int val, int min, int max) +{ + return std::max(min, std::min(val, max)); +} + +void cMathUtil::Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec) +{ + out_vec = out_vec.cwiseMin(max).cwiseMax(min); +} + +double cMathUtil::Clamp(double val, double min, double max) +{ + return std::max(min, std::min(val, max)); +} + +double cMathUtil::Saturate(double val) +{ + return Clamp(val, 0.0, 1.0); +} + +double cMathUtil::Lerp(double t, double val0, double val1) +{ + return (1 - t) * val0 + t * val1; +} + +double cMathUtil::NormalizeAngle(double theta) +{ + // normalizes theta to be between [-pi, pi] + double norm_theta = fmod(theta, 2 * M_PI); + if (norm_theta > M_PI) + { + norm_theta = -2 * M_PI + norm_theta; + } + else if (norm_theta < -M_PI) + { + norm_theta = 2 * M_PI + norm_theta; + } + return norm_theta; +} + +double cMathUtil::RandDouble() +{ + return RandDouble(0, 1); +} + +double cMathUtil::RandDouble(double min, double max) +{ + return gRand.RandDouble(min, max); +} + +double cMathUtil::RandDoubleNorm(double mean, double stdev) +{ + return gRand.RandDoubleNorm(mean, stdev); +} + +double cMathUtil::RandDoubleExp(double lambda) +{ + return gRand.RandDoubleExp(lambda); +} + +double cMathUtil::RandDoubleSeed(double seed) +{ + unsigned int int_seed = *reinterpret_cast(&seed); + std::default_random_engine rand_gen(int_seed); + std::uniform_real_distribution dist; + return dist(rand_gen); +} + +int cMathUtil::RandInt() +{ + return gRand.RandInt(); +} + +int cMathUtil::RandInt(int min, int max) +{ + return gRand.RandInt(min, max); +} + +int cMathUtil::RandUint() +{ + return gRand.RandUint(); +} + +int cMathUtil::RandUint(unsigned int min, unsigned int max) +{ + return gRand.RandUint(min, max); +} + +int cMathUtil::RandIntExclude(int min, int max, int exc) +{ + return gRand.RandIntExclude(min, max, exc); +} + +void cMathUtil::SeedRand(unsigned long int seed) +{ + gRand.Seed(seed); + srand(gRand.RandInt()); +} + +int cMathUtil::RandSign() +{ + return gRand.RandSign(); +} + +double cMathUtil::SmoothStep(double t) +{ + double val = t * t * t * (t * (t * 6 - 15) + 10); + return val; +} + +bool cMathUtil::FlipCoin(double p) +{ + return gRand.FlipCoin(p); +} + +tMatrix cMathUtil::TranslateMat(const tVector& trans) +{ + tMatrix mat = tMatrix::Identity(); + mat(0, 3) = trans[0]; + mat(1, 3) = trans[1]; + mat(2, 3) = trans[2]; + return mat; +} + +tMatrix cMathUtil::ScaleMat(double scale) +{ + return ScaleMat(tVector::Ones() * scale); +} + +tMatrix cMathUtil::ScaleMat(const tVector& scale) +{ + tMatrix mat = tMatrix::Identity(); + mat(0, 0) = scale[0]; + mat(1, 1) = scale[1]; + mat(2, 2) = scale[2]; + return mat; +} + +tMatrix cMathUtil::RotateMat(const tVector& euler) +{ + double x = euler[0]; + double y = euler[1]; + double z = euler[2]; + + double x_s = std::sin(x); + double x_c = std::cos(x); + double y_s = std::sin(y); + double y_c = std::cos(y); + double z_s = std::sin(z); + double z_c = std::cos(z); + + tMatrix mat = tMatrix::Identity(); + mat(0, 0) = y_c * z_c; + mat(1, 0) = y_c * z_s; + mat(2, 0) = -y_s; + + mat(0, 1) = x_s * y_s * z_c - x_c * z_s; + mat(1, 1) = x_s * y_s * z_s + x_c * z_c; + mat(2, 1) = x_s * y_c; + + mat(0, 2) = x_c * y_s * z_c + x_s * z_s; + mat(1, 2) = x_c * y_s * z_s - x_s * z_c; + mat(2, 2) = x_c * y_c; + + return mat; +} + +tMatrix cMathUtil::RotateMat(const tVector& axis, double theta) +{ + assert(std::abs(axis.squaredNorm() - 1) < 0.0001); + + double c = std::cos(theta); + double s = std::sin(theta); + double x = axis[0]; + double y = axis[1]; + double z = axis[2]; + + tMatrix mat; + mat << c + x * x * (1 - c), x * y * (1 - c) - z * s, x * z * (1 - c) + y * s, 0, + y * x * (1 - c) + z * s, c + y * y * (1 - c), y * z * (1 - c) - x * s, 0, + z * x * (1 - c) - y * s, z * y * (1 - c) + x * s, c + z * z * (1 - c), 0, + 0, 0, 0, 1; + + return mat; +} + +tMatrix cMathUtil::RotateMat(const tQuaternion& q) +{ + tMatrix mat = tMatrix::Identity(); + + double sqw = q.w() * q.w(); + double sqx = q.x()* q.x(); + double sqy = q.y() * q.y(); + double sqz = q.z() * q.z(); + double invs = 1 / (sqx + sqy + sqz + sqw); + + mat(0, 0) = (sqx - sqy - sqz + sqw) * invs; + mat(1, 1) = (-sqx + sqy - sqz + sqw) * invs; + mat(2, 2) = (-sqx - sqy + sqz + sqw) * invs; + + double tmp1 = q.x()*q.y(); + double tmp2 = q.z()*q.w(); + mat(1, 0) = 2.0 * (tmp1 + tmp2) * invs; + mat(0, 1) = 2.0 * (tmp1 - tmp2) * invs; + + tmp1 = q.x()*q.z(); + tmp2 = q.y()*q.w(); + mat(2, 0) = 2.0 * (tmp1 - tmp2) * invs; + mat(0, 2) = 2.0 * (tmp1 + tmp2) * invs; + + tmp1 = q.y()*q.z(); + tmp2 = q.x()*q.w(); + mat(2, 1) = 2.0 * (tmp1 + tmp2) * invs; + mat(1, 2) = 2.0 * (tmp1 - tmp2) * invs; + + return mat; +} + +tMatrix cMathUtil::CrossMat(const tVector& a) +{ + tMatrix m; + m << 0, -a[2], a[1], 0, + a[2], 0, -a[0], 0, + -a[1], a[0], 0, 0, + 0, 0, 0, 1; + return m; +} + +tMatrix cMathUtil::InvRigidMat(const tMatrix& mat) +{ + tMatrix inv_mat = tMatrix::Zero(); + inv_mat.block(0, 0, 3, 3) = mat.block(0, 0, 3, 3).transpose(); + inv_mat.col(3) = -inv_mat * mat.col(3); + inv_mat(3, 3) = 1; + return inv_mat; +} + +tVector cMathUtil::GetRigidTrans(const tMatrix& mat) +{ + return tVector(mat(0, 3), mat(1, 3), mat(2, 3), 0); +} + +tVector cMathUtil::InvEuler(const tVector& euler) +{ + tMatrix inv_mat = cMathUtil::RotateMat(tVector(1, 0, 0, 0), -euler[0]) + * cMathUtil::RotateMat(tVector(0, 1, 0, 0), -euler[1]) + * cMathUtil::RotateMat(tVector(0, 0, 1, 0), -euler[2]); + tVector inv_euler = cMathUtil::RotMatToEuler(inv_mat); + return inv_euler; +} + +void cMathUtil::RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta) +{ + double c = (mat(0, 0) + mat(1, 1) + mat(2, 2) - 1) * 0.5; + c = cMathUtil::Clamp(c, -1.0, 1.0); + + out_theta = std::acos(c); + if (std::abs(out_theta) < 0.00001) + { + out_axis = tVector(0, 0, 1, 0); + } + else + { + double m21 = mat(2, 1) - mat(1, 2); + double m02 = mat(0, 2) - mat(2, 0); + double m10 = mat(1, 0) - mat(0, 1); + double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10); + out_axis[0] = m21 / denom; + out_axis[1] = m02 / denom; + out_axis[2] = m10 / denom; + out_axis[3] = 0; + } +} + +tVector cMathUtil::RotMatToEuler(const tMatrix& mat) +{ + tVector euler; + euler[0] = std::atan2(mat(2, 1), mat(2, 2)); + euler[1] = std::atan2(-mat(2, 0), std::sqrt(mat(2, 1) * mat(2, 1) + mat(2, 2) * mat(2, 2))); + euler[2] = std::atan2(mat(1, 0), mat(0, 0)); + euler[3] = 0; + return euler; +} + +tQuaternion cMathUtil::RotMatToQuaternion(const tMatrix& mat) +{ + double tr = mat(0, 0) + mat(1, 1) + mat(2, 2); + tQuaternion q; + + if (tr > 0) { + double S = sqrt(tr + 1.0) * 2; // S=4*qw + q.w() = 0.25 * S; + q.x() = (mat(2, 1) - mat(1, 2)) / S; + q.y() = (mat(0, 2) - mat(2, 0)) / S; + q.z() = (mat(1, 0) - mat(0, 1)) / S; + } + else if ((mat(0, 0) > mat(1, 1) && (mat(0, 0) > mat(2, 2)))) { + double S = sqrt(1.0 + mat(0, 0) - mat(1, 1) - mat(2, 2)) * 2; // S=4*qx + q.w() = (mat(2, 1) - mat(1, 2)) / S; + q.x() = 0.25 * S; + q.y() = (mat(0, 1) + mat(1, 0)) / S; + q.z() = (mat(0, 2) + mat(2, 0)) / S; + } + else if (mat(1, 1) > mat(2, 2)) { + double S = sqrt(1.0 + mat(1, 1) - mat(0, 0) - mat(2, 2)) * 2; // S=4*qy + q.w() = (mat(0, 2) - mat(2, 0)) / S; + q.x() = (mat(0, 1) + mat(1, 0)) / S; + q.y() = 0.25 * S; + q.z() = (mat(1, 2) + mat(2, 1)) / S; + } + else { + double S = sqrt(1.0 + mat(2, 2) - mat(0, 0) - mat(1, 1)) * 2; // S=4*qz + q.w() = (mat(1, 0) - mat(0, 1)) / S; + q.x() = (mat(0, 2) + mat(2, 0)) / S; + q.y() = (mat(1, 2) + mat(2, 1)) / S; + q.z() = 0.25 * S; + } + + return q; +} + +void cMathUtil::EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta) +{ + double x = euler[0]; + double y = euler[1]; + double z = euler[2]; + + double x_s = std::sin(x); + double x_c = std::cos(x); + double y_s = std::sin(y); + double y_c = std::cos(y); + double z_s = std::sin(z); + double z_c = std::cos(z); + + double c = (y_c * z_c + x_s * y_s * z_s + x_c * z_c + x_c * y_c - 1) * 0.5; + c = Clamp(c, -1.0, 1.0); + + out_theta = std::acos(c); + if (std::abs(out_theta) < 0.00001) + { + out_axis = tVector(0, 0, 1, 0); + } + else + { + double m21 = x_s * y_c - x_c * y_s * z_s + x_s * z_c; + double m02 = x_c * y_s * z_c + x_s * z_s + y_s; + double m10 = y_c * z_s - x_s * y_s * z_c + x_c * z_s; + double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10); + out_axis[0] = m21 / denom; + out_axis[1] = m02 / denom; + out_axis[2] = m10 / denom; + out_axis[3] = 0; + } +} + +tVector cMathUtil::AxisAngleToEuler(const tVector& axis, double theta) +{ + tQuaternion q = AxisAngleToQuaternion(axis, theta); + return QuaternionToEuler(q); +} + +tMatrix cMathUtil::DirToRotMat(const tVector& dir, const tVector& up) +{ + tVector x = up.cross3(dir); + double x_norm = x.norm(); + if (x_norm == 0) + { + x_norm = 1; + x = (dir.dot(up) >= 0) ? tVector(1, 0, 0, 0) : tVector(-1, 0, 0, 0); + } + x /= x_norm; + + tVector y = dir.cross3(x).normalized(); + tVector z = dir; + + tMatrix mat = tMatrix::Identity(); + mat.block(0, 0, 3, 1) = x.segment(0, 3); + mat.block(0, 1, 3, 1) = y.segment(0, 3); + mat.block(0, 2, 3, 1) = z.segment(0, 3); + + return mat; +} + +void cMathUtil::DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1, + tVector& out_axis, double& out_theta) +{ + tMatrix R0 = RotateMat(axis0, theta0); + tMatrix R1 = RotateMat(axis1, theta1); + tMatrix M = DeltaRot(R0, R1); + RotMatToAxisAngle(M, out_axis, out_theta); +} + +tMatrix cMathUtil::DeltaRot(const tMatrix& R0, const tMatrix& R1) +{ + return R1 * R0.transpose(); +} + +tQuaternion cMathUtil::EulerToQuaternion(const tVector& euler) +{ + tVector axis; + double theta; + EulerToAxisAngle(euler, axis, theta); + return AxisAngleToQuaternion(axis, theta); +} + +tVector cMathUtil::QuaternionToEuler(const tQuaternion& q) +{ + double sinr = 2.0 * (q.w() * q.x() + q.y() * q.z()); + double cosr = 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y()); + double x = std::atan2(sinr, cosr); + + double sinp = 2.0 * (q.w() * q.y() - q.z() * q.x()); + double y = 0; + if (fabs(sinp) >= 1) + { + y = copysign(M_PI / 2, sinp); // use 90 degrees if out of range + } + else + { + y = asin(sinp); + } + + double siny = 2.0 * (q.w() * q.z() + q.x() * q.y()); + double cosy = 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()); + double z = std::atan2(siny, cosy); + + return tVector(x, y, z, 0); +} + +tQuaternion cMathUtil::AxisAngleToQuaternion(const tVector& axis, double theta) +{ + // axis must be normalized + double c = std::cos(theta / 2); + double s = std::sin(theta / 2); + tQuaternion q; + q.w() = c; + q.x() = s * axis[0]; + q.y() = s * axis[1]; + q.z() = s * axis[2]; + return q; +} + +void cMathUtil::QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta) +{ + out_theta = 0; + out_axis = tVector(0, 0, 1, 0); + + tQuaternion q1 = q; + if (q1.w() > 1) + { + q1.normalize(); + } + + double sin_theta = std::sqrt(1 - q1.w() * q1.w()); + if (sin_theta > 0.000001) + { + out_theta = 2 * std::acos(q1.w()); + out_theta = cMathUtil::NormalizeAngle(out_theta); + out_axis = tVector(q1.x(), q1.y(), q1.z(), 0) / sin_theta; + } +} + +tMatrix cMathUtil::BuildQuaternionDiffMat(const tQuaternion& q) +{ + tMatrix mat; + mat << -0.5 * q.x(), -0.5 * q.y(), -0.5 * q.z(), 0, + 0.5 * q.w(), -0.5 * q.z(), 0.5 * q.y(), 0, + 0.5 * q.z(), 0.5 * q.w(), -0.5 * q.x(), 0, + -0.5 * q.y(), 0.5 * q.x(), 0.5 * q.w(), 0; + return mat; +} + +tVector cMathUtil::CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt) +{ + tQuaternion q_diff = cMathUtil::QuatDiff(q0, q1); + tVector axis; + double theta; + QuaternionToAxisAngle(q_diff, axis, theta); + return (theta / dt) * axis; +} + +tVector cMathUtil::CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt) +{ + // calculate relative rotational velocity in the coordinate frame of q0 + tQuaternion q_diff = q0.conjugate() * q1; + tVector axis; + double theta; + QuaternionToAxisAngle(q_diff, axis, theta); + return (theta / dt) * axis; +} + +tQuaternion cMathUtil::VecToQuat(const tVector& v) +{ + return tQuaternion(v[0], v[1], v[2], v[3]); +} + +tVector cMathUtil::QuatToVec(const tQuaternion& q) +{ + return tVector(q.w(), q.x(), q.y(), q.z()); +} + +tQuaternion cMathUtil::QuatDiff(const tQuaternion& q0, const tQuaternion& q1) +{ + return q1 * q0.conjugate(); +} + +double cMathUtil::QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1) +{ + tQuaternion dq = QuatDiff(q0, q1); + return QuatTheta(dq); +} + +double cMathUtil::QuatTheta(const tQuaternion& dq) +{ + double theta = 0; + tQuaternion q1 = dq; + if (q1.w() > 1) + { + q1.normalize(); + } + + double sin_theta = std::sqrt(1 - q1.w() * q1.w()); + if (sin_theta > 0.0001) + { + theta = 2 * std::acos(q1.w()); + theta = cMathUtil::NormalizeAngle(theta); + } + return theta; +} + +tQuaternion cMathUtil::VecDiffQuat(const tVector& v0, const tVector& v1) +{ + return tQuaternion::FromTwoVectors(v0.segment(0, 3), v1.segment(0, 3)); +} + +tVector cMathUtil::QuatRotVec(const tQuaternion& q, const tVector& dir) +{ + tVector rot_dir = tVector::Zero(); + rot_dir.segment(0, 3) = q * dir.segment(0, 3); + return rot_dir; +} + +tQuaternion cMathUtil::MirrorQuaternion(const tQuaternion& q, eAxis axis) +{ + tQuaternion mirror_q; + mirror_q.w() = q.w(); + mirror_q.x() = (axis == eAxisX) ? q.x() : -q.x(); + mirror_q.y() = (axis == eAxisY) ? q.y() : -q.y(); + mirror_q.z() = (axis == eAxisZ) ? q.z() : -q.z(); + return mirror_q; +} + +double cMathUtil::Sign(double val) +{ + return SignAux(val); +} + +int cMathUtil::Sign(int val) +{ + return SignAux(val); +} + +double cMathUtil::AddAverage(double avg0, int count0, double avg1, int count1) +{ + double total = count0 + count1; + return (count0 / total) * avg0 + (count1 / total) * avg1; +} + +tVector cMathUtil::AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1) +{ + double total = count0 + count1; + return (count0 / total) * avg0 + (count1 / total) * avg1 ; +} + +void cMathUtil::AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result) +{ + double total = count0 + count1; + out_result = (count0 / total) * avg0 + (count1 / total) * avg1; +} + +void cMathUtil::CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob) +{ + assert(out_prob.size() == vals.size()); + int num_vals = static_cast(vals.size()); + double sum = 0; + double max_val = vals.maxCoeff(); + for (int i = 0; i < num_vals; ++i) + { + double val = vals[i]; + val = std::exp((val - max_val) / temp); + out_prob[i] = val; + sum += val; + } + + out_prob /= sum; +} + +double cMathUtil::EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample) +{ + assert(mean.size() == covar.size()); + assert(sample.size() == covar.size()); + + Eigen::VectorXd diff = sample - mean; + double exp_val = diff.dot(diff.cwiseQuotient(covar)); + double likelihood = std::exp(-0.5 * exp_val); + + double partition = CalcGaussianPartition(covar); + likelihood /= partition; + return likelihood; +} + +double cMathUtil::EvalGaussian(double mean, double covar, double sample) +{ + double diff = sample - mean; + double exp_val = diff * diff / covar; + double norm = 1 / std::sqrt(2 * M_PI * covar); + double likelihood = norm * std::exp(-0.5 * exp_val); + return likelihood; +} + +double cMathUtil::CalcGaussianPartition(const Eigen::VectorXd& covar) +{ + int data_size = static_cast(covar.size()); + double det = covar.prod(); + double partition = std::sqrt(std::pow(2 * M_PI, data_size) * det); + return partition; +} + +double cMathUtil::EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample) +{ + int data_size = static_cast(covar.size()); + + Eigen::VectorXd diff = sample - mean; + double logp = -0.5 * diff.dot(diff.cwiseQuotient(covar)); + double det = covar.prod(); + logp += -0.5 * (data_size * std::log(2 * M_PI) + std::log(det)); + + return logp; +} + +double cMathUtil::EvalGaussianLogp(double mean, double covar, double sample) +{ + double diff = sample - mean; + double logp = -0.5 * diff * diff / covar; + logp += -0.5 * (std::log(2 * M_PI) + std::log(covar)); + return logp; +} + +double cMathUtil::Sigmoid(double x) +{ + return Sigmoid(x, 1, 0); +} + +double cMathUtil::Sigmoid(double x, double gamma, double bias) +{ + double exp = -gamma * (x + bias); + double val = 1 / (1 + std::exp(exp)); + return val; +} + +int cMathUtil::SampleDiscreteProb(const Eigen::VectorXd& probs) +{ + assert(std::abs(probs.sum() - 1) < 0.00001); + double rand = RandDouble(); + + int rand_idx = gInvalidIdx; + int num_probs = static_cast(probs.size()); + for (int i = 0; i < num_probs; ++i) + { + double curr_prob = probs[i]; + rand -= curr_prob; + + if (rand <= 0) + { + rand_idx = i; + break; + } + } + return rand_idx; +} + +tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c) +{ + tVector v0 = b - a; + tVector v1 = c - a; + tVector v2 = p - a; + + double d00 = v0.dot(v0); + double d01 = v0.dot(v1); + double d11 = v1.dot(v1); + double d20 = v2.dot(v0); + double d21 = v2.dot(v1); + double denom = d00 * d11 - d01 * d01; + double v = (d11 * d20 - d01 * d21) / denom; + double w = (d00 * d21 - d01 * d20) / denom; + double u = 1.0f - v - w; + + return tVector(u, v, w, 0); +} + +bool cMathUtil::ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max) +{ + bool contains = pt[0] >= aabb_min[0] && pt[1] >= aabb_min[1] && pt[2] >= aabb_min[2] + && pt[0] <= aabb_max[0] && pt[1] <= aabb_max[1] && pt[2] <= aabb_max[2]; + return contains; +} + +bool cMathUtil::ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1) +{ + return ContainsAABB(aabb_min0, aabb_min1, aabb_max1) && ContainsAABB(aabb_max0, aabb_min1, aabb_max1); +} + +bool cMathUtil::ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max) +{ + bool contains = pt[0] >= aabb_min[0] && pt[2] >= aabb_min[2] + && pt[0] <= aabb_max[0] && pt[2] <= aabb_max[2]; + return contains; +} + +bool cMathUtil::ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1) +{ + return ContainsAABBXZ(aabb_min0, aabb_min1, aabb_max1) && ContainsAABBXZ(aabb_max0, aabb_min1, aabb_max1); +} + +void cMathUtil::CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1, + tVector& out_min, tVector& out_max) +{ + out_min = aabb_min0.cwiseMax(aabb_min1); + out_max = aabb_max0.cwiseMin(aabb_max1); + if (out_min[0] > out_max[0]) + { + out_min[0] = 0; + out_max[0] = 0; + } + if (out_min[1] > out_max[1]) + { + out_min[1] = 0; + out_max[1] = 0; + } + if (out_min[2] > out_max[2]) + { + out_min[2] = 0; + out_max[2] = 0; + } +} + +void cMathUtil::CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1, + tVector& out_min, tVector& out_max) +{ + out_min = aabb_min0.cwiseMin(aabb_min1); + out_max = aabb_max0.cwiseMax(aabb_max1); +} + +bool cMathUtil::IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1) +{ + tVector center0 = 0.5 * (aabb_max0 + aabb_min0); + tVector center1 = 0.5 * (aabb_max1 + aabb_min1); + tVector size0 = aabb_max0 - aabb_min0; + tVector size1 = aabb_max1 - aabb_min1; + tVector test_len = 0.5 * (size0 + size1); + tVector delta = center1 - center0; + bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[1]) <= test_len[1]) && (std::abs(delta[2]) <= test_len[2]); + return overlap; +} + +bool cMathUtil::IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1) +{ + tVector center0 = 0.5 * (aabb_max0 + aabb_min0); + tVector center1 = 0.5 * (aabb_max1 + aabb_min1); + tVector size0 = aabb_max0 - aabb_min0; + tVector size1 = aabb_max1 - aabb_min1; + tVector test_len = 0.5 * (size0 + size1); + tVector delta = center1 - center0; + bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[2]) <= test_len[2]); + return overlap; +} + +bool cMathUtil::CheckNextInterval(double delta, double curr_val, double int_size) +{ + double pad = 0.001 * delta; + int curr_count = static_cast(std::floor((curr_val + pad) / int_size)); + int prev_count = static_cast(std::floor((curr_val + pad - delta) / int_size)); + bool new_action = (curr_count != prev_count); + return new_action; +} + +tVector cMathUtil::SampleRandPt(const tVector& bound_min, const tVector& bound_max) +{ + tVector pt = tVector(RandDouble(bound_min[0], bound_max[0]), + RandDouble(bound_min[1], bound_max[1]), + RandDouble(bound_min[2], bound_max[2]), 0); + return pt; +} + +tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max) +{ + return SampleRandPtBias(bound_min, bound_max, 0.5 * (bound_max + bound_min)); +} + +tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus) +{ + double t = RandDouble(0, 1); + tVector size = bound_max - bound_min; + tVector new_min = focus + (t * 0.5) * size; + tVector new_max = focus - (t * 0.5) * size; + tVector offset = (bound_min - new_min).cwiseMax(0); + offset += (bound_max - new_max).cwiseMin(0); + new_min += offset; + new_max += offset; + + return SampleRandPt(new_min, new_max); +} + +void cMathUtil::QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist) +{ + assert(std::abs(dir.norm() - 1) < 0.000001); + assert(std::abs(q.norm() - 1) < 0.000001); + + tVector q_axis = tVector(q.x(), q.y(), q.z(), 0); + double p = q_axis.dot(dir); + tVector twist_axis = p * dir; + out_twist = tQuaternion(q.w(), twist_axis[0], twist_axis[1], twist_axis[2]); + out_twist.normalize(); + out_swing = q * out_twist.conjugate(); +} + +tQuaternion cMathUtil::ProjectQuat(const tQuaternion& q, const tVector& dir) +{ + assert(std::abs(dir.norm() - 1) < 0.00001); + tVector ref_axis = tVector::Zero(); + int min_idx = 0; + dir.cwiseAbs().minCoeff(&min_idx); + ref_axis[min_idx] = 1; + + tVector rot_dir0 = dir.cross3(ref_axis); + tVector rot_dir1 = cMathUtil::QuatRotVec(q, rot_dir0); + rot_dir1 -= rot_dir1.dot(dir) * dir; + + double dir1_norm = rot_dir1.norm(); + tQuaternion p_rot = tQuaternion::Identity(); + if (dir1_norm > 0.0001) + { + rot_dir1 /= dir1_norm; + p_rot = cMathUtil::VecDiffQuat(rot_dir0, rot_dir1); + } + return p_rot; +} + +void cMathUtil::ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x) +{ + double sampling_rate = 1 / dt; + int n = static_cast(out_x.size()); + + double wc = std::tan(cutoff * M_PI / sampling_rate); + double k1 = std::sqrt(2) * wc; + double k2 = wc * wc; + double a = k2 / (1 + k1 + k2); + double b = 2 * a; + double c = a; + double k3 = b / k2; + double d = -2 * a + k3; + double e = 1 - (2 * a) - k3; + + double xm2 = out_x[0]; + double xm1 = out_x[0]; + double ym2 = out_x[0]; + double ym1 = out_x[0]; + + for (int s = 0; s < n; ++s) + { + double x = out_x[s]; + double y = a * x + b * xm1 + c * xm2 + d * ym1 + e * ym2; + + out_x[s] = y; + xm2 = xm1; + xm1 = x; + ym2 = ym1; + ym1 = y; + } + + double yp2 = out_x[n - 1]; + double yp1 = out_x[n - 1]; + double zp2 = out_x[n - 1]; + double zp1 = out_x[n - 1]; + + for (int t = n - 1; t >= 0; --t) + { + double y = out_x[t]; + double z = a * y + b * yp1 + c * yp2 + d * zp1 + e * zp2; + + out_x[t] = z; + yp2 = yp1; + yp1 = y; + zp2 = zp1; + zp1 = z; + } +} \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h new file mode 100644 index 000000000..4e7413ed5 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h @@ -0,0 +1,155 @@ +#pragma once + +#include + +#include "Eigen/Dense" +#include "Eigen/StdVector" +#include "Eigen/Geometry" +#include "Rand.h" +#define _USE_MATH_DEFINES +#include "math.h" + +const int gInvalidIdx = -1; + +// for convenience define standard vector for rendering +typedef Eigen::Vector4d tVector; +typedef Eigen::Vector4d tVector3; +typedef Eigen::Matrix4d tMatrix; +typedef Eigen::Matrix3d tMatrix3; +typedef Eigen::Quaterniond tQuaternion; + +template +using tEigenArr = std::vector >; +typedef tEigenArr tVectorArr; + +const double gRadiansToDegrees = 57.2957795; +const double gDegreesToRadians = 1.0 / gRadiansToDegrees; +const tVector gGravity = tVector(0, -9.8, 0, 0); +const double gInchesToMeters = 0.0254; +const double gFeetToMeters = 0.3048; + +class cMathUtil +{ +public: + enum eAxis + { + eAxisX, + eAxisY, + eAxisZ, + eAxisMax + }; + + static int Clamp(int val, int min, int max); + static void Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec); + static double Clamp(double val, double min, double max); + static double Saturate(double val); + static double Lerp(double t, double val0, double val1); + + static double NormalizeAngle(double theta); + + // rand number + static double RandDouble(); + static double RandDouble(double min, double max); + static double RandDoubleNorm(double mean, double stdev); + static double RandDoubleExp(double lambda); + static double RandDoubleSeed(double seed); + static int RandInt(); + static int RandInt(int min, int max); + static int RandUint(); + static int RandUint(unsigned int min, unsigned int max); + static int RandIntExclude(int min, int max, int exc); + static void SeedRand(unsigned long int seed); + static int RandSign(); + static bool FlipCoin(double p = 0.5); + static double SmoothStep(double t); + + // matrices + static tMatrix TranslateMat(const tVector& trans); + static tMatrix ScaleMat(double scale); + static tMatrix ScaleMat(const tVector& scale); + static tMatrix RotateMat(const tVector& euler); // euler angles order rot(Z) * rot(Y) * rot(X) + static tMatrix RotateMat(const tVector& axis, double theta); + static tMatrix RotateMat(const tQuaternion& q); + static tMatrix CrossMat(const tVector& a); + // inverts a transformation consisting only of rotations and translations + static tMatrix InvRigidMat(const tMatrix& mat); + static tVector GetRigidTrans(const tMatrix& mat); + static tVector InvEuler(const tVector& euler); + static void RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta); + static tVector RotMatToEuler(const tMatrix& mat); + static tQuaternion RotMatToQuaternion(const tMatrix& mat); + static void EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta); + static tVector AxisAngleToEuler(const tVector& axis, double theta); + static tMatrix DirToRotMat(const tVector& dir, const tVector& up); + + static void DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1, + tVector& out_axis, double& out_theta); + static tMatrix DeltaRot(const tMatrix& R0, const tMatrix& R1); + + static tQuaternion EulerToQuaternion(const tVector& euler); + static tVector QuaternionToEuler(const tQuaternion& q); + static tQuaternion AxisAngleToQuaternion(const tVector& axis, double theta); + static void QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta); + static tMatrix BuildQuaternionDiffMat(const tQuaternion& q); + static tVector CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt); + static tVector CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt); + static tQuaternion VecToQuat(const tVector& v); + static tVector QuatToVec(const tQuaternion& q); + static tQuaternion QuatDiff(const tQuaternion& q0, const tQuaternion& q1); + static double QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1); + static double QuatTheta(const tQuaternion& dq); + static tQuaternion VecDiffQuat(const tVector& v0, const tVector& v1); + static tVector QuatRotVec(const tQuaternion& q, const tVector& dir); + static tQuaternion MirrorQuaternion(const tQuaternion& q, eAxis axis); + + static double Sign(double val); + static int Sign(int val); + + static double AddAverage(double avg0, int count0, double avg1, int count1); + static tVector AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1); + static void AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result); + static void CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob); + static double EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample); + static double EvalGaussian(double mean, double covar, double sample); + static double CalcGaussianPartition(const Eigen::VectorXd& covar); + static double EvalGaussianLogp(double mean, double covar, double sample); + static double EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample); + static double Sigmoid(double x); + static double Sigmoid(double x, double gamma, double bias); + + static int SampleDiscreteProb(const Eigen::VectorXd& probs); + static tVector CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c); + + static bool ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max); + static bool ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1); + static bool ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max); + static bool ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1); + static void CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1, + tVector& out_min, tVector& out_max); + static void CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1, + tVector& out_min, tVector& out_max); + static bool IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1); + static bool IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1); + + // check if curr_val and curr_val - delta belong to different intervals + static bool CheckNextInterval(double delta, double curr_val, double int_size); + + static tVector SampleRandPt(const tVector& bound_min, const tVector& bound_max); + // samples a bound within the given bounds with a benter towards the focus pt + static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max); + static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus); + + static void QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist); + static tQuaternion ProjectQuat(const tQuaternion& q, const tVector& dir); + + static void ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x); + +private: + static cRand gRand; + + template + static T SignAux(T val) + { + return (T(0) < val) - (val < T(0)); + } +}; diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp b/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp new file mode 100644 index 000000000..bc88b38a2 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp @@ -0,0 +1,245 @@ +#include "RBDModel.h" +#include "RBDUtil.h" +#include "KinTree.h" + +cRBDModel::cRBDModel() +{ +} + +cRBDModel::~cRBDModel() +{ +} + +void cRBDModel::Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity) +{ + assert(joint_mat.rows() == body_defs.rows()); + mGravity = gravity; + mJointMat = joint_mat; + mBodyDefs = body_defs; + + int num_dofs = GetNumDof(); + int num_joints = GetNumJoints(); + const int svs = cSpAlg::gSpVecSize; + + mPose = Eigen::VectorXd::Zero(num_dofs); + mVel = Eigen::VectorXd::Zero(num_dofs); + + tMatrix trans_mat; + InitJointSubspaceArr(); + mChildParentMatArr = Eigen::MatrixXd::Zero(num_joints * trans_mat.rows(), trans_mat.cols()); + mSpWorldJointTransArr = Eigen::MatrixXd::Zero(num_joints * cSpAlg::gSVTransRows, cSpAlg::gSVTransCols); + mMassMat = Eigen::MatrixXd::Zero(num_dofs, num_dofs); + mBiasForce = Eigen::VectorXd::Zero(num_dofs); + mInertiaBuffer = Eigen::MatrixXd::Zero(num_joints * svs, svs); +} + +void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel) +{ + SetPose(pose); + SetVel(vel); + + UpdateJointSubspaceArr(); + UpdateChildParentMatArr(); + UpdateSpWorldTrans(); + UpdateMassMat(); + UpdateBiasForce(); +} + +int cRBDModel::GetNumDof() const +{ + return cKinTree::GetNumDof(mJointMat); +} + +int cRBDModel::GetNumJoints() const +{ + return cKinTree::GetNumJoints(mJointMat); +} + +void cRBDModel::SetGravity(const tVector& gravity) +{ + mGravity = gravity; +} + + + +const tVector& cRBDModel::GetGravity() const +{ + return mGravity; +} + +const Eigen::MatrixXd& cRBDModel::GetJointMat() const +{ + return mJointMat; +} + +const Eigen::MatrixXd& cRBDModel::GetBodyDefs() const +{ + return mBodyDefs; +} + +const Eigen::VectorXd& cRBDModel::GetPose() const +{ + return mPose; +} + +const Eigen::VectorXd& cRBDModel::GetVel() const +{ + return mVel; +} + +int cRBDModel::GetParent(int j) const +{ + return cKinTree::GetParent(mJointMat, j); +} + +const Eigen::MatrixXd& cRBDModel::GetMassMat() const +{ + return mMassMat; +} + +const Eigen::VectorXd& cRBDModel::GetBiasForce() const +{ + return mBiasForce; +} + +Eigen::MatrixXd& cRBDModel::GetInertiaBuffer() +{ + return mInertiaBuffer; +} + +tMatrix cRBDModel::GetChildParentMat(int j) const +{ + assert(j >= 0 && j < GetNumJoints()); + tMatrix trans; + int r = static_cast(trans.rows()); + int c = static_cast(trans.cols()); + trans = mChildParentMatArr.block(j * r, 0, r, c); + return trans; +} + +tMatrix cRBDModel::GetParentChildMat(int j) const +{ + tMatrix child_parent_trans = GetChildParentMat(j); + tMatrix parent_child_trans = cMathUtil::InvRigidMat(child_parent_trans); + return parent_child_trans; +} + +cSpAlg::tSpTrans cRBDModel::GetSpChildParentTrans(int j) const +{ + tMatrix mat = GetChildParentMat(j); + return cSpAlg::MatToTrans(mat); +} + +cSpAlg::tSpTrans cRBDModel::GetSpParentChildTrans(int j) const +{ + tMatrix mat = GetParentChildMat(j); + return cSpAlg::MatToTrans(mat); +} + +tMatrix cRBDModel::GetWorldJointMat(int j) const +{ + cSpAlg::tSpTrans trans = GetSpWorldJointTrans(j); + return cSpAlg::TransToMat(trans); +} + +tMatrix cRBDModel::GetJointWorldMat(int j) const +{ + cSpAlg::tSpTrans trans = GetSpJointWorldTrans(j); + return cSpAlg::TransToMat(trans); +} + +cSpAlg::tSpTrans cRBDModel::GetSpWorldJointTrans(int j) const +{ + assert(j >= 0 && j < GetNumJoints()); + cSpAlg::tSpTrans trans = cSpAlg::GetTrans(mSpWorldJointTransArr, j); + return trans; +} + +cSpAlg::tSpTrans cRBDModel::GetSpJointWorldTrans(int j) const +{ + cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j); + return cSpAlg::InvTrans(world_joint_trans); +} + +const Eigen::Block cRBDModel::GetJointSubspace(int j) const +{ + assert(j >= 0 && j < GetNumJoints()); + int offset = cKinTree::GetParamOffset(mJointMat, j); + int dim = cKinTree::GetParamSize(mJointMat, j); + int r = static_cast(mJointSubspaceArr.rows()); + return mJointSubspaceArr.block(0, offset, r, dim); +} + +tVector cRBDModel::CalcJointWorldPos(int j) const +{ + cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j); + tVector r = cSpAlg::GetRad(world_joint_trans); + return r; +} + +void cRBDModel::SetPose(const Eigen::VectorXd& pose) +{ + mPose = pose; +} + +void cRBDModel::SetVel(const Eigen::VectorXd& vel) +{ + mVel = vel; +} + +void cRBDModel::InitJointSubspaceArr() +{ + int num_dofs = GetNumDof(); + int num_joints = GetNumJoints(); + mJointSubspaceArr = Eigen::MatrixXd(cSpAlg::gSpVecSize, num_dofs); + for (int j = 0; j < num_joints; ++j) + { + int offset = cKinTree::GetParamOffset(mJointMat, j); + int dim = cKinTree::GetParamSize(mJointMat, j); + int r = static_cast(mJointSubspaceArr.rows()); + mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j); + } +} + +void cRBDModel::UpdateJointSubspaceArr() +{ + int num_joints = GetNumJoints(); + for (int j = 0; j < num_joints; ++j) + { + bool const_subspace = cRBDUtil::IsConstJointSubspace(mJointMat, j); + if (!const_subspace) + { + int offset = cKinTree::GetParamOffset(mJointMat, j); + int dim = cKinTree::GetParamSize(mJointMat, j); + int r = static_cast(mJointSubspaceArr.rows()); + mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j); + } + } +} + +void cRBDModel::UpdateChildParentMatArr() +{ + int num_joints = GetNumJoints(); + for (int j = 0; j < num_joints; ++j) + { + tMatrix child_parent_trans = cKinTree::ChildParentTrans(mJointMat, mPose, j); + int r = static_cast(child_parent_trans.rows()); + int c = static_cast(child_parent_trans.cols()); + mChildParentMatArr.block(j * r, 0, r, c) = child_parent_trans; + } +} + +void cRBDModel::UpdateSpWorldTrans() +{ + cRBDUtil::CalcWorldJointTransforms(*this, mSpWorldJointTransArr); +} + +void cRBDModel::UpdateMassMat() +{ + cRBDUtil::BuildMassMat(*this, mInertiaBuffer, mMassMat); +} + +void cRBDModel::UpdateBiasForce() +{ + cRBDUtil::BuildBiasForce(*this, mBiasForce); +} \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h b/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h new file mode 100644 index 000000000..54ad9eaf2 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h @@ -0,0 +1,71 @@ +#pragma once + +#include "SpAlg.h" +#include "MathUtil.h" + +// this class is mostly to help with efficiency by precomputing some useful +// quantities for RBD calculations +class cRBDModel +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + cRBDModel(); + ~cRBDModel(); + + virtual void Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity); + virtual void Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel); + + virtual int GetNumDof() const; + virtual int GetNumJoints() const; + + virtual const tVector& GetGravity() const; + virtual void SetGravity(const tVector& gravity); + + virtual const Eigen::MatrixXd& GetJointMat() const; + virtual const Eigen::MatrixXd& GetBodyDefs() const; + virtual const Eigen::VectorXd& GetPose() const; + virtual const Eigen::VectorXd& GetVel() const; + virtual int GetParent(int j) const; + + virtual const Eigen::MatrixXd& GetMassMat() const; + virtual const Eigen::VectorXd& GetBiasForce() const; + virtual Eigen::MatrixXd& GetInertiaBuffer(); + + virtual tMatrix GetChildParentMat(int j) const; + virtual tMatrix GetParentChildMat(int j) const; + virtual cSpAlg::tSpTrans GetSpChildParentTrans(int j) const; + virtual cSpAlg::tSpTrans GetSpParentChildTrans(int j) const; + + virtual tMatrix GetWorldJointMat(int j) const; + virtual tMatrix GetJointWorldMat(int j) const; + virtual cSpAlg::tSpTrans GetSpWorldJointTrans(int j) const; + virtual cSpAlg::tSpTrans GetSpJointWorldTrans(int j) const; + + virtual const Eigen::Block GetJointSubspace(int j) const; + virtual tVector CalcJointWorldPos(int j) const; + +protected: + tVector mGravity; + Eigen::MatrixXd mJointMat; + Eigen::MatrixXd mBodyDefs; + Eigen::VectorXd mPose; + Eigen::VectorXd mVel; + + Eigen::MatrixXd mJointSubspaceArr; + Eigen::MatrixXd mChildParentMatArr; + Eigen::MatrixXd mSpWorldJointTransArr; + Eigen::MatrixXd mMassMat; + Eigen::VectorXd mBiasForce; + Eigen::MatrixXd mInertiaBuffer; + + virtual void SetPose(const Eigen::VectorXd& pose); + virtual void SetVel(const Eigen::VectorXd& vel); + + virtual void InitJointSubspaceArr(); + + virtual void UpdateJointSubspaceArr(); + virtual void UpdateChildParentMatArr(); + virtual void UpdateSpWorldTrans(); + virtual void UpdateMassMat(); + virtual void UpdateBiasForce(); +}; \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp new file mode 100644 index 000000000..64a5820a8 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp @@ -0,0 +1,1050 @@ +#include "RBDUtil.h" +#include + +#define _USE_MATH_DEFINES +#include "math.h" +void cRBDUtil::SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, Eigen::VectorXd& out_tau) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::MatrixXd& body_defs = model.GetBodyDefs(); + const tVector& gravity = model.GetGravity(); + const Eigen::VectorXd& pose = model.GetPose(); + const Eigen::VectorXd& vel = model.GetVel(); + + assert(joint_mat.rows() == body_defs.rows()); + assert(pose.rows() == vel.rows()); + assert(pose.rows() == acc.rows()); + assert(cKinTree::GetNumDof(joint_mat) == pose.rows()); + + cSpAlg::tSpVec vel0 = cSpAlg::tSpVec::Zero(); + cSpAlg::tSpVec acc0 = cSpAlg::BuildSV(tVector::Zero(), -gravity); + + int num_joints = cKinTree::GetNumJoints(joint_mat); + Eigen::MatrixXd vels = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize); + Eigen::MatrixXd accs = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize); + Eigen::MatrixXd fs = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize); + + for (int j = 0; j < num_joints; ++j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + cSpAlg::tSpTrans parent_child_trans = model.GetSpParentChildTrans(j); + cSpAlg::tSpTrans world_child_trans = model.GetSpWorldJointTrans(j); + + const auto S = model.GetJointSubspace(j); + Eigen::VectorXd q; + Eigen::VectorXd dq; + Eigen::VectorXd ddq; + cKinTree::GetJointParams(joint_mat, pose, j, q); + cKinTree::GetJointParams(joint_mat, vel, j, dq); + cKinTree::GetJointParams(joint_mat, acc, j, ddq); + + cSpAlg::tSpVec cj = BuildCj(joint_mat, q, dq, j); + cSpAlg::tSpVec vj = cSpAlg::tSpVec::Zero(); + if (S.cols() > 0) + { + vj = S * dq; + } + + cSpAlg::tSpMat I = BuildInertiaSpatialMat(body_defs, j); + + cSpAlg::tSpVec vel_p; + cSpAlg::tSpVec acc_p; + if (cKinTree::HasParent(joint_mat, j)) + { + int parent_id = cKinTree::GetParent(joint_mat, j); + vel_p = vels.row(parent_id); + acc_p = accs.row(parent_id); + } + else + { + vel_p = vel0; + acc_p = acc0; + } + + cSpAlg::tSpVec Sddq = cSpAlg::tSpVec::Zero(); + if (S.cols() > 0) + { + Sddq = S * ddq; + } + + cSpAlg::tSpVec curr_vel = cSpAlg::ApplyTransM(parent_child_trans, vel_p) + vj; + cSpAlg::tSpVec curr_acc = cSpAlg::ApplyTransM(parent_child_trans, acc_p) + Sddq + cj + cSpAlg::CrossM(curr_vel, vj); + cSpAlg::tSpVec curr_f = I * curr_acc + cSpAlg::CrossF(curr_vel, I * curr_vel); + + vels.row(j) = curr_vel; + accs.row(j) = curr_acc; + fs.row(j) = curr_f; + } + } + + out_tau = Eigen::VectorXd::Zero(pose.size()); + for (int j = num_joints - 1; j >= 0; --j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + cSpAlg::tSpVec curr_f = fs.row(j); + const auto S = model.GetJointSubspace(j); + Eigen::VectorXd curr_tau = S.transpose() * curr_f; + + cKinTree::SetJointParams(joint_mat, j, curr_tau, out_tau); + if (cKinTree::HasParent(joint_mat, j)) + { + int parent_id = cKinTree::GetParent(joint_mat, j); + cSpAlg::tSpTrans child_parent_trans = model.GetSpChildParentTrans(j); + fs.row(parent_id) += cSpAlg::ApplyTransF(child_parent_trans, curr_f); + } + } + } +} + +void cRBDUtil::SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, Eigen::VectorXd& out_acc) +{ + Eigen::VectorXd total_force = Eigen::VectorXd::Zero(model.GetNumDof()); + SolveForDyna(model, tau, total_force, out_acc); +} + +void cRBDUtil::SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, const Eigen::VectorXd& total_force, Eigen::VectorXd& out_acc) +{ + Eigen::VectorXd C; + Eigen::MatrixXd H; + BuildBiasForce(model, C); + BuildMassMat(model, H); + out_acc = H.ldlt().solve(tau + total_force - C); +} + + +void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& out_mass_mat) +{ + const int svs = cSpAlg::gSpVecSize; + int num_joints = model.GetNumJoints(); + Eigen::MatrixXd Is = Eigen::MatrixXd::Zero(num_joints * svs, svs); + BuildMassMat(model, Is, out_mass_mat); +} + +void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buffer, Eigen::MatrixXd& out_mass_mat) +{ + // use composite-rigid-body algorithm + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::MatrixXd& body_defs = model.GetBodyDefs(); + const Eigen::VectorXd& pose = model.GetPose(); + Eigen::MatrixXd& H = out_mass_mat; + + int dim = model.GetNumDof(); + int num_joints = model.GetNumJoints(); + H.setZero(dim, dim); + const int svs = cSpAlg::gSpVecSize; + + Eigen::MatrixXd child_parent_mats_F = Eigen::MatrixXd(svs * num_joints, svs); + Eigen::MatrixXd parent_child_mats_M = Eigen::MatrixXd(svs * num_joints, svs); + Eigen::MatrixXd& Is = inertia_buffer; + for (int j = 0; j < num_joints; ++j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + Is.block(j * svs, 0, svs, svs) = BuildInertiaSpatialMat(body_defs, j); + } + + cSpAlg::tSpTrans child_parent_trans = model.GetSpChildParentTrans(j); + cSpAlg::tSpMat child_parent_mat = cSpAlg::BuildSpatialMatF(child_parent_trans); + cSpAlg::tSpMat parent_child_mat = cSpAlg::BuildSpatialMatM(cSpAlg::InvTrans(child_parent_trans)); + child_parent_mats_F.block(j * svs, 0, svs, svs) = child_parent_mat; + parent_child_mats_M.block(j * svs, 0, svs, svs) = parent_child_mat; + } + + for (int j = num_joints - 1; j >= 0; --j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + const auto curr_I = Is.block(j * svs, 0, svs, svs); + int parent_id = cKinTree::GetParent(joint_mat, j); + if (cKinTree::HasParent(joint_mat, j)) + { + cSpAlg::tSpTrans child_parent_trans = model.GetSpChildParentTrans(j); + cSpAlg::tSpMat child_parent_mat = child_parent_mats_F.block(j * svs, 0, svs, svs); + cSpAlg::tSpMat parent_child_mat = parent_child_mats_M.block(j * svs, 0, svs, svs); + Is.block(parent_id * svs, 0, svs, svs) += child_parent_mat * curr_I * parent_child_mat; + } + + const auto S = model.GetJointSubspace(j); + int dim = cKinTree::GetParamSize(joint_mat, j); + if (dim > 0) + { + int offset = cKinTree::GetParamOffset(joint_mat, j); + Eigen::MatrixXd F = curr_I * S; + H.block(offset, offset, dim, dim) = S.transpose() * F; + + int curr_id = j; + while (cKinTree::HasParent(joint_mat, curr_id)) + { + cSpAlg::tSpMat child_parent_mat = child_parent_mats_F.block(curr_id * svs, 0, svs, svs); + F = child_parent_mat * F; + + curr_id = cKinTree::GetParent(joint_mat, curr_id); + int curr_offset = cKinTree::GetParamOffset(joint_mat, curr_id); + int curr_dim = cKinTree::GetParamSize(joint_mat, curr_id); + + if (curr_dim > 0) + { + const auto curr_S = model.GetJointSubspace(curr_id); + H.block(offset, curr_offset, dim, curr_dim) = F.transpose() * curr_S; + H.block(curr_offset, offset, curr_dim, dim) = H.block(offset, curr_offset, dim, curr_dim).transpose(); + } + } + } + } + } +} + +void cRBDUtil::BuildEndEffectorJacobian(const cRBDModel& model, int joint_id, Eigen::MatrixXd& out_J) +{ + // jacobian in world coordinates + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::VectorXd& pose = model.GetPose(); + + int num_dofs = cKinTree::GetNumDof(joint_mat); + out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs); + + int curr_id = joint_id; + cSpAlg::tSpTrans curr_trans = cSpAlg::BuildTrans(); + while (curr_id != cKinTree::gInvalidJointID) + { + int offset = cKinTree::GetParamOffset(joint_mat, curr_id); + int size = cKinTree::GetParamSize(joint_mat, curr_id); + const Eigen::MatrixXd S = model.GetJointSubspace(curr_id); + + out_J.block(0, offset, cSpAlg::gSpVecSize, size) = cSpAlg::ApplyTransM(curr_trans, S); + + int parent_id = cKinTree::GetParent(joint_mat, curr_id); + cSpAlg::tSpTrans parent_child_trans = model.GetSpParentChildTrans(curr_id); + curr_trans = cSpAlg::CompTrans(curr_trans, parent_child_trans); + curr_id = parent_id; + } + + out_J = cSpAlg::ApplyInvTransM(curr_trans, out_J); +} + +void cRBDUtil::BuildEndEffectorJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int joint_id, Eigen::MatrixXd& out_J) +{ + // jacobian in world coordinates + int num_dofs = cKinTree::GetNumDof(joint_mat); + out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs); + + int curr_id = joint_id; + cSpAlg::tSpTrans curr_trans = cSpAlg::BuildTrans(); + while (curr_id != cKinTree::gInvalidJointID) + { + int offset = cKinTree::GetParamOffset(joint_mat, curr_id); + int size = cKinTree::GetParamSize(joint_mat, curr_id); + Eigen::MatrixXd S = BuildJointSubspace(joint_mat, pose, curr_id); + + S = cSpAlg::ApplyTransM(curr_trans, S); + out_J.block(0, offset, cSpAlg::gSpVecSize, size) = S; + + int parent_id = cKinTree::GetParent(joint_mat, curr_id); + cSpAlg::tSpTrans parent_child_trans = BuildParentChildTransform(joint_mat, pose, curr_id); + curr_trans = cSpAlg::CompTrans(curr_trans, parent_child_trans); + curr_id = parent_id; + } + + out_J = cSpAlg::ApplyInvTransM(curr_trans, out_J); +} + +Eigen::MatrixXd cRBDUtil::MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::MatrixXd& J, int joint_id) +{ + // multiplies q by the jacobian of the end effector (joint_id) + int curr_id = joint_id; + cSpAlg::tSpVec joint_vel = cSpAlg::tSpVec::Zero(); + while (curr_id != cKinTree::gInvalidJointID) + { + int offset = cKinTree::GetParamOffset(joint_mat, curr_id); + int size = cKinTree::GetParamSize(joint_mat, curr_id); + Eigen::VectorXd curr_q; + cKinTree::GetJointParams(joint_mat, q, curr_id, curr_q); + const auto curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); + + joint_vel += curr_J * curr_q; + curr_id = cKinTree::GetParent(joint_mat, curr_id); + } + return joint_vel; +} + +void cRBDUtil::BuildJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::VectorXd& pose = model.GetPose(); + + int num_dofs = model.GetNumDof(); + out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs); + + int num_joints = cKinTree::GetNumJoints(joint_mat); + for (int j = 0; j < num_joints; ++j) + { + cSpAlg::tSpTrans world_joint_trans = model.GetSpWorldJointTrans(j); + + int offset = cKinTree::GetParamOffset(joint_mat, j); + int size = cKinTree::GetParamSize(joint_mat, j); + const Eigen::MatrixXd S = model.GetJointSubspace(j); + + out_J.block(0, offset, cSpAlg::gSpVecSize, size) = cSpAlg::ApplyInvTransM(world_joint_trans, S); + } +} + +Eigen::MatrixXd cRBDUtil::ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& J, int joint_id) +{ + int curr_id = joint_id; + Eigen::MatrixXd J_end_eff = Eigen::MatrixXd::Zero(J.rows(), J.cols()); + + while (curr_id != cKinTree::gInvalidJointID) + { + int offset = cKinTree::GetParamOffset(joint_mat, curr_id); + int size = cKinTree::GetParamSize(joint_mat, curr_id); + const auto curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); + + J_end_eff.block(0, offset, cSpAlg::gSpVecSize, size) = curr_J; + curr_id = cKinTree::GetParent(joint_mat, curr_id); + } + return J_end_eff; +} + + +void cRBDUtil::BuildCOMJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J) +{ + // coord frame for jacobian has origin at the com + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::MatrixXd& body_defs = model.GetBodyDefs(); + const Eigen::VectorXd& pose = model.GetPose(); + + Eigen::MatrixXd J; + BuildJacobian(model, J); + BuildCOMJacobian(model, J, out_J); +} + +void cRBDUtil::BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J, Eigen::MatrixXd& out_J) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::MatrixXd& body_defs = model.GetBodyDefs(); + + int num_dofs = cKinTree::GetNumDof(joint_mat); + out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs); + double total_mass = cKinTree::CalcTotalMass(body_defs); + + int num_joints = cKinTree::GetNumJoints(joint_mat); + for (int j = num_joints - 1; j >= 0; --j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + double mass = cKinTree::GetBodyMass(body_defs, j); + double mass_frac = mass / total_mass; + + cSpAlg::tSpTrans world_child_trans = model.GetSpWorldJointTrans(j); + tMatrix body_joint_mat = cKinTree::BodyJointTrans(body_defs, j); + cSpAlg::tSpTrans body_world_trans = cSpAlg::CompTrans(cSpAlg::InvTrans(world_child_trans), cSpAlg::MatToTrans(body_joint_mat)); + tMatrix body_world_mat = cSpAlg::TransToMat(body_world_trans); + + tVector body_pos = body_world_mat.col(3); + body_pos[3] = 0; + cSpAlg::tSpTrans body_pos_trans = cSpAlg::BuildTrans(body_pos); + + int curr_id = j; + while (curr_id != cKinTree::gInvalidJointID) + { + int offset = cKinTree::GetParamOffset(joint_mat, curr_id); + int size = cKinTree::GetParamSize(joint_mat, curr_id); + + const Eigen::MatrixXd& J_block = J.block(0, offset, cSpAlg::gSpVecSize, size); + auto J_com_block = out_J.block(0, offset, cSpAlg::gSpVecSize, size); + J_com_block += mass_frac * cSpAlg::ApplyTransM(body_pos_trans, J_block); + + curr_id = cKinTree::GetParent(joint_mat, curr_id); + } + } + } +} + + +void cRBDUtil::BuildJacobianDot(const cRBDModel& model, Eigen::MatrixXd& out_J_dot) +{ + // for comput the velocity product acceleration + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::VectorXd& pose = model.GetPose(); + const Eigen::VectorXd& vel = model.GetVel(); + + int num_dofs = cKinTree::GetNumDof(joint_mat); + int num_joints = cKinTree::GetNumJoints(joint_mat); + out_J_dot = Eigen::MatrixXd(cSpAlg::gSpVecSize, num_dofs); + Eigen::MatrixXd Sqs(cSpAlg::gSpVecSize, num_joints); + + for (int j = 0; j < num_joints; ++j) + { + cSpAlg::tSpTrans world_child_trans = model.GetSpWorldJointTrans(j); + Eigen::MatrixXd S = model.GetJointSubspace(j); + S = cSpAlg::ApplyInvTransM(world_child_trans, S); + Eigen::VectorXd dq; + cKinTree::GetJointParams(joint_mat, vel, j, dq); + cSpAlg::tSpVec Sq = S * dq; + + cSpAlg::tSpVec parent_Sq = cSpAlg::tSpVec::Zero(); + int parent_id = cKinTree::GetParent(joint_mat, j); + if (parent_id != cKinTree::gInvalidJointID) + { + parent_Sq = Sqs.col(parent_id); + } + Sqs.col(j) = parent_Sq + Sq; + + int offset = cKinTree::GetParamOffset(joint_mat, j); + int size = cKinTree::GetParamSize(joint_mat, j); + out_J_dot.block(0, offset, cSpAlg::gSpVecSize, size) = cSpAlg::CrossMs(parent_Sq, S); + } +} + +cSpAlg::tSpVec cRBDUtil::BuildCOMVelProdAcc(const cRBDModel& model) +{ + Eigen::MatrixXd Jd; + BuildJacobianDot(model, Jd); + return BuildCOMVelProdAccAux(model, Jd); +} + +cSpAlg::tSpVec cRBDUtil::BuildCOMVelProdAccAux(const cRBDModel& model, const Eigen::MatrixXd& Jd) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::MatrixXd& body_defs = model.GetBodyDefs(); + const Eigen::VectorXd& pose = model.GetPose(); + const Eigen::VectorXd& vel = model.GetVel(); + const tVector& gravity = model.GetGravity(); + + // coord frame origin at com + int num_dofs = cKinTree::GetNumDof(joint_mat); + int num_joints = cKinTree::GetNumJoints(joint_mat); + + double total_mass = cKinTree::CalcTotalMass(body_defs); + cSpAlg::tSpVec com_vpa = cSpAlg::tSpVec::Zero(); + for (int j = 0; j < num_joints; ++j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + double mass = cKinTree::GetBodyMass(body_defs, j); + double mass_frac = mass / total_mass; + + cSpAlg::tSpVec vpa = CalcVelProdAcc(model, Jd, j); + com_vpa += mass_frac * vpa; + } + } + + tVector com = CalcCoMPos(model); + cSpAlg::tSpTrans trans = cSpAlg::BuildTrans(com); + com_vpa = cSpAlg::ApplyTransM(trans, com_vpa); + + return com_vpa; +} + +cSpAlg::tSpVec cRBDUtil::CalcVelProdAcc(const cRBDModel& model, const Eigen::MatrixXd& Jd, int joint_id) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::VectorXd& pose = model.GetPose(); + const Eigen::VectorXd& vel = model.GetVel(); + const tVector& gravity = model.GetGravity(); + + int curr_id = joint_id; + cSpAlg::tSpVec acc = cSpAlg::BuildSV(tVector::Zero(), -gravity); + + while (curr_id != cKinTree::gInvalidJointID) + { + int offset = cKinTree::GetParamOffset(joint_mat, curr_id); + int size = cKinTree::GetParamSize(joint_mat, curr_id); + Eigen::VectorXd q; + Eigen::VectorXd dq; + cKinTree::GetJointParams(joint_mat, pose, curr_id, q); + cKinTree::GetJointParams(joint_mat, vel, curr_id, dq); + const auto curr_Jd = Jd.block(0, offset, cSpAlg::gSpVecSize, size); + + cSpAlg::tSpVec cj = BuildCj(joint_mat, q, dq, curr_id); + if (cj.squaredNorm() > 0) + { + cSpAlg::tSpTrans world_joint_trans = model.GetSpWorldJointTrans(curr_id); + cj = cSpAlg::ApplyInvTransM(world_joint_trans, cj); + } + + acc += curr_Jd * dq + cj; + curr_id = cKinTree::GetParent(joint_mat, curr_id); + } + + return acc; +} + +cSpAlg::tSpVec cRBDUtil::CalcJointWorldVel(const cRBDModel& model, int joint_id) +{ + cSpAlg::tSpVec joint_vel = CalcJointWorldVel(model.GetJointMat(), model.GetPose(), model.GetVel(), joint_id); + return joint_vel; +} + +cSpAlg::tSpVec cRBDUtil::CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id) +{ + cSpAlg::tSpVec joint_vel = CalcWorldVel(joint_mat, state, vel, joint_id); + return joint_vel; +} + +cSpAlg::tSpVec cRBDUtil::CalcWorldVel(const cRBDModel& model, int joint_id) +{ + return CalcWorldVel(model.GetJointMat(), model.GetPose(), model.GetVel(), joint_id); +} + +cSpAlg::tSpVec cRBDUtil::CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, int joint_id) +{ + Eigen::MatrixXd J; + BuildEndEffectorJacobian(joint_mat, pose, joint_id, J); + cSpAlg::tSpVec sv = J * vel; + return sv; +} + +tVector cRBDUtil::CalcCoMPos(const cRBDModel& model) +{ + tVector com; + tVector com_vel; + Eigen::VectorXd vel = Eigen::VectorXd::Zero(model.GetNumDof()); + CalcCoM(model, com, com_vel); + return com; +} + +tVector cRBDUtil::CalcCoMVel(const cRBDModel& model) +{ + tVector com; + tVector com_vel; + CalcCoM(model, com, com_vel); + return com_vel; +} + +tVector cRBDUtil::CalcCoMVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel) +{ + tVector com; + tVector com_vel; + CalcCoM(joint_mat, body_defs, pose, vel, com, com_vel); + return com_vel; +} + +void cRBDUtil::CalcCoM(const cRBDModel& model, tVector& out_com, tVector& out_vel) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::MatrixXd& body_defs = model.GetBodyDefs(); + const Eigen::VectorXd& pose = model.GetPose(); + const Eigen::VectorXd& vel = model.GetVel(); + + int num_joints = cKinTree::GetNumJoints(joint_mat); + out_com.setZero(); + out_vel.setZero(); + double total_mass = 0; + + for (int j = 0; j < num_joints; ++j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + tVector local_com = cKinTree::GetBodyLocalCoM(body_defs, j); + cSpAlg::tSpTrans world_joint_trans = model.GetSpWorldJointTrans(j); + + tMatrix joint_world_mat = cSpAlg::TransToMat(cSpAlg::InvTrans(world_joint_trans)); + tMatrix body_joint_mat = cKinTree::BodyJointTrans(body_defs, j); + tMatrix body_world_mat = joint_world_mat * body_joint_mat; + + tVector attach_pt = cKinTree::GetBodyAttachPt(body_defs, j); + attach_pt[3] = 1; + attach_pt = body_joint_mat * attach_pt; + + local_com[3] = 1; + tVector world_com = body_world_mat * local_com; + world_com[3] = 0; + + cSpAlg::tSpTrans com_trans = cSpAlg::BuildTrans(world_com); + cSpAlg::tSpVec sv = CalcWorldVel(model, j); + sv = cSpAlg::ApplyTransM(com_trans, sv); + + tVector com_vel = cSpAlg::GetV(sv); + + double m = cKinTree::GetBodyMass(body_defs, j); + out_com += m * world_com; + out_vel += m * com_vel; + total_mass += m; + } + } + + assert(total_mass > 0); + out_com /= total_mass; + out_vel /= total_mass; +} + +void cRBDUtil::CalcCoM(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, + tVector& out_com, tVector& out_vel) +{ + int num_joints = cKinTree::GetNumJoints(joint_mat); + out_com.setZero(); + out_vel.setZero(); + double total_mass = 0; + + for (int j = 0; j < num_joints; ++j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + tVector local_com = cKinTree::GetBodyLocalCoM(body_defs, j); + tMatrix joint_world_mat = cKinTree::JointWorldTrans(joint_mat, pose, j); + tMatrix body_joint_mat = cKinTree::BodyJointTrans(body_defs, j); + tMatrix body_world_mat = joint_world_mat * body_joint_mat; + + tVector attach_pt = cKinTree::GetBodyAttachPt(body_defs, j); + attach_pt[3] = 1; + attach_pt = body_joint_mat * attach_pt; + + local_com[3] = 1; + tVector world_com = body_world_mat * local_com; + world_com[3] = 0; + + cSpAlg::tSpTrans com_trans = cSpAlg::BuildTrans(world_com); + cSpAlg::tSpVec sv = CalcWorldVel(joint_mat, pose, vel, j); + sv = cSpAlg::ApplyTransM(com_trans, sv); + + tVector com_vel = cSpAlg::GetV(sv); + + double m = cKinTree::GetBodyMass(body_defs, j); + out_com += m * world_com; + out_vel += m * com_vel; + total_mass += m; + } + } + + assert(total_mass > 0); + out_com /= total_mass; + out_vel /= total_mass; +} + +cSpAlg::tSpMat cRBDUtil::BuildMomentInertia(const Eigen::MatrixXd& body_defs, int part_id) +{ + // inertia tensor of shape centered at the com + assert(cKinTree::IsValidBody(body_defs, part_id)); + cShape::eShape shape = cKinTree::GetBodyShape(body_defs, part_id); + + cSpAlg::tSpMat I; + switch (shape) + { + case cShape::eShapeBox: + I = BuildMomentInertiaBox(body_defs, part_id); + break; + case cShape::eShapeCapsule: + I = BuildMomentInertiaCapsule(body_defs, part_id); + break; + case cShape::eShapeSphere: + I = BuildMomentInertiaSphere(body_defs, part_id); + break; + case cShape::eShapeCylinder: + I = BuildMomentInertiaCylinder(body_defs, part_id); + break; + default: + assert(false); // unsupported shape + break; + } + + return I; +} + +cSpAlg::tSpMat cRBDUtil::BuildMomentInertiaBox(const Eigen::MatrixXd& body_defs, int part_id) +{ + const cKinTree::tBodyDef& def = body_defs.row(part_id); + double mass = cKinTree::GetBodyMass(body_defs, part_id); + double sx = def(cKinTree::eBodyParam0); + double sy = def(cKinTree::eBodyParam1); + double sz = def(cKinTree::eBodyParam2); + + double x = mass / 12.0 * (sy * sy + sz * sz); + double y = mass / 12.0 * (sx * sx + sz * sz); + double z = mass / 12.0 * (sx * sx + sy * sy); + + cSpAlg::tSpMat I = cSpAlg::tSpMat::Zero(); + I(0, 0) = x; + I(1, 1) = y; + I(2, 2) = z; + I(3, 3) = mass; + I(4, 4) = mass; + I(5, 5) = mass; + + return I; +} + +cSpAlg::tSpMat cRBDUtil::BuildMomentInertiaCapsule(const Eigen::MatrixXd& body_defs, int part_id) +{ + const cKinTree::tBodyDef& def = body_defs.row(part_id); + double mass = cKinTree::GetBodyMass(body_defs, part_id); + double r = 0.5 * def(cKinTree::eBodyParam0); + double h = def(cKinTree::eBodyParam1); + + double c_vol = M_PI * r * r * h; + double hs_vol = M_PI * 2.0 / 3.0 * r * r * r; + double density = mass / (c_vol + 2 * hs_vol); + double cm = c_vol * density; + double hsm = hs_vol * density; + + double x = cm*(0.25 * r * r + (1.0 / 12.0) * h * h) + + 2 * hsm *(0.4 * r * r + (3.0 / 8) * r * h + 0.5 * h * h); + double y = (0.5 * cm + 0.8 * hsm) * r * r; + double z = x; + + cSpAlg::tSpMat I = cSpAlg::tSpMat::Zero(); + I(0, 0) = x; + I(1, 1) = y; + I(2, 2) = z; + I(3, 3) = mass; + I(4, 4) = mass; + I(5, 5) = mass; + + return I; +} + +cSpAlg::tSpMat cRBDUtil::BuildMomentInertiaSphere(const Eigen::MatrixXd& body_defs, int part_id) +{ + const cKinTree::tBodyDef& def = body_defs.row(part_id); + double mass = cKinTree::GetBodyMass(body_defs, part_id); + double r = 0.5 * def(cKinTree::eBodyParam0); + + double x = 0.4 * mass * r * r; + double y = x; + double z = x; + + cSpAlg::tSpMat I = cSpAlg::tSpMat::Zero(); + I(0, 0) = x; + I(1, 1) = y; + I(2, 2) = z; + I(3, 3) = mass; + I(4, 4) = mass; + I(5, 5) = mass; + + return I; +} + +cSpAlg::tSpMat cRBDUtil::BuildMomentInertiaCylinder(const Eigen::MatrixXd& body_defs, int part_id) +{ + const cKinTree::tBodyDef& def = body_defs.row(part_id); + double mass = cKinTree::GetBodyMass(body_defs, part_id); + double r = 0.5 * def(cKinTree::eBodyParam0); + double h = def(cKinTree::eBodyParam1); + + double c_vol = M_PI * r * r * h; + double hs_vol = M_PI * 2.0 / 3.0 * r * r * r; + + double x = mass / 12 * (3 * r * r + h * h); + double y = mass * r * r / 2; + double z = x; + + cSpAlg::tSpMat I = cSpAlg::tSpMat::Zero(); + I(0, 0) = x; + I(1, 1) = y; + I(2, 2) = z; + I(3, 3) = mass; + I(4, 4) = mass; + I(5, 5) = mass; + + return I; +} + +cSpAlg::tSpMat cRBDUtil::BuildInertiaSpatialMat(const Eigen::MatrixXd& body_defs, int part_id) +{ + cSpAlg::tSpMat Ic = BuildMomentInertia(body_defs, part_id); + tMatrix body_joint = cKinTree::BodyJointTrans(body_defs, part_id); + cSpAlg::tSpTrans X = cSpAlg::MatToTrans(body_joint); + cSpAlg::tSpMat Io = cSpAlg::BuildSpatialMatF(X) * Ic * cSpAlg::BuildSpatialMatM(cSpAlg::InvTrans(X)); + return Io; +} + +void cRBDUtil::CalcWorldJointTransforms(const cRBDModel& model, Eigen::MatrixXd& out_trans_arr) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::VectorXd& pose = model.GetPose(); + + int num_joints = cKinTree::GetNumJoints(joint_mat); + out_trans_arr.resize(num_joints * cSpAlg::gSVTransRows, cSpAlg::gSVTransCols); + + for (int j = 0; j < num_joints; ++j) + { + int row_idx = j * cSpAlg::gSVTransRows; + int parent_id = cKinTree::GetParent(joint_mat, j); + + cSpAlg::tSpTrans parent_child_trans = model.GetSpParentChildTrans(j); + cSpAlg::tSpTrans world_parent_trans = cSpAlg::BuildTrans(); + + if (parent_id != cKinTree::gInvalidJointID) + { + world_parent_trans = cSpAlg::GetTrans(out_trans_arr, parent_id); + } + + cSpAlg::tSpTrans world_child_trans = cSpAlg::CompTrans(parent_child_trans, world_parent_trans); + out_trans_arr.block(row_idx, 0, cSpAlg::gSVTransRows, cSpAlg::gSVTransCols) = world_child_trans; + } +} + +cSpAlg::tSpTrans cRBDUtil::BuildParentChildTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + cSpAlg::tSpTrans trans = BuildChildParentTransform(joint_mat, pose, j); + trans = cSpAlg::InvTrans(trans); + return trans; +} + +cSpAlg::tSpTrans cRBDUtil::BuildChildParentTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + tMatrix m = cKinTree::ChildParentTrans(joint_mat, pose, j); + cSpAlg::tSpTrans trans = cSpAlg::MatToTrans(m); + return trans; +} + +bool cRBDUtil::IsConstJointSubspace(const Eigen::MatrixXd& joint_mat, int j) +{ + bool is_root = cKinTree::IsRoot(joint_mat, j); + bool is_const = !is_root; + return is_const; +} + +Eigen::MatrixXd cRBDUtil::BuildJointSubspace(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + cKinTree::eJointType j_type = cKinTree::GetJointType(joint_mat, j); + bool is_root = cKinTree::IsRoot(joint_mat, j); + Eigen::MatrixXd S; + + if (is_root) + { + S = BuildJointSubspaceRoot(joint_mat, pose); + } + else + { + switch (j_type) + { + case cKinTree::eJointTypeRevolute: + S = BuildJointSubspaceRevolute(joint_mat, pose, j); + break; + case cKinTree::eJointTypePrismatic: + S = BuildJointSubspacePrismatic(joint_mat, pose, j); + break; + case cKinTree::eJointTypePlanar: + S = BuildJointSubspacePlanar(joint_mat, pose, j); + break; + case cKinTree::eJointTypeFixed: + S = BuildJointSubspaceFixed(joint_mat, pose, j); + break; + case cKinTree::eJointTypeSpherical: + S = BuildJointSubspaceSpherical(joint_mat, pose, j); + break; + default: + assert(false); // unsupported joint type; + break; + } + } + return S; +} + +Eigen::MatrixXd cRBDUtil::BuildJointSubspaceRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose) +{ + int dim = cKinTree::gRootDim; + int pos_dim = cKinTree::gPosDim; + int rot_dim = cKinTree::gRotDim; + + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim); + tQuaternion quat = cKinTree::GetRootRot(joint_mat, pose); + tMatrix E = cMathUtil::RotateMat(quat); + + S.block(3, 0, 3, pos_dim) = E.block(0, 0, 3, pos_dim).transpose(); + //S.block(0, pos_dim, 3, rot_dim).setIdentity(); + S.block(0, pos_dim, 3, rot_dim - 1) = E.block(0, 0, 3, rot_dim - 1).transpose(); + return S; +} + +Eigen::MatrixXd cRBDUtil::BuildJointSubspaceRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypeRevolute); + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim); + S(2, 0) = 1; + return S; +} + +Eigen::MatrixXd cRBDUtil::BuildJointSubspacePrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypePrismatic); + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim); + + S(3, 0) = 1; + return S; +} + +Eigen::MatrixXd cRBDUtil::BuildJointSubspacePlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypePlanar); + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim); + S(3, 0) = 1; + S(4, 1) = 1; + S(2, 2) = 1; + return S; +} + +Eigen::MatrixXd cRBDUtil::BuildJointSubspaceFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypeFixed); + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim); + return S; +} + +Eigen::MatrixXd cRBDUtil::BuildJointSubspaceSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j) +{ + int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypeSpherical); + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim); + S(0, 0) = 1; + S(1, 1) = 1; + S(2, 2) = 1; + return S; +} + +cSpAlg::tSpVec cRBDUtil::BuildCj(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j) +{ + cKinTree::eJointType j_type = cKinTree::GetJointType(joint_mat, j); + bool is_root = cKinTree::IsRoot(joint_mat, j); + cSpAlg::tSpVec cj; + + if (is_root) + { + cj = BuildCjRoot(joint_mat, q, q_dot, j); + } + else + { + switch (j_type) + { + case cKinTree::eJointTypeRevolute: + cj = BuildCjRevolute(joint_mat, q_dot, j); + break; + case cKinTree::eJointTypePrismatic: + cj = BuildCjPrismatic(joint_mat, q_dot, j); + break; + case cKinTree::eJointTypePlanar: + cj = BuildCjPlanar(joint_mat, q_dot, j); + break; + case cKinTree::eJointTypeFixed: + cj = BuildCjFixed(joint_mat, q_dot, j); + break; + case cKinTree::eJointTypeSpherical: + cj = BuildCjSpherical(joint_mat, q_dot, j); + break; + default: + assert(false); // unsupported joint type; + break; + } + } + return cj; +} + +cSpAlg::tSpVec cRBDUtil::BuildCjRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j) +{ + tQuaternion quat = cKinTree::GetRootRot(joint_mat, q); + tVector vel_lin = cKinTree::GetRootVel(joint_mat, q_dot); + tVector vel_ang = cKinTree::GetRootAngVel(joint_mat, q_dot); + vel_ang[3] = 0; + + Eigen::VectorXd joint_params; + cKinTree::GetJointParams(joint_mat, q_dot, j, joint_params); + + tMatrix vel_dquat_mat = cMathUtil::BuildQuaternionDiffMat(quat); + tVector dq_vec = vel_dquat_mat * vel_ang; + tQuaternion dquat = cMathUtil::VecToQuat(dq_vec); + + tMatrix mat = tMatrix::Identity(); + mat(0, 0) = 4 * (quat.w() * dquat.w() + quat.x() * dquat.x()); + mat(1, 1) = 4 * (quat.w() * dquat.w() + quat.y() * dquat.y()); + mat(2, 2) = 4 * (quat.w() * dquat.w() + quat.z() * dquat.z()); + + mat(1, 0) = 2 * (dquat.x() * quat.y() + quat.x() * dquat.y() + - dquat.w() * quat.z() - quat.w() * dquat.z()); + mat(0, 1) = 2 * (dquat.x() * quat.y() + quat.x() * dquat.y() + + dquat.w() * quat.z() + quat.w() * dquat.z()); + + mat(2, 0) = 2 * (dquat.x() * quat.z() + quat.x() * dquat.z() + + dquat.w() * quat.y() + quat.w() * dquat.y()); + mat(0, 2) = 2 * (dquat.x() * quat.z() + quat.x() * dquat.z() + - dquat.w() * quat.y() - quat.w() * dquat.y()); + + mat(2, 1) = 2 * (dquat.y() * quat.z() + quat.y() * dquat.z() + - dquat.w() * quat.x() - quat.w() * dquat.x()); + mat(1, 2) = 2 * (dquat.y() * quat.z() + quat.y() * dquat.z() + + dquat.w() * quat.x() + quat.w() * dquat.x()); + + cSpAlg::tSpVec cj = cSpAlg::tSpVec::Zero(); + cj.segment(3, 3) = (mat * vel_lin).segment(0, 3); + + return cj; +} + +cSpAlg::tSpVec cRBDUtil::BuildCjRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j) +{ + return cSpAlg::tSpVec::Zero(); +} + +cSpAlg::tSpVec cRBDUtil::BuildCjPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j) +{ + return cSpAlg::tSpVec::Zero(); +} + +cSpAlg::tSpVec cRBDUtil::BuildCjPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j) +{ + return cSpAlg::tSpVec::Zero(); +} + +cSpAlg::tSpVec cRBDUtil::BuildCjFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j) +{ + return cSpAlg::tSpVec::Zero(); +} + +cSpAlg::tSpVec cRBDUtil::BuildCjSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j) +{ + return cSpAlg::tSpVec::Zero(); +} + +void cRBDUtil::BuildBiasForce(const cRBDModel& model, Eigen::VectorXd& out_bias_force) +{ + Eigen::VectorXd acc = Eigen::VectorXd::Zero(model.GetNumDof()); + SolveInvDyna(model, acc, out_bias_force); +} + +void cRBDUtil::CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_force) +{ + const Eigen::MatrixXd& joint_mat = model.GetJointMat(); + const Eigen::MatrixXd& body_defs = model.GetBodyDefs(); + const tVector& gravity = model.GetGravity(); + const Eigen::VectorXd& pose = model.GetPose(); + + assert(joint_mat.rows() == body_defs.rows()); + assert(cKinTree::GetNumDof(joint_mat) == pose.rows()); + + cSpAlg::tSpVec acc0 = cSpAlg::BuildSV(tVector::Zero(), gravity); + + int num_joints = cKinTree::GetNumJoints(joint_mat); + Eigen::MatrixXd fs = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize); + + for (int j = 0; j < num_joints; ++j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + cSpAlg::tSpTrans world_child_trans = model.GetSpWorldJointTrans(j); + cSpAlg::tSpMat I = BuildInertiaSpatialMat(body_defs, j); + cSpAlg::tSpVec curr_acc = cSpAlg::ApplyTransM(world_child_trans, acc0); + cSpAlg::tSpVec curr_f = I * curr_acc; + fs.row(j) = curr_f; + } + } + + out_g_force = Eigen::VectorXd::Zero(pose.size()); + for (int j = num_joints - 1; j >= 0; --j) + { + if (cKinTree::IsValidBody(body_defs, j)) + { + cSpAlg::tSpVec curr_f = fs.row(j); + const auto S = model.GetJointSubspace(j); + Eigen::VectorXd curr_tau = S.transpose() * curr_f; + + cKinTree::SetJointParams(joint_mat, j, curr_tau, out_g_force); + if (cKinTree::HasParent(joint_mat, j)) + { + int parent_id = cKinTree::GetParent(joint_mat, j); + cSpAlg::tSpTrans child_parent_trans = model.GetSpChildParentTrans(j); + fs.row(parent_id) += cSpAlg::ApplyTransF(child_parent_trans, curr_f); + } + } + } +} \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h new file mode 100644 index 000000000..aae03ebdf --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include + +#include "RBDModel.h" +#include "KinTree.h" + +class cRBDUtil +{ +public: + static void SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, Eigen::VectorXd& out_tau); + + static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, Eigen::VectorXd& out_acc); + static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, const Eigen::VectorXd& total_force, Eigen::VectorXd& out_acc); + + static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& out_mass_mat); + static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buffer, Eigen::MatrixXd& out_mass_mat); + static void BuildBiasForce(const cRBDModel& model, Eigen::VectorXd& out_bias_force); + + static void CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_force); + + static void BuildEndEffectorJacobian(const cRBDModel& model, int joint_id, Eigen::MatrixXd& out_J); + static void BuildEndEffectorJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int joint_id, Eigen::MatrixXd& out_J); + static Eigen::MatrixXd MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel, const Eigen::MatrixXd& J, int joint_id); + + static void BuildJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J); + static Eigen::MatrixXd ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& J, int joint_id); + + static void BuildCOMJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J); + static void BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J, Eigen::MatrixXd& out_J); + static void BuildJacobianDot(const cRBDModel& model, Eigen::MatrixXd& out_J_dot); + + static cSpAlg::tSpVec BuildCOMVelProdAcc(const cRBDModel& model); + static cSpAlg::tSpVec BuildCOMVelProdAccAux(const cRBDModel& model, const Eigen::MatrixXd& Jd); + static cSpAlg::tSpVec CalcVelProdAcc(const cRBDModel& model, const Eigen::MatrixXd& Jd, int joint_id); + + static cSpAlg::tSpVec CalcJointWorldVel(const cRBDModel& model, int joint_id); + static cSpAlg::tSpVec CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id); + static cSpAlg::tSpVec CalcWorldVel(const cRBDModel& model, int joint_id); + static cSpAlg::tSpVec CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id); + + + static tVector CalcCoMPos(const cRBDModel& model); + static tVector CalcCoMVel(const cRBDModel& model); + static tVector CalcCoMVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel); + static void CalcCoM(const cRBDModel& model, tVector& out_com, tVector& out_vel); + static void CalcCoM(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, + tVector& out_com, tVector& out_vel); + + static cSpAlg::tSpTrans BuildParentChildTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + static cSpAlg::tSpTrans BuildChildParentTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + + // extract a cSpAlg::tSpTrans from a matrix presentating a stack of transforms + static void CalcWorldJointTransforms(const cRBDModel& model, Eigen::MatrixXd& out_trans_arr); + static bool IsConstJointSubspace(const Eigen::MatrixXd& joint_mat, int j); + static Eigen::MatrixXd BuildJointSubspace(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + + static cSpAlg::tSpMat BuildMomentInertia(const Eigen::MatrixXd& body_defs, int part_id); + +protected: + + static Eigen::MatrixXd BuildJointSubspaceRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose); + static Eigen::MatrixXd BuildJointSubspaceRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + static Eigen::MatrixXd BuildJointSubspacePrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + static Eigen::MatrixXd BuildJointSubspacePlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + static Eigen::MatrixXd BuildJointSubspaceFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + static Eigen::MatrixXd BuildJointSubspaceSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j); + + static cSpAlg::tSpVec BuildCj(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j); + static cSpAlg::tSpVec BuildCjRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j); + static cSpAlg::tSpVec BuildCjRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j); + static cSpAlg::tSpVec BuildCjPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j); + static cSpAlg::tSpVec BuildCjPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j); + static cSpAlg::tSpVec BuildCjFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j); + static cSpAlg::tSpVec BuildCjSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j); + + static cSpAlg::tSpMat BuildMomentInertiaBox(const Eigen::MatrixXd& body_defs, int part_id); + static cSpAlg::tSpMat BuildMomentInertiaCapsule(const Eigen::MatrixXd& body_defs, int part_id); + static cSpAlg::tSpMat BuildMomentInertiaSphere(const Eigen::MatrixXd& body_defs, int part_id); + static cSpAlg::tSpMat BuildMomentInertiaCylinder(const Eigen::MatrixXd& body_defs, int part_id); + + // builds the spatial inertial matrix in the coordinate frame of the parent joint + static cSpAlg::tSpMat BuildInertiaSpatialMat(const Eigen::MatrixXd& body_defs, int part_id); +}; \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp b/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp new file mode 100644 index 000000000..599d2d0b7 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp @@ -0,0 +1,140 @@ +#include "Rand.h" +#include +#include +#include + +cRand::cRand() +{ + unsigned long int seed = static_cast(time(NULL)); + mRandGen = std::default_random_engine(seed); + mRandDoubleDist = std::uniform_real_distribution(0, 1); + mRandDoubleDistNorm = std::normal_distribution(0, 1); + mRandIntDist = std::uniform_int_distribution(std::numeric_limits::min() + 1, std::numeric_limits::max()); // + 1 since there is one more neg int than pos int + mRandUintDist = std::uniform_int_distribution(std::numeric_limits::min(), std::numeric_limits::max()); +} + +cRand::cRand(unsigned long int seed) +{ + Seed(seed); +} + +cRand::~cRand() +{ +} + +double cRand::RandDouble() +{ + return mRandDoubleDist(mRandGen); +} + +double cRand::RandDouble(double min, double max) +{ + if (min == max) + { + return min; + } + + // generate random double in [min, max] + double rand_double = mRandDoubleDist(mRandGen); + rand_double = min + (rand_double * (max - min)); + return rand_double; +} + +double cRand::RandDoubleExp(double lambda) +{ + std::exponential_distribution dist(lambda); + double rand_double = dist(mRandGen); + return rand_double; +} + +double cRand::RandDoubleNorm(double mean, double stdev) +{ + double rand_double = mRandDoubleDistNorm(mRandGen); + rand_double = mean + stdev * rand_double; + return rand_double; +} + +int cRand::RandInt() +{ + return mRandIntDist(mRandGen); +} + +int cRand::RandInt(int min, int max) +{ + if (min == max) + { + return min; + } + + // generate random double in [min, max) + int delta = max - min; + int rand_int = std::abs(RandInt()); + rand_int = min + rand_int % delta; + + return rand_int; +} + +int cRand::RandUint() +{ + return mRandUintDist(mRandGen); +} + +int cRand::RandUint(unsigned int min, unsigned int max) +{ + if (min == max) + { + return min; + } + + // generate random double in [min, max) + int delta = max - min; + int rand_int = RandUint(); + rand_int = min + rand_int % delta; + + return rand_int; +} + +int cRand::RandIntExclude(int min, int max, int exc) +{ + int rand_int = 0; + if (exc < min || exc >= max) + { + rand_int = RandInt(min, max); + } + else + { + int new_max = max - 1; + if (new_max <= min) + { + rand_int = min; + } + else + { + rand_int = RandInt(min, new_max); + if (rand_int >= exc) + { + ++rand_int; + } + } + } + return rand_int; +} + +void cRand::Seed(unsigned long int seed) +{ + mRandGen.seed(seed); + mRandDoubleDist.reset(); + mRandDoubleDistNorm.reset(); + mRandIntDist.reset(); + mRandUintDist.reset(); +} + +int cRand::RandSign() +{ + return FlipCoin() ? -1 : 1; +} + +bool cRand::FlipCoin(double p) +{ + return (RandDouble(0, 1) < p); +} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.h b/examples/SharedMemory/plugins/stablePDPlugin/Rand.h new file mode 100644 index 000000000..c7dd543bf --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/Rand.h @@ -0,0 +1,31 @@ +#pragma once + +#include + +class cRand +{ +public: + cRand(); + cRand(unsigned long int seed); + virtual ~cRand(); + + virtual double RandDouble(); + virtual double RandDouble(double min, double max); + virtual double RandDoubleExp(double lambda); + virtual double RandDoubleNorm(double mean, double stdev); + virtual int RandInt(); + virtual int RandInt(int min, int max); + virtual int RandUint(); + virtual int RandUint(unsigned int min, unsigned int max); + virtual int RandIntExclude(int min, int max, int exc); + virtual void Seed(unsigned long int seed); + virtual int RandSign(); + virtual bool FlipCoin(double p = 0.5); + +private: + std::default_random_engine mRandGen; + std::uniform_real_distribution mRandDoubleDist; + std::normal_distribution mRandDoubleDistNorm; + std::uniform_int_distribution mRandIntDist; + std::uniform_int_distribution mRandUintDist; +}; \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp b/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp new file mode 100644 index 000000000..67fae6ca0 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp @@ -0,0 +1,37 @@ +#include "Shape.h" +#include + +bool cShape::ParseShape(const std::string& str, eShape& out_shape) +{ + bool succ = true; + if (str == "null") + { + out_shape = eShapeNull; + } + else if (str == "box") + { + out_shape = eShapeBox; + } + else if (str == "capsule") + { + out_shape = eShapeCapsule; + } + else if (str == "sphere") + { + out_shape = eShapeSphere; + } + else if (str == "cylinder") + { + out_shape = eShapeCylinder; + } + else if (str == "plane") + { + out_shape = eShapePlane; + } + else + { + printf("Unsupported body shape %s\n", str.c_str()); + assert(false); + } + return succ; +} \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Shape.h b/examples/SharedMemory/plugins/stablePDPlugin/Shape.h new file mode 100644 index 000000000..e58fdf067 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/Shape.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +class cShape +{ +public: + enum eShape + { + eShapeNull, + eShapeBox, + eShapeCapsule, + eShapeSphere, + eShapeCylinder, + eShapePlane, + eShapeMax, + }; + + static bool ParseShape(const std::string& str, cShape::eShape& out_shape); +}; \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp b/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp new file mode 100644 index 000000000..60a5dc92d --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp @@ -0,0 +1,356 @@ +#include "SpAlg.h" +#include + +const int cSpAlg::gSpVecSize; +const int cSpAlg::gSVTransRows; +const int cSpAlg::gSVTransCols; + +cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1) +{ + return ConvertCoordM(m0, origin0, origin1, tMatrix::Identity()); +} + +cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, + const tVector& origin1, const tMatrix& R1) +{ + tMatrix R = R1 * R0.transpose(); + return ConvertCoordM(m0, origin0, origin1, R); +} + + +cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R) +{ + tSpTrans X = BuildTrans(R, origin1 - origin0); + return ApplyTransM(X, m0); +} + +cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tVector& origin1) +{ + return ConvertCoordF(f0, origin0, origin1, tMatrix::Identity()); +} + +cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tMatrix& R0, + const tVector& origin1, const tMatrix& R1) +{ + tMatrix R = R1 * R0.transpose(); + return ConvertCoordF(f0, origin0, origin1, R); +} + +cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, + const tVector& origin1, const tMatrix& R) +{ + tSpTrans X = BuildTrans(R, origin1 - origin0); + return ApplyTransF(X, f0); +} + + +cSpAlg::tSpVec cSpAlg::CrossM(const tSpVec& sv, const tSpVec& m) +{ + tVector sv_o = GetOmega(sv); + tVector sv_v = GetV(sv); + tVector m_o = GetOmega(m); + tVector m_v = GetV(m); + + tVector o = sv_o.cross3(m_o); + tVector v = sv_v.cross3(m_o) + sv_o.cross3(m_v); + + return BuildSV(o, v); +} + +Eigen::MatrixXd cSpAlg::CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms) +{ + assert(ms.rows() == gSpVecSize); + Eigen::MatrixXd result(gSpVecSize, ms.cols()); + for (int i = 0; i < ms.cols(); ++i) + { + const tSpVec& curr_m = ms.col(i); + result.col(i) = CrossM(sv, curr_m); + } + return result; +} + +cSpAlg::tSpVec cSpAlg::CrossF(const tSpVec& sv, const tSpVec& f) +{ + tVector sv_o = GetOmega(sv); + tVector sv_v = GetV(sv); + tVector f_o = GetOmega(f); + tVector f_v = GetV(f); + + tVector o = sv_o.cross3(f_o) + sv_v.cross3(f_v); + tVector v = sv_o.cross3(f_v); + + return BuildSV(o, v); +} + +Eigen::MatrixXd cSpAlg::CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs) +{ + assert(fs.rows() == gSpVecSize); + Eigen::MatrixXd result(gSpVecSize, fs.cols()); + for (int i = 0; i < fs.cols(); ++i) + { + const tSpVec& curr_f = fs.col(i); + result.col(i) = CrossF(sv, curr_f); + } + return result; +} + +cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& v) +{ + return BuildSV(tVector::Zero(), v); +} + +cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& o, const tVector& v) +{ + tSpVec sv; + SetOmega(o, sv); + SetV(v, sv); + return sv; +} + +tVector cSpAlg::GetOmega(const tSpVec& sv) +{ + tVector o = tVector::Zero(); + o.block(0, 0, 3, 1) = sv.block(0, 0, 3, 1); + return o; +} + +void cSpAlg::SetOmega(const tVector& o, tSpVec& out_sv) +{ + out_sv.block(0, 0, 3, 1) = o.block(0, 0, 3, 1); +} + +tVector cSpAlg::GetV(const tSpVec& sv) +{ + tVector v = tVector::Zero(); + v.block(0, 0, 3, 1) = sv.block(3, 0, 3, 1); + return v; +} + +void cSpAlg::SetV(const tVector& v, tSpVec& out_sv) +{ + out_sv.block(3, 0, 3, 1) = v.block(0, 0, 3, 1); +} + +cSpAlg::tSpTrans cSpAlg::BuildTrans() +{ + return BuildTrans(tMatrix::Identity(), tVector::Zero()); +} + +cSpAlg::tSpTrans cSpAlg::BuildTrans(const tMatrix& E, const tVector& r) +{ + tSpTrans X; + SetRad(r, X); + SetRot(E, X); + return X; +} + +cSpAlg::tSpTrans cSpAlg::BuildTrans(const tVector& r) +{ + return BuildTrans(tMatrix::Identity(), r); +} + +cSpAlg::tSpTrans cSpAlg::MatToTrans(const tMatrix& mat) +{ + tMatrix E = mat; + tVector r = mat.col(3); + r[3] = 0; + r = -E.transpose() * r; + return BuildTrans(E, r); +} + +tMatrix cSpAlg::TransToMat(const tSpTrans& X) +{ + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tMatrix m = E; + m.col(3) = -E * r; + m(3, 3) = 1; + return m; +} + +cSpAlg::tSpMat cSpAlg::BuildSpatialMatM(const tSpTrans& X) +{ + tSpMat m = tSpMat::Zero(); + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tMatrix Er = E * cMathUtil::CrossMat(r); + + m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3); + m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3); + m.block(3, 0, 3, 3) = -Er.block(0, 0, 3, 3); + return m; +} + +cSpAlg::tSpMat cSpAlg::BuildSpatialMatF(const tSpTrans& X) +{ + tSpMat m = tSpMat::Zero(); + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tMatrix Er = E * cMathUtil::CrossMat(r); + + m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3); + m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3); + m.block(0, 3, 3, 3) = -Er.block(0, 0, 3, 3); + return m; +} + + +cSpAlg::tSpTrans cSpAlg::InvTrans(const tSpTrans& X) +{ + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tSpTrans inv_X = BuildTrans(E.transpose(), -E * r); + return inv_X; +} + +tMatrix cSpAlg::GetRot(const tSpTrans& X) +{ + tMatrix E = tMatrix::Zero(); + E.block(0, 0, 3, 3) = X.block(0, 0, 3, 3); + return E; +} + +void cSpAlg::SetRot(const tMatrix& E, tSpTrans& out_X) +{ + out_X.block(0, 0, 3, 3) = E.block(0, 0, 3, 3); +} + +tVector cSpAlg::GetRad(const tSpTrans& X) +{ + tVector r = tVector::Zero(); + r.block(0, 0, 3, 1) = X.block(0, 3, 3, 1); + return r; +} + +void cSpAlg::SetRad(const tVector& r, tSpTrans& out_X) +{ + out_X.block(0, 3, 3, 1) = r.block(0, 0, 3, 1); +} + +cSpAlg::tSpVec cSpAlg::ApplyTransM(const tSpTrans& X, const tSpVec& sv) +{ + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tVector o0 = GetOmega(sv); + tVector v0 = GetV(sv); + + tVector o1 = E * o0; + tVector v1 = E * (v0 - r.cross3(o0)); + + tSpVec new_vec = BuildSV(o1, v1); + return new_vec; +} + +cSpAlg::tSpVec cSpAlg::ApplyTransF(const tSpTrans& X, const tSpVec& sv) +{ + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tVector o0 = GetOmega(sv); + tVector v0 = GetV(sv); + + tVector o1 = E * (o0 - r.cross3(v0)); + tVector v1 = E * v0; + + tSpVec new_vec = BuildSV(o1, v1); + return new_vec; +} + +Eigen::MatrixXd cSpAlg::ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm) +{ + assert(sm.rows() == gSpVecSize); + Eigen::MatrixXd result(gSpVecSize, sm.cols()); + for (int i = 0; i < sm.cols(); ++i) + { + const tSpVec& curr_sv = sm.col(i); + result.col(i) = ApplyTransM(X, curr_sv); + } + return result; +} + +Eigen::MatrixXd cSpAlg::ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm) +{ + assert(sm.rows() == gSpVecSize); + Eigen::MatrixXd result(gSpVecSize, sm.cols()); + for (int i = 0; i < sm.cols(); ++i) + { + const tSpVec& curr_sv = sm.col(i); + result.col(i) = ApplyTransF(X, curr_sv); + } + return result; +} + +cSpAlg::tSpVec cSpAlg::ApplyInvTransM(const tSpTrans& X, const tSpVec& sv) +{ + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tVector o0 = GetOmega(sv); + tVector v0 = GetV(sv); + + tVector o1 = E.transpose() * o0; + tVector v1 = E.transpose() * v0 + r.cross3(E.transpose() * o0); + + tSpVec new_vec = BuildSV(o1, v1); + return new_vec; +} + +cSpAlg::tSpVec cSpAlg::ApplyInvTransF(const tSpTrans& X, const tSpVec& sv) +{ + tMatrix E = GetRot(X); + tVector r = GetRad(X); + tVector o0 = GetOmega(sv); + tVector v0 = GetV(sv); + + tVector o1 = E.transpose() * o0 + r.cross3(E.transpose() * v0); + tVector v1 = E.transpose() * v0; + + tSpVec new_vec = BuildSV(o1, v1); + return new_vec; +} + +Eigen::MatrixXd cSpAlg::ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm) +{ + assert(sm.rows() == gSpVecSize); + Eigen::MatrixXd result(sm.rows(), sm.cols()); + for (int i = 0; i < sm.cols(); ++i) + { + const tSpVec& curr_sv = sm.col(i); + result.col(i) = ApplyInvTransM(X, curr_sv); + } + return result; +} + +Eigen::MatrixXd cSpAlg::ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm) +{ + assert(sm.rows() == gSpVecSize); + Eigen::MatrixXd result(sm.rows(), sm.cols()); + for (int i = 0; i < sm.cols(); ++i) + { + const tSpVec& curr_sv = sm.col(i); + result.col(i) = ApplyInvTransF(X, curr_sv); + } + return result; +} + +cSpAlg::tSpTrans cSpAlg::CompTrans(const tSpTrans& X0, const tSpTrans& X1) +{ + tMatrix E0 = GetRot(X0); + tMatrix E1 = GetRot(X1); + tVector r0 = GetRad(X0); + tVector r1 = GetRad(X1); + + tSpTrans X = BuildTrans(E0 * E1, r1 + E1.transpose() * r0); + return X; +} + +cSpAlg::tSpTrans cSpAlg::GetTrans(const Eigen::MatrixXd& trans_arr, int j) +{ + assert(trans_arr.rows() >= gSVTransRows); + assert((trans_arr.rows() % gSVTransRows) == 0); + assert(trans_arr.cols() == gSVTransCols); + + int row_idx = j * gSVTransRows; + assert(row_idx <= trans_arr.rows() - gSVTransRows); + + tSpTrans X = trans_arr.block(row_idx, 0, gSVTransRows, gSVTransCols); + return X; +} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h b/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h new file mode 100644 index 000000000..6055530f1 --- /dev/null +++ b/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h @@ -0,0 +1,68 @@ +#pragma once + +#include "MathUtil.h" + +// spatial algebra util +class cSpAlg +{ +public: + const static int gSpVecSize = 6; + const static int gSVTransRows = 3; + const static int gSVTransCols = 4; + + + typedef Eigen::Matrix tSpTrans; + typedef Eigen::Matrix tSpVec; + typedef Eigen::Matrix tSpMat; + + static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1); + // rows of R shoold be the basis for the coordinate frame centered ar origin + static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1); + // R is a rotation from coord frame 0 to coord frame 1 + static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R); + + static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1); + static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1); + static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R); + + static tSpVec CrossM(const tSpVec& sv, const tSpVec& m); + static Eigen::MatrixXd CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms); + static tSpVec CrossF(const tSpVec& sv, const tSpVec& f); + static Eigen::MatrixXd CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs); + + // Spatial Vector methods + static tSpVec BuildSV(const tVector& v); + static tSpVec BuildSV(const tVector& o, const tVector& v); + static tVector GetOmega(const tSpVec& sv); + static void SetOmega(const tVector& o, tSpVec& out_sv); + static tVector GetV(const tSpVec& sv); + static void SetV(const tVector& v, tSpVec& out_sv); + + // tSpTrans methods + static tSpTrans BuildTrans(); + static tSpTrans BuildTrans(const tMatrix& E, const tVector& r); + static tSpTrans BuildTrans(const tVector& r); + + static tSpTrans MatToTrans(const tMatrix& mat); + static tMatrix TransToMat(const tSpTrans& X); + static tSpMat BuildSpatialMatM(const tSpTrans& X); + static tSpMat BuildSpatialMatF(const tSpTrans& X); + static tSpTrans InvTrans(const tSpTrans& X); + static tMatrix GetRot(const tSpTrans& X); + static void SetRot(const tMatrix& E, tSpTrans& out_X); + static tVector GetRad(const tSpTrans& X); + static void SetRad(const tVector& r, tSpTrans& out_X); + + static tSpVec ApplyTransM(const tSpTrans& X, const tSpVec& sv); + static tSpVec ApplyTransF(const tSpTrans& X, const tSpVec& sv); + static Eigen::MatrixXd ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm); + static Eigen::MatrixXd ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm); + static tSpVec ApplyInvTransM(const tSpTrans& X, const tSpVec& sv); + static tSpVec ApplyInvTransF(const tSpTrans& X, const tSpVec& sv); + static Eigen::MatrixXd ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm); + static Eigen::MatrixXd ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm); + static tSpTrans CompTrans(const tSpTrans& X0, const tSpTrans& X1); + + // extract a tSpTrans from a matrix presentating a stack of transforms + static tSpTrans GetTrans(const Eigen::MatrixXd& trans_arr, int j); +}; diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index d9d8eb203..64d385cbe 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -66,6 +66,7 @@ struct TinyRendererVisualShapeConverterInternalData // Maps bodyUniqueId to a list of visual shapes belonging to the body. btHashMap > m_visualShapesMap; + int m_uidGenerator; int m_upAxis; int m_swWidth; int m_swHeight; @@ -92,6 +93,7 @@ struct TinyRendererVisualShapeConverterInternalData TinyRendererVisualShapeConverterInternalData() : m_upAxis(2), + m_uidGenerator(1), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB), @@ -606,12 +608,13 @@ static btVector4 sColors[4] = //btVector4(1,1,0,1), }; -void TinyRendererVisualShapeConverter::convertVisualShapes( +int TinyRendererVisualShapeConverter::convertVisualShapes( int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, - const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, + const UrdfLink* linkPtr, const UrdfModel* model, int unused, int bodyUniqueId, struct CommonFileIOInterface* fileIO) { + int uniqueId = m_data->m_uidGenerator++; btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) if (linkPtr) { @@ -699,12 +702,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } } - TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[uniqueId]; if (visualsPtr == 0) { - m_data->m_swRenderInstances.insert(collisionObjectUniqueId, new TinyRendererObjectArray); + m_data->m_swRenderInstances.insert(uniqueId, new TinyRendererObjectArray); } - visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId]; + visualsPtr = m_data->m_swRenderInstances[uniqueId]; btAssert(visualsPtr); TinyRendererObjectArray* visuals = *visualsPtr; @@ -778,6 +781,8 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( shapes->push_back(visualShape); } } + + return uniqueId; } int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId) diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h index 5ada574be..5fec39cdd 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h @@ -11,7 +11,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface virtual ~TinyRendererVisualShapeConverter(); - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO); + virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO); virtual int getNumVisualShapes(int bodyUniqueId); diff --git a/examples/ThirdPartyLibs/Eigen/CMakeLists.txt b/examples/ThirdPartyLibs/Eigen/CMakeLists.txt new file mode 100644 index 000000000..9eb502b79 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/CMakeLists.txt @@ -0,0 +1,19 @@ +include(RegexUtils) +test_escape_string_as_regex() + +file(GLOB Eigen_directory_files "*") + +escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") + +foreach(f ${Eigen_directory_files}) + if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src") + list(APPEND Eigen_directory_files_to_install ${f}) + endif() +endforeach(f ${Eigen_directory_files}) + +install(FILES + ${Eigen_directory_files_to_install} + DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel + ) + +install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h") diff --git a/examples/ThirdPartyLibs/Eigen/Cholesky b/examples/ThirdPartyLibs/Eigen/Cholesky new file mode 100644 index 000000000..1332b540d --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Cholesky @@ -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_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 + */ + +#include "src/Cholesky/LLT.h" +#include "src/Cholesky/LDLT.h" +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +#include "mkl_lapacke.h" +#else +#include "src/misc/lapacke.h" +#endif +#include "src/Cholesky/LLT_LAPACKE.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_CHOLESKY_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/CholmodSupport b/examples/ThirdPartyLibs/Eigen/CholmodSupport new file mode 100644 index 000000000..bed8924d3 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/CholmodSupport @@ -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_CHOLMODSUPPORT_MODULE_H +#define EIGEN_CHOLMODSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +extern "C" { + #include +} + +/** \ingroup Support_modules + * \defgroup CholmodSupport_Module CholmodSupport module + * + * This module provides an interface to the Cholmod library which is part of the suitesparse package. + * It provides the two following main factorization classes: + * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. + * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). + * + * For the sake of completeness, this module also propose the two following classes: + * - class CholmodSimplicialLLT + * - class CholmodSimplicialLDLT + * Note that these classes does not bring any particular advantage compared to the built-in + * SimplicialLLT and SimplicialLDLT factorization classes. + * + * \code + * #include + * \endcode + * + * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies. + * The dependencies depend on how cholmod has been compiled. + * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task. + * + */ + +#include "src/CholmodSupport/CholmodSupport.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_CHOLMODSUPPORT_MODULE_H + diff --git a/examples/ThirdPartyLibs/Eigen/Core b/examples/ThirdPartyLibs/Eigen/Core new file mode 100644 index 000000000..c66359b79 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Core @@ -0,0 +1,562 @@ +// 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_H +#define EIGEN_CORE_H + +// first thing Eigen does: stop the compiler from committing suicide +#include "src/Core/util/DisableStupidWarnings.h" + +#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA) + #define EIGEN_CUDACC __CUDACC__ +#endif + +#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA) + #define EIGEN_CUDA_ARCH __CUDA_ARCH__ +#endif + +// Starting with CUDA 9 the composite __CUDACC_VER__ is not available. +#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9) +#define EIGEN_CUDACC_VER ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100)) +#elif defined(__CUDACC_VER__) +#define EIGEN_CUDACC_VER __CUDACC_VER__ +#else +#define EIGEN_CUDACC_VER 0 +#endif + +// Handle NVCC/CUDA/SYCL +#if defined(EIGEN_CUDACC) || defined(__SYCL_DEVICE_ONLY__) + // Do not try asserts on CUDA and SYCL! + #ifndef EIGEN_NO_DEBUG + #define EIGEN_NO_DEBUG + #endif + + #ifdef EIGEN_INTERNAL_DEBUGGING + #undef EIGEN_INTERNAL_DEBUGGING + #endif + + #ifdef EIGEN_EXCEPTIONS + #undef EIGEN_EXCEPTIONS + #endif + + // All functions callable from CUDA code must be qualified with __device__ + #ifdef EIGEN_CUDACC + // Do not try to vectorize on CUDA and SYCL! + #ifndef EIGEN_DONT_VECTORIZE + #define EIGEN_DONT_VECTORIZE + #endif + + #define EIGEN_DEVICE_FUNC __host__ __device__ + // We need math_functions.hpp to ensure that that EIGEN_USING_STD_MATH macro + // works properly on the device side + #include + #else + #define EIGEN_DEVICE_FUNC + #endif +#else + #define EIGEN_DEVICE_FUNC +#endif + +#ifdef __NVCC__ +#define EIGEN_DONT_VECTORIZE +#endif + +// When compiling CUDA device code with NVCC, pull in math functions from the +// global namespace. In host mode, and when device doee with clang, use the +// std versions. +#if defined(EIGEN_CUDA_ARCH) && defined(__NVCC__) + #define EIGEN_USING_STD_MATH(FUNC) using ::FUNC; +#else + #define EIGEN_USING_STD_MATH(FUNC) using std::FUNC; +#endif + +#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_CUDA_ARCH) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL) + #define EIGEN_EXCEPTIONS +#endif + +#ifdef EIGEN_EXCEPTIONS + #include +#endif + +// 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 alignment settings (platform detection and honoring the user's will if he +// defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization. +#include "src/Core/util/Macros.h" + +// Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3) +// See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details. +#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6) + #pragma GCC optimize ("-fno-ipa-cp-clone") +#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 alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into +// account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks +#if EIGEN_MAX_ALIGN_BYTES==0 + #ifndef EIGEN_DONT_VECTORIZE + #define EIGEN_DONT_VECTORIZE + #endif +#endif + +#if EIGEN_COMP_MSVC + #include // for _aligned_malloc -- need it regardless of whether vectorization is enabled + #if (EIGEN_COMP_MSVC >= 1500) // 2008 or later + // Remember that usage of defined() in a #define is undefined by the standard. + // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP. + #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64 + #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER + #endif + #endif +#else + // Remember that usage of defined() in a #define is undefined by the standard + #if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) ) + #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC + #endif +#endif + +#ifndef EIGEN_DONT_VECTORIZE + + #if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER) + + // Defines symbols for compile-time detection of which instructions are + // used. + // EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used + #define EIGEN_VECTORIZE + #define EIGEN_VECTORIZE_SSE + #define EIGEN_VECTORIZE_SSE2 + + // Detect sse3/ssse3/sse4: + // gcc and icc defines __SSE3__, ... + // there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you + // want to force the use of those instructions with msvc. + #ifdef __SSE3__ + #define EIGEN_VECTORIZE_SSE3 + #endif + #ifdef __SSSE3__ + #define EIGEN_VECTORIZE_SSSE3 + #endif + #ifdef __SSE4_1__ + #define EIGEN_VECTORIZE_SSE4_1 + #endif + #ifdef __SSE4_2__ + #define EIGEN_VECTORIZE_SSE4_2 + #endif + #ifdef __AVX__ + #define EIGEN_VECTORIZE_AVX + #define EIGEN_VECTORIZE_SSE3 + #define EIGEN_VECTORIZE_SSSE3 + #define EIGEN_VECTORIZE_SSE4_1 + #define EIGEN_VECTORIZE_SSE4_2 + #endif + #ifdef __AVX2__ + #define EIGEN_VECTORIZE_AVX2 + #define EIGEN_VECTORIZE_AVX + #define EIGEN_VECTORIZE_SSE3 + #define EIGEN_VECTORIZE_SSSE3 + #define EIGEN_VECTORIZE_SSE4_1 + #define EIGEN_VECTORIZE_SSE4_2 + #endif + #ifdef __FMA__ + #define EIGEN_VECTORIZE_FMA + #endif + #if defined(__AVX512F__) + #define EIGEN_VECTORIZE_AVX512 + #define EIGEN_VECTORIZE_AVX2 + #define EIGEN_VECTORIZE_AVX + #define EIGEN_VECTORIZE_FMA + #define EIGEN_VECTORIZE_SSE3 + #define EIGEN_VECTORIZE_SSSE3 + #define EIGEN_VECTORIZE_SSE4_1 + #define EIGEN_VECTORIZE_SSE4_2 + #ifdef __AVX512DQ__ + #define EIGEN_VECTORIZE_AVX512DQ + #endif + #endif + + // include files + + // This extern "C" works around a MINGW-w64 compilation issue + // https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354 + // In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do). + // However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations + // with conflicting linkage. The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know; + // so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too. + // notice that since these are C headers, the extern "C" is theoretically needed anyways. + extern "C" { + // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly. + // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus: + #if EIGEN_COMP_ICC >= 1110 + #include + #else + #include + #include + #include + #ifdef EIGEN_VECTORIZE_SSE3 + #include + #endif + #ifdef EIGEN_VECTORIZE_SSSE3 + #include + #endif + #ifdef EIGEN_VECTORIZE_SSE4_1 + #include + #endif + #ifdef EIGEN_VECTORIZE_SSE4_2 + #include + #endif + #if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512) + #include + #endif + #endif + } // end extern "C" + #elif defined __VSX__ + #define EIGEN_VECTORIZE + #define EIGEN_VECTORIZE_VSX + #include + // We need to #undef all these ugly tokens defined in + // => use __vector instead of vector + #undef bool + #undef vector + #undef pixel + #elif defined __ALTIVEC__ + #define EIGEN_VECTORIZE + #define EIGEN_VECTORIZE_ALTIVEC + #include + // We need to #undef all these ugly tokens defined in + // => use __vector instead of vector + #undef bool + #undef vector + #undef pixel + #elif (defined __ARM_NEON) || (defined __ARM_NEON__) + #define EIGEN_VECTORIZE + #define EIGEN_VECTORIZE_NEON + #include + #elif (defined __s390x__ && defined __VEC__) + #define EIGEN_VECTORIZE + #define EIGEN_VECTORIZE_ZVECTOR + #include + #endif +#endif + +#if defined(__F16C__) && !defined(EIGEN_COMP_CLANG) + // We can use the optimized fp16 to float and float to fp16 conversion routines + #define EIGEN_HAS_FP16_C +#endif + +#if defined EIGEN_CUDACC + #define EIGEN_VECTORIZE_CUDA + #include + #if EIGEN_CUDACC_VER >= 70500 + #define EIGEN_HAS_CUDA_FP16 + #endif +#endif + +#if defined EIGEN_HAS_CUDA_FP16 + #include + #include +#endif + +#if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE) + #define EIGEN_HAS_OPENMP +#endif + +#ifdef EIGEN_HAS_OPENMP +#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 +#include +#include +#include +#include +#include +#include // for CHAR_BIT +// for min/max: +#include + +// for std::is_nothrow_move_assignable +#ifdef EIGEN_INCLUDE_TYPE_TRAITS +#include +#endif + +// for outputting debug info +#ifdef EIGEN_DEBUG_ASSIGN +#include +#endif + +// required for __cpuid, needs to be included after cmath +#if EIGEN_COMP_MSVC && EIGEN_ARCH_i386_OR_x86_64 && !EIGEN_OS_WINCE + #include +#endif + +#if defined(__SYCL_DEVICE_ONLY__) + #undef min + #undef max + #undef isnan + #undef isinf + #undef isfinite + #include +#endif + +/** \brief Namespace containing all symbols from the %Eigen library. */ +namespace Eigen { + +inline static const char *SimdInstructionSetsInUse(void) { +#if defined(EIGEN_VECTORIZE_AVX512) + return "AVX512, FMA, AVX2, AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2"; +#elif defined(EIGEN_VECTORIZE_AVX) + return "AVX SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2"; +#elif defined(EIGEN_VECTORIZE_SSE4_2) + return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2"; +#elif defined(EIGEN_VECTORIZE_SSE4_1) + return "SSE, SSE2, SSE3, SSSE3, SSE4.1"; +#elif defined(EIGEN_VECTORIZE_SSSE3) + return "SSE, SSE2, SSE3, SSSE3"; +#elif defined(EIGEN_VECTORIZE_SSE3) + return "SSE, SSE2, SSE3"; +#elif defined(EIGEN_VECTORIZE_SSE2) + return "SSE, SSE2"; +#elif defined(EIGEN_VECTORIZE_ALTIVEC) + return "AltiVec"; +#elif defined(EIGEN_VECTORIZE_VSX) + return "VSX"; +#elif defined(EIGEN_VECTORIZE_NEON) + return "ARM NEON"; +#elif defined(EIGEN_VECTORIZE_ZVECTOR) + return "S390X ZVECTOR"; +#else + return "None"; +#endif +} + +} // end namespace Eigen + +#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:: everytime just to +// ensure QNX/QCC support +using std::size_t; +// gcc 4.6.0 wants std:: for ptrdiff_t +using std::ptrdiff_t; + +} + +/** \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 + */ + +#include "src/Core/util/Constants.h" +#include "src/Core/util/Meta.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/SymbolicIndex.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" + +#if defined EIGEN_VECTORIZE_AVX512 + #include "src/Core/arch/SSE/PacketMath.h" + #include "src/Core/arch/AVX/PacketMath.h" + #include "src/Core/arch/AVX512/PacketMath.h" + #include "src/Core/arch/SSE/MathFunctions.h" + #include "src/Core/arch/AVX/MathFunctions.h" + #include "src/Core/arch/AVX512/MathFunctions.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/Complex.h" + #include "src/Core/arch/SSE/MathFunctions.h" + #include "src/Core/arch/AVX/PacketMath.h" + #include "src/Core/arch/AVX/MathFunctions.h" + #include "src/Core/arch/AVX/Complex.h" + #include "src/Core/arch/AVX/TypeCasting.h" +#elif defined EIGEN_VECTORIZE_SSE + #include "src/Core/arch/SSE/PacketMath.h" + #include "src/Core/arch/SSE/MathFunctions.h" + #include "src/Core/arch/SSE/Complex.h" + #include "src/Core/arch/SSE/TypeCasting.h" +#elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX) + #include "src/Core/arch/AltiVec/PacketMath.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/MathFunctions.h" + #include "src/Core/arch/NEON/Complex.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" +#endif + +// Half float support +#include "src/Core/arch/CUDA/Half.h" +#include "src/Core/arch/CUDA/PacketMathHalf.h" +#include "src/Core/arch/CUDA/TypeCasting.h" + +#if defined EIGEN_VECTORIZE_CUDA + #include "src/Core/arch/CUDA/PacketMath.h" + #include "src/Core/arch/CUDA/MathFunctions.h" +#endif + +#include "src/Core/arch/Default/Settings.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 to enable the processing of complex numbers +// on CUDA devices +#include "src/Core/arch/CUDA/Complex.h" + +#include "src/Core/util/IndexedViewHelper.h" +#include "src/Core/ArithmeticSequence.h" +#include "src/Core/IO.h" +#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/Transpose.h" +#include "src/Core/DiagonalMatrix.h" +#include "src/Core/Diagonal.h" +#include "src/Core/DiagonalProduct.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" +#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" + +#include "src/Core/BooleanRedux.h" +#include "src/Core/Select.h" +#include "src/Core/VectorwiseOp.h" +#include "src/Core/Random.h" +#include "src/Core/Replicate.h" +#include "src/Core/Reverse.h" +#include "src/Core/ArrayWrapper.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" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_CORE_H diff --git a/examples/ThirdPartyLibs/Eigen/Dense b/examples/ThirdPartyLibs/Eigen/Dense new file mode 100644 index 000000000..5768910bd --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Dense @@ -0,0 +1,7 @@ +#include "Core" +#include "LU" +#include "Cholesky" +#include "QR" +#include "SVD" +#include "Geometry" +#include "Eigenvalues" diff --git a/examples/ThirdPartyLibs/Eigen/Eigen b/examples/ThirdPartyLibs/Eigen/Eigen new file mode 100644 index 000000000..654c8dc63 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Eigen @@ -0,0 +1,2 @@ +#include "Dense" +#include "Sparse" diff --git a/examples/ThirdPartyLibs/Eigen/Eigenvalues b/examples/ThirdPartyLibs/Eigen/Eigenvalues new file mode 100644 index 000000000..f3f661b07 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Eigenvalues @@ -0,0 +1,61 @@ +// 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 "src/Core/util/DisableStupidWarnings.h" + +#include "Cholesky" +#include "Jacobi" +#include "Householder" +#include "LU" +#include "Geometry" + +/** \defgroup Eigenvalues_Module Eigenvalues module + * + * + * + * This module mainly provides various eigenvalue solvers. + * This module also provides some MatrixBase methods, including: + * - MatrixBase::eigenvalues(), + * - MatrixBase::operatorNorm() + * + * \code + * #include + * \endcode + */ + +#include "src/misc/RealSvd2x2.h" +#include "src/Eigenvalues/Tridiagonalization.h" +#include "src/Eigenvalues/RealSchur.h" +#include "src/Eigenvalues/EigenSolver.h" +#include "src/Eigenvalues/SelfAdjointEigenSolver.h" +#include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" +#include "src/Eigenvalues/HessenbergDecomposition.h" +#include "src/Eigenvalues/ComplexSchur.h" +#include "src/Eigenvalues/ComplexEigenSolver.h" +#include "src/Eigenvalues/RealQZ.h" +#include "src/Eigenvalues/GeneralizedEigenSolver.h" +#include "src/Eigenvalues/MatrixBaseEigenvalues.h" +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +#include "mkl_lapacke.h" +#else +#include "src/misc/lapacke.h" +#endif +#include "src/Eigenvalues/RealSchur_LAPACKE.h" +#include "src/Eigenvalues/ComplexSchur_LAPACKE.h" +#include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_EIGENVALUES_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/Geometry b/examples/ThirdPartyLibs/Eigen/Geometry new file mode 100644 index 000000000..131a4edfc --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Geometry @@ -0,0 +1,61 @@ +// 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_GEOMETRY_MODULE_H +#define EIGEN_GEOMETRY_MODULE_H + +#include "Core" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include "SVD" +#include "LU" +#include + +/** \defgroup Geometry_Module Geometry module + * + * This module provides support for: + * - fixed-size homogeneous transformations + * - translation, scaling, 2D and 3D rotations + * - \link Quaternion quaternions \endlink + * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) + * - orthognal vector generation (\ref MatrixBase::unitOrthogonal) + * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink + * - \link AlignedBox axis aligned bounding boxes \endlink + * - \link umeyama least-square transformation fitting \endlink + * + * \code + * #include + * \endcode + */ + +#include "src/Geometry/OrthoMethods.h" +#include "src/Geometry/EulerAngles.h" + +#include "src/Geometry/Homogeneous.h" +#include "src/Geometry/RotationBase.h" +#include "src/Geometry/Rotation2D.h" +#include "src/Geometry/Quaternion.h" +#include "src/Geometry/AngleAxis.h" +#include "src/Geometry/Transform.h" +#include "src/Geometry/Translation.h" +#include "src/Geometry/Scaling.h" +#include "src/Geometry/Hyperplane.h" +#include "src/Geometry/ParametrizedLine.h" +#include "src/Geometry/AlignedBox.h" +#include "src/Geometry/Umeyama.h" + +// Use the SSE optimized version whenever possible. At the moment the +// SSE version doesn't compile when AVX is enabled +#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX +#include "src/Geometry/arch/Geometry_SSE.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_GEOMETRY_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/Householder b/examples/ThirdPartyLibs/Eigen/Householder new file mode 100644 index 000000000..89cd81b1a --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Householder @@ -0,0 +1,30 @@ +// 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 + */ + +#include "src/Householder/Householder.h" +#include "src/Householder/HouseholderSequence.h" +#include "src/Householder/BlockHouseholder.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_HOUSEHOLDER_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/IterativeLinearSolvers b/examples/ThirdPartyLibs/Eigen/IterativeLinearSolvers new file mode 100644 index 000000000..957d5750b --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/IterativeLinearSolvers @@ -0,0 +1,48 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H +#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H + +#include "SparseCore" +#include "OrderingMethods" + +#include "src/Core/util/DisableStupidWarnings.h" + +/** + * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module + * + * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. + * Those solvers are accessible via the following classes: + * - ConjugateGradient for selfadjoint (hermitian) matrices, + * - LeastSquaresConjugateGradient for rectangular least-square problems, + * - BiCGSTAB for general square matrices. + * + * These iterative solvers are associated with some preconditioners: + * - IdentityPreconditioner - not really useful + * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. + * - IncompleteLUT - incomplete LU factorization with dual thresholding + * + * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. + * + \code + #include + \endcode + */ + +#include "src/IterativeLinearSolvers/SolveWithGuess.h" +#include "src/IterativeLinearSolvers/IterativeSolverBase.h" +#include "src/IterativeLinearSolvers/BasicPreconditioners.h" +#include "src/IterativeLinearSolvers/ConjugateGradient.h" +#include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" +#include "src/IterativeLinearSolvers/BiCGSTAB.h" +#include "src/IterativeLinearSolvers/IncompleteLUT.h" +#include "src/IterativeLinearSolvers/IncompleteCholesky.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/Jacobi b/examples/ThirdPartyLibs/Eigen/Jacobi new file mode 100644 index 000000000..17c1d785a --- /dev/null +++ b/examples/ThirdPartyLibs/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(). + */ + +#include "src/Jacobi/Jacobi.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_JACOBI_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ + diff --git a/examples/ThirdPartyLibs/Eigen/LU b/examples/ThirdPartyLibs/Eigen/LU new file mode 100644 index 000000000..6418a86e1 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/LU @@ -0,0 +1,50 @@ +// 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" +#include "src/LU/FullPivLU.h" +#include "src/LU/PartialPivLU.h" +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +#include "mkl_lapacke.h" +#else +#include "src/misc/lapacke.h" +#endif +#include "src/LU/PartialPivLU_LAPACKE.h" +#endif +#include "src/LU/Determinant.h" +#include "src/LU/InverseImpl.h" + +// Use the SSE optimized version whenever possible. At the moment the +// SSE version doesn't compile when AVX is enabled +#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX + #include "src/LU/arch/Inverse_SSE.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_LU_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/MetisSupport b/examples/ThirdPartyLibs/Eigen/MetisSupport new file mode 100644 index 000000000..85c41bf34 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/MetisSupport @@ -0,0 +1,35 @@ +// 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_METISSUPPORT_MODULE_H +#define EIGEN_METISSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +extern "C" { +#include +} + + +/** \ingroup Support_modules + * \defgroup MetisSupport_Module MetisSupport module + * + * \code + * #include + * \endcode + * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). + * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink + */ + + +#include "src/MetisSupport/MetisSupport.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_METISSUPPORT_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/OrderingMethods b/examples/ThirdPartyLibs/Eigen/OrderingMethods new file mode 100644 index 000000000..d8ea36193 --- /dev/null +++ b/examples/ThirdPartyLibs/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 + */ + +#ifndef EIGEN_MPL2_ONLY +#include "src/OrderingMethods/Amd.h" +#endif + +#include "src/OrderingMethods/Ordering.h" +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_ORDERINGMETHODS_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/PaStiXSupport b/examples/ThirdPartyLibs/Eigen/PaStiXSupport new file mode 100644 index 000000000..de3a63b4d --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/PaStiXSupport @@ -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_PASTIXSUPPORT_MODULE_H +#define EIGEN_PASTIXSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +extern "C" { +#include +#include +} + +#ifdef complex +#undef complex +#endif + +/** \ingroup Support_modules + * \defgroup PaStiXSupport_Module PaStiXSupport module + * + * This module provides an interface to the PaSTiX library. + * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver. + * It provides the two following main factorization classes: + * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization. + * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization. + * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern). + * + * \code + * #include + * \endcode + * + * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies. + * The dependencies depend on how PaSTiX has been compiled. + * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task. + * + */ + +#include "src/PaStiXSupport/PaStiXSupport.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_PASTIXSUPPORT_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/PardisoSupport b/examples/ThirdPartyLibs/Eigen/PardisoSupport new file mode 100644 index 000000000..340edf51f --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/PardisoSupport @@ -0,0 +1,35 @@ +// 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_PARDISOSUPPORT_MODULE_H +#define EIGEN_PARDISOSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include + +/** \ingroup Support_modules + * \defgroup PardisoSupport_Module PardisoSupport module + * + * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers. + * + * \code + * #include + * \endcode + * + * In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies. + * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration. + * + */ + +#include "src/PardisoSupport/PardisoSupport.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_PARDISOSUPPORT_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/QR b/examples/ThirdPartyLibs/Eigen/QR new file mode 100644 index 000000000..c7e914469 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/QR @@ -0,0 +1,51 @@ +// 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 "src/Core/util/DisableStupidWarnings.h" + +#include "Cholesky" +#include "Jacobi" +#include "Householder" + +/** \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 + */ + +#include "src/QR/HouseholderQR.h" +#include "src/QR/FullPivHouseholderQR.h" +#include "src/QR/ColPivHouseholderQR.h" +#include "src/QR/CompleteOrthogonalDecomposition.h" +#ifdef EIGEN_USE_LAPACKE +#ifdef EIGEN_USE_MKL +#include "mkl_lapacke.h" +#else +#include "src/misc/lapacke.h" +#endif +#include "src/QR/HouseholderQR_LAPACKE.h" +#include "src/QR/ColPivHouseholderQR_LAPACKE.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_QR_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/QtAlignedMalloc b/examples/ThirdPartyLibs/Eigen/QtAlignedMalloc new file mode 100644 index 000000000..4f07df02a --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/QtAlignedMalloc @@ -0,0 +1,40 @@ +// 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_QTMALLOC_MODULE_H +#define EIGEN_QTMALLOC_MODULE_H + +#include "Core" + +#if (!EIGEN_MALLOC_ALREADY_ALIGNED) + +#include "src/Core/util/DisableStupidWarnings.h" + +void *qMalloc(std::size_t size) +{ + return Eigen::internal::aligned_malloc(size); +} + +void qFree(void *ptr) +{ + Eigen::internal::aligned_free(ptr); +} + +void *qRealloc(void *ptr, std::size_t size) +{ + void* newPtr = Eigen::internal::aligned_malloc(size); + std::memcpy(newPtr, ptr, size); + Eigen::internal::aligned_free(ptr); + return newPtr; +} + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif + +#endif // EIGEN_QTMALLOC_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/SPQRSupport b/examples/ThirdPartyLibs/Eigen/SPQRSupport new file mode 100644 index 000000000..f70390c17 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/SPQRSupport @@ -0,0 +1,34 @@ +// 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_SPQRSUPPORT_MODULE_H +#define EIGEN_SPQRSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include "SuiteSparseQR.hpp" + +/** \ingroup Support_modules + * \defgroup SPQRSupport_Module SuiteSparseQR module + * + * This module provides an interface to the SPQR library, which is part of the suitesparse package. + * + * \code + * #include + * \endcode + * + * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...). + * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules + * + */ + +#include "src/CholmodSupport/CholmodSupport.h" +#include "src/SPQRSupport/SuiteSparseQRSupport.h" + +#endif diff --git a/examples/ThirdPartyLibs/Eigen/SVD b/examples/ThirdPartyLibs/Eigen/SVD new file mode 100644 index 000000000..5d0e75f7f --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/SVD @@ -0,0 +1,51 @@ +// 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 + */ + +#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" +#if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) +#ifdef EIGEN_USE_MKL +#include "mkl_lapacke.h" +#else +#include "src/misc/lapacke.h" +#endif +#include "src/SVD/JacobiSVD_LAPACKE.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SVD_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/examples/ThirdPartyLibs/Eigen/Sparse b/examples/ThirdPartyLibs/Eigen/Sparse new file mode 100644 index 000000000..136e681a1 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/Sparse @@ -0,0 +1,36 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SPARSE_MODULE_H +#define EIGEN_SPARSE_MODULE_H + +/** \defgroup Sparse_Module Sparse meta-module + * + * Meta-module including all related modules: + * - \ref SparseCore_Module + * - \ref OrderingMethods_Module + * - \ref SparseCholesky_Module + * - \ref SparseLU_Module + * - \ref SparseQR_Module + * - \ref IterativeLinearSolvers_Module + * + \code + #include + \endcode + */ + +#include "SparseCore" +#include "OrderingMethods" +#ifndef EIGEN_MPL2_ONLY +#include "SparseCholesky" +#endif +#include "SparseLU" +#include "SparseQR" +#include "IterativeLinearSolvers" + +#endif // EIGEN_SPARSE_MODULE_H + diff --git a/examples/ThirdPartyLibs/Eigen/SparseCholesky b/examples/ThirdPartyLibs/Eigen/SparseCholesky new file mode 100644 index 000000000..b6a320c40 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/SparseCholesky @@ -0,0 +1,45 @@ +// 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 + */ + +#ifdef EIGEN_MPL2_ONLY +#error The SparseCholesky module has nothing to offer in MPL2 only mode +#endif + +#include "src/SparseCholesky/SimplicialCholesky.h" + +#ifndef EIGEN_MPL2_ONLY +#include "src/SparseCholesky/SimplicialCholesky_impl.h" +#endif + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSECHOLESKY_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/SparseCore b/examples/ThirdPartyLibs/Eigen/SparseCore new file mode 100644 index 000000000..76966c4c4 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/SparseCore @@ -0,0 +1,69 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SPARSECORE_MODULE_H +#define EIGEN_SPARSECORE_MODULE_H + +#include "Core" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include +#include +#include +#include +#include + +/** + * \defgroup SparseCore_Module SparseCore module + * + * This module provides a sparse matrix representation, and basic associated matrix manipulations + * and operations. + * + * See the \ref TutorialSparse "Sparse tutorial" + * + * \code + * #include + * \endcode + * + * This module depends on: Core. + */ + +#include "src/SparseCore/SparseUtil.h" +#include "src/SparseCore/SparseMatrixBase.h" +#include "src/SparseCore/SparseAssign.h" +#include "src/SparseCore/CompressedStorage.h" +#include "src/SparseCore/AmbiVector.h" +#include "src/SparseCore/SparseCompressedBase.h" +#include "src/SparseCore/SparseMatrix.h" +#include "src/SparseCore/SparseMap.h" +#include "src/SparseCore/MappedSparseMatrix.h" +#include "src/SparseCore/SparseVector.h" +#include "src/SparseCore/SparseRef.h" +#include "src/SparseCore/SparseCwiseUnaryOp.h" +#include "src/SparseCore/SparseCwiseBinaryOp.h" +#include "src/SparseCore/SparseTranspose.h" +#include "src/SparseCore/SparseBlock.h" +#include "src/SparseCore/SparseDot.h" +#include "src/SparseCore/SparseRedux.h" +#include "src/SparseCore/SparseView.h" +#include "src/SparseCore/SparseDiagonalProduct.h" +#include "src/SparseCore/ConservativeSparseSparseProduct.h" +#include "src/SparseCore/SparseSparseProductWithPruning.h" +#include "src/SparseCore/SparseProduct.h" +#include "src/SparseCore/SparseDenseProduct.h" +#include "src/SparseCore/SparseSelfAdjointView.h" +#include "src/SparseCore/SparseTriangularView.h" +#include "src/SparseCore/TriangularSolver.h" +#include "src/SparseCore/SparsePermutation.h" +#include "src/SparseCore/SparseFuzzy.h" +#include "src/SparseCore/SparseSolverBase.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSECORE_MODULE_H + diff --git a/examples/ThirdPartyLibs/Eigen/SparseLU b/examples/ThirdPartyLibs/Eigen/SparseLU new file mode 100644 index 000000000..38b38b531 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/SparseLU @@ -0,0 +1,46 @@ +// 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/SparseLU/SparseLU_gemm_kernel.h" + +#include "src/SparseLU/SparseLU_Structs.h" +#include "src/SparseLU/SparseLU_SupernodalMatrix.h" +#include "src/SparseLU/SparseLUImpl.h" +#include "src/SparseCore/SparseColEtree.h" +#include "src/SparseLU/SparseLU_Memory.h" +#include "src/SparseLU/SparseLU_heap_relax_snode.h" +#include "src/SparseLU/SparseLU_relax_snode.h" +#include "src/SparseLU/SparseLU_pivotL.h" +#include "src/SparseLU/SparseLU_panel_dfs.h" +#include "src/SparseLU/SparseLU_kernel_bmod.h" +#include "src/SparseLU/SparseLU_panel_bmod.h" +#include "src/SparseLU/SparseLU_column_dfs.h" +#include "src/SparseLU/SparseLU_column_bmod.h" +#include "src/SparseLU/SparseLU_copy_to_ucol.h" +#include "src/SparseLU/SparseLU_pruneL.h" +#include "src/SparseLU/SparseLU_Utils.h" +#include "src/SparseLU/SparseLU.h" + +#endif // EIGEN_SPARSELU_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/SparseQR b/examples/ThirdPartyLibs/Eigen/SparseQR new file mode 100644 index 000000000..a6f3b7f7d --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/SparseQR @@ -0,0 +1,37 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SPARSEQR_MODULE_H +#define EIGEN_SPARSEQR_MODULE_H + +#include "SparseCore" +#include "OrderingMethods" +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup SparseQR_Module SparseQR module + * \brief Provides QR decomposition for sparse matrices + * + * This module provides a simplicial version of the left-looking Sparse QR decomposition. + * The columns of the input matrix should be reordered to limit the fill-in during the + * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. + * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list + * of built-in and external ordering methods. + * + * \code + * #include + * \endcode + * + * + */ + +#include "OrderingMethods" +#include "src/SparseCore/SparseColEtree.h" +#include "src/SparseQR/SparseQR.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif diff --git a/examples/ThirdPartyLibs/Eigen/StdDeque b/examples/ThirdPartyLibs/Eigen/StdDeque new file mode 100644 index 000000000..bc68397be --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/StdDeque @@ -0,0 +1,27 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud +// Copyright (C) 2009 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_STDDEQUE_MODULE_H +#define EIGEN_STDDEQUE_MODULE_H + +#include "Core" +#include + +#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ + +#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) + +#else + +#include "src/StlSupport/StdDeque.h" + +#endif + +#endif // EIGEN_STDDEQUE_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/StdList b/examples/ThirdPartyLibs/Eigen/StdList new file mode 100644 index 000000000..4c6262c08 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/StdList @@ -0,0 +1,26 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 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_STDLIST_MODULE_H +#define EIGEN_STDLIST_MODULE_H + +#include "Core" +#include + +#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ + +#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) + +#else + +#include "src/StlSupport/StdList.h" + +#endif + +#endif // EIGEN_STDLIST_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/StdVector b/examples/ThirdPartyLibs/Eigen/StdVector new file mode 100644 index 000000000..0c4697ad5 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/StdVector @@ -0,0 +1,27 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud +// Copyright (C) 2009 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_STDVECTOR_MODULE_H +#define EIGEN_STDVECTOR_MODULE_H + +#include "Core" +#include + +#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ + +#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) + +#else + +#include "src/StlSupport/StdVector.h" + +#endif + +#endif // EIGEN_STDVECTOR_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/SuperLUSupport b/examples/ThirdPartyLibs/Eigen/SuperLUSupport new file mode 100644 index 000000000..59312a82d --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/SuperLUSupport @@ -0,0 +1,64 @@ +// 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_SUPERLUSUPPORT_MODULE_H +#define EIGEN_SUPERLUSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +#ifdef EMPTY +#define EIGEN_EMPTY_WAS_ALREADY_DEFINED +#endif + +typedef int int_t; +#include +#include +#include + +// slu_util.h defines a preprocessor token named EMPTY which is really polluting, +// so we remove it in favor of a SUPERLU_EMPTY token. +// If EMPTY was already defined then we don't undef it. + +#if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED) +# undef EIGEN_EMPTY_WAS_ALREADY_DEFINED +#elif defined(EMPTY) +# undef EMPTY +#endif + +#define SUPERLU_EMPTY (-1) + +namespace Eigen { struct SluMatrix; } + +/** \ingroup Support_modules + * \defgroup SuperLUSupport_Module SuperLUSupport module + * + * This module provides an interface to the SuperLU library. + * It provides the following factorization class: + * - class SuperLU: a supernodal sequential LU factorization. + * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). + * + * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported. + * + * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. + * + * \code + * #include + * \endcode + * + * In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies. + * The dependencies depend on how superlu has been compiled. + * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task. + * + */ + +#include "src/SuperLUSupport/SuperLUSupport.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SUPERLUSUPPORT_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/UmfPackSupport b/examples/ThirdPartyLibs/Eigen/UmfPackSupport new file mode 100644 index 000000000..00eec8087 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/UmfPackSupport @@ -0,0 +1,40 @@ +// 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_UMFPACKSUPPORT_MODULE_H +#define EIGEN_UMFPACKSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +extern "C" { +#include +} + +/** \ingroup Support_modules + * \defgroup UmfPackSupport_Module UmfPackSupport module + * + * This module provides an interface to the UmfPack library which is part of the suitesparse package. + * It provides the following factorization class: + * - class UmfPackLU: a multifrontal sequential LU factorization. + * + * \code + * #include + * \endcode + * + * In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies. + * The dependencies depend on how umfpack has been compiled. + * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task. + * + */ + +#include "src/UmfPackSupport/UmfPackSupport.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_UMFPACKSUPPORT_MODULE_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Cholesky/LDLT.h b/examples/ThirdPartyLibs/Eigen/src/Cholesky/LDLT.h new file mode 100644 index 000000000..968427b3a --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Cholesky/LDLT.h @@ -0,0 +1,668 @@ +// 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 + +namespace Eigen { + +namespace internal { + template struct LDLT_Traits; + + // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef + enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite }; +} + +/** \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 decompositon: 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 L 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: + typedef _MatrixType MatrixType; + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, + UpLo = _UpLo + }; + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + typedef typename MatrixType::StorageIndex StorageIndex; + 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; + } + + /** \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$ is \f$ A \f$ is singular. + * + * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt() + */ + template + inline const Solve + solve(const MatrixBase& b) const + { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + eigen_assert(m_matrix.rows()==b.rows() + && "LDLT::solve(): invalid number of rows of the right hand side matrix b"); + return Solve(*this, b.derived()); + } + + 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; }; + + inline Index rows() const { return m_matrix.rows(); } + inline Index cols() const { return m_matrix.cols(); } + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was succesful, + * \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; + #endif + + protected: + + static void check_template_parameters() + { + 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 (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::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; j0) && pivot_is_valid) + A21 /= realAkk; + + 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(gamma != 0) + 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) +{ + check_template_parameters(); + + 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<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const +{ + eigen_assert(rhs.rows() == rows()); + // dst = P b + dst = m_transpositions * rhs; + + // dst = L^-1 (P b) + matrixL().solveInPlace(dst); + + // dst = D^-1 (L^-1 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 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. + RealScalar tolerance = RealScalar(1) / NumTraits::highest(); + + 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^-T (D^-1 L^-1 P b) + matrixU().solveInPlace(dst); + + // dst = P^-1 (L^-T D^-1 L^-1 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/examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT.h b/examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT.h new file mode 100644 index 000000000..814174d47 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT.h @@ -0,0 +1,541 @@ +// 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 + +namespace Eigen { + +namespace internal{ +template struct LLT_Traits; +} + +/** \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 decompositon: 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: + typedef _MatrixType MatrixType; + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + typedef typename MatrixType::StorageIndex StorageIndex; + + 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 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 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); + } + + /** \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 + { + eigen_assert(m_isInitialized && "LLT is not initialized."); + eigen_assert(m_matrix.rows()==b.rows() + && "LLT::solve(): invalid number of rows of the right hand side matrix b"); + return Solve(*this, b.derived()); + } + + 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 succesful, + * \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 { return *this; }; + + inline Index rows() const { return m_matrix.rows(); } + inline Index cols() const { 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; + #endif + + protected: + + static void check_template_parameters() + { + 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 typename internal::remove_all::type 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 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 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 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) +{ + check_template_parameters(); + + 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<_MatrixType,_UpLo> LLT<_MatrixType,_UpLo>::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<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const +{ + dst = rhs; + 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/examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT_LAPACKE.h b/examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT_LAPACKE.h new file mode 100644 index 000000000..bc6489e69 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT_LAPACKE.h @@ -0,0 +1,99 @@ +/* + Copyright (c) 2011, Intel Corporation. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of Intel Corporation nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + ******************************************************************************** + * Content : Eigen bindings to LAPACKe + * LLt decomposition based on LAPACKE_?potrf function. + ******************************************************************************** +*/ + +#ifndef EIGEN_LLT_LAPACKE_H +#define EIGEN_LLT_LAPACKE_H + +namespace Eigen { + +namespace internal { + +template struct lapacke_llt; + +#define EIGEN_LAPACKE_LLT(EIGTYPE, BLASTYPE, LAPACKE_PREFIX) \ +template<> struct lapacke_llt \ +{ \ + template \ + static inline Index potrf(MatrixType& m, char uplo) \ + { \ + lapack_int matrix_order; \ + lapack_int size, lda, info, StorageOrder; \ + EIGTYPE* a; \ + eigen_assert(m.rows()==m.cols()); \ + /* Set up parameters for ?potrf */ \ + size = convert_index(m.rows()); \ + StorageOrder = MatrixType::Flags&RowMajorBit?RowMajor:ColMajor; \ + matrix_order = StorageOrder==RowMajor ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ + a = &(m.coeffRef(0,0)); \ + lda = convert_index(m.outerStride()); \ +\ + info = LAPACKE_##LAPACKE_PREFIX##potrf( matrix_order, uplo, size, (BLASTYPE*)a, lda ); \ + info = (info==0) ? -1 : info>0 ? info-1 : size; \ + return info; \ + } \ +}; \ +template<> struct llt_inplace \ +{ \ + template \ + static Index blocked(MatrixType& m) \ + { \ + return lapacke_llt::potrf(m, 'L'); \ + } \ + template \ + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \ + { return Eigen::internal::llt_rank_update_lower(mat, vec, sigma); } \ +}; \ +template<> struct llt_inplace \ +{ \ + template \ + static Index blocked(MatrixType& m) \ + { \ + return lapacke_llt::potrf(m, 'U'); \ + } \ + template \ + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \ + { \ + Transpose matt(mat); \ + return llt_inplace::rankUpdate(matt, vec.conjugate(), sigma); \ + } \ +}; + +EIGEN_LAPACKE_LLT(double, double, d) +EIGEN_LAPACKE_LLT(float, float, s) +EIGEN_LAPACKE_LLT(dcomplex, lapack_complex_double, z) +EIGEN_LAPACKE_LLT(scomplex, lapack_complex_float, c) + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_LLT_LAPACKE_H diff --git a/examples/ThirdPartyLibs/Eigen/src/CholmodSupport/CholmodSupport.h b/examples/ThirdPartyLibs/Eigen/src/CholmodSupport/CholmodSupport.h new file mode 100644 index 000000000..dc199ece6 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/CholmodSupport/CholmodSupport.h @@ -0,0 +1,682 @@ +// 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_CHOLMODSUPPORT_H +#define EIGEN_CHOLMODSUPPORT_H + +namespace Eigen { + +namespace internal { + +template struct cholmod_configure_matrix; + +template<> struct cholmod_configure_matrix { + template + static void run(CholmodType& mat) { + mat.xtype = CHOLMOD_REAL; + mat.dtype = CHOLMOD_DOUBLE; + } +}; + +template<> struct cholmod_configure_matrix > { + template + static void run(CholmodType& mat) { + mat.xtype = CHOLMOD_COMPLEX; + mat.dtype = CHOLMOD_DOUBLE; + } +}; + +// Other scalar types are not yet supported by Cholmod +// template<> struct cholmod_configure_matrix { +// template +// static void run(CholmodType& mat) { +// mat.xtype = CHOLMOD_REAL; +// mat.dtype = CHOLMOD_SINGLE; +// } +// }; +// +// template<> struct cholmod_configure_matrix > { +// template +// static void run(CholmodType& mat) { +// mat.xtype = CHOLMOD_COMPLEX; +// mat.dtype = CHOLMOD_SINGLE; +// } +// }; + +} // namespace internal + +/** Wraps the Eigen sparse matrix \a mat into a Cholmod sparse matrix object. + * Note that the data are shared. + */ +template +cholmod_sparse viewAsCholmod(Ref > mat) +{ + cholmod_sparse res; + res.nzmax = mat.nonZeros(); + res.nrow = mat.rows(); + res.ncol = mat.cols(); + res.p = mat.outerIndexPtr(); + res.i = mat.innerIndexPtr(); + res.x = mat.valuePtr(); + res.z = 0; + res.sorted = 1; + if(mat.isCompressed()) + { + res.packed = 1; + res.nz = 0; + } + else + { + res.packed = 0; + res.nz = mat.innerNonZeroPtr(); + } + + res.dtype = 0; + res.stype = -1; + + if (internal::is_same<_StorageIndex,int>::value) + { + res.itype = CHOLMOD_INT; + } + else if (internal::is_same<_StorageIndex,long>::value) + { + res.itype = CHOLMOD_LONG; + } + else + { + eigen_assert(false && "Index type not supported yet"); + } + + // setup res.xtype + internal::cholmod_configure_matrix<_Scalar>::run(res); + + res.stype = 0; + + return res; +} + +template +const cholmod_sparse viewAsCholmod(const SparseMatrix<_Scalar,_Options,_Index>& mat) +{ + cholmod_sparse res = viewAsCholmod(Ref >(mat.const_cast_derived())); + return res; +} + +template +const cholmod_sparse viewAsCholmod(const SparseVector<_Scalar,_Options,_Index>& mat) +{ + cholmod_sparse res = viewAsCholmod(Ref >(mat.const_cast_derived())); + return res; +} + +/** Returns a view of the Eigen sparse matrix \a mat as Cholmod sparse matrix. + * The data are not copied but shared. */ +template +cholmod_sparse viewAsCholmod(const SparseSelfAdjointView, UpLo>& mat) +{ + cholmod_sparse res = viewAsCholmod(Ref >(mat.matrix().const_cast_derived())); + + if(UpLo==Upper) res.stype = 1; + if(UpLo==Lower) res.stype = -1; + // swap stype for rowmajor matrices (only works for real matrices) + EIGEN_STATIC_ASSERT((_Options & RowMajorBit) == 0 || NumTraits<_Scalar>::IsComplex == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); + if(_Options & RowMajorBit) res.stype *=-1; + + return res; +} + +/** Returns a view of the Eigen \b dense matrix \a mat as Cholmod dense matrix. + * The data are not copied but shared. */ +template +cholmod_dense viewAsCholmod(MatrixBase& mat) +{ + EIGEN_STATIC_ASSERT((internal::traits::Flags&RowMajorBit)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); + typedef typename Derived::Scalar Scalar; + + cholmod_dense res; + res.nrow = mat.rows(); + res.ncol = mat.cols(); + res.nzmax = res.nrow * res.ncol; + res.d = Derived::IsVectorAtCompileTime ? mat.derived().size() : mat.derived().outerStride(); + res.x = (void*)(mat.derived().data()); + res.z = 0; + + internal::cholmod_configure_matrix::run(res); + + return res; +} + +/** Returns a view of the Cholmod sparse matrix \a cm as an Eigen sparse matrix. + * The data are not copied but shared. */ +template +MappedSparseMatrix viewAsEigen(cholmod_sparse& cm) +{ + return MappedSparseMatrix + (cm.nrow, cm.ncol, static_cast(cm.p)[cm.ncol], + static_cast(cm.p), static_cast(cm.i),static_cast(cm.x) ); +} + +namespace internal { + +// template specializations for int and long that call the correct cholmod method + +#define EIGEN_CHOLMOD_SPECIALIZE0(ret, name) \ + template inline ret cm_ ## name (cholmod_common &Common) { return cholmod_ ## name (&Common); } \ + template<> inline ret cm_ ## name (cholmod_common &Common) { return cholmod_l_ ## name (&Common); } + +#define EIGEN_CHOLMOD_SPECIALIZE1(ret, name, t1, a1) \ + template inline ret cm_ ## name (t1& a1, cholmod_common &Common) { return cholmod_ ## name (&a1, &Common); } \ + template<> inline ret cm_ ## name (t1& a1, cholmod_common &Common) { return cholmod_l_ ## name (&a1, &Common); } + +EIGEN_CHOLMOD_SPECIALIZE0(int, start) +EIGEN_CHOLMOD_SPECIALIZE0(int, finish) + +EIGEN_CHOLMOD_SPECIALIZE1(int, free_factor, cholmod_factor*, L) +EIGEN_CHOLMOD_SPECIALIZE1(int, free_dense, cholmod_dense*, X) +EIGEN_CHOLMOD_SPECIALIZE1(int, free_sparse, cholmod_sparse*, A) + +EIGEN_CHOLMOD_SPECIALIZE1(cholmod_factor*, analyze, cholmod_sparse, A) + +template inline cholmod_dense* cm_solve (int sys, cholmod_factor& L, cholmod_dense& B, cholmod_common &Common) { return cholmod_solve (sys, &L, &B, &Common); } +template<> inline cholmod_dense* cm_solve (int sys, cholmod_factor& L, cholmod_dense& B, cholmod_common &Common) { return cholmod_l_solve (sys, &L, &B, &Common); } + +template inline cholmod_sparse* cm_spsolve (int sys, cholmod_factor& L, cholmod_sparse& B, cholmod_common &Common) { return cholmod_spsolve (sys, &L, &B, &Common); } +template<> inline cholmod_sparse* cm_spsolve (int sys, cholmod_factor& L, cholmod_sparse& B, cholmod_common &Common) { return cholmod_l_spsolve (sys, &L, &B, &Common); } + +template +inline int cm_factorize_p (cholmod_sparse* A, double beta[2], _StorageIndex* fset, std::size_t fsize, cholmod_factor* L, cholmod_common &Common) { return cholmod_factorize_p (A, beta, fset, fsize, L, &Common); } +template<> +inline int cm_factorize_p (cholmod_sparse* A, double beta[2], long* fset, std::size_t fsize, cholmod_factor* L, cholmod_common &Common) { return cholmod_l_factorize_p (A, beta, fset, fsize, L, &Common); } + +#undef EIGEN_CHOLMOD_SPECIALIZE0 +#undef EIGEN_CHOLMOD_SPECIALIZE1 + +} // namespace internal + + +enum CholmodMode { + CholmodAuto, CholmodSimplicialLLt, CholmodSupernodalLLt, CholmodLDLt +}; + + +/** \ingroup CholmodSupport_Module + * \class CholmodBase + * \brief The base class for the direct Cholesky factorization of Cholmod + * \sa class CholmodSupernodalLLT, class CholmodSimplicialLDLT, class CholmodSimplicialLLT + */ +template +class CholmodBase : public SparseSolverBase +{ + protected: + typedef SparseSolverBase Base; + using Base::derived; + using Base::m_isInitialized; + public: + typedef _MatrixType MatrixType; + enum { UpLo = _UpLo }; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef MatrixType CholMatrixType; + typedef typename MatrixType::StorageIndex StorageIndex; + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; + + public: + + CholmodBase() + : m_cholmodFactor(0), m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false) + { + EIGEN_STATIC_ASSERT((internal::is_same::value), CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY); + m_shiftOffset[0] = m_shiftOffset[1] = 0.0; + internal::cm_start(m_cholmod); + } + + explicit CholmodBase(const MatrixType& matrix) + : m_cholmodFactor(0), m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false) + { + EIGEN_STATIC_ASSERT((internal::is_same::value), CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY); + m_shiftOffset[0] = m_shiftOffset[1] = 0.0; + internal::cm_start(m_cholmod); + compute(matrix); + } + + ~CholmodBase() + { + if(m_cholmodFactor) + internal::cm_free_factor(m_cholmodFactor, m_cholmod); + internal::cm_finish(m_cholmod); + } + + inline StorageIndex cols() const { return internal::convert_index(m_cholmodFactor->n); } + inline StorageIndex rows() const { return internal::convert_index(m_cholmodFactor->n); } + + /** \brief Reports whether previous computation was successful. + * + * \returns \c Success if computation was successful, + * \c NumericalIssue if the matrix.appears to be negative. + */ + ComputationInfo info() const + { + eigen_assert(m_isInitialized && "Decomposition is not initialized."); + return m_info; + } + + /** Computes the sparse Cholesky decomposition of \a matrix */ + Derived& compute(const MatrixType& matrix) + { + analyzePattern(matrix); + factorize(matrix); + return derived(); + } + + /** Performs a symbolic decomposition on the sparsity pattern of \a matrix. + * + * This function is particularly useful when solving for several problems having the same structure. + * + * \sa factorize() + */ + void analyzePattern(const MatrixType& matrix) + { + if(m_cholmodFactor) + { + internal::cm_free_factor(m_cholmodFactor, m_cholmod); + m_cholmodFactor = 0; + } + cholmod_sparse A = viewAsCholmod(matrix.template selfadjointView()); + m_cholmodFactor = internal::cm_analyze(A, m_cholmod); + + this->m_isInitialized = true; + this->m_info = Success; + m_analysisIsOk = true; + m_factorizationIsOk = false; + } + + /** Performs a numeric decomposition of \a matrix + * + * The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed. + * + * \sa analyzePattern() + */ + void factorize(const MatrixType& matrix) + { + eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); + cholmod_sparse A = viewAsCholmod(matrix.template selfadjointView()); + internal::cm_factorize_p(&A, m_shiftOffset, 0, 0, m_cholmodFactor, m_cholmod); + + // If the factorization failed, minor is the column at which it did. On success minor == n. + this->m_info = (m_cholmodFactor->minor == m_cholmodFactor->n ? Success : NumericalIssue); + m_factorizationIsOk = true; + } + + /** Returns a reference to the Cholmod's configuration structure to get a full control over the performed operations. + * See the Cholmod user guide for details. */ + cholmod_common& cholmod() { return m_cholmod; } + + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal */ + template + void _solve_impl(const MatrixBase &b, MatrixBase &dest) const + { + eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); + const Index size = m_cholmodFactor->n; + EIGEN_UNUSED_VARIABLE(size); + eigen_assert(size==b.rows()); + + // Cholmod needs column-major storage without inner-stride, which corresponds to the default behavior of Ref. + Ref > b_ref(b.derived()); + + cholmod_dense b_cd = viewAsCholmod(b_ref); + cholmod_dense* x_cd = internal::cm_solve(CHOLMOD_A, *m_cholmodFactor, b_cd, m_cholmod); + if(!x_cd) + { + this->m_info = NumericalIssue; + return; + } + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) + // NOTE Actually, the copy can be avoided by calling cholmod_solve2 instead of cholmod_solve + dest = Matrix::Map(reinterpret_cast(x_cd->x),b.rows(),b.cols()); + internal::cm_free_dense(x_cd, m_cholmod); + } + + /** \internal */ + template + void _solve_impl(const SparseMatrixBase &b, SparseMatrixBase &dest) const + { + eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); + const Index size = m_cholmodFactor->n; + EIGEN_UNUSED_VARIABLE(size); + eigen_assert(size==b.rows()); + + // note: cs stands for Cholmod Sparse + Ref > b_ref(b.const_cast_derived()); + cholmod_sparse b_cs = viewAsCholmod(b_ref); + cholmod_sparse* x_cs = internal::cm_spsolve(CHOLMOD_A, *m_cholmodFactor, b_cs, m_cholmod); + if(!x_cs) + { + this->m_info = NumericalIssue; + return; + } + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) + // NOTE cholmod_spsolve in fact just calls the dense solver for blocks of 4 columns at a time (similar to Eigen's sparse solver) + dest.derived() = viewAsEigen(*x_cs); + internal::cm_free_sparse(x_cs, m_cholmod); + } + #endif // EIGEN_PARSED_BY_DOXYGEN + + + /** Sets the shift parameter that will be used to adjust the diagonal coefficients during the numerical factorization. + * + * During the numerical factorization, an offset term is added to the diagonal coefficients:\n + * \c d_ii = \a offset + \c d_ii + * + * The default is \a offset=0. + * + * \returns a reference to \c *this. + */ + Derived& setShift(const RealScalar& offset) + { + m_shiftOffset[0] = double(offset); + return derived(); + } + + /** \returns the determinant of the underlying matrix from the current factorization */ + Scalar determinant() const + { + using std::exp; + return exp(logDeterminant()); + } + + /** \returns the log determinant of the underlying matrix from the current factorization */ + Scalar logDeterminant() const + { + using std::log; + using numext::real; + eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); + + RealScalar logDet = 0; + Scalar *x = static_cast(m_cholmodFactor->x); + if (m_cholmodFactor->is_super) + { + // Supernodal factorization stored as a packed list of dense column-major blocs, + // as described by the following structure: + + // super[k] == index of the first column of the j-th super node + StorageIndex *super = static_cast(m_cholmodFactor->super); + // pi[k] == offset to the description of row indices + StorageIndex *pi = static_cast(m_cholmodFactor->pi); + // px[k] == offset to the respective dense block + StorageIndex *px = static_cast(m_cholmodFactor->px); + + Index nb_super_nodes = m_cholmodFactor->nsuper; + for (Index k=0; k < nb_super_nodes; ++k) + { + StorageIndex ncols = super[k + 1] - super[k]; + StorageIndex nrows = pi[k + 1] - pi[k]; + + Map, 0, InnerStride<> > sk(x + px[k], ncols, InnerStride<>(nrows+1)); + logDet += sk.real().log().sum(); + } + } + else + { + // Simplicial factorization stored as standard CSC matrix. + StorageIndex *p = static_cast(m_cholmodFactor->p); + Index size = m_cholmodFactor->n; + for (Index k=0; kis_ll) + logDet *= 2.0; + return logDet; + }; + + template + void dumpMemory(Stream& /*s*/) + {} + + protected: + mutable cholmod_common m_cholmod; + cholmod_factor* m_cholmodFactor; + double m_shiftOffset[2]; + mutable ComputationInfo m_info; + int m_factorizationIsOk; + int m_analysisIsOk; +}; + +/** \ingroup CholmodSupport_Module + * \class CholmodSimplicialLLT + * \brief A simplicial direct Cholesky (LLT) factorization and solver based on Cholmod + * + * This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization + * using the Cholmod library. + * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices + * X and B can be either dense or sparse. + * + * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower + * or Upper. Default is Lower. + * + * \implsparsesolverconcept + * + * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. + * + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept, class CholmodSupernodalLLT, class SimplicialLLT + */ +template +class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLLT<_MatrixType, _UpLo> > +{ + typedef CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLLT> Base; + using Base::m_cholmod; + + public: + + typedef _MatrixType MatrixType; + + CholmodSimplicialLLT() : Base() { init(); } + + CholmodSimplicialLLT(const MatrixType& matrix) : Base() + { + init(); + this->compute(matrix); + } + + ~CholmodSimplicialLLT() {} + protected: + void init() + { + m_cholmod.final_asis = 0; + m_cholmod.supernodal = CHOLMOD_SIMPLICIAL; + m_cholmod.final_ll = 1; + } +}; + + +/** \ingroup CholmodSupport_Module + * \class CholmodSimplicialLDLT + * \brief A simplicial direct Cholesky (LDLT) factorization and solver based on Cholmod + * + * This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization + * using the Cholmod library. + * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices + * X and B can be either dense or sparse. + * + * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower + * or Upper. Default is Lower. + * + * \implsparsesolverconcept + * + * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. + * + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept, class CholmodSupernodalLLT, class SimplicialLDLT + */ +template +class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLDLT<_MatrixType, _UpLo> > +{ + typedef CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLDLT> Base; + using Base::m_cholmod; + + public: + + typedef _MatrixType MatrixType; + + CholmodSimplicialLDLT() : Base() { init(); } + + CholmodSimplicialLDLT(const MatrixType& matrix) : Base() + { + init(); + this->compute(matrix); + } + + ~CholmodSimplicialLDLT() {} + protected: + void init() + { + m_cholmod.final_asis = 1; + m_cholmod.supernodal = CHOLMOD_SIMPLICIAL; + } +}; + +/** \ingroup CholmodSupport_Module + * \class CholmodSupernodalLLT + * \brief A supernodal Cholesky (LLT) factorization and solver based on Cholmod + * + * This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization + * using the Cholmod library. + * This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices + * X and B can be either dense or sparse. + * + * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower + * or Upper. Default is Lower. + * + * \implsparsesolverconcept + * + * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. + * + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept + */ +template +class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSupernodalLLT<_MatrixType, _UpLo> > +{ + typedef CholmodBase<_MatrixType, _UpLo, CholmodSupernodalLLT> Base; + using Base::m_cholmod; + + public: + + typedef _MatrixType MatrixType; + + CholmodSupernodalLLT() : Base() { init(); } + + CholmodSupernodalLLT(const MatrixType& matrix) : Base() + { + init(); + this->compute(matrix); + } + + ~CholmodSupernodalLLT() {} + protected: + void init() + { + m_cholmod.final_asis = 1; + m_cholmod.supernodal = CHOLMOD_SUPERNODAL; + } +}; + +/** \ingroup CholmodSupport_Module + * \class CholmodDecomposition + * \brief A general Cholesky factorization and solver based on Cholmod + * + * This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization + * using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices + * X and B can be either dense or sparse. + * + * This variant permits to change the underlying Cholesky method at runtime. + * On the other hand, it does not provide access to the result of the factorization. + * The default is to let Cholmod automatically choose between a simplicial and supernodal factorization. + * + * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower + * or Upper. Default is Lower. + * + * \implsparsesolverconcept + * + * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. + * + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept + */ +template +class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecomposition<_MatrixType, _UpLo> > +{ + typedef CholmodBase<_MatrixType, _UpLo, CholmodDecomposition> Base; + using Base::m_cholmod; + + public: + + typedef _MatrixType MatrixType; + + CholmodDecomposition() : Base() { init(); } + + CholmodDecomposition(const MatrixType& matrix) : Base() + { + init(); + this->compute(matrix); + } + + ~CholmodDecomposition() {} + + void setMode(CholmodMode mode) + { + switch(mode) + { + case CholmodAuto: + m_cholmod.final_asis = 1; + m_cholmod.supernodal = CHOLMOD_AUTO; + break; + case CholmodSimplicialLLt: + m_cholmod.final_asis = 0; + m_cholmod.supernodal = CHOLMOD_SIMPLICIAL; + m_cholmod.final_ll = 1; + break; + case CholmodSupernodalLLt: + m_cholmod.final_asis = 1; + m_cholmod.supernodal = CHOLMOD_SUPERNODAL; + break; + case CholmodLDLt: + m_cholmod.final_asis = 1; + m_cholmod.supernodal = CHOLMOD_SIMPLICIAL; + break; + default: + break; + } + } + protected: + void init() + { + m_cholmod.final_asis = 1; + m_cholmod.supernodal = CHOLMOD_AUTO; + } +}; + +} // end namespace Eigen + +#endif // EIGEN_CHOLMODSUPPORT_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/ArithmeticSequence.h b/examples/ThirdPartyLibs/Eigen/src/Core/ArithmeticSequence.h new file mode 100644 index 000000000..ada1571f1 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/ArithmeticSequence.h @@ -0,0 +1,350 @@ +// 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 + +namespace Eigen { + +namespace internal { + +#if (!EIGEN_HAS_CXX11) || !((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48) +template struct aseq_negate {}; + +template<> struct aseq_negate { + typedef Index type; +}; + +template struct aseq_negate > { + typedef FixedInt<-N> type; +}; + +// Compilation error in the following case: +template<> struct aseq_negate > {}; + +template::value, + bool SizeIsSymbolic =Symbolic::is_symbolic::value> +struct aseq_reverse_first_type { + typedef Index type; +}; + +template +struct aseq_reverse_first_type { + typedef Symbolic::AddExpr > >, + Symbolic::ValueExpr > + > type; +}; + +template +struct aseq_reverse_first_type_aux { + typedef Index type; +}; + +template +struct aseq_reverse_first_type_aux::type> { + typedef FixedInt<(SizeType::value-1)*IncrType::value> type; +}; + +template +struct aseq_reverse_first_type { + typedef typename aseq_reverse_first_type_aux::type Aux; + typedef Symbolic::AddExpr > type; +}; + +template +struct aseq_reverse_first_type { + typedef Symbolic::AddExpr > >, + Symbolic::ValueExpr >, + Symbolic::ValueExpr<> > type; +}; +#endif + +// Helper to cleanup the type of the increment: +template struct cleanup_seq_incr { + typedef typename cleanup_index_type::type type; +}; + +} + +//-------------------------------------------------------------------------------- +// 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: + +#if EIGEN_HAS_CXX11 && ((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48) + 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); + } +#else +protected: + typedef typename internal::aseq_negate::type ReverseIncrType; + typedef typename internal::aseq_reverse_first_type::type ReverseFirstType; +public: + ArithmeticSequence + reverse() const { + return seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr); + } +#endif +}; + +/** \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 + +#if EIGEN_HAS_CXX11 +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)); +} +#else + +template +typename internal::enable_if::value || Symbolic::is_symbolic::value), + ArithmeticSequence::type,Index> >::type +seq(FirstType f, LastType l) +{ + return seqN(typename internal::cleanup_index_type::type(f), + Index((typename internal::cleanup_index_type::type(l)-typename internal::cleanup_index_type::type(f)+fix<1>()))); +} + +template +typename internal::enable_if::value, + ArithmeticSequence,Symbolic::ValueExpr<> >, + Symbolic::ValueExpr > > > >::type +seq(const Symbolic::BaseExpr &f, LastType l) +{ + return seqN(f.derived(),(typename internal::cleanup_index_type::type(l)-f.derived()+fix<1>())); +} + +template +typename internal::enable_if::value, + ArithmeticSequence::type, + Symbolic::AddExpr >, + Symbolic::ValueExpr > > > >::type +seq(FirstType f, const Symbolic::BaseExpr &l) +{ + return seqN(typename internal::cleanup_index_type::type(f),(l.derived()-typename internal::cleanup_index_type::type(f)+fix<1>())); +} + +template +ArithmeticSequence >,Symbolic::ValueExpr > > > +seq(const Symbolic::BaseExpr &f, const Symbolic::BaseExpr &l) +{ + return seqN(f.derived(),(l.derived()-f.derived()+fix<1>())); +} + + +template +typename internal::enable_if::value || Symbolic::is_symbolic::value), + ArithmeticSequence::type,Index,typename internal::cleanup_seq_incr::type> >::type +seq(FirstType f, LastType l, IncrType incr) +{ + typedef typename internal::cleanup_seq_incr::type CleanedIncrType; + return seqN(typename internal::cleanup_index_type::type(f), + Index((typename internal::cleanup_index_type::type(l)-typename internal::cleanup_index_type::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr)), incr); +} + +template +typename internal::enable_if::value, + ArithmeticSequence, + Symbolic::ValueExpr<> >, + Symbolic::ValueExpr::type> >, + Symbolic::ValueExpr::type> >, + typename internal::cleanup_seq_incr::type> >::type +seq(const Symbolic::BaseExpr &f, LastType l, IncrType incr) +{ + typedef typename internal::cleanup_seq_incr::type CleanedIncrType; + return seqN(f.derived(),(typename internal::cleanup_index_type::type(l)-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr); +} + +template +typename internal::enable_if::value, + ArithmeticSequence::type, + Symbolic::QuotientExpr >, + Symbolic::ValueExpr::type> >, + Symbolic::ValueExpr::type> >, + typename internal::cleanup_seq_incr::type> >::type +seq(FirstType f, const Symbolic::BaseExpr &l, IncrType incr) +{ + typedef typename internal::cleanup_seq_incr::type CleanedIncrType; + return seqN(typename internal::cleanup_index_type::type(f), + (l.derived()-typename internal::cleanup_index_type::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr), incr); +} + +template +ArithmeticSequence >, + Symbolic::ValueExpr::type> >, + Symbolic::ValueExpr::type> >, + typename internal::cleanup_seq_incr::type> +seq(const Symbolic::BaseExpr &f, const Symbolic::BaseExpr &l, IncrType incr) +{ + typedef typename internal::cleanup_seq_incr::type CleanedIncrType; + return seqN(f.derived(),(l.derived()-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr); +} +#endif + +#endif // EIGEN_PARSED_BY_DOXYGEN + +namespace internal { + +// Convert a symbolic span into a usable one (i.e., remove last/end "keywords") +template +struct make_size_type { + typedef typename internal::conditional::value, Index, T>::type 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 + +} // end namespace Eigen + +#endif // EIGEN_ARITHMETIC_SEQUENCE_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Array.h b/examples/ThirdPartyLibs/Eigen/src/Core/Array.h new file mode 100644 index 000000000..e10020d4f --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Array.h @@ -0,0 +1,331 @@ +// 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 + +namespace Eigen { + +namespace internal { +template +struct traits > : traits > +{ + typedef ArrayXpr XprKind; + typedef ArrayBase > XprBase; +}; +} + +/** \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() + { + Base::_check_template_params(); + 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()) + { + Base::_check_template_params(); + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } +#endif + +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + Array(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) + : Base(std::move(other)) + { + Base::_check_template_params(); + if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) + Base::_set_noalias(other); + } + EIGEN_DEVICE_FUNC + Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) + { + other.swap(*this); + return *this; + } +#endif + + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE explicit Array(const T& x) + { + Base::_check_template_params(); + Base::template _init1(x); + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Array(const T0& val0, const T1& val1) + { + Base::_check_template_params(); + 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 */ + 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 */ + Array(const Scalar& val0, const Scalar& val1); + #endif + + /** constructs an initialized 3D vector with given coefficients */ + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2) + { + Base::_check_template_params(); + 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 */ + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2, const Scalar& val3) + { + Base::_check_template_params(); + 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, + typename internal::enable_if::value, + PrivateType>::type = PrivateType()) + : Base(other.derived()) + { } + + EIGEN_DEVICE_FUNC inline Index innerStride() const { return 1; } + EIGEN_DEVICE_FUNC inline Index outerStride() const { 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. + * + * \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_TYPEDEFS_LARGE + +#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/examples/ThirdPartyLibs/Eigen/src/Core/ArrayBase.h b/examples/ThirdPartyLibs/Eigen/src/Core/ArrayBase.h new file mode 100644 index 000000000..9da960f08 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/ArrayBase.h @@ -0,0 +1,226 @@ +// 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 + +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 dimensionnal 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::RowsAtCompileTime; + using Base::ColsAtCompileTime; + using Base::SizeAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxSizeAtCompileTime; + using Base::IsVectorAtCompileTime; + using Base::Flags; + + using Base::derived; + using Base::const_cast_derived; + using Base::rows; + using Base::cols; + using Base::size; + using Base::coeff; + using Base::coeffRef; + using Base::lazyAssign; + 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.h" +# include "../plugins/ArrayCwiseUnaryOps.h" +# include "../plugins/CommonCwiseBinaryOps.h" +# include "../plugins/MatrixCwiseBinaryOps.h" +# include "../plugins/ArrayCwiseBinaryOps.h" +# 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_DEVICE_FUNC + ArrayBase() : Base() {} + + 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/examples/ThirdPartyLibs/Eigen/src/Core/ArrayWrapper.h b/examples/ThirdPartyLibs/Eigen/src/Core/ArrayWrapper.h new file mode 100644 index 000000000..688aadd62 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/ArrayWrapper.h @@ -0,0 +1,209 @@ +// 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 + +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::type > +{ + typedef ArrayXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + LvalueBitFlag = is_lvalue::value ? LvalueBit : 0, + Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag + }; +}; +} + +template +class ArrayWrapper : public ArrayBase > +{ + public: + typedef ArrayBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(ArrayWrapper) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ArrayWrapper) + typedef typename internal::remove_all::type NestedExpression; + + typedef typename internal::conditional< + internal::is_lvalue::value, + Scalar, + const Scalar + >::type 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 + inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC + inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC + inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC + inline Index innerStride() const { 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; } + + const typename internal::remove_all::type& + EIGEN_DEVICE_FUNC + 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::type > +{ + typedef MatrixXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + LvalueBitFlag = is_lvalue::value ? LvalueBit : 0, + Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag + }; +}; +} + +template +class MatrixWrapper : public MatrixBase > +{ + public: + typedef MatrixBase > Base; + EIGEN_DENSE_PUBLIC_INTERFACE(MatrixWrapper) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MatrixWrapper) + typedef typename internal::remove_all::type NestedExpression; + + typedef typename internal::conditional< + internal::is_lvalue::value, + Scalar, + const Scalar + >::type 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 + inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC + inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC + inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC + inline Index innerStride() const { 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 typename internal::remove_all::type& + 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/examples/ThirdPartyLibs/Eigen/src/Core/Assign.h b/examples/ThirdPartyLibs/Eigen/src/Core/Assign.h new file mode 100644 index 000000000..655412efd --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Assign.h @@ -0,0 +1,90 @@ +// 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 + +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/examples/ThirdPartyLibs/Eigen/src/Core/AssignEvaluator.h b/examples/ThirdPartyLibs/Eigen/src/Core/AssignEvaluator.h new file mode 100644 index 000000000..dbe435d86 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/AssignEvaluator.h @@ -0,0 +1,935 @@ +// 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 + +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 = EIGEN_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), + 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(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 typename conditional::type 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(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 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 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; + +/************************ +*** 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 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 void run(Kernel &kernel, + Index start, + Index end) +#endif + { + for (Index index = start; index < end; ++index) + kernel.assignCoeff(index); + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE 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 void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { size = DstXprType::SizeAtCompileTime, + packetSize =unpacket_traits::size, + alignedSize = (size/packetSize)*packetSize }; + + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + copy_using_evaluator_DefaultTraversal_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 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 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 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 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)) && (UIntPtr(dst_ptr) % sizeof(Scalar))>0) + { + // the pointer is not aligend-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(outer, inner); + + // do the non-vectorizable part of the assignment + for(Index inner = alignedEnd; inner +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { size = DstXprType::InnerSizeAtCompileTime, + packetSize =unpacket_traits::size, + vectorizableSize = (size/packetSize)*packetSize }; + + 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 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 Index size() const { return m_dstExpr.size(); } + EIGEN_DEVICE_FUNC Index innerSize() const { return m_dstExpr.innerSize(); } + EIGEN_DEVICE_FUNC Index outerSize() const { return m_dstExpr.outerSize(); } + EIGEN_DEVICE_FUNC Index rows() const { return m_dstExpr.rows(); } + EIGEN_DEVICE_FUNC Index cols() const { return m_dstExpr.cols(); } + EIGEN_DEVICE_FUNC Index outerStride() const { return m_dstExpr.outerStride(); } + + EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() { return m_dst; } + EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const { 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; +}; + +/*************************************************************************** +* 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 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); +} + +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; + +// Assignement 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< typename DstXprType, typename SrcXprType, typename Functor, + typename Kind = typename AssignmentKind< typename evaluator_traits::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 +void call_assignment(Dst& dst, const Src& src, const Func& func, typename enable_if< evaluator_assume_aliasing::value, void*>::type = 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, typename enable_if::value, void*>::type = 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 +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 +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 typename internal::conditional, Dst>::type ActualDstTypeCleaned; + typedef typename internal::conditional, Dst&>::type 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_assignment_no_alias(Dst& dst, const Src& src) +{ + call_assignment_no_alias(dst, src, internal::assign_op()); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +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 +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 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< typename DstXprType, typename SrcXprType, typename Functor, typename Weak> +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< typename DstXprType, typename SrcXprType, typename Functor, typename Weak> +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 instanciation 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/examples/ThirdPartyLibs/Eigen/src/Core/Assign_MKL.h b/examples/ThirdPartyLibs/Eigen/src/Core/Assign_MKL.h new file mode 100644 index 000000000..6c2ab9264 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Assign_MKL.h @@ -0,0 +1,176 @@ +/* + Copyright (c) 2011, Intel Corporation. All rights reserved. + Copyright (C) 2015 Gael Guennebaud + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of Intel Corporation nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + ******************************************************************************** + * Content : Eigen bindings to Intel(R) MKL + * MKL VML support for coefficient-wise unary Eigen expressions like a=b.sin() + ******************************************************************************** +*/ + +#ifndef EIGEN_ASSIGN_VML_H +#define EIGEN_ASSIGN_VML_H + +namespace Eigen { + +namespace internal { + +template +class vml_assign_traits +{ + private: + enum { + DstHasDirectAccess = Dst::Flags & DirectAccessBit, + SrcHasDirectAccess = Src::Flags & DirectAccessBit, + StorageOrdersAgree = (int(Dst::IsRowMajor) == int(Src::IsRowMajor)), + InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime) + : int(Dst::Flags)&RowMajorBit ? int(Dst::ColsAtCompileTime) + : int(Dst::RowsAtCompileTime), + InnerMaxSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime) + : int(Dst::Flags)&RowMajorBit ? int(Dst::MaxColsAtCompileTime) + : int(Dst::MaxRowsAtCompileTime), + MaxSizeAtCompileTime = Dst::SizeAtCompileTime, + + MightEnableVml = StorageOrdersAgree && DstHasDirectAccess && SrcHasDirectAccess && Src::InnerStrideAtCompileTime==1 && Dst::InnerStrideAtCompileTime==1, + MightLinearize = MightEnableVml && (int(Dst::Flags) & int(Src::Flags) & LinearAccessBit), + VmlSize = MightLinearize ? MaxSizeAtCompileTime : InnerMaxSize, + LargeEnough = VmlSize==Dynamic || VmlSize>=EIGEN_MKL_VML_THRESHOLD + }; + public: + enum { + EnableVml = MightEnableVml && LargeEnough, + Traversal = MightLinearize ? LinearTraversal : DefaultTraversal + }; +}; + +#define EIGEN_PP_EXPAND(ARG) ARG +#if !defined (EIGEN_FAST_MATH) || (EIGEN_FAST_MATH != 1) +#define EIGEN_VMLMODE_EXPAND_LA , VML_HA +#else +#define EIGEN_VMLMODE_EXPAND_LA , VML_LA +#endif + +#define EIGEN_VMLMODE_EXPAND__ + +#define EIGEN_VMLMODE_PREFIX_LA vm +#define EIGEN_VMLMODE_PREFIX__ v +#define EIGEN_VMLMODE_PREFIX(VMLMODE) EIGEN_CAT(EIGEN_VMLMODE_PREFIX_,VMLMODE) + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE) \ + template< typename DstXprType, typename SrcXprNested> \ + struct Assignment, SrcXprNested>, assign_op, \ + Dense2Dense, typename enable_if::EnableVml>::type> { \ + typedef CwiseUnaryOp, SrcXprNested> SrcXprType; \ + static void run(DstXprType &dst, const SrcXprType &src, const assign_op &/*func*/) { \ + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); \ + if(vml_assign_traits::Traversal==LinearTraversal) { \ + VMLOP(dst.size(), (const VMLTYPE*)src.nestedExpression().data(), \ + (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) ); \ + } else { \ + const Index outerSize = dst.outerSize(); \ + for(Index outer = 0; outer < outerSize; ++outer) { \ + const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.nestedExpression().coeffRef(outer,0)) : \ + &(src.nestedExpression().coeffRef(0, outer)); \ + EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer)); \ + VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr, \ + (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE)); \ + } \ + } \ + } \ + }; \ + + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),s##VMLOP), float, float, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),d##VMLOP), double, double, VMLMODE) + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),c##VMLOP), scomplex, MKL_Complex8, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),z##VMLOP), dcomplex, MKL_Complex16, VMLMODE) + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE) + + +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sin, Sin, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(asin, Asin, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sinh, Sinh, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cos, Cos, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(acos, Acos, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cosh, Cosh, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tan, Tan, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(atan, Atan, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tanh, Tanh, LA) +// EIGEN_MKL_VML_DECLARE_UNARY_CALLS(abs, Abs, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(exp, Exp, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log, Ln, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log10, Log10, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sqrt, Sqrt, _) + +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(square, Sqr, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(arg, Arg, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(round, Round, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(floor, Floor, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(ceil, Ceil, _) + +#define EIGEN_MKL_VML_DECLARE_POW_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE) \ + template< typename DstXprType, typename SrcXprNested, typename Plain> \ + struct Assignment, SrcXprNested, \ + const CwiseNullaryOp,Plain> >, assign_op, \ + Dense2Dense, typename enable_if::EnableVml>::type> { \ + typedef CwiseBinaryOp, SrcXprNested, \ + const CwiseNullaryOp,Plain> > SrcXprType; \ + static void run(DstXprType &dst, const SrcXprType &src, const assign_op &/*func*/) { \ + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); \ + VMLTYPE exponent = reinterpret_cast(src.rhs().functor().m_other); \ + if(vml_assign_traits::Traversal==LinearTraversal) \ + { \ + VMLOP( dst.size(), (const VMLTYPE*)src.lhs().data(), exponent, \ + (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) ); \ + } else { \ + const Index outerSize = dst.outerSize(); \ + for(Index outer = 0; outer < outerSize; ++outer) { \ + const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.lhs().coeffRef(outer,0)) : \ + &(src.lhs().coeffRef(0, outer)); \ + EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer)); \ + VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr, exponent, \ + (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE)); \ + } \ + } \ + } \ + }; + +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmsPowx, float, float, LA) +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmdPowx, double, double, LA) +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmcPowx, scomplex, MKL_Complex8, LA) +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmzPowx, dcomplex, MKL_Complex16, LA) + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_ASSIGN_VML_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/BandMatrix.h b/examples/ThirdPartyLibs/Eigen/src/Core/BandMatrix.h new file mode 100644 index 000000000..4978c9140 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/BandMatrix.h @@ -0,0 +1,353 @@ +// 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 + +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 = EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime,ColsAtCompileTime) + }; + + public: + + using Base::derived; + using Base::rows; + using Base::cols; + + /** \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((Options&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 = (Options&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 + ? EIGEN_SIZE_MIN_PREFER_DYNAMIC(ColsAtCompileTime, RowsAtCompileTime + ActualIndex) + : EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime, ColsAtCompileTime - ActualIndex)) + }; + typedef Block BuildType; + typedef typename internal::conditional,BuildType >, + BuildType>::type 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 Index rows() const { return m_rows.value(); } + + /** \returns the number of rows */ + inline Index cols() const { return m_coeffs.cols(); } + + /** \returns the number of super diagonals */ + inline Index supers() const { return m_supers.value(); } + + /** \returns the number of sub diagonals */ + inline 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<_CoefficientsType>::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); + //internal::assert(coeffs.cols()==cols() && (supers()+subs()+1)==coeffs.rows()); + } + + /** \returns the number of columns */ + inline Index rows() const { return m_rows.value(); } + + /** \returns the number of rows */ + inline Index cols() const { return m_coeffs.cols(); } + + /** \returns the number of super diagonals */ + inline Index supers() const { return m_supers.value(); } + + /** \returns the number of sub diagonals */ + inline 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/examples/ThirdPartyLibs/Eigen/src/Core/Block.h b/examples/ThirdPartyLibs/Eigen/src/Core/Block.h new file mode 100644 index 000000000..11de45c2e --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Block.h @@ -0,0 +1,452 @@ +// 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 + +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 typename remove_reference::type _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 + }; +}; + +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 maniputate 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; + public: + //typedef typename Impl::Base Base; + typedef Impl Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(Block) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block) + + typedef typename internal::remove_all::type NestedExpression; + + /** Column or Row constructor + */ + EIGEN_DEVICE_FUNC + inline Block(XprType& xpr, Index i) : Impl(xpr,i) + { + eigen_assert( (i>=0) && ( + ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i= 0 && BlockRows >= 0 && startRow + BlockRows <= xpr.rows() + && startCol >= 0 && BlockCols >= 0 && startCol + BlockCols <= xpr.cols()); + } + + /** Dynamic-size constructor + */ + EIGEN_DEVICE_FUNC + 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); + } +}; + +// The generic default implementation for dense block simplu 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 inline BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {} + EIGEN_DEVICE_FUNC inline BlockImpl(XprType& xpr, Index startRow, Index startCol) : Impl(xpr, startRow, startCol) {} + EIGEN_DEVICE_FUNC + 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 + inline PacketScalar packet(Index rowId, Index colId) const + { + return m_xpr.template packet(rowId + m_startRow.value(), colId + m_startCol.value()); + } + + template + inline void writePacket(Index rowId, Index colId, const PacketScalar& val) + { + m_xpr.template writePacket(rowId + m_startRow.value(), colId + m_startCol.value(), val); + } + + template + 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 + 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 + const typename internal::remove_all::type& nestedExpression() const + { + return m_xpr; + } + + EIGEN_DEVICE_FUNC + XprType& nestedExpression() { return m_xpr; } + + EIGEN_DEVICE_FUNC + StorageIndex startRow() const + { + return m_startRow.value(); + } + + EIGEN_DEVICE_FUNC + StorageIndex startCol() const + { + 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 + }; + public: + + typedef MapBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(BlockType) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense) + + /** Column or Row constructor + */ + EIGEN_DEVICE_FUNC + inline BlockImpl_dense(XprType& xpr, Index i) + : Base(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 + inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol) + : Base(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 + inline BlockImpl_dense(XprType& xpr, + Index startRow, Index startCol, + Index blockRows, Index blockCols) + : Base(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 + const typename internal::remove_all::type& nestedExpression() const + { + return m_xpr; + } + + EIGEN_DEVICE_FUNC + XprType& nestedExpression() { return m_xpr; } + + /** \sa MapBase::innerStride() */ + EIGEN_DEVICE_FUNC + inline Index innerStride() const + { + return internal::traits::HasSameStorageOrderAsXprType + ? m_xpr.innerStride() + : m_xpr.outerStride(); + } + + /** \sa MapBase::outerStride() */ + EIGEN_DEVICE_FUNC + inline Index outerStride() const + { + return m_outerStride; + } + + EIGEN_DEVICE_FUNC + StorageIndex startRow() const + { + return m_startRow.value(); + } + + EIGEN_DEVICE_FUNC + StorageIndex startCol() const + { + 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 + 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 + 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/examples/ThirdPartyLibs/Eigen/src/Core/BooleanRedux.h b/examples/ThirdPartyLibs/Eigen/src/Core/BooleanRedux.h new file mode 100644 index 000000000..ccf519067 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/BooleanRedux.h @@ -0,0 +1,162 @@ +// 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_ALLANDANY_H +#define EIGEN_ALLANDANY_H + +namespace Eigen { + +namespace internal { + +template +struct all_unroller +{ + enum { + col = (UnrollCount-1) / Rows, + row = (UnrollCount-1) % Rows + }; + + static inline bool run(const Derived &mat) + { + return all_unroller::run(mat) && mat.coeff(row, col); + } +}; + +template +struct all_unroller +{ + static inline bool run(const Derived &/*mat*/) { return true; } +}; + +template +struct all_unroller +{ + static inline bool run(const Derived &) { return false; } +}; + +template +struct any_unroller +{ + enum { + col = (UnrollCount-1) / Rows, + row = (UnrollCount-1) % Rows + }; + + static inline bool run(const Derived &mat) + { + return any_unroller::run(mat) || mat.coeff(row, col); + } +}; + +template +struct any_unroller +{ + static inline bool run(const Derived & /*mat*/) { return false; } +}; + +template +struct any_unroller +{ + static inline bool run(const Derived &) { return false; } +}; + +} // end namespace internal + +/** \returns true if all coefficients are true + * + * Example: \include MatrixBase_all.cpp + * Output: \verbinclude MatrixBase_all.out + * + * \sa any(), Cwise::operator<() + */ +template +EIGEN_DEVICE_FUNC inline bool DenseBase::all() const +{ + typedef internal::evaluator Evaluator; + enum { + unroll = SizeAtCompileTime != Dynamic + && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT + }; + Evaluator evaluator(derived()); + if(unroll) + return internal::all_unroller::RowsAtCompileTime>::run(evaluator); + else + { + for(Index j = 0; j < cols(); ++j) + for(Index i = 0; i < rows(); ++i) + if (!evaluator.coeff(i, j)) return false; + return true; + } +} + +/** \returns true if at least one coefficient is true + * + * \sa all() + */ +template +EIGEN_DEVICE_FUNC inline bool DenseBase::any() const +{ + typedef internal::evaluator Evaluator; + enum { + unroll = SizeAtCompileTime != Dynamic + && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT + }; + Evaluator evaluator(derived()); + if(unroll) + return internal::any_unroller::RowsAtCompileTime>::run(evaluator); + else + { + for(Index j = 0; j < cols(); ++j) + for(Index i = 0; i < rows(); ++i) + if (evaluator.coeff(i, j)) return true; + return false; + } +} + +/** \returns the number of coefficients which evaluate to true + * + * \sa all(), any() + */ +template +EIGEN_DEVICE_FUNC inline Eigen::Index DenseBase::count() const +{ + return derived().template cast().template cast().sum(); +} + +/** \returns true is \c *this contains at least one Not A Number (NaN). + * + * \sa allFinite() + */ +template +inline bool DenseBase::hasNaN() const +{ +#if EIGEN_COMP_MSVC || (defined __FAST_MATH__) + return derived().array().isNaN().any(); +#else + return !((derived().array()==derived().array()).all()); +#endif +} + +/** \returns true if \c *this contains only finite numbers, i.e., no NaN and no +/-INF values. + * + * \sa hasNaN() + */ +template +inline bool DenseBase::allFinite() const +{ +#if EIGEN_COMP_MSVC || (defined __FAST_MATH__) + return derived().array().isFinite().all(); +#else + return !((derived()-derived()).hasNaN()); +#endif +} + +} // end namespace Eigen + +#endif // EIGEN_ALLANDANY_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/CommaInitializer.h b/examples/ThirdPartyLibs/Eigen/src/Core/CommaInitializer.h new file mode 100644 index 000000000..35fdbb819 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/CommaInitializer.h @@ -0,0 +1,160 @@ +// 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 + +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) + { + 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()) + { + m_xpr.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 + 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/examples/ThirdPartyLibs/Eigen/src/Core/ConditionEstimator.h b/examples/ThirdPartyLibs/Eigen/src/Core/ConditionEstimator.h new file mode 100644 index 000000000..aa7efdc76 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/ConditionEstimator.h @@ -0,0 +1,175 @@ +// 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 + +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 RealScalar(1); + if (matrix_norm == RealScalar(0)) return RealScalar(0); + if (dec.rows() == 1) return RealScalar(1); + const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec); + return (inverse_matrix_norm == RealScalar(0) ? RealScalar(0) + : (RealScalar(1) / inverse_matrix_norm) / matrix_norm); +} + +} // namespace internal + +} // namespace Eigen + +#endif diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/CoreEvaluators.h b/examples/ThirdPartyLibs/Eigen/src/Core/CoreEvaluators.h new file mode 100644 index 000000000..15b361b38 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/CoreEvaluators.h @@ -0,0 +1,1728 @@ +// 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 + +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< typename T, + typename Arg1Kind = typename evaluator_traits::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< typename T, + typename LhsKind = typename evaluator_traits::Kind, + typename RhsKind = typename evaluator_traits::Kind, + typename LhsScalar = typename traits::Scalar, + typename RhsScalar = typename traits::Scalar> struct binary_evaluator; + +template< typename T, + typename Kind = typename evaluator_traits::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 explicit evaluator(const T& xpr) : Base(xpr) {} +}; + + +// TODO: Think about const-correctness +template +struct evaluator + : evaluator +{ + EIGEN_DEVICE_FUNC + 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 plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) : data(ptr) + { + EIGEN_ONLY_USED_FOR_DEBUG(outerStride); + eigen_internal_assert(outerStride==OuterStride); + } + EIGEN_DEVICE_FUNC Index outerStride() const { return OuterStride; } + const Scalar *data; +}; + +template class plainobjectbase_evaluator_data { +public: + EIGEN_DEVICE_FUNC plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) : data(ptr), m_outerStride(outerStride) {} + EIGEN_DEVICE_FUNC 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 evaluator() + : m_d(0,OuterStrideAtCompileTime) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC 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 evaluator() {} + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& m) + : evaluator >(m) + { } +}; + +template +struct evaluator > + : evaluator > > +{ + typedef Array XprType; + + EIGEN_DEVICE_FUNC evaluator() {} + + EIGEN_DEVICE_FUNC 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 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 typename internal::remove_all::type 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 = evaluator::CoeffReadCost + 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 + class Data : private UnaryOp + { + public: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Data(const XprType& xpr) : UnaryOp(xpr.functor()), argImpl(xpr.nestedExpression()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const UnaryOp& func() const { return static_cast(*this); } + evaluator argImpl; + }; + + Data m_d; +}; + +// -------------------- 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 = evaluator::CoeffReadCost + evaluator::CoeffReadCost + evaluator::CoeffReadCost + 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 = EIGEN_PLAIN_ENUM_MIN( + EIGEN_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 : private TernaryOp + { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Data(const XprType& xpr) : TernaryOp(xpr.functor()), arg1Impl(xpr.arg1()), arg2Impl(xpr.arg2()), arg3Impl(xpr.arg3()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TernaryOp& func() const { return static_cast(*this); } + 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 explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +template +struct binary_evaluator, IndexBased, IndexBased> + : evaluator_base > +{ + typedef CwiseBinaryOp XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost + evaluator::CoeffReadCost + 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 = EIGEN_PLAIN_ENUM_MIN(evaluator::Alignment,evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC 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 : private BinaryOp + { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Data(const XprType& xpr) : BinaryOp(xpr.functor()), lhsImpl(xpr.lhs()), rhsImpl(xpr.rhs()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const BinaryOp& func() const { return static_cast(*this); } + evaluator lhsImpl; + evaluator rhsImpl; + }; + + Data m_d; +}; + +// -------------------- CwiseUnaryView -------------------- + +template +struct unary_evaluator, IndexBased> + : evaluator_base > +{ + typedef CwiseUnaryView XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost + 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 : private UnaryOp + { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Data(const XprType& xpr) : UnaryOp(xpr.functor()), argImpl(xpr.nestedExpression()) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const UnaryOp& func() const { return static_cast(*this); } + 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 explicit mapbase_evaluator(const XprType& map) + : m_data(const_cast(map.data())), + m_innerStride(map.innerStride()), + m_outerStride(map.outerStride()) + { + EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(evaluator::Flags&PacketAccessBit, 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 + inline Index rowStride() const { return XprType::IsRowMajor ? m_outerStride.value() : m_innerStride.value(); } + EIGEN_DEVICE_FUNC + inline Index colStride() const { 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 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) ? 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 * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) ? int(PacketAlignment) : 0, + Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator::Alignment, Alignment0) + }; + typedef block_evaluator block_evaluator_type; + EIGEN_DEVICE_FUNC 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 explicit block_evaluator(const XprType& block) + : unary_evaluator(block) + {} +}; + +template +struct unary_evaluator, IndexBased> + : evaluator_base > +{ + typedef Block XprType; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& block) + : m_argImpl(block.nestedExpression()), + m_startRow(block.startRow()), + m_startCol(block.startCol()) + { } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + RowsAtCompileTime = XprType::RowsAtCompileTime + }; + + 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 coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0); + } + + 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 coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0); + } + + 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 + { + 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) + { + return writePacket(RowsAtCompileTime == 1 ? 0 : index, + RowsAtCompileTime == 1 ? index : 0, + x); + } + +protected: + evaluator m_argImpl; + const variable_if_dynamic m_startRow; + const variable_if_dynamic m_startCol; +}; + +// 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 explicit block_evaluator(const XprType& block) + : mapbase_evaluator(block) + { + // TODO: for the 3.3 release, this should be turned to an internal assertion, but let's keep it as is for the beta lifetime + eigen_assert(((internal::UIntPtr(block.data()) % EIGEN_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 + + EIGEN_PLAIN_ENUM_MAX(evaluator::CoeffReadCost, + evaluator::CoeffReadCost), + + Flags = (unsigned int)evaluator::Flags & evaluator::Flags & HereditaryBits, + + Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator::Alignment, evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC 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 typename internal::remove_all::type 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 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; +}; + + +// -------------------- PartialReduxExpr -------------------- + +template< typename ArgType, typename MemberOp, int Direction> +struct evaluator > + : evaluator_base > +{ + typedef PartialReduxExpr XprType; + typedef typename internal::nested_eval::type ArgTypeNested; + typedef typename internal::remove_all::type 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 * evaluator::CoeffReadCost + int(CostOpType::value), + + Flags = (traits::Flags&RowMajorBit) | (evaluator::Flags&(HereditaryBits&(~RowMajorBit))) | 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 : 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 + { + if (Direction==Vertical) + return m_functor(m_arg.col(j)); + else + return m_functor(m_arg.row(i)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const Scalar coeff(Index index) const + { + if (Direction==Vertical) + return m_functor(m_arg.col(index)); + else + return m_functor(m_arg.row(index)); + } + +protected: + typename internal::add_const_on_value_type::type m_arg; + const MemberOp m_functor; +}; + + +// -------------------- 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 typename remove_all::type ArgType; + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = evaluator::Flags, + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC 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 explicit unary_evaluator(const XprType& wrapper) + : evaluator_wrapper_base >(wrapper.nestedExpression()) + { } +}; + +template +struct unary_evaluator > + : evaluator_wrapper_base > +{ + typedef ArrayWrapper XprType; + + EIGEN_DEVICE_FUNC 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 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 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 Index rowOffset() const { return m_index.value() > 0 ? 0 : -m_index.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 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; + } + + Index rows() const + { + return m_arg.rows(); + } + + Index cols() const + { + 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()) + { + ::new (static_cast(this)) Base(m_result); + } + + // This constructor is used when nesting an EvalTo evaluator in another evaluator + EIGEN_DEVICE_FUNC evaluator(const ArgType& arg) + : m_result(arg) + { + ::new (static_cast(this)) Base(m_result); + } + +protected: + PlainObject m_result; +}; + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_COREEVALUATORS_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/CoreIterators.h b/examples/ThirdPartyLibs/Eigen/src/Core/CoreIterators.h new file mode 100644 index 000000000..b96719681 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/CoreIterators.h @@ -0,0 +1,132 @@ +// 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 + +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/examples/ThirdPartyLibs/Eigen/src/Core/CwiseBinaryOp.h b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseBinaryOp.h new file mode 100644 index 000000000..bf2632d9e --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseBinaryOp.h @@ -0,0 +1,183 @@ +// 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 + +namespace Eigen { + +namespace internal { +template +struct traits > +{ + // we must not inherit from traits since it has + // the potential to cause problems with MSVC + typedef typename remove_all::type 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< + BinaryOp( + const typename Lhs::Scalar&, + const typename Rhs::Scalar& + ) + >::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 typename remove_reference::type _LhsNested; + typedef typename remove_reference::type _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< + BinaryOp, LhsType, RhsType, + typename internal::cwise_promote_storage_type::StorageKind, + typename internal::traits::StorageKind, + BinaryOp>::ret>, + internal::no_assignment_operator +{ + public: + + typedef typename internal::remove_all::type Functor; + typedef typename internal::remove_all::type Lhs; + typedef typename internal::remove_all::type 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) + + typedef typename internal::ref_selector::type LhsNested; + typedef typename internal::ref_selector::type RhsNested; + typedef typename internal::remove_reference::type _LhsNested; + typedef typename internal::remove_reference::type _RhsNested; + + 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_CHECK_BINARY_COMPATIBILIY(BinaryOp,typename Lhs::Scalar,typename Rhs::Scalar); + // require the sizes to match + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs) + eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.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::type>::RowsAtCompileTime==Dynamic) + return m_rhs.rows(); + else + return m_lhs.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::type>::ColsAtCompileTime==Dynamic) + return m_rhs.cols(); + else + return m_lhs.cols(); + } + + /** \returns the left hand side nested expression */ + EIGEN_DEVICE_FUNC + const _LhsNested& lhs() const { return m_lhs; } + /** \returns the right hand side nested expression */ + EIGEN_DEVICE_FUNC + const _RhsNested& rhs() const { return m_rhs; } + /** \returns the functor representing the binary operation */ + EIGEN_DEVICE_FUNC + 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/examples/ThirdPartyLibs/Eigen/src/Core/CwiseNullaryOp.h b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseNullaryOp.h new file mode 100644 index 000000000..b1923da0f --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseNullaryOp.h @@ -0,0 +1,902 @@ +// 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 + +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< CwiseNullaryOp >::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 Index rows() const { return m_rows.value(); } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE 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 const CwiseNullaryOp::PlainObject> +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 const CwiseNullaryOp::PlainObject> +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 const CwiseNullaryOp::PlainObject> +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&) + * + * \sa LinSpaced(Index,Scalar,Scalar), setLinSpaced(Index,const Scalar&,const Scalar&) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::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(Scalar,Scalar) + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::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)); +} + +/** \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); +} + +/** + * \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); +} + +// 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)); +} + +// 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)); +} + +// 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 +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::setUnit(Index newSize, Index i) +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + eigen_assert(i +// 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 + +namespace Eigen { + +namespace internal { +template +struct traits > { + // we must not inherit from traits since it has + // the potential to cause problems with MSVC + typedef typename remove_all::type 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 typename remove_reference::type _Arg1Nested; + typedef typename remove_reference::type _Arg2Nested; + typedef typename remove_reference::type _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< + TernaryOp, Arg1Type, Arg2Type, Arg3Type, + typename internal::traits::StorageKind>, + internal::no_assignment_operator +{ + public: + typedef typename internal::remove_all::type Arg1; + typedef typename internal::remove_all::type Arg2; + typedef typename internal::remove_all::type Arg3; + + typedef typename CwiseTernaryOpImpl< + TernaryOp, Arg1Type, Arg2Type, Arg3Type, + typename internal::traits::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 typename internal::remove_reference::type _Arg1Nested; + typedef typename internal::remove_reference::type _Arg2Nested; + typedef typename internal::remove_reference::type _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) { + // 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< + typename internal::traits::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + EIGEN_STATIC_ASSERT((internal::is_same< + typename internal::traits::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + + 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::type>:: + RowsAtCompileTime == Dynamic && + internal::traits::type>:: + RowsAtCompileTime == Dynamic) + return m_arg3.rows(); + else if (internal::traits::type>:: + RowsAtCompileTime == Dynamic && + internal::traits::type>:: + 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::type>:: + ColsAtCompileTime == Dynamic && + internal::traits::type>:: + ColsAtCompileTime == Dynamic) + return m_arg3.cols(); + else if (internal::traits::type>:: + ColsAtCompileTime == Dynamic && + internal::traits::type>:: + 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< + CwiseTernaryOp >::type { + public: + typedef typename internal::generic_xpr_base< + CwiseTernaryOp >::type Base; +}; + +} // end namespace Eigen + +#endif // EIGEN_CWISE_TERNARY_OP_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryOp.h b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryOp.h new file mode 100644 index 000000000..1d2dd19f2 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryOp.h @@ -0,0 +1,103 @@ +// 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 + +namespace Eigen { + +namespace internal { +template +struct traits > + : traits +{ + typedef typename result_of< + UnaryOp(const typename XprType::Scalar&) + >::type Scalar; + typedef typename XprType::Nested XprTypeNested; + typedef typename remove_reference::type _XprTypeNested; + enum { + Flags = _XprTypeNested::Flags & RowMajorBit + }; +}; +} + +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 typename internal::remove_all::type 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 + Index rows() const { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Index cols() const { 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 typename internal::remove_all::type& + nestedExpression() const { return m_xpr; } + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename internal::remove_all::type& + 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/examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryView.h b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryView.h new file mode 100644 index 000000000..271033056 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryView.h @@ -0,0 +1,128 @@ +// 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 + +namespace Eigen { + +namespace internal { +template +struct traits > + : traits +{ + typedef typename result_of< + ViewOp(const typename traits::Scalar&) + >::type Scalar; + typedef typename MatrixType::Nested MatrixTypeNested; + typedef typename remove_all::type _MatrixTypeNested; + enum { + FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, + Flags = traits<_MatrixTypeNested>::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 = MatrixTypeInnerStride == Dynamic + ? int(Dynamic) + : int(MatrixTypeInnerStride) * int(sizeof(typename traits::Scalar) / sizeof(Scalar)), + OuterStrideAtCompileTime = outer_stride_at_compile_time::ret == Dynamic + ? int(Dynamic) + : outer_stride_at_compile_time::ret * int(sizeof(typename traits::Scalar) / sizeof(Scalar)) + }; +}; +} + +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 typename internal::remove_all::type NestedExpression; + + explicit inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp()) + : m_matrix(mat), m_functor(func) {} + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView) + + EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); } + + /** \returns the functor representing unary operation */ + const ViewOp& functor() const { return m_functor; } + + /** \returns the nested expression */ + const typename internal::remove_all::type& + nestedExpression() const { return m_matrix; } + + /** \returns the nested expression */ + typename internal::remove_reference::type& + nestedExpression() { return m_matrix.const_cast_derived(); } + + 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< CwiseUnaryView >::type +{ + public: + + typedef CwiseUnaryView Derived; + typedef typename internal::dense_xpr_base< CwiseUnaryView >::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 inline Index innerStride() const + { + return derived().nestedExpression().innerStride() * sizeof(typename internal::traits::Scalar) / sizeof(Scalar); + } + + EIGEN_DEVICE_FUNC inline Index outerStride() const + { + return derived().nestedExpression().outerStride() * sizeof(typename internal::traits::Scalar) / sizeof(Scalar); + } +}; + +} // end namespace Eigen + +#endif // EIGEN_CWISE_UNARY_VIEW_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/DenseBase.h b/examples/ThirdPartyLibs/Eigen/src/Core/DenseBase.h new file mode 100644 index 000000000..fd933eed4 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/DenseBase.h @@ -0,0 +1,615 @@ +// 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 + +namespace Eigen { + +namespace internal { + +// The index type defined by EIGEN_DEFAULT_DENSE_INDEX_TYPE must be a signed type. +// This dummy function simply aims at checking that at compile time. +static inline void check_DenseIndex_is_signed() { + EIGEN_STATIC_ASSERT(NumTraits::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE); +} + +} // end namespace internal + +/** \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 +#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 Base; + + using Base::derived; + using Base::const_cast_derived; + using Base::rows; + using Base::cols; + using Base::size; + using Base::rowIndexByOuterInner; + using Base::colIndexByOuterInner; + using Base::coeff; + using Base::coeffByOuterInner; + using Base::operator(); + using Base::operator[]; + using Base::x; + using Base::y; + using Base::z; + using Base::w; + using Base::stride; + using Base::innerStride; + using Base::outerStride; + using Base::rowStride; + using Base::colStride; + 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_at_compile_time::RowsAtCompileTime, + internal::traits::ColsAtCompileTime>::ret), + /**< This is equal to the number of coefficients, i.e. the number of + * rows times the number of columns, or to \a Dynamic if this is not + * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */ + + MaxRowsAtCompileTime = 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::MaxRowsAtCompileTime, + internal::traits::MaxColsAtCompileTime>::ret), + /**< 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::MaxRowsAtCompileTime == 1 + || internal::traits::MaxColsAtCompileTime == 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). */ + + 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 typename internal::conditional::XprKind,MatrixXpr >::value, + PlainMatrix, PlainArray>::type PlainObject; + + /** \returns the number of nonzero coefficients which is in practice the number + * of stored coefficients. */ + EIGEN_DEVICE_FUNC + inline Index nonZeros() const { return size(); } + + /** \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 + 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 + 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. */ + typedef CwiseNullaryOp,PlainObject> SequentialLinSpacedReturnType; + /** \internal Represents a vector with linearly spaced coefficients that allows random access. */ + typedef CwiseNullaryOp,PlainObject> RandomAccessLinSpacedReturnType; + /** \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. + * \deprecated */ + template + EIGEN_DEVICE_FUNC + Derived& lazyAssign(const DenseBase& other); + + EIGEN_DEVICE_FUNC + CommaInitializer operator<< (const Scalar& s); + + /** \deprecated it now returns \c *this */ + template + 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 typename internal::add_const >::type ConstTransposeReturnType; + EIGEN_DEVICE_FUNC + 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_DEVICE_FUNC static const SequentialLinSpacedReturnType + LinSpaced(Sequential_t, Index size, 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 SequentialLinSpacedReturnType + LinSpaced(Sequential_t, const Scalar& low, const Scalar& high); + EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType + LinSpaced(const Scalar& low, const Scalar& high); + + 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& 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; + + inline bool hasNaN() const; + 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 typename internal::add_const_on_value_type::type>::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 carefull 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 + 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 + 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 typename internal::conditional,Derived&>::type forceAlignedAccessIf() const; + template EIGEN_DEVICE_FUNC + inline typename internal::conditional,Derived&>::type 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; + + EIGEN_DEVICE_FUNC typename internal::traits::Scalar minCoeff() const; + EIGEN_DEVICE_FUNC typename internal::traits::Scalar maxCoeff() const; + + 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; + + 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 providing additional partial reduction operations + * + * 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 providing additional partial reduction operations + * + * 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 + const Select + select(const DenseBase& thenMatrix, + const DenseBase& elseMatrix) const; + + template + inline const Select + select(const DenseBase& thenMatrix, const typename ThenDerived::Scalar& elseScalar) const; + + template + inline const Select + select(const typename ElseDerived::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(); + +#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.h" +# include "../plugins/BlockMethods.h" +# include "../plugins/IndexedViewMethods.h" +# 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: + /** Default constructor. Do nothing. */ + EIGEN_DEVICE_FUNC DenseBase() + { + /* Just checks for self-consistency of the flags. + * Only do it when debugging Eigen, as this borders on paranoiac and could slow compilation down + */ +#ifdef EIGEN_INTERNAL_DEBUGGING + EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, int(IsRowMajor)) + && EIGEN_IMPLIES(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/examples/ThirdPartyLibs/Eigen/src/Core/DenseCoeffsBase.h b/examples/ThirdPartyLibs/Eigen/src/Core/DenseCoeffsBase.h new file mode 100644 index 000000000..c4af48ab6 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/DenseCoeffsBase.h @@ -0,0 +1,681 @@ +// 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 + +namespace Eigen { + +namespace internal { +template struct add_const_on_value_type_if_arithmetic +{ + typedef typename conditional::value, T, typename add_const_on_value_type::type>::type type; +}; +} + +/** \brief Base class providing read-only coefficient access to matrices and arrays. + * \ingroup Core_Module + * \tparam Derived Type of the derived class + * \tparam #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_artihmetic 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 typename internal::conditional::Flags&LvalueBit), + const Scalar&, + typename internal::conditional::value, Scalar, const Scalar>::type + >::type CoeffReturnType; + + typedef typename internal::add_const_on_value_type_if_arithmetic< + typename internal::packet_traits::type + >::type PacketReturnType; + + typedef EigenBase Base; + using Base::rows; + using Base::cols; + using Base::size; + using Base::derived; + + 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 + * \tparam #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::rows; + using Base::cols; + using Base::size; + using Base::derived; + using Base::rowIndexByOuterInner; + using Base::colIndexByOuterInner; + using Base::operator[]; + using Base::operator(); + using Base::x; + using Base::y; + using Base::z; + using Base::w; + + /** 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 + * \tparam #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::rows; + using Base::cols; + using Base::size; + using Base::derived; + + /** \returns the pointer increment between two consecutive elements within a slice in the inner direction. + * + * \sa outerStride(), rowStride(), colStride() + */ + EIGEN_DEVICE_FUNC + 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 + inline Index outerStride() const + { + return derived().outerStride(); + } + + // FIXME shall we remove it ? + inline Index stride() const + { + return Derived::IsVectorAtCompileTime ? innerStride() : outerStride(); + } + + /** \returns the pointer increment between two consecutive rows. + * + * \sa innerStride(), outerStride(), colStride() + */ + EIGEN_DEVICE_FUNC + inline Index rowStride() const + { + return Derived::IsRowMajor ? outerStride() : innerStride(); + } + + /** \returns the pointer increment between two consecutive columns. + * + * \sa innerStride(), outerStride(), rowStride() + */ + EIGEN_DEVICE_FUNC + 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 + * \tparam #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::rows; + using Base::cols; + using Base::size; + using Base::derived; + + /** \returns the pointer increment between two consecutive elements within a slice in the inner direction. + * + * \sa outerStride(), rowStride(), colStride() + */ + EIGEN_DEVICE_FUNC + 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 + inline Index outerStride() const + { + return derived().outerStride(); + } + + // FIXME shall we remove it ? + inline Index stride() const + { + return Derived::IsVectorAtCompileTime ? innerStride() : outerStride(); + } + + /** \returns the pointer increment between two consecutive rows. + * + * \sa innerStride(), outerStride(), colStride() + */ + EIGEN_DEVICE_FUNC + inline Index rowStride() const + { + return Derived::IsRowMajor ? outerStride() : innerStride(); + } + + /** \returns the pointer increment between two consecutive columns. + * + * \sa innerStride(), outerStride(), rowStride() + */ + EIGEN_DEVICE_FUNC + inline Index colStride() const + { + return Derived::IsRowMajor ? innerStride() : outerStride(); + } +}; + +namespace internal { + +template +struct first_aligned_impl +{ + static inline Index run(const Derived&) + { 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/examples/ThirdPartyLibs/Eigen/src/Core/DenseStorage.h b/examples/ThirdPartyLibs/Eigen/src/Core/DenseStorage.h new file mode 100644 index 000000000..7958feeb9 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/DenseStorage.h @@ -0,0 +1,570 @@ +// 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 + +namespace Eigen { + +namespace internal { + +struct constructor_without_unaligned_array_assert {}; + +template +EIGEN_DEVICE_FUNC +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 + plain_array() + { + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC + 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) +#elif EIGEN_GNUC_AT_LEAST(4,7) + // GCC 4.7 is too aggressive in its optimizations and remove the alignement test based on the fact the array is declared to be aligned. + // See this bug report: http://gcc.gnu.org/bugzilla/show_bug.cgi?id=53900 + // Hiding the origin of the array pointer behind a function argument seems to do the trick even if the function is inlined: + template + EIGEN_ALWAYS_INLINE PtrType eigen_unaligned_array_assert_workaround_gcc47(PtrType array) { return array; } + #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \ + eigen_assert((internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (sizemask)) == 0 \ + && "this assertion is explained here: " \ + "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \ + " **** READ THIS WEB PAGE !!! ****"); +#else + #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \ + eigen_assert((internal::UIntPtr(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 + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(7); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC + 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 + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(15); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC + 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 + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(31); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC + 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 + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(63); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC + plain_array(constructor_without_unaligned_array_assert) + { + check_static_allocation_size(); + } +}; + +template +struct plain_array +{ + T array[1]; + EIGEN_DEVICE_FUNC plain_array() {} + EIGEN_DEVICE_FUNC plain_array(constructor_without_unaligned_array_assert) {} +}; + +} // 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: + EIGEN_DEVICE_FUNC DenseStorage() { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size) + } + EIGEN_DEVICE_FUNC + explicit DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()) {} + EIGEN_DEVICE_FUNC + DenseStorage(const DenseStorage& other) : m_data(other.m_data) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size) + } + EIGEN_DEVICE_FUNC + DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) m_data = other.m_data; + return *this; + } + EIGEN_DEVICE_FUNC 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) { std::swap(m_data,other.m_data); } + EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;} + EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; } + EIGEN_DEVICE_FUNC T *data() { return m_data.array; } +}; + +// null matrix +template class DenseStorage +{ + public: + EIGEN_DEVICE_FUNC DenseStorage() {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage&) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage&) { return *this; } + EIGEN_DEVICE_FUNC DenseStorage(Index,Index,Index) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& ) {} + EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;} + EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC const T *data() const { return 0; } + EIGEN_DEVICE_FUNC T *data() { return 0; } +}; + +// more specializations for null matrices; these are necessary to resolve ambiguities +template class DenseStorage +: public DenseStorage { }; + +template class DenseStorage +: public DenseStorage { }; + +template class DenseStorage +: public DenseStorage { }; + +// 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 DenseStorage() : m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows), m_cols(other.m_cols) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + m_data = other.m_data; + 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_DEVICE_FUNC void swap(DenseStorage& other) + { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); } + EIGEN_DEVICE_FUNC Index rows() const {return m_rows;} + EIGEN_DEVICE_FUNC Index cols() const {return m_cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index cols) { m_rows = rows; m_cols = cols; } + EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index cols) { m_rows = rows; m_cols = cols; } + EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; } + EIGEN_DEVICE_FUNC 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 DenseStorage() : m_rows(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + m_data = other.m_data; + m_rows = other.m_rows; + } + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index) : m_rows(rows) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;} + EIGEN_DEVICE_FUNC Index cols(void) const {return _Cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index) { m_rows = rows; } + EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index) { m_rows = rows; } + EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; } + EIGEN_DEVICE_FUNC 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 DenseStorage() : m_cols(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) + : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_cols(other.m_cols) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + m_data = other.m_data; + m_cols = other.m_cols; + } + return *this; + } + EIGEN_DEVICE_FUNC DenseStorage(Index, Index, Index cols) : m_cols(cols) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } + EIGEN_DEVICE_FUNC Index rows(void) const {return _Rows;} + EIGEN_DEVICE_FUNC Index cols(void) const {return m_cols;} + void conservativeResize(Index, Index, Index cols) { m_cols = cols; } + void resize(Index, Index, Index cols) { m_cols = cols; } + EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; } + EIGEN_DEVICE_FUNC 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 DenseStorage() : m_data(0), m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC explicit 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; + } +#if EIGEN_HAS_RVALUE_REFERENCES + 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 + { + using std::swap; + swap(m_data, other.m_data); + swap(m_rows, other.m_rows); + swap(m_cols, other.m_cols); + return *this; + } +#endif + EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, m_rows*m_cols); } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) + { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); } + EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;} + EIGEN_DEVICE_FUNC Index cols(void) const {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) + 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 DenseStorage() : m_data(0), m_cols(0) {} + explicit 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; + } +#if EIGEN_HAS_RVALUE_REFERENCES + 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 + { + using std::swap; + swap(m_data, other.m_data); + swap(m_cols, other.m_cols); + return *this; + } +#endif + EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Rows*m_cols); } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } + EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;} + EIGEN_DEVICE_FUNC Index cols(void) const {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) + 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 DenseStorage() : m_data(0), m_rows(0) {} + explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {} + EIGEN_DEVICE_FUNC 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; + } +#if EIGEN_HAS_RVALUE_REFERENCES + 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 + { + using std::swap; + swap(m_data, other.m_data); + swap(m_rows, other.m_rows); + return *this; + } +#endif + EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Cols*m_rows); } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;} + EIGEN_DEVICE_FUNC static 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) + 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/examples/ThirdPartyLibs/Eigen/src/Core/Diagonal.h b/examples/ThirdPartyLibs/Eigen/src/Core/Diagonal.h new file mode 100644 index 000000000..c62f5ff21 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Diagonal.h @@ -0,0 +1,259 @@ +// 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 + +namespace Eigen { + +/** \class Diagonal + * \ingroup Core_Module + * + * \brief Expression of a diagonal/subdiagonal/superdiagonal in a matrix + * + * \param MatrixType the type of the object in which we are taking a sub/main/super diagonal + * \param 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 typename remove_reference::type _MatrixTypeNested; + typedef typename MatrixType::StorageKind StorageKind; + enum { + RowsAtCompileTime = (int(DiagIndex) == DynamicIndex || int(MatrixType::SizeAtCompileTime) == Dynamic) ? Dynamic + : (EIGEN_PLAIN_ENUM_MIN(MatrixType::RowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0), + MatrixType::ColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))), + ColsAtCompileTime = 1, + MaxRowsAtCompileTime = int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic + : DiagIndex == DynamicIndex ? EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::MaxRowsAtCompileTime, + MatrixType::MaxColsAtCompileTime) + : (EIGEN_PLAIN_ENUM_MIN(MatrixType::MaxRowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0), + MatrixType::MaxColsAtCompileTime - EIGEN_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 + }; +}; +} + +template class Diagonal + : public internal::dense_xpr_base< Diagonal >::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_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 + inline Index cols() const { return 1; } + + EIGEN_DEVICE_FUNC + inline Index innerStride() const + { + return m_matrix.outerStride() + 1; + } + + EIGEN_DEVICE_FUNC + inline Index outerStride() const + { + return 0; + } + + typedef typename internal::conditional< + internal::is_lvalue::value, + Scalar, + const Scalar + >::type 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 typename internal::remove_all::type& + 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 Index absDiagIndex() const { return m_index.value()>0 ? m_index.value() : -m_index.value(); } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index rowOffset() const { return m_index.value()>0 ? 0 : -m_index.value(); } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index colOffset() const { 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 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 typename MatrixBase::DiagonalDynamicIndexReturnType +MatrixBase::diagonal(Index index) +{ + return DiagonalDynamicIndexReturnType(derived(), index); +} + +/** This is the const version of diagonal(Index). */ +template +EIGEN_DEVICE_FUNC inline typename MatrixBase::ConstDiagonalDynamicIndexReturnType +MatrixBase::diagonal(Index index) const +{ + return ConstDiagonalDynamicIndexReturnType(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 typename MatrixBase::template DiagonalIndexReturnType::Type +MatrixBase::diagonal() +{ + return typename DiagonalIndexReturnType::Type(derived()); +} + +/** This is the const version of diagonal(). */ +template +template +EIGEN_DEVICE_FUNC +inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type +MatrixBase::diagonal() const +{ + return typename ConstDiagonalIndexReturnType::Type(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_DIAGONAL_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/DiagonalMatrix.h b/examples/ThirdPartyLibs/Eigen/src/Core/DiagonalMatrix.h new file mode 100644 index 000000000..4e8297ee6 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/DiagonalMatrix.h @@ -0,0 +1,343 @@ +// 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 + +namespace Eigen { + +#ifndef EIGEN_PARSED_BY_DOXYGEN +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; + + EIGEN_DEVICE_FUNC + inline const Derived& derived() const { return *static_cast(this); } + EIGEN_DEVICE_FUNC + inline Derived& derived() { return *static_cast(this); } + + EIGEN_DEVICE_FUNC + DenseMatrixType toDenseMatrix() const { return derived(); } + + EIGEN_DEVICE_FUNC + inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); } + EIGEN_DEVICE_FUNC + inline DiagonalVectorType& diagonal() { return derived().diagonal(); } + + EIGEN_DEVICE_FUNC + inline Index rows() const { return diagonal().size(); } + EIGEN_DEVICE_FUNC + inline Index cols() const { return diagonal().size(); } + + template + EIGEN_DEVICE_FUNC + const Product + operator*(const MatrixBase &matrix) const + { + return Product(derived(),matrix.derived()); + } + + typedef DiagonalWrapper, const DiagonalVectorType> > InverseReturnType; + EIGEN_DEVICE_FUNC + inline const InverseReturnType + inverse() const + { + return InverseReturnType(diagonal().cwiseInverse()); + } + + EIGEN_DEVICE_FUNC + inline const DiagonalWrapper + operator*(const Scalar& scalar) const + { + return DiagonalWrapper(diagonal() * scalar); + } + EIGEN_DEVICE_FUNC + friend inline const DiagonalWrapper + operator*(const Scalar& scalar, const DiagonalBase& other) + { + return DiagonalWrapper(scalar * other.diagonal()); + } +}; + +#endif + +/** \class DiagonalMatrix + * \ingroup Core_Module + * + * \brief Represents a diagonal matrix with its storage + * + * \param _Scalar the type of coefficients + * \param SizeAtCompileTime the dimension of the matrix, or Dynamic + * \param 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 DiagonalWrapper + */ + +namespace internal { +template +struct traits > + : traits > +{ + typedef Matrix<_Scalar,SizeAtCompileTime,1,0,MaxSizeAtCompileTime,1> DiagonalVectorType; + typedef DiagonalShape StorageKind; + enum { + Flags = LvalueBit | NoPreferredStorageOrderBit + }; +}; +} +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) {} + + /** 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 + + /** 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 + * + * \param _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 + }; +}; +} + +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< typename DstXprType, typename SrcXprType, typename Functor> +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/examples/ThirdPartyLibs/Eigen/src/Core/DiagonalProduct.h b/examples/ThirdPartyLibs/Eigen/src/Core/DiagonalProduct.h new file mode 100644 index 000000000..7911d1cd1 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/DiagonalProduct.h @@ -0,0 +1,28 @@ +// 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 + +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/examples/ThirdPartyLibs/Eigen/src/Core/Dot.h b/examples/ThirdPartyLibs/Eigen/src/Core/Dot.h new file mode 100644 index 000000000..bb8e3fecc --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Dot.h @@ -0,0 +1,315 @@ +// 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 + +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 + static inline 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 + static inline 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 +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)) + typedef internal::scalar_conj_product_op func; + EIGEN_CHECK_BINARY_COMPATIBILIY(func,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 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 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 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 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 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 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_MATH(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/examples/ThirdPartyLibs/Eigen/src/Core/EigenBase.h b/examples/ThirdPartyLibs/Eigen/src/Core/EigenBase.h new file mode 100644 index 000000000..b195506a9 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/EigenBase.h @@ -0,0 +1,159 @@ +// 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 + +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. + * \deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. + * \sa StorageIndex, \ref TopicPreprocessorDirectives. + */ + 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 + inline Index rows() const { return derived().rows(); } + /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/ + EIGEN_DEVICE_FUNC + inline Index cols() const { return derived().cols(); } + /** \returns the number of coefficients, which is rows()*cols(). + * \sa rows(), cols(), SizeAtCompileTime. */ + EIGEN_DEVICE_FUNC + inline Index size() const { 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/examples/ThirdPartyLibs/Eigen/src/Core/ForceAlignedAccess.h b/examples/ThirdPartyLibs/Eigen/src/Core/ForceAlignedAccess.h new file mode 100644 index 000000000..7b08b45e6 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/ForceAlignedAccess.h @@ -0,0 +1,146 @@ +// 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 + +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 +{}; +} + +template class ForceAlignedAccess + : public internal::dense_xpr_base< ForceAlignedAccess >::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 inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { 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 typename internal::add_const_on_value_type,Derived&>::type>::type +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 typename internal::conditional,Derived&>::type +MatrixBase::forceAlignedAccessIf() +{ + return derived(); // FIXME This should not work but apparently is never used +} + +} // end namespace Eigen + +#endif // EIGEN_FORCEALIGNEDACCESS_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Fuzzy.h b/examples/ThirdPartyLibs/Eigen/src/Core/Fuzzy.h new file mode 100644 index 000000000..43aa49b2b --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Fuzzy.h @@ -0,0 +1,155 @@ +// 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 + +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 - otherNested).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/examples/ThirdPartyLibs/Eigen/src/Core/GeneralProduct.h b/examples/ThirdPartyLibs/Eigen/src/Core/GeneralProduct.h new file mode 100644 index 000000000..694f7cbde --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/GeneralProduct.h @@ -0,0 +1,466 @@ +// 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 + +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_CUDA_ARCH + 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 typename remove_all::type _Lhs; + typedef typename remove_all::type _Rhs; + enum { + MaxRows = traits<_Lhs>::MaxRowsAtCompileTime, + Rows = traits<_Lhs>::RowsAtCompileTime, + MaxCols = traits<_Rhs>::MaxColsAtCompileTime, + Cols = traits<_Rhs>::ColsAtCompileTime, + MaxDepth = EIGEN_SIZE_MIN_PREFER_FIXED(traits<_Lhs>::MaxColsAtCompileTime, + traits<_Rhs>::MaxRowsAtCompileTime), + Depth = EIGEN_SIZE_MIN_PREFER_FIXED(traits<_Lhs>::ColsAtCompileTime, + traits<_Rhs>::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 Scalar* data() { eigen_internal_assert(false && "should never be called"); return 0; } +}; + +template +struct gemv_static_vector_if +{ + EIGEN_STRONG_INLINE 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 m_data; + EIGEN_STRONG_INLINE Scalar* data() { + return ForceAlignment + ? reinterpret_cast((internal::UIntPtr(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 typename Dest::RealScalar RealScalar; + + typedef internal::blas_traits LhsBlasTraits; + typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType; + typedef internal::blas_traits RhsBlasTraits; + typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType; + + typedef Map, EIGEN_PLAIN_ENUM_MIN(AlignedMax,internal::packet_traits::size)> MappedDest; + + ActualLhsType actualLhs = LhsBlasTraits::extract(lhs); + ActualRhsType actualRhs = RhsBlasTraits::extract(rhs); + + ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs) + * RhsBlasTraits::extractScalarFactor(rhs); + + // make sure Dest is a compile-time vector type (bug 1166) + typedef typename conditional::type 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 + }; + + 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::imag(actualAlpha)==RealScalar(0)); + 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 typename internal::remove_all::type ActualRhsTypeCleaned; + + typename add_const::type actualLhs = LhsBlasTraits::extract(lhs); + typename add_const::type actualRhs = RhsBlasTraits::extract(rhs); + + ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs) + * RhsBlasTraits::extractScalarFactor(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 + }; + + 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 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 +template +EIGEN_DEVICE_FUNC +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 +const Product +EIGEN_DEVICE_FUNC 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/examples/ThirdPartyLibs/Eigen/src/Core/GenericPacketMath.h b/examples/ThirdPartyLibs/Eigen/src/Core/GenericPacketMath.h new file mode 100644 index 000000000..30878eda6 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/GenericPacketMath.h @@ -0,0 +1,598 @@ +// 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 + +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 { + HasHalfPacket = 0, + + HasAdd = 1, + HasSub = 1, + HasMul = 1, + HasNegate = 1, + HasAbs = 1, + HasArg = 0, + HasAbs2 = 1, + HasMin = 1, + HasMax = 1, + HasConj = 1, + HasSetLinear = 1, + HasBlend = 0, + + HasDiv = 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, + HasSinh = 0, + HasCosh = 0, + HasTanh = 0, + HasLGamma = 0, + HasDiGamma = 0, + HasZeta = 0, + HasPolygamma = 0, + HasErf = 0, + HasErfc = 0, + HasIGamma = 0, + HasIGammac = 0, + HasBetaInc = 0, + + HasRound = 0, + HasFloor = 0, + HasCeil = 0, + + HasSign = 0 + }; +}; + +template struct packet_traits : default_packet_traits +{ + typedef T type; + typedef T half; + enum { + Vectorizable = 0, + size = 1, + AlignedOnScalar = 0, + HasHalfPacket = 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 type_casting_traits { + enum { + VectorizedCast = 0, + SrcCoeffRatio = 1, + TgtCoeffRatio = 1 + }; +}; + + +/** \internal \returns static_cast(a) (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline TgtPacket +pcast(const SrcPacket& a) { + return static_cast(a); +} +template +EIGEN_DEVICE_FUNC inline TgtPacket +pcast(const SrcPacket& a, const SrcPacket& /*b*/) { + return static_cast(a); +} + +template +EIGEN_DEVICE_FUNC inline TgtPacket +pcast(const SrcPacket& a, const SrcPacket& /*b*/, const SrcPacket& /*c*/, const SrcPacket& /*d*/) { + return static_cast(a); +} + +/** \internal \returns a + b (coeff-wise) */ +template EIGEN_DEVICE_FUNC inline Packet +padd(const Packet& a, + const Packet& b) { return a+b; } + +/** \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; } + +/** \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; } + +/** \internal \returns a / b (coeff-wise) */ +template EIGEN_DEVICE_FUNC inline Packet +pdiv(const Packet& a, + const Packet& b) { return a/b; } + +/** \internal \returns the min of \a a and \a b (coeff-wise) */ +template EIGEN_DEVICE_FUNC inline Packet +pmin(const Packet& a, + const Packet& b) { return numext::mini(a, b); } + +/** \internal \returns the max of \a a and \a b (coeff-wise) */ +template EIGEN_DEVICE_FUNC inline Packet +pmax(const Packet& a, + const Packet& b) { return numext::maxi(a, b); } + +/** \internal \returns the absolute value of \a a */ +template EIGEN_DEVICE_FUNC inline Packet +pabs(const Packet& a) { using std::abs; return abs(a); } + +/** \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 the bitwise and of \a a and \a b */ +template EIGEN_DEVICE_FUNC inline Packet +pand(const Packet& a, const Packet& b) { return 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 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 a ^ b; } + +/** \internal \returns the bitwise andnot of \a a and \a b */ +template EIGEN_DEVICE_FUNC inline Packet +pandnot(const Packet& a, const Packet& b) { return a & (!b); } + +/** \internal \returns a packet version of \a *from, from must be 16 bytes aligned */ +template EIGEN_DEVICE_FUNC inline Packet +pload(const typename unpacket_traits::type* from) { return *from; } + +/** \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 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 \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 copy the packet \a from to \a *to, \a to must be 16 bytes aligned */ +template EIGEN_DEVICE_FUNC inline void pstore(Scalar* to, const Packet& from) +{ (*to) = from; } + +/** \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; } + + template EIGEN_DEVICE_FUNC inline Packet pgather(const Scalar* from, Index /*stride*/) + { return ploadu(from); } + + template EIGEN_DEVICE_FUNC inline void pscatter(Scalar* to, const Packet& from, Index /*stride*/) + { pstore(to, from); } + +/** \internal tries to do cache prefetching of \a addr */ +template EIGEN_DEVICE_FUNC inline void prefetch(const Scalar* addr) +{ +#ifdef EIGEN_CUDA_ARCH +#if defined(__LP64__) + // 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 first element of a packet */ +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type pfirst(const Packet& a) +{ return a; } + +/** \internal \returns a packet where the element i contains the sum of the packet of \a vec[i] */ +template EIGEN_DEVICE_FUNC inline Packet +preduxp(const Packet* vecs) { return vecs[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 sum of the elements of \a a by block of 4 elements. + * 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 +typename conditional<(unpacket_traits::size%8)==0,typename unpacket_traits::half,Packet>::type +predux_downto4(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) +{ return a; } + +/** \internal \returns the min of the elements of \a a*/ +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_min(const Packet& a) +{ return a; } + +/** \internal \returns the max of the elements of \a a*/ +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_max(const Packet& a) +{ return a; } + +/** \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) +{ + // FIXME: uncomment the following in case we drop the internal imag and real functions. +// using std::imag; +// using std::real; + return Packet(imag(a),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) { 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) { 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) { 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) { 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) { using std::acos; return acos(a); } + +/** \internal \returns the arc tangent of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet patan(const Packet& a) { using std::atan; return atan(a); } + +/** \internal \returns the hyperbolic sine of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet psinh(const Packet& a) { 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) { using std::cosh; return cosh(a); } + +/** \internal \returns the hyperbolic tan of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet ptanh(const Packet& a) { using std::tanh; return tanh(a); } + +/** \internal \returns the exp of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet pexp(const Packet& a) { 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) { 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) { using std::log10; return log10(a); } + +/** \internal \returns the square-root of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet psqrt(const Packet& a) { using std::sqrt; return sqrt(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 pdiv(pset1(1), psqrt(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 ceil of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet pceil(const Packet& a) { using numext::ceil; return ceil(a); } + +/*************************************************************************** +* The following functions might not have to be overwritten for vectorized types +***************************************************************************/ + +/** \internal copy a packet with constant coeficient \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 * 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 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 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 \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); +} + +/** \internal default implementation of palign() allowing partial specialization */ +template +struct palign_impl +{ + // by default data are aligned, so there is nothing to be done :) + static inline void run(PacketType&, const PacketType&) {} +}; + +/** \internal update \a first using the concatenation of the packet_size minus \a Offset last elements + * of \a first and \a Offset first elements of \a second. + * + * This function is currently only used to optimize matrix-vector products on unligned matrices. + * It takes 2 packets that represent a contiguous memory array, and returns a packet starting + * at the position \a Offset. For instance, for packets of 4 elements, we have: + * Input: + * - first = {f0,f1,f2,f3} + * - second = {s0,s1,s2,s3} + * Output: + * - if Offset==0 then {f0,f1,f2,f3} + * - if Offset==1 then {f1,f2,f3,s0} + * - if Offset==2 then {f2,f3,s0,s1} + * - if Offset==3 then {f3,s0,s1,s3} + */ +template +inline void palign(PacketType& first, const PacketType& second) +{ + palign_impl::run(first,second); +} + +/*************************************************************************** +* Fast complex products (GCC generates a function call which is very slow) +***************************************************************************/ + +// Eigen+CUDA does not support complexes. +#ifndef EIGEN_CUDACC + +template<> inline std::complex pmul(const std::complex& a, const std::complex& b) +{ return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } + +template<> inline std::complex pmul(const std::complex& a, const std::complex& b) +{ return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } + +#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 \a a with the first coefficient replaced by the scalar b */ +template EIGEN_DEVICE_FUNC inline Packet +pinsertfirst(const Packet& a, typename unpacket_traits::type b) +{ + // Default implementation based on pblend. + // It must be specialized for higher performance. + Selector::size> mask; + mask.select[0] = true; + // This for loop should be optimized away by the compiler. + for(Index i=1; i::size; ++i) + mask.select[i] = false; + return pblend(mask, pset1(b), a); +} + +/** \internal \returns \a a with the last coefficient replaced by the scalar b */ +template EIGEN_DEVICE_FUNC inline Packet +pinsertlast(const Packet& a, typename unpacket_traits::type b) +{ + // Default implementation based on pblend. + // It must be specialized for higher performance. + Selector::size> mask; + // This for loop should be optimized away by the compiler. + for(Index i=0; i::size-1; ++i) + mask.select[i] = false; + mask.select[unpacket_traits::size-1] = true; + return pblend(mask, pset1(b), a); +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_GENERIC_PACKET_MATH_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/GlobalFunctions.h b/examples/ThirdPartyLibs/Eigen/src/Core/GlobalFunctions.h new file mode 100644 index 000000000..50406400b --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/GlobalFunctions.h @@ -0,0 +1,185 @@ +// 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()); \ + } \ + }; + +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(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(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::log) + 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) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt,scalar_sqrt_op,square root,\sa ArrayBase::sqrt DOXCOMMA MatrixBase::cwiseSqrt) + 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(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) + + /** \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 + inline const CwiseBinaryOp,Derived,Constant > + pow(const Eigen::ArrayBase& x, const ScalarExponent& exponent); +#else + template + EIGEN_DEVICE_FUNC inline + EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE( + const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename internal::promote_scalar_arg::type,pow)) + pow(const Eigen::ArrayBase& x, const ScalarExponent& exponent) + { + typedef typename internal::promote_scalar_arg::type PromotedExponent; + return EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,PromotedExponent,pow)(x.derived(), + typename internal::plain_constant_type::type(x.derived().rows(), x.derived().cols(), internal::scalar_constant_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, const Derived, const ExponentDerived> + pow(const Eigen::ArrayBase& x, const Eigen::ArrayBase& exponents) + { + return Eigen::CwiseBinaryOp, 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 + EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE( + 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 + + + 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) + } +} + +// 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/examples/ThirdPartyLibs/Eigen/src/Core/IO.h b/examples/ThirdPartyLibs/Eigen/src/Core/IO.h new file mode 100644 index 000000000..da7fd6cce --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/IO.h @@ -0,0 +1,225 @@ +// 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 + +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 + * + * 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="") + : matPrefix(_matPrefix), matSuffix(_matSuffix), rowPrefix(_rowPrefix), rowSuffix(_rowSuffix), rowSeparator(_rowSeparator), + rowSpacer(""), coeffSeparator(_coeffSeparator), 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; + 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 digits10() which has been introduced in July 2016 in 3.3. +template +struct significant_decimals_impl +{ + static inline int run() + { + return NumTraits::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) +{ + if(_m.size() == 0) + { + s << fmt.matPrefix << fmt.matSuffix; + return s; + } + + typename Derived::Nested m = _m; + typedef typename Derived::Scalar Scalar; + + 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 << m.coeff(i,j); + width = std::max(width, Index(sstr.str().length())); + } + } + s << fmt.matPrefix; + for(Index i = 0; i < m.rows(); ++i) + { + if (i) + s << fmt.rowSpacer; + s << fmt.rowPrefix; + if(width) s.width(width); + s << m.coeff(i, 0); + for(Index j = 1; j < m.cols(); ++j) + { + s << fmt.coeffSeparator; + if (width) s.width(width); + s << 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); + 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); +} + +} // end namespace Eigen + +#endif // EIGEN_IO_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/IndexedView.h b/examples/ThirdPartyLibs/Eigen/src/Core/IndexedView.h new file mode 100644 index 000000000..8c57a277c --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/IndexedView.h @@ -0,0 +1,207 @@ +// 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 + +namespace Eigen { + +namespace internal { + +template +struct traits > + : traits +{ + enum { + RowsAtCompileTime = int(array_size::value), + ColsAtCompileTime = int(array_size::value), + MaxRowsAtCompileTime = RowsAtCompileTime != Dynamic ? int(RowsAtCompileTime) : int(traits::MaxRowsAtCompileTime), + MaxColsAtCompileTime = 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, + + 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,typename conditional::type>::value, + + InnerStrideAtCompileTime = InnerIncr<0 || InnerIncr==DynamicIndex || XprInnerStride==Dynamic ? Dynamic : XprInnerStride * InnerIncr, + OuterStrideAtCompileTime = OuterIncr<0 || OuterIncr==DynamicIndex || XprOuterstride==Dynamic ? 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, + Flags = (traits::Flags & (HereditaryBits | DirectAccessMask)) | FlagsLvalueBit | FlagsRowMajorBit + }; + + typedef Block BlockType; +}; + +} + +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 + * - Plain C arrays: int[N] + * - Eigen::ArrayXi + * - decltype(ArrayXi::LinSpaced(...)) + * - Any view/expressions of the previous types + * - Eigen::ArithmeticSequence + * - Eigen::internal::AllRange (helper for Eigen::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 typename internal::remove_all::type 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::size(m_rowIndices); } + + /** \returns number of columns */ + Index cols() const { return internal::size(m_colIndices); } + + /** \returns the nested expression */ + const typename internal::remove_all::type& + nestedExpression() const { return m_xpr; } + + /** \returns the nested expression */ + typename internal::remove_reference::type& + nestedExpression() { return m_xpr.const_cast_derived(); } + + /** \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 */, + + Flags = (evaluator::Flags & (HereditaryBits /*| LinearAccessBit | DirectAccessBit*/)), + + 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 + { + return m_argImpl.coeff(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + return m_argImpl.coeffRef(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/examples/ThirdPartyLibs/Eigen/src/Core/Inverse.h b/examples/ThirdPartyLibs/Eigen/src/Core/Inverse.h new file mode 100644 index 000000000..b76f0439d --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Inverse.h @@ -0,0 +1,118 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_INVERSE_H +#define EIGEN_INVERSE_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::PlainObject PlainObject; + typedef typename XprType::Scalar Scalar; + typedef typename internal::ref_selector::type XprTypeNested; + typedef typename internal::remove_all::type XprTypeNestedCleaned; + typedef typename internal::ref_selector::type Nested; + typedef typename internal::remove_all::type NestedExpression; + + explicit EIGEN_DEVICE_FUNC Inverse(const XprType &xpr) + : m_xpr(xpr) + {} + + EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); } + + 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()) + { + ::new (static_cast(this)) Base(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/examples/ThirdPartyLibs/Eigen/src/Core/Map.h b/examples/ThirdPartyLibs/Eigen/src/Core/Map.h new file mode 100644 index 000000000..c437f1a92 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Map.h @@ -0,0 +1,171 @@ +// 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 + +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 +}; +} + +/** \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 + inline Index innerStride() const + { + return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; + } + + EIGEN_DEVICE_FUNC + 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) + { + PlainObjectType::Base::_check_template_params(); + } + + /** 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) + { + PlainObjectType::Base::_check_template_params(); + } + + /** 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) + { + PlainObjectType::Base::_check_template_params(); + } + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + + protected: + StrideType m_stride; +}; + + +} // end namespace Eigen + +#endif // EIGEN_MAP_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/MapBase.h b/examples/ThirdPartyLibs/Eigen/src/Core/MapBase.h new file mode 100644 index 000000000..020f939ad --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/MapBase.h @@ -0,0 +1,299 @@ +// 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) + +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, + 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 typename internal::conditional< + bool(internal::is_lvalue::value), + Scalar *, + const Scalar *>::type + PointerType; + + using Base::derived; +// using Base::RowsAtCompileTime; +// using Base::ColsAtCompileTime; +// using Base::SizeAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxSizeAtCompileTime; + using Base::IsVectorAtCompileTime; + using Base::Flags; + using Base::IsRowMajor; + + using Base::rows; + using Base::cols; + using Base::size; + using Base::coeff; + using Base::coeffRef; + using Base::lazyAssign; + using Base::eval; + + using Base::innerStride; + using Base::outerStride; + using Base::rowStride; + using Base::colStride; + + // bug 217 - compile error on ICC 11.1 + using Base::operator=; + + typedef typename Base::CoeffReturnType CoeffReturnType; + + /** \copydoc DenseBase::rows() */ + EIGEN_DEVICE_FUNC inline Index rows() const { return m_rows.value(); } + /** \copydoc DenseBase::cols() */ + EIGEN_DEVICE_FUNC inline Index cols() const { 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: + + template + EIGEN_DEVICE_FUNC + void checkSanity(typename internal::enable_if<(internal::traits::Alignment>0),void*>::type = 0) const + { +#if EIGEN_MAX_ALIGN_BYTES>0 + eigen_assert(( ((internal::UIntPtr(m_data) % internal::traits::Alignment) == 0) + || (cols() * rows() * innerStride() * sizeof(Scalar)) < internal::traits::Alignment ) && "data is not aligned"); +#endif + } + + template + EIGEN_DEVICE_FUNC + void checkSanity(typename internal::enable_if::Alignment==0,void*>::type = 0) const + {} + + 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::derived; + using Base::rows; + using Base::cols; + using Base::size; + using Base::coeff; + using Base::coeffRef; + + using Base::innerStride; + using Base::outerStride; + using Base::rowStride; + using Base::colStride; + + typedef typename internal::conditional< + internal::is_lvalue::value, + Scalar, + const Scalar + >::type 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=; +}; + +#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS + +} // end namespace Eigen + +#endif // EIGEN_MAPBASE_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/MathFunctions.h b/examples/ThirdPartyLibs/Eigen/src/Core/MathFunctions.h new file mode 100644 index 000000000..5ba5293a0 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/MathFunctions.h @@ -0,0 +1,1711 @@ +// 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_MATHFUNCTIONS_H +#define EIGEN_MATHFUNCTIONS_H + +// source: http://www.geom.uiuc.edu/~huberty/math5337/groupe/digits.html +// TODO this should better be moved to NumTraits +#define EIGEN_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406L + +namespace Eigen { + +// On WINCE, std::abs is defined for int only, so let's defined our own overloads: +// This issue has been confirmed with MSVC 2008 only, but the issue might exist for more recent versions too. +#if EIGEN_OS_WINCE && EIGEN_COMP_MSVC && EIGEN_COMP_MSVC<=1500 +long abs(long x) { return (labs(x)); } +double abs(double x) { return (fabs(x)); } +float abs(float x) { return (fabsf(x)); } +long double abs(long double x) { return (fabsl(x)); } +#endif + +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 + ::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::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 {}; + +#ifdef EIGEN_CUDA_ARCH +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 {}; + +#ifdef EIGEN_CUDA_ARCH +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 + static inline Scalar run(Scalar&) + { + return Scalar(0); + } + EIGEN_DEVICE_FUNC + 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_impl +{ + EIGEN_DEVICE_FUNC + static inline Scalar run(const Scalar& x) + { + return x; + } +}; + +template +struct conj_impl +{ + EIGEN_DEVICE_FUNC + static inline Scalar run(const Scalar& x) + { + using std::conj; + return conj(x); + } +}; + +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 real(x)*real(x) + imag(x)*imag(x); + } +}; + +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 norm1 * +****************************************************************************/ + +template +struct norm1_default_impl +{ + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const Scalar& x) + { + EIGEN_USING_STD_MATH(abs); + return abs(real(x)) + abs(imag(x)); + } +}; + +template +struct norm1_default_impl +{ + EIGEN_DEVICE_FUNC + static inline Scalar run(const Scalar& x) + { + EIGEN_USING_STD_MATH(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 +{ + typedef typename NumTraits::Real RealScalar; + static inline RealScalar run(const Scalar& x, const Scalar& y) + { + EIGEN_USING_STD_MATH(abs); + EIGEN_USING_STD_MATH(sqrt); + RealScalar _x = abs(x); + RealScalar _y = abs(y); + Scalar p, qp; + if(_x>_y) + { + p = _x; + qp = _y / p; + } + else + { + p = _y; + qp = _x / p; + } + if(p==RealScalar(0)) return RealScalar(0); + return p * sqrt(RealScalar(1) + qp*qp); + } +}; + +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); + } +}; + +// 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 round * +****************************************************************************/ + +#if EIGEN_HAS_CXX11_MATH + template + struct round_impl { + static inline Scalar run(const Scalar& x) + { + EIGEN_STATIC_ASSERT((!NumTraits::IsComplex), NUMERIC_TYPE_MUST_BE_REAL) + EIGEN_USING_STD_MATH(round); + return round(x); + } + }; +#else + template + struct round_impl + { + static inline Scalar run(const Scalar& x) + { + EIGEN_STATIC_ASSERT((!NumTraits::IsComplex), NUMERIC_TYPE_MUST_BE_REAL) + EIGEN_USING_STD_MATH(floor); + EIGEN_USING_STD_MATH(ceil); + return (x > Scalar(0)) ? floor(x + Scalar(0.5)) : ceil(x - Scalar(0.5)); + } + }; +#endif + +template +struct round_retval +{ + typedef Scalar type; +}; + +/**************************************************************************** +* Implementation of arg * +****************************************************************************/ + +#if EIGEN_HAS_CXX11_MATH + template + struct arg_impl { + static inline Scalar run(const Scalar& x) + { + EIGEN_USING_STD_MATH(arg); + return arg(x); + } + }; +#else + template::IsComplex> + struct arg_default_impl + { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const Scalar& x) + { + return (x < Scalar(0)) ? Scalar(EIGEN_PI) : Scalar(0); } + }; + + template + struct arg_default_impl + { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const Scalar& x) + { + EIGEN_USING_STD_MATH(arg); + return arg(x); + } + }; + + template struct arg_impl : arg_default_impl {}; +#endif + +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_MATH(exp); + Scalar u = exp(x); + if (u == Scalar(1)) { + return x; + } + Scalar um1 = u - RealScalar(1); + if (um1 == Scalar(-1)) { + return RealScalar(-1); + } + + EIGEN_USING_STD_MATH(log); + return (u - RealScalar(1)) * x / log(u); + } +} + +template +struct expm1_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + #if EIGEN_HAS_CXX11_MATH + using std::expm1; + #endif + using std_fallback::expm1; + return expm1(x); + } +}; + + +template +struct expm1_retval +{ + typedef Scalar type; +}; + +/**************************************************************************** +* 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_MATH(log); + Scalar x1p = RealScalar(1) + x; + return ( x1p == Scalar(1) ) ? x : x * ( log(x1p) / (x1p - RealScalar(1)) ); + } +} + +template +struct log1p_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + #if EIGEN_HAS_CXX11_MATH + using std::log1p; + #endif + using std_fallback::log1p; + return 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_MATH(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) + { + typedef typename conditional::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX; + if(y=x the result converted to an unsigned long is still correct. + std::size_t range = ScalarX(y)-ScalarX(x); + std::size_t offset = 0; + // rejection sampling + std::size_t divisor = 1; + std::size_t multiplier = 1; + if(range 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 = EIGEN_PLAIN_ENUM_MAX(0, int(rand_bits) - int(scalar_bits)), + offset = NumTraits::IsSigned ? (1 << (EIGEN_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(real(x), real(y)), + random(imag(x), imag(y))); + } + 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(); +} + +// Implementatin of is* functions + +// std::is* do not work with fast-math and gcc, std::is* are available on MSVC 2013 and newer, as well as in clang. +#if (EIGEN_HAS_CXX11_MATH && !(EIGEN_COMP_GNUC_STRICT && __FINITE_MATH_ONLY__)) || (EIGEN_COMP_MSVC>=1800) || (EIGEN_COMP_CLANG) +#define EIGEN_USE_STD_FPCLASSIFY 1 +#else +#define EIGEN_USE_STD_FPCLASSIFY 0 +#endif + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if::value,bool>::type +isnan_impl(const T&) { return false; } + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if::value,bool>::type +isinf_impl(const T&) { return false; } + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if::value,bool>::type +isfinite_impl(const T&) { return true; } + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if<(!internal::is_integral::value)&&(!NumTraits::IsComplex),bool>::type +isfinite_impl(const T& x) +{ + #ifdef EIGEN_CUDA_ARCH + return (::isfinite)(x); + #elif EIGEN_USE_STD_FPCLASSIFY + using std::isfinite; + return isfinite EIGEN_NOT_A_MACRO (x); + #else + return x<=NumTraits::highest() && x>=NumTraits::lowest(); + #endif +} + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if<(!internal::is_integral::value)&&(!NumTraits::IsComplex),bool>::type +isinf_impl(const T& x) +{ + #ifdef EIGEN_CUDA_ARCH + return (::isinf)(x); + #elif EIGEN_USE_STD_FPCLASSIFY + using std::isinf; + return isinf EIGEN_NOT_A_MACRO (x); + #else + return x>NumTraits::highest() || x::lowest(); + #endif +} + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if<(!internal::is_integral::value)&&(!NumTraits::IsComplex),bool>::type +isnan_impl(const T& x) +{ + #ifdef EIGEN_CUDA_ARCH + return (::isnan)(x); + #elif EIGEN_USE_STD_FPCLASSIFY + using std::isnan; + return isnan EIGEN_NOT_A_MACRO (x); + #else + return x != x; + #endif +} + +#if (!EIGEN_USE_STD_FPCLASSIFY) + +#if EIGEN_COMP_MSVC + +template EIGEN_DEVICE_FUNC bool isinf_msvc_helper(T x) +{ + return _fpclass(x)==_FPCLASS_NINF || _fpclass(x)==_FPCLASS_PINF; +} + +//MSVC defines a _isnan builtin function, but for double only +EIGEN_DEVICE_FUNC inline bool isnan_impl(const long double& x) { return _isnan(x)!=0; } +EIGEN_DEVICE_FUNC inline bool isnan_impl(const double& x) { return _isnan(x)!=0; } +EIGEN_DEVICE_FUNC inline bool isnan_impl(const float& x) { return _isnan(x)!=0; } + +EIGEN_DEVICE_FUNC inline bool isinf_impl(const long double& x) { return isinf_msvc_helper(x); } +EIGEN_DEVICE_FUNC inline bool isinf_impl(const double& x) { return isinf_msvc_helper(x); } +EIGEN_DEVICE_FUNC inline bool isinf_impl(const float& x) { return isinf_msvc_helper(x); } + +#elif (defined __FINITE_MATH_ONLY__ && __FINITE_MATH_ONLY__ && EIGEN_COMP_GNUC) + +#if EIGEN_GNUC_AT_LEAST(5,0) + #define EIGEN_TMP_NOOPT_ATTRIB EIGEN_DEVICE_FUNC inline __attribute__((optimize("no-finite-math-only"))) +#else + // NOTE the inline qualifier and noinline attribute are both needed: the former is to avoid linking issue (duplicate symbol), + // while the second prevent too aggressive optimizations in fast-math mode: + #define EIGEN_TMP_NOOPT_ATTRIB EIGEN_DEVICE_FUNC inline __attribute__((noinline,optimize("no-finite-math-only"))) +#endif + +template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const long double& x) { return __builtin_isnan(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const double& x) { return __builtin_isnan(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const float& x) { return __builtin_isnan(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const double& x) { return __builtin_isinf(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const float& x) { return __builtin_isinf(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const long double& x) { return __builtin_isinf(x); } + +#undef EIGEN_TMP_NOOPT_ATTRIB + +#endif + +#endif + +// 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); + +} // end namespace internal + +/**************************************************************************** +* Generic math functions * +****************************************************************************/ + +namespace numext { + +#if !defined(EIGEN_CUDA_ARCH) && !defined(__SYCL_DEVICE_ONLY__) +template +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) +{ + EIGEN_USING_STD_MATH(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_MATH(max); + return max EIGEN_NOT_A_MACRO (x,y); +} + + +#elif defined(__SYCL_DEVICE_ONLY__) +template +EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) +{ + + return y < x ? y : x; +} + +template +EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y) +{ + + return x < y ? y : x; +} + +EIGEN_ALWAYS_INLINE int mini(const int& x, const int& y) +{ + return cl::sycl::min(x,y); +} + +EIGEN_ALWAYS_INLINE int maxi(const int& x, const int& y) +{ + return cl::sycl::max(x,y); +} + +EIGEN_ALWAYS_INLINE unsigned int mini(const unsigned int& x, const unsigned int& y) +{ + return cl::sycl::min(x,y); +} + +EIGEN_ALWAYS_INLINE unsigned int maxi(const unsigned int& x, const unsigned int& y) +{ + return cl::sycl::max(x,y); +} + +EIGEN_ALWAYS_INLINE long mini(const long & x, const long & y) +{ + return cl::sycl::min(x,y); +} + +EIGEN_ALWAYS_INLINE long maxi(const long & x, const long & y) +{ + return cl::sycl::max(x,y); +} + +EIGEN_ALWAYS_INLINE unsigned long mini(const unsigned long& x, const unsigned long& y) +{ + return cl::sycl::min(x,y); +} + +EIGEN_ALWAYS_INLINE unsigned long maxi(const unsigned long& x, const unsigned long& y) +{ + return cl::sycl::max(x,y); +} + + +EIGEN_ALWAYS_INLINE float mini(const float& x, const float& y) +{ + return cl::sycl::fmin(x,y); +} + +EIGEN_ALWAYS_INLINE float maxi(const float& x, const float& y) +{ + return cl::sycl::fmax(x,y); +} + +EIGEN_ALWAYS_INLINE double mini(const double& x, const double& y) +{ + return cl::sycl::fmin(x,y); +} + +EIGEN_ALWAYS_INLINE double maxi(const double& x, const double& y) +{ + return cl::sycl::fmax(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 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); +} +#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 typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) >::type 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 typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) >::type 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(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 +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); +} + +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__) +EIGEN_ALWAYS_INLINE float log1p(float x) { return cl::sycl::log1p(x); } +EIGEN_ALWAYS_INLINE double log1p(double x) { return cl::sycl::log1p(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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__) +EIGEN_ALWAYS_INLINE float pow(float x, float y) { return cl::sycl::pow(x, y); } +EIGEN_ALWAYS_INLINE double pow(double x, double y) { return cl::sycl::pow(x, y); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +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__) +EIGEN_ALWAYS_INLINE float isnan(float x) { return cl::sycl::isnan(x); } +EIGEN_ALWAYS_INLINE double isnan(double x) { return cl::sycl::isnan(x); } +EIGEN_ALWAYS_INLINE float isinf(float x) { return cl::sycl::isinf(x); } +EIGEN_ALWAYS_INLINE double isinf(double x) { return cl::sycl::isinf(x); } +EIGEN_ALWAYS_INLINE float isfinite(float x) { return cl::sycl::isfinite(x); } +EIGEN_ALWAYS_INLINE double isfinite(double x) { return cl::sycl::isfinite(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +template +EIGEN_DEVICE_FUNC +inline EIGEN_MATHFUNC_RETVAL(round, Scalar) round(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(round, Scalar)::run(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float round(float x) { return cl::sycl::round(x); } +EIGEN_ALWAYS_INLINE double round(double x) { return cl::sycl::round(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +template +EIGEN_DEVICE_FUNC +T (floor)(const T& x) +{ + EIGEN_USING_STD_MATH(floor); + return floor(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float floor(float x) { return cl::sycl::floor(x); } +EIGEN_ALWAYS_INLINE double floor(double x) { return cl::sycl::floor(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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 +T (ceil)(const T& x) +{ + EIGEN_USING_STD_MATH(ceil); + return ceil(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float ceil(float x) { return cl::sycl::ceil(x); } +EIGEN_ALWAYS_INLINE double ceil(double x) { return cl::sycl::ceil(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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 + + +/** 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 +T sqrt(const T &x) +{ + EIGEN_USING_STD_MATH(sqrt); + return sqrt(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float sqrt(float x) { return cl::sycl::sqrt(x); } +EIGEN_ALWAYS_INLINE double sqrt(double x) { return cl::sycl::sqrt(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T log(const T &x) { + EIGEN_USING_STD_MATH(log); + return log(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float log(float x) { return cl::sycl::log(x); } +EIGEN_ALWAYS_INLINE double log(double x) { return cl::sycl::log(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + + +#ifdef EIGEN_CUDACC +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 +typename internal::enable_if::IsSigned || NumTraits::IsComplex,typename NumTraits::Real>::type +abs(const T &x) { + EIGEN_USING_STD_MATH(abs); + return abs(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +typename internal::enable_if::IsSigned || NumTraits::IsComplex),typename NumTraits::Real>::type +abs(const T &x) { + return x; +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float abs(float x) { return cl::sycl::fabs(x); } +EIGEN_ALWAYS_INLINE double abs(double x) { return cl::sycl::fabs(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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 +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T exp(const T &x) { + EIGEN_USING_STD_MATH(exp); + return exp(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float exp(float x) { return cl::sycl::exp(x); } +EIGEN_ALWAYS_INLINE double exp(double x) { return cl::sycl::exp(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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); } +#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__) +EIGEN_ALWAYS_INLINE float expm1(float x) { return cl::sycl::expm1(x); } +EIGEN_ALWAYS_INLINE double expm1(double x) { return cl::sycl::expm1(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(cos); + return cos(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float cos(float x) { return cl::sycl::cos(x); } +EIGEN_ALWAYS_INLINE double cos(double x) { return cl::sycl::cos(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(sin); + return sin(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float sin(float x) { return cl::sycl::sin(x); } +EIGEN_ALWAYS_INLINE double sin(double x) { return cl::sycl::sin(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(tan); + return tan(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float tan(float x) { return cl::sycl::tan(x); } +EIGEN_ALWAYS_INLINE double tan(double x) { return cl::sycl::tan(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(acos); + return acos(x); +} + +#if EIGEN_HAS_CXX11_MATH +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T acosh(const T &x) { + EIGEN_USING_STD_MATH(acosh); + return acosh(x); +} +#endif + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float acos(float x) { return cl::sycl::acos(x); } +EIGEN_ALWAYS_INLINE double acos(double x) { return cl::sycl::acos(x); } +EIGEN_ALWAYS_INLINE float acosh(float x) { return cl::sycl::acosh(x); } +EIGEN_ALWAYS_INLINE double acosh(double x) { return cl::sycl::acosh(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(asin); + return asin(x); +} + +#if EIGEN_HAS_CXX11_MATH +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T asinh(const T &x) { + EIGEN_USING_STD_MATH(asinh); + return asinh(x); +} +#endif + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float asin(float x) { return cl::sycl::asin(x); } +EIGEN_ALWAYS_INLINE double asin(double x) { return cl::sycl::asin(x); } +EIGEN_ALWAYS_INLINE float asinh(float x) { return cl::sycl::asinh(x); } +EIGEN_ALWAYS_INLINE double asinh(double x) { return cl::sycl::asinh(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(atan); + return atan(x); +} + +#if EIGEN_HAS_CXX11_MATH +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T atanh(const T &x) { + EIGEN_USING_STD_MATH(atanh); + return atanh(x); +} +#endif + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float atan(float x) { return cl::sycl::atan(x); } +EIGEN_ALWAYS_INLINE double atan(double x) { return cl::sycl::atan(x); } +EIGEN_ALWAYS_INLINE float atanh(float x) { return cl::sycl::atanh(x); } +EIGEN_ALWAYS_INLINE double atanh(double x) { return cl::sycl::atanh(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(cosh); + return cosh(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float cosh(float x) { return cl::sycl::cosh(x); } +EIGEN_ALWAYS_INLINE double cosh(double x) { return cl::sycl::cosh(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(sinh); + return sinh(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float sinh(float x) { return cl::sycl::sinh(x); } +EIGEN_ALWAYS_INLINE double sinh(double x) { return cl::sycl::sinh(x); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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_MATH(tanh); + return tanh(x); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float tanh(float x) { return cl::sycl::tanh(x); } +EIGEN_ALWAYS_INLINE double tanh(double x) { return cl::sycl::tanh(x); } +#elif (!defined(EIGEN_CUDACC)) && EIGEN_FAST_MATH +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float tanh(float x) { return internal::generic_fast_tanh_float(x); } +#endif + +#ifdef EIGEN_CUDACC +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_MATH(fmod); + return fmod(a, b); +} + +#if defined(__SYCL_DEVICE_ONLY__) +EIGEN_ALWAYS_INLINE float fmod(float x, float y) { return cl::sycl::fmod(x, y); } +EIGEN_ALWAYS_INLINE double fmod(double x, double y) { return cl::sycl::fmod(x, y); } +#endif // defined(__SYCL_DEVICE_ONLY__) + +#ifdef EIGEN_CUDACC +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 + +} // 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; + } +}; + +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 + +} // end namespace Eigen + +#endif // EIGEN_MATHFUNCTIONS_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/MathFunctionsImpl.h b/examples/ThirdPartyLibs/Eigen/src/Core/MathFunctionsImpl.h new file mode 100644 index 000000000..ae1386b4c --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/MathFunctionsImpl.h @@ -0,0 +1,73 @@ +// 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 + +namespace Eigen { + +namespace internal { + +/** \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 ulp in the range [-9, 9], outside of which + the tanh(x) = +/-1. + + This implementation works on both scalars and packets. +*/ +template +T generic_fast_tanh_float(const T& a_x) +{ + // Clamp the inputs to the range [-9, 9] since anything outside + // this range is +/-1.0f in single-precision. + const T plus_9 = pset1(9.f); + const T minus_9 = pset1(-9.f); + const T x = pmax(pmin(a_x, plus_9), minus_9); + // 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 p. + 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 pdiv(p, q); +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_MATHFUNCTIONSIMPL_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Matrix.h b/examples/ThirdPartyLibs/Eigen/src/Core/Matrix.h new file mode 100644 index 000000000..90c336d8c --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Matrix.h @@ -0,0 +1,461 @@ +// 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 + +namespace Eigen { + +namespace internal { +template +struct traits > +{ +private: + enum { size = internal::size_at_compile_time<_Rows,_Cols>::ret }; + typedef typename find_best_packet<_Scalar,size>::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<_Scalar,max_size>::value, + actual_alignment = ((_Options&DontAlign)==0) ? default_alignment : 0, + required_alignment = unpacket_traits::alignment, + packet_access_bit = (packet_traits<_Scalar>::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<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret, + 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 + }; +}; +} + +/** \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() + { + Base::_check_template_params(); + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } + + // FIXME is it still needed + EIGEN_DEVICE_FUNC + explicit Matrix(internal::constructor_without_unaligned_array_assert) + : Base(internal::constructor_without_unaligned_array_assert()) + { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } + +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + Matrix(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) + : Base(std::move(other)) + { + Base::_check_template_params(); + if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) + Base::_set_noalias(other); + } + EIGEN_DEVICE_FUNC + Matrix& operator=(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) + { + other.swap(*this); + return *this; + } +#endif + + #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::_check_template_params(); + Base::template _init1(x); + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Matrix(const T0& x, const T1& y) + { + Base::_check_template_params(); + 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 */ + 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 */ + Matrix(const Scalar& x, const Scalar& y); + #endif + + /** \brief Constructs an initialized 3D vector with given coefficients */ + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z) + { + Base::_check_template_params(); + 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 */ + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w) + { + Base::_check_template_params(); + 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 inline Index innerStride() const { return 1; } + EIGEN_DEVICE_FUNC inline Index outerStride() const { 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. + * + * \sa class Matrix + */ + +#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ +/** \ingroup matrixtypedefs */ \ +typedef Matrix Matrix##SizeSuffix##TypeSuffix; \ +/** \ingroup matrixtypedefs */ \ +typedef Matrix Vector##SizeSuffix##TypeSuffix; \ +/** \ingroup matrixtypedefs */ \ +typedef Matrix RowVector##SizeSuffix##TypeSuffix; + +#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \ +/** \ingroup matrixtypedefs */ \ +typedef Matrix Matrix##Size##X##TypeSuffix; \ +/** \ingroup matrixtypedefs */ \ +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 + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/MatrixBase.h b/examples/ThirdPartyLibs/Eigen/src/Core/MatrixBase.h new file mode 100644 index 000000000..11435903b --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/MatrixBase.h @@ -0,0 +1,525 @@ +// 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 + +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::RowsAtCompileTime; + using Base::ColsAtCompileTime; + using Base::SizeAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxSizeAtCompileTime; + using Base::IsVectorAtCompileTime; + using Base::Flags; + + using Base::derived; + using Base::const_cast_derived; + using Base::rows; + using Base::cols; + using Base::size; + using Base::coeff; + using Base::coeffRef; + using Base::lazyAssign; + using Base::eval; + 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 typename internal::conditional::IsComplex, + CwiseUnaryOp, ConstTransposeReturnType>, + ConstTransposeReturnType + >::type 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.h" +# include "../plugins/MatrixCwiseUnaryOps.h" +# include "../plugins/MatrixCwiseBinaryOps.h" +# 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 + 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 typename internal::add_const >::type ConstDiagonalReturnType; + EIGEN_DEVICE_FUNC + ConstDiagonalReturnType diagonal() const; + + template struct DiagonalIndexReturnType { typedef Diagonal Type; }; + template struct ConstDiagonalIndexReturnType { typedef const Diagonal Type; }; + + template + EIGEN_DEVICE_FUNC + typename DiagonalIndexReturnType::Type diagonal(); + + template + EIGEN_DEVICE_FUNC + typename ConstDiagonalIndexReturnType::Type diagonal() const; + + typedef Diagonal DiagonalDynamicIndexReturnType; + typedef typename internal::add_const >::type ConstDiagonalDynamicIndexReturnType; + + EIGEN_DEVICE_FUNC + DiagonalDynamicIndexReturnType diagonal(Index index); + EIGEN_DEVICE_FUNC + ConstDiagonalDynamicIndexReturnType 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 + 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; + + 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 /////////// + + inline const FullPivLU fullPivLu() const; + inline const PartialPivLU partialPivLu() const; + + inline const PartialPivLU lu() const; + + 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; + Scalar determinant() const; + +/////////// Cholesky module /////////// + + inline const LLT llt() const; + inline const LDLT ldlt() const; + +/////////// QR module /////////// + + inline const HouseholderQR householderQr() const; + inline const ColPivHouseholderQR colPivHouseholderQr() const; + inline const FullPivHouseholderQR fullPivHouseholderQr() const; + inline const CompleteOrthogonalDecomposition completeOrthogonalDecomposition() const; + +/////////// Eigenvalues module /////////// + + inline EigenvaluesReturnType eigenvalues() const; + inline RealScalar operatorNorm() const; + +/////////// SVD module /////////// + + inline JacobiSVD jacobiSvd(unsigned int computationOptions = 0) const; + inline BDCSVD bdcSvd(unsigned int computationOptions = 0) const; + +/////////// Geometry module /////////// + + #ifndef EIGEN_PARSED_BY_DOXYGEN + /// \internal helper struct to form the return type of the cross product + template struct cross_product_return_type { + typedef typename ScalarBinaryOpTraits::Scalar,typename internal::traits::Scalar>::ReturnType Scalar; + typedef Matrix type; + }; + #endif // EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC +#ifndef EIGEN_PARSED_BY_DOXYGEN + inline typename cross_product_return_type::type +#else + inline PlainObject +#endif + 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_DEVICE_FUNC + inline Matrix eulerAngles(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 /////////// + + void makeHouseholderInPlace(Scalar& tau, RealScalar& beta); + template + void makeHouseholder(EssentialPart& essential, + Scalar& tau, RealScalar& beta) const; + template + void applyHouseholderOnTheLeft(const EssentialPart& essential, + const Scalar& tau, + Scalar* workspace); + template + 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; + const MatrixExponentialReturnValue exp() const; + const MatrixFunctionReturnValue matrixFunction(StemFunction f) const; + const MatrixFunctionReturnValue cosh() const; + const MatrixFunctionReturnValue sinh() const; + const MatrixFunctionReturnValue cos() const; + const MatrixFunctionReturnValue sin() const; + const MatrixSquareRootReturnValue sqrt() const; + const MatrixLogarithmReturnValue log() const; + const MatrixPowerReturnValue pow(const RealScalar& p) const; + const MatrixComplexPowerReturnValue pow(const std::complex& p) const; + + protected: + EIGEN_DEVICE_FUNC MatrixBase() : Base() {} + + 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/examples/ThirdPartyLibs/Eigen/src/Core/NestByValue.h b/examples/ThirdPartyLibs/Eigen/src/Core/NestByValue.h new file mode 100644 index 000000000..01cf192e9 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/NestByValue.h @@ -0,0 +1,110 @@ +// 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 + +namespace Eigen { + +namespace internal { +template +struct traits > : public traits +{}; +} + +/** \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< NestByValue >::type +{ + public: + + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(NestByValue) + + EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {} + + EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { 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 + EIGEN_DEVICE_FUNC inline const PacketScalar packet(Index row, Index col) const + { + return m_expression.template packet(row, col); + } + + template + EIGEN_DEVICE_FUNC inline void writePacket(Index row, Index col, const PacketScalar& x) + { + m_expression.const_cast_derived().template writePacket(row, col, x); + } + + template + EIGEN_DEVICE_FUNC inline const PacketScalar packet(Index index) const + { + return m_expression.template packet(index); + } + + template + EIGEN_DEVICE_FUNC 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; +}; + +/** \returns an expression of the temporary version of *this. + */ +template +EIGEN_DEVICE_FUNC inline const NestByValue +DenseBase::nestByValue() const +{ + return NestByValue(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_NESTBYVALUE_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/NoAlias.h b/examples/ThirdPartyLibs/Eigen/src/Core/NoAlias.h new file mode 100644 index 000000000..e94c8ee96 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/NoAlias.h @@ -0,0 +1,109 @@ +// 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 + +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 usefull when + * the source expression contains a matrix product. + * + * Here are some examples where noalias is usefull: + * \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/examples/ThirdPartyLibs/Eigen/src/Core/NumTraits.h b/examples/ThirdPartyLibs/Eigen/src/Core/NumTraits.h new file mode 100644 index 000000000..92a9ae1ea --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/NumTraits.h @@ -0,0 +1,248 @@ +// 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 + +namespace Eigen { + +namespace internal { + +// default implementation of digits10(), based on numeric_limits if specialized, +// 0 for integer types, and log10(epsilon()) otherwise. +template< typename T, + bool use_numeric_limits = std::numeric_limits::is_specialized, + bool is_integer = NumTraits::IsInteger> +struct default_digits10_impl +{ + static int run() { return std::numeric_limits::digits10; } +}; + +template +struct default_digits10_impl // Floating point +{ + static int run() { + using std::log10; + using std::ceil; + typedef typename NumTraits::Real Real; + return int(ceil(-log10(NumTraits::epsilon()))); + } +}; + +template +struct default_digits10_impl // Integer +{ + static int run() { return 0; } +}; + +} // end namespace internal + +/** \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 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. + */ + +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 typename internal::conditional< + IsInteger, + typename internal::conditional::type, + T + >::type NonInteger; + typedef T Nested; + typedef T Literal; + + EIGEN_DEVICE_FUNC + static inline Real epsilon() + { + return numext::numeric_limits::epsilon(); + } + + EIGEN_DEVICE_FUNC + static inline int digits10() + { + return internal::default_digits10_impl::run(); + } + + EIGEN_DEVICE_FUNC + static inline Real dummy_precision() + { + // make sure to override this for floating-point types + return Real(0); + } + + + EIGEN_DEVICE_FUNC + static inline T highest() { + return (numext::numeric_limits::max)(); + } + + EIGEN_DEVICE_FUNC + static inline T lowest() { + return IsInteger ? (numext::numeric_limits::min)() : (-(numext::numeric_limits::max)()); + } + + EIGEN_DEVICE_FUNC + static inline T infinity() { + return numext::numeric_limits::infinity(); + } + + EIGEN_DEVICE_FUNC + static inline T quiet_NaN() { + return numext::numeric_limits::quiet_NaN(); + } +}; + +template struct NumTraits : GenericNumTraits +{}; + +template<> struct NumTraits + : GenericNumTraits +{ + EIGEN_DEVICE_FUNC + static inline float dummy_precision() { return 1e-5f; } +}; + +template<> struct NumTraits : GenericNumTraits +{ + EIGEN_DEVICE_FUNC + static inline double dummy_precision() { return 1e-12; } +}; + +template<> struct NumTraits + : GenericNumTraits +{ + static inline long double dummy_precision() { return 1e-15l; } +}; + +template struct NumTraits > + : GenericNumTraits > +{ + typedef _Real Real; + typedef typename NumTraits<_Real>::Literal Literal; + enum { + IsComplex = 1, + RequireInitialization = NumTraits<_Real>::RequireInitialization, + ReadCost = 2 * NumTraits<_Real>::ReadCost, + AddCost = 2 * NumTraits::AddCost, + MulCost = 4 * NumTraits::MulCost + 2 * NumTraits::AddCost + }; + + EIGEN_DEVICE_FUNC + static inline Real epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC + static inline Real dummy_precision() { return NumTraits::dummy_precision(); } + EIGEN_DEVICE_FUNC + static inline int digits10() { return NumTraits::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 * NumTraits::ReadCost, + AddCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits::AddCost, + MulCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits::MulCost + }; + + EIGEN_DEVICE_FUNC + static inline RealScalar epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC + static inline RealScalar dummy_precision() { return NumTraits::dummy_precision(); } + + static inline int digits10() { return NumTraits::digits10(); } +}; + +template<> struct NumTraits + : GenericNumTraits +{ + enum { + RequireInitialization = 1, + ReadCost = HugeCost, + AddCost = HugeCost, + MulCost = HugeCost + }; + + static inline int 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 {}; + +} // end namespace Eigen + +#endif // EIGEN_NUMTRAITS_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/PermutationMatrix.h b/examples/ThirdPartyLibs/Eigen/src/Core/PermutationMatrix.h new file mode 100644 index 000000000..b1fb455b9 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/PermutationMatrix.h @@ -0,0 +1,633 @@ +// 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 + +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(); + } + + #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=. + */ + Derived& operator=(const PermutationBase& other) + { + indices() = other.indices(); + return derived(); + } + #endif + + /** \returns the number of rows */ + inline Index rows() const { return Index(indices().size()); } + + /** \returns the number of columns */ + inline 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 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=0 && j>=0 && i=0 && j>=0 && i + void assignTranspose(const PermutationBase& other) + { + for (Index i=0; i + void assignProduct(const Lhs& lhs, const Rhs& rhs) + { + eigen_assert(lhs.cols() == rhs.rows()); + for (Index i=0; i + 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) + 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 > +{ + typedef PermutationStorage StorageKind; + typedef Matrix<_StorageIndex, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1> IndicesType; + typedef _StorageIndex StorageIndex; + typedef void Scalar; +}; +} + +/** \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()) {} + + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** Standard copy constructor. Defined only to prevent a default copy constructor + * from hiding the other templated constructor */ + inline PermutationMatrix(const PermutationMatrix& other) : m_indices(other.indices()) {} + #endif + + /** 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()); + } + + #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=. + */ + PermutationMatrix& operator=(const PermutationMatrix& 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; } + + + /**** 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 + 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 > +{ + typedef PermutationStorage StorageKind; + typedef Map, _PacketAccess> IndicesType; + typedef _StorageIndex StorageIndex; + typedef void Scalar; +}; +} + +template +class Map,_PacketAccess> + : public PermutationBase,_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 + }; +}; +} + +/** \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 typename internal::remove_all::type& + 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 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/examples/ThirdPartyLibs/Eigen/src/Core/PlainObjectBase.h b/examples/ThirdPartyLibs/Eigen/src/Core/PlainObjectBase.h new file mode 100644 index 000000000..1dc7e223a --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/PlainObjectBase.h @@ -0,0 +1,1035 @@ +// 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(int i=0;i::quiet_NaN(); +#else +# undef EIGEN_INITIALIZE_COEFFS +# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED +#endif + +namespace Eigen { + +namespace internal { + +template struct check_rows_cols_for_overflow { + template + EIGEN_DEVICE_FUNC + static EIGEN_ALWAYS_INLINE void run(Index, Index) + { + } +}; + +template<> struct check_rows_cols_for_overflow { + template + EIGEN_DEVICE_FUNC + static EIGEN_ALWAYS_INLINE void run(Index rows, Index cols) + { + // http://hg.mozilla.org/mozilla-central/file/6c8a909977d3/xpcom/ds/CheckedInt.h#l242 + // we assume Index is signed + Index max_index = (std::size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed + bool error = (rows == 0 || cols == 0) ? false + : (rows > max_index / 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::RowsAtCompileTime; + using Base::ColsAtCompileTime; + using Base::SizeAtCompileTime; + using Base::MaxRowsAtCompileTime; + using Base::MaxColsAtCompileTime; + using Base::MaxSizeAtCompileTime; + using Base::IsVectorAtCompileTime; + using Base::Flags; + + template friend class Eigen::Map; + friend class Eigen::Map; + typedef Eigen::Map MapType; + friend class Eigen::Map; + typedef const Eigen::Map ConstMapType; +#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 + 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_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 Index rows() const { return m_storage.rows(); } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index cols() const { 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 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 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 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 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 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 void resize(Index rows, Index cols) + { + eigen_assert( EIGEN_IMPLIES(RowsAtCompileTime!=Dynamic,rows==RowsAtCompileTime) + && EIGEN_IMPLIES(ColsAtCompileTime!=Dynamic,cols==ColsAtCompileTime) + && EIGEN_IMPLIES(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic,rows<=MaxRowsAtCompileTime) + && EIGEN_IMPLIES(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 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 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 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() + { +// _check_template_params(); +// 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()) + { +// _check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } +#endif + +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + PlainObjectBase(PlainObjectBase&& other) EIGEN_NOEXCEPT + : m_storage( std::move(other.m_storage) ) + { + } + + EIGEN_DEVICE_FUNC + PlainObjectBase& operator=(PlainObjectBase&& other) EIGEN_NOEXCEPT + { + using std::swap; + swap(m_storage, other.m_storage); + return *this; + } +#endif + + /** 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) + { +// _check_template_params(); +// EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } + + /** \sa PlainObjectBase::operator=(const EigenBase&) */ + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase &other) + : m_storage() + { + _check_template_params(); + resizeLike(other); + _set_noalias(other); + } + + /** \sa PlainObjectBase::operator=(const EigenBase&) */ + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase &other) + : m_storage() + { + _check_template_params(); + resizeLike(other); + *this = other.derived(); + } + /** \brief Copy constructor with in-place evaluation */ + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE PlainObjectBase(const ReturnByValue& other) + { + _check_template_params(); + // 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); + + using Base::setZero; + EIGEN_DEVICE_FUNC Derived& setZero(Index size); + EIGEN_DEVICE_FUNC Derived& setZero(Index rows, Index cols); + + using Base::setOnes; + EIGEN_DEVICE_FUNC Derived& setOnes(Index size); + EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, Index cols); + + using Base::setRandom; + Derived& setRandom(Index size); + Derived& setRandom(Index rows, Index cols); + + #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 internall::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, typename internal::enable_if::type* = 0) + { + EIGEN_STATIC_ASSERT(bool(NumTraits::IsInteger) && + bool(NumTraits::IsInteger), + FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) + resize(rows,cols); + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init2(const T0& val0, const T1& val1, typename internal::enable_if::type* = 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, + typename internal::enable_if< (!internal::is_same::value) + && (internal::is_same::value) + && (internal::is_same::value) + && Base::SizeAtCompileTime==2,T1>::type* = 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, typename internal::enable_if< (Base::SizeAtCompileTime!=1 || !internal::is_convertible::value) + && ((!internal::is_same::XprKind,ArrayXpr>::value || Base::SizeAtCompileTime==Dynamic)),T>::type* = 0) + { + // NOTE MSVC 2008 complains if we directly put bool(NumTraits::IsInteger) as the EIGEN_STATIC_ASSERT argument. + const bool is_integer = NumTraits::IsInteger; + EIGEN_UNUSED_VARIABLE(is_integer); + EIGEN_STATIC_ASSERT(is_integer, + 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 implicitely converted) + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Scalar& val0, typename internal::enable_if::value,T>::type* = 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, + typename internal::enable_if< (!internal::is_same::value) + && (internal::is_same::value) + && Base::SizeAtCompileTime==1 + && internal::is_convertible::value,T*>::type* = 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, + typename internal::enable_if< Base::SizeAtCompileTime!=Dynamic + && Base::SizeAtCompileTime!=1 + && internal::is_convertible::value + && internal::is_same::XprKind,ArrayXpr>::value,T>::type* = 0) + { + Base::setConstant(val0); + } + + // For fixed-size Array + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Index& val0, + typename internal::enable_if< (!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*>::type* = 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 + 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 + void swap(DenseBase const & other) + { Base::swap(other.derived()); } + + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE void _check_template_params() + { + EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, (Options&RowMajor)==RowMajor) + && EIGEN_IMPLIES(MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1, (Options&RowMajor)==0) + && ((RowsAtCompileTime == Dynamic) || (RowsAtCompileTime >= 0)) + && ((ColsAtCompileTime == Dynamic) || (ColsAtCompileTime >= 0)) + && ((MaxRowsAtCompileTime == Dynamic) || (MaxRowsAtCompileTime >= 0)) + && ((MaxColsAtCompileTime == Dynamic) || (MaxColsAtCompileTime >= 0)) + && (MaxRowsAtCompileTime == RowsAtCompileTime || RowsAtCompileTime==Dynamic) + && (MaxColsAtCompileTime == ColsAtCompileTime || ColsAtCompileTime==Dynamic) + && (Options & (DontAlign|RowMajor)) == Options), + INVALID_MATRIX_TEMPLATE_PARAMETERS) + } + + enum { IsPlainObjectBase = 1 }; +#endif +}; + +namespace internal { + +template +struct conservative_resize_like_impl +{ + static void run(DenseBase& _this, Index rows, Index cols) + { + if (_this.rows() == rows && _this.cols() == cols) return; + EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(Derived) + + if ( ( 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. + typename Derived::PlainObject 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 ( ( 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. + typename Derived::PlainObject 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 +{ + using conservative_resize_like_impl::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; + _this.derived().m_storage.conservativeResize(size,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; + _this.derived().m_storage.conservativeResize(other.size(),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 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/examples/ThirdPartyLibs/Eigen/src/Core/Product.h b/examples/ThirdPartyLibs/Eigen/src/Core/Product.h new file mode 100644 index 000000000..ae0c94b38 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Product.h @@ -0,0 +1,186 @@ +// 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 + +namespace Eigen { + +template class ProductImpl; + +namespace internal { + +template +struct traits > +{ + typedef typename remove_all::type LhsCleaned; + typedef typename remove_all::type 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 = EIGEN_SIZE_MIN_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<_Lhs,_Rhs,Option, + typename internal::product_promote_storage_type::StorageKind, + typename internal::traits<_Rhs>::StorageKind, + internal::product_type<_Lhs,_Rhs>::ret>::ret> +{ + public: + + typedef _Lhs Lhs; + typedef _Rhs Rhs; + + typedef typename ProductImpl< + Lhs, Rhs, Option, + typename internal::product_promote_storage_type::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 typename internal::remove_all::type LhsNestedCleaned; + typedef typename internal::remove_all::type RhsNestedCleaned; + + EIGEN_DEVICE_FUNC 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 inline Index rows() const { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_rhs.cols(); } + + EIGEN_DEVICE_FUNC const LhsNestedCleaned& lhs() const { return m_lhs; } + EIGEN_DEVICE_FUNC 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 +{}; + +/** Convertion 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; + + 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 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 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/examples/ThirdPartyLibs/Eigen/src/Core/ProductEvaluators.h b/examples/ThirdPartyLibs/Eigen/src/Core/ProductEvaluators.h new file mode 100644 index 000000000..86966abdb --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/ProductEvaluators.h @@ -0,0 +1,1105 @@ +// 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 + +namespace Eigen { + +namespace internal { + +/** \internal + * Evaluator of a product expression. + * Since products require special treatments to handle all possible cases, + * we simply deffer 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 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 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 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< typename Lhs, typename Rhs, + typename LhsShape = typename evaluator_traits::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()) + { + ::new (static_cast(this)) Base(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 typename internal::remove_all::type LhsNestedCleaned; +// typedef typename internal::remove_all::type 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 excatly. +// TODO: we could enable them for different scalar types when the product is not vectorized. + +// Dense = Product +template< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar> +struct Assignment, internal::assign_op, Dense2Dense, + typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type> +{ + typedef Product SrcXprType; + static 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< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar> +struct Assignment, internal::add_assign_op, Dense2Dense, + typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type> +{ + typedef Product SrcXprType; + static 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< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar> +struct Assignment, internal::sub_assign_op, Dense2Dense, + typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type> +{ + typedef Product SrcXprType; + static 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< typename DstXprType, typename Lhs, typename Rhs, typename AssignFunc, typename Scalar, typename ScalarBis, typename Plain> +struct Assignment, const CwiseNullaryOp,Plain>, + const Product >, AssignFunc, Dense2Dense> +{ + typedef CwiseBinaryOp, + const CwiseNullaryOp,Plain>, + const Product > SrcXprType; + static 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::Scalar>, const OtherXpr, + const Product >, DenseShape > { + static const bool value = true; +}; + +template +struct evaluator_assume_aliasing::Scalar>, const OtherXpr, + const Product >, DenseShape > { + static const bool value = true; +}; + +template +struct assignment_from_xpr_op_product +{ + template + static 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< typename DstXprType, typename OtherXpr, typename Lhs, typename Rhs, typename DstScalar, typename SrcScalar, typename OtherScalar,typename ProdScalar> \ + 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 inline void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + dst.coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum(); + } + + template + static inline void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + dst.coeffRef(0,0) += (lhs.transpose().cwiseProduct(rhs)).sum(); + } + + template + static 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 outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const false_type&) +{ + evaluator rhsEval(rhs); + typename nested_eval::type actual_lhs(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 +void outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const true_type&) +{ + evaluator lhsEval(lhs); + typename nested_eval::type actual_rhs(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 +struct generic_product_impl +{ + template struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::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 void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() = src; } }; + struct add { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; } }; + struct sub { template 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 operator()(const Dst& dst, const Src& src) const { + dst.const_cast_derived() += m_scale * src; + } + }; + + template + static 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 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 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 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_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { dst.setZero(); scaleAndAddTo(dst, lhs, rhs, Scalar(1)); } + + template + static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { scaleAndAddTo(dst,lhs, rhs, Scalar(1)); } + + template + static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { scaleAndAddTo(dst, lhs, rhs, Scalar(-1)); } + + template + static 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 typename internal::remove_all::type>::type MatrixType; + + template + static EIGEN_STRONG_INLINE void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + 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_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_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_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()); + } + +// template +// static inline void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) +// { dst.noalias() += alpha * lhs.lazyProduct(rhs); } +}; + +// 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 typename internal::remove_all::type LhsNestedCleaned; + typedef typename internal::remove_all::type RhsNestedCleaned; + + typedef evaluator LhsEtorType; + typedef evaluator RhsEtorType; + + enum { + RowsAtCompileTime = LhsNestedCleaned::RowsAtCompileTime, + ColsAtCompileTime = RhsNestedCleaned::ColsAtCompileTime, + InnerSize = EIGEN_SIZE_MIN_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 + LhsCoeffReadCost + 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 = EIGEN_PLAIN_ENUM_MIN(LhsEtorType::Alignment,LhsVecPacketSize*int(sizeof(typename LhsNestedCleaned::Scalar))), + RhsAlignment = EIGEN_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 = ((unsigned int)(LhsFlags | 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) % EIGEN_PLAIN_ENUM_MAX(1,LhsAlignment))!=0 ? 0 : LhsAlignment) + : bool(CanVectorizeRhs) ? (RhsOuterStrideBytes<=0 || (int(RhsOuterStrideBytes) % EIGEN_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) + && (LhsFlags & RhsFlags & ActualPacketAccessBit) + && (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 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 + 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 + 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: + typename internal::add_const_on_value_type::type m_lhs; + typename internal::add_const_on_value_type::type 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 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_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_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_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_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_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_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_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_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 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 = NumTraits::MulCost + evaluator::CoeffReadCost + evaluator::CoeffReadCost, + + MatrixFlags = evaluator::Flags, + DiagFlags = evaluator::Flags, + _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 && (_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 + }; + + 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 + { + 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 = EIGEN_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::m_diagImpl; + using Base::m_matImpl; + using Base::coeff; + typedef typename Base::Scalar Scalar; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + + enum { + StorageOrder = int(Rhs::Flags) & RowMajorBit ? RowMajor : ColMajor + }; + + 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_CUDACC + 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, + typename internal::conditional::type()); + } + + 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::m_diagImpl; + using Base::m_matImpl; + using Base::coeff; + typedef typename Base::Scalar Scalar; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + + enum { StorageOrder = int(Lhs::Flags) & RowMajorBit ? RowMajor : ColMajor }; + + 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_CUDACC + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const + { + return this->template packet_impl(row,col, col, + typename internal::conditional::type()); + } + + 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 typename remove_all::type MatrixTypeCleaned; + + template + static 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()) + 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 + (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 + (mat, ((Side==OnTheRight) ^ Transposed) ? perm.indices().coeff(i) : i); + } + } + } +}; + +template +struct generic_product_impl +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + permutation_matrix_product::run(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl +{ + template + static 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 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 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 typename remove_all::type MatrixTypeCleaned; + + template + static 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 +struct generic_product_impl +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + transposition_matrix_product::run(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl +{ + template + static 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 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 void evalTo(Dest& dst, const Lhs& lhs, const Transpose& rhs) + { + transposition_matrix_product::run(dst, rhs.nestedExpression(), lhs); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_PRODUCT_EVALUATORS_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Random.h b/examples/ThirdPartyLibs/Eigen/src/Core/Random.h new file mode 100644 index 000000000..486e9ed52 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Random.h @@ -0,0 +1,182 @@ +// 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 + +namespace Eigen { + +namespace internal { + +template struct scalar_random_op { + EIGEN_EMPTY_STRUCT_CTOR(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(); +} + +} // end namespace Eigen + +#endif // EIGEN_RANDOM_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Redux.h b/examples/ThirdPartyLibs/Eigen/src/Core/Redux.h new file mode 100644 index 000000000..2b5b73bf7 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Redux.h @@ -0,0 +1,505 @@ +// 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 + +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(Derived::IsRowMajor) + ? Derived::MaxColsAtCompileTime + : Derived::MaxRowsAtCompileTime + }; + + enum { + MightVectorize = (int(Derived::Flags)&ActualPacketAccessBit) + && (functor_traits::PacketAccess), + MayLinearVectorize = bool(MightVectorize) && (int(Derived::Flags)&LinearAccessBit), + MaySliceVectorize = bool(MightVectorize) && int(InnerMaxSize)>=3*PacketSize + }; + +public: + enum { + Traversal = int(MayLinearVectorize) ? int(LinearVectorizedTraversal) + : int(MaySliceVectorize) ? int(SliceVectorizedTraversal) + : int(DefaultTraversal) + }; + +public: + enum { + Cost = Derived::SizeAtCompileTime == Dynamic ? HugeCost + : Derived::SizeAtCompileTime * Derived::CoeffReadCost + (Derived::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 Derived::XprType).name() << std::endl; + std::cerr.setf(std::ios::hex, std::ios::basefield); + EIGEN_DEBUG_VAR(Derived::Flags) + std::cerr.unsetf(std::ios::hex); + EIGEN_DEBUG_VAR(InnerMaxSize) + EIGEN_DEBUG_VAR(PacketSize) + EIGEN_DEBUG_VAR(MightVectorize) + EIGEN_DEBUG_VAR(MayLinearVectorize) + EIGEN_DEBUG_VAR(MaySliceVectorize) + EIGEN_DEBUG_VAR(Traversal) + EIGEN_DEBUG_VAR(UnrollingLimit) + EIGEN_DEBUG_VAR(Unrolling) + std::cerr << std::endl; + } +#endif +}; + +/*************************************************************************** +* Part 2 : unrollers +***************************************************************************/ + +/*** no vectorization ***/ + +template +struct redux_novec_unroller +{ + enum { + HalfLength = Length/2 + }; + + typedef typename Derived::Scalar Scalar; + + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func) + { + return func(redux_novec_unroller::run(mat,func), + redux_novec_unroller::run(mat,func)); + } +}; + +template +struct redux_novec_unroller +{ + enum { + outer = Start / Derived::InnerSizeAtCompileTime, + inner = Start % Derived::InnerSizeAtCompileTime + }; + + typedef typename Derived::Scalar Scalar; + + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func&) + { + return mat.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 Derived::Scalar Scalar; + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE Scalar run(const Derived&, const Func&) { return Scalar(); } +}; + +/*** vectorization ***/ + +template +struct redux_vec_unroller +{ + enum { + PacketSize = redux_traits::PacketSize, + HalfLength = Length/2 + }; + + typedef typename Derived::Scalar Scalar; + typedef typename redux_traits::PacketType PacketScalar; + + static EIGEN_STRONG_INLINE PacketScalar run(const Derived &mat, const Func& func) + { + return func.packetOp( + redux_vec_unroller::run(mat,func), + redux_vec_unroller::run(mat,func) ); + } +}; + +template +struct redux_vec_unroller +{ + enum { + index = Start * redux_traits::PacketSize, + outer = index / int(Derived::InnerSizeAtCompileTime), + inner = index % int(Derived::InnerSizeAtCompileTime), + alignment = Derived::Alignment + }; + + typedef typename Derived::Scalar Scalar; + typedef typename redux_traits::PacketType PacketScalar; + + static EIGEN_STRONG_INLINE PacketScalar run(const Derived &mat, const Func&) + { + return mat.template packetByOuterInner(outer, inner); + } +}; + +/*************************************************************************** +* Part 3 : implementation of all cases +***************************************************************************/ + +template::Traversal, + int Unrolling = redux_traits::Unrolling +> +struct redux_impl; + +template +struct redux_impl +{ + typedef typename Derived::Scalar Scalar; + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func) + { + eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix"); + Scalar res; + res = mat.coeffByOuterInner(0, 0); + for(Index i = 1; i < mat.innerSize(); ++i) + res = func(res, mat.coeffByOuterInner(0, i)); + for(Index i = 1; i < mat.outerSize(); ++i) + for(Index j = 0; j < mat.innerSize(); ++j) + res = func(res, mat.coeffByOuterInner(i, j)); + return res; + } +}; + +template +struct redux_impl + : public redux_novec_unroller +{}; + +template +struct redux_impl +{ + typedef typename Derived::Scalar Scalar; + typedef typename redux_traits::PacketType PacketScalar; + + static Scalar run(const Derived &mat, const Func& func) + { + const Index size = mat.size(); + + const Index packetSize = redux_traits::PacketSize; + const int packetAlignment = unpacket_traits::alignment; + enum { + alignment0 = (bool(Derived::Flags & DirectAccessBit) && bool(packet_traits::AlignedOnScalar)) ? int(packetAlignment) : int(Unaligned), + alignment = EIGEN_PLAIN_ENUM_MAX(alignment0, Derived::Alignment) + }; + const Index alignedStart = internal::first_default_aligned(mat.nestedExpression()); + 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 = mat.template packet(alignedStart); + if(alignedSize>packetSize) // we have at least two packets to partly unroll the loop + { + PacketScalar packet_res1 = mat.template packet(alignedStart+packetSize); + for(Index index = alignedStart + 2*packetSize; index < alignedEnd2; index += 2*packetSize) + { + packet_res0 = func.packetOp(packet_res0, mat.template packet(index)); + packet_res1 = func.packetOp(packet_res1, mat.template packet(index+packetSize)); + } + + packet_res0 = func.packetOp(packet_res0,packet_res1); + if(alignedEnd>alignedEnd2) + packet_res0 = func.packetOp(packet_res0, mat.template packet(alignedEnd2)); + } + res = func.predux(packet_res0); + + for(Index index = 0; index < alignedStart; ++index) + res = func(res,mat.coeff(index)); + + for(Index index = alignedEnd; index < size; ++index) + res = func(res,mat.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 = mat.coeff(0); + for(Index index = 1; index < size; ++index) + res = func(res,mat.coeff(index)); + } + + return res; + } +}; + +// NOTE: for SliceVectorizedTraversal we simply bypass unrolling +template +struct redux_impl +{ + typedef typename Derived::Scalar Scalar; + typedef typename redux_traits::PacketType PacketType; + + EIGEN_DEVICE_FUNC static Scalar run(const Derived &mat, const Func& func) + { + eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix"); + const Index innerSize = mat.innerSize(); + const Index outerSize = mat.outerSize(); + enum { + packetSize = redux_traits::PacketSize + }; + const Index packetedInnerSize = ((innerSize)/packetSize)*packetSize; + Scalar res; + if(packetedInnerSize) + { + PacketType packet_res = mat.template packet(0,0); + for(Index j=0; j(j,i)); + + res = func.predux(packet_res); + for(Index j=0; j::run(mat, func); + } + + return res; + } +}; + +template +struct redux_impl +{ + typedef typename Derived::Scalar Scalar; + + typedef typename redux_traits::PacketType PacketScalar; + enum { + PacketSize = redux_traits::PacketSize, + Size = Derived::SizeAtCompileTime, + VectorizedSize = (Size / PacketSize) * PacketSize + }; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func) + { + eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix"); + if (VectorizedSize > 0) { + Scalar res = func.predux(redux_vec_unroller::run(mat,func)); + if (VectorizedSize != Size) + res = func(res,redux_novec_unroller::run(mat,func)); + return res; + } + else { + return redux_novec_unroller::run(mat,func); + } + } +}; + +// evaluator adaptor +template +class redux_evaluator +{ +public: + typedef _XprType XprType; + EIGEN_DEVICE_FUNC explicit redux_evaluator(const XprType &xpr) : m_evaluator(xpr), m_xpr(xpr) {} + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename XprType::PacketScalar PacketScalar; + typedef typename XprType::PacketReturnType PacketReturnType; + + 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 = evaluator::Flags & ~DirectAccessBit, + IsRowMajor = XprType::IsRowMajor, + SizeAtCompileTime = XprType::SizeAtCompileTime, + InnerSizeAtCompileTime = XprType::InnerSizeAtCompileTime, + CoeffReadCost = evaluator::CoeffReadCost, + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); } + EIGEN_DEVICE_FUNC Index size() const { return m_xpr.size(); } + EIGEN_DEVICE_FUNC Index innerSize() const { return m_xpr.innerSize(); } + EIGEN_DEVICE_FUNC Index outerSize() const { return m_xpr.outerSize(); } + + EIGEN_DEVICE_FUNC + CoeffReturnType coeff(Index row, Index col) const + { return m_evaluator.coeff(row, col); } + + EIGEN_DEVICE_FUNC + CoeffReturnType coeff(Index index) const + { return m_evaluator.coeff(index); } + + template + PacketType packet(Index row, Index col) const + { return m_evaluator.template packet(row, col); } + + template + PacketType packet(Index index) const + { return m_evaluator.template packet(index); } + + EIGEN_DEVICE_FUNC + CoeffReturnType coeffByOuterInner(Index outer, Index inner) const + { return m_evaluator.coeff(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); } + + template + PacketType packetByOuterInner(Index outer, Index inner) const + { return m_evaluator.template packet(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); } + + const XprType & nestedExpression() const { return m_xpr; } + +protected: + internal::evaluator m_evaluator; + const XprType &m_xpr; +}; + +} // 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. + * + * \sa DenseBase::sum(), DenseBase::minCoeff(), DenseBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise() + */ +template +template +EIGEN_DEVICE_FUNC 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()); + + return internal::redux_impl::run(thisEval, func); +} + +/** \returns the minimum of all coefficients of \c *this. + * \warning the result is undefined if \c *this contains NaN. + */ +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. + * \warning the result is undefined if \c *this contains NaN. + */ +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/examples/ThirdPartyLibs/Eigen/src/Core/Ref.h b/examples/ThirdPartyLibs/Eigen/src/Core/Ref.h new file mode 100644 index 000000000..abb1e5121 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Ref.h @@ -0,0 +1,283 @@ +// 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 + +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 + }; + + template struct match { + enum { + HasDirectAccess = internal::has_direct_access::ret, + StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)), + InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic) + || int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime) + || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1), + OuterStrideMatch = Derived::IsVectorAtCompileTime + || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::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 typename internal::conditional::type type; + }; + +}; + +template +struct traits > : public traits {}; + +} + +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 inline Index innerStride() const + { + return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; + } + + EIGEN_DEVICE_FUNC 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; + + template + EIGEN_DEVICE_FUNC void construct(Expression& expr) + { + if(PlainObjectType::RowsAtCompileTime==1) + { + eigen_assert(expr.rows()==1 || expr.cols()==1); + ::new (static_cast(this)) Base(expr.data(), 1, expr.size()); + } + else if(PlainObjectType::ColsAtCompileTime==1) + { + eigen_assert(expr.rows()==1 || expr.cols()==1); + ::new (static_cast(this)) Base(expr.data(), expr.size(), 1); + } + else + ::new (static_cast(this)) Base(expr.data(), expr.rows(), expr.cols()); + + if(Expression::IsVectorAtCompileTime && (!PlainObjectType::IsVectorAtCompileTime) && ((Expression::Flags&RowMajorBit)!=(PlainObjectType::Flags&RowMajorBit))) + ::new (&m_stride) StrideBase(expr.innerStride(), StrideType::InnerStrideAtCompileTime==0?0:1); + else + ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(), + StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride()); + } + + 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, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0); + public: + + typedef RefBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Ref) + + + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC inline Ref(PlainObjectBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) + { + EIGEN_STATIC_ASSERT(bool(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + Base::construct(expr.derived()); + } + template + EIGEN_DEVICE_FUNC inline Ref(const DenseBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 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); + Base::construct(expr.const_cast_derived()); + } + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Ref) + +}; + +// this is the const ref version +template class Ref + : public RefBase > +{ + typedef internal::traits Traits; + public: + + typedef RefBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Ref) + + template + EIGEN_DEVICE_FUNC inline Ref(const DenseBase& expr, + typename internal::enable_if::ScalarTypeMatch),Derived>::type* = 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"; + 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 + } + + template + EIGEN_DEVICE_FUNC inline Ref(const RefBase& other) { + construct(other.derived(), typename Traits::template match::type()); + } + + protected: + + template + EIGEN_DEVICE_FUNC void construct(const Expression& expr,internal::true_type) + { + Base::construct(expr); + } + + template + EIGEN_DEVICE_FUNC void construct(const Expression& expr, internal::false_type) + { + internal::call_assignment_no_alias(m_object,expr,internal::assign_op()); + Base::construct(m_object); + } + + protected: + TPlainObjectType m_object; +}; + +} // end namespace Eigen + +#endif // EIGEN_REF_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Replicate.h b/examples/ThirdPartyLibs/Eigen/src/Core/Replicate.h new file mode 100644 index 000000000..0b2d6d743 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Replicate.h @@ -0,0 +1,142 @@ +// 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 + +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 typename remove_reference::type _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 + }; +}; +} + +/** + * \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< Replicate >::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 typename internal::remove_all::type 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::type,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::type,OriginalMatrixType>::value), + THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE) + } + + EIGEN_DEVICE_FUNC + inline Index rows() const { return m_matrix.rows() * m_rowFactor.value(); } + EIGEN_DEVICE_FUNC + 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/examples/ThirdPartyLibs/Eigen/src/Core/ReturnByValue.h b/examples/ThirdPartyLibs/Eigen/src/Core/ReturnByValue.h new file mode 100644 index 000000000..11dc86d07 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/ReturnByValue.h @@ -0,0 +1,117 @@ +// 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 + +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< ReturnByValue >::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 inline Index rows() const { return static_cast(this)->rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { 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()) + { + ::new (static_cast(this)) Base(m_result); + xpr.evalTo(m_result); + } + +protected: + PlainObject m_result; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_RETURNBYVALUE_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Reverse.h b/examples/ThirdPartyLibs/Eigen/src/Core/Reverse.h new file mode 100644 index 000000000..8b6b3ab03 --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Reverse.h @@ -0,0 +1,211 @@ +// 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 + +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 typename remove_reference::type _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< Reverse >::type +{ + public: + + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Reverse) + typedef typename internal::remove_all::type 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 inline Index rows() const { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.cols(); } + + EIGEN_DEVICE_FUNC inline Index innerStride() const + { + return -m_matrix.innerStride(); + } + + EIGEN_DEVICE_FUNC const typename internal::remove_all::type& + 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) + { + Index half = xpr.rows()/2; + xpr.topRows(half).swap(xpr.bottomRows(half).colwise().reverse()); + } +}; + +template<> +struct vectorwise_reverse_inplace_impl +{ + template + static void run(ExpressionType &xpr) + { + Index half = xpr.cols()/2; + xpr.leftCols(half).swap(xpr.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(_expression().const_cast_derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_REVERSE_H diff --git a/examples/ThirdPartyLibs/Eigen/src/Core/Select.h b/examples/ThirdPartyLibs/Eigen/src/Core/Select.h new file mode 100644 index 000000000..79eec1b5b --- /dev/null +++ b/examples/ThirdPartyLibs/Eigen/src/Core/Select.h @@ -0,0 +1,162 @@ +// 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 + +namespace Eigen { + +/** \class Select + * \ingroup Core_Module + * + * \brief Expression of a coefficient wise version of the C++ ternary operator ?: + * + * \param ConditionMatrixType the type of the \em condition expression which must be a boolean matrix + * \param ThenMatrixType the type of the \em then expression + * \param 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 + }; +}; +} + +template +class Select : public internal::dense_xpr_base< Select >::type, + internal::no_assignment_operator +{ + public: + + typedef typename internal::dense_xpr_base