diff options
Diffstat (limited to 'unsupported/Eigen')
81 files changed, 15136 insertions, 0 deletions
diff --git a/unsupported/Eigen/AdolcForward b/unsupported/Eigen/AdolcForward new file mode 100644 index 000000000..b96f9450e --- /dev/null +++ b/unsupported/Eigen/AdolcForward @@ -0,0 +1,156 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr> +// +// 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_ADLOC_FORWARD +#define EIGEN_ADLOC_FORWARD + +//-------------------------------------------------------------------------------- +// +// This file provides support for adolc's adouble type in forward mode. +// ADOL-C is a C++ automatic differentiation library, +// see https://projects.coin-or.org/ADOL-C for more information. +// +// Note that the maximal number of directions is controlled by +// the preprocessor token NUMBER_DIRECTIONS. The default is 2. +// +//-------------------------------------------------------------------------------- + +#define ADOLC_TAPELESS +#ifndef NUMBER_DIRECTIONS +# define NUMBER_DIRECTIONS 2 +#endif +#include <adolc/adouble.h> + +// adolc defines some very stupid macros: +#if defined(malloc) +# undef malloc +#endif + +#if defined(calloc) +# undef calloc +#endif + +#if defined(realloc) +# undef realloc +#endif + +#include <Eigen/Core> + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup AdolcForward_Module Adolc forward module + * This module provides support for adolc's adouble type in forward mode. + * ADOL-C is a C++ automatic differentiation library, + * see https://projects.coin-or.org/ADOL-C for more information. + * It mainly consists in: + * - a struct Eigen::NumTraits<adtl::adouble> specialization + * - overloads of internal::* math function for adtl::adouble type. + * + * Note that the maximal number of directions is controlled by + * the preprocessor token NUMBER_DIRECTIONS. The default is 2. + * + * \code + * #include <unsupported/Eigen/AdolcSupport> + * \endcode + */ + //@{ + +} // namespace Eigen + +// Eigen's require a few additional functions which must be defined in the same namespace +// than the custom scalar type own namespace +namespace adtl { + +inline const adouble& conj(const adouble& x) { return x; } +inline const adouble& real(const adouble& x) { return x; } +inline adouble imag(const adouble&) { return 0.; } +inline adouble abs(const adouble& x) { return fabs(x); } +inline adouble abs2(const adouble& x) { return x*x; } + +} + +namespace Eigen { + +template<> struct NumTraits<adtl::adouble> + : NumTraits<double> +{ + typedef adtl::adouble Real; + typedef adtl::adouble NonInteger; + typedef adtl::adouble Nested; + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 1, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 1, + MulCost = 1 + }; +}; + +template<typename Functor> class AdolcForwardJacobian : public Functor +{ + typedef adtl::adouble ActiveScalar; +public: + + AdolcForwardJacobian() : Functor() {} + AdolcForwardJacobian(const Functor& f) : Functor(f) {} + + // forward constructors + template<typename T0> + AdolcForwardJacobian(const T0& a0) : Functor(a0) {} + template<typename T0, typename T1> + AdolcForwardJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {} + template<typename T0, typename T1, typename T2> + AdolcForwardJacobian(const T0& a0, const T1& a1, const T1& a2) : Functor(a0, a1, a2) {} + + typedef typename Functor::InputType InputType; + typedef typename Functor::ValueType ValueType; + typedef typename Functor::JacobianType JacobianType; + + typedef Matrix<ActiveScalar, InputType::SizeAtCompileTime, 1> ActiveInput; + typedef Matrix<ActiveScalar, ValueType::SizeAtCompileTime, 1> ActiveValue; + + void operator() (const InputType& x, ValueType* v, JacobianType* _jac) const + { + eigen_assert(v!=0); + if (!_jac) + { + Functor::operator()(x, v); + return; + } + + JacobianType& jac = *_jac; + + ActiveInput ax = x.template cast<ActiveScalar>(); + ActiveValue av(jac.rows()); + + for (int j=0; j<jac.cols(); j++) + for (int i=0; i<jac.cols(); i++) + ax[i].setADValue(j, i==j ? 1 : 0); + + Functor::operator()(ax, &av); + + for (int i=0; i<jac.rows(); i++) + { + (*v)[i] = av[i].getValue(); + for (int j=0; j<jac.cols(); j++) + jac.coeffRef(i,j) = av[i].getADValue(j); + } + } +protected: + +}; + +//@} + +} + +#endif // EIGEN_ADLOC_FORWARD diff --git a/unsupported/Eigen/AlignedVector3 b/unsupported/Eigen/AlignedVector3 new file mode 100644 index 000000000..8ad0eb477 --- /dev/null +++ b/unsupported/Eigen/AlignedVector3 @@ -0,0 +1,189 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr> +// +// 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_ALIGNED_VECTOR3 +#define EIGEN_ALIGNED_VECTOR3 + +#include <Eigen/Geometry> + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup AlignedVector3_Module Aligned vector3 module + * + * \code + * #include <unsupported/Eigen/AlignedVector3> + * \endcode + */ + //@{ + + +/** \class AlignedVector3 + * + * \brief A vectorization friendly 3D vector + * + * This class represents a 3D vector internally using a 4D vector + * such that vectorization can be seamlessly enabled. Of course, + * the same result can be achieved by directly using a 4D vector. + * This class makes this process simpler. + * + */ +// TODO specialize Cwise +template<typename _Scalar> class AlignedVector3; + +namespace internal { +template<typename _Scalar> struct traits<AlignedVector3<_Scalar> > + : traits<Matrix<_Scalar,3,1,0,4,1> > +{ +}; +} + +template<typename _Scalar> class AlignedVector3 + : public MatrixBase<AlignedVector3<_Scalar> > +{ + typedef Matrix<_Scalar,4,1> CoeffType; + CoeffType m_coeffs; + public: + + typedef MatrixBase<AlignedVector3<_Scalar> > Base; + EIGEN_DENSE_PUBLIC_INTERFACE(AlignedVector3) + using Base::operator*; + + inline Index rows() const { return 3; } + inline Index cols() const { return 1; } + + inline const Scalar& coeff(Index row, Index col) const + { return m_coeffs.coeff(row, col); } + + inline Scalar& coeffRef(Index row, Index col) + { return m_coeffs.coeffRef(row, col); } + + inline const Scalar& coeff(Index index) const + { return m_coeffs.coeff(index); } + + inline Scalar& coeffRef(Index index) + { return m_coeffs.coeffRef(index);} + + + inline AlignedVector3(const Scalar& x, const Scalar& y, const Scalar& z) + : m_coeffs(x, y, z, Scalar(0)) + {} + + inline AlignedVector3(const AlignedVector3& other) + : Base(), m_coeffs(other.m_coeffs) + {} + + template<typename XprType, int Size=XprType::SizeAtCompileTime> + struct generic_assign_selector {}; + + template<typename XprType> struct generic_assign_selector<XprType,4> + { + inline static void run(AlignedVector3& dest, const XprType& src) + { + dest.m_coeffs = src; + } + }; + + template<typename XprType> struct generic_assign_selector<XprType,3> + { + inline static void run(AlignedVector3& dest, const XprType& src) + { + dest.m_coeffs.template head<3>() = src; + dest.m_coeffs.w() = Scalar(0); + } + }; + + template<typename Derived> + inline explicit AlignedVector3(const MatrixBase<Derived>& other) + { + generic_assign_selector<Derived>::run(*this,other.derived()); + } + + inline AlignedVector3& operator=(const AlignedVector3& other) + { m_coeffs = other.m_coeffs; return *this; } + + + inline AlignedVector3 operator+(const AlignedVector3& other) const + { return AlignedVector3(m_coeffs + other.m_coeffs); } + + inline AlignedVector3& operator+=(const AlignedVector3& other) + { m_coeffs += other.m_coeffs; return *this; } + + inline AlignedVector3 operator-(const AlignedVector3& other) const + { return AlignedVector3(m_coeffs - other.m_coeffs); } + + inline AlignedVector3 operator-=(const AlignedVector3& other) + { m_coeffs -= other.m_coeffs; return *this; } + + inline AlignedVector3 operator*(const Scalar& s) const + { return AlignedVector3(m_coeffs * s); } + + inline friend AlignedVector3 operator*(const Scalar& s,const AlignedVector3& vec) + { return AlignedVector3(s * vec.m_coeffs); } + + inline AlignedVector3& operator*=(const Scalar& s) + { m_coeffs *= s; return *this; } + + inline AlignedVector3 operator/(const Scalar& s) const + { return AlignedVector3(m_coeffs / s); } + + inline AlignedVector3& operator/=(const Scalar& s) + { m_coeffs /= s; return *this; } + + inline Scalar dot(const AlignedVector3& other) const + { + eigen_assert(m_coeffs.w()==Scalar(0)); + eigen_assert(other.m_coeffs.w()==Scalar(0)); + return m_coeffs.dot(other.m_coeffs); + } + + inline void normalize() + { + m_coeffs /= norm(); + } + + inline AlignedVector3 normalized() + { + return AlignedVector3(m_coeffs / norm()); + } + + inline Scalar sum() const + { + eigen_assert(m_coeffs.w()==Scalar(0)); + return m_coeffs.sum(); + } + + inline Scalar squaredNorm() const + { + eigen_assert(m_coeffs.w()==Scalar(0)); + return m_coeffs.squaredNorm(); + } + + inline Scalar norm() const + { + return internal::sqrt(squaredNorm()); + } + + inline AlignedVector3 cross(const AlignedVector3& other) const + { + return AlignedVector3(m_coeffs.cross3(other.m_coeffs)); + } + + template<typename Derived> + inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps=NumTraits<Scalar>::dummy_precision()) const + { + return m_coeffs.template head<3>().isApprox(other,eps); + } +}; + +//@} + +} + +#endif // EIGEN_ALIGNED_VECTOR3 diff --git a/unsupported/Eigen/AutoDiff b/unsupported/Eigen/AutoDiff new file mode 100644 index 000000000..3c73b424e --- /dev/null +++ b/unsupported/Eigen/AutoDiff @@ -0,0 +1,40 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr> +// +// 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_AUTODIFF_MODULE +#define EIGEN_AUTODIFF_MODULE + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup AutoDiff_Module Auto Diff module + * + * This module features forward automatic differentation via a simple + * templated scalar type wrapper AutoDiffScalar. + * + * Warning : this should NOT be confused with numerical differentiation, which + * is a different method and has its own module in Eigen : \ref NumericalDiff_Module. + * + * \code + * #include <unsupported/Eigen/AutoDiff> + * \endcode + */ +//@{ + +} + +#include "src/AutoDiff/AutoDiffScalar.h" +// #include "src/AutoDiff/AutoDiffVector.h" +#include "src/AutoDiff/AutoDiffJacobian.h" + +namespace Eigen { +//@} +} + +#endif // EIGEN_AUTODIFF_MODULE diff --git a/unsupported/Eigen/BVH b/unsupported/Eigen/BVH new file mode 100644 index 000000000..860a7dd89 --- /dev/null +++ b/unsupported/Eigen/BVH @@ -0,0 +1,95 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Ilya Baran <ibaran@mit.edu> +// +// 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_BVH_MODULE_H +#define EIGEN_BVH_MODULE_H + +#include <Eigen/Core> +#include <Eigen/Geometry> +#include <Eigen/StdVector> +#include <algorithm> +#include <queue> + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup BVH_Module BVH module + * \brief This module provides generic bounding volume hierarchy algorithms + * and reference tree implementations. + * + * + * \code + * #include <unsupported/Eigen/BVH> + * \endcode + * + * A bounding volume hierarchy (BVH) can accelerate many geometric queries. This module provides a generic implementation + * of the two basic algorithms over a BVH: intersection of a query object against all objects in the hierarchy and minimization + * of a function over the objects in the hierarchy. It also provides intersection and minimization over a cartesian product of + * two BVH's. A BVH accelerates intersection by using the fact that if a query object does not intersect a volume, then it cannot + * intersect any object contained in that volume. Similarly, a BVH accelerates minimization because the minimum of a function + * over a volume is no greater than the minimum of a function over any object contained in it. + * + * Some sample queries that can be written in terms of intersection are: + * - Determine all points where a ray intersects a triangle mesh + * - Given a set of points, determine which are contained in a query sphere + * - Given a set of spheres, determine which contain the query point + * - Given a set of disks, determine if any is completely contained in a query rectangle (represent each 2D disk as a point \f$(x,y,r)\f$ + * in 3D and represent the rectangle as a pyramid based on the original rectangle and shrinking in the \f$r\f$ direction) + * - Given a set of points, count how many pairs are \f$d\pm\epsilon\f$ apart (done by looking at the cartesian product of the set + * of points with itself) + * + * Some sample queries that can be written in terms of function minimization over a set of objects are: + * - Find the intersection between a ray and a triangle mesh closest to the ray origin (function is infinite off the ray) + * - Given a polyline and a query point, determine the closest point on the polyline to the query + * - Find the diameter of a point cloud (done by looking at the cartesian product and using negative distance as the function) + * - Determine how far two meshes are from colliding (this is also a cartesian product query) + * + * This implementation decouples the basic algorithms both from the type of hierarchy (and the types of the bounding volumes) and + * from the particulars of the query. To enable abstraction from the BVH, the BVH is required to implement a generic mechanism + * for traversal. To abstract from the query, the query is responsible for keeping track of results. + * + * To be used in the algorithms, a hierarchy must implement the following traversal mechanism (see KdBVH for a sample implementation): \code + typedef Volume //the type of bounding volume + typedef Object //the type of object in the hierarchy + typedef Index //a reference to a node in the hierarchy--typically an int or a pointer + typedef VolumeIterator //an iterator type over node children--returns Index + typedef ObjectIterator //an iterator over object (leaf) children--returns const Object & + Index getRootIndex() const //returns the index of the hierarchy root + const Volume &getVolume(Index index) const //returns the bounding volume of the node at given index + void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, + ObjectIterator &outOBegin, ObjectIterator &outOEnd) const + //getChildren takes a node index and makes [outVBegin, outVEnd) range over its node children + //and [outOBegin, outOEnd) range over its object children + \endcode + * + * To use the hierarchy, call BVIntersect or BVMinimize, passing it a BVH (or two, for cartesian product) and a minimizer or intersector. + * For an intersection query on a single BVH, the intersector encapsulates the query and must provide two functions: + * \code + bool intersectVolume(const Volume &volume) //returns true if the query intersects the volume + bool intersectObject(const Object &object) //returns true if the intersection search should terminate immediately + \endcode + * The guarantee that BVIntersect provides is that intersectObject will be called on every object whose bounding volume + * intersects the query (but possibly on other objects too) unless the search is terminated prematurely. It is the + * responsibility of the intersectObject function to keep track of the results in whatever manner is appropriate. + * The cartesian product intersection and the BVMinimize queries are similar--see their individual documentation. + * + * The following is a simple but complete example for how to use the BVH to accelerate the search for a closest red-blue point pair: + * \include BVH_Example.cpp + * Output: \verbinclude BVH_Example.out + */ +} + +//@{ + +#include "src/BVH/BVAlgorithms.h" +#include "src/BVH/KdBVH.h" + +//@} + +#endif // EIGEN_BVH_MODULE_H diff --git a/unsupported/Eigen/CMakeLists.txt b/unsupported/Eigen/CMakeLists.txt new file mode 100644 index 000000000..e961e72c5 --- /dev/null +++ b/unsupported/Eigen/CMakeLists.txt @@ -0,0 +1,11 @@ +set(Eigen_HEADERS AdolcForward BVH IterativeSolvers MatrixFunctions MoreVectorization AutoDiff AlignedVector3 Polynomials + FFT NonLinearOptimization SparseExtra IterativeSolvers + NumericalDiff Skyline MPRealSupport OpenGLSupport KroneckerProduct Splines + ) + +install(FILES + ${Eigen_HEADERS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen COMPONENT Devel + ) + +add_subdirectory(src) diff --git a/unsupported/Eigen/FFT b/unsupported/Eigen/FFT new file mode 100644 index 000000000..d233c6d5f --- /dev/null +++ b/unsupported/Eigen/FFT @@ -0,0 +1,418 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Mark Borgerding mark a borgerding net +// +// 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_FFT_H +#define EIGEN_FFT_H + +#include <complex> +#include <vector> +#include <map> +#include <Eigen/Core> + + +/** \ingroup Unsupported_modules + * \defgroup FFT_Module Fast Fourier Transform module + * + * \code + * #include <unsupported/Eigen/FFT> + * \endcode + * + * This module provides Fast Fourier transformation, with a configurable backend + * implementation. + * + * The default implementation is based on kissfft. It is a small, free, and + * reasonably efficient default. + * + * There are currently two implementation backend: + * + * - fftw (http://www.fftw.org) : faster, GPL -- incompatible with Eigen in LGPL form, bigger code size. + * - MKL (http://en.wikipedia.org/wiki/Math_Kernel_Library) : fastest, commercial -- may be incompatible with Eigen in GPL form. + * + * \section FFTDesign Design + * + * The following design decisions were made concerning scaling and + * half-spectrum for real FFT. + * + * The intent is to facilitate generic programming and ease migrating code + * from Matlab/octave. + * We think the default behavior of Eigen/FFT should favor correctness and + * generality over speed. Of course, the caller should be able to "opt-out" from this + * behavior and get the speed increase if they want it. + * + * 1) %Scaling: + * Other libraries (FFTW,IMKL,KISSFFT) do not perform scaling, so there + * is a constant gain incurred after the forward&inverse transforms , so + * IFFT(FFT(x)) = Kx; this is done to avoid a vector-by-value multiply. + * The downside is that algorithms that worked correctly in Matlab/octave + * don't behave the same way once implemented in C++. + * + * How Eigen/FFT differs: invertible scaling is performed so IFFT( FFT(x) ) = x. + * + * 2) Real FFT half-spectrum + * Other libraries use only half the frequency spectrum (plus one extra + * sample for the Nyquist bin) for a real FFT, the other half is the + * conjugate-symmetric of the first half. This saves them a copy and some + * memory. The downside is the caller needs to have special logic for the + * number of bins in complex vs real. + * + * How Eigen/FFT differs: The full spectrum is returned from the forward + * transform. This facilitates generic template programming by obviating + * separate specializations for real vs complex. On the inverse + * transform, only half the spectrum is actually used if the output type is real. + */ + + +#ifdef EIGEN_FFTW_DEFAULT +// FFTW: faster, GPL -- incompatible with Eigen in LGPL form, bigger code size +# include <fftw3.h> +# include "src/FFT/ei_fftw_impl.h" + namespace Eigen { + //template <typename T> typedef struct internal::fftw_impl default_fft_impl; this does not work + template <typename T> struct default_fft_impl : public internal::fftw_impl<T> {}; + } +#elif defined EIGEN_MKL_DEFAULT +// TODO +// intel Math Kernel Library: fastest, commercial -- may be incompatible with Eigen in GPL form +# include "src/FFT/ei_imklfft_impl.h" + namespace Eigen { + template <typename T> struct default_fft_impl : public internal::imklfft_impl {}; + } +#else +// internal::kissfft_impl: small, free, reasonably efficient default, derived from kissfft +// +# include "src/FFT/ei_kissfft_impl.h" + namespace Eigen { + template <typename T> + struct default_fft_impl : public internal::kissfft_impl<T> {}; + } +#endif + +namespace Eigen { + + +// +template<typename T_SrcMat,typename T_FftIfc> struct fft_fwd_proxy; +template<typename T_SrcMat,typename T_FftIfc> struct fft_inv_proxy; + +namespace internal { +template<typename T_SrcMat,typename T_FftIfc> +struct traits< fft_fwd_proxy<T_SrcMat,T_FftIfc> > +{ + typedef typename T_SrcMat::PlainObject ReturnType; +}; +template<typename T_SrcMat,typename T_FftIfc> +struct traits< fft_inv_proxy<T_SrcMat,T_FftIfc> > +{ + typedef typename T_SrcMat::PlainObject ReturnType; +}; +} + +template<typename T_SrcMat,typename T_FftIfc> +struct fft_fwd_proxy + : public ReturnByValue<fft_fwd_proxy<T_SrcMat,T_FftIfc> > +{ + typedef DenseIndex Index; + + fft_fwd_proxy(const T_SrcMat& src,T_FftIfc & fft, Index nfft) : m_src(src),m_ifc(fft), m_nfft(nfft) {} + + template<typename T_DestMat> void evalTo(T_DestMat& dst) const; + + Index rows() const { return m_src.rows(); } + Index cols() const { return m_src.cols(); } +protected: + const T_SrcMat & m_src; + T_FftIfc & m_ifc; + Index m_nfft; +private: + fft_fwd_proxy& operator=(const fft_fwd_proxy&); +}; + +template<typename T_SrcMat,typename T_FftIfc> +struct fft_inv_proxy + : public ReturnByValue<fft_inv_proxy<T_SrcMat,T_FftIfc> > +{ + typedef DenseIndex Index; + + fft_inv_proxy(const T_SrcMat& src,T_FftIfc & fft, Index nfft) : m_src(src),m_ifc(fft), m_nfft(nfft) {} + + template<typename T_DestMat> void evalTo(T_DestMat& dst) const; + + Index rows() const { return m_src.rows(); } + Index cols() const { return m_src.cols(); } +protected: + const T_SrcMat & m_src; + T_FftIfc & m_ifc; + Index m_nfft; +private: + fft_inv_proxy& operator=(const fft_inv_proxy&); +}; + + +template <typename T_Scalar, + typename T_Impl=default_fft_impl<T_Scalar> > +class FFT +{ + public: + typedef T_Impl impl_type; + typedef DenseIndex Index; + typedef typename impl_type::Scalar Scalar; + typedef typename impl_type::Complex Complex; + + enum Flag { + Default=0, // goof proof + Unscaled=1, + HalfSpectrum=2, + // SomeOtherSpeedOptimization=4 + Speedy=32767 + }; + + FFT( const impl_type & impl=impl_type() , Flag flags=Default ) :m_impl(impl),m_flag(flags) { } + + inline + bool HasFlag(Flag f) const { return (m_flag & (int)f) == f;} + + inline + void SetFlag(Flag f) { m_flag |= (int)f;} + + inline + void ClearFlag(Flag f) { m_flag &= (~(int)f);} + + inline + void fwd( Complex * dst, const Scalar * src, Index nfft) + { + m_impl.fwd(dst,src,static_cast<int>(nfft)); + if ( HasFlag(HalfSpectrum) == false) + ReflectSpectrum(dst,nfft); + } + + inline + void fwd( Complex * dst, const Complex * src, Index nfft) + { + m_impl.fwd(dst,src,static_cast<int>(nfft)); + } + + /* + inline + void fwd2(Complex * dst, const Complex * src, int n0,int n1) + { + m_impl.fwd2(dst,src,n0,n1); + } + */ + + template <typename _Input> + inline + void fwd( std::vector<Complex> & dst, const std::vector<_Input> & src) + { + if ( NumTraits<_Input>::IsComplex == 0 && HasFlag(HalfSpectrum) ) + dst.resize( (src.size()>>1)+1); // half the bins + Nyquist bin + else + dst.resize(src.size()); + fwd(&dst[0],&src[0],src.size()); + } + + template<typename InputDerived, typename ComplexDerived> + inline + void fwd( MatrixBase<ComplexDerived> & dst, const MatrixBase<InputDerived> & src, Index nfft=-1) + { + typedef typename ComplexDerived::Scalar dst_type; + typedef typename InputDerived::Scalar src_type; + EIGEN_STATIC_ASSERT_VECTOR_ONLY(InputDerived) + EIGEN_STATIC_ASSERT_VECTOR_ONLY(ComplexDerived) + EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ComplexDerived,InputDerived) // size at compile-time + EIGEN_STATIC_ASSERT((internal::is_same<dst_type, Complex>::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + EIGEN_STATIC_ASSERT(int(InputDerived::Flags)&int(ComplexDerived::Flags)&DirectAccessBit, + THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_WITH_DIRECT_MEMORY_ACCESS_SUCH_AS_MAP_OR_PLAIN_MATRICES) + + if (nfft<1) + nfft = src.size(); + + if ( NumTraits< src_type >::IsComplex == 0 && HasFlag(HalfSpectrum) ) + dst.derived().resize( (nfft>>1)+1); + else + dst.derived().resize(nfft); + + if ( src.innerStride() != 1 || src.size() < nfft ) { + Matrix<src_type,1,Dynamic> tmp; + if (src.size()<nfft) { + tmp.setZero(nfft); + tmp.block(0,0,src.size(),1 ) = src; + }else{ + tmp = src; + } + fwd( &dst[0],&tmp[0],nfft ); + }else{ + fwd( &dst[0],&src[0],nfft ); + } + } + + template<typename InputDerived> + inline + fft_fwd_proxy< MatrixBase<InputDerived>, FFT<T_Scalar,T_Impl> > + fwd( const MatrixBase<InputDerived> & src, Index nfft=-1) + { + return fft_fwd_proxy< MatrixBase<InputDerived> ,FFT<T_Scalar,T_Impl> >( src, *this,nfft ); + } + + template<typename InputDerived> + inline + fft_inv_proxy< MatrixBase<InputDerived>, FFT<T_Scalar,T_Impl> > + inv( const MatrixBase<InputDerived> & src, Index nfft=-1) + { + return fft_inv_proxy< MatrixBase<InputDerived> ,FFT<T_Scalar,T_Impl> >( src, *this,nfft ); + } + + inline + void inv( Complex * dst, const Complex * src, Index nfft) + { + m_impl.inv( dst,src,static_cast<int>(nfft) ); + if ( HasFlag( Unscaled ) == false) + scale(dst,Scalar(1./nfft),nfft); // scale the time series + } + + inline + void inv( Scalar * dst, const Complex * src, Index nfft) + { + m_impl.inv( dst,src,static_cast<int>(nfft) ); + if ( HasFlag( Unscaled ) == false) + scale(dst,Scalar(1./nfft),nfft); // scale the time series + } + + template<typename OutputDerived, typename ComplexDerived> + inline + void inv( MatrixBase<OutputDerived> & dst, const MatrixBase<ComplexDerived> & src, Index nfft=-1) + { + typedef typename ComplexDerived::Scalar src_type; + typedef typename OutputDerived::Scalar dst_type; + const bool realfft= (NumTraits<dst_type>::IsComplex == 0); + EIGEN_STATIC_ASSERT_VECTOR_ONLY(OutputDerived) + EIGEN_STATIC_ASSERT_VECTOR_ONLY(ComplexDerived) + EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ComplexDerived,OutputDerived) // size at compile-time + EIGEN_STATIC_ASSERT((internal::is_same<src_type, Complex>::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + EIGEN_STATIC_ASSERT(int(OutputDerived::Flags)&int(ComplexDerived::Flags)&DirectAccessBit, + THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_WITH_DIRECT_MEMORY_ACCESS_SUCH_AS_MAP_OR_PLAIN_MATRICES) + + if (nfft<1) { //automatic FFT size determination + if ( realfft && HasFlag(HalfSpectrum) ) + nfft = 2*(src.size()-1); //assume even fft size + else + nfft = src.size(); + } + dst.derived().resize( nfft ); + + // check for nfft that does not fit the input data size + Index resize_input= ( realfft && HasFlag(HalfSpectrum) ) + ? ( (nfft/2+1) - src.size() ) + : ( nfft - src.size() ); + + if ( src.innerStride() != 1 || resize_input ) { + // if the vector is strided, then we need to copy it to a packed temporary + Matrix<src_type,1,Dynamic> tmp; + if ( resize_input ) { + size_t ncopy = (std::min)(src.size(),src.size() + resize_input); + tmp.setZero(src.size() + resize_input); + if ( realfft && HasFlag(HalfSpectrum) ) { + // pad at the Nyquist bin + tmp.head(ncopy) = src.head(ncopy); + tmp(ncopy-1) = real(tmp(ncopy-1)); // enforce real-only Nyquist bin + }else{ + size_t nhead,ntail; + nhead = 1+ncopy/2-1; // range [0:pi) + ntail = ncopy/2-1; // range (-pi:0) + tmp.head(nhead) = src.head(nhead); + tmp.tail(ntail) = src.tail(ntail); + if (resize_input<0) { //shrinking -- create the Nyquist bin as the average of the two bins that fold into it + tmp(nhead) = ( src(nfft/2) + src( src.size() - nfft/2 ) )*src_type(.5); + }else{ // expanding -- split the old Nyquist bin into two halves + tmp(nhead) = src(nhead) * src_type(.5); + tmp(tmp.size()-nhead) = tmp(nhead); + } + } + }else{ + tmp = src; + } + inv( &dst[0],&tmp[0], nfft); + }else{ + inv( &dst[0],&src[0], nfft); + } + } + + template <typename _Output> + inline + void inv( std::vector<_Output> & dst, const std::vector<Complex> & src,Index nfft=-1) + { + if (nfft<1) + nfft = ( NumTraits<_Output>::IsComplex == 0 && HasFlag(HalfSpectrum) ) ? 2*(src.size()-1) : src.size(); + dst.resize( nfft ); + inv( &dst[0],&src[0],nfft); + } + + + /* + // TODO: multi-dimensional FFTs + inline + void inv2(Complex * dst, const Complex * src, int n0,int n1) + { + m_impl.inv2(dst,src,n0,n1); + if ( HasFlag( Unscaled ) == false) + scale(dst,1./(n0*n1),n0*n1); + } + */ + + inline + impl_type & impl() {return m_impl;} + private: + + template <typename T_Data> + inline + void scale(T_Data * x,Scalar s,Index nx) + { +#if 1 + for (int k=0;k<nx;++k) + *x++ *= s; +#else + if ( ((ptrdiff_t)x) & 15 ) + Matrix<T_Data, Dynamic, 1>::Map(x,nx) *= s; + else + Matrix<T_Data, Dynamic, 1>::MapAligned(x,nx) *= s; + //Matrix<T_Data, Dynamic, Dynamic>::Map(x,nx) * s; +#endif + } + + inline + void ReflectSpectrum(Complex * freq, Index nfft) + { + // create the implicit right-half spectrum (conjugate-mirror of the left-half) + Index nhbins=(nfft>>1)+1; + for (Index k=nhbins;k < nfft; ++k ) + freq[k] = conj(freq[nfft-k]); + } + + impl_type m_impl; + int m_flag; +}; + +template<typename T_SrcMat,typename T_FftIfc> +template<typename T_DestMat> inline +void fft_fwd_proxy<T_SrcMat,T_FftIfc>::evalTo(T_DestMat& dst) const +{ + m_ifc.fwd( dst, m_src, m_nfft); +} + +template<typename T_SrcMat,typename T_FftIfc> +template<typename T_DestMat> inline +void fft_inv_proxy<T_SrcMat,T_FftIfc>::evalTo(T_DestMat& dst) const +{ + m_ifc.inv( dst, m_src, m_nfft); +} + +} +#endif +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/unsupported/Eigen/IterativeSolvers b/unsupported/Eigen/IterativeSolvers new file mode 100644 index 000000000..6c6946d91 --- /dev/null +++ b/unsupported/Eigen/IterativeSolvers @@ -0,0 +1,40 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_ITERATIVE_SOLVERS_MODULE_H +#define EIGEN_ITERATIVE_SOLVERS_MODULE_H + +#include <Eigen/Sparse> + +/** \ingroup Unsupported_modules + * \defgroup IterativeSolvers_Module Iterative solvers module + * This module aims to provide various iterative linear and non linear solver algorithms. + * It currently provides: + * - a constrained conjugate gradient + * - a Householder GMRES implementation + * \code + * #include <unsupported/Eigen/IterativeSolvers> + * \endcode + */ +//@{ + +#include "../../Eigen/src/misc/Solve.h" +#include "../../Eigen/src/misc/SparseSolve.h" + +#include "src/IterativeSolvers/IterationController.h" +#include "src/IterativeSolvers/ConstrainedConjGrad.h" +#include "src/IterativeSolvers/IncompleteLU.h" +#include "../../Eigen/Jacobi" +#include "../../Eigen/Householder" +#include "src/IterativeSolvers/GMRES.h" +//#include "src/IterativeSolvers/SSORPreconditioner.h" + +//@} + +#endif // EIGEN_ITERATIVE_SOLVERS_MODULE_H diff --git a/unsupported/Eigen/KroneckerProduct b/unsupported/Eigen/KroneckerProduct new file mode 100644 index 000000000..796e386ad --- /dev/null +++ b/unsupported/Eigen/KroneckerProduct @@ -0,0 +1,26 @@ +#ifndef EIGEN_KRONECKER_PRODUCT_MODULE_H +#define EIGEN_KRONECKER_PRODUCT_MODULE_H + +#include "../../Eigen/Core" + +#include "../../Eigen/src/Core/util/DisableStupidWarnings.h" + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup KroneckerProduct_Module KroneckerProduct module + * + * This module contains an experimental Kronecker product implementation. + * + * \code + * #include <Eigen/KroneckerProduct> + * \endcode + */ + +} // namespace Eigen + +#include "src/KroneckerProduct/KroneckerTensorProduct.h" + +#include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_KRONECKER_PRODUCT_MODULE_H diff --git a/unsupported/Eigen/MPRealSupport b/unsupported/Eigen/MPRealSupport new file mode 100644 index 000000000..3895623fe --- /dev/null +++ b/unsupported/Eigen/MPRealSupport @@ -0,0 +1,148 @@ +// This file is part of a joint effort between Eigen, a lightweight C++ template library +// for linear algebra, and MPFR C++, a C++ interface to MPFR library (http://www.holoborodko.com/pavel/) +// +// Copyright (C) 2010 Pavel Holoborodko <pavel@holoborodko.com> +// Copyright (C) 2010 Konstantin Holoborodko <konstantin@holoborodko.com> +// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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/. +// +// Contributors: +// Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, Heinz van Saanen, Pere Constans + +#ifndef EIGEN_MPREALSUPPORT_MODULE_H +#define EIGEN_MPREALSUPPORT_MODULE_H + +#include <ctime> +#include <mpreal.h> +#include <Eigen/Core> + +namespace Eigen { + + /** \ingroup Unsupported_modules + * \defgroup MPRealSupport_Module MPFRC++ Support module + * + * \code + * #include <Eigen/MPRealSupport> + * \endcode + * + * This module provides support for multi precision floating point numbers + * via the <a href="http://www.holoborodko.com/pavel/mpfr">MPFR C++</a> + * library which itself is built upon <a href="http://www.mpfr.org/">MPFR</a>/<a href="http://gmplib.org/">GMP</a>. + * + * You can find a copy of MPFR C++ that is known to be compatible in the unsupported/test/mpreal folder. + * + * Here is an example: + * +\code +#include <iostream> +#include <Eigen/MPRealSupport> +#include <Eigen/LU> +using namespace mpfr; +using namespace Eigen; +int main() +{ + // set precision to 256 bits (double has only 53 bits) + mpreal::set_default_prec(256); + // Declare matrix and vector types with multi-precision scalar type + typedef Matrix<mpreal,Dynamic,Dynamic> MatrixXmp; + typedef Matrix<mpreal,Dynamic,1> VectorXmp; + + MatrixXmp A = MatrixXmp::Random(100,100); + VectorXmp b = VectorXmp::Random(100); + + // Solve Ax=b using LU + VectorXmp x = A.lu().solve(b); + std::cout << "relative error: " << (A*x - b).norm() / b.norm() << std::endl; + return 0; +} +\endcode + * + */ + + template<> struct NumTraits<mpfr::mpreal> + : GenericNumTraits<mpfr::mpreal> + { + enum { + IsInteger = 0, + IsSigned = 1, + IsComplex = 0, + RequireInitialization = 1, + ReadCost = 10, + AddCost = 10, + MulCost = 40 + }; + + typedef mpfr::mpreal Real; + typedef mpfr::mpreal NonInteger; + + inline static mpfr::mpreal highest() { return mpfr::mpreal_max(mpfr::mpreal::get_default_prec()); } + inline static mpfr::mpreal lowest() { return -mpfr::mpreal_max(mpfr::mpreal::get_default_prec()); } + + inline static Real epsilon() + { + return mpfr::machine_epsilon(mpfr::mpreal::get_default_prec()); + } + inline static Real dummy_precision() + { + unsigned int weak_prec = ((mpfr::mpreal::get_default_prec()-1)*90)/100; + return mpfr::machine_epsilon(weak_prec); + } + }; + +namespace internal { + + template<> mpfr::mpreal random<mpfr::mpreal>() + { +#if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) + static gmp_randstate_t state; + static bool isFirstTime = true; + + if(isFirstTime) + { + gmp_randinit_default(state); + gmp_randseed_ui(state,(unsigned)time(NULL)); + isFirstTime = false; + } + + return mpfr::urandom(state)*2-1; +#else + return mpfr::mpreal(random<double>()); +#endif + } + + template<> mpfr::mpreal random<mpfr::mpreal>(const mpfr::mpreal& a, const mpfr::mpreal& b) + { + return a + (b-a) * random<mpfr::mpreal>(); + } + + bool isMuchSmallerThan(const mpfr::mpreal& a, const mpfr::mpreal& b, const mpfr::mpreal& prec) + { + return mpfr::abs(a) <= mpfr::abs(b) * prec; + } + + inline bool isApprox(const mpfr::mpreal& a, const mpfr::mpreal& b, const mpfr::mpreal& prec) + { + return mpfr::abs(a - b) <= (mpfr::min)(mpfr::abs(a), mpfr::abs(b)) * prec; + } + + inline bool isApproxOrLessThan(const mpfr::mpreal& a, const mpfr::mpreal& b, const mpfr::mpreal& prec) + { + return a <= b || isApprox(a, b, prec); + } + + template<> inline long double cast<mpfr::mpreal,long double>(const mpfr::mpreal& x) + { return x.toLDouble(); } + template<> inline double cast<mpfr::mpreal,double>(const mpfr::mpreal& x) + { return x.toDouble(); } + template<> inline long cast<mpfr::mpreal,long>(const mpfr::mpreal& x) + { return x.toLong(); } + template<> inline int cast<mpfr::mpreal,int>(const mpfr::mpreal& x) + { return int(x.toLong()); } + +} // end namespace internal +} + +#endif // EIGEN_MPREALSUPPORT_MODULE_H diff --git a/unsupported/Eigen/MatrixFunctions b/unsupported/Eigen/MatrixFunctions new file mode 100644 index 000000000..56ab71cd3 --- /dev/null +++ b/unsupported/Eigen/MatrixFunctions @@ -0,0 +1,380 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Jitse Niesen <jitse@maths.leeds.ac.uk> +// +// 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_FUNCTIONS +#define EIGEN_MATRIX_FUNCTIONS + +#include <cfloat> +#include <list> +#include <functional> +#include <iterator> + +#include <Eigen/Core> +#include <Eigen/LU> +#include <Eigen/Eigenvalues> + +/** \ingroup Unsupported_modules + * \defgroup MatrixFunctions_Module Matrix functions module + * \brief This module aims to provide various methods for the computation of + * matrix functions. + * + * To use this module, add + * \code + * #include <unsupported/Eigen/MatrixFunctions> + * \endcode + * at the start of your source file. + * + * This module defines the following MatrixBase methods. + * - \ref matrixbase_cos "MatrixBase::cos()", for computing the matrix cosine + * - \ref matrixbase_cosh "MatrixBase::cosh()", for computing the matrix hyperbolic cosine + * - \ref matrixbase_exp "MatrixBase::exp()", for computing the matrix exponential + * - \ref matrixbase_log "MatrixBase::log()", for computing the matrix logarithm + * - \ref matrixbase_matrixfunction "MatrixBase::matrixFunction()", for computing general matrix functions + * - \ref matrixbase_sin "MatrixBase::sin()", for computing the matrix sine + * - \ref matrixbase_sinh "MatrixBase::sinh()", for computing the matrix hyperbolic sine + * - \ref matrixbase_sqrt "MatrixBase::sqrt()", for computing the matrix square root + * + * These methods are the main entry points to this module. + * + * %Matrix functions are defined as follows. Suppose that \f$ f \f$ + * is an entire function (that is, a function on the complex plane + * that is everywhere complex differentiable). Then its Taylor + * series + * \f[ f(0) + f'(0) x + \frac{f''(0)}{2} x^2 + \frac{f'''(0)}{3!} x^3 + \cdots \f] + * converges to \f$ f(x) \f$. In this case, we can define the matrix + * function by the same series: + * \f[ f(M) = f(0) + f'(0) M + \frac{f''(0)}{2} M^2 + \frac{f'''(0)}{3!} M^3 + \cdots \f] + * + */ + +#include "src/MatrixFunctions/MatrixExponential.h" +#include "src/MatrixFunctions/MatrixFunction.h" +#include "src/MatrixFunctions/MatrixSquareRoot.h" +#include "src/MatrixFunctions/MatrixLogarithm.h" + + + +/** +\page matrixbaseextra MatrixBase methods defined in the MatrixFunctions module +\ingroup MatrixFunctions_Module + +The remainder of the page documents the following MatrixBase methods +which are defined in the MatrixFunctions module. + + + +\section matrixbase_cos MatrixBase::cos() + +Compute the matrix cosine. + +\code +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const +\endcode + +\param[in] M a square matrix. +\returns expression representing \f$ \cos(M) \f$. + +This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cos(). + +\sa \ref matrixbase_sin "sin()" for an example. + + + +\section matrixbase_cosh MatrixBase::cosh() + +Compute the matrix hyberbolic cosine. + +\code +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const +\endcode + +\param[in] M a square matrix. +\returns expression representing \f$ \cosh(M) \f$ + +This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cosh(). + +\sa \ref matrixbase_sinh "sinh()" for an example. + + + +\section matrixbase_exp MatrixBase::exp() + +Compute the matrix exponential. + +\code +const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const +\endcode + +\param[in] M matrix whose exponential is to be computed. +\returns expression representing the matrix exponential of \p M. + +The matrix exponential of \f$ M \f$ is defined by +\f[ \exp(M) = \sum_{k=0}^\infty \frac{M^k}{k!}. \f] +The matrix exponential can be used to solve linear ordinary +differential equations: the solution of \f$ y' = My \f$ with the +initial condition \f$ y(0) = y_0 \f$ is given by +\f$ y(t) = \exp(M) y_0 \f$. + +The cost of the computation is approximately \f$ 20 n^3 \f$ for +matrices of size \f$ n \f$. The number 20 depends weakly on the +norm of the matrix. + +The matrix exponential is computed using the scaling-and-squaring +method combined with Padé approximation. The matrix is first +rescaled, then the exponential of the reduced matrix is computed +approximant, and then the rescaling is undone by repeated +squaring. The degree of the Padé approximant is chosen such +that the approximation error is less than the round-off +error. However, errors may accumulate during the squaring phase. + +Details of the algorithm can be found in: Nicholas J. Higham, "The +scaling and squaring method for the matrix exponential revisited," +<em>SIAM J. %Matrix Anal. Applic.</em>, <b>26</b>:1179–1193, +2005. + +Example: The following program checks that +\f[ \exp \left[ \begin{array}{ccc} + 0 & \frac14\pi & 0 \\ + -\frac14\pi & 0 & 0 \\ + 0 & 0 & 0 + \end{array} \right] = \left[ \begin{array}{ccc} + \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\ + \frac12\sqrt2 & \frac12\sqrt2 & 0 \\ + 0 & 0 & 1 + \end{array} \right]. \f] +This corresponds to a rotation of \f$ \frac14\pi \f$ radians around +the z-axis. + +\include MatrixExponential.cpp +Output: \verbinclude MatrixExponential.out + +\note \p M has to be a matrix of \c float, \c double, \c long double +\c complex<float>, \c complex<double>, or \c complex<long double> . + + +\section matrixbase_log MatrixBase::log() + +Compute the matrix logarithm. + +\code +const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const +\endcode + +\param[in] M invertible matrix whose logarithm is to be computed. +\returns expression representing the matrix logarithm root of \p M. + +The matrix logarithm of \f$ M \f$ is a matrix \f$ X \f$ such that +\f$ \exp(X) = M \f$ where exp denotes the matrix exponential. As for +the scalar logarithm, the equation \f$ \exp(X) = M \f$ may have +multiple solutions; this function returns a matrix whose eigenvalues +have imaginary part in the interval \f$ (-\pi,\pi] \f$. + +In the real case, the matrix \f$ M \f$ should be invertible and +it should have no eigenvalues which are real and negative (pairs of +complex conjugate eigenvalues are allowed). In the complex case, it +only needs to be invertible. + +This function computes the matrix logarithm using the Schur-Parlett +algorithm as implemented by MatrixBase::matrixFunction(). The +logarithm of an atomic block is computed by MatrixLogarithmAtomic, +which uses direct computation for 1-by-1 and 2-by-2 blocks and an +inverse scaling-and-squaring algorithm for bigger blocks, with the +square roots computed by MatrixBase::sqrt(). + +Details of the algorithm can be found in Section 11.6.2 of: +Nicholas J. Higham, +<em>Functions of Matrices: Theory and Computation</em>, +SIAM 2008. ISBN 978-0-898716-46-7. + +Example: The following program checks that +\f[ \log \left[ \begin{array}{ccc} + \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\ + \frac12\sqrt2 & \frac12\sqrt2 & 0 \\ + 0 & 0 & 1 + \end{array} \right] = \left[ \begin{array}{ccc} + 0 & \frac14\pi & 0 \\ + -\frac14\pi & 0 & 0 \\ + 0 & 0 & 0 + \end{array} \right]. \f] +This corresponds to a rotation of \f$ \frac14\pi \f$ radians around +the z-axis. This is the inverse of the example used in the +documentation of \ref matrixbase_exp "exp()". + +\include MatrixLogarithm.cpp +Output: \verbinclude MatrixLogarithm.out + +\note \p M has to be a matrix of \c float, \c double, \c long double +\c complex<float>, \c complex<double>, or \c complex<long double> . + +\sa MatrixBase::exp(), MatrixBase::matrixFunction(), + class MatrixLogarithmAtomic, MatrixBase::sqrt(). + + +\section matrixbase_matrixfunction MatrixBase::matrixFunction() + +Compute a matrix function. + +\code +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const +\endcode + +\param[in] M argument of matrix function, should be a square matrix. +\param[in] f an entire function; \c f(x,n) should compute the n-th +derivative of f at x. +\returns expression representing \p f applied to \p M. + +Suppose that \p M is a matrix whose entries have type \c Scalar. +Then, the second argument, \p f, should be a function with prototype +\code +ComplexScalar f(ComplexScalar, int) +\endcode +where \c ComplexScalar = \c std::complex<Scalar> if \c Scalar is +real (e.g., \c float or \c double) and \c ComplexScalar = +\c Scalar if \c Scalar is complex. The return value of \c f(x,n) +should be \f$ f^{(n)}(x) \f$, the n-th derivative of f at x. + +This routine uses the algorithm described in: +Philip Davies and Nicholas J. Higham, +"A Schur-Parlett algorithm for computing matrix functions", +<em>SIAM J. %Matrix Anal. Applic.</em>, <b>25</b>:464–485, 2003. + +The actual work is done by the MatrixFunction class. + +Example: The following program checks that +\f[ \exp \left[ \begin{array}{ccc} + 0 & \frac14\pi & 0 \\ + -\frac14\pi & 0 & 0 \\ + 0 & 0 & 0 + \end{array} \right] = \left[ \begin{array}{ccc} + \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\ + \frac12\sqrt2 & \frac12\sqrt2 & 0 \\ + 0 & 0 & 1 + \end{array} \right]. \f] +This corresponds to a rotation of \f$ \frac14\pi \f$ radians around +the z-axis. This is the same example as used in the documentation +of \ref matrixbase_exp "exp()". + +\include MatrixFunction.cpp +Output: \verbinclude MatrixFunction.out + +Note that the function \c expfn is defined for complex numbers +\c x, even though the matrix \c A is over the reals. Instead of +\c expfn, we could also have used StdStemFunctions::exp: +\code +A.matrixFunction(StdStemFunctions<std::complex<double> >::exp, &B); +\endcode + + + +\section matrixbase_sin MatrixBase::sin() + +Compute the matrix sine. + +\code +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const +\endcode + +\param[in] M a square matrix. +\returns expression representing \f$ \sin(M) \f$. + +This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sin(). + +Example: \include MatrixSine.cpp +Output: \verbinclude MatrixSine.out + + + +\section matrixbase_sinh MatrixBase::sinh() + +Compute the matrix hyperbolic sine. + +\code +MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const +\endcode + +\param[in] M a square matrix. +\returns expression representing \f$ \sinh(M) \f$ + +This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sinh(). + +Example: \include MatrixSinh.cpp +Output: \verbinclude MatrixSinh.out + + +\section matrixbase_sqrt MatrixBase::sqrt() + +Compute the matrix square root. + +\code +const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const +\endcode + +\param[in] M invertible matrix whose square root is to be computed. +\returns expression representing the matrix square root of \p M. + +The matrix square root of \f$ M \f$ is the matrix \f$ M^{1/2} \f$ +whose square is the original matrix; so if \f$ S = M^{1/2} \f$ then +\f$ S^2 = M \f$. + +In the <b>real case</b>, the matrix \f$ M \f$ should be invertible and +it should have no eigenvalues which are real and negative (pairs of +complex conjugate eigenvalues are allowed). In that case, the matrix +has a square root which is also real, and this is the square root +computed by this function. + +The matrix square root is computed by first reducing the matrix to +quasi-triangular form with the real Schur decomposition. The square +root of the quasi-triangular matrix can then be computed directly. The +cost is approximately \f$ 25 n^3 \f$ real flops for the real Schur +decomposition and \f$ 3\frac13 n^3 \f$ real flops for the remainder +(though the computation time in practice is likely more than this +indicates). + +Details of the algorithm can be found in: Nicholas J. Highan, +"Computing real square roots of a real matrix", <em>Linear Algebra +Appl.</em>, 88/89:405–430, 1987. + +If the matrix is <b>positive-definite symmetric</b>, then the square +root is also positive-definite symmetric. In this case, it is best to +use SelfAdjointEigenSolver::operatorSqrt() to compute it. + +In the <b>complex case</b>, the matrix \f$ M \f$ should be invertible; +this is a restriction of the algorithm. The square root computed by +this algorithm is the one whose eigenvalues have an argument in the +interval \f$ (-\frac12\pi, \frac12\pi] \f$. This is the usual branch +cut. + +The computation is the same as in the real case, except that the +complex Schur decomposition is used to reduce the matrix to a +triangular matrix. The theoretical cost is the same. Details are in: +Åke Björck and Sven Hammarling, "A Schur method for the +square root of a matrix", <em>Linear Algebra Appl.</em>, +52/53:127–140, 1983. + +Example: The following program checks that the square root of +\f[ \left[ \begin{array}{cc} + \cos(\frac13\pi) & -\sin(\frac13\pi) \\ + \sin(\frac13\pi) & \cos(\frac13\pi) + \end{array} \right], \f] +corresponding to a rotation over 60 degrees, is a rotation over 30 degrees: +\f[ \left[ \begin{array}{cc} + \cos(\frac16\pi) & -\sin(\frac16\pi) \\ + \sin(\frac16\pi) & \cos(\frac16\pi) + \end{array} \right]. \f] + +\include MatrixSquareRoot.cpp +Output: \verbinclude MatrixSquareRoot.out + +\sa class RealSchur, class ComplexSchur, class MatrixSquareRoot, + SelfAdjointEigenSolver::operatorSqrt(). + +*/ + +#endif // EIGEN_MATRIX_FUNCTIONS + diff --git a/unsupported/Eigen/MoreVectorization b/unsupported/Eigen/MoreVectorization new file mode 100644 index 000000000..9f0a39f75 --- /dev/null +++ b/unsupported/Eigen/MoreVectorization @@ -0,0 +1,16 @@ +#ifndef EIGEN_MOREVECTORIZATION_MODULE_H +#define EIGEN_MOREVECTORIZATION_MODULE_H + +#include <Eigen/Core> + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup MoreVectorization More vectorization module + */ + +} + +#include "src/MoreVectorization/MathFunctions.h" + +#endif // EIGEN_MOREVECTORIZATION_MODULE_H diff --git a/unsupported/Eigen/NonLinearOptimization b/unsupported/Eigen/NonLinearOptimization new file mode 100644 index 000000000..cf6ca58f8 --- /dev/null +++ b/unsupported/Eigen/NonLinearOptimization @@ -0,0 +1,134 @@ +// This file is part of Eugenio, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// 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_NONLINEAROPTIMIZATION_MODULE +#define EIGEN_NONLINEAROPTIMIZATION_MODULE + +#include <vector> + +#include <Eigen/Core> +#include <Eigen/Jacobi> +#include <Eigen/QR> +#include <unsupported/Eigen/NumericalDiff> + +/** \ingroup Unsupported_modules + * \defgroup NonLinearOptimization_Module Non linear optimization module + * + * \code + * #include <unsupported/Eigen/NonLinearOptimization> + * \endcode + * + * This module provides implementation of two important algorithms in non linear + * optimization. In both cases, we consider a system of non linear functions. Of + * course, this should work, and even work very well if those functions are + * actually linear. But if this is so, you should probably better use other + * methods more fitted to this special case. + * + * One algorithm allows to find an extremum of such a system (Levenberg + * Marquardt algorithm) and the second one is used to find + * a zero for the system (Powell hybrid "dogleg" method). + * + * This code is a port of minpack (http://en.wikipedia.org/wiki/MINPACK). + * Minpack is a very famous, old, robust and well-reknown package, written in + * fortran. Those implementations have been carefully tuned, tested, and used + * for several decades. + * + * The original fortran code was automatically translated using f2c (http://en.wikipedia.org/wiki/F2c) in C, + * then c++, and then cleaned by several different authors. + * The last one of those cleanings being our starting point : + * http://devernay.free.fr/hacks/cminpack.html + * + * Finally, we ported this code to Eigen, creating classes and API + * coherent with Eigen. When possible, we switched to Eigen + * implementation, such as most linear algebra (vectors, matrices, stable norms). + * + * Doing so, we were very careful to check the tests we setup at the very + * beginning, which ensure that the same results are found. + * + * \section Tests Tests + * + * The tests are placed in the file unsupported/test/NonLinear.cpp. + * + * There are two kinds of tests : those that come from examples bundled with cminpack. + * They guaranty we get the same results as the original algorithms (value for 'x', + * for the number of evaluations of the function, and for the number of evaluations + * of the jacobian if ever). + * + * Other tests were added by myself at the very beginning of the + * process and check the results for levenberg-marquardt using the reference data + * on http://www.itl.nist.gov/div898/strd/nls/nls_main.shtml. Since then i've + * carefully checked that the same results were obtained when modifiying the + * code. Please note that we do not always get the exact same decimals as they do, + * but this is ok : they use 128bits float, and we do the tests using the C type 'double', + * which is 64 bits on most platforms (x86 and amd64, at least). + * I've performed those tests on several other implementations of levenberg-marquardt, and + * (c)minpack performs VERY well compared to those, both in accuracy and speed. + * + * The documentation for running the tests is on the wiki + * http://eigen.tuxfamily.org/index.php?title=Tests + * + * \section API API : overview of methods + * + * Both algorithms can use either the jacobian (provided by the user) or compute + * an approximation by themselves (actually using Eigen \ref NumericalDiff_Module). + * The part of API referring to the latter use 'NumericalDiff' in the method names + * (exemple: LevenbergMarquardt.minimizeNumericalDiff() ) + * + * The methods LevenbergMarquardt.lmder1()/lmdif1()/lmstr1() and + * HybridNonLinearSolver.hybrj1()/hybrd1() are specific methods from the original + * minpack package that you probably should NOT use until you are porting a code that + * was previously using minpack. They just define a 'simple' API with default values + * for some parameters. + * + * All algorithms are provided using Two APIs : + * - one where the user inits the algorithm, and uses '*OneStep()' as much as he wants : + * this way the caller have control over the steps + * - one where the user just calls a method (optimize() or solve()) which will + * handle the loop: init + loop until a stop condition is met. Those are provided for + * convenience. + * + * As an example, the method LevenbergMarquardt::minimize() is + * implemented as follow : + * \code + * Status LevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x, const int mode) + * { + * Status status = minimizeInit(x, mode); + * do { + * status = minimizeOneStep(x, mode); + * } while (status==Running); + * return status; + * } + * \endcode + * + * \section examples Examples + * + * The easiest way to understand how to use this module is by looking at the many examples in the file + * unsupported/test/NonLinearOptimization.cpp. + */ + +#ifndef EIGEN_PARSED_BY_DOXYGEN + +#include "src/NonLinearOptimization/qrsolv.h" +#include "src/NonLinearOptimization/r1updt.h" +#include "src/NonLinearOptimization/r1mpyq.h" +#include "src/NonLinearOptimization/rwupdt.h" +#include "src/NonLinearOptimization/fdjac1.h" +#include "src/NonLinearOptimization/lmpar.h" +#include "src/NonLinearOptimization/dogleg.h" +#include "src/NonLinearOptimization/covar.h" + +#include "src/NonLinearOptimization/chkder.h" + +#endif + +#include "src/NonLinearOptimization/HybridNonLinearSolver.h" +#include "src/NonLinearOptimization/LevenbergMarquardt.h" + + +#endif // EIGEN_NONLINEAROPTIMIZATION_MODULE diff --git a/unsupported/Eigen/NumericalDiff b/unsupported/Eigen/NumericalDiff new file mode 100644 index 000000000..b3480312d --- /dev/null +++ b/unsupported/Eigen/NumericalDiff @@ -0,0 +1,56 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// 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_NUMERICALDIFF_MODULE +#define EIGEN_NUMERICALDIFF_MODULE + +#include <Eigen/Core> + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup NumericalDiff_Module Numerical differentiation module + * + * \code + * #include <unsupported/Eigen/NumericalDiff> + * \endcode + * + * See http://en.wikipedia.org/wiki/Numerical_differentiation + * + * Warning : this should NOT be confused with automatic differentiation, which + * is a different method and has its own module in Eigen : \ref + * AutoDiff_Module. + * + * Currently only "Forward" and "Central" schemes are implemented. Those + * are basic methods, and there exist some more elaborated way of + * computing such approximates. They are implemented using both + * proprietary and free software, and usually requires linking to an + * external library. It is very easy for you to write a functor + * using such software, and the purpose is quite orthogonal to what we + * want to achieve with Eigen. + * + * This is why we will not provide wrappers for every great numerical + * differentiation software that exist, but should rather stick with those + * basic ones, that still are useful for testing. + * + * Also, the \ref NonLinearOptimization_Module needs this in order to + * provide full features compatibility with the original (c)minpack + * package. + * + */ +} + +//@{ + +#include "src/NumericalDiff/NumericalDiff.h" + +//@} + + +#endif // EIGEN_NUMERICALDIFF_MODULE diff --git a/unsupported/Eigen/OpenGLSupport b/unsupported/Eigen/OpenGLSupport new file mode 100644 index 000000000..e66a425f8 --- /dev/null +++ b/unsupported/Eigen/OpenGLSupport @@ -0,0 +1,317 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_OPENGL_MODULE +#define EIGEN_OPENGL_MODULE + +#include <Eigen/Geometry> +#include <GL/gl.h> + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup OpenGLSUpport_Module OpenGL Support module + * + * This module provides wrapper functions for a couple of OpenGL functions + * which simplify the way to pass Eigen's object to openGL. + * Here is an exmaple: + * + * \code + * // You need to add path_to_eigen/unsupported to your include path. + * #include <Eigen/OpenGLSupport> + * // ... + * Vector3f x, y; + * Matrix3f rot; + * + * glVertex(y + x * rot); + * + * Quaternion q; + * glRotate(q); + * + * // ... + * \endcode + * + */ +//@{ + +#define EIGEN_GL_FUNC_DECLARATION(FUNC) \ +namespace internal { \ + template< typename XprType, \ + typename Scalar = typename XprType::Scalar, \ + int Rows = XprType::RowsAtCompileTime, \ + int Cols = XprType::ColsAtCompileTime, \ + bool IsGLCompatible = bool(XprType::Flags&LinearAccessBit) \ + && bool(XprType::Flags&DirectAccessBit) \ + && (XprType::IsVectorAtCompileTime || (XprType::Flags&RowMajorBit)==0)> \ + struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl); \ + \ + template<typename XprType, typename Scalar, int Rows, int Cols> \ + struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType,Scalar,Rows,Cols,false> { \ + inline static void run(const XprType& p) { \ + EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<typename plain_matrix_type_column_major<XprType>::type>::run(p); } \ + }; \ +} \ + \ +template<typename Derived> inline void FUNC(const Eigen::DenseBase<Derived>& p) { \ + EIGEN_CAT(EIGEN_CAT(internal::gl_,FUNC),_impl)<Derived>::run(p.derived()); \ +} + + +#define EIGEN_GL_FUNC_SPECIALIZATION_MAT(FUNC,SCALAR,ROWS,COLS,SUFFIX) \ +namespace internal { \ + template< typename XprType> struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType, SCALAR, ROWS, COLS, true> { \ + inline static void run(const XprType& p) { FUNC##SUFFIX(p.data()); } \ + }; \ +} + + +#define EIGEN_GL_FUNC_SPECIALIZATION_VEC(FUNC,SCALAR,SIZE,SUFFIX) \ +namespace internal { \ + template< typename XprType> struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType, SCALAR, SIZE, 1, true> { \ + inline static void run(const XprType& p) { FUNC##SUFFIX(p.data()); } \ + }; \ + template< typename XprType> struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType, SCALAR, 1, SIZE, true> { \ + inline static void run(const XprType& p) { FUNC##SUFFIX(p.data()); } \ + }; \ +} + + +EIGEN_GL_FUNC_DECLARATION (glVertex) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,int, 2,2iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,short, 2,2sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,float, 2,2fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,double, 2,2dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,int, 3,3iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,short, 3,3sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,float, 3,3fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,double, 3,3dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,int, 4,4iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,short, 4,4sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,float, 4,4fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glVertex,double, 4,4dv) + +EIGEN_GL_FUNC_DECLARATION (glTexCoord) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,int, 2,2iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,short, 2,2sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,float, 2,2fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,double, 2,2dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,int, 3,3iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,short, 3,3sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,float, 3,3fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,double, 3,3dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,int, 4,4iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,short, 4,4sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,float, 4,4fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTexCoord,double, 4,4dv) + +EIGEN_GL_FUNC_DECLARATION (glColor) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,int, 2,2iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,short, 2,2sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,float, 2,2fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,double, 2,2dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,int, 3,3iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,short, 3,3sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,float, 3,3fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,double, 3,3dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,int, 4,4iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,short, 4,4sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,float, 4,4fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glColor,double, 4,4dv) + +EIGEN_GL_FUNC_DECLARATION (glNormal) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glNormal,int, 3,3iv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glNormal,short, 3,3sv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glNormal,float, 3,3fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glNormal,double, 3,3dv) + +inline void glScale2fv(const float* v) { glScalef(v[0], v[1], 1.f); } +inline void glScale2dv(const double* v) { glScaled(v[0], v[1], 1.0); } +inline void glScale3fv(const float* v) { glScalef(v[0], v[1], v[2]); } +inline void glScale3dv(const double* v) { glScaled(v[0], v[1], v[2]); } + +EIGEN_GL_FUNC_DECLARATION (glScale) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glScale,float, 2,2fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glScale,double, 2,2dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glScale,float, 3,3fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glScale,double, 3,3dv) + +template<typename Scalar> void glScale(const UniformScaling<Scalar>& s) { glScale(Matrix<Scalar,3,1>::Constant(s.factor())); } + +inline void glTranslate2fv(const float* v) { glTranslatef(v[0], v[1], 0.f); } +inline void glTranslate2dv(const double* v) { glTranslated(v[0], v[1], 0.0); } +inline void glTranslate3fv(const float* v) { glTranslatef(v[0], v[1], v[2]); } +inline void glTranslate3dv(const double* v) { glTranslated(v[0], v[1], v[2]); } + +EIGEN_GL_FUNC_DECLARATION (glTranslate) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTranslate,float, 2,2fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTranslate,double, 2,2dv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTranslate,float, 3,3fv) +EIGEN_GL_FUNC_SPECIALIZATION_VEC(glTranslate,double, 3,3dv) + +template<typename Scalar> void glTranslate(const Translation<Scalar,2>& t) { glTranslate(t.vector()); } +template<typename Scalar> void glTranslate(const Translation<Scalar,3>& t) { glTranslate(t.vector()); } + +EIGEN_GL_FUNC_DECLARATION (glMultMatrix) +EIGEN_GL_FUNC_SPECIALIZATION_MAT(glMultMatrix,float, 4,4,f) +EIGEN_GL_FUNC_SPECIALIZATION_MAT(glMultMatrix,double, 4,4,d) + +template<typename Scalar> void glMultMatrix(const Transform<Scalar,3,Affine>& t) { glMultMatrix(t.matrix()); } +template<typename Scalar> void glMultMatrix(const Transform<Scalar,3,Projective>& t) { glMultMatrix(t.matrix()); } +template<typename Scalar> void glMultMatrix(const Transform<Scalar,3,AffineCompact>& t) { glMultMatrix(Transform<Scalar,3,Affine>(t).matrix()); } + +EIGEN_GL_FUNC_DECLARATION (glLoadMatrix) +EIGEN_GL_FUNC_SPECIALIZATION_MAT(glLoadMatrix,float, 4,4,f) +EIGEN_GL_FUNC_SPECIALIZATION_MAT(glLoadMatrix,double, 4,4,d) + +template<typename Scalar> void glLoadMatrix(const Transform<Scalar,3,Affine>& t) { glLoadMatrix(t.matrix()); } +template<typename Scalar> void glLoadMatrix(const Transform<Scalar,3,Projective>& t) { glLoadMatrix(t.matrix()); } +template<typename Scalar> void glLoadMatrix(const Transform<Scalar,3,AffineCompact>& t) { glLoadMatrix(Transform<Scalar,3,Affine>(t).matrix()); } + +static void glRotate(const Rotation2D<float>& rot) +{ + glRotatef(rot.angle()*180.f/float(M_PI), 0.f, 0.f, 1.f); +} +static void glRotate(const Rotation2D<double>& rot) +{ + glRotated(rot.angle()*180.0/M_PI, 0.0, 0.0, 1.0); +} + +template<typename Derived> void glRotate(const RotationBase<Derived,3>& rot) +{ + Transform<typename Derived::Scalar,3,Projective> tr(rot); + glMultMatrix(tr.matrix()); +} + +#define EIGEN_GL_MAKE_CONST_const const +#define EIGEN_GL_MAKE_CONST__ +#define EIGEN_GL_EVAL(X) X + +#define EIGEN_GL_FUNC1_DECLARATION(FUNC,ARG1,CONST) \ +namespace internal { \ + template< typename XprType, \ + typename Scalar = typename XprType::Scalar, \ + int Rows = XprType::RowsAtCompileTime, \ + int Cols = XprType::ColsAtCompileTime, \ + bool IsGLCompatible = bool(XprType::Flags&LinearAccessBit) \ + && bool(XprType::Flags&DirectAccessBit) \ + && (XprType::IsVectorAtCompileTime || (XprType::Flags&RowMajorBit)==0)> \ + struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl); \ + \ + template<typename XprType, typename Scalar, int Rows, int Cols> \ + struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType,Scalar,Rows,Cols,false> { \ + inline static void run(ARG1 a,EIGEN_GL_EVAL(EIGEN_GL_MAKE_CONST_##CONST) XprType& p) { \ + EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<typename plain_matrix_type_column_major<XprType>::type>::run(a,p); } \ + }; \ +} \ + \ +template<typename Derived> inline void FUNC(ARG1 a,EIGEN_GL_EVAL(EIGEN_GL_MAKE_CONST_##CONST) Eigen::DenseBase<Derived>& p) { \ + EIGEN_CAT(EIGEN_CAT(internal::gl_,FUNC),_impl)<Derived>::run(a,p.derived()); \ +} + + +#define EIGEN_GL_FUNC1_SPECIALIZATION_MAT(FUNC,ARG1,CONST,SCALAR,ROWS,COLS,SUFFIX) \ +namespace internal { \ + template< typename XprType> struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType, SCALAR, ROWS, COLS, true> { \ + inline static void run(ARG1 a, EIGEN_GL_EVAL(EIGEN_GL_MAKE_CONST_##CONST) XprType& p) { FUNC##SUFFIX(a,p.data()); } \ + }; \ +} + + +#define EIGEN_GL_FUNC1_SPECIALIZATION_VEC(FUNC,ARG1,CONST,SCALAR,SIZE,SUFFIX) \ +namespace internal { \ + template< typename XprType> struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType, SCALAR, SIZE, 1, true> { \ + inline static void run(ARG1 a, EIGEN_GL_EVAL(EIGEN_GL_MAKE_CONST_##CONST) XprType& p) { FUNC##SUFFIX(a,p.data()); } \ + }; \ + template< typename XprType> struct EIGEN_CAT(EIGEN_CAT(gl_,FUNC),_impl)<XprType, SCALAR, 1, SIZE, true> { \ + inline static void run(ARG1 a, EIGEN_GL_EVAL(EIGEN_GL_MAKE_CONST_##CONST) XprType& p) { FUNC##SUFFIX(a,p.data()); } \ + }; \ +} + +EIGEN_GL_FUNC1_DECLARATION (glGet,GLenum,_) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glGet,GLenum,_,float, 4,4,Floatv) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glGet,GLenum,_,double, 4,4,Doublev) + +// glUniform API + +#ifdef GL_VERSION_2_0 + +static void glUniform2fv_ei (GLint loc, const float* v) { glUniform2fv(loc,1,v); } +static void glUniform2iv_ei (GLint loc, const int* v) { glUniform2iv(loc,1,v); } + +static void glUniform3fv_ei (GLint loc, const float* v) { glUniform3fv(loc,1,v); } +static void glUniform3iv_ei (GLint loc, const int* v) { glUniform3iv(loc,1,v); } + +static void glUniform4fv_ei (GLint loc, const float* v) { glUniform4fv(loc,1,v); } +static void glUniform4iv_ei (GLint loc, const int* v) { glUniform4iv(loc,1,v); } + +static void glUniformMatrix2fv_ei (GLint loc, const float* v) { glUniformMatrix2fv(loc,1,false,v); } +static void glUniformMatrix3fv_ei (GLint loc, const float* v) { glUniformMatrix3fv(loc,1,false,v); } +static void glUniformMatrix4fv_ei (GLint loc, const float* v) { glUniformMatrix4fv(loc,1,false,v); } + + +EIGEN_GL_FUNC1_DECLARATION (glUniform,GLint,const) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,float, 2,2fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,int, 2,2iv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,float, 3,3fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,int, 3,3iv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,float, 4,4fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,int, 4,4iv_ei) + +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 2,2,Matrix2fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 3,3,Matrix3fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 4,4,Matrix4fv_ei) + +#endif + +#ifdef GL_VERSION_2_1 + +static void glUniformMatrix2x3fv_ei(GLint loc, const float* v) { glUniformMatrix2x3fv(loc,1,false,v); } +static void glUniformMatrix3x2fv_ei(GLint loc, const float* v) { glUniformMatrix3x2fv(loc,1,false,v); } +static void glUniformMatrix2x4fv_ei(GLint loc, const float* v) { glUniformMatrix2x4fv(loc,1,false,v); } +static void glUniformMatrix4x2fv_ei(GLint loc, const float* v) { glUniformMatrix4x2fv(loc,1,false,v); } +static void glUniformMatrix3x4fv_ei(GLint loc, const float* v) { glUniformMatrix3x4fv(loc,1,false,v); } +static void glUniformMatrix4x3fv_ei(GLint loc, const float* v) { glUniformMatrix4x3fv(loc,1,false,v); } + +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 2,3,Matrix2x3fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 3,2,Matrix3x2fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 2,4,Matrix2x4fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 4,2,Matrix4x2fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 3,4,Matrix3x4fv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 4,3,Matrix4x3fv_ei) + +#endif + +#ifdef GL_VERSION_3_0 + +static void glUniform2uiv_ei (GLint loc, const unsigned int* v) { glUniform2uiv(loc,1,v); } +static void glUniform3uiv_ei (GLint loc, const unsigned int* v) { glUniform3uiv(loc,1,v); } +static void glUniform4uiv_ei (GLint loc, const unsigned int* v) { glUniform4uiv(loc,1,v); } + +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 2,2uiv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 3,3uiv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 4,4uiv_ei) + +#endif + +#ifdef GL_ARB_gpu_shader_fp64 +static void glUniform2dv_ei (GLint loc, const double* v) { glUniform2dv(loc,1,v); } +static void glUniform3dv_ei (GLint loc, const double* v) { glUniform3dv(loc,1,v); } +static void glUniform4dv_ei (GLint loc, const double* v) { glUniform4dv(loc,1,v); } + +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 2,2dv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 3,3dv_ei) +EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 4,4dv_ei) +#endif + + +//@} + +} + +#endif // EIGEN_OPENGL_MODULE diff --git a/unsupported/Eigen/Polynomials b/unsupported/Eigen/Polynomials new file mode 100644 index 000000000..fa58b006d --- /dev/null +++ b/unsupported/Eigen/Polynomials @@ -0,0 +1,133 @@ +#ifndef EIGEN_POLYNOMIALS_MODULE_H +#define EIGEN_POLYNOMIALS_MODULE_H + +#include <Eigen/Core> + +#include <Eigen/src/Core/util/DisableStupidWarnings.h> + +#include <Eigen/Eigenvalues> + +// Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module +#if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2) + #ifndef EIGEN_HIDE_HEAVY_CODE + #define EIGEN_HIDE_HEAVY_CODE + #endif +#elif defined EIGEN_HIDE_HEAVY_CODE + #undef EIGEN_HIDE_HEAVY_CODE +#endif + +/** \ingroup Unsupported_modules + * \defgroup Polynomials_Module Polynomials module + * + * + * + * \brief This module provides a QR based polynomial solver. + * + * To use this module, add + * \code + * #include <unsupported/Eigen/Polynomials> + * \endcode + * at the start of your source file. + */ + +#include "src/Polynomials/PolynomialUtils.h" +#include "src/Polynomials/Companion.h" +#include "src/Polynomials/PolynomialSolver.h" + +/** + \page polynomials Polynomials defines functions for dealing with polynomials + and a QR based polynomial solver. + \ingroup Polynomials_Module + + The remainder of the page documents first the functions for evaluating, computing + polynomials, computing estimates about polynomials and next the QR based polynomial + solver. + + \section polynomialUtils convenient functions to deal with polynomials + \subsection roots_to_monicPolynomial + The function + \code + void roots_to_monicPolynomial( const RootVector& rv, Polynomial& poly ) + \endcode + computes the coefficients \f$ a_i \f$ of + + \f$ p(x) = a_0 + a_{1}x + ... + a_{n-1}x^{n-1} + x^n \f$ + + where \f$ p \f$ is known through its roots i.e. \f$ p(x) = (x-r_1)(x-r_2)...(x-r_n) \f$. + + \subsection poly_eval + The function + \code + T poly_eval( const Polynomials& poly, const T& x ) + \endcode + evaluates a polynomial at a given point using stabilized Hörner method. + + The following code: first computes the coefficients in the monomial basis of the monic polynomial that has the provided roots; + then, it evaluates the computed polynomial, using a stabilized Hörner method. + + \include PolynomialUtils1.cpp + Output: \verbinclude PolynomialUtils1.out + + \subsection Cauchy bounds + The function + \code + Real cauchy_max_bound( const Polynomial& poly ) + \endcode + provides a maximum bound (the Cauchy one: \f$C(p)\f$) for the absolute value of a root of the given polynomial i.e. + \f$ \forall r_i \f$ root of \f$ p(x) = \sum_{k=0}^d a_k x^k \f$, + \f$ |r_i| \le C(p) = \sum_{k=0}^{d} \left | \frac{a_k}{a_d} \right | \f$ + The leading coefficient \f$ p \f$: should be non zero \f$a_d \neq 0\f$. + + + The function + \code + Real cauchy_min_bound( const Polynomial& poly ) + \endcode + provides a minimum bound (the Cauchy one: \f$c(p)\f$) for the absolute value of a non zero root of the given polynomial i.e. + \f$ \forall r_i \neq 0 \f$ root of \f$ p(x) = \sum_{k=0}^d a_k x^k \f$, + \f$ |r_i| \ge c(p) = \left( \sum_{k=0}^{d} \left | \frac{a_k}{a_0} \right | \right)^{-1} \f$ + + + + + \section QR polynomial solver class + Computes the complex roots of a polynomial by computing the eigenvalues of the associated companion matrix with the QR algorithm. + + The roots of \f$ p(x) = a_0 + a_1 x + a_2 x^2 + a_{3} x^3 + x^4 \f$ are the eigenvalues of + \f$ + \left [ + \begin{array}{cccc} + 0 & 0 & 0 & a_0 \\ + 1 & 0 & 0 & a_1 \\ + 0 & 1 & 0 & a_2 \\ + 0 & 0 & 1 & a_3 + \end{array} \right ] + \f$ + + However, the QR algorithm is not guaranteed to converge when there are several eigenvalues with same modulus. + + Therefore the current polynomial solver is guaranteed to provide a correct result only when the complex roots \f$r_1,r_2,...,r_d\f$ have distinct moduli i.e. + + \f$ \forall i,j \in [1;d],~ \| r_i \| \neq \| r_j \| \f$. + + With 32bit (float) floating types this problem shows up frequently. + However, almost always, correct accuracy is reached even in these cases for 64bit + (double) floating types and small polynomial degree (<20). + + \include PolynomialSolver1.cpp + + In the above example: + + -# a simple use of the polynomial solver is shown; + -# the accuracy problem with the QR algorithm is presented: a polynomial with almost conjugate roots is provided to the solver. + Those roots have almost same module therefore the QR algorithm failed to converge: the accuracy + of the last root is bad; + -# a simple way to circumvent the problem is shown: use doubles instead of floats. + + Output: \verbinclude PolynomialSolver1.out +*/ + +#include <Eigen/src/Core/util/ReenableStupidWarnings.h> + +#endif // EIGEN_POLYNOMIALS_MODULE_H +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/unsupported/Eigen/Skyline b/unsupported/Eigen/Skyline new file mode 100644 index 000000000..c9823f358 --- /dev/null +++ b/unsupported/Eigen/Skyline @@ -0,0 +1,31 @@ +#ifndef EIGEN_SKYLINE_MODULE_H +#define EIGEN_SKYLINE_MODULE_H + + +#include "Eigen/Core" + +#include "Eigen/src/Core/util/DisableStupidWarnings.h" + +#include <map> +#include <cstdlib> +#include <cstring> +#include <algorithm> + +/** \ingroup Unsupported_modules + * \defgroup Skyline_Module Skyline module + * + * + * + * + */ + +#include "src/Skyline/SkylineUtil.h" +#include "src/Skyline/SkylineMatrixBase.h" +#include "src/Skyline/SkylineStorage.h" +#include "src/Skyline/SkylineMatrix.h" +#include "src/Skyline/SkylineInplaceLU.h" +#include "src/Skyline/SkylineProduct.h" + +#include "Eigen/src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SKYLINE_MODULE_H diff --git a/unsupported/Eigen/SparseExtra b/unsupported/Eigen/SparseExtra new file mode 100644 index 000000000..340c34736 --- /dev/null +++ b/unsupported/Eigen/SparseExtra @@ -0,0 +1,47 @@ +#ifndef EIGEN_SPARSE_EXTRA_MODULE_H +#define EIGEN_SPARSE_EXTRA_MODULE_H + +#include "../../Eigen/Sparse" + +#include "../../Eigen/src/Core/util/DisableStupidWarnings.h" + +#include <vector> +#include <map> +#include <cstdlib> +#include <cstring> +#include <algorithm> +#include <fstream> +#include <sstream> + +#ifdef EIGEN_GOOGLEHASH_SUPPORT + #include <google/dense_hash_map> +#endif + +/** \ingroup Unsupported_modules + * \defgroup SparseExtra_Module SparseExtra module + * + * This module contains some experimental features extending the sparse module. + * + * \code + * #include <Eigen/SparseExtra> + * \endcode + */ + + +#include "../../Eigen/src/misc/Solve.h" +#include "../../Eigen/src/misc/SparseSolve.h" + +#include "src/SparseExtra/DynamicSparseMatrix.h" +#include "src/SparseExtra/BlockOfDynamicSparseMatrix.h" +#include "src/SparseExtra/RandomSetter.h" + +#include "src/SparseExtra/MarketIO.h" + +#if !defined(_WIN32) +#include <dirent.h> +#include "src/SparseExtra/MatrixMarketIterator.h" +#endif + +#include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_SPARSE_EXTRA_MODULE_H diff --git a/unsupported/Eigen/Splines b/unsupported/Eigen/Splines new file mode 100644 index 000000000..801cec1a1 --- /dev/null +++ b/unsupported/Eigen/Splines @@ -0,0 +1,31 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.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_SPLINES_MODULE_H +#define EIGEN_SPLINES_MODULE_H + +namespace Eigen +{ +/** \ingroup Unsupported_modules + * \defgroup Splines_Module Spline and spline fitting module + * + * This module provides a simple multi-dimensional spline class while + * offering most basic functionality to fit a spline to point sets. + * + * \code + * #include <unsupported/Eigen/Splines> + * \endcode + */ +} + +#include "src/Splines/SplineFwd.h" +#include "src/Splines/Spline.h" +#include "src/Splines/SplineFitting.h" + +#endif // EIGEN_SPLINES_MODULE_H diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h new file mode 100644 index 000000000..1a61e3367 --- /dev/null +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h @@ -0,0 +1,83 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_AUTODIFF_JACOBIAN_H +#define EIGEN_AUTODIFF_JACOBIAN_H + +namespace Eigen +{ + +template<typename Functor> class AutoDiffJacobian : public Functor +{ +public: + AutoDiffJacobian() : Functor() {} + AutoDiffJacobian(const Functor& f) : Functor(f) {} + + // forward constructors + template<typename T0> + AutoDiffJacobian(const T0& a0) : Functor(a0) {} + template<typename T0, typename T1> + AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {} + template<typename T0, typename T1, typename T2> + AutoDiffJacobian(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2) {} + + enum { + InputsAtCompileTime = Functor::InputsAtCompileTime, + ValuesAtCompileTime = Functor::ValuesAtCompileTime + }; + + typedef typename Functor::InputType InputType; + typedef typename Functor::ValueType ValueType; + typedef typename Functor::JacobianType JacobianType; + typedef typename JacobianType::Scalar Scalar; + typedef typename JacobianType::Index Index; + + typedef Matrix<Scalar,InputsAtCompileTime,1> DerivativeType; + typedef AutoDiffScalar<DerivativeType> ActiveScalar; + + + typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput; + typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue; + + void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const + { + eigen_assert(v!=0); + if (!_jac) + { + Functor::operator()(x, v); + return; + } + + JacobianType& jac = *_jac; + + ActiveInput ax = x.template cast<ActiveScalar>(); + ActiveValue av(jac.rows()); + + if(InputsAtCompileTime==Dynamic) + for (Index j=0; j<jac.rows(); j++) + av[j].derivatives().resize(this->inputs()); + + for (Index i=0; i<jac.cols(); i++) + ax[i].derivatives() = DerivativeType::Unit(this->inputs(),i); + + Functor::operator()(ax, &av); + + for (Index i=0; i<jac.rows(); i++) + { + (*v)[i] = av[i].value(); + jac.row(i) = av[i].derivatives(); + } + } +protected: + +}; + +} + +#endif // EIGEN_AUTODIFF_JACOBIAN_H diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h new file mode 100644 index 000000000..b833df3c0 --- /dev/null +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h @@ -0,0 +1,632 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_AUTODIFF_SCALAR_H +#define EIGEN_AUTODIFF_SCALAR_H + +namespace Eigen { + +namespace internal { + +template<typename A, typename B> +struct make_coherent_impl { + static void run(A&, B&) {} +}; + +// resize a to match b is a.size()==0, and conversely. +template<typename A, typename B> +void make_coherent(const A& a, const B&b) +{ + make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived()); +} + +template<typename _DerType, bool Enable> struct auto_diff_special_op; + +} // end namespace internal + +/** \class AutoDiffScalar + * \brief A scalar type replacement with automatic differentation capability + * + * \param _DerType the vector type used to store/represent the derivatives. The base scalar type + * as well as the number of derivatives to compute are determined from this type. + * Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf + * if the number of derivatives is not known at compile time, and/or, the number + * of derivatives is large. + * Note that _DerType can also be a reference (e.g., \c VectorXf&) to wrap a + * existing vector into an AutoDiffScalar. + * Finally, _DerType can also be any Eigen compatible expression. + * + * This class represents a scalar value while tracking its respective derivatives using Eigen's expression + * template mechanism. + * + * It supports the following list of global math function: + * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, + * - internal::abs, internal::sqrt, internal::pow, internal::exp, internal::log, internal::sin, internal::cos, + * - internal::conj, internal::real, internal::imag, internal::abs2. + * + * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However, + * in that case, the expression template mechanism only occurs at the top Matrix level, + * while derivatives are computed right away. + * + */ + +template<typename _DerType> +class AutoDiffScalar + : public internal::auto_diff_special_op + <_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar, + typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value> +{ + public: + typedef internal::auto_diff_special_op + <_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar, + typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value> Base; + typedef typename internal::remove_all<_DerType>::type DerType; + typedef typename internal::traits<DerType>::Scalar Scalar; + typedef typename NumTraits<Scalar>::Real Real; + + using Base::operator+; + using Base::operator*; + + /** Default constructor without any initialization. */ + AutoDiffScalar() {} + + /** Constructs an active scalar from its \a value, + and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */ + AutoDiffScalar(const Scalar& value, int nbDer, int derNumber) + : m_value(value), m_derivatives(DerType::Zero(nbDer)) + { + m_derivatives.coeffRef(derNumber) = Scalar(1); + } + + /** Conversion from a scalar constant to an active scalar. + * The derivatives are set to zero. */ + /*explicit*/ AutoDiffScalar(const Real& value) + : m_value(value) + { + if(m_derivatives.size()>0) + m_derivatives.setZero(); + } + + /** Constructs an active scalar from its \a value and derivatives \a der */ + AutoDiffScalar(const Scalar& value, const DerType& der) + : m_value(value), m_derivatives(der) + {} + + template<typename OtherDerType> + AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other) + : m_value(other.value()), m_derivatives(other.derivatives()) + {} + + friend std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a) + { + return s << a.value(); + } + + AutoDiffScalar(const AutoDiffScalar& other) + : m_value(other.value()), m_derivatives(other.derivatives()) + {} + + template<typename OtherDerType> + inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other) + { + m_value = other.value(); + m_derivatives = other.derivatives(); + return *this; + } + + inline AutoDiffScalar& operator=(const AutoDiffScalar& other) + { + m_value = other.value(); + m_derivatives = other.derivatives(); + return *this; + } + +// inline operator const Scalar& () const { return m_value; } +// inline operator Scalar& () { return m_value; } + + inline const Scalar& value() const { return m_value; } + inline Scalar& value() { return m_value; } + + inline const DerType& derivatives() const { return m_derivatives; } + inline DerType& derivatives() { return m_derivatives; } + + inline bool operator< (const Scalar& other) const { return m_value < other; } + inline bool operator<=(const Scalar& other) const { return m_value <= other; } + inline bool operator> (const Scalar& other) const { return m_value > other; } + inline bool operator>=(const Scalar& other) const { return m_value >= other; } + inline bool operator==(const Scalar& other) const { return m_value == other; } + inline bool operator!=(const Scalar& other) const { return m_value != other; } + + friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a < b.value(); } + friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); } + friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a > b.value(); } + friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); } + friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); } + friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); } + + template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const { return m_value < b.value(); } + template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const { return m_value <= b.value(); } + template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const { return m_value > b.value(); } + template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const { return m_value >= b.value(); } + template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const { return m_value == b.value(); } + template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const { return m_value != b.value(); } + + inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const + { + return AutoDiffScalar<DerType&>(m_value + other, m_derivatives); + } + + friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b) + { + return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives()); + } + +// inline const AutoDiffScalar<DerType&> operator+(const Real& other) const +// { +// return AutoDiffScalar<DerType&>(m_value + other, m_derivatives); +// } + +// friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b) +// { +// return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives()); +// } + + inline AutoDiffScalar& operator+=(const Scalar& other) + { + value() += other; + return *this; + } + + template<typename OtherDerType> + inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> > + operator+(const AutoDiffScalar<OtherDerType>& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >( + m_value + other.value(), + m_derivatives + other.derivatives()); + } + + template<typename OtherDerType> + inline AutoDiffScalar& + operator+=(const AutoDiffScalar<OtherDerType>& other) + { + (*this) = (*this) + other; + return *this; + } + + inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const + { + return AutoDiffScalar<DerType&>(m_value - b, m_derivatives); + } + + friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> > + operator-(const Scalar& a, const AutoDiffScalar& b) + { + return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> > + (a - b.value(), -b.derivatives()); + } + + inline AutoDiffScalar& operator-=(const Scalar& other) + { + value() -= other; + return *this; + } + + template<typename OtherDerType> + inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> > + operator-(const AutoDiffScalar<OtherDerType>& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >( + m_value - other.value(), + m_derivatives - other.derivatives()); + } + + template<typename OtherDerType> + inline AutoDiffScalar& + operator-=(const AutoDiffScalar<OtherDerType>& other) + { + *this = *this - other; + return *this; + } + + inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> > + operator-() const + { + return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >( + -m_value, + -m_derivatives); + } + + inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> > + operator*(const Scalar& other) const + { + return AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> >( + m_value * other, + (m_derivatives * other)); + } + + friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> > + operator*(const Scalar& other, const AutoDiffScalar& a) + { + return AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> >( + a.value() * other, + a.derivatives() * other); + } + +// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type > +// operator*(const Real& other) const +// { +// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >( +// m_value * other, +// (m_derivatives * other)); +// } +// +// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type > +// operator*(const Real& other, const AutoDiffScalar& a) +// { +// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >( +// a.value() * other, +// a.derivatives() * other); +// } + + inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> > + operator/(const Scalar& other) const + { + return AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> >( + m_value / other, + (m_derivatives * (Scalar(1)/other))); + } + + friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> > + operator/(const Scalar& other, const AutoDiffScalar& a) + { + return AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType> >( + other / a.value(), + a.derivatives() * (Scalar(-other) / (a.value()*a.value()))); + } + +// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type > +// operator/(const Real& other) const +// { +// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >( +// m_value / other, +// (m_derivatives * (Real(1)/other))); +// } +// +// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type > +// operator/(const Real& other, const AutoDiffScalar& a) +// { +// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >( +// other / a.value(), +// a.derivatives() * (-Real(1)/other)); +// } + + template<typename OtherDerType> + inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, + const CwiseBinaryOp<internal::scalar_difference_op<Scalar>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const typename internal::remove_all<OtherDerType>::type > > > > + operator/(const AutoDiffScalar<OtherDerType>& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return AutoDiffScalar<CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, + const CwiseBinaryOp<internal::scalar_difference_op<Scalar>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const typename internal::remove_all<OtherDerType>::type > > > >( + m_value / other.value(), + ((m_derivatives * other.value()) - (m_value * other.derivatives())) + * (Scalar(1)/(other.value()*other.value()))); + } + + template<typename OtherDerType> + inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const typename internal::remove_all<OtherDerType>::type> > > + operator*(const AutoDiffScalar<OtherDerType>& other) const + { + internal::make_coherent(m_derivatives, other.derivatives()); + return AutoDiffScalar<const CwiseBinaryOp<internal::scalar_sum_op<Scalar>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const DerType>, + const CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, const typename internal::remove_all<OtherDerType>::type > > >( + m_value * other.value(), + (m_derivatives * other.value()) + (m_value * other.derivatives())); + } + + inline AutoDiffScalar& operator*=(const Scalar& other) + { + *this = *this * other; + return *this; + } + + template<typename OtherDerType> + inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other) + { + *this = *this * other; + return *this; + } + + inline AutoDiffScalar& operator/=(const Scalar& other) + { + *this = *this / other; + return *this; + } + + template<typename OtherDerType> + inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other) + { + *this = *this / other; + return *this; + } + + protected: + Scalar m_value; + DerType m_derivatives; + +}; + +namespace internal { + +template<typename _DerType> +struct auto_diff_special_op<_DerType, true> +// : auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real, +// is_same<Scalar,typename NumTraits<Scalar>::Real>::value> +{ + typedef typename remove_all<_DerType>::type DerType; + typedef typename traits<DerType>::Scalar Scalar; + typedef typename NumTraits<Scalar>::Real Real; + +// typedef auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real, +// is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base; + +// using Base::operator+; +// using Base::operator+=; +// using Base::operator-; +// using Base::operator-=; +// using Base::operator*; +// using Base::operator*=; + + const AutoDiffScalar<_DerType>& derived() const { return *static_cast<const AutoDiffScalar<_DerType>*>(this); } + AutoDiffScalar<_DerType>& derived() { return *static_cast<AutoDiffScalar<_DerType>*>(this); } + + + inline const AutoDiffScalar<DerType&> operator+(const Real& other) const + { + return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives()); + } + + friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<_DerType>& b) + { + return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives()); + } + + inline AutoDiffScalar<_DerType>& operator+=(const Real& other) + { + derived().value() += other; + return derived(); + } + + + inline const AutoDiffScalar<typename CwiseUnaryOp<scalar_multiple2_op<Scalar,Real>, DerType>::Type > + operator*(const Real& other) const + { + return AutoDiffScalar<typename CwiseUnaryOp<scalar_multiple2_op<Scalar,Real>, DerType>::Type >( + derived().value() * other, + derived().derivatives() * other); + } + + friend inline const AutoDiffScalar<typename CwiseUnaryOp<scalar_multiple2_op<Scalar,Real>, DerType>::Type > + operator*(const Real& other, const AutoDiffScalar<_DerType>& a) + { + return AutoDiffScalar<typename CwiseUnaryOp<scalar_multiple2_op<Scalar,Real>, DerType>::Type >( + a.value() * other, + a.derivatives() * other); + } + + inline AutoDiffScalar<_DerType>& operator*=(const Scalar& other) + { + *this = *this * other; + return derived(); + } +}; + +template<typename _DerType> +struct auto_diff_special_op<_DerType, false> +{ + void operator*() const; + void operator-() const; + void operator+() const; +}; + +template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B> +struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> { + typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + } +}; + +template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols> +struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > { + typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B; + static void run(A& a, B& b) { + if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } +}; + +template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, + typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols> +struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, + Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > { + typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A; + typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B; + static void run(A& a, B& b) { + if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0)) + { + a.resize(b.size()); + a.setZero(); + } + else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0)) + { + b.resize(a.size()); + b.setZero(); + } + } +}; + +template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols> struct scalar_product_traits<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,A_Scalar> +{ + typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> ReturnType; +}; + +template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols> struct scalar_product_traits<A_Scalar, Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> > +{ + typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> ReturnType; +}; + +template<typename DerType> +struct scalar_product_traits<AutoDiffScalar<DerType>,typename DerType::Scalar> +{ + typedef AutoDiffScalar<DerType> ReturnType; +}; + +} // end namespace internal + +#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \ + template<typename DerType> \ + inline const Eigen::AutoDiffScalar<Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar>, const typename Eigen::internal::remove_all<DerType>::type> > \ + FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \ + using namespace Eigen; \ + typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \ + typedef AutoDiffScalar<CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const typename Eigen::internal::remove_all<DerType>::type> > ReturnType; \ + CODE; \ + } + +template<typename DerType> +inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x) { return x; } +template<typename DerType> +inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x) { return x; } +template<typename DerType> +inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&) { return 0.; } +template<typename DerType, typename T> +inline AutoDiffScalar<DerType> (min)(const AutoDiffScalar<DerType>& x, const T& y) { return (x <= y ? x : y); } +template<typename DerType, typename T> +inline AutoDiffScalar<DerType> (max)(const AutoDiffScalar<DerType>& x, const T& y) { return (x >= y ? x : y); } +template<typename DerType, typename T> +inline AutoDiffScalar<DerType> (min)(const T& x, const AutoDiffScalar<DerType>& y) { return (x < y ? x : y); } +template<typename DerType, typename T> +inline AutoDiffScalar<DerType> (max)(const T& x, const AutoDiffScalar<DerType>& y) { return (x > y ? x : y); } + +#define sign(x) x >= 0 ? 1 : -1 // required for abs function below + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs, + using std::abs; + return ReturnType(abs(x.value()), x.derivatives() * (sign(x.value())));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2, + using internal::abs2; + return ReturnType(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt, + using std::sqrt; + Scalar sqrtx = sqrt(x.value()); + return ReturnType(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos, + using std::cos; + using std::sin; + return ReturnType(cos(x.value()), x.derivatives() * (-sin(x.value())));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin, + using std::sin; + using std::cos; + return ReturnType(sin(x.value()),x.derivatives() * cos(x.value()));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp, + using std::exp; + Scalar expx = exp(x.value()); + return ReturnType(expx,x.derivatives() * expx);) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log, + using std::log; + return ReturnType(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));) + +template<typename DerType> +inline const Eigen::AutoDiffScalar<Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<typename Eigen::internal::traits<DerType>::Scalar>, const DerType> > +pow(const Eigen::AutoDiffScalar<DerType>& x, typename Eigen::internal::traits<DerType>::Scalar y) +{ + using namespace Eigen; + typedef typename Eigen::internal::traits<DerType>::Scalar Scalar; + return AutoDiffScalar<CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const DerType> >( + std::pow(x.value(),y), + x.derivatives() * (y * std::pow(x.value(),y-1))); +} + + +template<typename DerTypeA,typename DerTypeB> +inline const AutoDiffScalar<Matrix<typename internal::traits<DerTypeA>::Scalar,Dynamic,1> > +atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b) +{ + using std::atan2; + using std::max; + typedef typename internal::traits<DerTypeA>::Scalar Scalar; + typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS; + PlainADS ret; + ret.value() = atan2(a.value(), b.value()); + + Scalar tmp2 = a.value() * a.value(); + Scalar tmp3 = b.value() * b.value(); + Scalar tmp4 = tmp3/(tmp2+tmp3); + + if (tmp4!=0) + ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) * (tmp2+tmp3); + + return ret; +} + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan, + using std::tan; + using std::cos; + return ReturnType(tan(x.value()),x.derivatives() * (Scalar(1)/internal::abs2(cos(x.value()))));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin, + using std::sqrt; + using std::asin; + return ReturnType(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-internal::abs2(x.value()))));) + +EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos, + using std::sqrt; + using std::acos; + return ReturnType(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-internal::abs2(x.value()))));) + +#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY + +template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> > + : NumTraits< typename NumTraits<typename DerType::Scalar>::Real > +{ + typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerType::Scalar>::Real,DerType::RowsAtCompileTime,DerType::ColsAtCompileTime> > Real; + typedef AutoDiffScalar<DerType> NonInteger; + typedef AutoDiffScalar<DerType>& Nested; + enum{ + RequireInitialization = 1 + }; +}; + +} + +#endif // EIGEN_AUTODIFF_SCALAR_H diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h new file mode 100644 index 000000000..0540add0a --- /dev/null +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h @@ -0,0 +1,220 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_AUTODIFF_VECTOR_H +#define EIGEN_AUTODIFF_VECTOR_H + +namespace Eigen { + +/* \class AutoDiffScalar + * \brief A scalar type replacement with automatic differentation capability + * + * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f) + * + * This class represents a scalar value while tracking its respective derivatives. + * + * It supports the following list of global math function: + * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos, + * - internal::abs, internal::sqrt, internal::pow, internal::exp, internal::log, internal::sin, internal::cos, + * - internal::conj, internal::real, internal::imag, internal::abs2. + * + * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However, + * in that case, the expression template mechanism only occurs at the top Matrix level, + * while derivatives are computed right away. + * + */ +template<typename ValueType, typename JacobianType> +class AutoDiffVector +{ + public: + //typedef typename internal::traits<ValueType>::Scalar Scalar; + typedef typename internal::traits<ValueType>::Scalar BaseScalar; + typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar; + typedef ActiveScalar Scalar; + typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType; + typedef typename JacobianType::Index Index; + + inline AutoDiffVector() {} + + inline AutoDiffVector(const ValueType& values) + : m_values(values) + { + m_jacobian.setZero(); + } + + + CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); } + const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); } + + Index size() const { return m_values.size(); } + + // FIXME here we could return an expression of the sum + Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); } + + + inline AutoDiffVector(const ValueType& values, const JacobianType& jac) + : m_values(values), m_jacobian(jac) + {} + + template<typename OtherValueType, typename OtherJacobianType> + inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other) + : m_values(other.values()), m_jacobian(other.jacobian()) + {} + + inline AutoDiffVector(const AutoDiffVector& other) + : m_values(other.values()), m_jacobian(other.jacobian()) + {} + + template<typename OtherValueType, typename OtherJacobianType> + inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other) + { + m_values = other.values(); + m_jacobian = other.jacobian(); + return *this; + } + + inline AutoDiffVector& operator=(const AutoDiffVector& other) + { + m_values = other.values(); + m_jacobian = other.jacobian(); + return *this; + } + + inline const ValueType& values() const { return m_values; } + inline ValueType& values() { return m_values; } + + inline const JacobianType& jacobian() const { return m_jacobian; } + inline JacobianType& jacobian() { return m_jacobian; } + + template<typename OtherValueType,typename OtherJacobianType> + inline const AutoDiffVector< + typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type > + operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const + { + return AutoDiffVector< + typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >( + m_values + other.values(), + m_jacobian + other.jacobian()); + } + + template<typename OtherValueType, typename OtherJacobianType> + inline AutoDiffVector& + operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) + { + m_values += other.values(); + m_jacobian += other.jacobian(); + return *this; + } + + template<typename OtherValueType,typename OtherJacobianType> + inline const AutoDiffVector< + typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type > + operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const + { + return AutoDiffVector< + typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type, + typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >( + m_values - other.values(), + m_jacobian - other.jacobian()); + } + + template<typename OtherValueType, typename OtherJacobianType> + inline AutoDiffVector& + operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) + { + m_values -= other.values(); + m_jacobian -= other.jacobian(); + return *this; + } + + inline const AutoDiffVector< + typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type > + operator-() const + { + return AutoDiffVector< + typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >( + -m_values, + -m_jacobian); + } + + inline const AutoDiffVector< + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type> + operator*(const BaseScalar& other) const + { + return AutoDiffVector< + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >( + m_values * other, + m_jacobian * other); + } + + friend inline const AutoDiffVector< + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type > + operator*(const Scalar& other, const AutoDiffVector& v) + { + return AutoDiffVector< + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type, + typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >( + v.values() * other, + v.jacobian() * other); + } + +// template<typename OtherValueType,typename OtherJacobianType> +// inline const AutoDiffVector< +// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType> +// CwiseBinaryOp<internal::scalar_sum_op<Scalar>, +// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>, +// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > > +// operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const +// { +// return AutoDiffVector< +// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType> +// CwiseBinaryOp<internal::scalar_sum_op<Scalar>, +// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>, +// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >( +// m_values.cwise() * other.values(), +// (m_jacobian * other.values()) + (m_values * other.jacobian())); +// } + + inline AutoDiffVector& operator*=(const Scalar& other) + { + m_values *= other; + m_jacobian *= other; + return *this; + } + + template<typename OtherValueType,typename OtherJacobianType> + inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) + { + *this = *this * other; + return *this; + } + + protected: + ValueType m_values; + JacobianType m_jacobian; + +}; + +} + +#endif // EIGEN_AUTODIFF_VECTOR_H diff --git a/unsupported/Eigen/src/AutoDiff/CMakeLists.txt b/unsupported/Eigen/src/AutoDiff/CMakeLists.txt new file mode 100644 index 000000000..ad91fd9c4 --- /dev/null +++ b/unsupported/Eigen/src/AutoDiff/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_AutoDiff_SRCS "*.h") + +INSTALL(FILES + ${Eigen_AutoDiff_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/AutoDiff COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/BVH/BVAlgorithms.h b/unsupported/Eigen/src/BVH/BVAlgorithms.h new file mode 100644 index 000000000..e5b51decb --- /dev/null +++ b/unsupported/Eigen/src/BVH/BVAlgorithms.h @@ -0,0 +1,293 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Ilya Baran <ibaran@mit.edu> +// +// 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_BVALGORITHMS_H +#define EIGEN_BVALGORITHMS_H + +namespace Eigen { + +namespace internal { + +#ifndef EIGEN_PARSED_BY_DOXYGEN +template<typename BVH, typename Intersector> +bool intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root) +{ + typedef typename BVH::Index Index; + typedef typename BVH::VolumeIterator VolIter; + typedef typename BVH::ObjectIterator ObjIter; + + VolIter vBegin = VolIter(), vEnd = VolIter(); + ObjIter oBegin = ObjIter(), oEnd = ObjIter(); + + std::vector<Index> todo(1, root); + + while(!todo.empty()) { + tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd); + todo.pop_back(); + + for(; vBegin != vEnd; ++vBegin) //go through child volumes + if(intersector.intersectVolume(tree.getVolume(*vBegin))) + todo.push_back(*vBegin); + + for(; oBegin != oEnd; ++oBegin) //go through child objects + if(intersector.intersectObject(*oBegin)) + return true; //intersector said to stop query + } + return false; +} +#endif //not EIGEN_PARSED_BY_DOXYGEN + +template<typename Volume1, typename Object1, typename Object2, typename Intersector> +struct intersector_helper1 +{ + intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {} + bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); } + bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); } + Object2 stored; + Intersector &intersector; +private: + intersector_helper1& operator=(const intersector_helper1&); +}; + +template<typename Volume2, typename Object2, typename Object1, typename Intersector> +struct intersector_helper2 +{ + intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {} + bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); } + bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); } + Object1 stored; + Intersector &intersector; +private: + intersector_helper2& operator=(const intersector_helper2&); +}; + +} // end namespace internal + +/** Given a BVH, runs the query encapsulated by \a intersector. + * The Intersector type must provide the following members: \code + bool intersectVolume(const BVH::Volume &volume) //returns true if volume intersects the query + bool intersectObject(const BVH::Object &object) //returns true if the search should terminate immediately + \endcode + */ +template<typename BVH, typename Intersector> +void BVIntersect(const BVH &tree, Intersector &intersector) +{ + internal::intersect_helper(tree, intersector, tree.getRootIndex()); +} + +/** Given two BVH's, runs the query on their Cartesian product encapsulated by \a intersector. + * The Intersector type must provide the following members: \code + bool intersectVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2) //returns true if product of volumes intersects the query + bool intersectVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2) //returns true if the volume-object product intersects the query + bool intersectObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2) //returns true if the volume-object product intersects the query + bool intersectObjectObject(const BVH1::Object &o1, const BVH2::Object &o2) //returns true if the search should terminate immediately + \endcode + */ +template<typename BVH1, typename BVH2, typename Intersector> +void BVIntersect(const BVH1 &tree1, const BVH2 &tree2, Intersector &intersector) //TODO: tandem descent when it makes sense +{ + typedef typename BVH1::Index Index1; + typedef typename BVH2::Index Index2; + typedef internal::intersector_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Intersector> Helper1; + typedef internal::intersector_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Intersector> Helper2; + typedef typename BVH1::VolumeIterator VolIter1; + typedef typename BVH1::ObjectIterator ObjIter1; + typedef typename BVH2::VolumeIterator VolIter2; + typedef typename BVH2::ObjectIterator ObjIter2; + + VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1(); + ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1(); + VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2(); + ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2(); + + std::vector<std::pair<Index1, Index2> > todo(1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())); + + while(!todo.empty()) { + tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1); + tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2); + todo.pop_back(); + + for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree + const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1); + for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree + if(intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2))) + todo.push_back(std::make_pair(*vBegin1, *vCur2)); + } + + for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree + Helper1 helper(*oCur2, intersector); + if(internal::intersect_helper(tree1, helper, *vBegin1)) + return; //intersector said to stop query + } + } + + for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree + for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree + Helper2 helper(*oBegin1, intersector); + if(internal::intersect_helper(tree2, helper, *vCur2)) + return; //intersector said to stop query + } + + for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree + if(intersector.intersectObjectObject(*oBegin1, *oCur2)) + return; //intersector said to stop query + } + } + } +} + +namespace internal { + +#ifndef EIGEN_PARSED_BY_DOXYGEN +template<typename BVH, typename Minimizer> +typename Minimizer::Scalar minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum) +{ + typedef typename Minimizer::Scalar Scalar; + typedef typename BVH::Index Index; + typedef std::pair<Scalar, Index> QueueElement; //first element is priority + typedef typename BVH::VolumeIterator VolIter; + typedef typename BVH::ObjectIterator ObjIter; + + VolIter vBegin = VolIter(), vEnd = VolIter(); + ObjIter oBegin = ObjIter(), oEnd = ObjIter(); + std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top + + todo.push(std::make_pair(Scalar(), root)); + + while(!todo.empty()) { + tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd); + todo.pop(); + + for(; oBegin != oEnd; ++oBegin) //go through child objects + minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin)); + + for(; vBegin != vEnd; ++vBegin) { //go through child volumes + Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin)); + if(val < minimum) + todo.push(std::make_pair(val, *vBegin)); + } + } + + return minimum; +} +#endif //not EIGEN_PARSED_BY_DOXYGEN + + +template<typename Volume1, typename Object1, typename Object2, typename Minimizer> +struct minimizer_helper1 +{ + typedef typename Minimizer::Scalar Scalar; + minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {} + Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); } + Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); } + Object2 stored; + Minimizer &minimizer; +private: + minimizer_helper1& operator=(const minimizer_helper1&) {} +}; + +template<typename Volume2, typename Object2, typename Object1, typename Minimizer> +struct minimizer_helper2 +{ + typedef typename Minimizer::Scalar Scalar; + minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {} + Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); } + Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); } + Object1 stored; + Minimizer &minimizer; +private: + minimizer_helper2& operator=(const minimizer_helper2&); +}; + +} // end namespace internal + +/** Given a BVH, runs the query encapsulated by \a minimizer. + * \returns the minimum value. + * The Minimizer type must provide the following members: \code + typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one) + Scalar minimumOnVolume(const BVH::Volume &volume) + Scalar minimumOnObject(const BVH::Object &object) + \endcode + */ +template<typename BVH, typename Minimizer> +typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer) +{ + return internal::minimize_helper(tree, minimizer, tree.getRootIndex(), (std::numeric_limits<typename Minimizer::Scalar>::max)()); +} + +/** Given two BVH's, runs the query on their cartesian product encapsulated by \a minimizer. + * \returns the minimum value. + * The Minimizer type must provide the following members: \code + typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one) + Scalar minimumOnVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2) + Scalar minimumOnVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2) + Scalar minimumOnObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2) + Scalar minimumOnObjectObject(const BVH1::Object &o1, const BVH2::Object &o2) + \endcode + */ +template<typename BVH1, typename BVH2, typename Minimizer> +typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer) +{ + typedef typename Minimizer::Scalar Scalar; + typedef typename BVH1::Index Index1; + typedef typename BVH2::Index Index2; + typedef internal::minimizer_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Minimizer> Helper1; + typedef internal::minimizer_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Minimizer> Helper2; + typedef std::pair<Scalar, std::pair<Index1, Index2> > QueueElement; //first element is priority + typedef typename BVH1::VolumeIterator VolIter1; + typedef typename BVH1::ObjectIterator ObjIter1; + typedef typename BVH2::VolumeIterator VolIter2; + typedef typename BVH2::ObjectIterator ObjIter2; + + VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1(); + ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1(); + VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2(); + ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2(); + std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top + + Scalar minimum = (std::numeric_limits<Scalar>::max)(); + todo.push(std::make_pair(Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()))); + + while(!todo.empty()) { + tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1); + tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2); + todo.pop(); + + for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree + for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree + minimum = (std::min)(minimum, minimizer.minimumOnObjectObject(*oBegin1, *oCur2)); + } + + for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree + Helper2 helper(*oBegin1, minimizer); + minimum = (std::min)(minimum, internal::minimize_helper(tree2, helper, *vCur2, minimum)); + } + } + + for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree + const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1); + + for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree + Helper1 helper(*oCur2, minimizer); + minimum = (std::min)(minimum, internal::minimize_helper(tree1, helper, *vBegin1, minimum)); + } + + for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree + Scalar val = minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2)); + if(val < minimum) + todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2))); + } + } + } + return minimum; +} + +} // end namespace Eigen + +#endif // EIGEN_BVALGORITHMS_H diff --git a/unsupported/Eigen/src/BVH/CMakeLists.txt b/unsupported/Eigen/src/BVH/CMakeLists.txt new file mode 100644 index 000000000..b377d865c --- /dev/null +++ b/unsupported/Eigen/src/BVH/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_BVH_SRCS "*.h") + +INSTALL(FILES + ${Eigen_BVH_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/BVH COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/BVH/KdBVH.h b/unsupported/Eigen/src/BVH/KdBVH.h new file mode 100644 index 000000000..1b8d75865 --- /dev/null +++ b/unsupported/Eigen/src/BVH/KdBVH.h @@ -0,0 +1,222 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Ilya Baran <ibaran@mit.edu> +// +// 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 KDBVH_H_INCLUDED +#define KDBVH_H_INCLUDED + +namespace Eigen { + +namespace internal { + +//internal pair class for the BVH--used instead of std::pair because of alignment +template<typename Scalar, int Dim> +struct vector_int_pair +{ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim) + typedef Matrix<Scalar, Dim, 1> VectorType; + + vector_int_pair(const VectorType &v, int i) : first(v), second(i) {} + + VectorType first; + int second; +}; + +//these templates help the tree initializer get the bounding boxes either from a provided +//iterator range or using bounding_box in a unified way +template<typename ObjectList, typename VolumeList, typename BoxIter> +struct get_boxes_helper { + void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes) + { + outBoxes.insert(outBoxes.end(), boxBegin, boxEnd); + eigen_assert(outBoxes.size() == objects.size()); + } +}; + +template<typename ObjectList, typename VolumeList> +struct get_boxes_helper<ObjectList, VolumeList, int> { + void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes) + { + outBoxes.reserve(objects.size()); + for(int i = 0; i < (int)objects.size(); ++i) + outBoxes.push_back(bounding_box(objects[i])); + } +}; + +} // end namespace internal + + +/** \class KdBVH + * \brief A simple bounding volume hierarchy based on AlignedBox + * + * \param _Scalar The underlying scalar type of the bounding boxes + * \param _Dim The dimension of the space in which the hierarchy lives + * \param _Object The object type that lives in the hierarchy. It must have value semantics. Either bounding_box(_Object) must + * be defined and return an AlignedBox<_Scalar, _Dim> or bounding boxes must be provided to the tree initializer. + * + * This class provides a simple (as opposed to optimized) implementation of a bounding volume hierarchy analogous to a Kd-tree. + * Given a sequence of objects, it computes their bounding boxes, constructs a Kd-tree of their centers + * and builds a BVH with the structure of that Kd-tree. When the elements of the tree are too expensive to be copied around, + * it is useful for _Object to be a pointer. + */ +template<typename _Scalar, int _Dim, typename _Object> class KdBVH +{ +public: + enum { Dim = _Dim }; + typedef _Object Object; + typedef std::vector<Object, aligned_allocator<Object> > ObjectList; + typedef _Scalar Scalar; + typedef AlignedBox<Scalar, Dim> Volume; + typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList; + typedef int Index; + typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors + typedef const Object *ObjectIterator; + + KdBVH() {} + + /** Given an iterator range over \a Object references, constructs the BVH. Requires that bounding_box(Object) return a Volume. */ + template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type + + /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, constructs the BVH */ + template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); } + + /** Given an iterator range over \a Object references, constructs the BVH, overwriting whatever is in there currently. + * Requires that bounding_box(Object) return a Volume. */ + template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); } + + /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, + * constructs the BVH, overwriting whatever is in there currently. */ + template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) + { + objects.clear(); + boxes.clear(); + children.clear(); + + objects.insert(objects.end(), begin, end); + int n = static_cast<int>(objects.size()); + + if(n < 2) + return; //if we have at most one object, we don't need any internal nodes + + VolumeList objBoxes; + VIPairList objCenters; + + //compute the bounding boxes depending on BIter type + internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes); + + objCenters.reserve(n); + boxes.reserve(n - 1); + children.reserve(2 * n - 2); + + for(int i = 0; i < n; ++i) + objCenters.push_back(VIPair(objBoxes[i].center(), i)); + + build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm + + ObjectList tmp(n); + tmp.swap(objects); + for(int i = 0; i < n; ++i) + objects[i] = tmp[objCenters[i].second]; + } + + /** \returns the index of the root of the hierarchy */ + inline Index getRootIndex() const { return (int)boxes.size() - 1; } + + /** Given an \a index of a node, on exit, \a outVBegin and \a outVEnd range over the indices of the volume children of the node + * and \a outOBegin and \a outOEnd range over the object children of the node */ + EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, + ObjectIterator &outOBegin, ObjectIterator &outOEnd) const + { //inlining this function should open lots of optimization opportunities to the compiler + if(index < 0) { + outVBegin = outVEnd; + if(!objects.empty()) + outOBegin = &(objects[0]); + outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object + return; + } + + int numBoxes = static_cast<int>(boxes.size()); + + int idx = index * 2; + if(children[idx + 1] < numBoxes) { //second index is always bigger + outVBegin = &(children[idx]); + outVEnd = outVBegin + 2; + outOBegin = outOEnd; + } + else if(children[idx] >= numBoxes) { //if both children are objects + outVBegin = outVEnd; + outOBegin = &(objects[children[idx] - numBoxes]); + outOEnd = outOBegin + 2; + } else { //if the first child is a volume and the second is an object + outVBegin = &(children[idx]); + outVEnd = outVBegin + 1; + outOBegin = &(objects[children[idx + 1] - numBoxes]); + outOEnd = outOBegin + 1; + } + } + + /** \returns the bounding box of the node at \a index */ + inline const Volume &getVolume(Index index) const + { + return boxes[index]; + } + +private: + typedef internal::vector_int_pair<Scalar, Dim> VIPair; + typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList; + typedef Matrix<Scalar, Dim, 1> VectorType; + struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension + { + VectorComparator(int inDim) : dim(inDim) {} + inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; } + int dim; + }; + + //Build the part of the tree between objects[from] and objects[to] (not including objects[to]). + //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs + //the two halves, and adds their parent node. TODO: a cache-friendlier layout + void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim) + { + eigen_assert(to - from > 1); + if(to - from == 2) { + boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second])); + children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes + children.push_back(from + (int)objects.size()); + } + else if(to - from == 3) { + int mid = from + 2; + std::nth_element(objCenters.begin() + from, objCenters.begin() + mid, + objCenters.begin() + to, VectorComparator(dim)); //partition + build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); + int idx1 = (int)boxes.size() - 1; + boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second])); + children.push_back(idx1); + children.push_back(mid + (int)objects.size() - 1); + } + else { + int mid = from + (to - from) / 2; + nth_element(objCenters.begin() + from, objCenters.begin() + mid, + objCenters.begin() + to, VectorComparator(dim)); //partition + build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); + int idx1 = (int)boxes.size() - 1; + build(objCenters, mid, to, objBoxes, (dim + 1) % Dim); + int idx2 = (int)boxes.size() - 1; + boxes.push_back(boxes[idx1].merged(boxes[idx2])); + children.push_back(idx1); + children.push_back(idx2); + } + } + + std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects. + VolumeList boxes; + ObjectList objects; +}; + +} // end namespace Eigen + +#endif //KDBVH_H_INCLUDED diff --git a/unsupported/Eigen/src/CMakeLists.txt b/unsupported/Eigen/src/CMakeLists.txt new file mode 100644 index 000000000..f3180b52b --- /dev/null +++ b/unsupported/Eigen/src/CMakeLists.txt @@ -0,0 +1,13 @@ +ADD_SUBDIRECTORY(AutoDiff) +ADD_SUBDIRECTORY(BVH) +ADD_SUBDIRECTORY(FFT) +ADD_SUBDIRECTORY(IterativeSolvers) +ADD_SUBDIRECTORY(MatrixFunctions) +ADD_SUBDIRECTORY(MoreVectorization) +ADD_SUBDIRECTORY(NonLinearOptimization) +ADD_SUBDIRECTORY(NumericalDiff) +ADD_SUBDIRECTORY(Polynomials) +ADD_SUBDIRECTORY(Skyline) +ADD_SUBDIRECTORY(SparseExtra) +ADD_SUBDIRECTORY(KroneckerProduct) +ADD_SUBDIRECTORY(Splines) diff --git a/unsupported/Eigen/src/FFT/CMakeLists.txt b/unsupported/Eigen/src/FFT/CMakeLists.txt new file mode 100644 index 000000000..edcffcb18 --- /dev/null +++ b/unsupported/Eigen/src/FFT/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_FFT_SRCS "*.h") + +INSTALL(FILES + ${Eigen_FFT_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/FFT COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/FFT/ei_fftw_impl.h b/unsupported/Eigen/src/FFT/ei_fftw_impl.h new file mode 100644 index 000000000..d49aa17f5 --- /dev/null +++ b/unsupported/Eigen/src/FFT/ei_fftw_impl.h @@ -0,0 +1,261 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Mark Borgerding mark a borgerding net +// +// 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/. + +namespace Eigen { + +namespace internal { + + // FFTW uses non-const arguments + // so we must use ugly const_cast calls for all the args it uses + // + // This should be safe as long as + // 1. we use FFTW_ESTIMATE for all our planning + // see the FFTW docs section 4.3.2 "Planner Flags" + // 2. fftw_complex is compatible with std::complex + // This assumes std::complex<T> layout is array of size 2 with real,imag + template <typename T> + inline + T * fftw_cast(const T* p) + { + return const_cast<T*>( p); + } + + inline + fftw_complex * fftw_cast( const std::complex<double> * p) + { + return const_cast<fftw_complex*>( reinterpret_cast<const fftw_complex*>(p) ); + } + + inline + fftwf_complex * fftw_cast( const std::complex<float> * p) + { + return const_cast<fftwf_complex*>( reinterpret_cast<const fftwf_complex*>(p) ); + } + + inline + fftwl_complex * fftw_cast( const std::complex<long double> * p) + { + return const_cast<fftwl_complex*>( reinterpret_cast<const fftwl_complex*>(p) ); + } + + template <typename T> + struct fftw_plan {}; + + template <> + struct fftw_plan<float> + { + typedef float scalar_type; + typedef fftwf_complex complex_type; + fftwf_plan m_plan; + fftw_plan() :m_plan(NULL) {} + ~fftw_plan() {if (m_plan) fftwf_destroy_plan(m_plan);} + + inline + void fwd(complex_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftwf_plan_dft_1d(nfft,src,dst, FFTW_FORWARD, FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwf_execute_dft( m_plan, src,dst); + } + inline + void inv(complex_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftwf_plan_dft_1d(nfft,src,dst, FFTW_BACKWARD , FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwf_execute_dft( m_plan, src,dst); + } + inline + void fwd(complex_type * dst,scalar_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftwf_plan_dft_r2c_1d(nfft,src,dst,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwf_execute_dft_r2c( m_plan,src,dst); + } + inline + void inv(scalar_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) + m_plan = fftwf_plan_dft_c2r_1d(nfft,src,dst,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwf_execute_dft_c2r( m_plan, src,dst); + } + + inline + void fwd2( complex_type * dst,complex_type * src,int n0,int n1) { + if (m_plan==NULL) m_plan = fftwf_plan_dft_2d(n0,n1,src,dst,FFTW_FORWARD,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwf_execute_dft( m_plan, src,dst); + } + inline + void inv2( complex_type * dst,complex_type * src,int n0,int n1) { + if (m_plan==NULL) m_plan = fftwf_plan_dft_2d(n0,n1,src,dst,FFTW_BACKWARD,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwf_execute_dft( m_plan, src,dst); + } + + }; + template <> + struct fftw_plan<double> + { + typedef double scalar_type; + typedef fftw_complex complex_type; + ::fftw_plan m_plan; + fftw_plan() :m_plan(NULL) {} + ~fftw_plan() {if (m_plan) fftw_destroy_plan(m_plan);} + + inline + void fwd(complex_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftw_plan_dft_1d(nfft,src,dst, FFTW_FORWARD, FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftw_execute_dft( m_plan, src,dst); + } + inline + void inv(complex_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftw_plan_dft_1d(nfft,src,dst, FFTW_BACKWARD , FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftw_execute_dft( m_plan, src,dst); + } + inline + void fwd(complex_type * dst,scalar_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftw_plan_dft_r2c_1d(nfft,src,dst,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftw_execute_dft_r2c( m_plan,src,dst); + } + inline + void inv(scalar_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) + m_plan = fftw_plan_dft_c2r_1d(nfft,src,dst,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftw_execute_dft_c2r( m_plan, src,dst); + } + inline + void fwd2( complex_type * dst,complex_type * src,int n0,int n1) { + if (m_plan==NULL) m_plan = fftw_plan_dft_2d(n0,n1,src,dst,FFTW_FORWARD,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftw_execute_dft( m_plan, src,dst); + } + inline + void inv2( complex_type * dst,complex_type * src,int n0,int n1) { + if (m_plan==NULL) m_plan = fftw_plan_dft_2d(n0,n1,src,dst,FFTW_BACKWARD,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftw_execute_dft( m_plan, src,dst); + } + }; + template <> + struct fftw_plan<long double> + { + typedef long double scalar_type; + typedef fftwl_complex complex_type; + fftwl_plan m_plan; + fftw_plan() :m_plan(NULL) {} + ~fftw_plan() {if (m_plan) fftwl_destroy_plan(m_plan);} + + inline + void fwd(complex_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftwl_plan_dft_1d(nfft,src,dst, FFTW_FORWARD, FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwl_execute_dft( m_plan, src,dst); + } + inline + void inv(complex_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftwl_plan_dft_1d(nfft,src,dst, FFTW_BACKWARD , FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwl_execute_dft( m_plan, src,dst); + } + inline + void fwd(complex_type * dst,scalar_type * src,int nfft) { + if (m_plan==NULL) m_plan = fftwl_plan_dft_r2c_1d(nfft,src,dst,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwl_execute_dft_r2c( m_plan,src,dst); + } + inline + void inv(scalar_type * dst,complex_type * src,int nfft) { + if (m_plan==NULL) + m_plan = fftwl_plan_dft_c2r_1d(nfft,src,dst,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwl_execute_dft_c2r( m_plan, src,dst); + } + inline + void fwd2( complex_type * dst,complex_type * src,int n0,int n1) { + if (m_plan==NULL) m_plan = fftwl_plan_dft_2d(n0,n1,src,dst,FFTW_FORWARD,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwl_execute_dft( m_plan, src,dst); + } + inline + void inv2( complex_type * dst,complex_type * src,int n0,int n1) { + if (m_plan==NULL) m_plan = fftwl_plan_dft_2d(n0,n1,src,dst,FFTW_BACKWARD,FFTW_ESTIMATE|FFTW_PRESERVE_INPUT); + fftwl_execute_dft( m_plan, src,dst); + } + }; + + template <typename _Scalar> + struct fftw_impl + { + typedef _Scalar Scalar; + typedef std::complex<Scalar> Complex; + + inline + void clear() + { + m_plans.clear(); + } + + // complex-to-complex forward FFT + inline + void fwd( Complex * dst,const Complex *src,int nfft) + { + get_plan(nfft,false,dst,src).fwd(fftw_cast(dst), fftw_cast(src),nfft ); + } + + // real-to-complex forward FFT + inline + void fwd( Complex * dst,const Scalar * src,int nfft) + { + get_plan(nfft,false,dst,src).fwd(fftw_cast(dst), fftw_cast(src) ,nfft); + } + + // 2-d complex-to-complex + inline + void fwd2(Complex * dst, const Complex * src, int n0,int n1) + { + get_plan(n0,n1,false,dst,src).fwd2(fftw_cast(dst), fftw_cast(src) ,n0,n1); + } + + // inverse complex-to-complex + inline + void inv(Complex * dst,const Complex *src,int nfft) + { + get_plan(nfft,true,dst,src).inv(fftw_cast(dst), fftw_cast(src),nfft ); + } + + // half-complex to scalar + inline + void inv( Scalar * dst,const Complex * src,int nfft) + { + get_plan(nfft,true,dst,src).inv(fftw_cast(dst), fftw_cast(src),nfft ); + } + + // 2-d complex-to-complex + inline + void inv2(Complex * dst, const Complex * src, int n0,int n1) + { + get_plan(n0,n1,true,dst,src).inv2(fftw_cast(dst), fftw_cast(src) ,n0,n1); + } + + + protected: + typedef fftw_plan<Scalar> PlanData; + + typedef std::map<int64_t,PlanData> PlanMap; + + PlanMap m_plans; + + inline + PlanData & get_plan(int nfft,bool inverse,void * dst,const void * src) + { + bool inplace = (dst==src); + bool aligned = ( (reinterpret_cast<size_t>(src)&15) | (reinterpret_cast<size_t>(dst)&15) ) == 0; + int64_t key = ( (nfft<<3 ) | (inverse<<2) | (inplace<<1) | aligned ) << 1; + return m_plans[key]; + } + + inline + PlanData & get_plan(int n0,int n1,bool inverse,void * dst,const void * src) + { + bool inplace = (dst==src); + bool aligned = ( (reinterpret_cast<size_t>(src)&15) | (reinterpret_cast<size_t>(dst)&15) ) == 0; + int64_t key = ( ( (((int64_t)n0) << 30)|(n1<<3 ) | (inverse<<2) | (inplace<<1) | aligned ) << 1 ) + 1; + return m_plans[key]; + } + }; + +} // end namespace internal + +} // end namespace Eigen + +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/unsupported/Eigen/src/FFT/ei_kissfft_impl.h b/unsupported/Eigen/src/FFT/ei_kissfft_impl.h new file mode 100644 index 000000000..37f5af4c1 --- /dev/null +++ b/unsupported/Eigen/src/FFT/ei_kissfft_impl.h @@ -0,0 +1,418 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Mark Borgerding mark a borgerding net +// +// 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/. + +namespace Eigen { + +namespace internal { + + // This FFT implementation was derived from kissfft http:sourceforge.net/projects/kissfft + // Copyright 2003-2009 Mark Borgerding + +template <typename _Scalar> +struct kiss_cpx_fft +{ + typedef _Scalar Scalar; + typedef std::complex<Scalar> Complex; + std::vector<Complex> m_twiddles; + std::vector<int> m_stageRadix; + std::vector<int> m_stageRemainder; + std::vector<Complex> m_scratchBuf; + bool m_inverse; + + inline + void make_twiddles(int nfft,bool inverse) + { + m_inverse = inverse; + m_twiddles.resize(nfft); + Scalar phinc = (inverse?2:-2)* acos( (Scalar) -1) / nfft; + for (int i=0;i<nfft;++i) + m_twiddles[i] = exp( Complex(0,i*phinc) ); + } + + void factorize(int nfft) + { + //start factoring out 4's, then 2's, then 3,5,7,9,... + int n= nfft; + int p=4; + do { + while (n % p) { + switch (p) { + case 4: p = 2; break; + case 2: p = 3; break; + default: p += 2; break; + } + if (p*p>n) + p=n;// impossible to have a factor > sqrt(n) + } + n /= p; + m_stageRadix.push_back(p); + m_stageRemainder.push_back(n); + if ( p > 5 ) + m_scratchBuf.resize(p); // scratchbuf will be needed in bfly_generic + }while(n>1); + } + + template <typename _Src> + inline + void work( int stage,Complex * xout, const _Src * xin, size_t fstride,size_t in_stride) + { + int p = m_stageRadix[stage]; + int m = m_stageRemainder[stage]; + Complex * Fout_beg = xout; + Complex * Fout_end = xout + p*m; + + if (m>1) { + do{ + // recursive call: + // DFT of size m*p performed by doing + // p instances of smaller DFTs of size m, + // each one takes a decimated version of the input + work(stage+1, xout , xin, fstride*p,in_stride); + xin += fstride*in_stride; + }while( (xout += m) != Fout_end ); + }else{ + do{ + *xout = *xin; + xin += fstride*in_stride; + }while(++xout != Fout_end ); + } + xout=Fout_beg; + + // recombine the p smaller DFTs + switch (p) { + case 2: bfly2(xout,fstride,m); break; + case 3: bfly3(xout,fstride,m); break; + case 4: bfly4(xout,fstride,m); break; + case 5: bfly5(xout,fstride,m); break; + default: bfly_generic(xout,fstride,m,p); break; + } + } + + inline + void bfly2( Complex * Fout, const size_t fstride, int m) + { + for (int k=0;k<m;++k) { + Complex t = Fout[m+k] * m_twiddles[k*fstride]; + Fout[m+k] = Fout[k] - t; + Fout[k] += t; + } + } + + inline + void bfly4( Complex * Fout, const size_t fstride, const size_t m) + { + Complex scratch[6]; + int negative_if_inverse = m_inverse * -2 +1; + for (size_t k=0;k<m;++k) { + scratch[0] = Fout[k+m] * m_twiddles[k*fstride]; + scratch[1] = Fout[k+2*m] * m_twiddles[k*fstride*2]; + scratch[2] = Fout[k+3*m] * m_twiddles[k*fstride*3]; + scratch[5] = Fout[k] - scratch[1]; + + Fout[k] += scratch[1]; + scratch[3] = scratch[0] + scratch[2]; + scratch[4] = scratch[0] - scratch[2]; + scratch[4] = Complex( scratch[4].imag()*negative_if_inverse , -scratch[4].real()* negative_if_inverse ); + + Fout[k+2*m] = Fout[k] - scratch[3]; + Fout[k] += scratch[3]; + Fout[k+m] = scratch[5] + scratch[4]; + Fout[k+3*m] = scratch[5] - scratch[4]; + } + } + + inline + void bfly3( Complex * Fout, const size_t fstride, const size_t m) + { + size_t k=m; + const size_t m2 = 2*m; + Complex *tw1,*tw2; + Complex scratch[5]; + Complex epi3; + epi3 = m_twiddles[fstride*m]; + + tw1=tw2=&m_twiddles[0]; + + do{ + scratch[1]=Fout[m] * *tw1; + scratch[2]=Fout[m2] * *tw2; + + scratch[3]=scratch[1]+scratch[2]; + scratch[0]=scratch[1]-scratch[2]; + tw1 += fstride; + tw2 += fstride*2; + Fout[m] = Complex( Fout->real() - Scalar(.5)*scratch[3].real() , Fout->imag() - Scalar(.5)*scratch[3].imag() ); + scratch[0] *= epi3.imag(); + *Fout += scratch[3]; + Fout[m2] = Complex( Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() ); + Fout[m] += Complex( -scratch[0].imag(),scratch[0].real() ); + ++Fout; + }while(--k); + } + + inline + void bfly5( Complex * Fout, const size_t fstride, const size_t m) + { + Complex *Fout0,*Fout1,*Fout2,*Fout3,*Fout4; + size_t u; + Complex scratch[13]; + Complex * twiddles = &m_twiddles[0]; + Complex *tw; + Complex ya,yb; + ya = twiddles[fstride*m]; + yb = twiddles[fstride*2*m]; + + Fout0=Fout; + Fout1=Fout0+m; + Fout2=Fout0+2*m; + Fout3=Fout0+3*m; + Fout4=Fout0+4*m; + + tw=twiddles; + for ( u=0; u<m; ++u ) { + scratch[0] = *Fout0; + + scratch[1] = *Fout1 * tw[u*fstride]; + scratch[2] = *Fout2 * tw[2*u*fstride]; + scratch[3] = *Fout3 * tw[3*u*fstride]; + scratch[4] = *Fout4 * tw[4*u*fstride]; + + scratch[7] = scratch[1] + scratch[4]; + scratch[10] = scratch[1] - scratch[4]; + scratch[8] = scratch[2] + scratch[3]; + scratch[9] = scratch[2] - scratch[3]; + + *Fout0 += scratch[7]; + *Fout0 += scratch[8]; + + scratch[5] = scratch[0] + Complex( + (scratch[7].real()*ya.real() ) + (scratch[8].real() *yb.real() ), + (scratch[7].imag()*ya.real()) + (scratch[8].imag()*yb.real()) + ); + + scratch[6] = Complex( + (scratch[10].imag()*ya.imag()) + (scratch[9].imag()*yb.imag()), + -(scratch[10].real()*ya.imag()) - (scratch[9].real()*yb.imag()) + ); + + *Fout1 = scratch[5] - scratch[6]; + *Fout4 = scratch[5] + scratch[6]; + + scratch[11] = scratch[0] + + Complex( + (scratch[7].real()*yb.real()) + (scratch[8].real()*ya.real()), + (scratch[7].imag()*yb.real()) + (scratch[8].imag()*ya.real()) + ); + + scratch[12] = Complex( + -(scratch[10].imag()*yb.imag()) + (scratch[9].imag()*ya.imag()), + (scratch[10].real()*yb.imag()) - (scratch[9].real()*ya.imag()) + ); + + *Fout2=scratch[11]+scratch[12]; + *Fout3=scratch[11]-scratch[12]; + + ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4; + } + } + + /* perform the butterfly for one stage of a mixed radix FFT */ + inline + void bfly_generic( + Complex * Fout, + const size_t fstride, + int m, + int p + ) + { + int u,k,q1,q; + Complex * twiddles = &m_twiddles[0]; + Complex t; + int Norig = static_cast<int>(m_twiddles.size()); + Complex * scratchbuf = &m_scratchBuf[0]; + + for ( u=0; u<m; ++u ) { + k=u; + for ( q1=0 ; q1<p ; ++q1 ) { + scratchbuf[q1] = Fout[ k ]; + k += m; + } + + k=u; + for ( q1=0 ; q1<p ; ++q1 ) { + int twidx=0; + Fout[ k ] = scratchbuf[0]; + for (q=1;q<p;++q ) { + twidx += static_cast<int>(fstride) * k; + if (twidx>=Norig) twidx-=Norig; + t=scratchbuf[q] * twiddles[twidx]; + Fout[ k ] += t; + } + k += m; + } + } + } +}; + +template <typename _Scalar> +struct kissfft_impl +{ + typedef _Scalar Scalar; + typedef std::complex<Scalar> Complex; + + void clear() + { + m_plans.clear(); + m_realTwiddles.clear(); + } + + inline + void fwd( Complex * dst,const Complex *src,int nfft) + { + get_plan(nfft,false).work(0, dst, src, 1,1); + } + + inline + void fwd2( Complex * dst,const Complex *src,int n0,int n1) + { + EIGEN_UNUSED_VARIABLE(dst); + EIGEN_UNUSED_VARIABLE(src); + EIGEN_UNUSED_VARIABLE(n0); + EIGEN_UNUSED_VARIABLE(n1); + } + + inline + void inv2( Complex * dst,const Complex *src,int n0,int n1) + { + EIGEN_UNUSED_VARIABLE(dst); + EIGEN_UNUSED_VARIABLE(src); + EIGEN_UNUSED_VARIABLE(n0); + EIGEN_UNUSED_VARIABLE(n1); + } + + // real-to-complex forward FFT + // perform two FFTs of src even and src odd + // then twiddle to recombine them into the half-spectrum format + // then fill in the conjugate symmetric half + inline + void fwd( Complex * dst,const Scalar * src,int nfft) + { + if ( nfft&3 ) { + // use generic mode for odd + m_tmpBuf1.resize(nfft); + get_plan(nfft,false).work(0, &m_tmpBuf1[0], src, 1,1); + std::copy(m_tmpBuf1.begin(),m_tmpBuf1.begin()+(nfft>>1)+1,dst ); + }else{ + int ncfft = nfft>>1; + int ncfft2 = nfft>>2; + Complex * rtw = real_twiddles(ncfft2); + + // use optimized mode for even real + fwd( dst, reinterpret_cast<const Complex*> (src), ncfft); + Complex dc = dst[0].real() + dst[0].imag(); + Complex nyquist = dst[0].real() - dst[0].imag(); + int k; + for ( k=1;k <= ncfft2 ; ++k ) { + Complex fpk = dst[k]; + Complex fpnk = conj(dst[ncfft-k]); + Complex f1k = fpk + fpnk; + Complex f2k = fpk - fpnk; + Complex tw= f2k * rtw[k-1]; + dst[k] = (f1k + tw) * Scalar(.5); + dst[ncfft-k] = conj(f1k -tw)*Scalar(.5); + } + dst[0] = dc; + dst[ncfft] = nyquist; + } + } + + // inverse complex-to-complex + inline + void inv(Complex * dst,const Complex *src,int nfft) + { + get_plan(nfft,true).work(0, dst, src, 1,1); + } + + // half-complex to scalar + inline + void inv( Scalar * dst,const Complex * src,int nfft) + { + if (nfft&3) { + m_tmpBuf1.resize(nfft); + m_tmpBuf2.resize(nfft); + std::copy(src,src+(nfft>>1)+1,m_tmpBuf1.begin() ); + for (int k=1;k<(nfft>>1)+1;++k) + m_tmpBuf1[nfft-k] = conj(m_tmpBuf1[k]); + inv(&m_tmpBuf2[0],&m_tmpBuf1[0],nfft); + for (int k=0;k<nfft;++k) + dst[k] = m_tmpBuf2[k].real(); + }else{ + // optimized version for multiple of 4 + int ncfft = nfft>>1; + int ncfft2 = nfft>>2; + Complex * rtw = real_twiddles(ncfft2); + m_tmpBuf1.resize(ncfft); + m_tmpBuf1[0] = Complex( src[0].real() + src[ncfft].real(), src[0].real() - src[ncfft].real() ); + for (int k = 1; k <= ncfft / 2; ++k) { + Complex fk = src[k]; + Complex fnkc = conj(src[ncfft-k]); + Complex fek = fk + fnkc; + Complex tmp = fk - fnkc; + Complex fok = tmp * conj(rtw[k-1]); + m_tmpBuf1[k] = fek + fok; + m_tmpBuf1[ncfft-k] = conj(fek - fok); + } + get_plan(ncfft,true).work(0, reinterpret_cast<Complex*>(dst), &m_tmpBuf1[0], 1,1); + } + } + + protected: + typedef kiss_cpx_fft<Scalar> PlanData; + typedef std::map<int,PlanData> PlanMap; + + PlanMap m_plans; + std::map<int, std::vector<Complex> > m_realTwiddles; + std::vector<Complex> m_tmpBuf1; + std::vector<Complex> m_tmpBuf2; + + inline + int PlanKey(int nfft, bool isinverse) const { return (nfft<<1) | int(isinverse); } + + inline + PlanData & get_plan(int nfft, bool inverse) + { + // TODO look for PlanKey(nfft, ! inverse) and conjugate the twiddles + PlanData & pd = m_plans[ PlanKey(nfft,inverse) ]; + if ( pd.m_twiddles.size() == 0 ) { + pd.make_twiddles(nfft,inverse); + pd.factorize(nfft); + } + return pd; + } + + inline + Complex * real_twiddles(int ncfft2) + { + std::vector<Complex> & twidref = m_realTwiddles[ncfft2];// creates new if not there + if ( (int)twidref.size() != ncfft2 ) { + twidref.resize(ncfft2); + int ncfft= ncfft2<<1; + Scalar pi = acos( Scalar(-1) ); + for (int k=1;k<=ncfft2;++k) + twidref[k-1] = exp( Complex(0,-pi * (Scalar(k) / ncfft + Scalar(.5)) ) ); + } + return &twidref[0]; + } +}; + +} // end namespace internal + +} // end namespace Eigen + +/* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/unsupported/Eigen/src/IterativeSolvers/CMakeLists.txt b/unsupported/Eigen/src/IterativeSolvers/CMakeLists.txt new file mode 100644 index 000000000..7986afc5e --- /dev/null +++ b/unsupported/Eigen/src/IterativeSolvers/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_IterativeSolvers_SRCS "*.h") + +INSTALL(FILES + ${Eigen_IterativeSolvers_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/IterativeSolvers COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h b/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h new file mode 100644 index 000000000..b83bf7aef --- /dev/null +++ b/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h @@ -0,0 +1,189 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +/* NOTE The functions of this file have been adapted from the GMM++ library */ + +//======================================================================== +// +// Copyright (C) 2002-2007 Yves Renard +// +// This file is a part of GETFEM++ +// +// Getfem++ is free software; you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as +// published by the Free Software Foundation; version 2.1 of the License. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// You should have received a copy of the GNU Lesser General Public +// License along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, +// USA. +// +//======================================================================== + +#include "../../../../Eigen/src/Core/util/NonMPL2.h" + +#ifndef EIGEN_CONSTRAINEDCG_H +#define EIGEN_CONSTRAINEDCG_H + +#include <Eigen/Core> + +namespace Eigen { + +namespace internal { + +/** \ingroup IterativeSolvers_Module + * Compute the pseudo inverse of the non-square matrix C such that + * \f$ CINV = (C * C^T)^{-1} * C \f$ based on a conjugate gradient method. + * + * This function is internally used by constrained_cg. + */ +template <typename CMatrix, typename CINVMatrix> +void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV) +{ + // optimisable : copie de la ligne, precalcul de C * trans(C). + typedef typename CMatrix::Scalar Scalar; + typedef typename CMatrix::Index Index; + // FIXME use sparse vectors ? + typedef Matrix<Scalar,Dynamic,1> TmpVec; + + Index rows = C.rows(), cols = C.cols(); + + TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows); + Scalar rho, rho_1, alpha; + d.setZero(); + + CINV.startFill(); // FIXME estimate the number of non-zeros + for (Index i = 0; i < rows; ++i) + { + d[i] = 1.0; + rho = 1.0; + e.setZero(); + r = d; + p = d; + + while (rho >= 1e-38) + { /* conjugate gradient to compute e */ + /* which is the i-th row of inv(C * trans(C)) */ + l = C.transpose() * p; + q = C * l; + alpha = rho / p.dot(q); + e += alpha * p; + r += -alpha * q; + rho_1 = rho; + rho = r.dot(r); + p = (rho/rho_1) * p + r; + } + + l = C.transpose() * e; // l is the i-th row of CINV + // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse + for (Index j=0; j<l.size(); ++j) + if (l[j]<1e-15) + CINV.fill(i,j) = l[j]; + + d[i] = 0.0; + } + CINV.endFill(); +} + + + +/** \ingroup IterativeSolvers_Module + * Constrained conjugate gradient + * + * Computes the minimum of \f$ 1/2((Ax).x) - bx \f$ under the contraint \f$ Cx \le f \f$ + */ +template<typename TMatrix, typename CMatrix, + typename VectorX, typename VectorB, typename VectorF> +void constrained_cg(const TMatrix& A, const CMatrix& C, VectorX& x, + const VectorB& b, const VectorF& f, IterationController &iter) +{ + typedef typename TMatrix::Scalar Scalar; + typedef typename TMatrix::Index Index; + typedef Matrix<Scalar,Dynamic,1> TmpVec; + + Scalar rho = 1.0, rho_1, lambda, gamma; + Index xSize = x.size(); + TmpVec p(xSize), q(xSize), q2(xSize), + r(xSize), old_z(xSize), z(xSize), + memox(xSize); + std::vector<bool> satured(C.rows()); + p.setZero(); + iter.setRhsNorm(sqrt(b.dot(b))); // gael vect_sp(PS, b, b) + if (iter.rhsNorm() == 0.0) iter.setRhsNorm(1.0); + + SparseMatrix<Scalar,RowMajor> CINV(C.rows(), C.cols()); + pseudo_inverse(C, CINV); + + while(true) + { + // computation of residual + old_z = z; + memox = x; + r = b; + r += A * -x; + z = r; + bool transition = false; + for (Index i = 0; i < C.rows(); ++i) + { + Scalar al = C.row(i).dot(x) - f.coeff(i); + if (al >= -1.0E-15) + { + if (!satured[i]) + { + satured[i] = true; + transition = true; + } + Scalar bb = CINV.row(i).dot(z); + if (bb > 0.0) + // FIXME: we should allow that: z += -bb * C.row(i); + for (typename CMatrix::InnerIterator it(C,i); it; ++it) + z.coeffRef(it.index()) -= bb*it.value(); + } + else + satured[i] = false; + } + + // descent direction + rho_1 = rho; + rho = r.dot(z); + + if (iter.finished(rho)) break; + + if (iter.noiseLevel() > 0 && transition) std::cerr << "CCG: transition\n"; + if (transition || iter.first()) gamma = 0.0; + else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1); + p = z + gamma*p; + + ++iter; + // one dimensionnal optimization + q = A * p; + lambda = rho / q.dot(p); + for (Index i = 0; i < C.rows(); ++i) + { + if (!satured[i]) + { + Scalar bb = C.row(i).dot(p) - f[i]; + if (bb > 0.0) + lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb); + } + } + x += lambda * p; + memox -= x; + } +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_CONSTRAINEDCG_H diff --git a/unsupported/Eigen/src/IterativeSolvers/GMRES.h b/unsupported/Eigen/src/IterativeSolvers/GMRES.h new file mode 100644 index 000000000..34e67db82 --- /dev/null +++ b/unsupported/Eigen/src/IterativeSolvers/GMRES.h @@ -0,0 +1,379 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr> +// Copyright (C) 2012 Kolja Brix <brix@igpm.rwth-aaachen.de> +// +// 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_GMRES_H +#define EIGEN_GMRES_H + +namespace Eigen { + +namespace internal { + +/** + * Generalized Minimal Residual Algorithm based on the + * Arnoldi algorithm implemented with Householder reflections. + * + * Parameters: + * \param mat matrix of linear system of equations + * \param Rhs right hand side vector of linear system of equations + * \param x on input: initial guess, on output: solution + * \param precond preconditioner used + * \param iters on input: maximum number of iterations to perform + * on output: number of iterations performed + * \param restart number of iterations for a restart + * \param tol_error on input: residual tolerance + * on output: residuum achieved + * + * \sa IterativeMethods::bicgstab() + * + * + * For references, please see: + * + * Saad, Y. and Schultz, M. H. + * GMRES: A Generalized Minimal Residual Algorithm for Solving Nonsymmetric Linear Systems. + * SIAM J.Sci.Stat.Comp. 7, 1986, pp. 856 - 869. + * + * Saad, Y. + * Iterative Methods for Sparse Linear Systems. + * Society for Industrial and Applied Mathematics, Philadelphia, 2003. + * + * Walker, H. F. + * Implementations of the GMRES method. + * Comput.Phys.Comm. 53, 1989, pp. 311 - 320. + * + * Walker, H. F. + * Implementation of the GMRES Method using Householder Transformations. + * SIAM J.Sci.Stat.Comp. 9, 1988, pp. 152 - 163. + * + */ +template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner> +bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Preconditioner & precond, + int &iters, const int &restart, typename Dest::RealScalar & tol_error) { + + using std::sqrt; + using std::abs; + + typedef typename Dest::RealScalar RealScalar; + typedef typename Dest::Scalar Scalar; + typedef Matrix < RealScalar, Dynamic, 1 > RealVectorType; + typedef Matrix < Scalar, Dynamic, 1 > VectorType; + typedef Matrix < Scalar, Dynamic, Dynamic > FMatrixType; + + RealScalar tol = tol_error; + const int maxIters = iters; + iters = 0; + + const int m = mat.rows(); + + VectorType p0 = rhs - mat*x; + VectorType r0 = precond.solve(p0); +// RealScalar r0_sqnorm = r0.squaredNorm(); + + VectorType w = VectorType::Zero(restart + 1); + + FMatrixType H = FMatrixType::Zero(m, restart + 1); + VectorType tau = VectorType::Zero(restart + 1); + std::vector < JacobiRotation < Scalar > > G(restart); + + // generate first Householder vector + VectorType e; + RealScalar beta; + r0.makeHouseholder(e, tau.coeffRef(0), beta); + w(0)=(Scalar) beta; + H.bottomLeftCorner(m - 1, 1) = e; + + for (int k = 1; k <= restart; ++k) { + + ++iters; + + VectorType v = VectorType::Unit(m, k - 1), workspace(m); + + // apply Householder reflections H_{1} ... H_{k-1} to v + for (int i = k - 1; i >= 0; --i) { + v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); + } + + // apply matrix M to v: v = mat * v; + VectorType t=mat*v; + v=precond.solve(t); + + // apply Householder reflections H_{k-1} ... H_{1} to v + for (int i = 0; i < k; ++i) { + v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); + } + + if (v.tail(m - k).norm() != 0.0) { + + if (k <= restart) { + + // generate new Householder vector + VectorType e(m - k - 1); + RealScalar beta; + v.tail(m - k).makeHouseholder(e, tau.coeffRef(k), beta); + H.col(k).tail(m - k - 1) = e; + + // apply Householder reflection H_{k} to v + v.tail(m - k).applyHouseholderOnTheLeft(H.col(k).tail(m - k - 1), tau.coeffRef(k), workspace.data()); + + } + } + + if (k > 1) { + for (int i = 0; i < k - 1; ++i) { + // apply old Givens rotations to v + v.applyOnTheLeft(i, i + 1, G[i].adjoint()); + } + } + + if (k<m && v(k) != (Scalar) 0) { + // determine next Givens rotation + G[k - 1].makeGivens(v(k - 1), v(k)); + + // apply Givens rotation to v and w + v.applyOnTheLeft(k - 1, k, G[k - 1].adjoint()); + w.applyOnTheLeft(k - 1, k, G[k - 1].adjoint()); + + } + + // insert coefficients into upper matrix triangle + H.col(k - 1).head(k) = v.head(k); + + bool stop=(k==m || abs(w(k)) < tol || iters == maxIters); + + if (stop || k == restart) { + + // solve upper triangular system + VectorType y = w.head(k); + H.topLeftCorner(k, k).template triangularView < Eigen::Upper > ().solveInPlace(y); + + // use Horner-like scheme to calculate solution vector + VectorType x_new = y(k - 1) * VectorType::Unit(m, k - 1); + + // apply Householder reflection H_{k} to x_new + x_new.tail(m - k + 1).applyHouseholderOnTheLeft(H.col(k - 1).tail(m - k), tau.coeffRef(k - 1), workspace.data()); + + for (int i = k - 2; i >= 0; --i) { + x_new += y(i) * VectorType::Unit(m, i); + // apply Householder reflection H_{i} to x_new + x_new.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); + } + + x += x_new; + + if (stop) { + return true; + } else { + k=0; + + // reset data for a restart r0 = rhs - mat * x; + VectorType p0=mat*x; + VectorType p1=precond.solve(p0); + r0 = rhs - p1; +// r0_sqnorm = r0.squaredNorm(); + w = VectorType::Zero(restart + 1); + H = FMatrixType::Zero(m, restart + 1); + tau = VectorType::Zero(restart + 1); + + // generate first Householder vector + RealScalar beta; + r0.makeHouseholder(e, tau.coeffRef(0), beta); + w(0)=(Scalar) beta; + H.bottomLeftCorner(m - 1, 1) = e; + + } + + } + + + + } + + return false; + +} + +} + +template< typename _MatrixType, + typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> > +class GMRES; + +namespace internal { + +template< typename _MatrixType, typename _Preconditioner> +struct traits<GMRES<_MatrixType,_Preconditioner> > +{ + typedef _MatrixType MatrixType; + typedef _Preconditioner Preconditioner; +}; + +} + +/** \ingroup IterativeLinearSolvers_Module + * \brief A GMRES solver for sparse square problems + * + * This class allows to solve for A.x = b sparse linear problems using a generalized minimal + * residual method. The vectors x and b can be either dense or sparse. + * + * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix. + * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner + * + * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations() + * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations + * and NumTraits<Scalar>::epsilon() for the tolerance. + * + * This class can be used as the direct solver classes. Here is a typical usage example: + * \code + * int n = 10000; + * VectorXd x(n), b(n); + * SparseMatrix<double> A(n,n); + * // fill A and b + * GMRES<SparseMatrix<double> > solver(A); + * x = solver.solve(b); + * std::cout << "#iterations: " << solver.iterations() << std::endl; + * std::cout << "estimated error: " << solver.error() << std::endl; + * // update b, and solve again + * x = solver.solve(b); + * \endcode + * + * By default the iterations start with x=0 as an initial guess of the solution. + * One can control the start using the solveWithGuess() method. Here is a step by + * step execution example starting with a random guess and printing the evolution + * of the estimated error: + * * \code + * x = VectorXd::Random(n); + * solver.setMaxIterations(1); + * int i = 0; + * do { + * x = solver.solveWithGuess(b,x); + * std::cout << i << " : " << solver.error() << std::endl; + * ++i; + * } while (solver.info()!=Success && i<100); + * \endcode + * Note that such a step by step excution is slightly slower. + * + * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner + */ +template< typename _MatrixType, typename _Preconditioner> +class GMRES : public IterativeSolverBase<GMRES<_MatrixType,_Preconditioner> > +{ + typedef IterativeSolverBase<GMRES> Base; + using Base::mp_matrix; + using Base::m_error; + using Base::m_iterations; + using Base::m_info; + using Base::m_isInitialized; + +private: + int m_restart; + +public: + typedef _MatrixType MatrixType; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::Index Index; + typedef typename MatrixType::RealScalar RealScalar; + typedef _Preconditioner Preconditioner; + +public: + + /** Default constructor. */ + GMRES() : Base(), m_restart(30) {} + + /** Initialize the solver with matrix \a A for further \c Ax=b solving. + * + * This constructor is a shortcut for the default constructor followed + * by a call to compute(). + * + * \warning this class stores a reference to the matrix A as well as some + * precomputed values that depend on it. Therefore, if \a A is changed + * this class becomes invalid. Call compute() to update it with the new + * matrix A, or modify a copy of A. + */ + GMRES(const MatrixType& A) : Base(A), m_restart(30) {} + + ~GMRES() {} + + /** Get the number of iterations after that a restart is performed. + */ + int get_restart() { return m_restart; } + + /** Set the number of iterations after that a restart is performed. + * \param restart number of iterations for a restarti, default is 30. + */ + void set_restart(const int restart) { m_restart=restart; } + + /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A + * \a x0 as an initial solution. + * + * \sa compute() + */ + template<typename Rhs,typename Guess> + inline const internal::solve_retval_with_guess<GMRES, Rhs, Guess> + solveWithGuess(const MatrixBase<Rhs>& b, const Guess& x0) const + { + eigen_assert(m_isInitialized && "GMRES is not initialized."); + eigen_assert(Base::rows()==b.rows() + && "GMRES::solve(): invalid number of rows of the right hand side matrix b"); + return internal::solve_retval_with_guess + <GMRES, Rhs, Guess>(*this, b.derived(), x0); + } + + /** \internal */ + template<typename Rhs,typename Dest> + void _solveWithGuess(const Rhs& b, Dest& x) const + { + bool failed = false; + for(int j=0; j<b.cols(); ++j) + { + m_iterations = Base::maxIterations(); + m_error = Base::m_tolerance; + + typename Dest::ColXpr xj(x,j); + if(!internal::gmres(*mp_matrix, b.col(j), xj, Base::m_preconditioner, m_iterations, m_restart, m_error)) + failed = true; + } + m_info = failed ? NumericalIssue + : m_error <= Base::m_tolerance ? Success + : NoConvergence; + m_isInitialized = true; + } + + /** \internal */ + template<typename Rhs,typename Dest> + void _solve(const Rhs& b, Dest& x) const + { + x.setZero(); + _solveWithGuess(b,x); + } + +protected: + +}; + + +namespace internal { + + template<typename _MatrixType, typename _Preconditioner, typename Rhs> +struct solve_retval<GMRES<_MatrixType, _Preconditioner>, Rhs> + : solve_retval_base<GMRES<_MatrixType, _Preconditioner>, Rhs> +{ + typedef GMRES<_MatrixType, _Preconditioner> Dec; + EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs) + + template<typename Dest> void evalTo(Dest& dst) const + { + dec()._solve(rhs(),dst); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_GMRES_H diff --git a/unsupported/Eigen/src/IterativeSolvers/IncompleteLU.h b/unsupported/Eigen/src/IterativeSolvers/IncompleteLU.h new file mode 100644 index 000000000..67e780181 --- /dev/null +++ b/unsupported/Eigen/src/IterativeSolvers/IncompleteLU.h @@ -0,0 +1,113 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_INCOMPLETE_LU_H +#define EIGEN_INCOMPLETE_LU_H + +namespace Eigen { + +template <typename _Scalar> +class IncompleteLU +{ + typedef _Scalar Scalar; + typedef Matrix<Scalar,Dynamic,1> Vector; + typedef typename Vector::Index Index; + typedef SparseMatrix<Scalar,RowMajor> FactorType; + + public: + typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType; + + IncompleteLU() : m_isInitialized(false) {} + + template<typename MatrixType> + IncompleteLU(const MatrixType& mat) : m_isInitialized(false) + { + compute(mat); + } + + Index rows() const { return m_lu.rows(); } + Index cols() const { return m_lu.cols(); } + + template<typename MatrixType> + IncompleteLU& compute(const MatrixType& mat) + { + m_lu = mat; + int size = mat.cols(); + Vector diag(size); + for(int i=0; i<size; ++i) + { + typename FactorType::InnerIterator k_it(m_lu,i); + for(; k_it && k_it.index()<i; ++k_it) + { + int k = k_it.index(); + k_it.valueRef() /= diag(k); + + typename FactorType::InnerIterator j_it(k_it); + typename FactorType::InnerIterator kj_it(m_lu, k); + while(kj_it && kj_it.index()<=k) ++kj_it; + for(++j_it; j_it; ) + { + if(kj_it.index()==j_it.index()) + { + j_it.valueRef() -= k_it.value() * kj_it.value(); + ++j_it; + ++kj_it; + } + else if(kj_it.index()<j_it.index()) ++kj_it; + else ++j_it; + } + } + if(k_it && k_it.index()==i) diag(i) = k_it.value(); + else diag(i) = 1; + } + m_isInitialized = true; + return *this; + } + + template<typename Rhs, typename Dest> + void _solve(const Rhs& b, Dest& x) const + { + x = m_lu.template triangularView<UnitLower>().solve(b); + x = m_lu.template triangularView<Upper>().solve(x); + } + + template<typename Rhs> inline const internal::solve_retval<IncompleteLU, Rhs> + solve(const MatrixBase<Rhs>& b) const + { + eigen_assert(m_isInitialized && "IncompleteLU is not initialized."); + eigen_assert(cols()==b.rows() + && "IncompleteLU::solve(): invalid number of rows of the right hand side matrix b"); + return internal::solve_retval<IncompleteLU, Rhs>(*this, b.derived()); + } + + protected: + FactorType m_lu; + bool m_isInitialized; +}; + +namespace internal { + +template<typename _MatrixType, typename Rhs> +struct solve_retval<IncompleteLU<_MatrixType>, Rhs> + : solve_retval_base<IncompleteLU<_MatrixType>, Rhs> +{ + typedef IncompleteLU<_MatrixType> Dec; + EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs) + + template<typename Dest> void evalTo(Dest& dst) const + { + dec()._solve(rhs(),dst); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_INCOMPLETE_LU_H diff --git a/unsupported/Eigen/src/IterativeSolvers/IterationController.h b/unsupported/Eigen/src/IterativeSolvers/IterationController.h new file mode 100644 index 000000000..aaf46d544 --- /dev/null +++ b/unsupported/Eigen/src/IterativeSolvers/IterationController.h @@ -0,0 +1,157 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +/* NOTE The class IterationController has been adapted from the iteration + * class of the GMM++ and ITL libraries. + */ + +//======================================================================= +// Copyright (C) 1997-2001 +// Authors: Andrew Lumsdaine <lums@osl.iu.edu> +// Lie-Quan Lee <llee@osl.iu.edu> +// +// This file is part of the Iterative Template Library +// +// You should have received a copy of the License Agreement for the +// Iterative Template Library along with the software; see the +// file LICENSE. +// +// Permission to modify the code and to distribute modified code is +// granted, provided the text of this NOTICE is retained, a notice that +// the code was modified is included with the above COPYRIGHT NOTICE and +// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE +// file is distributed with the modified code. +// +// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED. +// By way of example, but not limitation, Licensor MAKES NO +// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY +// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS +// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS +// OR OTHER RIGHTS. +//======================================================================= + +//======================================================================== +// +// Copyright (C) 2002-2007 Yves Renard +// +// This file is a part of GETFEM++ +// +// Getfem++ is free software; you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as +// published by the Free Software Foundation; version 2.1 of the License. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// You should have received a copy of the GNU Lesser General Public +// License along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, +// USA. +// +//======================================================================== + +#include "../../../../Eigen/src/Core/util/NonMPL2.h" + +#ifndef EIGEN_ITERATION_CONTROLLER_H +#define EIGEN_ITERATION_CONTROLLER_H + +namespace Eigen { + +/** \ingroup IterativeSolvers_Module + * \class IterationController + * + * \brief Controls the iterations of the iterative solvers + * + * This class has been adapted from the iteration class of GMM++ and ITL libraries. + * + */ +class IterationController +{ + protected : + double m_rhsn; ///< Right hand side norm + size_t m_maxiter; ///< Max. number of iterations + int m_noise; ///< if noise > 0 iterations are printed + double m_resmax; ///< maximum residual + double m_resminreach, m_resadd; + size_t m_nit; ///< iteration number + double m_res; ///< last computed residual + bool m_written; + void (*m_callback)(const IterationController&); + public : + + void init() + { + m_nit = 0; m_res = 0.0; m_written = false; + m_resminreach = 1E50; m_resadd = 0.0; + m_callback = 0; + } + + IterationController(double r = 1.0E-8, int noi = 0, size_t mit = size_t(-1)) + : m_rhsn(1.0), m_maxiter(mit), m_noise(noi), m_resmax(r) { init(); } + + void operator ++(int) { m_nit++; m_written = false; m_resadd += m_res; } + void operator ++() { (*this)++; } + + bool first() { return m_nit == 0; } + + /* get/set the "noisyness" (verbosity) of the solvers */ + int noiseLevel() const { return m_noise; } + void setNoiseLevel(int n) { m_noise = n; } + void reduceNoiseLevel() { if (m_noise > 0) m_noise--; } + + double maxResidual() const { return m_resmax; } + void setMaxResidual(double r) { m_resmax = r; } + + double residual() const { return m_res; } + + /* change the user-definable callback, called after each iteration */ + void setCallback(void (*t)(const IterationController&)) + { + m_callback = t; + } + + size_t iteration() const { return m_nit; } + void setIteration(size_t i) { m_nit = i; } + + size_t maxIterarions() const { return m_maxiter; } + void setMaxIterations(size_t i) { m_maxiter = i; } + + double rhsNorm() const { return m_rhsn; } + void setRhsNorm(double r) { m_rhsn = r; } + + bool converged() const { return m_res <= m_rhsn * m_resmax; } + bool converged(double nr) + { + m_res = internal::abs(nr); + m_resminreach = (std::min)(m_resminreach, m_res); + return converged(); + } + template<typename VectorType> bool converged(const VectorType &v) + { return converged(v.squaredNorm()); } + + bool finished(double nr) + { + if (m_callback) m_callback(*this); + if (m_noise > 0 && !m_written) + { + converged(nr); + m_written = true; + } + return (m_nit >= m_maxiter || converged(nr)); + } + template <typename VectorType> + bool finished(const MatrixBase<VectorType> &v) + { return finished(double(v.squaredNorm())); } + +}; + +} // end namespace Eigen + +#endif // EIGEN_ITERATION_CONTROLLER_H diff --git a/unsupported/Eigen/src/IterativeSolvers/Scaling.h b/unsupported/Eigen/src/IterativeSolvers/Scaling.h new file mode 100644 index 000000000..fdef0aca3 --- /dev/null +++ b/unsupported/Eigen/src/IterativeSolvers/Scaling.h @@ -0,0 +1,185 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Desire NUENTSA WAKAM <desire.nuentsa_wakam@inria.fr +// +// 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_SCALING_H +#define EIGEN_SCALING_H +/** + * \ingroup IterativeSolvers_Module + * \brief iterative scaling algorithm to equilibrate rows and column norms in matrices + * + * This class can be used as a preprocessing tool to accelerate the convergence of iterative methods + * + * This feature is useful to limit the pivoting amount during LU/ILU factorization + * The scaling strategy as presented here preserves the symmetry of the problem + * NOTE It is assumed that the matrix does not have empty row or column, + * + * Example with key steps + * \code + * VectorXd x(n), b(n); + * SparseMatrix<double> A; + * // fill A and b; + * Scaling<SparseMatrix<double> > scal; + * // Compute the left and right scaling vectors. The matrix is equilibrated at output + * scal.computeRef(A); + * // Scale the right hand side + * b = scal.LeftScaling().cwiseProduct(b); + * // Now, solve the equilibrated linear system with any available solver + * + * // Scale back the computed solution + * x = scal.RightScaling().cwiseProduct(x); + * \endcode + * + * \tparam _MatrixType the type of the matrix. It should be a real square sparsematrix + * + * References : D. Ruiz and B. Ucar, A Symmetry Preserving Algorithm for Matrix Scaling, INRIA Research report RR-7552 + * + * \sa \ref IncompleteLUT + */ +using std::abs; +using namespace Eigen; +template<typename _MatrixType> +class Scaling +{ + public: + typedef _MatrixType MatrixType; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::Index Index; + + public: + Scaling() { init(); } + + Scaling(const MatrixType& matrix) + { + init(); + compute(matrix); + } + + ~Scaling() { } + + /** + * Compute the left and right diagonal matrices to scale the input matrix @p mat + * + * FIXME This algorithm will be modified such that the diagonal elements are permuted on the diagonal. + * + * \sa LeftScaling() RightScaling() + */ + void compute (const MatrixType& mat) + { + int m = mat.rows(); + int n = mat.cols(); + assert((m>0 && m == n) && "Please give a non - empty matrix"); + m_left.resize(m); + m_right.resize(n); + m_left.setOnes(); + m_right.setOnes(); + m_matrix = mat; + VectorXd Dr, Dc, DrRes, DcRes; // Temporary Left and right scaling vectors + Dr.resize(m); Dc.resize(n); + DrRes.resize(m); DcRes.resize(n); + double EpsRow = 1.0, EpsCol = 1.0; + int its = 0; + do + { // Iterate until the infinite norm of each row and column is approximately 1 + // Get the maximum value in each row and column + Dr.setZero(); Dc.setZero(); + for (int k=0; k<m_matrix.outerSize(); ++k) + { + for (typename MatrixType::InnerIterator it(m_matrix, k); it; ++it) + { + if ( Dr(it.row()) < abs(it.value()) ) + Dr(it.row()) = abs(it.value()); + + if ( Dc(it.col()) < abs(it.value()) ) + Dc(it.col()) = abs(it.value()); + } + } + for (int i = 0; i < m; ++i) + { + Dr(i) = std::sqrt(Dr(i)); + Dc(i) = std::sqrt(Dc(i)); + } + // Save the scaling factors + for (int i = 0; i < m; ++i) + { + m_left(i) /= Dr(i); + m_right(i) /= Dc(i); + } + // Scale the rows and the columns of the matrix + DrRes.setZero(); DcRes.setZero(); + for (int k=0; k<m_matrix.outerSize(); ++k) + { + for (typename MatrixType::InnerIterator it(m_matrix, k); it; ++it) + { + it.valueRef() = it.value()/( Dr(it.row()) * Dc(it.col()) ); + // Accumulate the norms of the row and column vectors + if ( DrRes(it.row()) < abs(it.value()) ) + DrRes(it.row()) = abs(it.value()); + + if ( DcRes(it.col()) < abs(it.value()) ) + DcRes(it.col()) = abs(it.value()); + } + } + DrRes.array() = (1-DrRes.array()).abs(); + EpsRow = DrRes.maxCoeff(); + DcRes.array() = (1-DcRes.array()).abs(); + EpsCol = DcRes.maxCoeff(); + its++; + }while ( (EpsRow >m_tol || EpsCol > m_tol) && (its < m_maxits) ); + m_isInitialized = true; + } + /** Compute the left and right vectors to scale the vectors + * the input matrix is scaled with the computed vectors at output + * + * \sa compute() + */ + void computeRef (MatrixType& mat) + { + compute (mat); + mat = m_matrix; + } + /** Get the vector to scale the rows of the matrix + */ + VectorXd& LeftScaling() + { + return m_left; + } + + /** Get the vector to scale the columns of the matrix + */ + VectorXd& RightScaling() + { + return m_right; + } + + /** Set the tolerance for the convergence of the iterative scaling algorithm + */ + void setTolerance(double tol) + { + m_tol = tol; + } + + protected: + + void init() + { + m_tol = 1e-10; + m_maxits = 5; + m_isInitialized = false; + } + + MatrixType m_matrix; + mutable ComputationInfo m_info; + bool m_isInitialized; + VectorXd m_left; // Left scaling vector + VectorXd m_right; // m_right scaling vector + double m_tol; + int m_maxits; // Maximum number of iterations allowed +}; + +#endif
\ No newline at end of file diff --git a/unsupported/Eigen/src/KroneckerProduct/CMakeLists.txt b/unsupported/Eigen/src/KroneckerProduct/CMakeLists.txt new file mode 100644 index 000000000..4daefebee --- /dev/null +++ b/unsupported/Eigen/src/KroneckerProduct/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_KroneckerProduct_SRCS "*.h") + +INSTALL(FILES + ${Eigen_KroneckerProduct_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/KroneckerProduct COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/KroneckerProduct/KroneckerTensorProduct.h b/unsupported/Eigen/src/KroneckerProduct/KroneckerTensorProduct.h new file mode 100644 index 000000000..84fd72fc6 --- /dev/null +++ b/unsupported/Eigen/src/KroneckerProduct/KroneckerTensorProduct.h @@ -0,0 +1,157 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Kolja Brix <brix@igpm.rwth-aachen.de> +// Copyright (C) 2011 Andreas Platen <andiplaten@gmx.de> +// +// 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 KRONECKER_TENSOR_PRODUCT_H +#define KRONECKER_TENSOR_PRODUCT_H + + +namespace Eigen { + +namespace internal { + +/*! + * Kronecker tensor product helper function for dense matrices + * + * \param A Dense matrix A + * \param B Dense matrix B + * \param AB_ Kronecker tensor product of A and B + */ +template<typename Derived_A, typename Derived_B, typename Derived_AB> +void kroneckerProduct_full(const Derived_A& A, const Derived_B& B, Derived_AB & AB) +{ + const unsigned int Ar = A.rows(), + Ac = A.cols(), + Br = B.rows(), + Bc = B.cols(); + for (unsigned int i=0; i<Ar; ++i) + for (unsigned int j=0; j<Ac; ++j) + AB.block(i*Br,j*Bc,Br,Bc) = A(i,j)*B; +} + + +/*! + * Kronecker tensor product helper function for matrices, where at least one is sparse + * + * \param A Matrix A + * \param B Matrix B + * \param AB_ Kronecker tensor product of A and B + */ +template<typename Derived_A, typename Derived_B, typename Derived_AB> +void kroneckerProduct_sparse(const Derived_A &A, const Derived_B &B, Derived_AB &AB) +{ + const unsigned int Ar = A.rows(), + Ac = A.cols(), + Br = B.rows(), + Bc = B.cols(); + AB.resize(Ar*Br,Ac*Bc); + AB.resizeNonZeros(0); + AB.reserve(A.nonZeros()*B.nonZeros()); + + for (int kA=0; kA<A.outerSize(); ++kA) + { + for (int kB=0; kB<B.outerSize(); ++kB) + { + for (typename Derived_A::InnerIterator itA(A,kA); itA; ++itA) + { + for (typename Derived_B::InnerIterator itB(B,kB); itB; ++itB) + { + const unsigned int iA = itA.row(), + jA = itA.col(), + iB = itB.row(), + jB = itB.col(), + i = iA*Br + iB, + j = jA*Bc + jB; + AB.insert(i,j) = itA.value() * itB.value(); + } + } + } + } +} + +} // end namespace internal + + + +/*! + * Computes Kronecker tensor product of two dense matrices + * + * \param a Dense matrix a + * \param b Dense matrix b + * \param c Kronecker tensor product of a and b + */ +template<typename A,typename B,typename CScalar,int CRows,int CCols, int COptions, int CMaxRows, int CMaxCols> +void kroneckerProduct(const MatrixBase<A>& a, const MatrixBase<B>& b, Matrix<CScalar,CRows,CCols,COptions,CMaxRows,CMaxCols>& c) +{ + c.resize(a.rows()*b.rows(),a.cols()*b.cols()); + internal::kroneckerProduct_full(a.derived(), b.derived(), c); +} + +/*! + * Computes Kronecker tensor product of two dense matrices + * + * Remark: this function uses the const cast hack and has been + * implemented to make the function call possible, where the + * output matrix is a submatrix, e.g. + * kroneckerProduct(A,B,AB.block(2,5,6,6)); + * + * \param a Dense matrix a + * \param b Dense matrix b + * \param c Kronecker tensor product of a and b + */ +template<typename A,typename B,typename C> +void kroneckerProduct(const MatrixBase<A>& a, const MatrixBase<B>& b, MatrixBase<C> const & c_) +{ + MatrixBase<C>& c = const_cast<MatrixBase<C>& >(c_); + internal::kroneckerProduct_full(a.derived(), b.derived(), c.derived()); +} + +/*! + * Computes Kronecker tensor product of a dense and a sparse matrix + * + * \param a Dense matrix a + * \param b Sparse matrix b + * \param c Kronecker tensor product of a and b + */ +template<typename A,typename B,typename C> +void kroneckerProduct(const MatrixBase<A>& a, const SparseMatrixBase<B>& b, SparseMatrixBase<C>& c) +{ + internal::kroneckerProduct_sparse(a.derived(), b.derived(), c.derived()); +} + +/*! + * Computes Kronecker tensor product of a sparse and a dense matrix + * + * \param a Sparse matrix a + * \param b Dense matrix b + * \param c Kronecker tensor product of a and b + */ +template<typename A,typename B,typename C> +void kroneckerProduct(const SparseMatrixBase<A>& a, const MatrixBase<B>& b, SparseMatrixBase<C>& c) +{ + internal::kroneckerProduct_sparse(a.derived(), b.derived(), c.derived()); +} + +/*! + * Computes Kronecker tensor product of two sparse matrices + * + * \param a Sparse matrix a + * \param b Sparse matrix b + * \param c Kronecker tensor product of a and b + */ +template<typename A,typename B,typename C> +void kroneckerProduct(const SparseMatrixBase<A>& a, const SparseMatrixBase<B>& b, SparseMatrixBase<C>& c) +{ + internal::kroneckerProduct_sparse(a.derived(), b.derived(), c.derived()); +} + +} // end namespace Eigen + +#endif // KRONECKER_TENSOR_PRODUCT_H diff --git a/unsupported/Eigen/src/MatrixFunctions/CMakeLists.txt b/unsupported/Eigen/src/MatrixFunctions/CMakeLists.txt new file mode 100644 index 000000000..cdde64d2c --- /dev/null +++ b/unsupported/Eigen/src/MatrixFunctions/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_MatrixFunctions_SRCS "*.h") + +INSTALL(FILES + ${Eigen_MatrixFunctions_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/MatrixFunctions COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h new file mode 100644 index 000000000..642916764 --- /dev/null +++ b/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -0,0 +1,454 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009, 2010 Jitse Niesen <jitse@maths.leeds.ac.uk> +// Copyright (C) 2011 Chen-Pang He <jdh8@ms63.hinet.net> +// +// 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_EXPONENTIAL +#define EIGEN_MATRIX_EXPONENTIAL + +#include "StemFunction.h" + +namespace Eigen { + +#if defined(_MSC_VER) || defined(__FreeBSD__) + template <typename Scalar> Scalar log2(Scalar v) { using std::log; return log(v)/log(Scalar(2)); } +#endif + + +/** \ingroup MatrixFunctions_Module + * \brief Class for computing the matrix exponential. + * \tparam MatrixType type of the argument of the exponential, + * expected to be an instantiation of the Matrix class template. + */ +template <typename MatrixType> +class MatrixExponential { + + public: + + /** \brief Constructor. + * + * The class stores a reference to \p M, so it should not be + * changed (or destroyed) before compute() is called. + * + * \param[in] M matrix whose exponential is to be computed. + */ + MatrixExponential(const MatrixType &M); + + /** \brief Computes the matrix exponential. + * + * \param[out] result the matrix exponential of \p M in the constructor. + */ + template <typename ResultType> + void compute(ResultType &result); + + private: + + // Prevent copying + MatrixExponential(const MatrixExponential&); + MatrixExponential& operator=(const MatrixExponential&); + + /** \brief Compute the (3,3)-Padé approximant to the exponential. + * + * After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé + * approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$. + * + * \param[in] A Argument of matrix exponential + */ + void pade3(const MatrixType &A); + + /** \brief Compute the (5,5)-Padé approximant to the exponential. + * + * After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé + * approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$. + * + * \param[in] A Argument of matrix exponential + */ + void pade5(const MatrixType &A); + + /** \brief Compute the (7,7)-Padé approximant to the exponential. + * + * After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé + * approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$. + * + * \param[in] A Argument of matrix exponential + */ + void pade7(const MatrixType &A); + + /** \brief Compute the (9,9)-Padé approximant to the exponential. + * + * After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé + * approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$. + * + * \param[in] A Argument of matrix exponential + */ + void pade9(const MatrixType &A); + + /** \brief Compute the (13,13)-Padé approximant to the exponential. + * + * After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé + * approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$. + * + * \param[in] A Argument of matrix exponential + */ + void pade13(const MatrixType &A); + + /** \brief Compute the (17,17)-Padé approximant to the exponential. + * + * After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Padé + * approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$. + * + * This function activates only if your long double is double-double or quadruple. + * + * \param[in] A Argument of matrix exponential + */ + void pade17(const MatrixType &A); + + /** \brief Compute Padé approximant to the exponential. + * + * Computes \c m_U, \c m_V and \c m_squarings such that + * \f$ (V+U)(V-U)^{-1} \f$ is a Padé of + * \f$ \exp(2^{-\mbox{squarings}}M) \f$ around \f$ M = 0 \f$. The + * degree of the Padé approximant and the value of + * squarings are chosen such that the approximation error is no + * more than the round-off error. + * + * The argument of this function should correspond with the (real + * part of) the entries of \c m_M. It is used to select the + * correct implementation using overloading. + */ + void computeUV(double); + + /** \brief Compute Padé approximant to the exponential. + * + * \sa computeUV(double); + */ + void computeUV(float); + + /** \brief Compute Padé approximant to the exponential. + * + * \sa computeUV(double); + */ + void computeUV(long double); + + typedef typename internal::traits<MatrixType>::Scalar Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef typename std::complex<RealScalar> ComplexScalar; + + /** \brief Reference to matrix whose exponential is to be computed. */ + typename internal::nested<MatrixType>::type m_M; + + /** \brief Odd-degree terms in numerator of Padé approximant. */ + MatrixType m_U; + + /** \brief Even-degree terms in numerator of Padé approximant. */ + MatrixType m_V; + + /** \brief Used for temporary storage. */ + MatrixType m_tmp1; + + /** \brief Used for temporary storage. */ + MatrixType m_tmp2; + + /** \brief Identity matrix of the same size as \c m_M. */ + MatrixType m_Id; + + /** \brief Number of squarings required in the last step. */ + int m_squarings; + + /** \brief L1 norm of m_M. */ + RealScalar m_l1norm; +}; + +template <typename MatrixType> +MatrixExponential<MatrixType>::MatrixExponential(const MatrixType &M) : + m_M(M), + m_U(M.rows(),M.cols()), + m_V(M.rows(),M.cols()), + m_tmp1(M.rows(),M.cols()), + m_tmp2(M.rows(),M.cols()), + m_Id(MatrixType::Identity(M.rows(), M.cols())), + m_squarings(0), + m_l1norm(M.cwiseAbs().colwise().sum().maxCoeff()) +{ + /* empty body */ +} + +template <typename MatrixType> +template <typename ResultType> +void MatrixExponential<MatrixType>::compute(ResultType &result) +{ +#if LDBL_MANT_DIG > 112 // rarely happens + if(sizeof(RealScalar) > 14) { + result = m_M.matrixFunction(StdStemFunctions<ComplexScalar>::exp); + return; + } +#endif + computeUV(RealScalar()); + m_tmp1 = m_U + m_V; // numerator of Pade approximant + m_tmp2 = -m_U + m_V; // denominator of Pade approximant + result = m_tmp2.partialPivLu().solve(m_tmp1); + for (int i=0; i<m_squarings; i++) + result *= result; // undo scaling by repeated squaring +} + +template <typename MatrixType> +EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::pade3(const MatrixType &A) +{ + const RealScalar b[] = {120., 60., 12., 1.}; + m_tmp1.noalias() = A * A; + m_tmp2 = b[3]*m_tmp1 + b[1]*m_Id; + m_U.noalias() = A * m_tmp2; + m_V = b[2]*m_tmp1 + b[0]*m_Id; +} + +template <typename MatrixType> +EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::pade5(const MatrixType &A) +{ + const RealScalar b[] = {30240., 15120., 3360., 420., 30., 1.}; + MatrixType A2 = A * A; + m_tmp1.noalias() = A2 * A2; + m_tmp2 = b[5]*m_tmp1 + b[3]*A2 + b[1]*m_Id; + m_U.noalias() = A * m_tmp2; + m_V = b[4]*m_tmp1 + b[2]*A2 + b[0]*m_Id; +} + +template <typename MatrixType> +EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::pade7(const MatrixType &A) +{ + const RealScalar b[] = {17297280., 8648640., 1995840., 277200., 25200., 1512., 56., 1.}; + MatrixType A2 = A * A; + MatrixType A4 = A2 * A2; + m_tmp1.noalias() = A4 * A2; + m_tmp2 = b[7]*m_tmp1 + b[5]*A4 + b[3]*A2 + b[1]*m_Id; + m_U.noalias() = A * m_tmp2; + m_V = b[6]*m_tmp1 + b[4]*A4 + b[2]*A2 + b[0]*m_Id; +} + +template <typename MatrixType> +EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::pade9(const MatrixType &A) +{ + const RealScalar b[] = {17643225600., 8821612800., 2075673600., 302702400., 30270240., + 2162160., 110880., 3960., 90., 1.}; + MatrixType A2 = A * A; + MatrixType A4 = A2 * A2; + MatrixType A6 = A4 * A2; + m_tmp1.noalias() = A6 * A2; + m_tmp2 = b[9]*m_tmp1 + b[7]*A6 + b[5]*A4 + b[3]*A2 + b[1]*m_Id; + m_U.noalias() = A * m_tmp2; + m_V = b[8]*m_tmp1 + b[6]*A6 + b[4]*A4 + b[2]*A2 + b[0]*m_Id; +} + +template <typename MatrixType> +EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::pade13(const MatrixType &A) +{ + const RealScalar b[] = {64764752532480000., 32382376266240000., 7771770303897600., + 1187353796428800., 129060195264000., 10559470521600., 670442572800., + 33522128640., 1323241920., 40840800., 960960., 16380., 182., 1.}; + MatrixType A2 = A * A; + MatrixType A4 = A2 * A2; + m_tmp1.noalias() = A4 * A2; + m_V = b[13]*m_tmp1 + b[11]*A4 + b[9]*A2; // used for temporary storage + m_tmp2.noalias() = m_tmp1 * m_V; + m_tmp2 += b[7]*m_tmp1 + b[5]*A4 + b[3]*A2 + b[1]*m_Id; + m_U.noalias() = A * m_tmp2; + m_tmp2 = b[12]*m_tmp1 + b[10]*A4 + b[8]*A2; + m_V.noalias() = m_tmp1 * m_tmp2; + m_V += b[6]*m_tmp1 + b[4]*A4 + b[2]*A2 + b[0]*m_Id; +} + +#if LDBL_MANT_DIG > 64 +template <typename MatrixType> +EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::pade17(const MatrixType &A) +{ + const RealScalar b[] = {830034394580628357120000.L, 415017197290314178560000.L, + 100610229646136770560000.L, 15720348382208870400000.L, + 1774878043152614400000.L, 153822763739893248000.L, 10608466464820224000.L, + 595373117923584000.L, 27563570274240000.L, 1060137318240000.L, + 33924394183680.L, 899510451840.L, 19554575040.L, 341863200.L, 4651200.L, + 46512.L, 306.L, 1.L}; + MatrixType A2 = A * A; + MatrixType A4 = A2 * A2; + MatrixType A6 = A4 * A2; + m_tmp1.noalias() = A4 * A4; + m_V = b[17]*m_tmp1 + b[15]*A6 + b[13]*A4 + b[11]*A2; // used for temporary storage + m_tmp2.noalias() = m_tmp1 * m_V; + m_tmp2 += b[9]*m_tmp1 + b[7]*A6 + b[5]*A4 + b[3]*A2 + b[1]*m_Id; + m_U.noalias() = A * m_tmp2; + m_tmp2 = b[16]*m_tmp1 + b[14]*A6 + b[12]*A4 + b[10]*A2; + m_V.noalias() = m_tmp1 * m_tmp2; + m_V += b[8]*m_tmp1 + b[6]*A6 + b[4]*A4 + b[2]*A2 + b[0]*m_Id; +} +#endif + +template <typename MatrixType> +void MatrixExponential<MatrixType>::computeUV(float) +{ + using std::max; + using std::pow; + using std::ceil; + if (m_l1norm < 4.258730016922831e-001) { + pade3(m_M); + } else if (m_l1norm < 1.880152677804762e+000) { + pade5(m_M); + } else { + const float maxnorm = 3.925724783138660f; + m_squarings = (max)(0, (int)ceil(log2(m_l1norm / maxnorm))); + MatrixType A = m_M / pow(Scalar(2), m_squarings); + pade7(A); + } +} + +template <typename MatrixType> +void MatrixExponential<MatrixType>::computeUV(double) +{ + using std::max; + using std::pow; + using std::ceil; + if (m_l1norm < 1.495585217958292e-002) { + pade3(m_M); + } else if (m_l1norm < 2.539398330063230e-001) { + pade5(m_M); + } else if (m_l1norm < 9.504178996162932e-001) { + pade7(m_M); + } else if (m_l1norm < 2.097847961257068e+000) { + pade9(m_M); + } else { + const double maxnorm = 5.371920351148152; + m_squarings = (max)(0, (int)ceil(log2(m_l1norm / maxnorm))); + MatrixType A = m_M / pow(Scalar(2), m_squarings); + pade13(A); + } +} + +template <typename MatrixType> +void MatrixExponential<MatrixType>::computeUV(long double) +{ + using std::max; + using std::pow; + using std::ceil; +#if LDBL_MANT_DIG == 53 // double precision + computeUV(double()); +#elif LDBL_MANT_DIG <= 64 // extended precision + if (m_l1norm < 4.1968497232266989671e-003L) { + pade3(m_M); + } else if (m_l1norm < 1.1848116734693823091e-001L) { + pade5(m_M); + } else if (m_l1norm < 5.5170388480686700274e-001L) { + pade7(m_M); + } else if (m_l1norm < 1.3759868875587845383e+000L) { + pade9(m_M); + } else { + const long double maxnorm = 4.0246098906697353063L; + m_squarings = (max)(0, (int)ceil(log2(m_l1norm / maxnorm))); + MatrixType A = m_M / pow(Scalar(2), m_squarings); + pade13(A); + } +#elif LDBL_MANT_DIG <= 106 // double-double + if (m_l1norm < 3.2787892205607026992947488108213e-005L) { + pade3(m_M); + } else if (m_l1norm < 6.4467025060072760084130906076332e-003L) { + pade5(m_M); + } else if (m_l1norm < 6.8988028496595374751374122881143e-002L) { + pade7(m_M); + } else if (m_l1norm < 2.7339737518502231741495857201670e-001L) { + pade9(m_M); + } else if (m_l1norm < 1.3203382096514474905666448850278e+000L) { + pade13(m_M); + } else { + const long double maxnorm = 3.2579440895405400856599663723517L; + m_squarings = (max)(0, (int)ceil(log2(m_l1norm / maxnorm))); + MatrixType A = m_M / pow(Scalar(2), m_squarings); + pade17(A); + } +#elif LDBL_MANT_DIG <= 112 // quadruple precison + if (m_l1norm < 1.639394610288918690547467954466970e-005L) { + pade3(m_M); + } else if (m_l1norm < 4.253237712165275566025884344433009e-003L) { + pade5(m_M); + } else if (m_l1norm < 5.125804063165764409885122032933142e-002L) { + pade7(m_M); + } else if (m_l1norm < 2.170000765161155195453205651889853e-001L) { + pade9(m_M); + } else if (m_l1norm < 1.125358383453143065081397882891878e+000L) { + pade13(m_M); + } else { + const long double maxnorm = 2.884233277829519311757165057717815L; + m_squarings = (max)(0, (int)ceil(log2(m_l1norm / maxnorm))); + MatrixType A = m_M / pow(Scalar(2), m_squarings); + pade17(A); + } +#else + // this case should be handled in compute() + eigen_assert(false && "Bug in MatrixExponential"); +#endif // LDBL_MANT_DIG +} + +/** \ingroup MatrixFunctions_Module + * + * \brief Proxy for the matrix exponential of some matrix (expression). + * + * \tparam Derived Type of the argument to the matrix exponential. + * + * This class holds the argument to the matrix exponential until it + * is assigned or evaluated for some other reason (so the argument + * should not be changed in the meantime). It is the return type of + * MatrixBase::exp() and most of the time this is the only way it is + * used. + */ +template<typename Derived> struct MatrixExponentialReturnValue +: public ReturnByValue<MatrixExponentialReturnValue<Derived> > +{ + typedef typename Derived::Index Index; + public: + /** \brief Constructor. + * + * \param[in] src %Matrix (expression) forming the argument of the + * matrix exponential. + */ + MatrixExponentialReturnValue(const Derived& src) : m_src(src) { } + + /** \brief Compute the matrix exponential. + * + * \param[out] result the matrix exponential of \p src in the + * constructor. + */ + template <typename ResultType> + inline void evalTo(ResultType& result) const + { + const typename Derived::PlainObject srcEvaluated = m_src.eval(); + MatrixExponential<typename Derived::PlainObject> me(srcEvaluated); + me.compute(result); + } + + Index rows() const { return m_src.rows(); } + Index cols() const { return m_src.cols(); } + + protected: + const Derived& m_src; + private: + MatrixExponentialReturnValue& operator=(const MatrixExponentialReturnValue&); +}; + +namespace internal { +template<typename Derived> +struct traits<MatrixExponentialReturnValue<Derived> > +{ + typedef typename Derived::PlainObject ReturnType; +}; +} + +template <typename Derived> +const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const +{ + eigen_assert(rows() == cols()); + return MatrixExponentialReturnValue<Derived>(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_EXPONENTIAL diff --git a/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h new file mode 100644 index 000000000..c57ca87ed --- /dev/null +++ b/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h @@ -0,0 +1,590 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009-2011 Jitse Niesen <jitse@maths.leeds.ac.uk> +// +// 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_FUNCTION +#define EIGEN_MATRIX_FUNCTION + +#include "StemFunction.h" +#include "MatrixFunctionAtomic.h" + + +namespace Eigen { + +/** \ingroup MatrixFunctions_Module + * \brief Class for computing matrix functions. + * \tparam MatrixType type of the argument of the matrix function, + * expected to be an instantiation of the Matrix class template. + * \tparam AtomicType type for computing matrix function of atomic blocks. + * \tparam IsComplex used internally to select correct specialization. + * + * This class implements the Schur-Parlett algorithm for computing matrix functions. The spectrum of the + * matrix is divided in clustered of eigenvalues that lies close together. This class delegates the + * computation of the matrix function on every block corresponding to these clusters to an object of type + * \p AtomicType and uses these results to compute the matrix function of the whole matrix. The class + * \p AtomicType should have a \p compute() member function for computing the matrix function of a block. + * + * \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic + */ +template <typename MatrixType, + typename AtomicType, + int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex> +class MatrixFunction +{ + public: + + /** \brief Constructor. + * + * \param[in] A argument of matrix function, should be a square matrix. + * \param[in] atomic class for computing matrix function of atomic blocks. + * + * The class stores references to \p A and \p atomic, so they should not be + * changed (or destroyed) before compute() is called. + */ + MatrixFunction(const MatrixType& A, AtomicType& atomic); + + /** \brief Compute the matrix function. + * + * \param[out] result the function \p f applied to \p A, as + * specified in the constructor. + * + * See MatrixBase::matrixFunction() for details on how this computation + * is implemented. + */ + template <typename ResultType> + void compute(ResultType &result); +}; + + +/** \internal \ingroup MatrixFunctions_Module + * \brief Partial specialization of MatrixFunction for real matrices + */ +template <typename MatrixType, typename AtomicType> +class MatrixFunction<MatrixType, AtomicType, 0> +{ + private: + + typedef internal::traits<MatrixType> Traits; + typedef typename Traits::Scalar Scalar; + static const int Rows = Traits::RowsAtCompileTime; + static const int Cols = Traits::ColsAtCompileTime; + static const int Options = MatrixType::Options; + static const int MaxRows = Traits::MaxRowsAtCompileTime; + static const int MaxCols = Traits::MaxColsAtCompileTime; + + typedef std::complex<Scalar> ComplexScalar; + typedef Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols> ComplexMatrix; + + public: + + /** \brief Constructor. + * + * \param[in] A argument of matrix function, should be a square matrix. + * \param[in] atomic class for computing matrix function of atomic blocks. + */ + MatrixFunction(const MatrixType& A, AtomicType& atomic) : m_A(A), m_atomic(atomic) { } + + /** \brief Compute the matrix function. + * + * \param[out] result the function \p f applied to \p A, as + * specified in the constructor. + * + * This function converts the real matrix \c A to a complex matrix, + * uses MatrixFunction<MatrixType,1> and then converts the result back to + * a real matrix. + */ + template <typename ResultType> + void compute(ResultType& result) + { + ComplexMatrix CA = m_A.template cast<ComplexScalar>(); + ComplexMatrix Cresult; + MatrixFunction<ComplexMatrix, AtomicType> mf(CA, m_atomic); + mf.compute(Cresult); + result = Cresult.real(); + } + + private: + typename internal::nested<MatrixType>::type m_A; /**< \brief Reference to argument of matrix function. */ + AtomicType& m_atomic; /**< \brief Class for computing matrix function of atomic blocks. */ + + MatrixFunction& operator=(const MatrixFunction&); +}; + + +/** \internal \ingroup MatrixFunctions_Module + * \brief Partial specialization of MatrixFunction for complex matrices + */ +template <typename MatrixType, typename AtomicType> +class MatrixFunction<MatrixType, AtomicType, 1> +{ + private: + + typedef internal::traits<MatrixType> Traits; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::Index Index; + static const int RowsAtCompileTime = Traits::RowsAtCompileTime; + static const int ColsAtCompileTime = Traits::ColsAtCompileTime; + static const int Options = MatrixType::Options; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef Matrix<Scalar, Traits::RowsAtCompileTime, 1> VectorType; + typedef Matrix<Index, Traits::RowsAtCompileTime, 1> IntVectorType; + typedef Matrix<Index, Dynamic, 1> DynamicIntVectorType; + typedef std::list<Scalar> Cluster; + typedef std::list<Cluster> ListOfClusters; + typedef Matrix<Scalar, Dynamic, Dynamic, Options, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType; + + public: + + MatrixFunction(const MatrixType& A, AtomicType& atomic); + template <typename ResultType> void compute(ResultType& result); + + private: + + void computeSchurDecomposition(); + void partitionEigenvalues(); + typename ListOfClusters::iterator findCluster(Scalar key); + void computeClusterSize(); + void computeBlockStart(); + void constructPermutation(); + void permuteSchur(); + void swapEntriesInSchur(Index index); + void computeBlockAtomic(); + Block<MatrixType> block(MatrixType& A, Index i, Index j); + void computeOffDiagonal(); + DynMatrixType solveTriangularSylvester(const DynMatrixType& A, const DynMatrixType& B, const DynMatrixType& C); + + typename internal::nested<MatrixType>::type m_A; /**< \brief Reference to argument of matrix function. */ + AtomicType& m_atomic; /**< \brief Class for computing matrix function of atomic blocks. */ + MatrixType m_T; /**< \brief Triangular part of Schur decomposition */ + MatrixType m_U; /**< \brief Unitary part of Schur decomposition */ + MatrixType m_fT; /**< \brief %Matrix function applied to #m_T */ + ListOfClusters m_clusters; /**< \brief Partition of eigenvalues into clusters of ei'vals "close" to each other */ + DynamicIntVectorType m_eivalToCluster; /**< \brief m_eivalToCluster[i] = j means i-th ei'val is in j-th cluster */ + DynamicIntVectorType m_clusterSize; /**< \brief Number of eigenvalues in each clusters */ + DynamicIntVectorType m_blockStart; /**< \brief Row index at which block corresponding to i-th cluster starts */ + IntVectorType m_permutation; /**< \brief Permutation which groups ei'vals in the same cluster together */ + + /** \brief Maximum distance allowed between eigenvalues to be considered "close". + * + * This is morally a \c static \c const \c Scalar, but only + * integers can be static constant class members in C++. The + * separation constant is set to 0.1, a value taken from the + * paper by Davies and Higham. */ + static const RealScalar separation() { return static_cast<RealScalar>(0.1); } + + MatrixFunction& operator=(const MatrixFunction&); +}; + +/** \brief Constructor. + * + * \param[in] A argument of matrix function, should be a square matrix. + * \param[in] atomic class for computing matrix function of atomic blocks. + */ +template <typename MatrixType, typename AtomicType> +MatrixFunction<MatrixType,AtomicType,1>::MatrixFunction(const MatrixType& A, AtomicType& atomic) + : m_A(A), m_atomic(atomic) +{ + /* empty body */ +} + +/** \brief Compute the matrix function. + * + * \param[out] result the function \p f applied to \p A, as + * specified in the constructor. + */ +template <typename MatrixType, typename AtomicType> +template <typename ResultType> +void MatrixFunction<MatrixType,AtomicType,1>::compute(ResultType& result) +{ + computeSchurDecomposition(); + partitionEigenvalues(); + computeClusterSize(); + computeBlockStart(); + constructPermutation(); + permuteSchur(); + computeBlockAtomic(); + computeOffDiagonal(); + result = m_U * m_fT * m_U.adjoint(); +} + +/** \brief Store the Schur decomposition of #m_A in #m_T and #m_U */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::computeSchurDecomposition() +{ + const ComplexSchur<MatrixType> schurOfA(m_A); + m_T = schurOfA.matrixT(); + m_U = schurOfA.matrixU(); +} + +/** \brief Partition eigenvalues in clusters of ei'vals close to each other + * + * This function computes #m_clusters. This is a partition of the + * eigenvalues of #m_T in clusters, such that + * # Any eigenvalue in a certain cluster is at most separation() away + * from another eigenvalue in the same cluster. + * # The distance between two eigenvalues in different clusters is + * more than separation(). + * The implementation follows Algorithm 4.1 in the paper of Davies + * and Higham. + */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::partitionEigenvalues() +{ + const Index rows = m_T.rows(); + VectorType diag = m_T.diagonal(); // contains eigenvalues of A + + for (Index i=0; i<rows; ++i) { + // Find set containing diag(i), adding a new set if necessary + typename ListOfClusters::iterator qi = findCluster(diag(i)); + if (qi == m_clusters.end()) { + Cluster l; + l.push_back(diag(i)); + m_clusters.push_back(l); + qi = m_clusters.end(); + --qi; + } + + // Look for other element to add to the set + for (Index j=i+1; j<rows; ++j) { + if (internal::abs(diag(j) - diag(i)) <= separation() && std::find(qi->begin(), qi->end(), diag(j)) == qi->end()) { + typename ListOfClusters::iterator qj = findCluster(diag(j)); + if (qj == m_clusters.end()) { + qi->push_back(diag(j)); + } else { + qi->insert(qi->end(), qj->begin(), qj->end()); + m_clusters.erase(qj); + } + } + } + } +} + +/** \brief Find cluster in #m_clusters containing some value + * \param[in] key Value to find + * \returns Iterator to cluster containing \c key, or + * \c m_clusters.end() if no cluster in m_clusters contains \c key. + */ +template <typename MatrixType, typename AtomicType> +typename MatrixFunction<MatrixType,AtomicType,1>::ListOfClusters::iterator MatrixFunction<MatrixType,AtomicType,1>::findCluster(Scalar key) +{ + typename Cluster::iterator j; + for (typename ListOfClusters::iterator i = m_clusters.begin(); i != m_clusters.end(); ++i) { + j = std::find(i->begin(), i->end(), key); + if (j != i->end()) + return i; + } + return m_clusters.end(); +} + +/** \brief Compute #m_clusterSize and #m_eivalToCluster using #m_clusters */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::computeClusterSize() +{ + const Index rows = m_T.rows(); + VectorType diag = m_T.diagonal(); + const Index numClusters = static_cast<Index>(m_clusters.size()); + + m_clusterSize.setZero(numClusters); + m_eivalToCluster.resize(rows); + Index clusterIndex = 0; + for (typename ListOfClusters::const_iterator cluster = m_clusters.begin(); cluster != m_clusters.end(); ++cluster) { + for (Index i = 0; i < diag.rows(); ++i) { + if (std::find(cluster->begin(), cluster->end(), diag(i)) != cluster->end()) { + ++m_clusterSize[clusterIndex]; + m_eivalToCluster[i] = clusterIndex; + } + } + ++clusterIndex; + } +} + +/** \brief Compute #m_blockStart using #m_clusterSize */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::computeBlockStart() +{ + m_blockStart.resize(m_clusterSize.rows()); + m_blockStart(0) = 0; + for (Index i = 1; i < m_clusterSize.rows(); i++) { + m_blockStart(i) = m_blockStart(i-1) + m_clusterSize(i-1); + } +} + +/** \brief Compute #m_permutation using #m_eivalToCluster and #m_blockStart */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::constructPermutation() +{ + DynamicIntVectorType indexNextEntry = m_blockStart; + m_permutation.resize(m_T.rows()); + for (Index i = 0; i < m_T.rows(); i++) { + Index cluster = m_eivalToCluster[i]; + m_permutation[i] = indexNextEntry[cluster]; + ++indexNextEntry[cluster]; + } +} + +/** \brief Permute Schur decomposition in #m_U and #m_T according to #m_permutation */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::permuteSchur() +{ + IntVectorType p = m_permutation; + for (Index i = 0; i < p.rows() - 1; i++) { + Index j; + for (j = i; j < p.rows(); j++) { + if (p(j) == i) break; + } + eigen_assert(p(j) == i); + for (Index k = j-1; k >= i; k--) { + swapEntriesInSchur(k); + std::swap(p.coeffRef(k), p.coeffRef(k+1)); + } + } +} + +/** \brief Swap rows \a index and \a index+1 in Schur decomposition in #m_U and #m_T */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::swapEntriesInSchur(Index index) +{ + JacobiRotation<Scalar> rotation; + rotation.makeGivens(m_T(index, index+1), m_T(index+1, index+1) - m_T(index, index)); + m_T.applyOnTheLeft(index, index+1, rotation.adjoint()); + m_T.applyOnTheRight(index, index+1, rotation); + m_U.applyOnTheRight(index, index+1, rotation); +} + +/** \brief Compute block diagonal part of #m_fT. + * + * This routine computes the matrix function applied to the block diagonal part of #m_T, with the blocking + * given by #m_blockStart. The matrix function of each diagonal block is computed by #m_atomic. The + * off-diagonal parts of #m_fT are set to zero. + */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::computeBlockAtomic() +{ + m_fT.resize(m_T.rows(), m_T.cols()); + m_fT.setZero(); + for (Index i = 0; i < m_clusterSize.rows(); ++i) { + block(m_fT, i, i) = m_atomic.compute(block(m_T, i, i)); + } +} + +/** \brief Return block of matrix according to blocking given by #m_blockStart */ +template <typename MatrixType, typename AtomicType> +Block<MatrixType> MatrixFunction<MatrixType,AtomicType,1>::block(MatrixType& A, Index i, Index j) +{ + return A.block(m_blockStart(i), m_blockStart(j), m_clusterSize(i), m_clusterSize(j)); +} + +/** \brief Compute part of #m_fT above block diagonal. + * + * This routine assumes that the block diagonal part of #m_fT (which + * equals the matrix function applied to #m_T) has already been computed and computes + * the part above the block diagonal. The part below the diagonal is + * zero, because #m_T is upper triangular. + */ +template <typename MatrixType, typename AtomicType> +void MatrixFunction<MatrixType,AtomicType,1>::computeOffDiagonal() +{ + for (Index diagIndex = 1; diagIndex < m_clusterSize.rows(); diagIndex++) { + for (Index blockIndex = 0; blockIndex < m_clusterSize.rows() - diagIndex; blockIndex++) { + // compute (blockIndex, blockIndex+diagIndex) block + DynMatrixType A = block(m_T, blockIndex, blockIndex); + DynMatrixType B = -block(m_T, blockIndex+diagIndex, blockIndex+diagIndex); + DynMatrixType C = block(m_fT, blockIndex, blockIndex) * block(m_T, blockIndex, blockIndex+diagIndex); + C -= block(m_T, blockIndex, blockIndex+diagIndex) * block(m_fT, blockIndex+diagIndex, blockIndex+diagIndex); + for (Index k = blockIndex + 1; k < blockIndex + diagIndex; k++) { + C += block(m_fT, blockIndex, k) * block(m_T, k, blockIndex+diagIndex); + C -= block(m_T, blockIndex, k) * block(m_fT, k, blockIndex+diagIndex); + } + block(m_fT, blockIndex, blockIndex+diagIndex) = solveTriangularSylvester(A, B, C); + } + } +} + +/** \brief Solve a triangular Sylvester equation AX + XB = C + * + * \param[in] A the matrix A; should be square and upper triangular + * \param[in] B the matrix B; should be square and upper triangular + * \param[in] C the matrix C; should have correct size. + * + * \returns the solution X. + * + * If A is m-by-m and B is n-by-n, then both C and X are m-by-n. + * The (i,j)-th component of the Sylvester equation is + * \f[ + * \sum_{k=i}^m A_{ik} X_{kj} + \sum_{k=1}^j X_{ik} B_{kj} = C_{ij}. + * \f] + * This can be re-arranged to yield: + * \f[ + * X_{ij} = \frac{1}{A_{ii} + B_{jj}} \Bigl( C_{ij} + * - \sum_{k=i+1}^m A_{ik} X_{kj} - \sum_{k=1}^{j-1} X_{ik} B_{kj} \Bigr). + * \f] + * It is assumed that A and B are such that the numerator is never + * zero (otherwise the Sylvester equation does not have a unique + * solution). In that case, these equations can be evaluated in the + * order \f$ i=m,\ldots,1 \f$ and \f$ j=1,\ldots,n \f$. + */ +template <typename MatrixType, typename AtomicType> +typename MatrixFunction<MatrixType,AtomicType,1>::DynMatrixType MatrixFunction<MatrixType,AtomicType,1>::solveTriangularSylvester( + const DynMatrixType& A, + const DynMatrixType& B, + const DynMatrixType& C) +{ + eigen_assert(A.rows() == A.cols()); + eigen_assert(A.isUpperTriangular()); + eigen_assert(B.rows() == B.cols()); + eigen_assert(B.isUpperTriangular()); + eigen_assert(C.rows() == A.rows()); + eigen_assert(C.cols() == B.rows()); + + Index m = A.rows(); + Index n = B.rows(); + DynMatrixType X(m, n); + + for (Index i = m - 1; i >= 0; --i) { + for (Index j = 0; j < n; ++j) { + + // Compute AX = \sum_{k=i+1}^m A_{ik} X_{kj} + Scalar AX; + if (i == m - 1) { + AX = 0; + } else { + Matrix<Scalar,1,1> AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i); + AX = AXmatrix(0,0); + } + + // Compute XB = \sum_{k=1}^{j-1} X_{ik} B_{kj} + Scalar XB; + if (j == 0) { + XB = 0; + } else { + Matrix<Scalar,1,1> XBmatrix = X.row(i).head(j) * B.col(j).head(j); + XB = XBmatrix(0,0); + } + + X(i,j) = (C(i,j) - AX - XB) / (A(i,i) + B(j,j)); + } + } + return X; +} + +/** \ingroup MatrixFunctions_Module + * + * \brief Proxy for the matrix function of some matrix (expression). + * + * \tparam Derived Type of the argument to the matrix function. + * + * This class holds the argument to the matrix function until it is + * assigned or evaluated for some other reason (so the argument + * should not be changed in the meantime). It is the return type of + * matrixBase::matrixFunction() and related functions and most of the + * time this is the only way it is used. + */ +template<typename Derived> class MatrixFunctionReturnValue +: public ReturnByValue<MatrixFunctionReturnValue<Derived> > +{ + public: + + typedef typename Derived::Scalar Scalar; + typedef typename Derived::Index Index; + typedef typename internal::stem_function<Scalar>::type StemFunction; + + /** \brief Constructor. + * + * \param[in] A %Matrix (expression) forming the argument of the + * matrix function. + * \param[in] f Stem function for matrix function under consideration. + */ + MatrixFunctionReturnValue(const Derived& A, StemFunction f) : m_A(A), m_f(f) { } + + /** \brief Compute the matrix function. + * + * \param[out] result \p f applied to \p A, where \p f and \p A + * are as in the constructor. + */ + template <typename ResultType> + inline void evalTo(ResultType& result) const + { + typedef typename Derived::PlainObject PlainObject; + typedef internal::traits<PlainObject> Traits; + static const int RowsAtCompileTime = Traits::RowsAtCompileTime; + static const int ColsAtCompileTime = Traits::ColsAtCompileTime; + static const int Options = PlainObject::Options; + typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar; + typedef Matrix<ComplexScalar, Dynamic, Dynamic, Options, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType; + typedef MatrixFunctionAtomic<DynMatrixType> AtomicType; + AtomicType atomic(m_f); + + const PlainObject Aevaluated = m_A.eval(); + MatrixFunction<PlainObject, AtomicType> mf(Aevaluated, atomic); + mf.compute(result); + } + + Index rows() const { return m_A.rows(); } + Index cols() const { return m_A.cols(); } + + private: + typename internal::nested<Derived>::type m_A; + StemFunction *m_f; + + MatrixFunctionReturnValue& operator=(const MatrixFunctionReturnValue&); +}; + +namespace internal { +template<typename Derived> +struct traits<MatrixFunctionReturnValue<Derived> > +{ + typedef typename Derived::PlainObject ReturnType; +}; +} + + +/********** MatrixBase methods **********/ + + +template <typename Derived> +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const +{ + eigen_assert(rows() == cols()); + return MatrixFunctionReturnValue<Derived>(derived(), f); +} + +template <typename Derived> +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const +{ + eigen_assert(rows() == cols()); + typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar; + return MatrixFunctionReturnValue<Derived>(derived(), StdStemFunctions<ComplexScalar>::sin); +} + +template <typename Derived> +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const +{ + eigen_assert(rows() == cols()); + typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar; + return MatrixFunctionReturnValue<Derived>(derived(), StdStemFunctions<ComplexScalar>::cos); +} + +template <typename Derived> +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const +{ + eigen_assert(rows() == cols()); + typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar; + return MatrixFunctionReturnValue<Derived>(derived(), StdStemFunctions<ComplexScalar>::sinh); +} + +template <typename Derived> +const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const +{ + eigen_assert(rows() == cols()); + typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar; + return MatrixFunctionReturnValue<Derived>(derived(), StdStemFunctions<ComplexScalar>::cosh); +} + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_FUNCTION diff --git a/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h b/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h new file mode 100644 index 000000000..efe332c48 --- /dev/null +++ b/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h @@ -0,0 +1,131 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Jitse Niesen <jitse@maths.leeds.ac.uk> +// +// 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_FUNCTION_ATOMIC +#define EIGEN_MATRIX_FUNCTION_ATOMIC + +namespace Eigen { + +/** \ingroup MatrixFunctions_Module + * \class MatrixFunctionAtomic + * \brief Helper class for computing matrix functions of atomic matrices. + * + * \internal + * Here, an atomic matrix is a triangular matrix whose diagonal + * entries are close to each other. + */ +template <typename MatrixType> +class MatrixFunctionAtomic +{ + public: + + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::Index Index; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef typename internal::stem_function<Scalar>::type StemFunction; + typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType; + + /** \brief Constructor + * \param[in] f matrix function to compute. + */ + MatrixFunctionAtomic(StemFunction f) : m_f(f) { } + + /** \brief Compute matrix function of atomic matrix + * \param[in] A argument of matrix function, should be upper triangular and atomic + * \returns f(A), the matrix function evaluated at the given matrix + */ + MatrixType compute(const MatrixType& A); + + private: + + // Prevent copying + MatrixFunctionAtomic(const MatrixFunctionAtomic&); + MatrixFunctionAtomic& operator=(const MatrixFunctionAtomic&); + + void computeMu(); + bool taylorConverged(Index s, const MatrixType& F, const MatrixType& Fincr, const MatrixType& P); + + /** \brief Pointer to scalar function */ + StemFunction* m_f; + + /** \brief Size of matrix function */ + Index m_Arows; + + /** \brief Mean of eigenvalues */ + Scalar m_avgEival; + + /** \brief Argument shifted by mean of eigenvalues */ + MatrixType m_Ashifted; + + /** \brief Constant used to determine whether Taylor series has converged */ + RealScalar m_mu; +}; + +template <typename MatrixType> +MatrixType MatrixFunctionAtomic<MatrixType>::compute(const MatrixType& A) +{ + // TODO: Use that A is upper triangular + m_Arows = A.rows(); + m_avgEival = A.trace() / Scalar(RealScalar(m_Arows)); + m_Ashifted = A - m_avgEival * MatrixType::Identity(m_Arows, m_Arows); + computeMu(); + MatrixType F = m_f(m_avgEival, 0) * MatrixType::Identity(m_Arows, m_Arows); + MatrixType P = m_Ashifted; + MatrixType Fincr; + for (Index s = 1; s < 1.1 * m_Arows + 10; s++) { // upper limit is fairly arbitrary + Fincr = m_f(m_avgEival, static_cast<int>(s)) * P; + F += Fincr; + P = Scalar(RealScalar(1.0/(s + 1))) * P * m_Ashifted; + if (taylorConverged(s, F, Fincr, P)) { + return F; + } + } + eigen_assert("Taylor series does not converge" && 0); + return F; +} + +/** \brief Compute \c m_mu. */ +template <typename MatrixType> +void MatrixFunctionAtomic<MatrixType>::computeMu() +{ + const MatrixType N = MatrixType::Identity(m_Arows, m_Arows) - m_Ashifted; + VectorType e = VectorType::Ones(m_Arows); + N.template triangularView<Upper>().solveInPlace(e); + m_mu = e.cwiseAbs().maxCoeff(); +} + +/** \brief Determine whether Taylor series has converged */ +template <typename MatrixType> +bool MatrixFunctionAtomic<MatrixType>::taylorConverged(Index s, const MatrixType& F, + const MatrixType& Fincr, const MatrixType& P) +{ + const Index n = F.rows(); + const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff(); + const RealScalar Fincr_norm = Fincr.cwiseAbs().rowwise().sum().maxCoeff(); + if (Fincr_norm < NumTraits<Scalar>::epsilon() * F_norm) { + RealScalar delta = 0; + RealScalar rfactorial = 1; + for (Index r = 0; r < n; r++) { + RealScalar mx = 0; + for (Index i = 0; i < n; i++) + mx = (std::max)(mx, std::abs(m_f(m_Ashifted(i, i) + m_avgEival, static_cast<int>(s+r)))); + if (r != 0) + rfactorial *= RealScalar(r); + delta = (std::max)(delta, mx / rfactorial); + } + const RealScalar P_norm = P.cwiseAbs().rowwise().sum().maxCoeff(); + if (m_mu * delta * P_norm < NumTraits<Scalar>::epsilon() * F_norm) + return true; + } + return false; +} + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_FUNCTION_ATOMIC diff --git a/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h new file mode 100644 index 000000000..3a50514b9 --- /dev/null +++ b/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h @@ -0,0 +1,495 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Jitse Niesen <jitse@maths.leeds.ac.uk> +// Copyright (C) 2011 Chen-Pang He <jdh8@ms63.hinet.net> +// +// 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_LOGARITHM +#define EIGEN_MATRIX_LOGARITHM + +#ifndef M_PI +#define M_PI 3.141592653589793238462643383279503L +#endif + +namespace Eigen { + +/** \ingroup MatrixFunctions_Module + * \class MatrixLogarithmAtomic + * \brief Helper class for computing matrix logarithm of atomic matrices. + * + * \internal + * Here, an atomic matrix is a triangular matrix whose diagonal + * entries are close to each other. + * + * \sa class MatrixFunctionAtomic, MatrixBase::log() + */ +template <typename MatrixType> +class MatrixLogarithmAtomic +{ +public: + + typedef typename MatrixType::Scalar Scalar; + // typedef typename MatrixType::Index Index; + typedef typename NumTraits<Scalar>::Real RealScalar; + // typedef typename internal::stem_function<Scalar>::type StemFunction; + // typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType; + + /** \brief Constructor. */ + MatrixLogarithmAtomic() { } + + /** \brief Compute matrix logarithm of atomic matrix + * \param[in] A argument of matrix logarithm, should be upper triangular and atomic + * \returns The logarithm of \p A. + */ + MatrixType compute(const MatrixType& A); + +private: + + void compute2x2(const MatrixType& A, MatrixType& result); + void computeBig(const MatrixType& A, MatrixType& result); + static Scalar atanh(Scalar x); + int getPadeDegree(float normTminusI); + int getPadeDegree(double normTminusI); + int getPadeDegree(long double normTminusI); + void computePade(MatrixType& result, const MatrixType& T, int degree); + void computePade3(MatrixType& result, const MatrixType& T); + void computePade4(MatrixType& result, const MatrixType& T); + void computePade5(MatrixType& result, const MatrixType& T); + void computePade6(MatrixType& result, const MatrixType& T); + void computePade7(MatrixType& result, const MatrixType& T); + void computePade8(MatrixType& result, const MatrixType& T); + void computePade9(MatrixType& result, const MatrixType& T); + void computePade10(MatrixType& result, const MatrixType& T); + void computePade11(MatrixType& result, const MatrixType& T); + + static const int minPadeDegree = 3; + static const int maxPadeDegree = std::numeric_limits<RealScalar>::digits<= 24? 5: // single precision + std::numeric_limits<RealScalar>::digits<= 53? 7: // double precision + std::numeric_limits<RealScalar>::digits<= 64? 8: // extended precision + std::numeric_limits<RealScalar>::digits<=106? 10: 11; // double-double or quadruple precision + + // Prevent copying + MatrixLogarithmAtomic(const MatrixLogarithmAtomic&); + MatrixLogarithmAtomic& operator=(const MatrixLogarithmAtomic&); +}; + +/** \brief Compute logarithm of triangular matrix with clustered eigenvalues. */ +template <typename MatrixType> +MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A) +{ + using std::log; + MatrixType result(A.rows(), A.rows()); + if (A.rows() == 1) + result(0,0) = log(A(0,0)); + else if (A.rows() == 2) + compute2x2(A, result); + else + computeBig(A, result); + return result; +} + +/** \brief Compute atanh (inverse hyperbolic tangent). */ +template <typename MatrixType> +typename MatrixType::Scalar MatrixLogarithmAtomic<MatrixType>::atanh(typename MatrixType::Scalar x) +{ + using std::abs; + using std::sqrt; + if (abs(x) > sqrt(NumTraits<Scalar>::epsilon())) + return Scalar(0.5) * log((Scalar(1) + x) / (Scalar(1) - x)); + else + return x + x*x*x / Scalar(3); +} + +/** \brief Compute logarithm of 2x2 triangular matrix. */ +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::compute2x2(const MatrixType& A, MatrixType& result) +{ + using std::abs; + using std::ceil; + using std::imag; + using std::log; + + Scalar logA00 = log(A(0,0)); + Scalar logA11 = log(A(1,1)); + + result(0,0) = logA00; + result(1,0) = Scalar(0); + result(1,1) = logA11; + + if (A(0,0) == A(1,1)) { + result(0,1) = A(0,1) / A(0,0); + } else if ((abs(A(0,0)) < 0.5*abs(A(1,1))) || (abs(A(0,0)) > 2*abs(A(1,1)))) { + result(0,1) = A(0,1) * (logA11 - logA00) / (A(1,1) - A(0,0)); + } else { + // computation in previous branch is inaccurate if A(1,1) \approx A(0,0) + int unwindingNumber = static_cast<int>(ceil((imag(logA11 - logA00) - M_PI) / (2*M_PI))); + Scalar z = (A(1,1) - A(0,0)) / (A(1,1) + A(0,0)); + result(0,1) = A(0,1) * (Scalar(2) * atanh(z) + Scalar(0,2*M_PI*unwindingNumber)) / (A(1,1) - A(0,0)); + } +} + +/** \brief Compute logarithm of triangular matrices with size > 2. + * \details This uses a inverse scale-and-square algorithm. */ +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computeBig(const MatrixType& A, MatrixType& result) +{ + int numberOfSquareRoots = 0; + int numberOfExtraSquareRoots = 0; + int degree; + MatrixType T = A; + const RealScalar maxNormForPade = maxPadeDegree<= 5? 5.3149729967117310e-1: // single precision + maxPadeDegree<= 7? 2.6429608311114350e-1: // double precision + maxPadeDegree<= 8? 2.32777776523703892094e-1L: // extended precision + maxPadeDegree<=10? 1.05026503471351080481093652651105e-1L: // double-double + 1.1880960220216759245467951592883642e-1L; // quadruple precision + + while (true) { + RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff(); + if (normTminusI < maxNormForPade) { + degree = getPadeDegree(normTminusI); + int degree2 = getPadeDegree(normTminusI / RealScalar(2)); + if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) + break; + ++numberOfExtraSquareRoots; + } + MatrixType sqrtT; + MatrixSquareRootTriangular<MatrixType>(T).compute(sqrtT); + T = sqrtT; + ++numberOfSquareRoots; + } + + computePade(result, T, degree); + result *= pow(RealScalar(2), numberOfSquareRoots); +} + +/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = float) */ +template <typename MatrixType> +int MatrixLogarithmAtomic<MatrixType>::getPadeDegree(float normTminusI) +{ + const float maxNormForPade[] = { 2.5111573934555054e-1 /* degree = 3 */ , 4.0535837411880493e-1, + 5.3149729967117310e-1 }; + for (int degree = 3; degree <= maxPadeDegree; ++degree) + if (normTminusI <= maxNormForPade[degree - minPadeDegree]) + return degree; + assert(false); // this line should never be reached +} + +/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = double) */ +template <typename MatrixType> +int MatrixLogarithmAtomic<MatrixType>::getPadeDegree(double normTminusI) +{ + const double maxNormForPade[] = { 1.6206284795015624e-2 /* degree = 3 */ , 5.3873532631381171e-2, + 1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 }; + for (int degree = 3; degree <= maxPadeDegree; ++degree) + if (normTminusI <= maxNormForPade[degree - minPadeDegree]) + return degree; + assert(false); // this line should never be reached +} + +/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = long double) */ +template <typename MatrixType> +int MatrixLogarithmAtomic<MatrixType>::getPadeDegree(long double normTminusI) +{ +#if LDBL_MANT_DIG == 53 // double precision + const double maxNormForPade[] = { 1.6206284795015624e-2 /* degree = 3 */ , 5.3873532631381171e-2, + 1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 }; +#elif LDBL_MANT_DIG <= 64 // extended precision + const double maxNormForPade[] = { 5.48256690357782863103e-3 /* degree = 3 */, 2.34559162387971167321e-2, + 5.84603923897347449857e-2, 1.08486423756725170223e-1, 1.68385767881294446649e-1, + 2.32777776523703892094e-1 }; +#elif LDBL_MANT_DIG <= 106 // double-double + const double maxNormForPade[] = { 8.58970550342939562202529664318890e-5 /* degree = 3 */, + 9.34074328446359654039446552677759e-4, 4.26117194647672175773064114582860e-3, + 1.21546224740281848743149666560464e-2, 2.61100544998339436713088248557444e-2, + 4.66170074627052749243018566390567e-2, 7.32585144444135027565872014932387e-2, + 1.05026503471351080481093652651105e-1 }; +#else // quadruple precision + const double maxNormForPade[] = { 4.7419931187193005048501568167858103e-5 /* degree = 3 */, + 5.8853168473544560470387769480192666e-4, 2.9216120366601315391789493628113520e-3, + 8.8415758124319434347116734705174308e-3, 1.9850836029449446668518049562565291e-2, + 3.6688019729653446926585242192447447e-2, 5.9290962294020186998954055264528393e-2, + 8.6998436081634343903250580992127677e-2, 1.1880960220216759245467951592883642e-1 }; +#endif + for (int degree = 3; degree <= maxPadeDegree; ++degree) + if (normTminusI <= maxNormForPade[degree - minPadeDegree]) + return degree; + assert(false); // this line should never be reached +} + +/* \brief Compute Pade approximation to matrix logarithm */ +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade(MatrixType& result, const MatrixType& T, int degree) +{ + switch (degree) { + case 3: computePade3(result, T); break; + case 4: computePade4(result, T); break; + case 5: computePade5(result, T); break; + case 6: computePade6(result, T); break; + case 7: computePade7(result, T); break; + case 8: computePade8(result, T); break; + case 9: computePade9(result, T); break; + case 10: computePade10(result, T); break; + case 11: computePade11(result, T); break; + default: assert(false); // should never happen + } +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade3(MatrixType& result, const MatrixType& T) +{ + const int degree = 3; + const RealScalar nodes[] = { 0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L, + 0.8872983346207416885179265399782400L }; + const RealScalar weights[] = { 0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L, + 0.2777777777777777777777777777777778L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade4(MatrixType& result, const MatrixType& T) +{ + const int degree = 4; + const RealScalar nodes[] = { 0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L, + 0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L }; + const RealScalar weights[] = { 0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L, + 0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade5(MatrixType& result, const MatrixType& T) +{ + const int degree = 5; + const RealScalar nodes[] = { 0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L, + 0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L, + 0.9530899229693319963988134391496965L }; + const RealScalar weights[] = { 0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L, + 0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L, + 0.1184634425280945437571320203599587L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade6(MatrixType& result, const MatrixType& T) +{ + const int degree = 6; + const RealScalar nodes[] = { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L, + 0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L, + 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L }; + const RealScalar weights[] = { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L, + 0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L, + 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade7(MatrixType& result, const MatrixType& T) +{ + const int degree = 7; + const RealScalar nodes[] = { 0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L, + 0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L, + 0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L, + 0.9745539561713792622630948420239256L }; + const RealScalar weights[] = { 0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L, + 0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L, + 0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L, + 0.0647424830844348466353057163395410L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade8(MatrixType& result, const MatrixType& T) +{ + const int degree = 8; + const RealScalar nodes[] = { 0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L, + 0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L, + 0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L, + 0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L }; + const RealScalar weights[] = { 0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L, + 0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L, + 0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L, + 0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade9(MatrixType& result, const MatrixType& T) +{ + const int degree = 9; + const RealScalar nodes[] = { 0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L, + 0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L, + 0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L, + 0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L, + 0.9840801197538130449177881014518364L }; + const RealScalar weights[] = { 0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L, + 0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L, + 0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L, + 0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L, + 0.0406371941807872059859460790552618L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade10(MatrixType& result, const MatrixType& T) +{ + const int degree = 10; + const RealScalar nodes[] = { 0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L, + 0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L, + 0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L, + 0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L, + 0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L }; + const RealScalar weights[] = { 0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L, + 0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L, + 0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L, + 0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L, + 0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +template <typename MatrixType> +void MatrixLogarithmAtomic<MatrixType>::computePade11(MatrixType& result, const MatrixType& T) +{ + const int degree = 11; + const RealScalar nodes[] = { 0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L, + 0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L, + 0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L, + 0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L, + 0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L, + 0.9891143290730284964019690005614287L }; + const RealScalar weights[] = { 0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L, + 0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L, + 0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L, + 0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L, + 0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L, + 0.0278342835580868332413768602212743L }; + assert(degree <= maxPadeDegree); + MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); + result.setZero(T.rows(), T.rows()); + for (int k = 0; k < degree; ++k) + result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI) + .template triangularView<Upper>().solve(TminusI); +} + +/** \ingroup MatrixFunctions_Module + * + * \brief Proxy for the matrix logarithm of some matrix (expression). + * + * \tparam Derived Type of the argument to the matrix function. + * + * This class holds the argument to the matrix function until it is + * assigned or evaluated for some other reason (so the argument + * should not be changed in the meantime). It is the return type of + * matrixBase::matrixLogarithm() and most of the time this is the + * only way it is used. + */ +template<typename Derived> class MatrixLogarithmReturnValue +: public ReturnByValue<MatrixLogarithmReturnValue<Derived> > +{ +public: + + typedef typename Derived::Scalar Scalar; + typedef typename Derived::Index Index; + + /** \brief Constructor. + * + * \param[in] A %Matrix (expression) forming the argument of the matrix logarithm. + */ + MatrixLogarithmReturnValue(const Derived& A) : m_A(A) { } + + /** \brief Compute the matrix logarithm. + * + * \param[out] result Logarithm of \p A, where \A is as specified in the constructor. + */ + template <typename ResultType> + inline void evalTo(ResultType& result) const + { + typedef typename Derived::PlainObject PlainObject; + typedef internal::traits<PlainObject> Traits; + static const int RowsAtCompileTime = Traits::RowsAtCompileTime; + static const int ColsAtCompileTime = Traits::ColsAtCompileTime; + static const int Options = PlainObject::Options; + typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar; + typedef Matrix<ComplexScalar, Dynamic, Dynamic, Options, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType; + typedef MatrixLogarithmAtomic<DynMatrixType> AtomicType; + AtomicType atomic; + + const PlainObject Aevaluated = m_A.eval(); + MatrixFunction<PlainObject, AtomicType> mf(Aevaluated, atomic); + mf.compute(result); + } + + Index rows() const { return m_A.rows(); } + Index cols() const { return m_A.cols(); } + +private: + typename internal::nested<Derived>::type m_A; + + MatrixLogarithmReturnValue& operator=(const MatrixLogarithmReturnValue&); +}; + +namespace internal { + template<typename Derived> + struct traits<MatrixLogarithmReturnValue<Derived> > + { + typedef typename Derived::PlainObject ReturnType; + }; +} + + +/********** MatrixBase method **********/ + + +template <typename Derived> +const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const +{ + eigen_assert(rows() == cols()); + return MatrixLogarithmReturnValue<Derived>(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_LOGARITHM diff --git a/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h new file mode 100644 index 000000000..10319fa17 --- /dev/null +++ b/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h @@ -0,0 +1,484 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Jitse Niesen <jitse@maths.leeds.ac.uk> +// +// 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_SQUARE_ROOT +#define EIGEN_MATRIX_SQUARE_ROOT + +namespace Eigen { + +/** \ingroup MatrixFunctions_Module + * \brief Class for computing matrix square roots of upper quasi-triangular matrices. + * \tparam MatrixType type of the argument of the matrix square root, + * expected to be an instantiation of the Matrix class template. + * + * This class computes the square root of the upper quasi-triangular + * matrix stored in the upper Hessenberg part of the matrix passed to + * the constructor. + * + * \sa MatrixSquareRoot, MatrixSquareRootTriangular + */ +template <typename MatrixType> +class MatrixSquareRootQuasiTriangular +{ + public: + + /** \brief Constructor. + * + * \param[in] A upper quasi-triangular matrix whose square root + * is to be computed. + * + * The class stores a reference to \p A, so it should not be + * changed (or destroyed) before compute() is called. + */ + MatrixSquareRootQuasiTriangular(const MatrixType& A) + : m_A(A) + { + eigen_assert(A.rows() == A.cols()); + } + + /** \brief Compute the matrix square root + * + * \param[out] result square root of \p A, as specified in the constructor. + * + * Only the upper Hessenberg part of \p result is updated, the + * rest is not touched. See MatrixBase::sqrt() for details on + * how this computation is implemented. + */ + template <typename ResultType> void compute(ResultType &result); + + private: + typedef typename MatrixType::Index Index; + typedef typename MatrixType::Scalar Scalar; + + void computeDiagonalPartOfSqrt(MatrixType& sqrtT, const MatrixType& T); + void computeOffDiagonalPartOfSqrt(MatrixType& sqrtT, const MatrixType& T); + void compute2x2diagonalBlock(MatrixType& sqrtT, const MatrixType& T, typename MatrixType::Index i); + void compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j); + void compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j); + void compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j); + void compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j); + + template <typename SmallMatrixType> + static void solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A, + const SmallMatrixType& B, const SmallMatrixType& C); + + const MatrixType& m_A; +}; + +template <typename MatrixType> +template <typename ResultType> +void MatrixSquareRootQuasiTriangular<MatrixType>::compute(ResultType &result) +{ + // Compute Schur decomposition of m_A + const RealSchur<MatrixType> schurOfA(m_A); + const MatrixType& T = schurOfA.matrixT(); + const MatrixType& U = schurOfA.matrixU(); + + // Compute square root of T + MatrixType sqrtT = MatrixType::Zero(m_A.rows(), m_A.rows()); + computeDiagonalPartOfSqrt(sqrtT, T); + computeOffDiagonalPartOfSqrt(sqrtT, T); + + // Compute square root of m_A + result = U * sqrtT * U.adjoint(); +} + +// pre: T is quasi-upper-triangular and sqrtT is a zero matrix of the same size +// post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T +template <typename MatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType>::computeDiagonalPartOfSqrt(MatrixType& sqrtT, + const MatrixType& T) +{ + const Index size = m_A.rows(); + for (Index i = 0; i < size; i++) { + if (i == size - 1 || T.coeff(i+1, i) == 0) { + eigen_assert(T(i,i) > 0); + sqrtT.coeffRef(i,i) = internal::sqrt(T.coeff(i,i)); + } + else { + compute2x2diagonalBlock(sqrtT, T, i); + ++i; + } + } +} + +// pre: T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T. +// post: sqrtT is the square root of T. +template <typename MatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType>::computeOffDiagonalPartOfSqrt(MatrixType& sqrtT, + const MatrixType& T) +{ + const Index size = m_A.rows(); + for (Index j = 1; j < size; j++) { + if (T.coeff(j, j-1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block + continue; + for (Index i = j-1; i >= 0; i--) { + if (i > 0 && T.coeff(i, i-1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block + continue; + bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0); + bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0); + if (iBlockIs2x2 && jBlockIs2x2) + compute2x2offDiagonalBlock(sqrtT, T, i, j); + else if (iBlockIs2x2 && !jBlockIs2x2) + compute2x1offDiagonalBlock(sqrtT, T, i, j); + else if (!iBlockIs2x2 && jBlockIs2x2) + compute1x2offDiagonalBlock(sqrtT, T, i, j); + else if (!iBlockIs2x2 && !jBlockIs2x2) + compute1x1offDiagonalBlock(sqrtT, T, i, j); + } + } +} + +// pre: T.block(i,i,2,2) has complex conjugate eigenvalues +// post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2) +template <typename MatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType> + ::compute2x2diagonalBlock(MatrixType& sqrtT, const MatrixType& T, typename MatrixType::Index i) +{ + // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere + // in EigenSolver. If we expose it, we could call it directly from here. + Matrix<Scalar,2,2> block = T.template block<2,2>(i,i); + EigenSolver<Matrix<Scalar,2,2> > es(block); + sqrtT.template block<2,2>(i,i) + = (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real(); +} + +// pre: block structure of T is such that (i,j) is a 1x1 block, +// all blocks of sqrtT to left of and below (i,j) are correct +// post: sqrtT(i,j) has the correct value +template <typename MatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType> + ::compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j) +{ + Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value(); + sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j)); +} + +// similar to compute1x1offDiagonalBlock() +template <typename MatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType> + ::compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j) +{ + Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j); + if (j-i > 1) + rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2); + Matrix<Scalar,2,2> A = sqrtT.coeff(i,i) * Matrix<Scalar,2,2>::Identity(); + A += sqrtT.template block<2,2>(j,j).transpose(); + sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose()); +} + +// similar to compute1x1offDiagonalBlock() +template <typename MatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType> + ::compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j) +{ + Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j); + if (j-i > 2) + rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1); + Matrix<Scalar,2,2> A = sqrtT.coeff(j,j) * Matrix<Scalar,2,2>::Identity(); + A += sqrtT.template block<2,2>(i,i); + sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs); +} + +// similar to compute1x1offDiagonalBlock() +template <typename MatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType> + ::compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, + typename MatrixType::Index i, typename MatrixType::Index j) +{ + Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i); + Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j); + Matrix<Scalar,2,2> C = T.template block<2,2>(i,j); + if (j-i > 2) + C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2); + Matrix<Scalar,2,2> X; + solveAuxiliaryEquation(X, A, B, C); + sqrtT.template block<2,2>(i,j) = X; +} + +// solves the equation A X + X B = C where all matrices are 2-by-2 +template <typename MatrixType> +template <typename SmallMatrixType> +void MatrixSquareRootQuasiTriangular<MatrixType> + ::solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A, + const SmallMatrixType& B, const SmallMatrixType& C) +{ + EIGEN_STATIC_ASSERT((internal::is_same<SmallMatrixType, Matrix<Scalar,2,2> >::value), + EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT); + + Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero(); + coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0); + coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1); + coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0); + coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1); + coeffMatrix.coeffRef(0,1) = B.coeff(1,0); + coeffMatrix.coeffRef(0,2) = A.coeff(0,1); + coeffMatrix.coeffRef(1,0) = B.coeff(0,1); + coeffMatrix.coeffRef(1,3) = A.coeff(0,1); + coeffMatrix.coeffRef(2,0) = A.coeff(1,0); + coeffMatrix.coeffRef(2,3) = B.coeff(1,0); + coeffMatrix.coeffRef(3,1) = A.coeff(1,0); + coeffMatrix.coeffRef(3,2) = B.coeff(0,1); + + Matrix<Scalar,4,1> rhs; + rhs.coeffRef(0) = C.coeff(0,0); + rhs.coeffRef(1) = C.coeff(0,1); + rhs.coeffRef(2) = C.coeff(1,0); + rhs.coeffRef(3) = C.coeff(1,1); + + Matrix<Scalar,4,1> result; + result = coeffMatrix.fullPivLu().solve(rhs); + + X.coeffRef(0,0) = result.coeff(0); + X.coeffRef(0,1) = result.coeff(1); + X.coeffRef(1,0) = result.coeff(2); + X.coeffRef(1,1) = result.coeff(3); +} + + +/** \ingroup MatrixFunctions_Module + * \brief Class for computing matrix square roots of upper triangular matrices. + * \tparam MatrixType type of the argument of the matrix square root, + * expected to be an instantiation of the Matrix class template. + * + * This class computes the square root of the upper triangular matrix + * stored in the upper triangular part (including the diagonal) of + * the matrix passed to the constructor. + * + * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular + */ +template <typename MatrixType> +class MatrixSquareRootTriangular +{ + public: + MatrixSquareRootTriangular(const MatrixType& A) + : m_A(A) + { + eigen_assert(A.rows() == A.cols()); + } + + /** \brief Compute the matrix square root + * + * \param[out] result square root of \p A, as specified in the constructor. + * + * Only the upper triangular part (including the diagonal) of + * \p result is updated, the rest is not touched. See + * MatrixBase::sqrt() for details on how this computation is + * implemented. + */ + template <typename ResultType> void compute(ResultType &result); + + private: + const MatrixType& m_A; +}; + +template <typename MatrixType> +template <typename ResultType> +void MatrixSquareRootTriangular<MatrixType>::compute(ResultType &result) +{ + // Compute Schur decomposition of m_A + const ComplexSchur<MatrixType> schurOfA(m_A); + const MatrixType& T = schurOfA.matrixT(); + const MatrixType& U = schurOfA.matrixU(); + + // Compute square root of T and store it in upper triangular part of result + // This uses that the square root of triangular matrices can be computed directly. + result.resize(m_A.rows(), m_A.cols()); + typedef typename MatrixType::Index Index; + for (Index i = 0; i < m_A.rows(); i++) { + result.coeffRef(i,i) = internal::sqrt(T.coeff(i,i)); + } + for (Index j = 1; j < m_A.cols(); j++) { + for (Index i = j-1; i >= 0; i--) { + typedef typename MatrixType::Scalar Scalar; + // if i = j-1, then segment has length 0 so tmp = 0 + Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value(); + // denominator may be zero if original matrix is singular + result.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j)); + } + } + + // Compute square root of m_A as U * result * U.adjoint() + MatrixType tmp; + tmp.noalias() = U * result.template triangularView<Upper>(); + result.noalias() = tmp * U.adjoint(); +} + + +/** \ingroup MatrixFunctions_Module + * \brief Class for computing matrix square roots of general matrices. + * \tparam MatrixType type of the argument of the matrix square root, + * expected to be an instantiation of the Matrix class template. + * + * \sa MatrixSquareRootTriangular, MatrixSquareRootQuasiTriangular, MatrixBase::sqrt() + */ +template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex> +class MatrixSquareRoot +{ + public: + + /** \brief Constructor. + * + * \param[in] A matrix whose square root is to be computed. + * + * The class stores a reference to \p A, so it should not be + * changed (or destroyed) before compute() is called. + */ + MatrixSquareRoot(const MatrixType& A); + + /** \brief Compute the matrix square root + * + * \param[out] result square root of \p A, as specified in the constructor. + * + * See MatrixBase::sqrt() for details on how this computation is + * implemented. + */ + template <typename ResultType> void compute(ResultType &result); +}; + + +// ********** Partial specialization for real matrices ********** + +template <typename MatrixType> +class MatrixSquareRoot<MatrixType, 0> +{ + public: + + MatrixSquareRoot(const MatrixType& A) + : m_A(A) + { + eigen_assert(A.rows() == A.cols()); + } + + template <typename ResultType> void compute(ResultType &result) + { + // Compute Schur decomposition of m_A + const RealSchur<MatrixType> schurOfA(m_A); + const MatrixType& T = schurOfA.matrixT(); + const MatrixType& U = schurOfA.matrixU(); + + // Compute square root of T + MatrixSquareRootQuasiTriangular<MatrixType> tmp(T); + MatrixType sqrtT = MatrixType::Zero(m_A.rows(), m_A.rows()); + tmp.compute(sqrtT); + + // Compute square root of m_A + result = U * sqrtT * U.adjoint(); + } + + private: + const MatrixType& m_A; +}; + + +// ********** Partial specialization for complex matrices ********** + +template <typename MatrixType> +class MatrixSquareRoot<MatrixType, 1> +{ + public: + + MatrixSquareRoot(const MatrixType& A) + : m_A(A) + { + eigen_assert(A.rows() == A.cols()); + } + + template <typename ResultType> void compute(ResultType &result) + { + // Compute Schur decomposition of m_A + const ComplexSchur<MatrixType> schurOfA(m_A); + const MatrixType& T = schurOfA.matrixT(); + const MatrixType& U = schurOfA.matrixU(); + + // Compute square root of T + MatrixSquareRootTriangular<MatrixType> tmp(T); + MatrixType sqrtT = MatrixType::Zero(m_A.rows(), m_A.rows()); + tmp.compute(sqrtT); + + // Compute square root of m_A + result = U * sqrtT * U.adjoint(); + } + + private: + const MatrixType& m_A; +}; + + +/** \ingroup MatrixFunctions_Module + * + * \brief Proxy for the matrix square root of some matrix (expression). + * + * \tparam Derived Type of the argument to the matrix square root. + * + * This class holds the argument to the matrix square root until it + * is assigned or evaluated for some other reason (so the argument + * should not be changed in the meantime). It is the return type of + * MatrixBase::sqrt() and most of the time this is the only way it is + * used. + */ +template<typename Derived> class MatrixSquareRootReturnValue +: public ReturnByValue<MatrixSquareRootReturnValue<Derived> > +{ + typedef typename Derived::Index Index; + public: + /** \brief Constructor. + * + * \param[in] src %Matrix (expression) forming the argument of the + * matrix square root. + */ + MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { } + + /** \brief Compute the matrix square root. + * + * \param[out] result the matrix square root of \p src in the + * constructor. + */ + template <typename ResultType> + inline void evalTo(ResultType& result) const + { + const typename Derived::PlainObject srcEvaluated = m_src.eval(); + MatrixSquareRoot<typename Derived::PlainObject> me(srcEvaluated); + me.compute(result); + } + + Index rows() const { return m_src.rows(); } + Index cols() const { return m_src.cols(); } + + protected: + const Derived& m_src; + private: + MatrixSquareRootReturnValue& operator=(const MatrixSquareRootReturnValue&); +}; + +namespace internal { +template<typename Derived> +struct traits<MatrixSquareRootReturnValue<Derived> > +{ + typedef typename Derived::PlainObject ReturnType; +}; +} + +template <typename Derived> +const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const +{ + eigen_assert(rows() == cols()); + return MatrixSquareRootReturnValue<Derived>(derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_MATRIX_FUNCTION diff --git a/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/unsupported/Eigen/src/MatrixFunctions/StemFunction.h new file mode 100644 index 000000000..724e55c1d --- /dev/null +++ b/unsupported/Eigen/src/MatrixFunctions/StemFunction.h @@ -0,0 +1,105 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk> +// +// 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_STEM_FUNCTION +#define EIGEN_STEM_FUNCTION + +namespace Eigen { + +/** \ingroup MatrixFunctions_Module + * \brief Stem functions corresponding to standard mathematical functions. + */ +template <typename Scalar> +class StdStemFunctions +{ + public: + + /** \brief The exponential function (and its derivatives). */ + static Scalar exp(Scalar x, int) + { + return std::exp(x); + } + + /** \brief Cosine (and its derivatives). */ + static Scalar cos(Scalar x, int n) + { + Scalar res; + switch (n % 4) { + case 0: + res = std::cos(x); + break; + case 1: + res = -std::sin(x); + break; + case 2: + res = -std::cos(x); + break; + case 3: + res = std::sin(x); + break; + } + return res; + } + + /** \brief Sine (and its derivatives). */ + static Scalar sin(Scalar x, int n) + { + Scalar res; + switch (n % 4) { + case 0: + res = std::sin(x); + break; + case 1: + res = std::cos(x); + break; + case 2: + res = -std::sin(x); + break; + case 3: + res = -std::cos(x); + break; + } + return res; + } + + /** \brief Hyperbolic cosine (and its derivatives). */ + static Scalar cosh(Scalar x, int n) + { + Scalar res; + switch (n % 2) { + case 0: + res = std::cosh(x); + break; + case 1: + res = std::sinh(x); + break; + } + return res; + } + + /** \brief Hyperbolic sine (and its derivatives). */ + static Scalar sinh(Scalar x, int n) + { + Scalar res; + switch (n % 2) { + case 0: + res = std::sinh(x); + break; + case 1: + res = std::cosh(x); + break; + } + return res; + } + +}; // end of class StdStemFunctions + +} // end namespace Eigen + +#endif // EIGEN_STEM_FUNCTION diff --git a/unsupported/Eigen/src/MoreVectorization/CMakeLists.txt b/unsupported/Eigen/src/MoreVectorization/CMakeLists.txt new file mode 100644 index 000000000..1b887cc8e --- /dev/null +++ b/unsupported/Eigen/src/MoreVectorization/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_MoreVectorization_SRCS "*.h") + +INSTALL(FILES + ${Eigen_MoreVectorization_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/MoreVectorization COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/MoreVectorization/MathFunctions.h b/unsupported/Eigen/src/MoreVectorization/MathFunctions.h new file mode 100644 index 000000000..63cb28dea --- /dev/null +++ b/unsupported/Eigen/src/MoreVectorization/MathFunctions.h @@ -0,0 +1,95 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Rohit Garg <rpg.314@gmail.com> +// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.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_MOREVECTORIZATION_MATHFUNCTIONS_H +#define EIGEN_MOREVECTORIZATION_MATHFUNCTIONS_H + +namespace Eigen { + +namespace internal { + +/** \internal \returns the arcsin of \a a (coeff-wise) */ +template<typename Packet> inline static Packet pasin(Packet a) { return std::asin(a); } + +#ifdef EIGEN_VECTORIZE_SSE + +template<> EIGEN_DONT_INLINE Packet4f pasin(Packet4f x) +{ + _EIGEN_DECLARE_CONST_Packet4f(half, 0.5); + _EIGEN_DECLARE_CONST_Packet4f(minus_half, -0.5); + _EIGEN_DECLARE_CONST_Packet4f(3half, 1.5); + + _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(sign_mask, 0x80000000); + + _EIGEN_DECLARE_CONST_Packet4f(pi, 3.141592654); + _EIGEN_DECLARE_CONST_Packet4f(pi_over_2, 3.141592654*0.5); + + _EIGEN_DECLARE_CONST_Packet4f(asin1, 4.2163199048E-2); + _EIGEN_DECLARE_CONST_Packet4f(asin2, 2.4181311049E-2); + _EIGEN_DECLARE_CONST_Packet4f(asin3, 4.5470025998E-2); + _EIGEN_DECLARE_CONST_Packet4f(asin4, 7.4953002686E-2); + _EIGEN_DECLARE_CONST_Packet4f(asin5, 1.6666752422E-1); + + Packet4f a = pabs(x);//got the absolute value + + Packet4f sign_bit= _mm_and_ps(x, p4f_sign_mask);//extracted the sign bit + + Packet4f z1,z2;//will need them during computation + + +//will compute the two branches for asin +//so first compare with half + + Packet4f branch_mask= _mm_cmpgt_ps(a, p4f_half);//this is to select which branch to take +//both will be taken, and finally results will be merged +//the branch for values >0.5 + + { +//the core series expansion + z1=pmadd(p4f_minus_half,a,p4f_half); + Packet4f x1=psqrt(z1); + Packet4f s1=pmadd(p4f_asin1, z1, p4f_asin2); + Packet4f s2=pmadd(s1, z1, p4f_asin3); + Packet4f s3=pmadd(s2,z1, p4f_asin4); + Packet4f s4=pmadd(s3,z1, p4f_asin5); + Packet4f temp=pmul(s4,z1);//not really a madd but a mul by z so that the next term can be a madd + z1=pmadd(temp,x1,x1); + z1=padd(z1,z1); + z1=psub(p4f_pi_over_2,z1); + } + + { +//the core series expansion + Packet4f x2=a; + z2=pmul(x2,x2); + Packet4f s1=pmadd(p4f_asin1, z2, p4f_asin2); + Packet4f s2=pmadd(s1, z2, p4f_asin3); + Packet4f s3=pmadd(s2,z2, p4f_asin4); + Packet4f s4=pmadd(s3,z2, p4f_asin5); + Packet4f temp=pmul(s4,z2);//not really a madd but a mul by z so that the next term can be a madd + z2=pmadd(temp,x2,x2); + } + +/* select the correct result from the two branch evaluations */ + z1 = _mm_and_ps(branch_mask, z1); + z2 = _mm_andnot_ps(branch_mask, z2); + Packet4f z = _mm_or_ps(z1,z2); + +/* update the sign */ + return _mm_xor_ps(z, sign_bit); +} + +#endif // EIGEN_VECTORIZE_SSE + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_MOREVECTORIZATION_MATHFUNCTIONS_H diff --git a/unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt b/unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt new file mode 100644 index 000000000..9322ddadf --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_NonLinearOptimization_SRCS "*.h") + +INSTALL(FILES + ${Eigen_NonLinearOptimization_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/NonLinearOptimization COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h new file mode 100644 index 000000000..d9ce4eab6 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h @@ -0,0 +1,596 @@ +// -*- coding: utf-8 +// vim: set fileencoding=utf-8 + +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// 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_HYBRIDNONLINEARSOLVER_H +#define EIGEN_HYBRIDNONLINEARSOLVER_H + +namespace Eigen { + +namespace HybridNonLinearSolverSpace { + enum Status { + Running = -1, + ImproperInputParameters = 0, + RelativeErrorTooSmall = 1, + TooManyFunctionEvaluation = 2, + TolTooSmall = 3, + NotMakingProgressJacobian = 4, + NotMakingProgressIterations = 5, + UserAsked = 6 + }; +} + +/** + * \ingroup NonLinearOptimization_Module + * \brief Finds a zero of a system of n + * nonlinear functions in n variables by a modification of the Powell + * hybrid method ("dogleg"). + * + * The user must provide a subroutine which calculates the + * functions. The Jacobian is either provided by the user, or approximated + * using a forward-difference method. + * + */ +template<typename FunctorType, typename Scalar=double> +class HybridNonLinearSolver +{ +public: + typedef DenseIndex Index; + + HybridNonLinearSolver(FunctorType &_functor) + : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=false;} + + struct Parameters { + Parameters() + : factor(Scalar(100.)) + , maxfev(1000) + , xtol(internal::sqrt(NumTraits<Scalar>::epsilon())) + , nb_of_subdiagonals(-1) + , nb_of_superdiagonals(-1) + , epsfcn(Scalar(0.)) {} + Scalar factor; + Index maxfev; // maximum number of function evaluation + Scalar xtol; + Index nb_of_subdiagonals; + Index nb_of_superdiagonals; + Scalar epsfcn; + }; + typedef Matrix< Scalar, Dynamic, 1 > FVectorType; + typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType; + /* TODO: if eigen provides a triangular storage, use it here */ + typedef Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType; + + HybridNonLinearSolverSpace::Status hybrj1( + FVectorType &x, + const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) + ); + + HybridNonLinearSolverSpace::Status solveInit(FVectorType &x); + HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x); + HybridNonLinearSolverSpace::Status solve(FVectorType &x); + + HybridNonLinearSolverSpace::Status hybrd1( + FVectorType &x, + const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) + ); + + HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x); + HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x); + HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x); + + void resetParameters(void) { parameters = Parameters(); } + Parameters parameters; + FVectorType fvec, qtf, diag; + JacobianType fjac; + UpperTriangularType R; + Index nfev; + Index njev; + Index iter; + Scalar fnorm; + bool useExternalScaling; +private: + FunctorType &functor; + Index n; + Scalar sum; + bool sing; + Scalar temp; + Scalar delta; + bool jeval; + Index ncsuc; + Scalar ratio; + Scalar pnorm, xnorm, fnorm1; + Index nslow1, nslow2; + Index ncfail; + Scalar actred, prered; + FVectorType wa1, wa2, wa3, wa4; + + HybridNonLinearSolver& operator=(const HybridNonLinearSolver&); +}; + + + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::hybrj1( + FVectorType &x, + const Scalar tol + ) +{ + n = x.size(); + + /* check the input parameters for errors. */ + if (n <= 0 || tol < 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + resetParameters(); + parameters.maxfev = 100*(n+1); + parameters.xtol = tol; + diag.setConstant(n, 1.); + useExternalScaling = true; + return solve(x); +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x) +{ + n = x.size(); + + wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); + fvec.resize(n); + qtf.resize(n); + fjac.resize(n, n); + if (!useExternalScaling) + diag.resize(n); + assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); + + /* Function Body */ + nfev = 0; + njev = 0; + + /* check the input parameters for errors. */ + if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. ) + return HybridNonLinearSolverSpace::ImproperInputParameters; + if (useExternalScaling) + for (Index j = 0; j < n; ++j) + if (diag[j] <= 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + nfev = 1; + if ( functor(x, fvec) < 0) + return HybridNonLinearSolverSpace::UserAsked; + fnorm = fvec.stableNorm(); + + /* initialize iteration counter and monitors. */ + iter = 1; + ncsuc = 0; + ncfail = 0; + nslow1 = 0; + nslow2 = 0; + + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x) +{ + assert(x.size()==n); // check the caller is not cheating us + + Index j; + std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); + + jeval = true; + + /* calculate the jacobian matrix. */ + if ( functor.df(x, fjac) < 0) + return HybridNonLinearSolverSpace::UserAsked; + ++njev; + + wa2 = fjac.colwise().blueNorm(); + + /* on the first iteration and if external scaling is not used, scale according */ + /* to the norms of the columns of the initial jacobian. */ + if (iter == 1) { + if (!useExternalScaling) + for (j = 0; j < n; ++j) + diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ + xnorm = diag.cwiseProduct(x).stableNorm(); + delta = parameters.factor * xnorm; + if (delta == 0.) + delta = parameters.factor; + } + + /* compute the qr factorization of the jacobian. */ + HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: + + /* copy the triangular factor of the qr factorization into r. */ + R = qrfac.matrixQR(); + + /* accumulate the orthogonal factor in fjac. */ + fjac = qrfac.householderQ(); + + /* form (q transpose)*fvec and store in qtf. */ + qtf = fjac.transpose() * fvec; + + /* rescale if necessary. */ + if (!useExternalScaling) + diag = diag.cwiseMax(wa2); + + while (true) { + /* determine the direction p. */ + internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); + + /* store the direction p and x + p. calculate the norm of p. */ + wa1 = -wa1; + wa2 = x + wa1; + pnorm = diag.cwiseProduct(wa1).stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + if (iter == 1) + delta = (std::min)(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + if ( functor(wa2, wa4) < 0) + return HybridNonLinearSolverSpace::UserAsked; + ++nfev; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + actred = -1.; + if (fnorm1 < fnorm) /* Computing 2nd power */ + actred = 1. - internal::abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction. */ + wa3 = R.template triangularView<Upper>()*wa1 + qtf; + temp = wa3.stableNorm(); + prered = 0.; + if (temp < fnorm) /* Computing 2nd power */ + prered = 1. - internal::abs2(temp / fnorm); + + /* compute the ratio of the actual to the predicted reduction. */ + ratio = 0.; + if (prered > 0.) + ratio = actred / prered; + + /* update the step bound. */ + if (ratio < Scalar(.1)) { + ncsuc = 0; + ++ncfail; + delta = Scalar(.5) * delta; + } else { + ncfail = 0; + ++ncsuc; + if (ratio >= Scalar(.5) || ncsuc > 1) + delta = (std::max)(delta, pnorm / Scalar(.5)); + if (internal::abs(ratio - 1.) <= Scalar(.1)) { + delta = pnorm / Scalar(.5); + } + } + + /* test for successful iteration. */ + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwiseProduct(x); + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* determine the progress of the iteration. */ + ++nslow1; + if (actred >= Scalar(.001)) + nslow1 = 0; + if (jeval) + ++nslow2; + if (actred >= Scalar(.1)) + nslow2 = 0; + + /* test for convergence. */ + if (delta <= parameters.xtol * xnorm || fnorm == 0.) + return HybridNonLinearSolverSpace::RelativeErrorTooSmall; + + /* tests for termination and stringent tolerances. */ + if (nfev >= parameters.maxfev) + return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; + if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) + return HybridNonLinearSolverSpace::TolTooSmall; + if (nslow2 == 5) + return HybridNonLinearSolverSpace::NotMakingProgressJacobian; + if (nslow1 == 10) + return HybridNonLinearSolverSpace::NotMakingProgressIterations; + + /* criterion for recalculating jacobian. */ + if (ncfail == 2) + break; // leave inner loop and go for the next outer loop iteration + + /* calculate the rank one modification to the jacobian */ + /* and update qtf if necessary. */ + wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); + wa2 = fjac.transpose() * wa4; + if (ratio >= Scalar(1e-4)) + qtf = wa2; + wa2 = (wa2-wa3)/pnorm; + + /* compute the qr factorization of the updated jacobian. */ + internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); + internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); + internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); + + jeval = false; + } + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x) +{ + HybridNonLinearSolverSpace::Status status = solveInit(x); + if (status==HybridNonLinearSolverSpace::ImproperInputParameters) + return status; + while (status==HybridNonLinearSolverSpace::Running) + status = solveOneStep(x); + return status; +} + + + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::hybrd1( + FVectorType &x, + const Scalar tol + ) +{ + n = x.size(); + + /* check the input parameters for errors. */ + if (n <= 0 || tol < 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + resetParameters(); + parameters.maxfev = 200*(n+1); + parameters.xtol = tol; + + diag.setConstant(n, 1.); + useExternalScaling = true; + return solveNumericalDiff(x); +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x) +{ + n = x.size(); + + if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; + if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; + + wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); + qtf.resize(n); + fjac.resize(n, n); + fvec.resize(n); + if (!useExternalScaling) + diag.resize(n); + assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); + + /* Function Body */ + nfev = 0; + njev = 0; + + /* check the input parameters for errors. */ + if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. ) + return HybridNonLinearSolverSpace::ImproperInputParameters; + if (useExternalScaling) + for (Index j = 0; j < n; ++j) + if (diag[j] <= 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + nfev = 1; + if ( functor(x, fvec) < 0) + return HybridNonLinearSolverSpace::UserAsked; + fnorm = fvec.stableNorm(); + + /* initialize iteration counter and monitors. */ + iter = 1; + ncsuc = 0; + ncfail = 0; + nslow1 = 0; + nslow2 = 0; + + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x) +{ + assert(x.size()==n); // check the caller is not cheating us + + Index j; + std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); + + jeval = true; + if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; + if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; + + /* calculate the jacobian matrix. */ + if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0) + return HybridNonLinearSolverSpace::UserAsked; + nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n); + + wa2 = fjac.colwise().blueNorm(); + + /* on the first iteration and if external scaling is not used, scale according */ + /* to the norms of the columns of the initial jacobian. */ + if (iter == 1) { + if (!useExternalScaling) + for (j = 0; j < n; ++j) + diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ + xnorm = diag.cwiseProduct(x).stableNorm(); + delta = parameters.factor * xnorm; + if (delta == 0.) + delta = parameters.factor; + } + + /* compute the qr factorization of the jacobian. */ + HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: + + /* copy the triangular factor of the qr factorization into r. */ + R = qrfac.matrixQR(); + + /* accumulate the orthogonal factor in fjac. */ + fjac = qrfac.householderQ(); + + /* form (q transpose)*fvec and store in qtf. */ + qtf = fjac.transpose() * fvec; + + /* rescale if necessary. */ + if (!useExternalScaling) + diag = diag.cwiseMax(wa2); + + while (true) { + /* determine the direction p. */ + internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); + + /* store the direction p and x + p. calculate the norm of p. */ + wa1 = -wa1; + wa2 = x + wa1; + pnorm = diag.cwiseProduct(wa1).stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + if (iter == 1) + delta = (std::min)(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + if ( functor(wa2, wa4) < 0) + return HybridNonLinearSolverSpace::UserAsked; + ++nfev; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + actred = -1.; + if (fnorm1 < fnorm) /* Computing 2nd power */ + actred = 1. - internal::abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction. */ + wa3 = R.template triangularView<Upper>()*wa1 + qtf; + temp = wa3.stableNorm(); + prered = 0.; + if (temp < fnorm) /* Computing 2nd power */ + prered = 1. - internal::abs2(temp / fnorm); + + /* compute the ratio of the actual to the predicted reduction. */ + ratio = 0.; + if (prered > 0.) + ratio = actred / prered; + + /* update the step bound. */ + if (ratio < Scalar(.1)) { + ncsuc = 0; + ++ncfail; + delta = Scalar(.5) * delta; + } else { + ncfail = 0; + ++ncsuc; + if (ratio >= Scalar(.5) || ncsuc > 1) + delta = (std::max)(delta, pnorm / Scalar(.5)); + if (internal::abs(ratio - 1.) <= Scalar(.1)) { + delta = pnorm / Scalar(.5); + } + } + + /* test for successful iteration. */ + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwiseProduct(x); + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* determine the progress of the iteration. */ + ++nslow1; + if (actred >= Scalar(.001)) + nslow1 = 0; + if (jeval) + ++nslow2; + if (actred >= Scalar(.1)) + nslow2 = 0; + + /* test for convergence. */ + if (delta <= parameters.xtol * xnorm || fnorm == 0.) + return HybridNonLinearSolverSpace::RelativeErrorTooSmall; + + /* tests for termination and stringent tolerances. */ + if (nfev >= parameters.maxfev) + return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; + if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) + return HybridNonLinearSolverSpace::TolTooSmall; + if (nslow2 == 5) + return HybridNonLinearSolverSpace::NotMakingProgressJacobian; + if (nslow1 == 10) + return HybridNonLinearSolverSpace::NotMakingProgressIterations; + + /* criterion for recalculating jacobian. */ + if (ncfail == 2) + break; // leave inner loop and go for the next outer loop iteration + + /* calculate the rank one modification to the jacobian */ + /* and update qtf if necessary. */ + wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); + wa2 = fjac.transpose() * wa4; + if (ratio >= Scalar(1e-4)) + qtf = wa2; + wa2 = (wa2-wa3)/pnorm; + + /* compute the qr factorization of the updated jacobian. */ + internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); + internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); + internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); + + jeval = false; + } + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x) +{ + HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x); + if (status==HybridNonLinearSolverSpace::ImproperInputParameters) + return status; + while (status==HybridNonLinearSolverSpace::Running) + status = solveNumericalDiffOneStep(x); + return status; +} + +} // end namespace Eigen + +#endif // EIGEN_HYBRIDNONLINEARSOLVER_H + +//vim: ai ts=4 sts=4 et sw=4 diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h new file mode 100644 index 000000000..075faeeb0 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h @@ -0,0 +1,644 @@ +// -*- coding: utf-8 +// vim: set fileencoding=utf-8 + +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// 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_LEVENBERGMARQUARDT__H +#define EIGEN_LEVENBERGMARQUARDT__H + +namespace Eigen { + +namespace LevenbergMarquardtSpace { + enum Status { + NotStarted = -2, + Running = -1, + ImproperInputParameters = 0, + RelativeReductionTooSmall = 1, + RelativeErrorTooSmall = 2, + RelativeErrorAndReductionTooSmall = 3, + CosinusTooSmall = 4, + TooManyFunctionEvaluation = 5, + FtolTooSmall = 6, + XtolTooSmall = 7, + GtolTooSmall = 8, + UserAsked = 9 + }; +} + + + +/** + * \ingroup NonLinearOptimization_Module + * \brief Performs non linear optimization over a non-linear function, + * using a variant of the Levenberg Marquardt algorithm. + * + * Check wikipedia for more information. + * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm + */ +template<typename FunctorType, typename Scalar=double> +class LevenbergMarquardt +{ +public: + LevenbergMarquardt(FunctorType &_functor) + : functor(_functor) { nfev = njev = iter = 0; fnorm = gnorm = 0.; useExternalScaling=false; } + + typedef DenseIndex Index; + + struct Parameters { + Parameters() + : factor(Scalar(100.)) + , maxfev(400) + , ftol(internal::sqrt(NumTraits<Scalar>::epsilon())) + , xtol(internal::sqrt(NumTraits<Scalar>::epsilon())) + , gtol(Scalar(0.)) + , epsfcn(Scalar(0.)) {} + Scalar factor; + Index maxfev; // maximum number of function evaluation + Scalar ftol; + Scalar xtol; + Scalar gtol; + Scalar epsfcn; + }; + + typedef Matrix< Scalar, Dynamic, 1 > FVectorType; + typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType; + + LevenbergMarquardtSpace::Status lmder1( + FVectorType &x, + const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) + ); + + LevenbergMarquardtSpace::Status minimize(FVectorType &x); + LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x); + LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x); + + static LevenbergMarquardtSpace::Status lmdif1( + FunctorType &functor, + FVectorType &x, + Index *nfev, + const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) + ); + + LevenbergMarquardtSpace::Status lmstr1( + FVectorType &x, + const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon()) + ); + + LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x); + LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x); + LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x); + + void resetParameters(void) { parameters = Parameters(); } + + Parameters parameters; + FVectorType fvec, qtf, diag; + JacobianType fjac; + PermutationMatrix<Dynamic,Dynamic> permutation; + Index nfev; + Index njev; + Index iter; + Scalar fnorm, gnorm; + bool useExternalScaling; + + Scalar lm_param(void) { return par; } +private: + FunctorType &functor; + Index n; + Index m; + FVectorType wa1, wa2, wa3, wa4; + + Scalar par, sum; + Scalar temp, temp1, temp2; + Scalar delta; + Scalar ratio; + Scalar pnorm, xnorm, fnorm1, actred, dirder, prered; + + LevenbergMarquardt& operator=(const LevenbergMarquardt&); +}; + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::lmder1( + FVectorType &x, + const Scalar tol + ) +{ + n = x.size(); + m = functor.values(); + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || tol < 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + resetParameters(); + parameters.ftol = tol; + parameters.xtol = tol; + parameters.maxfev = 100*(n+1); + + return minimize(x); +} + + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x) +{ + LevenbergMarquardtSpace::Status status = minimizeInit(x); + if (status==LevenbergMarquardtSpace::ImproperInputParameters) + return status; + do { + status = minimizeOneStep(x); + } while (status==LevenbergMarquardtSpace::Running); + return status; +} + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::minimizeInit(FVectorType &x) +{ + n = x.size(); + m = functor.values(); + + wa1.resize(n); wa2.resize(n); wa3.resize(n); + wa4.resize(m); + fvec.resize(m); + fjac.resize(m, n); + if (!useExternalScaling) + diag.resize(n); + assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); + qtf.resize(n); + + /* Function Body */ + nfev = 0; + njev = 0; + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + if (useExternalScaling) + for (Index j = 0; j < n; ++j) + if (diag[j] <= 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + nfev = 1; + if ( functor(x, fvec) < 0) + return LevenbergMarquardtSpace::UserAsked; + fnorm = fvec.stableNorm(); + + /* initialize levenberg-marquardt parameter and iteration counter. */ + par = 0.; + iter = 1; + + return LevenbergMarquardtSpace::NotStarted; +} + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x) +{ + assert(x.size()==n); // check the caller is not cheating us + + /* calculate the jacobian matrix. */ + Index df_ret = functor.df(x, fjac); + if (df_ret<0) + return LevenbergMarquardtSpace::UserAsked; + if (df_ret>0) + // numerical diff, we evaluated the function df_ret times + nfev += df_ret; + else njev++; + + /* compute the qr factorization of the jacobian. */ + wa2 = fjac.colwise().blueNorm(); + ColPivHouseholderQR<JacobianType> qrfac(fjac); + fjac = qrfac.matrixQR(); + permutation = qrfac.colsPermutation(); + + /* on the first iteration and if external scaling is not used, scale according */ + /* to the norms of the columns of the initial jacobian. */ + if (iter == 1) { + if (!useExternalScaling) + for (Index j = 0; j < n; ++j) + diag[j] = (wa2[j]==0.)? 1. : wa2[j]; + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ + xnorm = diag.cwiseProduct(x).stableNorm(); + delta = parameters.factor * xnorm; + if (delta == 0.) + delta = parameters.factor; + } + + /* form (q transpose)*fvec and store the first n components in */ + /* qtf. */ + wa4 = fvec; + wa4.applyOnTheLeft(qrfac.householderQ().adjoint()); + qtf = wa4.head(n); + + /* compute the norm of the scaled gradient. */ + gnorm = 0.; + if (fnorm != 0.) + for (Index j = 0; j < n; ++j) + if (wa2[permutation.indices()[j]] != 0.) + gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]])); + + /* test for convergence of the gradient norm. */ + if (gnorm <= parameters.gtol) + return LevenbergMarquardtSpace::CosinusTooSmall; + + /* rescale if necessary. */ + if (!useExternalScaling) + diag = diag.cwiseMax(wa2); + + do { + + /* determine the levenberg-marquardt parameter. */ + internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1); + + /* store the direction p and x + p. calculate the norm of p. */ + wa1 = -wa1; + wa2 = x + wa1; + pnorm = diag.cwiseProduct(wa1).stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + if (iter == 1) + delta = (std::min)(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + if ( functor(wa2, wa4) < 0) + return LevenbergMarquardtSpace::UserAsked; + ++nfev; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + actred = -1.; + if (Scalar(.1) * fnorm1 < fnorm) + actred = 1. - internal::abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction and */ + /* the scaled directional derivative. */ + wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1); + temp1 = internal::abs2(wa3.stableNorm() / fnorm); + temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm); + prered = temp1 + temp2 / Scalar(.5); + dirder = -(temp1 + temp2); + + /* compute the ratio of the actual to the predicted */ + /* reduction. */ + ratio = 0.; + if (prered != 0.) + ratio = actred / prered; + + /* update the step bound. */ + if (ratio <= Scalar(.25)) { + if (actred >= 0.) + temp = Scalar(.5); + if (actred < 0.) + temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); + if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) + temp = Scalar(.1); + /* Computing MIN */ + delta = temp * (std::min)(delta, pnorm / Scalar(.1)); + par /= temp; + } else if (!(par != 0. && ratio < Scalar(.75))) { + delta = pnorm / Scalar(.5); + par = Scalar(.5) * par; + } + + /* test for successful iteration. */ + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwiseProduct(x); + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* tests for convergence. */ + if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm) + return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; + if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.) + return LevenbergMarquardtSpace::RelativeReductionTooSmall; + if (delta <= parameters.xtol * xnorm) + return LevenbergMarquardtSpace::RelativeErrorTooSmall; + + /* tests for termination and stringent tolerances. */ + if (nfev >= parameters.maxfev) + return LevenbergMarquardtSpace::TooManyFunctionEvaluation; + if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) + return LevenbergMarquardtSpace::FtolTooSmall; + if (delta <= NumTraits<Scalar>::epsilon() * xnorm) + return LevenbergMarquardtSpace::XtolTooSmall; + if (gnorm <= NumTraits<Scalar>::epsilon()) + return LevenbergMarquardtSpace::GtolTooSmall; + + } while (ratio < Scalar(1e-4)); + + return LevenbergMarquardtSpace::Running; +} + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::lmstr1( + FVectorType &x, + const Scalar tol + ) +{ + n = x.size(); + m = functor.values(); + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || tol < 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + resetParameters(); + parameters.ftol = tol; + parameters.xtol = tol; + parameters.maxfev = 100*(n+1); + + return minimizeOptimumStorage(x); +} + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(FVectorType &x) +{ + n = x.size(); + m = functor.values(); + + wa1.resize(n); wa2.resize(n); wa3.resize(n); + wa4.resize(m); + fvec.resize(m); + // Only R is stored in fjac. Q is only used to compute 'qtf', which is + // Q.transpose()*rhs. qtf will be updated using givens rotation, + // instead of storing them in Q. + // The purpose it to only use a nxn matrix, instead of mxn here, so + // that we can handle cases where m>>n : + fjac.resize(n, n); + if (!useExternalScaling) + diag.resize(n); + assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); + qtf.resize(n); + + /* Function Body */ + nfev = 0; + njev = 0; + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + if (useExternalScaling) + for (Index j = 0; j < n; ++j) + if (diag[j] <= 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + nfev = 1; + if ( functor(x, fvec) < 0) + return LevenbergMarquardtSpace::UserAsked; + fnorm = fvec.stableNorm(); + + /* initialize levenberg-marquardt parameter and iteration counter. */ + par = 0.; + iter = 1; + + return LevenbergMarquardtSpace::NotStarted; +} + + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x) +{ + assert(x.size()==n); // check the caller is not cheating us + + Index i, j; + bool sing; + + /* compute the qr factorization of the jacobian matrix */ + /* calculated one row at a time, while simultaneously */ + /* forming (q transpose)*fvec and storing the first */ + /* n components in qtf. */ + qtf.fill(0.); + fjac.fill(0.); + Index rownb = 2; + for (i = 0; i < m; ++i) { + if (functor.df(x, wa3, rownb) < 0) return LevenbergMarquardtSpace::UserAsked; + internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]); + ++rownb; + } + ++njev; + + /* if the jacobian is rank deficient, call qrfac to */ + /* reorder its columns and update the components of qtf. */ + sing = false; + for (j = 0; j < n; ++j) { + if (fjac(j,j) == 0.) + sing = true; + wa2[j] = fjac.col(j).head(j).stableNorm(); + } + permutation.setIdentity(n); + if (sing) { + wa2 = fjac.colwise().blueNorm(); + // TODO We have no unit test covering this code path, do not modify + // until it is carefully tested + ColPivHouseholderQR<JacobianType> qrfac(fjac); + fjac = qrfac.matrixQR(); + wa1 = fjac.diagonal(); + fjac.diagonal() = qrfac.hCoeffs(); + permutation = qrfac.colsPermutation(); + // TODO : avoid this: + for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors + + for (j = 0; j < n; ++j) { + if (fjac(j,j) != 0.) { + sum = 0.; + for (i = j; i < n; ++i) + sum += fjac(i,j) * qtf[i]; + temp = -sum / fjac(j,j); + for (i = j; i < n; ++i) + qtf[i] += fjac(i,j) * temp; + } + fjac(j,j) = wa1[j]; + } + } + + /* on the first iteration and if external scaling is not used, scale according */ + /* to the norms of the columns of the initial jacobian. */ + if (iter == 1) { + if (!useExternalScaling) + for (j = 0; j < n; ++j) + diag[j] = (wa2[j]==0.)? 1. : wa2[j]; + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ + xnorm = diag.cwiseProduct(x).stableNorm(); + delta = parameters.factor * xnorm; + if (delta == 0.) + delta = parameters.factor; + } + + /* compute the norm of the scaled gradient. */ + gnorm = 0.; + if (fnorm != 0.) + for (j = 0; j < n; ++j) + if (wa2[permutation.indices()[j]] != 0.) + gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]])); + + /* test for convergence of the gradient norm. */ + if (gnorm <= parameters.gtol) + return LevenbergMarquardtSpace::CosinusTooSmall; + + /* rescale if necessary. */ + if (!useExternalScaling) + diag = diag.cwiseMax(wa2); + + do { + + /* determine the levenberg-marquardt parameter. */ + internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1); + + /* store the direction p and x + p. calculate the norm of p. */ + wa1 = -wa1; + wa2 = x + wa1; + pnorm = diag.cwiseProduct(wa1).stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + if (iter == 1) + delta = (std::min)(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + if ( functor(wa2, wa4) < 0) + return LevenbergMarquardtSpace::UserAsked; + ++nfev; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + actred = -1.; + if (Scalar(.1) * fnorm1 < fnorm) + actred = 1. - internal::abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction and */ + /* the scaled directional derivative. */ + wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1); + temp1 = internal::abs2(wa3.stableNorm() / fnorm); + temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm); + prered = temp1 + temp2 / Scalar(.5); + dirder = -(temp1 + temp2); + + /* compute the ratio of the actual to the predicted */ + /* reduction. */ + ratio = 0.; + if (prered != 0.) + ratio = actred / prered; + + /* update the step bound. */ + if (ratio <= Scalar(.25)) { + if (actred >= 0.) + temp = Scalar(.5); + if (actred < 0.) + temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred); + if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) + temp = Scalar(.1); + /* Computing MIN */ + delta = temp * (std::min)(delta, pnorm / Scalar(.1)); + par /= temp; + } else if (!(par != 0. && ratio < Scalar(.75))) { + delta = pnorm / Scalar(.5); + par = Scalar(.5) * par; + } + + /* test for successful iteration. */ + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwiseProduct(x); + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* tests for convergence. */ + if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm) + return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; + if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.) + return LevenbergMarquardtSpace::RelativeReductionTooSmall; + if (delta <= parameters.xtol * xnorm) + return LevenbergMarquardtSpace::RelativeErrorTooSmall; + + /* tests for termination and stringent tolerances. */ + if (nfev >= parameters.maxfev) + return LevenbergMarquardtSpace::TooManyFunctionEvaluation; + if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) + return LevenbergMarquardtSpace::FtolTooSmall; + if (delta <= NumTraits<Scalar>::epsilon() * xnorm) + return LevenbergMarquardtSpace::XtolTooSmall; + if (gnorm <= NumTraits<Scalar>::epsilon()) + return LevenbergMarquardtSpace::GtolTooSmall; + + } while (ratio < Scalar(1e-4)); + + return LevenbergMarquardtSpace::Running; +} + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x) +{ + LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x); + if (status==LevenbergMarquardtSpace::ImproperInputParameters) + return status; + do { + status = minimizeOptimumStorageOneStep(x); + } while (status==LevenbergMarquardtSpace::Running); + return status; +} + +template<typename FunctorType, typename Scalar> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType,Scalar>::lmdif1( + FunctorType &functor, + FVectorType &x, + Index *nfev, + const Scalar tol + ) +{ + Index n = x.size(); + Index m = functor.values(); + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || tol < 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + NumericalDiff<FunctorType> numDiff(functor); + // embedded LevenbergMarquardt + LevenbergMarquardt<NumericalDiff<FunctorType>, Scalar > lm(numDiff); + lm.parameters.ftol = tol; + lm.parameters.xtol = tol; + lm.parameters.maxfev = 200*(n+1); + + LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x)); + if (nfev) + * nfev = lm.nfev; + return info; +} + +} // end namespace Eigen + +#endif // EIGEN_LEVENBERGMARQUARDT__H + +//vim: ai ts=4 sts=4 et sw=4 diff --git a/unsupported/Eigen/src/NonLinearOptimization/chkder.h b/unsupported/Eigen/src/NonLinearOptimization/chkder.h new file mode 100644 index 000000000..ad37c5029 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/chkder.h @@ -0,0 +1,62 @@ +#define chkder_log10e 0.43429448190325182765 +#define chkder_factor 100. + +namespace Eigen { + +namespace internal { + +template<typename Scalar> +void chkder( + const Matrix< Scalar, Dynamic, 1 > &x, + const Matrix< Scalar, Dynamic, 1 > &fvec, + const Matrix< Scalar, Dynamic, Dynamic > &fjac, + Matrix< Scalar, Dynamic, 1 > &xp, + const Matrix< Scalar, Dynamic, 1 > &fvecp, + int mode, + Matrix< Scalar, Dynamic, 1 > &err + ) +{ + typedef DenseIndex Index; + + const Scalar eps = sqrt(NumTraits<Scalar>::epsilon()); + const Scalar epsf = chkder_factor * NumTraits<Scalar>::epsilon(); + const Scalar epslog = chkder_log10e * log(eps); + Scalar temp; + + const Index m = fvec.size(), n = x.size(); + + if (mode != 2) { + /* mode = 1. */ + xp.resize(n); + for (Index j = 0; j < n; ++j) { + temp = eps * abs(x[j]); + if (temp == 0.) + temp = eps; + xp[j] = x[j] + temp; + } + } + else { + /* mode = 2. */ + err.setZero(m); + for (Index j = 0; j < n; ++j) { + temp = abs(x[j]); + if (temp == 0.) + temp = 1.; + err += temp * fjac.col(j); + } + for (Index i = 0; i < m; ++i) { + temp = 1.; + if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i])) + temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i])); + err[i] = 1.; + if (temp > NumTraits<Scalar>::epsilon() && temp < eps) + err[i] = (chkder_log10e * log(temp) - epslog) / epslog; + if (temp >= eps) + err[i] = 0.; + } + } +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/covar.h b/unsupported/Eigen/src/NonLinearOptimization/covar.h new file mode 100644 index 000000000..c73a09645 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/covar.h @@ -0,0 +1,69 @@ +namespace Eigen { + +namespace internal { + +template <typename Scalar> +void covar( + Matrix< Scalar, Dynamic, Dynamic > &r, + const VectorXi &ipvt, + Scalar tol = sqrt(NumTraits<Scalar>::epsilon()) ) +{ + typedef DenseIndex Index; + + /* Local variables */ + Index i, j, k, l, ii, jj; + bool sing; + Scalar temp; + + /* Function Body */ + const Index n = r.cols(); + const Scalar tolr = tol * abs(r(0,0)); + Matrix< Scalar, Dynamic, 1 > wa(n); + assert(ipvt.size()==n); + + /* form the inverse of r in the full upper triangle of r. */ + l = -1; + for (k = 0; k < n; ++k) + if (abs(r(k,k)) > tolr) { + r(k,k) = 1. / r(k,k); + for (j = 0; j <= k-1; ++j) { + temp = r(k,k) * r(j,k); + r(j,k) = 0.; + r.col(k).head(j+1) -= r.col(j).head(j+1) * temp; + } + l = k; + } + + /* form the full upper triangle of the inverse of (r transpose)*r */ + /* in the full upper triangle of r. */ + for (k = 0; k <= l; ++k) { + for (j = 0; j <= k-1; ++j) + r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k); + r.col(k).head(k+1) *= r(k,k); + } + + /* form the full lower triangle of the covariance matrix */ + /* in the strict lower triangle of r and in wa. */ + for (j = 0; j < n; ++j) { + jj = ipvt[j]; + sing = j > l; + for (i = 0; i <= j; ++i) { + if (sing) + r(i,j) = 0.; + ii = ipvt[i]; + if (ii > jj) + r(ii,jj) = r(i,j); + if (ii < jj) + r(jj,ii) = r(i,j); + } + wa[jj] = r(j,j); + } + + /* symmetrize the covariance matrix in r. */ + r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose(); + r.diagonal() = wa; +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/dogleg.h b/unsupported/Eigen/src/NonLinearOptimization/dogleg.h new file mode 100644 index 000000000..4fbc98bfc --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/dogleg.h @@ -0,0 +1,104 @@ +namespace Eigen { + +namespace internal { + +template <typename Scalar> +void dogleg( + const Matrix< Scalar, Dynamic, Dynamic > &qrfac, + const Matrix< Scalar, Dynamic, 1 > &diag, + const Matrix< Scalar, Dynamic, 1 > &qtb, + Scalar delta, + Matrix< Scalar, Dynamic, 1 > &x) +{ + typedef DenseIndex Index; + + /* Local variables */ + Index i, j; + Scalar sum, temp, alpha, bnorm; + Scalar gnorm, qnorm; + Scalar sgnorm; + + /* Function Body */ + const Scalar epsmch = NumTraits<Scalar>::epsilon(); + const Index n = qrfac.cols(); + assert(n==qtb.size()); + assert(n==x.size()); + assert(n==diag.size()); + Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n); + + /* first, calculate the gauss-newton direction. */ + for (j = n-1; j >=0; --j) { + temp = qrfac(j,j); + if (temp == 0.) { + temp = epsmch * qrfac.col(j).head(j+1).maxCoeff(); + if (temp == 0.) + temp = epsmch; + } + if (j==n-1) + x[j] = qtb[j] / temp; + else + x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp; + } + + /* test whether the gauss-newton direction is acceptable. */ + qnorm = diag.cwiseProduct(x).stableNorm(); + if (qnorm <= delta) + return; + + // TODO : this path is not tested by Eigen unit tests + + /* the gauss-newton direction is not acceptable. */ + /* next, calculate the scaled gradient direction. */ + + wa1.fill(0.); + for (j = 0; j < n; ++j) { + wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j]; + wa1[j] /= diag[j]; + } + + /* calculate the norm of the scaled gradient and test for */ + /* the special case in which the scaled gradient is zero. */ + gnorm = wa1.stableNorm(); + sgnorm = 0.; + alpha = delta / qnorm; + if (gnorm == 0.) + goto algo_end; + + /* calculate the point along the scaled gradient */ + /* at which the quadratic is minimized. */ + wa1.array() /= (diag*gnorm).array(); + // TODO : once unit tests cover this part,: + // wa2 = qrfac.template triangularView<Upper>() * wa1; + for (j = 0; j < n; ++j) { + sum = 0.; + for (i = j; i < n; ++i) { + sum += qrfac(j,i) * wa1[i]; + } + wa2[j] = sum; + } + temp = wa2.stableNorm(); + sgnorm = gnorm / temp / temp; + + /* test whether the scaled gradient direction is acceptable. */ + alpha = 0.; + if (sgnorm >= delta) + goto algo_end; + + /* the scaled gradient direction is not acceptable. */ + /* finally, calculate the point along the dogleg */ + /* at which the quadratic is minimized. */ + bnorm = qtb.stableNorm(); + temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta); + temp = temp - delta / qnorm * abs2(sgnorm / delta) + sqrt(abs2(temp - delta / qnorm) + (1.-abs2(delta / qnorm)) * (1.-abs2(sgnorm / delta))); + alpha = delta / qnorm * (1. - abs2(sgnorm / delta)) / temp; +algo_end: + + /* form appropriate convex combination of the gauss-newton */ + /* direction and the scaled gradient direction. */ + temp = (1.-alpha) * (std::min)(sgnorm,delta); + x = temp * wa1 + alpha * x; +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h b/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h new file mode 100644 index 000000000..1cabe69ae --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h @@ -0,0 +1,76 @@ +namespace Eigen { + +namespace internal { + +template<typename FunctorType, typename Scalar> +DenseIndex fdjac1( + const FunctorType &Functor, + Matrix< Scalar, Dynamic, 1 > &x, + Matrix< Scalar, Dynamic, 1 > &fvec, + Matrix< Scalar, Dynamic, Dynamic > &fjac, + DenseIndex ml, DenseIndex mu, + Scalar epsfcn) +{ + typedef DenseIndex Index; + + /* Local variables */ + Scalar h; + Index j, k; + Scalar eps, temp; + Index msum; + int iflag; + Index start, length; + + /* Function Body */ + const Scalar epsmch = NumTraits<Scalar>::epsilon(); + const Index n = x.size(); + assert(fvec.size()==n); + Matrix< Scalar, Dynamic, 1 > wa1(n); + Matrix< Scalar, Dynamic, 1 > wa2(n); + + eps = sqrt((std::max)(epsfcn,epsmch)); + msum = ml + mu + 1; + if (msum >= n) { + /* computation of dense approximate jacobian. */ + for (j = 0; j < n; ++j) { + temp = x[j]; + h = eps * abs(temp); + if (h == 0.) + h = eps; + x[j] = temp + h; + iflag = Functor(x, wa1); + if (iflag < 0) + return iflag; + x[j] = temp; + fjac.col(j) = (wa1-fvec)/h; + } + + }else { + /* computation of banded approximate jacobian. */ + for (k = 0; k < msum; ++k) { + for (j = k; (msum<0) ? (j>n): (j<n); j += msum) { + wa2[j] = x[j]; + h = eps * abs(wa2[j]); + if (h == 0.) h = eps; + x[j] = wa2[j] + h; + } + iflag = Functor(x, wa1); + if (iflag < 0) + return iflag; + for (j = k; (msum<0) ? (j>n): (j<n); j += msum) { + x[j] = wa2[j]; + h = eps * abs(wa2[j]); + if (h == 0.) h = eps; + fjac.col(j).setZero(); + start = std::max<Index>(0,j-mu); + length = (std::min)(n-1, j+ml) - start + 1; + fjac.col(j).segment(start, length) = ( wa1.segment(start, length)-fvec.segment(start, length))/h; + } + } + } + return 0; +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h new file mode 100644 index 000000000..cc1ca530f --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h @@ -0,0 +1,294 @@ +namespace Eigen { + +namespace internal { + +template <typename Scalar> +void lmpar( + Matrix< Scalar, Dynamic, Dynamic > &r, + const VectorXi &ipvt, + const Matrix< Scalar, Dynamic, 1 > &diag, + const Matrix< Scalar, Dynamic, 1 > &qtb, + Scalar delta, + Scalar &par, + Matrix< Scalar, Dynamic, 1 > &x) +{ + typedef DenseIndex Index; + + /* Local variables */ + Index i, j, l; + Scalar fp; + Scalar parc, parl; + Index iter; + Scalar temp, paru; + Scalar gnorm; + Scalar dxnorm; + + + /* Function Body */ + const Scalar dwarf = std::numeric_limits<Scalar>::min(); + const Index n = r.cols(); + assert(n==diag.size()); + assert(n==qtb.size()); + assert(n==x.size()); + + Matrix< Scalar, Dynamic, 1 > wa1, wa2; + + /* compute and store in x the gauss-newton direction. if the */ + /* jacobian is rank-deficient, obtain a least squares solution. */ + Index nsing = n-1; + wa1 = qtb; + for (j = 0; j < n; ++j) { + if (r(j,j) == 0. && nsing == n-1) + nsing = j - 1; + if (nsing < n-1) + wa1[j] = 0.; + } + for (j = nsing; j>=0; --j) { + wa1[j] /= r(j,j); + temp = wa1[j]; + for (i = 0; i < j ; ++i) + wa1[i] -= r(i,j) * temp; + } + + for (j = 0; j < n; ++j) + x[ipvt[j]] = wa1[j]; + + /* initialize the iteration counter. */ + /* evaluate the function at the origin, and test */ + /* for acceptance of the gauss-newton direction. */ + iter = 0; + wa2 = diag.cwiseProduct(x); + dxnorm = wa2.blueNorm(); + fp = dxnorm - delta; + if (fp <= Scalar(0.1) * delta) { + par = 0; + return; + } + + /* if the jacobian is not rank deficient, the newton */ + /* step provides a lower bound, parl, for the zero of */ + /* the function. otherwise set this bound to zero. */ + parl = 0.; + if (nsing >= n-1) { + for (j = 0; j < n; ++j) { + l = ipvt[j]; + wa1[j] = diag[l] * (wa2[l] / dxnorm); + } + // it's actually a triangularView.solveInplace(), though in a weird + // way: + for (j = 0; j < n; ++j) { + Scalar sum = 0.; + for (i = 0; i < j; ++i) + sum += r(i,j) * wa1[i]; + wa1[j] = (wa1[j] - sum) / r(j,j); + } + temp = wa1.blueNorm(); + parl = fp / delta / temp / temp; + } + + /* calculate an upper bound, paru, for the zero of the function. */ + for (j = 0; j < n; ++j) + wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]]; + + gnorm = wa1.stableNorm(); + paru = gnorm / delta; + if (paru == 0.) + paru = dwarf / (std::min)(delta,Scalar(0.1)); + + /* if the input par lies outside of the interval (parl,paru), */ + /* set par to the closer endpoint. */ + par = (std::max)(par,parl); + par = (std::min)(par,paru); + if (par == 0.) + par = gnorm / dxnorm; + + /* beginning of an iteration. */ + while (true) { + ++iter; + + /* evaluate the function at the current value of par. */ + if (par == 0.) + par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ + wa1 = sqrt(par)* diag; + + Matrix< Scalar, Dynamic, 1 > sdiag(n); + qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag); + + wa2 = diag.cwiseProduct(x); + dxnorm = wa2.blueNorm(); + temp = fp; + fp = dxnorm - delta; + + /* if the function is small enough, accept the current value */ + /* of par. also test for the exceptional cases where parl */ + /* is zero or the number of iterations has reached 10. */ + if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) + break; + + /* compute the newton correction. */ + for (j = 0; j < n; ++j) { + l = ipvt[j]; + wa1[j] = diag[l] * (wa2[l] / dxnorm); + } + for (j = 0; j < n; ++j) { + wa1[j] /= sdiag[j]; + temp = wa1[j]; + for (i = j+1; i < n; ++i) + wa1[i] -= r(i,j) * temp; + } + temp = wa1.blueNorm(); + parc = fp / delta / temp / temp; + + /* depending on the sign of the function, update parl or paru. */ + if (fp > 0.) + parl = (std::max)(parl,par); + if (fp < 0.) + paru = (std::min)(paru,par); + + /* compute an improved estimate for par. */ + /* Computing MAX */ + par = (std::max)(parl,par+parc); + + /* end of an iteration. */ + } + + /* termination. */ + if (iter == 0) + par = 0.; + return; +} + +template <typename Scalar> +void lmpar2( + const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr, + const Matrix< Scalar, Dynamic, 1 > &diag, + const Matrix< Scalar, Dynamic, 1 > &qtb, + Scalar delta, + Scalar &par, + Matrix< Scalar, Dynamic, 1 > &x) + +{ + typedef DenseIndex Index; + + /* Local variables */ + Index j; + Scalar fp; + Scalar parc, parl; + Index iter; + Scalar temp, paru; + Scalar gnorm; + Scalar dxnorm; + + + /* Function Body */ + const Scalar dwarf = std::numeric_limits<Scalar>::min(); + const Index n = qr.matrixQR().cols(); + assert(n==diag.size()); + assert(n==qtb.size()); + + Matrix< Scalar, Dynamic, 1 > wa1, wa2; + + /* compute and store in x the gauss-newton direction. if the */ + /* jacobian is rank-deficient, obtain a least squares solution. */ + +// const Index rank = qr.nonzeroPivots(); // exactly double(0.) + const Index rank = qr.rank(); // use a threshold + wa1 = qtb; + wa1.tail(n-rank).setZero(); + qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank)); + + x = qr.colsPermutation()*wa1; + + /* initialize the iteration counter. */ + /* evaluate the function at the origin, and test */ + /* for acceptance of the gauss-newton direction. */ + iter = 0; + wa2 = diag.cwiseProduct(x); + dxnorm = wa2.blueNorm(); + fp = dxnorm - delta; + if (fp <= Scalar(0.1) * delta) { + par = 0; + return; + } + + /* if the jacobian is not rank deficient, the newton */ + /* step provides a lower bound, parl, for the zero of */ + /* the function. otherwise set this bound to zero. */ + parl = 0.; + if (rank==n) { + wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; + qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); + temp = wa1.blueNorm(); + parl = fp / delta / temp / temp; + } + + /* calculate an upper bound, paru, for the zero of the function. */ + for (j = 0; j < n; ++j) + wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; + + gnorm = wa1.stableNorm(); + paru = gnorm / delta; + if (paru == 0.) + paru = dwarf / (std::min)(delta,Scalar(0.1)); + + /* if the input par lies outside of the interval (parl,paru), */ + /* set par to the closer endpoint. */ + par = (std::max)(par,parl); + par = (std::min)(par,paru); + if (par == 0.) + par = gnorm / dxnorm; + + /* beginning of an iteration. */ + Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR(); + while (true) { + ++iter; + + /* evaluate the function at the current value of par. */ + if (par == 0.) + par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ + wa1 = sqrt(par)* diag; + + Matrix< Scalar, Dynamic, 1 > sdiag(n); + qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag); + + wa2 = diag.cwiseProduct(x); + dxnorm = wa2.blueNorm(); + temp = fp; + fp = dxnorm - delta; + + /* if the function is small enough, accept the current value */ + /* of par. also test for the exceptional cases where parl */ + /* is zero or the number of iterations has reached 10. */ + if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) + break; + + /* compute the newton correction. */ + wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); + // we could almost use this here, but the diagonal is outside qr, in sdiag[] + // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1); + for (j = 0; j < n; ++j) { + wa1[j] /= sdiag[j]; + temp = wa1[j]; + for (Index i = j+1; i < n; ++i) + wa1[i] -= s(i,j) * temp; + } + temp = wa1.blueNorm(); + parc = fp / delta / temp / temp; + + /* depending on the sign of the function, update parl or paru. */ + if (fp > 0.) + parl = (std::max)(parl,par); + if (fp < 0.) + paru = (std::min)(paru,par); + + /* compute an improved estimate for par. */ + par = (std::max)(parl,par+parc); + } + if (iter == 0) + par = 0.; + return; +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h new file mode 100644 index 000000000..feafd62a8 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h @@ -0,0 +1,91 @@ +namespace Eigen { + +namespace internal { + +// TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt +template <typename Scalar> +void qrsolv( + Matrix< Scalar, Dynamic, Dynamic > &s, + // TODO : use a PermutationMatrix once lmpar is no more: + const VectorXi &ipvt, + const Matrix< Scalar, Dynamic, 1 > &diag, + const Matrix< Scalar, Dynamic, 1 > &qtb, + Matrix< Scalar, Dynamic, 1 > &x, + Matrix< Scalar, Dynamic, 1 > &sdiag) + +{ + typedef DenseIndex Index; + + /* Local variables */ + Index i, j, k, l; + Scalar temp; + Index n = s.cols(); + Matrix< Scalar, Dynamic, 1 > wa(n); + JacobiRotation<Scalar> givens; + + /* Function Body */ + // the following will only change the lower triangular part of s, including + // the diagonal, though the diagonal is restored afterward + + /* copy r and (q transpose)*b to preserve input and initialize s. */ + /* in particular, save the diagonal elements of r in x. */ + x = s.diagonal(); + wa = qtb; + + s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose(); + + /* eliminate the diagonal matrix d using a givens rotation. */ + for (j = 0; j < n; ++j) { + + /* prepare the row of d to be eliminated, locating the */ + /* diagonal element using p from the qr factorization. */ + l = ipvt[j]; + if (diag[l] == 0.) + break; + sdiag.tail(n-j).setZero(); + sdiag[j] = diag[l]; + + /* the transformations to eliminate the row of d */ + /* modify only a single element of (q transpose)*b */ + /* beyond the first n, which is initially zero. */ + Scalar qtbpj = 0.; + for (k = j; k < n; ++k) { + /* determine a givens rotation which eliminates the */ + /* appropriate element in the current row of d. */ + givens.makeGivens(-s(k,k), sdiag[k]); + + /* compute the modified diagonal element of r and */ + /* the modified element of ((q transpose)*b,0). */ + s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; + temp = givens.c() * wa[k] + givens.s() * qtbpj; + qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; + wa[k] = temp; + + /* accumulate the tranformation in the row of s. */ + for (i = k+1; i<n; ++i) { + temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; + sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; + s(i,k) = temp; + } + } + } + + /* solve the triangular system for z. if the system is */ + /* singular, then obtain a least squares solution. */ + Index nsing; + for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} + + wa.tail(n-nsing).setZero(); + s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); + + // restore + sdiag = s.diagonal(); + s.diagonal() = x; + + /* permute the components of z back to components of x. */ + for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j]; +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h new file mode 100644 index 000000000..36ff700e9 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h @@ -0,0 +1,30 @@ +namespace Eigen { + +namespace internal { + +// TODO : move this to GivensQR once there's such a thing in Eigen + +template <typename Scalar> +void r1mpyq(DenseIndex m, DenseIndex n, Scalar *a, const std::vector<JacobiRotation<Scalar> > &v_givens, const std::vector<JacobiRotation<Scalar> > &w_givens) +{ + typedef DenseIndex Index; + + /* apply the first set of givens rotations to a. */ + for (Index j = n-2; j>=0; --j) + for (Index i = 0; i<m; ++i) { + Scalar temp = v_givens[j].c() * a[i+m*j] - v_givens[j].s() * a[i+m*(n-1)]; + a[i+m*(n-1)] = v_givens[j].s() * a[i+m*j] + v_givens[j].c() * a[i+m*(n-1)]; + a[i+m*j] = temp; + } + /* apply the second set of givens rotations to a. */ + for (Index j = 0; j<n-1; ++j) + for (Index i = 0; i<m; ++i) { + Scalar temp = w_givens[j].c() * a[i+m*j] + w_givens[j].s() * a[i+m*(n-1)]; + a[i+m*(n-1)] = -w_givens[j].s() * a[i+m*j] + w_givens[j].c() * a[i+m*(n-1)]; + a[i+m*j] = temp; + } +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1updt.h b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h new file mode 100644 index 000000000..55fae5ae8 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h @@ -0,0 +1,99 @@ +namespace Eigen { + +namespace internal { + +template <typename Scalar> +void r1updt( + Matrix< Scalar, Dynamic, Dynamic > &s, + const Matrix< Scalar, Dynamic, 1> &u, + std::vector<JacobiRotation<Scalar> > &v_givens, + std::vector<JacobiRotation<Scalar> > &w_givens, + Matrix< Scalar, Dynamic, 1> &v, + Matrix< Scalar, Dynamic, 1> &w, + bool *sing) +{ + typedef DenseIndex Index; + const JacobiRotation<Scalar> IdentityRotation = JacobiRotation<Scalar>(1,0); + + /* Local variables */ + const Index m = s.rows(); + const Index n = s.cols(); + Index i, j=1; + Scalar temp; + JacobiRotation<Scalar> givens; + + // r1updt had a broader usecase, but we dont use it here. And, more + // importantly, we can not test it. + assert(m==n); + assert(u.size()==m); + assert(v.size()==n); + assert(w.size()==n); + + /* move the nontrivial part of the last column of s into w. */ + w[n-1] = s(n-1,n-1); + + /* rotate the vector v into a multiple of the n-th unit vector */ + /* in such a way that a spike is introduced into w. */ + for (j=n-2; j>=0; --j) { + w[j] = 0.; + if (v[j] != 0.) { + /* determine a givens rotation which eliminates the */ + /* j-th element of v. */ + givens.makeGivens(-v[n-1], v[j]); + + /* apply the transformation to v and store the information */ + /* necessary to recover the givens rotation. */ + v[n-1] = givens.s() * v[j] + givens.c() * v[n-1]; + v_givens[j] = givens; + + /* apply the transformation to s and extend the spike in w. */ + for (i = j; i < m; ++i) { + temp = givens.c() * s(j,i) - givens.s() * w[i]; + w[i] = givens.s() * s(j,i) + givens.c() * w[i]; + s(j,i) = temp; + } + } else + v_givens[j] = IdentityRotation; + } + + /* add the spike from the rank 1 update to w. */ + w += v[n-1] * u; + + /* eliminate the spike. */ + *sing = false; + for (j = 0; j < n-1; ++j) { + if (w[j] != 0.) { + /* determine a givens rotation which eliminates the */ + /* j-th element of the spike. */ + givens.makeGivens(-s(j,j), w[j]); + + /* apply the transformation to s and reduce the spike in w. */ + for (i = j; i < m; ++i) { + temp = givens.c() * s(j,i) + givens.s() * w[i]; + w[i] = -givens.s() * s(j,i) + givens.c() * w[i]; + s(j,i) = temp; + } + + /* store the information necessary to recover the */ + /* givens rotation. */ + w_givens[j] = givens; + } else + v_givens[j] = IdentityRotation; + + /* test for zero diagonal elements in the output s. */ + if (s(j,j) == 0.) { + *sing = true; + } + } + /* move w back into the last column of the output s. */ + s(n-1,n-1) = w[n-1]; + + if (s(j,j) == 0.) { + *sing = true; + } + return; +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h new file mode 100644 index 000000000..9ce079e22 --- /dev/null +++ b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h @@ -0,0 +1,49 @@ +namespace Eigen { + +namespace internal { + +template <typename Scalar> +void rwupdt( + Matrix< Scalar, Dynamic, Dynamic > &r, + const Matrix< Scalar, Dynamic, 1> &w, + Matrix< Scalar, Dynamic, 1> &b, + Scalar alpha) +{ + typedef DenseIndex Index; + + const Index n = r.cols(); + assert(r.rows()>=n); + std::vector<JacobiRotation<Scalar> > givens(n); + + /* Local variables */ + Scalar temp, rowj; + + /* Function Body */ + for (Index j = 0; j < n; ++j) { + rowj = w[j]; + + /* apply the previous transformations to */ + /* r(i,j), i=0,1,...,j-1, and to w(j). */ + for (Index i = 0; i < j; ++i) { + temp = givens[i].c() * r(i,j) + givens[i].s() * rowj; + rowj = -givens[i].s() * r(i,j) + givens[i].c() * rowj; + r(i,j) = temp; + } + + /* determine a givens rotation which eliminates w(j). */ + givens[j].makeGivens(-r(j,j), rowj); + + if (rowj == 0.) + continue; // givens[j] is identity + + /* apply the current transformation to r(j,j), b(j), and alpha. */ + r(j,j) = givens[j].c() * r(j,j) + givens[j].s() * rowj; + temp = givens[j].c() * b[j] + givens[j].s() * alpha; + alpha = -givens[j].s() * b[j] + givens[j].c() * alpha; + b[j] = temp; + } +} + +} // end namespace internal + +} // end namespace Eigen diff --git a/unsupported/Eigen/src/NumericalDiff/CMakeLists.txt b/unsupported/Eigen/src/NumericalDiff/CMakeLists.txt new file mode 100644 index 000000000..1199aca2f --- /dev/null +++ b/unsupported/Eigen/src/NumericalDiff/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_NumericalDiff_SRCS "*.h") + +INSTALL(FILES + ${Eigen_NumericalDiff_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/NumericalDiff COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h b/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h new file mode 100644 index 000000000..d848cb407 --- /dev/null +++ b/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h @@ -0,0 +1,128 @@ +// -*- coding: utf-8 +// vim: set fileencoding=utf-8 + +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// 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_NUMERICAL_DIFF_H +#define EIGEN_NUMERICAL_DIFF_H + +namespace Eigen { + +enum NumericalDiffMode { + Forward, + Central +}; + + +/** + * This class allows you to add a method df() to your functor, which will + * use numerical differentiation to compute an approximate of the + * derivative for the functor. Of course, if you have an analytical form + * for the derivative, you should rather implement df() by yourself. + * + * More information on + * http://en.wikipedia.org/wiki/Numerical_differentiation + * + * Currently only "Forward" and "Central" scheme are implemented. + */ +template<typename _Functor, NumericalDiffMode mode=Forward> +class NumericalDiff : public _Functor +{ +public: + typedef _Functor Functor; + typedef typename Functor::Scalar Scalar; + typedef typename Functor::InputType InputType; + typedef typename Functor::ValueType ValueType; + typedef typename Functor::JacobianType JacobianType; + + NumericalDiff(Scalar _epsfcn=0.) : Functor(), epsfcn(_epsfcn) {} + NumericalDiff(const Functor& f, Scalar _epsfcn=0.) : Functor(f), epsfcn(_epsfcn) {} + + // forward constructors + template<typename T0> + NumericalDiff(const T0& a0) : Functor(a0), epsfcn(0) {} + template<typename T0, typename T1> + NumericalDiff(const T0& a0, const T1& a1) : Functor(a0, a1), epsfcn(0) {} + template<typename T0, typename T1, typename T2> + NumericalDiff(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2), epsfcn(0) {} + + enum { + InputsAtCompileTime = Functor::InputsAtCompileTime, + ValuesAtCompileTime = Functor::ValuesAtCompileTime + }; + + /** + * return the number of evaluation of functor + */ + int df(const InputType& _x, JacobianType &jac) const + { + /* Local variables */ + Scalar h; + int nfev=0; + const typename InputType::Index n = _x.size(); + const Scalar eps = internal::sqrt(((std::max)(epsfcn,NumTraits<Scalar>::epsilon() ))); + ValueType val1, val2; + InputType x = _x; + // TODO : we should do this only if the size is not already known + val1.resize(Functor::values()); + val2.resize(Functor::values()); + + // initialization + switch(mode) { + case Forward: + // compute f(x) + Functor::operator()(x, val1); nfev++; + break; + case Central: + // do nothing + break; + default: + assert(false); + }; + + // Function Body + for (int j = 0; j < n; ++j) { + h = eps * internal::abs(x[j]); + if (h == 0.) { + h = eps; + } + switch(mode) { + case Forward: + x[j] += h; + Functor::operator()(x, val2); + nfev++; + x[j] = _x[j]; + jac.col(j) = (val2-val1)/h; + break; + case Central: + x[j] += h; + Functor::operator()(x, val2); nfev++; + x[j] -= 2*h; + Functor::operator()(x, val1); nfev++; + x[j] = _x[j]; + jac.col(j) = (val2-val1)/(2*h); + break; + default: + assert(false); + }; + } + return nfev; + } +private: + Scalar epsfcn; + + NumericalDiff& operator=(const NumericalDiff&); +}; + +} // end namespace Eigen + +//vim: ai ts=4 sts=4 et sw=4 +#endif // EIGEN_NUMERICAL_DIFF_H + diff --git a/unsupported/Eigen/src/Polynomials/CMakeLists.txt b/unsupported/Eigen/src/Polynomials/CMakeLists.txt new file mode 100644 index 000000000..51f13f3cb --- /dev/null +++ b/unsupported/Eigen/src/Polynomials/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_Polynomials_SRCS "*.h") + +INSTALL(FILES + ${Eigen_Polynomials_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/Polynomials COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/Polynomials/Companion.h b/unsupported/Eigen/src/Polynomials/Companion.h new file mode 100644 index 000000000..4badd9d58 --- /dev/null +++ b/unsupported/Eigen/src/Polynomials/Companion.h @@ -0,0 +1,275 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.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_COMPANION_H +#define EIGEN_COMPANION_H + +// This file requires the user to include +// * Eigen/Core +// * Eigen/src/PolynomialSolver.h + +namespace Eigen { + +namespace internal { + +#ifndef EIGEN_PARSED_BY_DOXYGEN + +template <typename T> +T radix(){ return 2; } + +template <typename T> +T radix2(){ return radix<T>()*radix<T>(); } + +template<int Size> +struct decrement_if_fixed_size +{ + enum { + ret = (Size == Dynamic) ? Dynamic : Size-1 }; +}; + +#endif + +template< typename _Scalar, int _Deg > +class companion +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg) + + enum { + Deg = _Deg, + Deg_1=decrement_if_fixed_size<Deg>::ret + }; + + typedef _Scalar Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef Matrix<Scalar, Deg, 1> RightColumn; + //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal; + typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal; + + typedef Matrix<Scalar, Deg, Deg> DenseCompanionMatrixType; + typedef Matrix< Scalar, _Deg, Deg_1 > LeftBlock; + typedef Matrix< Scalar, Deg_1, Deg_1 > BottomLeftBlock; + typedef Matrix< Scalar, 1, Deg_1 > LeftBlockFirstRow; + + typedef DenseIndex Index; + + public: + EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const + { + if( m_bl_diag.rows() > col ) + { + if( 0 < row ){ return m_bl_diag[col]; } + else{ return 0; } + } + else{ return m_monic[row]; } + } + + public: + template<typename VectorType> + void setPolynomial( const VectorType& poly ) + { + const Index deg = poly.size()-1; + m_monic = -1/poly[deg] * poly.head(deg); + //m_bl_diag.setIdentity( deg-1 ); + m_bl_diag.setOnes(deg-1); + } + + template<typename VectorType> + companion( const VectorType& poly ){ + setPolynomial( poly ); } + + public: + DenseCompanionMatrixType denseMatrix() const + { + const Index deg = m_monic.size(); + const Index deg_1 = deg-1; + DenseCompanionMatrixType companion(deg,deg); + companion << + ( LeftBlock(deg,deg_1) + << LeftBlockFirstRow::Zero(1,deg_1), + BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished() + , m_monic; + return companion; + } + + + + protected: + /** Helper function for the balancing algorithm. + * \returns true if the row and the column, having colNorm and rowNorm + * as norms, are balanced, false otherwise. + * colB and rowB are repectively the multipliers for + * the column and the row in order to balance them. + * */ + bool balanced( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ); + + /** Helper function for the balancing algorithm. + * \returns true if the row and the column, having colNorm and rowNorm + * as norms, are balanced, false otherwise. + * colB and rowB are repectively the multipliers for + * the column and the row in order to balance them. + * */ + bool balancedR( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ); + + public: + /** + * Balancing algorithm from B. N. PARLETT and C. REINSCH (1969) + * "Balancing a matrix for calculation of eigenvalues and eigenvectors" + * adapted to the case of companion matrices. + * A matrix with non zero row and non zero column is balanced + * for a certain norm if the i-th row and the i-th column + * have same norm for all i. + */ + void balance(); + + protected: + RightColumn m_monic; + BottomLeftDiagonal m_bl_diag; +}; + + + +template< typename _Scalar, int _Deg > +inline +bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ) +{ + if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; } + else + { + //To find the balancing coefficients, if the radix is 2, + //one finds \f$ \sigma \f$ such that + // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$ + // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$ + // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$ + rowB = rowNorm / radix<Scalar>(); + colB = Scalar(1); + const Scalar s = colNorm + rowNorm; + + while (colNorm < rowB) + { + colB *= radix<Scalar>(); + colNorm *= radix2<Scalar>(); + } + + rowB = rowNorm * radix<Scalar>(); + + while (colNorm >= rowB) + { + colB /= radix<Scalar>(); + colNorm /= radix2<Scalar>(); + } + + //This line is used to avoid insubstantial balancing + if ((rowNorm + colNorm) < Scalar(0.95) * s * colB) + { + isBalanced = false; + rowB = Scalar(1) / colB; + return false; + } + else{ + return true; } + } +} + +template< typename _Scalar, int _Deg > +inline +bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ) +{ + if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; } + else + { + /** + * Set the norm of the column and the row to the geometric mean + * of the row and column norm + */ + const _Scalar q = colNorm/rowNorm; + if( !isApprox( q, _Scalar(1) ) ) + { + rowB = sqrt( colNorm/rowNorm ); + colB = Scalar(1)/rowB; + + isBalanced = false; + return false; + } + else{ + return true; } + } +} + + +template< typename _Scalar, int _Deg > +void companion<_Scalar,_Deg>::balance() +{ + EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE ); + const Index deg = m_monic.size(); + const Index deg_1 = deg-1; + + bool hasConverged=false; + while( !hasConverged ) + { + hasConverged = true; + Scalar colNorm,rowNorm; + Scalar colB,rowB; + + //First row, first column excluding the diagonal + //============================================== + colNorm = abs(m_bl_diag[0]); + rowNorm = abs(m_monic[0]); + + //Compute balancing of the row and the column + if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) ) + { + m_bl_diag[0] *= colB; + m_monic[0] *= rowB; + } + + //Middle rows and columns excluding the diagonal + //============================================== + for( Index i=1; i<deg_1; ++i ) + { + // column norm, excluding the diagonal + colNorm = abs(m_bl_diag[i]); + + // row norm, excluding the diagonal + rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]); + + //Compute balancing of the row and the column + if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) ) + { + m_bl_diag[i] *= colB; + m_bl_diag[i-1] *= rowB; + m_monic[i] *= rowB; + } + } + + //Last row, last column excluding the diagonal + //============================================ + const Index ebl = m_bl_diag.size()-1; + VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 ); + colNorm = headMonic.array().abs().sum(); + rowNorm = abs( m_bl_diag[ebl] ); + + //Compute balancing of the row and the column + if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) ) + { + headMonic *= colB; + m_bl_diag[ebl] *= rowB; + } + } +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_COMPANION_H diff --git a/unsupported/Eigen/src/Polynomials/PolynomialSolver.h b/unsupported/Eigen/src/Polynomials/PolynomialSolver.h new file mode 100644 index 000000000..70b873dbc --- /dev/null +++ b/unsupported/Eigen/src/Polynomials/PolynomialSolver.h @@ -0,0 +1,386 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.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_POLYNOMIAL_SOLVER_H +#define EIGEN_POLYNOMIAL_SOLVER_H + +namespace Eigen { + +/** \ingroup Polynomials_Module + * \class PolynomialSolverBase. + * + * \brief Defined to be inherited by polynomial solvers: it provides + * convenient methods such as + * - real roots, + * - greatest, smallest complex roots, + * - real roots with greatest, smallest absolute real value, + * - greatest, smallest real roots. + * + * It stores the set of roots as a vector of complexes. + * + */ +template< typename _Scalar, int _Deg > +class PolynomialSolverBase +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg) + + typedef _Scalar Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef std::complex<RealScalar> RootType; + typedef Matrix<RootType,_Deg,1> RootsType; + + typedef DenseIndex Index; + + protected: + template< typename OtherPolynomial > + inline void setPolynomial( const OtherPolynomial& poly ){ + m_roots.resize(poly.size()); } + + public: + template< typename OtherPolynomial > + inline PolynomialSolverBase( const OtherPolynomial& poly ){ + setPolynomial( poly() ); } + + inline PolynomialSolverBase(){} + + public: + /** \returns the complex roots of the polynomial */ + inline const RootsType& roots() const { return m_roots; } + + public: + /** Clear and fills the back insertion sequence with the real roots of the polynomial + * i.e. the real part of the complex roots that have an imaginary part which + * absolute value is smaller than absImaginaryThreshold. + * absImaginaryThreshold takes the dummy_precision associated + * with the _Scalar template parameter of the PolynomialSolver class as the default value. + * + * \param[out] bi_seq : the back insertion sequence (stl concept) + * \param[in] absImaginaryThreshold : the maximum bound of the imaginary part of a complex + * number that is considered as real. + * */ + template<typename Stl_back_insertion_sequence> + inline void realRoots( Stl_back_insertion_sequence& bi_seq, + const RealScalar& absImaginaryThreshold = NumTraits<Scalar>::dummy_precision() ) const + { + bi_seq.clear(); + for(Index i=0; i<m_roots.size(); ++i ) + { + if( internal::abs( m_roots[i].imag() ) < absImaginaryThreshold ){ + bi_seq.push_back( m_roots[i].real() ); } + } + } + + protected: + template<typename squaredNormBinaryPredicate> + inline const RootType& selectComplexRoot_withRespectToNorm( squaredNormBinaryPredicate& pred ) const + { + Index res=0; + RealScalar norm2 = internal::abs2( m_roots[0] ); + for( Index i=1; i<m_roots.size(); ++i ) + { + const RealScalar currNorm2 = internal::abs2( m_roots[i] ); + if( pred( currNorm2, norm2 ) ){ + res=i; norm2=currNorm2; } + } + return m_roots[res]; + } + + public: + /** + * \returns the complex root with greatest norm. + */ + inline const RootType& greatestRoot() const + { + std::greater<Scalar> greater; + return selectComplexRoot_withRespectToNorm( greater ); + } + + /** + * \returns the complex root with smallest norm. + */ + inline const RootType& smallestRoot() const + { + std::less<Scalar> less; + return selectComplexRoot_withRespectToNorm( less ); + } + + protected: + template<typename squaredRealPartBinaryPredicate> + inline const RealScalar& selectRealRoot_withRespectToAbsRealPart( + squaredRealPartBinaryPredicate& pred, + bool& hasArealRoot, + const RealScalar& absImaginaryThreshold = NumTraits<Scalar>::dummy_precision() ) const + { + hasArealRoot = false; + Index res=0; + RealScalar abs2(0); + + for( Index i=0; i<m_roots.size(); ++i ) + { + if( internal::abs( m_roots[i].imag() ) < absImaginaryThreshold ) + { + if( !hasArealRoot ) + { + hasArealRoot = true; + res = i; + abs2 = m_roots[i].real() * m_roots[i].real(); + } + else + { + const RealScalar currAbs2 = m_roots[i].real() * m_roots[i].real(); + if( pred( currAbs2, abs2 ) ) + { + abs2 = currAbs2; + res = i; + } + } + } + else + { + if( internal::abs( m_roots[i].imag() ) < internal::abs( m_roots[res].imag() ) ){ + res = i; } + } + } + return internal::real_ref(m_roots[res]); + } + + + template<typename RealPartBinaryPredicate> + inline const RealScalar& selectRealRoot_withRespectToRealPart( + RealPartBinaryPredicate& pred, + bool& hasArealRoot, + const RealScalar& absImaginaryThreshold = NumTraits<Scalar>::dummy_precision() ) const + { + hasArealRoot = false; + Index res=0; + RealScalar val(0); + + for( Index i=0; i<m_roots.size(); ++i ) + { + if( internal::abs( m_roots[i].imag() ) < absImaginaryThreshold ) + { + if( !hasArealRoot ) + { + hasArealRoot = true; + res = i; + val = m_roots[i].real(); + } + else + { + const RealScalar curr = m_roots[i].real(); + if( pred( curr, val ) ) + { + val = curr; + res = i; + } + } + } + else + { + if( internal::abs( m_roots[i].imag() ) < internal::abs( m_roots[res].imag() ) ){ + res = i; } + } + } + return internal::real_ref(m_roots[res]); + } + + public: + /** + * \returns a real root with greatest absolute magnitude. + * A real root is defined as the real part of a complex root with absolute imaginary + * part smallest than absImaginaryThreshold. + * absImaginaryThreshold takes the dummy_precision associated + * with the _Scalar template parameter of the PolynomialSolver class as the default value. + * If no real root is found the boolean hasArealRoot is set to false and the real part of + * the root with smallest absolute imaginary part is returned instead. + * + * \param[out] hasArealRoot : boolean true if a real root is found according to the + * absImaginaryThreshold criterion, false otherwise. + * \param[in] absImaginaryThreshold : threshold on the absolute imaginary part to decide + * whether or not a root is real. + */ + inline const RealScalar& absGreatestRealRoot( + bool& hasArealRoot, + const RealScalar& absImaginaryThreshold = NumTraits<Scalar>::dummy_precision() ) const + { + std::greater<Scalar> greater; + return selectRealRoot_withRespectToAbsRealPart( greater, hasArealRoot, absImaginaryThreshold ); + } + + + /** + * \returns a real root with smallest absolute magnitude. + * A real root is defined as the real part of a complex root with absolute imaginary + * part smallest than absImaginaryThreshold. + * absImaginaryThreshold takes the dummy_precision associated + * with the _Scalar template parameter of the PolynomialSolver class as the default value. + * If no real root is found the boolean hasArealRoot is set to false and the real part of + * the root with smallest absolute imaginary part is returned instead. + * + * \param[out] hasArealRoot : boolean true if a real root is found according to the + * absImaginaryThreshold criterion, false otherwise. + * \param[in] absImaginaryThreshold : threshold on the absolute imaginary part to decide + * whether or not a root is real. + */ + inline const RealScalar& absSmallestRealRoot( + bool& hasArealRoot, + const RealScalar& absImaginaryThreshold = NumTraits<Scalar>::dummy_precision() ) const + { + std::less<Scalar> less; + return selectRealRoot_withRespectToAbsRealPart( less, hasArealRoot, absImaginaryThreshold ); + } + + + /** + * \returns the real root with greatest value. + * A real root is defined as the real part of a complex root with absolute imaginary + * part smallest than absImaginaryThreshold. + * absImaginaryThreshold takes the dummy_precision associated + * with the _Scalar template parameter of the PolynomialSolver class as the default value. + * If no real root is found the boolean hasArealRoot is set to false and the real part of + * the root with smallest absolute imaginary part is returned instead. + * + * \param[out] hasArealRoot : boolean true if a real root is found according to the + * absImaginaryThreshold criterion, false otherwise. + * \param[in] absImaginaryThreshold : threshold on the absolute imaginary part to decide + * whether or not a root is real. + */ + inline const RealScalar& greatestRealRoot( + bool& hasArealRoot, + const RealScalar& absImaginaryThreshold = NumTraits<Scalar>::dummy_precision() ) const + { + std::greater<Scalar> greater; + return selectRealRoot_withRespectToRealPart( greater, hasArealRoot, absImaginaryThreshold ); + } + + + /** + * \returns the real root with smallest value. + * A real root is defined as the real part of a complex root with absolute imaginary + * part smallest than absImaginaryThreshold. + * absImaginaryThreshold takes the dummy_precision associated + * with the _Scalar template parameter of the PolynomialSolver class as the default value. + * If no real root is found the boolean hasArealRoot is set to false and the real part of + * the root with smallest absolute imaginary part is returned instead. + * + * \param[out] hasArealRoot : boolean true if a real root is found according to the + * absImaginaryThreshold criterion, false otherwise. + * \param[in] absImaginaryThreshold : threshold on the absolute imaginary part to decide + * whether or not a root is real. + */ + inline const RealScalar& smallestRealRoot( + bool& hasArealRoot, + const RealScalar& absImaginaryThreshold = NumTraits<Scalar>::dummy_precision() ) const + { + std::less<Scalar> less; + return selectRealRoot_withRespectToRealPart( less, hasArealRoot, absImaginaryThreshold ); + } + + protected: + RootsType m_roots; +}; + +#define EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( BASE ) \ + typedef typename BASE::Scalar Scalar; \ + typedef typename BASE::RealScalar RealScalar; \ + typedef typename BASE::RootType RootType; \ + typedef typename BASE::RootsType RootsType; + + + +/** \ingroup Polynomials_Module + * + * \class PolynomialSolver + * + * \brief A polynomial solver + * + * Computes the complex roots of a real polynomial. + * + * \param _Scalar the scalar type, i.e., the type of the polynomial coefficients + * \param _Deg the degree of the polynomial, can be a compile time value or Dynamic. + * Notice that the number of polynomial coefficients is _Deg+1. + * + * This class implements a polynomial solver and provides convenient methods such as + * - real roots, + * - greatest, smallest complex roots, + * - real roots with greatest, smallest absolute real value. + * - greatest, smallest real roots. + * + * WARNING: this polynomial solver is experimental, part of the unsuported Eigen modules. + * + * + * Currently a QR algorithm is used to compute the eigenvalues of the companion matrix of + * the polynomial to compute its roots. + * This supposes that the complex moduli of the roots are all distinct: e.g. there should + * be no multiple roots or conjugate roots for instance. + * With 32bit (float) floating types this problem shows up frequently. + * However, almost always, correct accuracy is reached even in these cases for 64bit + * (double) floating types and small polynomial degree (<20). + */ +template< typename _Scalar, int _Deg > +class PolynomialSolver : public PolynomialSolverBase<_Scalar,_Deg> +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg) + + typedef PolynomialSolverBase<_Scalar,_Deg> PS_Base; + EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base ) + + typedef Matrix<Scalar,_Deg,_Deg> CompanionMatrixType; + typedef EigenSolver<CompanionMatrixType> EigenSolverType; + + public: + /** Computes the complex roots of a new polynomial. */ + template< typename OtherPolynomial > + void compute( const OtherPolynomial& poly ) + { + assert( Scalar(0) != poly[poly.size()-1] ); + internal::companion<Scalar,_Deg> companion( poly ); + companion.balance(); + m_eigenSolver.compute( companion.denseMatrix() ); + m_roots = m_eigenSolver.eigenvalues(); + } + + public: + template< typename OtherPolynomial > + inline PolynomialSolver( const OtherPolynomial& poly ){ + compute( poly ); } + + inline PolynomialSolver(){} + + protected: + using PS_Base::m_roots; + EigenSolverType m_eigenSolver; +}; + + +template< typename _Scalar > +class PolynomialSolver<_Scalar,1> : public PolynomialSolverBase<_Scalar,1> +{ + public: + typedef PolynomialSolverBase<_Scalar,1> PS_Base; + EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base ) + + public: + /** Computes the complex roots of a new polynomial. */ + template< typename OtherPolynomial > + void compute( const OtherPolynomial& poly ) + { + assert( Scalar(0) != poly[poly.size()-1] ); + m_roots[0] = -poly[0]/poly[poly.size()-1]; + } + + protected: + using PS_Base::m_roots; +}; + +} // end namespace Eigen + +#endif // EIGEN_POLYNOMIAL_SOLVER_H diff --git a/unsupported/Eigen/src/Polynomials/PolynomialUtils.h b/unsupported/Eigen/src/Polynomials/PolynomialUtils.h new file mode 100644 index 000000000..c23204c65 --- /dev/null +++ b/unsupported/Eigen/src/Polynomials/PolynomialUtils.h @@ -0,0 +1,141 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.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_POLYNOMIAL_UTILS_H +#define EIGEN_POLYNOMIAL_UTILS_H + +namespace Eigen { + +/** \ingroup Polynomials_Module + * \returns the evaluation of the polynomial at x using Horner algorithm. + * + * \param[in] poly : the vector of coefficients of the polynomial ordered + * by degrees i.e. poly[i] is the coefficient of degree i of the polynomial + * e.g. \f$ 1 + 3x^2 \f$ is stored as a vector \f$ [ 1, 0, 3 ] \f$. + * \param[in] x : the value to evaluate the polynomial at. + * + * <i><b>Note for stability:</b></i> + * <dd> \f$ |x| \le 1 \f$ </dd> + */ +template <typename Polynomials, typename T> +inline +T poly_eval_horner( const Polynomials& poly, const T& x ) +{ + T val=poly[poly.size()-1]; + for(DenseIndex i=poly.size()-2; i>=0; --i ){ + val = val*x + poly[i]; } + return val; +} + +/** \ingroup Polynomials_Module + * \returns the evaluation of the polynomial at x using stabilized Horner algorithm. + * + * \param[in] poly : the vector of coefficients of the polynomial ordered + * by degrees i.e. poly[i] is the coefficient of degree i of the polynomial + * e.g. \f$ 1 + 3x^2 \f$ is stored as a vector \f$ [ 1, 0, 3 ] \f$. + * \param[in] x : the value to evaluate the polynomial at. + */ +template <typename Polynomials, typename T> +inline +T poly_eval( const Polynomials& poly, const T& x ) +{ + typedef typename NumTraits<T>::Real Real; + + if( internal::abs2( x ) <= Real(1) ){ + return poly_eval_horner( poly, x ); } + else + { + T val=poly[0]; + T inv_x = T(1)/x; + for( DenseIndex i=1; i<poly.size(); ++i ){ + val = val*inv_x + poly[i]; } + + return std::pow(x,(T)(poly.size()-1)) * val; + } +} + +/** \ingroup Polynomials_Module + * \returns a maximum bound for the absolute value of any root of the polynomial. + * + * \param[in] poly : the vector of coefficients of the polynomial ordered + * by degrees i.e. poly[i] is the coefficient of degree i of the polynomial + * e.g. \f$ 1 + 3x^2 \f$ is stored as a vector \f$ [ 1, 0, 3 ] \f$. + * + * <i><b>Precondition:</b></i> + * <dd> the leading coefficient of the input polynomial poly must be non zero </dd> + */ +template <typename Polynomial> +inline +typename NumTraits<typename Polynomial::Scalar>::Real cauchy_max_bound( const Polynomial& poly ) +{ + typedef typename Polynomial::Scalar Scalar; + typedef typename NumTraits<Scalar>::Real Real; + + assert( Scalar(0) != poly[poly.size()-1] ); + const Scalar inv_leading_coeff = Scalar(1)/poly[poly.size()-1]; + Real cb(0); + + for( DenseIndex i=0; i<poly.size()-1; ++i ){ + cb += internal::abs(poly[i]*inv_leading_coeff); } + return cb + Real(1); +} + +/** \ingroup Polynomials_Module + * \returns a minimum bound for the absolute value of any non zero root of the polynomial. + * \param[in] poly : the vector of coefficients of the polynomial ordered + * by degrees i.e. poly[i] is the coefficient of degree i of the polynomial + * e.g. \f$ 1 + 3x^2 \f$ is stored as a vector \f$ [ 1, 0, 3 ] \f$. + */ +template <typename Polynomial> +inline +typename NumTraits<typename Polynomial::Scalar>::Real cauchy_min_bound( const Polynomial& poly ) +{ + typedef typename Polynomial::Scalar Scalar; + typedef typename NumTraits<Scalar>::Real Real; + + DenseIndex i=0; + while( i<poly.size()-1 && Scalar(0) == poly(i) ){ ++i; } + if( poly.size()-1 == i ){ + return Real(1); } + + const Scalar inv_min_coeff = Scalar(1)/poly[i]; + Real cb(1); + for( DenseIndex j=i+1; j<poly.size(); ++j ){ + cb += internal::abs(poly[j]*inv_min_coeff); } + return Real(1)/cb; +} + +/** \ingroup Polynomials_Module + * Given the roots of a polynomial compute the coefficients in the + * monomial basis of the monic polynomial with same roots and minimal degree. + * If RootVector is a vector of complexes, Polynomial should also be a vector + * of complexes. + * \param[in] rv : a vector containing the roots of a polynomial. + * \param[out] poly : the vector of coefficients of the polynomial ordered + * by degrees i.e. poly[i] is the coefficient of degree i of the polynomial + * e.g. \f$ 3 + x^2 \f$ is stored as a vector \f$ [ 3, 0, 1 ] \f$. + */ +template <typename RootVector, typename Polynomial> +void roots_to_monicPolynomial( const RootVector& rv, Polynomial& poly ) +{ + + typedef typename Polynomial::Scalar Scalar; + + poly.setZero( rv.size()+1 ); + poly[0] = -rv[0]; poly[1] = Scalar(1); + for( DenseIndex i=1; i< rv.size(); ++i ) + { + for( DenseIndex j=i+1; j>0; --j ){ poly[j] = poly[j-1] - rv[i]*poly[j]; } + poly[0] = -rv[i]*poly[0]; + } +} + +} // end namespace Eigen + +#endif // EIGEN_POLYNOMIAL_UTILS_H diff --git a/unsupported/Eigen/src/Skyline/CMakeLists.txt b/unsupported/Eigen/src/Skyline/CMakeLists.txt new file mode 100644 index 000000000..3bf1b0dd4 --- /dev/null +++ b/unsupported/Eigen/src/Skyline/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_Skyline_SRCS "*.h") + +INSTALL(FILES + ${Eigen_Skyline_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/Skyline COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/Skyline/SkylineInplaceLU.h b/unsupported/Eigen/src/Skyline/SkylineInplaceLU.h new file mode 100644 index 000000000..a1f54ed35 --- /dev/null +++ b/unsupported/Eigen/src/Skyline/SkylineInplaceLU.h @@ -0,0 +1,352 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Guillaume Saupin <guillaume.saupin@cea.fr> +// +// 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_SKYLINEINPLACELU_H +#define EIGEN_SKYLINEINPLACELU_H + +namespace Eigen { + +/** \ingroup Skyline_Module + * + * \class SkylineInplaceLU + * + * \brief Inplace LU decomposition of a skyline matrix and associated features + * + * \param MatrixType the type of the matrix of which we are computing the LU factorization + * + */ +template<typename MatrixType> +class SkylineInplaceLU { +protected: + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::Index Index; + + typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar; + +public: + + /** Creates a LU object and compute the respective factorization of \a matrix using + * flags \a flags. */ + SkylineInplaceLU(MatrixType& matrix, int flags = 0) + : /*m_matrix(matrix.rows(), matrix.cols()),*/ m_flags(flags), m_status(0), m_lu(matrix) { + m_precision = RealScalar(0.1) * Eigen::dummy_precision<RealScalar > (); + m_lu.IsRowMajor ? computeRowMajor() : compute(); + } + + /** Sets the relative threshold value used to prune zero coefficients during the decomposition. + * + * Setting a value greater than zero speeds up computation, and yields to an imcomplete + * factorization with fewer non zero coefficients. Such approximate factors are especially + * useful to initialize an iterative solver. + * + * Note that the exact meaning of this parameter might depends on the actual + * backend. Moreover, not all backends support this feature. + * + * \sa precision() */ + void setPrecision(RealScalar v) { + m_precision = v; + } + + /** \returns the current precision. + * + * \sa setPrecision() */ + RealScalar precision() const { + return m_precision; + } + + /** Sets the flags. Possible values are: + * - CompleteFactorization + * - IncompleteFactorization + * - MemoryEfficient + * - one of the ordering methods + * - etc... + * + * \sa flags() */ + void setFlags(int f) { + m_flags = f; + } + + /** \returns the current flags */ + int flags() const { + return m_flags; + } + + void setOrderingMethod(int m) { + m_flags = m; + } + + int orderingMethod() const { + return m_flags; + } + + /** Computes/re-computes the LU factorization */ + void compute(); + void computeRowMajor(); + + /** \returns the lower triangular matrix L */ + //inline const MatrixType& matrixL() const { return m_matrixL; } + + /** \returns the upper triangular matrix U */ + //inline const MatrixType& matrixU() const { return m_matrixU; } + + template<typename BDerived, typename XDerived> + bool solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x, + const int transposed = 0) const; + + /** \returns true if the factorization succeeded */ + inline bool succeeded(void) const { + return m_succeeded; + } + +protected: + RealScalar m_precision; + int m_flags; + mutable int m_status; + bool m_succeeded; + MatrixType& m_lu; +}; + +/** Computes / recomputes the in place LU decomposition of the SkylineInplaceLU. + * using the default algorithm. + */ +template<typename MatrixType> +//template<typename _Scalar> +void SkylineInplaceLU<MatrixType>::compute() { + const size_t rows = m_lu.rows(); + const size_t cols = m_lu.cols(); + + eigen_assert(rows == cols && "We do not (yet) support rectangular LU."); + eigen_assert(!m_lu.IsRowMajor && "LU decomposition does not work with rowMajor Storage"); + + for (Index row = 0; row < rows; row++) { + const double pivot = m_lu.coeffDiag(row); + + //Lower matrix Columns update + const Index& col = row; + for (typename MatrixType::InnerLowerIterator lIt(m_lu, col); lIt; ++lIt) { + lIt.valueRef() /= pivot; + } + + //Upper matrix update -> contiguous memory access + typename MatrixType::InnerLowerIterator lIt(m_lu, col); + for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) { + typename MatrixType::InnerUpperIterator uItPivot(m_lu, row); + typename MatrixType::InnerUpperIterator uIt(m_lu, rrow); + const double coef = lIt.value(); + + uItPivot += (rrow - row - 1); + + //update upper part -> contiguous memory access + for (++uItPivot; uIt && uItPivot;) { + uIt.valueRef() -= uItPivot.value() * coef; + + ++uIt; + ++uItPivot; + } + ++lIt; + } + + //Upper matrix update -> non contiguous memory access + typename MatrixType::InnerLowerIterator lIt3(m_lu, col); + for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) { + typename MatrixType::InnerUpperIterator uItPivot(m_lu, row); + const double coef = lIt3.value(); + + //update lower part -> non contiguous memory access + for (Index i = 0; i < rrow - row - 1; i++) { + m_lu.coeffRefLower(rrow, row + i + 1) -= uItPivot.value() * coef; + ++uItPivot; + } + ++lIt3; + } + //update diag -> contiguous + typename MatrixType::InnerLowerIterator lIt2(m_lu, col); + for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) { + + typename MatrixType::InnerUpperIterator uItPivot(m_lu, row); + typename MatrixType::InnerUpperIterator uIt(m_lu, rrow); + const double coef = lIt2.value(); + + uItPivot += (rrow - row - 1); + m_lu.coeffRefDiag(rrow) -= uItPivot.value() * coef; + ++lIt2; + } + } +} + +template<typename MatrixType> +void SkylineInplaceLU<MatrixType>::computeRowMajor() { + const size_t rows = m_lu.rows(); + const size_t cols = m_lu.cols(); + + eigen_assert(rows == cols && "We do not (yet) support rectangular LU."); + eigen_assert(m_lu.IsRowMajor && "You're trying to apply rowMajor decomposition on a ColMajor matrix !"); + + for (Index row = 0; row < rows; row++) { + typename MatrixType::InnerLowerIterator llIt(m_lu, row); + + + for (Index col = llIt.col(); col < row; col++) { + if (m_lu.coeffExistLower(row, col)) { + const double diag = m_lu.coeffDiag(col); + + typename MatrixType::InnerLowerIterator lIt(m_lu, row); + typename MatrixType::InnerUpperIterator uIt(m_lu, col); + + + const Index offset = lIt.col() - uIt.row(); + + + Index stop = offset > 0 ? col - lIt.col() : col - uIt.row(); + + //#define VECTORIZE +#ifdef VECTORIZE + Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop); + Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop); + + + Scalar newCoeff = m_lu.coeffLower(row, col) - rowVal.dot(colVal); +#else + if (offset > 0) //Skip zero value of lIt + uIt += offset; + else //Skip zero values of uIt + lIt += -offset; + Scalar newCoeff = m_lu.coeffLower(row, col); + + for (Index k = 0; k < stop; ++k) { + const Scalar tmp = newCoeff; + newCoeff = tmp - lIt.value() * uIt.value(); + ++lIt; + ++uIt; + } +#endif + + m_lu.coeffRefLower(row, col) = newCoeff / diag; + } + } + + //Upper matrix update + const Index col = row; + typename MatrixType::InnerUpperIterator uuIt(m_lu, col); + for (Index rrow = uuIt.row(); rrow < col; rrow++) { + + typename MatrixType::InnerLowerIterator lIt(m_lu, rrow); + typename MatrixType::InnerUpperIterator uIt(m_lu, col); + const Index offset = lIt.col() - uIt.row(); + + Index stop = offset > 0 ? rrow - lIt.col() : rrow - uIt.row(); + +#ifdef VECTORIZE + Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop); + Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop); + + Scalar newCoeff = m_lu.coeffUpper(rrow, col) - rowVal.dot(colVal); +#else + if (offset > 0) //Skip zero value of lIt + uIt += offset; + else //Skip zero values of uIt + lIt += -offset; + Scalar newCoeff = m_lu.coeffUpper(rrow, col); + for (Index k = 0; k < stop; ++k) { + const Scalar tmp = newCoeff; + newCoeff = tmp - lIt.value() * uIt.value(); + + ++lIt; + ++uIt; + } +#endif + m_lu.coeffRefUpper(rrow, col) = newCoeff; + } + + + //Diag matrix update + typename MatrixType::InnerLowerIterator lIt(m_lu, row); + typename MatrixType::InnerUpperIterator uIt(m_lu, row); + + const Index offset = lIt.col() - uIt.row(); + + + Index stop = offset > 0 ? lIt.size() : uIt.size(); +#ifdef VECTORIZE + Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop); + Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop); + Scalar newCoeff = m_lu.coeffDiag(row) - rowVal.dot(colVal); +#else + if (offset > 0) //Skip zero value of lIt + uIt += offset; + else //Skip zero values of uIt + lIt += -offset; + Scalar newCoeff = m_lu.coeffDiag(row); + for (Index k = 0; k < stop; ++k) { + const Scalar tmp = newCoeff; + newCoeff = tmp - lIt.value() * uIt.value(); + ++lIt; + ++uIt; + } +#endif + m_lu.coeffRefDiag(row) = newCoeff; + } +} + +/** Computes *x = U^-1 L^-1 b + * + * If \a transpose is set to SvTranspose or SvAdjoint, the solution + * of the transposed/adjoint system is computed instead. + * + * Not all backends implement the solution of the transposed or + * adjoint system. + */ +template<typename MatrixType> +template<typename BDerived, typename XDerived> +bool SkylineInplaceLU<MatrixType>::solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x, const int transposed) const { + const size_t rows = m_lu.rows(); + const size_t cols = m_lu.cols(); + + + for (Index row = 0; row < rows; row++) { + x->coeffRef(row) = b.coeff(row); + Scalar newVal = x->coeff(row); + typename MatrixType::InnerLowerIterator lIt(m_lu, row); + + Index col = lIt.col(); + while (lIt.col() < row) { + + newVal -= x->coeff(col++) * lIt.value(); + ++lIt; + } + + x->coeffRef(row) = newVal; + } + + + for (Index col = rows - 1; col > 0; col--) { + x->coeffRef(col) = x->coeff(col) / m_lu.coeffDiag(col); + + const Scalar x_col = x->coeff(col); + + typename MatrixType::InnerUpperIterator uIt(m_lu, col); + uIt += uIt.size()-1; + + + while (uIt) { + x->coeffRef(uIt.row()) -= x_col * uIt.value(); + //TODO : introduce --operator + uIt += -1; + } + + + } + x->coeffRef(0) = x->coeff(0) / m_lu.coeffDiag(0); + + return true; +} + +} // end namespace Eigen + +#endif // EIGEN_SKYLINELU_H diff --git a/unsupported/Eigen/src/Skyline/SkylineMatrix.h b/unsupported/Eigen/src/Skyline/SkylineMatrix.h new file mode 100644 index 000000000..a2a8933ca --- /dev/null +++ b/unsupported/Eigen/src/Skyline/SkylineMatrix.h @@ -0,0 +1,862 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Guillaume Saupin <guillaume.saupin@cea.fr> +// +// 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_SKYLINEMATRIX_H +#define EIGEN_SKYLINEMATRIX_H + +#include "SkylineStorage.h" +#include "SkylineMatrixBase.h" + +namespace Eigen { + +/** \ingroup Skyline_Module + * + * \class SkylineMatrix + * + * \brief The main skyline matrix class + * + * This class implements a skyline matrix using the very uncommon storage + * scheme. + * + * \param _Scalar the scalar type, i.e. the type of the coefficients + * \param _Options Union of bit flags controlling the storage scheme. Currently the only possibility + * is RowMajor. The default is 0 which means column-major. + * + * + */ +namespace internal { +template<typename _Scalar, int _Options> +struct traits<SkylineMatrix<_Scalar, _Options> > { + typedef _Scalar Scalar; + typedef Sparse StorageKind; + + enum { + RowsAtCompileTime = Dynamic, + ColsAtCompileTime = Dynamic, + MaxRowsAtCompileTime = Dynamic, + MaxColsAtCompileTime = Dynamic, + Flags = SkylineBit | _Options, + CoeffReadCost = NumTraits<Scalar>::ReadCost, + }; +}; +} + +template<typename _Scalar, int _Options> +class SkylineMatrix +: public SkylineMatrixBase<SkylineMatrix<_Scalar, _Options> > { +public: + EIGEN_SKYLINE_GENERIC_PUBLIC_INTERFACE(SkylineMatrix) + EIGEN_SKYLINE_INHERIT_ASSIGNMENT_OPERATOR(SkylineMatrix, +=) + EIGEN_SKYLINE_INHERIT_ASSIGNMENT_OPERATOR(SkylineMatrix, -=) + + using Base::IsRowMajor; + +protected: + + typedef SkylineMatrix<Scalar, (Flags&~RowMajorBit) | (IsRowMajor ? RowMajorBit : 0) > TransposedSkylineMatrix; + + Index m_outerSize; + Index m_innerSize; + +public: + Index* m_colStartIndex; + Index* m_rowStartIndex; + SkylineStorage<Scalar> m_data; + +public: + + inline Index rows() const { + return IsRowMajor ? m_outerSize : m_innerSize; + } + + inline Index cols() const { + return IsRowMajor ? m_innerSize : m_outerSize; + } + + inline Index innerSize() const { + return m_innerSize; + } + + inline Index outerSize() const { + return m_outerSize; + } + + inline Index upperNonZeros() const { + return m_data.upperSize(); + } + + inline Index lowerNonZeros() const { + return m_data.lowerSize(); + } + + inline Index upperNonZeros(Index j) const { + return m_colStartIndex[j + 1] - m_colStartIndex[j]; + } + + inline Index lowerNonZeros(Index j) const { + return m_rowStartIndex[j + 1] - m_rowStartIndex[j]; + } + + inline const Scalar* _diagPtr() const { + return &m_data.diag(0); + } + + inline Scalar* _diagPtr() { + return &m_data.diag(0); + } + + inline const Scalar* _upperPtr() const { + return &m_data.upper(0); + } + + inline Scalar* _upperPtr() { + return &m_data.upper(0); + } + + inline const Scalar* _lowerPtr() const { + return &m_data.lower(0); + } + + inline Scalar* _lowerPtr() { + return &m_data.lower(0); + } + + inline const Index* _upperProfilePtr() const { + return &m_data.upperProfile(0); + } + + inline Index* _upperProfilePtr() { + return &m_data.upperProfile(0); + } + + inline const Index* _lowerProfilePtr() const { + return &m_data.lowerProfile(0); + } + + inline Index* _lowerProfilePtr() { + return &m_data.lowerProfile(0); + } + + inline Scalar coeff(Index row, Index col) const { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + + if (outer == inner) + return this->m_data.diag(outer); + + if (IsRowMajor) { + if (inner > outer) //upper matrix + { + const Index minOuterIndex = inner - m_data.upperProfile(inner); + if (outer >= minOuterIndex) + return this->m_data.upper(m_colStartIndex[inner] + outer - (inner - m_data.upperProfile(inner))); + else + return Scalar(0); + } + if (inner < outer) //lower matrix + { + const Index minInnerIndex = outer - m_data.lowerProfile(outer); + if (inner >= minInnerIndex) + return this->m_data.lower(m_rowStartIndex[outer] + inner - (outer - m_data.lowerProfile(outer))); + else + return Scalar(0); + } + return m_data.upper(m_colStartIndex[inner] + outer - inner); + } else { + if (outer > inner) //upper matrix + { + const Index maxOuterIndex = inner + m_data.upperProfile(inner); + if (outer <= maxOuterIndex) + return this->m_data.upper(m_colStartIndex[inner] + (outer - inner)); + else + return Scalar(0); + } + if (outer < inner) //lower matrix + { + const Index maxInnerIndex = outer + m_data.lowerProfile(outer); + + if (inner <= maxInnerIndex) + return this->m_data.lower(m_rowStartIndex[outer] + (inner - outer)); + else + return Scalar(0); + } + } + } + + inline Scalar& coeffRef(Index row, Index col) { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + + if (outer == inner) + return this->m_data.diag(outer); + + if (IsRowMajor) { + if (col > row) //upper matrix + { + const Index minOuterIndex = inner - m_data.upperProfile(inner); + eigen_assert(outer >= minOuterIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.upper(m_colStartIndex[inner] + outer - (inner - m_data.upperProfile(inner))); + } + if (col < row) //lower matrix + { + const Index minInnerIndex = outer - m_data.lowerProfile(outer); + eigen_assert(inner >= minInnerIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.lower(m_rowStartIndex[outer] + inner - (outer - m_data.lowerProfile(outer))); + } + } else { + if (outer > inner) //upper matrix + { + const Index maxOuterIndex = inner + m_data.upperProfile(inner); + eigen_assert(outer <= maxOuterIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.upper(m_colStartIndex[inner] + (outer - inner)); + } + if (outer < inner) //lower matrix + { + const Index maxInnerIndex = outer + m_data.lowerProfile(outer); + eigen_assert(inner <= maxInnerIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.lower(m_rowStartIndex[outer] + (inner - outer)); + } + } + } + + inline Scalar coeffDiag(Index idx) const { + eigen_assert(idx < outerSize()); + eigen_assert(idx < innerSize()); + return this->m_data.diag(idx); + } + + inline Scalar coeffLower(Index row, Index col) const { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + eigen_assert(inner != outer); + + if (IsRowMajor) { + const Index minInnerIndex = outer - m_data.lowerProfile(outer); + if (inner >= minInnerIndex) + return this->m_data.lower(m_rowStartIndex[outer] + inner - (outer - m_data.lowerProfile(outer))); + else + return Scalar(0); + + } else { + const Index maxInnerIndex = outer + m_data.lowerProfile(outer); + if (inner <= maxInnerIndex) + return this->m_data.lower(m_rowStartIndex[outer] + (inner - outer)); + else + return Scalar(0); + } + } + + inline Scalar coeffUpper(Index row, Index col) const { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + eigen_assert(inner != outer); + + if (IsRowMajor) { + const Index minOuterIndex = inner - m_data.upperProfile(inner); + if (outer >= minOuterIndex) + return this->m_data.upper(m_colStartIndex[inner] + outer - (inner - m_data.upperProfile(inner))); + else + return Scalar(0); + } else { + const Index maxOuterIndex = inner + m_data.upperProfile(inner); + if (outer <= maxOuterIndex) + return this->m_data.upper(m_colStartIndex[inner] + (outer - inner)); + else + return Scalar(0); + } + } + + inline Scalar& coeffRefDiag(Index idx) { + eigen_assert(idx < outerSize()); + eigen_assert(idx < innerSize()); + return this->m_data.diag(idx); + } + + inline Scalar& coeffRefLower(Index row, Index col) { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + eigen_assert(inner != outer); + + if (IsRowMajor) { + const Index minInnerIndex = outer - m_data.lowerProfile(outer); + eigen_assert(inner >= minInnerIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.lower(m_rowStartIndex[outer] + inner - (outer - m_data.lowerProfile(outer))); + } else { + const Index maxInnerIndex = outer + m_data.lowerProfile(outer); + eigen_assert(inner <= maxInnerIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.lower(m_rowStartIndex[outer] + (inner - outer)); + } + } + + inline bool coeffExistLower(Index row, Index col) { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + eigen_assert(inner != outer); + + if (IsRowMajor) { + const Index minInnerIndex = outer - m_data.lowerProfile(outer); + return inner >= minInnerIndex; + } else { + const Index maxInnerIndex = outer + m_data.lowerProfile(outer); + return inner <= maxInnerIndex; + } + } + + inline Scalar& coeffRefUpper(Index row, Index col) { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + eigen_assert(inner != outer); + + if (IsRowMajor) { + const Index minOuterIndex = inner - m_data.upperProfile(inner); + eigen_assert(outer >= minOuterIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.upper(m_colStartIndex[inner] + outer - (inner - m_data.upperProfile(inner))); + } else { + const Index maxOuterIndex = inner + m_data.upperProfile(inner); + eigen_assert(outer <= maxOuterIndex && "you try to acces a coeff that do not exist in the storage"); + return this->m_data.upper(m_colStartIndex[inner] + (outer - inner)); + } + } + + inline bool coeffExistUpper(Index row, Index col) { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + eigen_assert(inner != outer); + + if (IsRowMajor) { + const Index minOuterIndex = inner - m_data.upperProfile(inner); + return outer >= minOuterIndex; + } else { + const Index maxOuterIndex = inner + m_data.upperProfile(inner); + return outer <= maxOuterIndex; + } + } + + +protected: + +public: + class InnerUpperIterator; + class InnerLowerIterator; + + class OuterUpperIterator; + class OuterLowerIterator; + + /** Removes all non zeros */ + inline void setZero() { + m_data.clear(); + memset(m_colStartIndex, 0, (m_outerSize + 1) * sizeof (Index)); + memset(m_rowStartIndex, 0, (m_outerSize + 1) * sizeof (Index)); + } + + /** \returns the number of non zero coefficients */ + inline Index nonZeros() const { + return m_data.diagSize() + m_data.upperSize() + m_data.lowerSize(); + } + + /** Preallocates \a reserveSize non zeros */ + inline void reserve(Index reserveSize, Index reserveUpperSize, Index reserveLowerSize) { + m_data.reserve(reserveSize, reserveUpperSize, reserveLowerSize); + } + + /** \returns a reference to a novel non zero coefficient with coordinates \a row x \a col. + + * + * \warning This function can be extremely slow if the non zero coefficients + * are not inserted in a coherent order. + * + * After an insertion session, you should call the finalize() function. + */ + EIGEN_DONT_INLINE Scalar & insert(Index row, Index col) { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + eigen_assert(outer < outerSize()); + eigen_assert(inner < innerSize()); + + if (outer == inner) + return m_data.diag(col); + + if (IsRowMajor) { + if (outer < inner) //upper matrix + { + Index minOuterIndex = 0; + minOuterIndex = inner - m_data.upperProfile(inner); + + if (outer < minOuterIndex) //The value does not yet exist + { + const Index previousProfile = m_data.upperProfile(inner); + + m_data.upperProfile(inner) = inner - outer; + + + const Index bandIncrement = m_data.upperProfile(inner) - previousProfile; + //shift data stored after this new one + const Index stop = m_colStartIndex[cols()]; + const Index start = m_colStartIndex[inner]; + + + for (Index innerIdx = stop; innerIdx >= start; innerIdx--) { + m_data.upper(innerIdx + bandIncrement) = m_data.upper(innerIdx); + } + + for (Index innerIdx = cols(); innerIdx > inner; innerIdx--) { + m_colStartIndex[innerIdx] += bandIncrement; + } + + //zeros new data + memset(this->_upperPtr() + start, 0, (bandIncrement - 1) * sizeof (Scalar)); + + return m_data.upper(m_colStartIndex[inner]); + } else { + return m_data.upper(m_colStartIndex[inner] + outer - (inner - m_data.upperProfile(inner))); + } + } + + if (outer > inner) //lower matrix + { + const Index minInnerIndex = outer - m_data.lowerProfile(outer); + if (inner < minInnerIndex) //The value does not yet exist + { + const Index previousProfile = m_data.lowerProfile(outer); + m_data.lowerProfile(outer) = outer - inner; + + const Index bandIncrement = m_data.lowerProfile(outer) - previousProfile; + //shift data stored after this new one + const Index stop = m_rowStartIndex[rows()]; + const Index start = m_rowStartIndex[outer]; + + + for (Index innerIdx = stop; innerIdx >= start; innerIdx--) { + m_data.lower(innerIdx + bandIncrement) = m_data.lower(innerIdx); + } + + for (Index innerIdx = rows(); innerIdx > outer; innerIdx--) { + m_rowStartIndex[innerIdx] += bandIncrement; + } + + //zeros new data + memset(this->_lowerPtr() + start, 0, (bandIncrement - 1) * sizeof (Scalar)); + return m_data.lower(m_rowStartIndex[outer]); + } else { + return m_data.lower(m_rowStartIndex[outer] + inner - (outer - m_data.lowerProfile(outer))); + } + } + } else { + if (outer > inner) //upper matrix + { + const Index maxOuterIndex = inner + m_data.upperProfile(inner); + if (outer > maxOuterIndex) //The value does not yet exist + { + const Index previousProfile = m_data.upperProfile(inner); + m_data.upperProfile(inner) = outer - inner; + + const Index bandIncrement = m_data.upperProfile(inner) - previousProfile; + //shift data stored after this new one + const Index stop = m_rowStartIndex[rows()]; + const Index start = m_rowStartIndex[inner + 1]; + + for (Index innerIdx = stop; innerIdx >= start; innerIdx--) { + m_data.upper(innerIdx + bandIncrement) = m_data.upper(innerIdx); + } + + for (Index innerIdx = inner + 1; innerIdx < outerSize() + 1; innerIdx++) { + m_rowStartIndex[innerIdx] += bandIncrement; + } + memset(this->_upperPtr() + m_rowStartIndex[inner] + previousProfile + 1, 0, (bandIncrement - 1) * sizeof (Scalar)); + return m_data.upper(m_rowStartIndex[inner] + m_data.upperProfile(inner)); + } else { + return m_data.upper(m_rowStartIndex[inner] + (outer - inner)); + } + } + + if (outer < inner) //lower matrix + { + const Index maxInnerIndex = outer + m_data.lowerProfile(outer); + if (inner > maxInnerIndex) //The value does not yet exist + { + const Index previousProfile = m_data.lowerProfile(outer); + m_data.lowerProfile(outer) = inner - outer; + + const Index bandIncrement = m_data.lowerProfile(outer) - previousProfile; + //shift data stored after this new one + const Index stop = m_colStartIndex[cols()]; + const Index start = m_colStartIndex[outer + 1]; + + for (Index innerIdx = stop; innerIdx >= start; innerIdx--) { + m_data.lower(innerIdx + bandIncrement) = m_data.lower(innerIdx); + } + + for (Index innerIdx = outer + 1; innerIdx < outerSize() + 1; innerIdx++) { + m_colStartIndex[innerIdx] += bandIncrement; + } + memset(this->_lowerPtr() + m_colStartIndex[outer] + previousProfile + 1, 0, (bandIncrement - 1) * sizeof (Scalar)); + return m_data.lower(m_colStartIndex[outer] + m_data.lowerProfile(outer)); + } else { + return m_data.lower(m_colStartIndex[outer] + (inner - outer)); + } + } + } + } + + /** Must be called after inserting a set of non zero entries. + */ + inline void finalize() { + if (IsRowMajor) { + if (rows() > cols()) + m_data.resize(cols(), cols(), rows(), m_colStartIndex[cols()] + 1, m_rowStartIndex[rows()] + 1); + else + m_data.resize(rows(), cols(), rows(), m_colStartIndex[cols()] + 1, m_rowStartIndex[rows()] + 1); + + // eigen_assert(rows() == cols() && "memory reorganisatrion only works with suare matrix"); + // + // Scalar* newArray = new Scalar[m_colStartIndex[cols()] + 1 + m_rowStartIndex[rows()] + 1]; + // Index dataIdx = 0; + // for (Index row = 0; row < rows(); row++) { + // + // const Index nbLowerElts = m_rowStartIndex[row + 1] - m_rowStartIndex[row]; + // // std::cout << "nbLowerElts" << nbLowerElts << std::endl; + // memcpy(newArray + dataIdx, m_data.m_lower + m_rowStartIndex[row], nbLowerElts * sizeof (Scalar)); + // m_rowStartIndex[row] = dataIdx; + // dataIdx += nbLowerElts; + // + // const Index nbUpperElts = m_colStartIndex[row + 1] - m_colStartIndex[row]; + // memcpy(newArray + dataIdx, m_data.m_upper + m_colStartIndex[row], nbUpperElts * sizeof (Scalar)); + // m_colStartIndex[row] = dataIdx; + // dataIdx += nbUpperElts; + // + // + // } + // //todo : don't access m_data profile directly : add an accessor from SkylineMatrix + // m_rowStartIndex[rows()] = m_rowStartIndex[rows()-1] + m_data.lowerProfile(rows()-1); + // m_colStartIndex[cols()] = m_colStartIndex[cols()-1] + m_data.upperProfile(cols()-1); + // + // delete[] m_data.m_lower; + // delete[] m_data.m_upper; + // + // m_data.m_lower = newArray; + // m_data.m_upper = newArray; + } else { + if (rows() > cols()) + m_data.resize(cols(), rows(), cols(), m_rowStartIndex[cols()] + 1, m_colStartIndex[cols()] + 1); + else + m_data.resize(rows(), rows(), cols(), m_rowStartIndex[rows()] + 1, m_colStartIndex[rows()] + 1); + } + } + + inline void squeeze() { + finalize(); + m_data.squeeze(); + } + + void prune(Scalar reference, RealScalar epsilon = dummy_precision<RealScalar > ()) { + //TODO + } + + /** Resizes the matrix to a \a rows x \a cols matrix and initializes it to zero + * \sa resizeNonZeros(Index), reserve(), setZero() + */ + void resize(size_t rows, size_t cols) { + const Index diagSize = rows > cols ? cols : rows; + m_innerSize = IsRowMajor ? cols : rows; + + eigen_assert(rows == cols && "Skyline matrix must be square matrix"); + + if (diagSize % 2) { // diagSize is odd + const Index k = (diagSize - 1) / 2; + + m_data.resize(diagSize, IsRowMajor ? cols : rows, IsRowMajor ? rows : cols, + 2 * k * k + k + 1, + 2 * k * k + k + 1); + + } else // diagSize is even + { + const Index k = diagSize / 2; + m_data.resize(diagSize, IsRowMajor ? cols : rows, IsRowMajor ? rows : cols, + 2 * k * k - k + 1, + 2 * k * k - k + 1); + } + + if (m_colStartIndex && m_rowStartIndex) { + delete[] m_colStartIndex; + delete[] m_rowStartIndex; + } + m_colStartIndex = new Index [cols + 1]; + m_rowStartIndex = new Index [rows + 1]; + m_outerSize = diagSize; + + m_data.reset(); + m_data.clear(); + + m_outerSize = diagSize; + memset(m_colStartIndex, 0, (cols + 1) * sizeof (Index)); + memset(m_rowStartIndex, 0, (rows + 1) * sizeof (Index)); + } + + void resizeNonZeros(Index size) { + m_data.resize(size); + } + + inline SkylineMatrix() + : m_outerSize(-1), m_innerSize(0), m_colStartIndex(0), m_rowStartIndex(0) { + resize(0, 0); + } + + inline SkylineMatrix(size_t rows, size_t cols) + : m_outerSize(0), m_innerSize(0), m_colStartIndex(0), m_rowStartIndex(0) { + resize(rows, cols); + } + + template<typename OtherDerived> + inline SkylineMatrix(const SkylineMatrixBase<OtherDerived>& other) + : m_outerSize(0), m_innerSize(0), m_colStartIndex(0), m_rowStartIndex(0) { + *this = other.derived(); + } + + inline SkylineMatrix(const SkylineMatrix & other) + : Base(), m_outerSize(0), m_innerSize(0), m_colStartIndex(0), m_rowStartIndex(0) { + *this = other.derived(); + } + + inline void swap(SkylineMatrix & other) { + //EIGEN_DBG_SKYLINE(std::cout << "SkylineMatrix:: swap\n"); + std::swap(m_colStartIndex, other.m_colStartIndex); + std::swap(m_rowStartIndex, other.m_rowStartIndex); + std::swap(m_innerSize, other.m_innerSize); + std::swap(m_outerSize, other.m_outerSize); + m_data.swap(other.m_data); + } + + inline SkylineMatrix & operator=(const SkylineMatrix & other) { + std::cout << "SkylineMatrix& operator=(const SkylineMatrix& other)\n"; + if (other.isRValue()) { + swap(other.const_cast_derived()); + } else { + resize(other.rows(), other.cols()); + memcpy(m_colStartIndex, other.m_colStartIndex, (m_outerSize + 1) * sizeof (Index)); + memcpy(m_rowStartIndex, other.m_rowStartIndex, (m_outerSize + 1) * sizeof (Index)); + m_data = other.m_data; + } + return *this; + } + + template<typename OtherDerived> + inline SkylineMatrix & operator=(const SkylineMatrixBase<OtherDerived>& other) { + const bool needToTranspose = (Flags & RowMajorBit) != (OtherDerived::Flags & RowMajorBit); + if (needToTranspose) { + // TODO + // return *this; + } else { + // there is no special optimization + return SkylineMatrixBase<SkylineMatrix>::operator=(other.derived()); + } + } + + friend std::ostream & operator <<(std::ostream & s, const SkylineMatrix & m) { + + EIGEN_DBG_SKYLINE( + std::cout << "upper elements : " << std::endl; + for (Index i = 0; i < m.m_data.upperSize(); i++) + std::cout << m.m_data.upper(i) << "\t"; + std::cout << std::endl; + std::cout << "upper profile : " << std::endl; + for (Index i = 0; i < m.m_data.upperProfileSize(); i++) + std::cout << m.m_data.upperProfile(i) << "\t"; + std::cout << std::endl; + std::cout << "lower startIdx : " << std::endl; + for (Index i = 0; i < m.m_data.upperProfileSize(); i++) + std::cout << (IsRowMajor ? m.m_colStartIndex[i] : m.m_rowStartIndex[i]) << "\t"; + std::cout << std::endl; + + + std::cout << "lower elements : " << std::endl; + for (Index i = 0; i < m.m_data.lowerSize(); i++) + std::cout << m.m_data.lower(i) << "\t"; + std::cout << std::endl; + std::cout << "lower profile : " << std::endl; + for (Index i = 0; i < m.m_data.lowerProfileSize(); i++) + std::cout << m.m_data.lowerProfile(i) << "\t"; + std::cout << std::endl; + std::cout << "lower startIdx : " << std::endl; + for (Index i = 0; i < m.m_data.lowerProfileSize(); i++) + std::cout << (IsRowMajor ? m.m_rowStartIndex[i] : m.m_colStartIndex[i]) << "\t"; + std::cout << std::endl; + ); + for (Index rowIdx = 0; rowIdx < m.rows(); rowIdx++) { + for (Index colIdx = 0; colIdx < m.cols(); colIdx++) { + s << m.coeff(rowIdx, colIdx) << "\t"; + } + s << std::endl; + } + return s; + } + + /** Destructor */ + inline ~SkylineMatrix() { + delete[] m_colStartIndex; + delete[] m_rowStartIndex; + } + + /** Overloaded for performance */ + Scalar sum() const; +}; + +template<typename Scalar, int _Options> +class SkylineMatrix<Scalar, _Options>::InnerUpperIterator { +public: + + InnerUpperIterator(const SkylineMatrix& mat, Index outer) + : m_matrix(mat), m_outer(outer), + m_id(_Options == RowMajor ? mat.m_colStartIndex[outer] : mat.m_rowStartIndex[outer] + 1), + m_start(m_id), + m_end(_Options == RowMajor ? mat.m_colStartIndex[outer + 1] : mat.m_rowStartIndex[outer + 1] + 1) { + } + + inline InnerUpperIterator & operator++() { + m_id++; + return *this; + } + + inline InnerUpperIterator & operator+=(Index shift) { + m_id += shift; + return *this; + } + + inline Scalar value() const { + return m_matrix.m_data.upper(m_id); + } + + inline Scalar* valuePtr() { + return const_cast<Scalar*> (&(m_matrix.m_data.upper(m_id))); + } + + inline Scalar& valueRef() { + return const_cast<Scalar&> (m_matrix.m_data.upper(m_id)); + } + + inline Index index() const { + return IsRowMajor ? m_outer - m_matrix.m_data.upperProfile(m_outer) + (m_id - m_start) : + m_outer + (m_id - m_start) + 1; + } + + inline Index row() const { + return IsRowMajor ? index() : m_outer; + } + + inline Index col() const { + return IsRowMajor ? m_outer : index(); + } + + inline size_t size() const { + return m_matrix.m_data.upperProfile(m_outer); + } + + inline operator bool() const { + return (m_id < m_end) && (m_id >= m_start); + } + +protected: + const SkylineMatrix& m_matrix; + const Index m_outer; + Index m_id; + const Index m_start; + const Index m_end; +}; + +template<typename Scalar, int _Options> +class SkylineMatrix<Scalar, _Options>::InnerLowerIterator { +public: + + InnerLowerIterator(const SkylineMatrix& mat, Index outer) + : m_matrix(mat), + m_outer(outer), + m_id(_Options == RowMajor ? mat.m_rowStartIndex[outer] : mat.m_colStartIndex[outer] + 1), + m_start(m_id), + m_end(_Options == RowMajor ? mat.m_rowStartIndex[outer + 1] : mat.m_colStartIndex[outer + 1] + 1) { + } + + inline InnerLowerIterator & operator++() { + m_id++; + return *this; + } + + inline InnerLowerIterator & operator+=(Index shift) { + m_id += shift; + return *this; + } + + inline Scalar value() const { + return m_matrix.m_data.lower(m_id); + } + + inline Scalar* valuePtr() { + return const_cast<Scalar*> (&(m_matrix.m_data.lower(m_id))); + } + + inline Scalar& valueRef() { + return const_cast<Scalar&> (m_matrix.m_data.lower(m_id)); + } + + inline Index index() const { + return IsRowMajor ? m_outer - m_matrix.m_data.lowerProfile(m_outer) + (m_id - m_start) : + m_outer + (m_id - m_start) + 1; + ; + } + + inline Index row() const { + return IsRowMajor ? m_outer : index(); + } + + inline Index col() const { + return IsRowMajor ? index() : m_outer; + } + + inline size_t size() const { + return m_matrix.m_data.lowerProfile(m_outer); + } + + inline operator bool() const { + return (m_id < m_end) && (m_id >= m_start); + } + +protected: + const SkylineMatrix& m_matrix; + const Index m_outer; + Index m_id; + const Index m_start; + const Index m_end; +}; + +} // end namespace Eigen + +#endif // EIGEN_SkylineMatrix_H diff --git a/unsupported/Eigen/src/Skyline/SkylineMatrixBase.h b/unsupported/Eigen/src/Skyline/SkylineMatrixBase.h new file mode 100644 index 000000000..b3a237230 --- /dev/null +++ b/unsupported/Eigen/src/Skyline/SkylineMatrixBase.h @@ -0,0 +1,212 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Guillaume Saupin <guillaume.saupin@cea.fr> +// +// 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_SKYLINEMATRIXBASE_H +#define EIGEN_SKYLINEMATRIXBASE_H + +#include "SkylineUtil.h" + +namespace Eigen { + +/** \ingroup Skyline_Module + * + * \class SkylineMatrixBase + * + * \brief Base class of any skyline matrices or skyline expressions + * + * \param Derived + * + */ +template<typename Derived> class SkylineMatrixBase : public EigenBase<Derived> { +public: + + typedef typename internal::traits<Derived>::Scalar Scalar; + typedef typename internal::traits<Derived>::StorageKind StorageKind; + typedef typename internal::index<StorageKind>::type Index; + + enum { + RowsAtCompileTime = internal::traits<Derived>::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<Derived>::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<internal::traits<Derived>::RowsAtCompileTime, + internal::traits<Derived>::ColsAtCompileTime>::ret), + /**< This is equal to the number of coefficients, i.e. the number of + * rows times the number of columns, or to \a Dynamic if this is not + * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */ + + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + + MaxSizeAtCompileTime = (internal::size_at_compile_time<MaxRowsAtCompileTime, + MaxColsAtCompileTime>::ret), + + IsVectorAtCompileTime = RowsAtCompileTime == 1 || ColsAtCompileTime == 1, + /**< This is set to true if either the number of rows or the number of + * columns is known at compile-time to be equal to 1. Indeed, in that case, + * we are dealing with a column-vector (if there is only one column) or with + * a row-vector (if there is only one row). */ + + Flags = internal::traits<Derived>::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". + */ + + CoeffReadCost = internal::traits<Derived>::CoeffReadCost, + /**< This is a rough measure of how expensive it is to read one coefficient from + * this expression. + */ + + IsRowMajor = Flags & RowMajorBit ? 1 : 0 + }; + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** This is the "real scalar" type; if the \a Scalar type is already real numbers + * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If + * \a Scalar is \a std::complex<T> then RealScalar is \a T. + * + * \sa class NumTraits + */ + typedef typename NumTraits<Scalar>::Real RealScalar; + + /** type of the equivalent square matrix */ + typedef Matrix<Scalar, EIGEN_SIZE_MAX(RowsAtCompileTime, ColsAtCompileTime), + EIGEN_SIZE_MAX(RowsAtCompileTime, ColsAtCompileTime) > SquareMatrixType; + + inline const Derived& derived() const { + return *static_cast<const Derived*> (this); + } + + inline Derived& derived() { + return *static_cast<Derived*> (this); + } + + inline Derived& const_cast_derived() const { + return *static_cast<Derived*> (const_cast<SkylineMatrixBase*> (this)); + } +#endif // not EIGEN_PARSED_BY_DOXYGEN + + /** \returns the number of rows. \sa cols(), RowsAtCompileTime */ + inline Index rows() const { + return derived().rows(); + } + + /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/ + inline Index cols() const { + return derived().cols(); + } + + /** \returns the number of coefficients, which is \a rows()*cols(). + * \sa rows(), cols(), SizeAtCompileTime. */ + inline Index size() const { + return rows() * cols(); + } + + /** \returns the number of nonzero coefficients which is in practice the number + * of stored coefficients. */ + inline Index nonZeros() const { + return derived().nonZeros(); + } + + /** \returns the size of the storage major dimension, + * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */ + Index outerSize() const { + return (int(Flags) & RowMajorBit) ? this->rows() : this->cols(); + } + + /** \returns the size of the inner dimension according to the storage order, + * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */ + Index innerSize() const { + return (int(Flags) & RowMajorBit) ? this->cols() : this->rows(); + } + + bool isRValue() const { + return m_isRValue; + } + + Derived& markAsRValue() { + m_isRValue = true; + return derived(); + } + + SkylineMatrixBase() : m_isRValue(false) { + /* TODO check flags */ + } + + inline Derived & operator=(const Derived& other) { + this->operator=<Derived > (other); + return derived(); + } + + template<typename OtherDerived> + inline void assignGeneric(const OtherDerived& other) { + derived().resize(other.rows(), other.cols()); + for (Index row = 0; row < rows(); row++) + for (Index col = 0; col < cols(); col++) { + if (other.coeff(row, col) != Scalar(0)) + derived().insert(row, col) = other.coeff(row, col); + } + derived().finalize(); + } + + template<typename OtherDerived> + inline Derived & operator=(const SkylineMatrixBase<OtherDerived>& other) { + //TODO + } + + template<typename Lhs, typename Rhs> + inline Derived & operator=(const SkylineProduct<Lhs, Rhs, SkylineTimeSkylineProduct>& product); + + friend std::ostream & operator <<(std::ostream & s, const SkylineMatrixBase& m) { + s << m.derived(); + return s; + } + + template<typename OtherDerived> + const typename SkylineProductReturnType<Derived, OtherDerived>::Type + operator*(const MatrixBase<OtherDerived> &other) const; + + /** \internal use operator= */ + template<typename DenseDerived> + void evalTo(MatrixBase<DenseDerived>& dst) const { + dst.setZero(); + for (Index i = 0; i < rows(); i++) + for (Index j = 0; j < rows(); j++) + dst(i, j) = derived().coeff(i, j); + } + + Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> toDense() const { + return derived(); + } + + /** \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. + */ + EIGEN_STRONG_INLINE const typename internal::eval<Derived, IsSkyline>::type eval() const { + return typename internal::eval<Derived>::type(derived()); + } + +protected: + bool m_isRValue; +}; + +} // end namespace Eigen + +#endif // EIGEN_SkylineMatrixBase_H diff --git a/unsupported/Eigen/src/Skyline/SkylineProduct.h b/unsupported/Eigen/src/Skyline/SkylineProduct.h new file mode 100644 index 000000000..1ddf455e2 --- /dev/null +++ b/unsupported/Eigen/src/Skyline/SkylineProduct.h @@ -0,0 +1,295 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Guillaume Saupin <guillaume.saupin@cea.fr> +// +// 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_SKYLINEPRODUCT_H +#define EIGEN_SKYLINEPRODUCT_H + +namespace Eigen { + +template<typename Lhs, typename Rhs, int ProductMode> +struct SkylineProductReturnType { + typedef const typename internal::nested<Lhs, Rhs::RowsAtCompileTime>::type LhsNested; + typedef const typename internal::nested<Rhs, Lhs::RowsAtCompileTime>::type RhsNested; + + typedef SkylineProduct<LhsNested, RhsNested, ProductMode> Type; +}; + +template<typename LhsNested, typename RhsNested, int ProductMode> +struct internal::traits<SkylineProduct<LhsNested, RhsNested, ProductMode> > { + // clean the nested types: + typedef typename internal::remove_all<LhsNested>::type _LhsNested; + typedef typename internal::remove_all<RhsNested>::type _RhsNested; + typedef typename _LhsNested::Scalar Scalar; + + enum { + LhsCoeffReadCost = _LhsNested::CoeffReadCost, + RhsCoeffReadCost = _RhsNested::CoeffReadCost, + LhsFlags = _LhsNested::Flags, + RhsFlags = _RhsNested::Flags, + + RowsAtCompileTime = _LhsNested::RowsAtCompileTime, + ColsAtCompileTime = _RhsNested::ColsAtCompileTime, + InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(_LhsNested::ColsAtCompileTime, _RhsNested::RowsAtCompileTime), + + MaxRowsAtCompileTime = _LhsNested::MaxRowsAtCompileTime, + MaxColsAtCompileTime = _RhsNested::MaxColsAtCompileTime, + + EvalToRowMajor = (RhsFlags & LhsFlags & RowMajorBit), + ResultIsSkyline = ProductMode == SkylineTimeSkylineProduct, + + RemovedBits = ~((EvalToRowMajor ? 0 : RowMajorBit) | (ResultIsSkyline ? 0 : SkylineBit)), + + Flags = (int(LhsFlags | RhsFlags) & HereditaryBits & RemovedBits) + | EvalBeforeAssigningBit + | EvalBeforeNestingBit, + + CoeffReadCost = Dynamic + }; + + typedef typename internal::conditional<ResultIsSkyline, + SkylineMatrixBase<SkylineProduct<LhsNested, RhsNested, ProductMode> >, + MatrixBase<SkylineProduct<LhsNested, RhsNested, ProductMode> > >::type Base; +}; + +namespace internal { +template<typename LhsNested, typename RhsNested, int ProductMode> +class SkylineProduct : no_assignment_operator, +public traits<SkylineProduct<LhsNested, RhsNested, ProductMode> >::Base { +public: + + EIGEN_GENERIC_PUBLIC_INTERFACE(SkylineProduct) + +private: + + typedef typename traits<SkylineProduct>::_LhsNested _LhsNested; + typedef typename traits<SkylineProduct>::_RhsNested _RhsNested; + +public: + + template<typename Lhs, typename Rhs> + EIGEN_STRONG_INLINE SkylineProduct(const Lhs& lhs, const Rhs& rhs) + : m_lhs(lhs), m_rhs(rhs) { + eigen_assert(lhs.cols() == rhs.rows()); + + enum { + ProductIsValid = _LhsNested::ColsAtCompileTime == Dynamic + || _RhsNested::RowsAtCompileTime == Dynamic + || int(_LhsNested::ColsAtCompileTime) == int(_RhsNested::RowsAtCompileTime), + AreVectors = _LhsNested::IsVectorAtCompileTime && _RhsNested::IsVectorAtCompileTime, + SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(_LhsNested, _RhsNested) + }; + // note to the lost user: + // * for a dot product use: v1.dot(v2) + // * for a coeff-wise product use: v1.cwise()*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) + } + + EIGEN_STRONG_INLINE Index rows() const { + return m_lhs.rows(); + } + + EIGEN_STRONG_INLINE Index cols() const { + return m_rhs.cols(); + } + + EIGEN_STRONG_INLINE const _LhsNested& lhs() const { + return m_lhs; + } + + EIGEN_STRONG_INLINE const _RhsNested& rhs() const { + return m_rhs; + } + +protected: + LhsNested m_lhs; + RhsNested m_rhs; +}; + +// dense = skyline * dense +// Note that here we force no inlining and separate the setZero() because GCC messes up otherwise + +template<typename Lhs, typename Rhs, typename Dest> +EIGEN_DONT_INLINE void skyline_row_major_time_dense_product(const Lhs& lhs, const Rhs& rhs, Dest& dst) { + typedef typename remove_all<Lhs>::type _Lhs; + typedef typename remove_all<Rhs>::type _Rhs; + typedef typename traits<Lhs>::Scalar Scalar; + + enum { + LhsIsRowMajor = (_Lhs::Flags & RowMajorBit) == RowMajorBit, + LhsIsSelfAdjoint = (_Lhs::Flags & SelfAdjointBit) == SelfAdjointBit, + ProcessFirstHalf = LhsIsSelfAdjoint + && (((_Lhs::Flags & (UpperTriangularBit | LowerTriangularBit)) == 0) + || ((_Lhs::Flags & UpperTriangularBit) && !LhsIsRowMajor) + || ((_Lhs::Flags & LowerTriangularBit) && LhsIsRowMajor)), + ProcessSecondHalf = LhsIsSelfAdjoint && (!ProcessFirstHalf) + }; + + //Use matrix diagonal part <- Improvement : use inner iterator on dense matrix. + for (Index col = 0; col < rhs.cols(); col++) { + for (Index row = 0; row < lhs.rows(); row++) { + dst(row, col) = lhs.coeffDiag(row) * rhs(row, col); + } + } + //Use matrix lower triangular part + for (Index row = 0; row < lhs.rows(); row++) { + typename _Lhs::InnerLowerIterator lIt(lhs, row); + const Index stop = lIt.col() + lIt.size(); + for (Index col = 0; col < rhs.cols(); col++) { + + Index k = lIt.col(); + Scalar tmp = 0; + while (k < stop) { + tmp += + lIt.value() * + rhs(k++, col); + ++lIt; + } + dst(row, col) += tmp; + lIt += -lIt.size(); + } + + } + + //Use matrix upper triangular part + for (Index lhscol = 0; lhscol < lhs.cols(); lhscol++) { + typename _Lhs::InnerUpperIterator uIt(lhs, lhscol); + const Index stop = uIt.size() + uIt.row(); + for (Index rhscol = 0; rhscol < rhs.cols(); rhscol++) { + + + const Scalar rhsCoeff = rhs.coeff(lhscol, rhscol); + Index k = uIt.row(); + while (k < stop) { + dst(k++, rhscol) += + uIt.value() * + rhsCoeff; + ++uIt; + } + uIt += -uIt.size(); + } + } + +} + +template<typename Lhs, typename Rhs, typename Dest> +EIGEN_DONT_INLINE void skyline_col_major_time_dense_product(const Lhs& lhs, const Rhs& rhs, Dest& dst) { + typedef typename remove_all<Lhs>::type _Lhs; + typedef typename remove_all<Rhs>::type _Rhs; + typedef typename traits<Lhs>::Scalar Scalar; + + enum { + LhsIsRowMajor = (_Lhs::Flags & RowMajorBit) == RowMajorBit, + LhsIsSelfAdjoint = (_Lhs::Flags & SelfAdjointBit) == SelfAdjointBit, + ProcessFirstHalf = LhsIsSelfAdjoint + && (((_Lhs::Flags & (UpperTriangularBit | LowerTriangularBit)) == 0) + || ((_Lhs::Flags & UpperTriangularBit) && !LhsIsRowMajor) + || ((_Lhs::Flags & LowerTriangularBit) && LhsIsRowMajor)), + ProcessSecondHalf = LhsIsSelfAdjoint && (!ProcessFirstHalf) + }; + + //Use matrix diagonal part <- Improvement : use inner iterator on dense matrix. + for (Index col = 0; col < rhs.cols(); col++) { + for (Index row = 0; row < lhs.rows(); row++) { + dst(row, col) = lhs.coeffDiag(row) * rhs(row, col); + } + } + + //Use matrix upper triangular part + for (Index row = 0; row < lhs.rows(); row++) { + typename _Lhs::InnerUpperIterator uIt(lhs, row); + const Index stop = uIt.col() + uIt.size(); + for (Index col = 0; col < rhs.cols(); col++) { + + Index k = uIt.col(); + Scalar tmp = 0; + while (k < stop) { + tmp += + uIt.value() * + rhs(k++, col); + ++uIt; + } + + + dst(row, col) += tmp; + uIt += -uIt.size(); + } + } + + //Use matrix lower triangular part + for (Index lhscol = 0; lhscol < lhs.cols(); lhscol++) { + typename _Lhs::InnerLowerIterator lIt(lhs, lhscol); + const Index stop = lIt.size() + lIt.row(); + for (Index rhscol = 0; rhscol < rhs.cols(); rhscol++) { + + const Scalar rhsCoeff = rhs.coeff(lhscol, rhscol); + Index k = lIt.row(); + while (k < stop) { + dst(k++, rhscol) += + lIt.value() * + rhsCoeff; + ++lIt; + } + lIt += -lIt.size(); + } + } + +} + +template<typename Lhs, typename Rhs, typename ResultType, + int LhsStorageOrder = traits<Lhs>::Flags&RowMajorBit> + struct skyline_product_selector; + +template<typename Lhs, typename Rhs, typename ResultType> +struct skyline_product_selector<Lhs, Rhs, ResultType, RowMajor> { + typedef typename traits<typename remove_all<Lhs>::type>::Scalar Scalar; + + static void run(const Lhs& lhs, const Rhs& rhs, ResultType & res) { + skyline_row_major_time_dense_product<Lhs, Rhs, ResultType > (lhs, rhs, res); + } +}; + +template<typename Lhs, typename Rhs, typename ResultType> +struct skyline_product_selector<Lhs, Rhs, ResultType, ColMajor> { + typedef typename traits<typename remove_all<Lhs>::type>::Scalar Scalar; + + static void run(const Lhs& lhs, const Rhs& rhs, ResultType & res) { + skyline_col_major_time_dense_product<Lhs, Rhs, ResultType > (lhs, rhs, res); + } +}; + +} // end namespace internal + +// template<typename Derived> +// template<typename Lhs, typename Rhs > +// Derived & MatrixBase<Derived>::lazyAssign(const SkylineProduct<Lhs, Rhs, SkylineTimeDenseProduct>& product) { +// typedef typename internal::remove_all<Lhs>::type _Lhs; +// internal::skyline_product_selector<typename internal::remove_all<Lhs>::type, +// typename internal::remove_all<Rhs>::type, +// Derived>::run(product.lhs(), product.rhs(), derived()); +// +// return derived(); +// } + +// skyline * dense + +template<typename Derived> +template<typename OtherDerived > +EIGEN_STRONG_INLINE const typename SkylineProductReturnType<Derived, OtherDerived>::Type +SkylineMatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const { + + return typename SkylineProductReturnType<Derived, OtherDerived>::Type(derived(), other.derived()); +} + +} // end namespace Eigen + +#endif // EIGEN_SKYLINEPRODUCT_H diff --git a/unsupported/Eigen/src/Skyline/SkylineStorage.h b/unsupported/Eigen/src/Skyline/SkylineStorage.h new file mode 100644 index 000000000..378a8deb4 --- /dev/null +++ b/unsupported/Eigen/src/Skyline/SkylineStorage.h @@ -0,0 +1,259 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Guillaume Saupin <guillaume.saupin@cea.fr> +// +// 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_SKYLINE_STORAGE_H +#define EIGEN_SKYLINE_STORAGE_H + +namespace Eigen { + +/** Stores a skyline set of values in three structures : + * The diagonal elements + * The upper elements + * The lower elements + * + */ +template<typename Scalar> +class SkylineStorage { + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef SparseIndex Index; +public: + + SkylineStorage() + : m_diag(0), + m_lower(0), + m_upper(0), + m_lowerProfile(0), + m_upperProfile(0), + m_diagSize(0), + m_upperSize(0), + m_lowerSize(0), + m_upperProfileSize(0), + m_lowerProfileSize(0), + m_allocatedSize(0) { + } + + SkylineStorage(const SkylineStorage& other) + : m_diag(0), + m_lower(0), + m_upper(0), + m_lowerProfile(0), + m_upperProfile(0), + m_diagSize(0), + m_upperSize(0), + m_lowerSize(0), + m_upperProfileSize(0), + m_lowerProfileSize(0), + m_allocatedSize(0) { + *this = other; + } + + SkylineStorage & operator=(const SkylineStorage& other) { + resize(other.diagSize(), other.m_upperProfileSize, other.m_lowerProfileSize, other.upperSize(), other.lowerSize()); + memcpy(m_diag, other.m_diag, m_diagSize * sizeof (Scalar)); + memcpy(m_upper, other.m_upper, other.upperSize() * sizeof (Scalar)); + memcpy(m_lower, other.m_lower, other.lowerSize() * sizeof (Scalar)); + memcpy(m_upperProfile, other.m_upperProfile, m_upperProfileSize * sizeof (Index)); + memcpy(m_lowerProfile, other.m_lowerProfile, m_lowerProfileSize * sizeof (Index)); + return *this; + } + + void swap(SkylineStorage& other) { + std::swap(m_diag, other.m_diag); + std::swap(m_upper, other.m_upper); + std::swap(m_lower, other.m_lower); + std::swap(m_upperProfile, other.m_upperProfile); + std::swap(m_lowerProfile, other.m_lowerProfile); + std::swap(m_diagSize, other.m_diagSize); + std::swap(m_upperSize, other.m_upperSize); + std::swap(m_lowerSize, other.m_lowerSize); + std::swap(m_allocatedSize, other.m_allocatedSize); + } + + ~SkylineStorage() { + delete[] m_diag; + delete[] m_upper; + if (m_upper != m_lower) + delete[] m_lower; + delete[] m_upperProfile; + delete[] m_lowerProfile; + } + + void reserve(Index size, Index upperProfileSize, Index lowerProfileSize, Index upperSize, Index lowerSize) { + Index newAllocatedSize = size + upperSize + lowerSize; + if (newAllocatedSize > m_allocatedSize) + reallocate(size, upperProfileSize, lowerProfileSize, upperSize, lowerSize); + } + + void squeeze() { + if (m_allocatedSize > m_diagSize + m_upperSize + m_lowerSize) + reallocate(m_diagSize, m_upperProfileSize, m_lowerProfileSize, m_upperSize, m_lowerSize); + } + + void resize(Index diagSize, Index upperProfileSize, Index lowerProfileSize, Index upperSize, Index lowerSize, float reserveSizeFactor = 0) { + if (m_allocatedSize < diagSize + upperSize + lowerSize) + reallocate(diagSize, upperProfileSize, lowerProfileSize, upperSize + Index(reserveSizeFactor * upperSize), lowerSize + Index(reserveSizeFactor * lowerSize)); + m_diagSize = diagSize; + m_upperSize = upperSize; + m_lowerSize = lowerSize; + m_upperProfileSize = upperProfileSize; + m_lowerProfileSize = lowerProfileSize; + } + + inline Index diagSize() const { + return m_diagSize; + } + + inline Index upperSize() const { + return m_upperSize; + } + + inline Index lowerSize() const { + return m_lowerSize; + } + + inline Index upperProfileSize() const { + return m_upperProfileSize; + } + + inline Index lowerProfileSize() const { + return m_lowerProfileSize; + } + + inline Index allocatedSize() const { + return m_allocatedSize; + } + + inline void clear() { + m_diagSize = 0; + } + + inline Scalar& diag(Index i) { + return m_diag[i]; + } + + inline const Scalar& diag(Index i) const { + return m_diag[i]; + } + + inline Scalar& upper(Index i) { + return m_upper[i]; + } + + inline const Scalar& upper(Index i) const { + return m_upper[i]; + } + + inline Scalar& lower(Index i) { + return m_lower[i]; + } + + inline const Scalar& lower(Index i) const { + return m_lower[i]; + } + + inline Index& upperProfile(Index i) { + return m_upperProfile[i]; + } + + inline const Index& upperProfile(Index i) const { + return m_upperProfile[i]; + } + + inline Index& lowerProfile(Index i) { + return m_lowerProfile[i]; + } + + inline const Index& lowerProfile(Index i) const { + return m_lowerProfile[i]; + } + + static SkylineStorage Map(Index* upperProfile, Index* lowerProfile, Scalar* diag, Scalar* upper, Scalar* lower, Index size, Index upperSize, Index lowerSize) { + SkylineStorage res; + res.m_upperProfile = upperProfile; + res.m_lowerProfile = lowerProfile; + res.m_diag = diag; + res.m_upper = upper; + res.m_lower = lower; + res.m_allocatedSize = res.m_diagSize = size; + res.m_upperSize = upperSize; + res.m_lowerSize = lowerSize; + return res; + } + + inline void reset() { + memset(m_diag, 0, m_diagSize * sizeof (Scalar)); + memset(m_upper, 0, m_upperSize * sizeof (Scalar)); + memset(m_lower, 0, m_lowerSize * sizeof (Scalar)); + memset(m_upperProfile, 0, m_diagSize * sizeof (Index)); + memset(m_lowerProfile, 0, m_diagSize * sizeof (Index)); + } + + void prune(Scalar reference, RealScalar epsilon = dummy_precision<RealScalar>()) { + //TODO + } + +protected: + + inline void reallocate(Index diagSize, Index upperProfileSize, Index lowerProfileSize, Index upperSize, Index lowerSize) { + + Scalar* diag = new Scalar[diagSize]; + Scalar* upper = new Scalar[upperSize]; + Scalar* lower = new Scalar[lowerSize]; + Index* upperProfile = new Index[upperProfileSize]; + Index* lowerProfile = new Index[lowerProfileSize]; + + Index copyDiagSize = (std::min)(diagSize, m_diagSize); + Index copyUpperSize = (std::min)(upperSize, m_upperSize); + Index copyLowerSize = (std::min)(lowerSize, m_lowerSize); + Index copyUpperProfileSize = (std::min)(upperProfileSize, m_upperProfileSize); + Index copyLowerProfileSize = (std::min)(lowerProfileSize, m_lowerProfileSize); + + // copy + memcpy(diag, m_diag, copyDiagSize * sizeof (Scalar)); + memcpy(upper, m_upper, copyUpperSize * sizeof (Scalar)); + memcpy(lower, m_lower, copyLowerSize * sizeof (Scalar)); + memcpy(upperProfile, m_upperProfile, copyUpperProfileSize * sizeof (Index)); + memcpy(lowerProfile, m_lowerProfile, copyLowerProfileSize * sizeof (Index)); + + + + // delete old stuff + delete[] m_diag; + delete[] m_upper; + delete[] m_lower; + delete[] m_upperProfile; + delete[] m_lowerProfile; + m_diag = diag; + m_upper = upper; + m_lower = lower; + m_upperProfile = upperProfile; + m_lowerProfile = lowerProfile; + m_allocatedSize = diagSize + upperSize + lowerSize; + m_upperSize = upperSize; + m_lowerSize = lowerSize; + } + +public: + Scalar* m_diag; + Scalar* m_upper; + Scalar* m_lower; + Index* m_upperProfile; + Index* m_lowerProfile; + Index m_diagSize; + Index m_upperSize; + Index m_lowerSize; + Index m_upperProfileSize; + Index m_lowerProfileSize; + Index m_allocatedSize; + +}; + +} // end namespace Eigen + +#endif // EIGEN_COMPRESSED_STORAGE_H diff --git a/unsupported/Eigen/src/Skyline/SkylineUtil.h b/unsupported/Eigen/src/Skyline/SkylineUtil.h new file mode 100644 index 000000000..75eb612f4 --- /dev/null +++ b/unsupported/Eigen/src/Skyline/SkylineUtil.h @@ -0,0 +1,89 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Guillaume Saupin <guillaume.saupin@cea.fr> +// +// 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_SKYLINEUTIL_H +#define EIGEN_SKYLINEUTIL_H + +namespace Eigen { + +#ifdef NDEBUG +#define EIGEN_DBG_SKYLINE(X) +#else +#define EIGEN_DBG_SKYLINE(X) X +#endif + +const unsigned int SkylineBit = 0x1200; +template<typename Lhs, typename Rhs, int ProductMode> class SkylineProduct; +enum AdditionalProductEvaluationMode {SkylineTimeDenseProduct, SkylineTimeSkylineProduct, DenseTimeSkylineProduct}; +enum {IsSkyline = SkylineBit}; + + +#define EIGEN_SKYLINE_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \ +template<typename OtherDerived> \ +EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::SkylineMatrixBase<OtherDerived>& other) \ +{ \ + return Base::operator Op(other.derived()); \ +} \ +EIGEN_STRONG_INLINE Derived& operator Op(const Derived& other) \ +{ \ + return Base::operator Op(other); \ +} + +#define EIGEN_SKYLINE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \ +template<typename Other> \ +EIGEN_STRONG_INLINE Derived& operator Op(const Other& scalar) \ +{ \ + return Base::operator Op(scalar); \ +} + +#define EIGEN_SKYLINE_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ + EIGEN_SKYLINE_INHERIT_ASSIGNMENT_OPERATOR(Derived, =) \ + EIGEN_SKYLINE_INHERIT_ASSIGNMENT_OPERATOR(Derived, +=) \ + EIGEN_SKYLINE_INHERIT_ASSIGNMENT_OPERATOR(Derived, -=) \ + EIGEN_SKYLINE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, *=) \ + EIGEN_SKYLINE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, /=) + +#define _EIGEN_SKYLINE_GENERIC_PUBLIC_INTERFACE(Derived, BaseClass) \ + typedef BaseClass Base; \ + typedef typename Eigen::internal::traits<Derived>::Scalar Scalar; \ + typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; \ + typedef typename Eigen::internal::traits<Derived>::StorageKind StorageKind; \ + typedef typename Eigen::internal::index<StorageKind>::type Index; \ + enum { Flags = Eigen::internal::traits<Derived>::Flags, }; + +#define EIGEN_SKYLINE_GENERIC_PUBLIC_INTERFACE(Derived) \ + _EIGEN_SKYLINE_GENERIC_PUBLIC_INTERFACE(Derived, Eigen::SkylineMatrixBase<Derived>) + +template<typename Derived> class SkylineMatrixBase; +template<typename _Scalar, int _Flags = 0> class SkylineMatrix; +template<typename _Scalar, int _Flags = 0> class DynamicSkylineMatrix; +template<typename _Scalar, int _Flags = 0> class SkylineVector; +template<typename _Scalar, int _Flags = 0> class MappedSkylineMatrix; + +namespace internal { + +template<typename Lhs, typename Rhs> struct skyline_product_mode; +template<typename Lhs, typename Rhs, int ProductMode = skyline_product_mode<Lhs,Rhs>::value> struct SkylineProductReturnType; + +template<typename T> class eval<T,IsSkyline> +{ + typedef typename traits<T>::Scalar _Scalar; + enum { + _Flags = traits<T>::Flags + }; + + public: + typedef SkylineMatrix<_Scalar, _Flags> type; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_SKYLINEUTIL_H diff --git a/unsupported/Eigen/src/SparseExtra/BlockOfDynamicSparseMatrix.h b/unsupported/Eigen/src/SparseExtra/BlockOfDynamicSparseMatrix.h new file mode 100644 index 000000000..fd24a732d --- /dev/null +++ b/unsupported/Eigen/src/SparseExtra/BlockOfDynamicSparseMatrix.h @@ -0,0 +1,114 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_BLOCKFORDYNAMICMATRIX_H +#define EIGEN_SPARSE_BLOCKFORDYNAMICMATRIX_H + +namespace Eigen { + +/*************************************************************************** +* specialisation for DynamicSparseMatrix +***************************************************************************/ + +template<typename _Scalar, int _Options, typename _Index, int Size> +class SparseInnerVectorSet<DynamicSparseMatrix<_Scalar, _Options, _Index>, Size> + : public SparseMatrixBase<SparseInnerVectorSet<DynamicSparseMatrix<_Scalar, _Options, _Index>, Size> > +{ + typedef DynamicSparseMatrix<_Scalar, _Options, _Index> MatrixType; + public: + + enum { IsRowMajor = internal::traits<SparseInnerVectorSet>::IsRowMajor }; + + EIGEN_SPARSE_PUBLIC_INTERFACE(SparseInnerVectorSet) + class InnerIterator: public MatrixType::InnerIterator + { + public: + inline InnerIterator(const SparseInnerVectorSet& xpr, Index outer) + : MatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline SparseInnerVectorSet(const MatrixType& matrix, Index outerStart, Index outerSize) + : m_matrix(matrix), m_outerStart(outerStart), m_outerSize(outerSize) + { + eigen_assert( (outerStart>=0) && ((outerStart+outerSize)<=matrix.outerSize()) ); + } + + inline SparseInnerVectorSet(const MatrixType& matrix, Index outer) + : m_matrix(matrix), m_outerStart(outer), m_outerSize(Size) + { + eigen_assert(Size!=Dynamic); + eigen_assert( (outer>=0) && (outer<matrix.outerSize()) ); + } + + template<typename OtherDerived> + inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other) + { + if (IsRowMajor != ((OtherDerived::Flags&RowMajorBit)==RowMajorBit)) + { + // need to transpose => perform a block evaluation followed by a big swap + DynamicSparseMatrix<Scalar,IsRowMajor?RowMajorBit:0> aux(other); + *this = aux.markAsRValue(); + } + else + { + // evaluate/copy vector per vector + for (Index j=0; j<m_outerSize.value(); ++j) + { + SparseVector<Scalar,IsRowMajor ? RowMajorBit : 0> aux(other.innerVector(j)); + m_matrix.const_cast_derived()._data()[m_outerStart+j].swap(aux._data()); + } + } + return *this; + } + + inline SparseInnerVectorSet& operator=(const SparseInnerVectorSet& other) + { + return operator=<SparseInnerVectorSet>(other); + } + + Index nonZeros() const + { + Index count = 0; + for (Index j=0; j<m_outerSize.value(); ++j) + count += m_matrix._data()[m_outerStart+j].size(); + return count; + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(SparseInnerVectorSet); + eigen_assert(m_matrix.data()[m_outerStart].size()>0); + return m_matrix.data()[m_outerStart].vale(m_matrix.data()[m_outerStart].size()-1); + } + +// template<typename Sparse> +// inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other) +// { +// return *this; +// } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + const typename MatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic<Index, Size> m_outerSize; + +}; + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_BLOCKFORDYNAMICMATRIX_H diff --git a/unsupported/Eigen/src/SparseExtra/CMakeLists.txt b/unsupported/Eigen/src/SparseExtra/CMakeLists.txt new file mode 100644 index 000000000..7ea32ca5e --- /dev/null +++ b/unsupported/Eigen/src/SparseExtra/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_SparseExtra_SRCS "*.h") + +INSTALL(FILES + ${Eigen_SparseExtra_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/SparseExtra COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h b/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h new file mode 100644 index 000000000..dec16df28 --- /dev/null +++ b/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h @@ -0,0 +1,357 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_DYNAMIC_SPARSEMATRIX_H +#define EIGEN_DYNAMIC_SPARSEMATRIX_H + +namespace Eigen { + +/** \deprecated use a SparseMatrix in an uncompressed mode + * + * \class DynamicSparseMatrix + * + * \brief A sparse matrix class designed for matrix assembly purpose + * + * \param _Scalar the scalar type, i.e. the type of the coefficients + * + * Unlike SparseMatrix, this class provides a much higher degree of flexibility. In particular, it allows + * random read/write accesses in log(rho*outer_size) where \c rho is the probability that a coefficient is + * nonzero and outer_size is the number of columns if the matrix is column-major and the number of rows + * otherwise. + * + * Internally, the data are stored as a std::vector of compressed vector. The performances of random writes might + * decrease as the number of nonzeros per inner-vector increase. In practice, we observed very good performance + * till about 100 nonzeros/vector, and the performance remains relatively good till 500 nonzeros/vectors. + * + * \see SparseMatrix + */ + +namespace internal { +template<typename _Scalar, int _Options, typename _Index> +struct traits<DynamicSparseMatrix<_Scalar, _Options, _Index> > +{ + typedef _Scalar Scalar; + typedef _Index Index; + typedef Sparse StorageKind; + typedef MatrixXpr XprKind; + enum { + RowsAtCompileTime = Dynamic, + ColsAtCompileTime = Dynamic, + MaxRowsAtCompileTime = Dynamic, + MaxColsAtCompileTime = Dynamic, + Flags = _Options | NestByRefBit | LvalueBit, + CoeffReadCost = NumTraits<Scalar>::ReadCost, + SupportedAccessPatterns = OuterRandomAccessPattern + }; +}; +} + +template<typename _Scalar, int _Options, typename _Index> + class DynamicSparseMatrix + : public SparseMatrixBase<DynamicSparseMatrix<_Scalar, _Options, _Index> > +{ + public: + EIGEN_SPARSE_PUBLIC_INTERFACE(DynamicSparseMatrix) + // FIXME: why are these operator already alvailable ??? + // EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(DynamicSparseMatrix, +=) + // EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(DynamicSparseMatrix, -=) + typedef MappedSparseMatrix<Scalar,Flags> Map; + using Base::IsRowMajor; + using Base::operator=; + enum { + Options = _Options + }; + + protected: + + typedef DynamicSparseMatrix<Scalar,(Flags&~RowMajorBit)|(IsRowMajor?RowMajorBit:0)> TransposedSparseMatrix; + + Index m_innerSize; + std::vector<internal::CompressedStorage<Scalar,Index> > m_data; + + public: + + inline Index rows() const { return IsRowMajor ? outerSize() : m_innerSize; } + inline Index cols() const { return IsRowMajor ? m_innerSize : outerSize(); } + inline Index innerSize() const { return m_innerSize; } + inline Index outerSize() const { return static_cast<Index>(m_data.size()); } + inline Index innerNonZeros(Index j) const { return m_data[j].size(); } + + std::vector<internal::CompressedStorage<Scalar,Index> >& _data() { return m_data; } + const std::vector<internal::CompressedStorage<Scalar,Index> >& _data() const { return m_data; } + + /** \returns the coefficient value at given position \a row, \a col + * This operation involes a log(rho*outer_size) binary search. + */ + inline Scalar coeff(Index row, Index col) const + { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + return m_data[outer].at(inner); + } + + /** \returns a reference to the coefficient value at given position \a row, \a col + * This operation involes a log(rho*outer_size) binary search. If the coefficient does not + * exist yet, then a sorted insertion into a sequential buffer is performed. + */ + inline Scalar& coeffRef(Index row, Index col) + { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + return m_data[outer].atWithInsertion(inner); + } + + class InnerIterator; + class ReverseInnerIterator; + + void setZero() + { + for (Index j=0; j<outerSize(); ++j) + m_data[j].clear(); + } + + /** \returns the number of non zero coefficients */ + Index nonZeros() const + { + Index res = 0; + for (Index j=0; j<outerSize(); ++j) + res += static_cast<Index>(m_data[j].size()); + return res; + } + + + + void reserve(Index reserveSize = 1000) + { + if (outerSize()>0) + { + Index reserveSizePerVector = (std::max)(reserveSize/outerSize(),Index(4)); + for (Index j=0; j<outerSize(); ++j) + { + m_data[j].reserve(reserveSizePerVector); + } + } + } + + /** Does nothing: provided for compatibility with SparseMatrix */ + inline void startVec(Index /*outer*/) {} + + /** \returns a reference to the non zero coefficient at position \a row, \a col assuming that: + * - the nonzero does not already exist + * - the new coefficient is the last one of the given inner vector. + * + * \sa insert, insertBackByOuterInner */ + inline Scalar& insertBack(Index row, Index col) + { + return insertBackByOuterInner(IsRowMajor?row:col, IsRowMajor?col:row); + } + + /** \sa insertBack */ + inline Scalar& insertBackByOuterInner(Index outer, Index inner) + { + eigen_assert(outer<Index(m_data.size()) && inner<m_innerSize && "out of range"); + eigen_assert(((m_data[outer].size()==0) || (m_data[outer].index(m_data[outer].size()-1)<inner)) + && "wrong sorted insertion"); + m_data[outer].append(0, inner); + return m_data[outer].value(m_data[outer].size()-1); + } + + inline Scalar& insert(Index row, Index col) + { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + + Index startId = 0; + Index id = static_cast<Index>(m_data[outer].size()) - 1; + m_data[outer].resize(id+2,1); + + while ( (id >= startId) && (m_data[outer].index(id) > inner) ) + { + m_data[outer].index(id+1) = m_data[outer].index(id); + m_data[outer].value(id+1) = m_data[outer].value(id); + --id; + } + m_data[outer].index(id+1) = inner; + m_data[outer].value(id+1) = 0; + return m_data[outer].value(id+1); + } + + /** Does nothing: provided for compatibility with SparseMatrix */ + inline void finalize() {} + + /** Suppress all nonzeros which are smaller than \a reference under the tolerence \a epsilon */ + void prune(Scalar reference, RealScalar epsilon = NumTraits<RealScalar>::dummy_precision()) + { + for (Index j=0; j<outerSize(); ++j) + m_data[j].prune(reference,epsilon); + } + + /** Resize the matrix without preserving the data (the matrix is set to zero) + */ + void resize(Index rows, Index cols) + { + const Index outerSize = IsRowMajor ? rows : cols; + m_innerSize = IsRowMajor ? cols : rows; + setZero(); + if (Index(m_data.size()) != outerSize) + { + m_data.resize(outerSize); + } + } + + void resizeAndKeepData(Index rows, Index cols) + { + const Index outerSize = IsRowMajor ? rows : cols; + const Index innerSize = IsRowMajor ? cols : rows; + if (m_innerSize>innerSize) + { + // remove all coefficients with innerCoord>=innerSize + // TODO + //std::cerr << "not implemented yet\n"; + exit(2); + } + if (m_data.size() != outerSize) + { + m_data.resize(outerSize); + } + } + + /** The class DynamicSparseMatrix is deprectaed */ + EIGEN_DEPRECATED inline DynamicSparseMatrix() + : m_innerSize(0), m_data(0) + { + eigen_assert(innerSize()==0 && outerSize()==0); + } + + /** The class DynamicSparseMatrix is deprectaed */ + EIGEN_DEPRECATED inline DynamicSparseMatrix(Index rows, Index cols) + : m_innerSize(0) + { + resize(rows, cols); + } + + /** The class DynamicSparseMatrix is deprectaed */ + template<typename OtherDerived> + EIGEN_DEPRECATED explicit inline DynamicSparseMatrix(const SparseMatrixBase<OtherDerived>& other) + : m_innerSize(0) + { + Base::operator=(other.derived()); + } + + inline DynamicSparseMatrix(const DynamicSparseMatrix& other) + : Base(), m_innerSize(0) + { + *this = other.derived(); + } + + inline void swap(DynamicSparseMatrix& other) + { + //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n"); + std::swap(m_innerSize, other.m_innerSize); + //std::swap(m_outerSize, other.m_outerSize); + m_data.swap(other.m_data); + } + + inline DynamicSparseMatrix& operator=(const DynamicSparseMatrix& other) + { + if (other.isRValue()) + { + swap(other.const_cast_derived()); + } + else + { + resize(other.rows(), other.cols()); + m_data = other.m_data; + } + return *this; + } + + /** Destructor */ + inline ~DynamicSparseMatrix() {} + + public: + + /** \deprecated + * Set the matrix to zero and reserve the memory for \a reserveSize nonzero coefficients. */ + EIGEN_DEPRECATED void startFill(Index reserveSize = 1000) + { + setZero(); + reserve(reserveSize); + } + + /** \deprecated use insert() + * inserts a nonzero coefficient at given coordinates \a row, \a col and returns its reference assuming that: + * 1 - the coefficient does not exist yet + * 2 - this the coefficient with greater inner coordinate for the given outer coordinate. + * In other words, assuming \c *this is column-major, then there must not exists any nonzero coefficient of coordinates + * \c i \c x \a col such that \c i >= \a row. Otherwise the matrix is invalid. + * + * \see fillrand(), coeffRef() + */ + EIGEN_DEPRECATED Scalar& fill(Index row, Index col) + { + const Index outer = IsRowMajor ? row : col; + const Index inner = IsRowMajor ? col : row; + return insertBack(outer,inner); + } + + /** \deprecated use insert() + * Like fill() but with random inner coordinates. + * Compared to the generic coeffRef(), the unique limitation is that we assume + * the coefficient does not exist yet. + */ + EIGEN_DEPRECATED Scalar& fillrand(Index row, Index col) + { + return insert(row,col); + } + + /** \deprecated use finalize() + * Does nothing. Provided for compatibility with SparseMatrix. */ + EIGEN_DEPRECATED void endFill() {} + +# ifdef EIGEN_DYNAMICSPARSEMATRIX_PLUGIN +# include EIGEN_DYNAMICSPARSEMATRIX_PLUGIN +# endif + }; + +template<typename Scalar, int _Options, typename _Index> +class DynamicSparseMatrix<Scalar,_Options,_Index>::InnerIterator : public SparseVector<Scalar,_Options,_Index>::InnerIterator +{ + typedef typename SparseVector<Scalar,_Options,_Index>::InnerIterator Base; + public: + InnerIterator(const DynamicSparseMatrix& mat, Index outer) + : Base(mat.m_data[outer]), m_outer(outer) + {} + + inline Index row() const { return IsRowMajor ? m_outer : Base::index(); } + inline Index col() const { return IsRowMajor ? Base::index() : m_outer; } + + protected: + const Index m_outer; +}; + +template<typename Scalar, int _Options, typename _Index> +class DynamicSparseMatrix<Scalar,_Options,_Index>::ReverseInnerIterator : public SparseVector<Scalar,_Options,_Index>::ReverseInnerIterator +{ + typedef typename SparseVector<Scalar,_Options,_Index>::ReverseInnerIterator Base; + public: + ReverseInnerIterator(const DynamicSparseMatrix& mat, Index outer) + : Base(mat.m_data[outer]), m_outer(outer) + {} + + inline Index row() const { return IsRowMajor ? m_outer : Base::index(); } + inline Index col() const { return IsRowMajor ? Base::index() : m_outer; } + + protected: + const Index m_outer; +}; + +} // end namespace Eigen + +#endif // EIGEN_DYNAMIC_SPARSEMATRIX_H diff --git a/unsupported/Eigen/src/SparseExtra/MarketIO.h b/unsupported/Eigen/src/SparseExtra/MarketIO.h new file mode 100644 index 000000000..de958de9f --- /dev/null +++ b/unsupported/Eigen/src/SparseExtra/MarketIO.h @@ -0,0 +1,273 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr> +// Copyright (C) 2012 Desire NUENTSA WAKAM <desire.nuentsa_wakam@inria.fr> +// +// 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_MARKET_IO_H +#define EIGEN_SPARSE_MARKET_IO_H + +#include <iostream> + +namespace Eigen { + +namespace internal +{ + template <typename Scalar> + inline bool GetMarketLine (std::stringstream& line, int& M, int& N, int& i, int& j, Scalar& value) + { + line >> i >> j >> value; + i--; + j--; + if(i>=0 && j>=0 && i<M && j<N) + { + return true; + } + else + return false; + } + template <typename Scalar> + inline bool GetMarketLine (std::stringstream& line, int& M, int& N, int& i, int& j, std::complex<Scalar>& value) + { + Scalar valR, valI; + line >> i >> j >> valR >> valI; + i--; + j--; + if(i>=0 && j>=0 && i<M && j<N) + { + value = std::complex<Scalar>(valR, valI); + return true; + } + else + return false; + } + + template <typename RealScalar> + inline void GetVectorElt (const std::string& line, RealScalar& val) + { + std::istringstream newline(line); + newline >> val; + } + + template <typename RealScalar> + inline void GetVectorElt (const std::string& line, std::complex<RealScalar>& val) + { + RealScalar valR, valI; + std::istringstream newline(line); + newline >> valR >> valI; + val = std::complex<RealScalar>(valR, valI); + } + + template<typename Scalar> + inline void putMarketHeader(std::string& header,int sym) + { + header= "%%MatrixMarket matrix coordinate "; + if(internal::is_same<Scalar, std::complex<float> >::value || internal::is_same<Scalar, std::complex<double> >::value) + { + header += " complex"; + if(sym == Symmetric) header += " symmetric"; + else if (sym == SelfAdjoint) header += " Hermitian"; + else header += " general"; + } + else + { + header += " real"; + if(sym == Symmetric) header += " symmetric"; + else header += " general"; + } + } + + template<typename Scalar> + inline void PutMatrixElt(Scalar value, int row, int col, std::ofstream& out) + { + out << row << " "<< col << " " << value << "\n"; + } + template<typename Scalar> + inline void PutMatrixElt(std::complex<Scalar> value, int row, int col, std::ofstream& out) + { + out << row << " " << col << " " << value.real() << " " << value.imag() << "\n"; + } + + + template<typename Scalar> + inline void putVectorElt(Scalar value, std::ofstream& out) + { + out << value << "\n"; + } + template<typename Scalar> + inline void putVectorElt(std::complex<Scalar> value, std::ofstream& out) + { + out << value.real << " " << value.imag()<< "\n"; + } + +} // end namepsace internal + +inline bool getMarketHeader(const std::string& filename, int& sym, bool& iscomplex, bool& isvector) +{ + sym = 0; + isvector = false; + std::ifstream in(filename.c_str(),std::ios::in); + if(!in) + return false; + + std::string line; + // The matrix header is always the first line in the file + std::getline(in, line); assert(in.good()); + + std::stringstream fmtline(line); + std::string substr[5]; + fmtline>> substr[0] >> substr[1] >> substr[2] >> substr[3] >> substr[4]; + if(substr[2].compare("array") == 0) isvector = true; + if(substr[3].compare("complex") == 0) iscomplex = true; + if(substr[4].compare("symmetric") == 0) sym = Symmetric; + else if (substr[4].compare("Hermitian") == 0) sym = SelfAdjoint; + + return true; +} + +template<typename SparseMatrixType> +bool loadMarket(SparseMatrixType& mat, const std::string& filename) +{ + typedef typename SparseMatrixType::Scalar Scalar; + std::ifstream input(filename.c_str(),std::ios::in); + if(!input) + return false; + + const int maxBuffersize = 2048; + char buffer[maxBuffersize]; + + bool readsizes = false; + + typedef Triplet<Scalar,int> T; + std::vector<T> elements; + + int M(-1), N(-1), NNZ(-1); + int count = 0; + while(input.getline(buffer, maxBuffersize)) + { + // skip comments + //NOTE An appropriate test should be done on the header to get the symmetry + if(buffer[0]=='%') + continue; + + std::stringstream line(buffer); + + if(!readsizes) + { + line >> M >> N >> NNZ; + if(M > 0 && N > 0 && NNZ > 0) + { + readsizes = true; + std::cout << "sizes: " << M << "," << N << "," << NNZ << "\n"; + mat.resize(M,N); + mat.reserve(NNZ); + } + } + else + { + int i(-1), j(-1); + Scalar value; + if( internal::GetMarketLine(line, M, N, i, j, value) ) + { + ++ count; + elements.push_back(T(i,j,value)); + } + else + std::cerr << "Invalid read: " << i << "," << j << "\n"; + } + } + mat.setFromTriplets(elements.begin(), elements.end()); + if(count!=NNZ) + std::cerr << count << "!=" << NNZ << "\n"; + + input.close(); + return true; +} + +template<typename VectorType> +bool loadMarketVector(VectorType& vec, const std::string& filename) +{ + typedef typename VectorType::Scalar Scalar; + std::ifstream in(filename.c_str(), std::ios::in); + if(!in) + return false; + + std::string line; + int n(0), col(0); + do + { // Skip comments + std::getline(in, line); assert(in.good()); + } while (line[0] == '%'); + std::istringstream newline(line); + newline >> n >> col; + assert(n>0 && col>0); + vec.resize(n); + int i = 0; + Scalar value; + while ( std::getline(in, line) && (i < n) ){ + internal::GetVectorElt(line, value); + vec(i++) = value; + } + in.close(); + if (i!=n){ + std::cerr<< "Unable to read all elements from file " << filename << "\n"; + return false; + } + return true; +} + +template<typename SparseMatrixType> +bool saveMarket(const SparseMatrixType& mat, const std::string& filename, int sym = 0) +{ + typedef typename SparseMatrixType::Scalar Scalar; + std::ofstream out(filename.c_str(),std::ios::out); + if(!out) + return false; + + out.flags(std::ios_base::scientific); + out.precision(64); + std::string header; + internal::putMarketHeader<Scalar>(header, sym); + out << header << std::endl; + out << mat.rows() << " " << mat.cols() << " " << mat.nonZeros() << "\n"; + int count = 0; + for(int j=0; j<mat.outerSize(); ++j) + for(typename SparseMatrixType::InnerIterator it(mat,j); it; ++it) + { + ++ count; + internal::PutMatrixElt(it.value(), it.row()+1, it.col()+1, out); + // out << it.row()+1 << " " << it.col()+1 << " " << it.value() << "\n"; + } + out.close(); + return true; +} + +template<typename VectorType> +bool saveMarketVector (const VectorType& vec, const std::string& filename) +{ + typedef typename VectorType::Scalar Scalar; + std::ofstream out(filename.c_str(),std::ios::out); + if(!out) + return false; + + out.flags(std::ios_base::scientific); + out.precision(64); + if(internal::is_same<Scalar, std::complex<float> >::value || internal::is_same<Scalar, std::complex<double> >::value) + out << "%%MatrixMarket matrix array complex general\n"; + else + out << "%%MatrixMarket matrix array real general\n"; + out << vec.size() << " "<< 1 << "\n"; + for (int i=0; i < vec.size(); i++){ + internal::putVectorElt(vec(i), out); + } + out.close(); + return true; +} + +} // end namespace Eigen + +#endif // EIGEN_SPARSE_MARKET_IO_H diff --git a/unsupported/Eigen/src/SparseExtra/MatrixMarketIterator.h b/unsupported/Eigen/src/SparseExtra/MatrixMarketIterator.h new file mode 100644 index 000000000..4716b68e7 --- /dev/null +++ b/unsupported/Eigen/src/SparseExtra/MatrixMarketIterator.h @@ -0,0 +1,221 @@ + +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Desire NUENTSA WAKAM <desire.nuentsa_wakam@inria.fr> +// +// 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_BROWSE_MATRICES_H +#define EIGEN_BROWSE_MATRICES_H + +namespace Eigen { + +enum { + SPD = 0x100, + NonSymmetric = 0x0 +}; + +/** + * @brief Iterator to browse matrices from a specified folder + * + * This is used to load all the matrices from a folder. + * The matrices should be in Matrix Market format + * It is assumed that the matrices are named as matname.mtx + * and matname_SPD.mtx if the matrix is Symmetric and positive definite (or Hermitian) + * The right hand side vectors are loaded as well, if they exist. + * They should be named as matname_b.mtx. + * Note that the right hand side for a SPD matrix is named as matname_SPD_b.mtx + * + * Sometimes a reference solution is available. In this case, it should be named as matname_x.mtx + * + * Sample code + * \code + * + * \endcode + * + * \tparam Scalar The scalar type + */ +template <typename Scalar> +class MatrixMarketIterator +{ + public: + typedef Matrix<Scalar,Dynamic,1> VectorType; + typedef SparseMatrix<Scalar,ColMajor> MatrixType; + + public: + MatrixMarketIterator(const std::string folder):m_sym(0),m_isvalid(false),m_matIsLoaded(false),m_hasRhs(false),m_hasrefX(false),m_folder(folder) + { + m_folder_id = opendir(folder.c_str()); + if (!m_folder_id){ + m_isvalid = false; + std::cerr << "The provided Matrix folder could not be opened \n\n"; + abort(); + } + Getnextvalidmatrix(); + } + + ~MatrixMarketIterator() + { + if (m_folder_id) closedir(m_folder_id); + } + + inline MatrixMarketIterator& operator++() + { + m_matIsLoaded = false; + m_hasrefX = false; + m_hasRhs = false; + Getnextvalidmatrix(); + return *this; + } + inline operator bool() const { return m_isvalid;} + + /** Return the sparse matrix corresponding to the current file */ + inline MatrixType& matrix() + { + // Read the matrix + if (m_matIsLoaded) return m_mat; + + std::string matrix_file = m_folder + "/" + m_matname + ".mtx"; + if ( !loadMarket(m_mat, matrix_file)) + { + m_matIsLoaded = false; + return m_mat; + } + m_matIsLoaded = true; + + if (m_sym != NonSymmetric) + { // Store the upper part of the matrix. It is needed by the solvers dealing with nonsymmetric matrices ?? + MatrixType B; + B = m_mat; + m_mat = B.template selfadjointView<Lower>(); + } + return m_mat; + } + + /** Return the right hand side corresponding to the current matrix. + * If the rhs file is not provided, a random rhs is generated + */ + inline VectorType& rhs() + { + // Get the right hand side + if (m_hasRhs) return m_rhs; + + std::string rhs_file; + rhs_file = m_folder + "/" + m_matname + "_b.mtx"; // The pattern is matname_b.mtx + m_hasRhs = Fileexists(rhs_file); + if (m_hasRhs) + { + m_rhs.resize(m_mat.cols()); + m_hasRhs = loadMarketVector(m_rhs, rhs_file); + } + if (!m_hasRhs) + { + // Generate a random right hand side + if (!m_matIsLoaded) this->matrix(); + m_refX.resize(m_mat.cols()); + m_refX.setRandom(); + m_rhs = m_mat * m_refX; + m_hasrefX = true; + m_hasRhs = true; + } + return m_rhs; + } + + /** Return a reference solution + * If it is not provided and if the right hand side is not available + * then refX is randomly generated such that A*refX = b + * where A and b are the matrix and the rhs. + * Note that when a rhs is provided, refX is not available + */ + inline VectorType& refX() + { + // Check if a reference solution is provided + if (m_hasrefX) return m_refX; + + std::string lhs_file; + lhs_file = m_folder + "/" + m_matname + "_x.mtx"; + m_hasrefX = Fileexists(lhs_file); + if (m_hasrefX) + { + m_refX.resize(m_mat.cols()); + m_hasrefX = loadMarketVector(m_refX, lhs_file); + } + return m_refX; + } + + inline std::string& matname() { return m_matname; } + + inline int sym() { return m_sym; } + + inline bool hasRhs() {return m_hasRhs; } + inline bool hasrefX() {return m_hasrefX; } + + protected: + + inline bool Fileexists(std::string file) + { + std::ifstream file_id(file.c_str()); + if (!file_id.good() ) + { + return false; + } + else + { + file_id.close(); + return true; + } + } + + void Getnextvalidmatrix( ) + { + m_isvalid = false; + // Here, we return with the next valid matrix in the folder + while ( (m_curs_id = readdir(m_folder_id)) != NULL) { + m_isvalid = false; + std::string curfile; + curfile = m_folder + "/" + m_curs_id->d_name; + // Discard if it is a folder + if (m_curs_id->d_type == DT_DIR) continue; //FIXME This may not be available on non BSD systems +// struct stat st_buf; +// stat (curfile.c_str(), &st_buf); +// if (S_ISDIR(st_buf.st_mode)) continue; + + // Determine from the header if it is a matrix or a right hand side + bool isvector,iscomplex; + if(!getMarketHeader(curfile,m_sym,iscomplex,isvector)) continue; + if(isvector) continue; + + // Get the matrix name + std::string filename = m_curs_id->d_name; + m_matname = filename.substr(0, filename.length()-4); + + // Find if the matrix is SPD + size_t found = m_matname.find("SPD"); + if( (found!=std::string::npos) && (m_sym != NonSymmetric) ) + m_sym = SPD; + + m_isvalid = true; + break; + } + } + int m_sym; // Symmetry of the matrix + MatrixType m_mat; // Current matrix + VectorType m_rhs; // Current vector + VectorType m_refX; // The reference solution, if exists + std::string m_matname; // Matrix Name + bool m_isvalid; + bool m_matIsLoaded; // Determine if the matrix has already been loaded from the file + bool m_hasRhs; // The right hand side exists + bool m_hasrefX; // A reference solution is provided + std::string m_folder; + DIR * m_folder_id; + struct dirent *m_curs_id; + +}; + +} // end namespace Eigen + +#endif diff --git a/unsupported/Eigen/src/SparseExtra/RandomSetter.h b/unsupported/Eigen/src/SparseExtra/RandomSetter.h new file mode 100644 index 000000000..dee1708e7 --- /dev/null +++ b/unsupported/Eigen/src/SparseExtra/RandomSetter.h @@ -0,0 +1,327 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> +// +// 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_RANDOMSETTER_H +#define EIGEN_RANDOMSETTER_H + +namespace Eigen { + +/** Represents a std::map + * + * \see RandomSetter + */ +template<typename Scalar> struct StdMapTraits +{ + typedef int KeyType; + typedef std::map<KeyType,Scalar> Type; + enum { + IsSorted = 1 + }; + + static void setInvalidKey(Type&, const KeyType&) {} +}; + +#ifdef EIGEN_UNORDERED_MAP_SUPPORT +/** Represents a std::unordered_map + * + * To use it you need to both define EIGEN_UNORDERED_MAP_SUPPORT and include the unordered_map header file + * yourself making sure that unordered_map is defined in the std namespace. + * + * For instance, with current version of gcc you can either enable C++0x standard (-std=c++0x) or do: + * \code + * #include <tr1/unordered_map> + * #define EIGEN_UNORDERED_MAP_SUPPORT + * namespace std { + * using std::tr1::unordered_map; + * } + * \endcode + * + * \see RandomSetter + */ +template<typename Scalar> struct StdUnorderedMapTraits +{ + typedef int KeyType; + typedef std::unordered_map<KeyType,Scalar> Type; + enum { + IsSorted = 0 + }; + + static void setInvalidKey(Type&, const KeyType&) {} +}; +#endif // EIGEN_UNORDERED_MAP_SUPPORT + +#ifdef _DENSE_HASH_MAP_H_ +/** Represents a google::dense_hash_map + * + * \see RandomSetter + */ +template<typename Scalar> struct GoogleDenseHashMapTraits +{ + typedef int KeyType; + typedef google::dense_hash_map<KeyType,Scalar> Type; + enum { + IsSorted = 0 + }; + + static void setInvalidKey(Type& map, const KeyType& k) + { map.set_empty_key(k); } +}; +#endif + +#ifdef _SPARSE_HASH_MAP_H_ +/** Represents a google::sparse_hash_map + * + * \see RandomSetter + */ +template<typename Scalar> struct GoogleSparseHashMapTraits +{ + typedef int KeyType; + typedef google::sparse_hash_map<KeyType,Scalar> Type; + enum { + IsSorted = 0 + }; + + static void setInvalidKey(Type&, const KeyType&) {} +}; +#endif + +/** \class RandomSetter + * + * \brief The RandomSetter is a wrapper object allowing to set/update a sparse matrix with random access + * + * \param SparseMatrixType the type of the sparse matrix we are updating + * \param MapTraits a traits class representing the map implementation used for the temporary sparse storage. + * Its default value depends on the system. + * \param OuterPacketBits defines the number of rows (or columns) manage by a single map object + * as a power of two exponent. + * + * This class temporarily represents a sparse matrix object using a generic map implementation allowing for + * efficient random access. The conversion from the compressed representation to a hash_map object is performed + * in the RandomSetter constructor, while the sparse matrix is updated back at destruction time. This strategy + * suggest the use of nested blocks as in this example: + * + * \code + * SparseMatrix<double> m(rows,cols); + * { + * RandomSetter<SparseMatrix<double> > w(m); + * // don't use m but w instead with read/write random access to the coefficients: + * for(;;) + * w(rand(),rand()) = rand; + * } + * // when w is deleted, the data are copied back to m + * // and m is ready to use. + * \endcode + * + * Since hash_map objects are not fully sorted, representing a full matrix as a single hash_map would + * involve a big and costly sort to update the compressed matrix back. To overcome this issue, a RandomSetter + * use multiple hash_map, each representing 2^OuterPacketBits columns or rows according to the storage order. + * To reach optimal performance, this value should be adjusted according to the average number of nonzeros + * per rows/columns. + * + * The possible values for the template parameter MapTraits are: + * - \b StdMapTraits: corresponds to std::map. (does not perform very well) + * - \b GnuHashMapTraits: corresponds to __gnu_cxx::hash_map (available only with GCC) + * - \b GoogleDenseHashMapTraits: corresponds to google::dense_hash_map (best efficiency, reasonable memory consumption) + * - \b GoogleSparseHashMapTraits: corresponds to google::sparse_hash_map (best memory consumption, relatively good performance) + * + * The default map implementation depends on the availability, and the preferred order is: + * GoogleSparseHashMapTraits, GnuHashMapTraits, and finally StdMapTraits. + * + * For performance and memory consumption reasons it is highly recommended to use one of + * the Google's hash_map implementation. To enable the support for them, you have two options: + * - \#include <google/dense_hash_map> yourself \b before Eigen/Sparse header + * - define EIGEN_GOOGLEHASH_SUPPORT + * In the later case the inclusion of <google/dense_hash_map> is made for you. + * + * \see http://code.google.com/p/google-sparsehash/ + */ +template<typename SparseMatrixType, + template <typename T> class MapTraits = +#if defined _DENSE_HASH_MAP_H_ + GoogleDenseHashMapTraits +#elif defined _HASH_MAP + GnuHashMapTraits +#else + StdMapTraits +#endif + ,int OuterPacketBits = 6> +class RandomSetter +{ + typedef typename SparseMatrixType::Scalar Scalar; + typedef typename SparseMatrixType::Index Index; + + struct ScalarWrapper + { + ScalarWrapper() : value(0) {} + Scalar value; + }; + typedef typename MapTraits<ScalarWrapper>::KeyType KeyType; + typedef typename MapTraits<ScalarWrapper>::Type HashMapType; + static const int OuterPacketMask = (1 << OuterPacketBits) - 1; + enum { + SwapStorage = 1 - MapTraits<ScalarWrapper>::IsSorted, + TargetRowMajor = (SparseMatrixType::Flags & RowMajorBit) ? 1 : 0, + SetterRowMajor = SwapStorage ? 1-TargetRowMajor : TargetRowMajor + }; + + public: + + /** Constructs a random setter object from the sparse matrix \a target + * + * Note that the initial value of \a target are imported. If you want to re-set + * a sparse matrix from scratch, then you must set it to zero first using the + * setZero() function. + */ + inline RandomSetter(SparseMatrixType& target) + : mp_target(&target) + { + const Index outerSize = SwapStorage ? target.innerSize() : target.outerSize(); + const Index innerSize = SwapStorage ? target.outerSize() : target.innerSize(); + m_outerPackets = outerSize >> OuterPacketBits; + if (outerSize&OuterPacketMask) + m_outerPackets += 1; + m_hashmaps = new HashMapType[m_outerPackets]; + // compute number of bits needed to store inner indices + Index aux = innerSize - 1; + m_keyBitsOffset = 0; + while (aux) + { + ++m_keyBitsOffset; + aux = aux >> 1; + } + KeyType ik = (1<<(OuterPacketBits+m_keyBitsOffset)); + for (Index k=0; k<m_outerPackets; ++k) + MapTraits<ScalarWrapper>::setInvalidKey(m_hashmaps[k],ik); + + // insert current coeffs + for (Index j=0; j<mp_target->outerSize(); ++j) + for (typename SparseMatrixType::InnerIterator it(*mp_target,j); it; ++it) + (*this)(TargetRowMajor?j:it.index(), TargetRowMajor?it.index():j) = it.value(); + } + + /** Destructor updating back the sparse matrix target */ + ~RandomSetter() + { + KeyType keyBitsMask = (1<<m_keyBitsOffset)-1; + if (!SwapStorage) // also means the map is sorted + { + mp_target->setZero(); + mp_target->makeCompressed(); + mp_target->reserve(nonZeros()); + Index prevOuter = -1; + for (Index k=0; k<m_outerPackets; ++k) + { + const Index outerOffset = (1<<OuterPacketBits) * k; + typename HashMapType::iterator end = m_hashmaps[k].end(); + for (typename HashMapType::iterator it = m_hashmaps[k].begin(); it!=end; ++it) + { + const Index outer = (it->first >> m_keyBitsOffset) + outerOffset; + const Index inner = it->first & keyBitsMask; + if (prevOuter!=outer) + { + for (Index j=prevOuter+1;j<=outer;++j) + mp_target->startVec(j); + prevOuter = outer; + } + mp_target->insertBackByOuterInner(outer, inner) = it->second.value; + } + } + mp_target->finalize(); + } + else + { + VectorXi positions(mp_target->outerSize()); + positions.setZero(); + // pass 1 + for (Index k=0; k<m_outerPackets; ++k) + { + typename HashMapType::iterator end = m_hashmaps[k].end(); + for (typename HashMapType::iterator it = m_hashmaps[k].begin(); it!=end; ++it) + { + const Index outer = it->first & keyBitsMask; + ++positions[outer]; + } + } + // prefix sum + Index count = 0; + for (Index j=0; j<mp_target->outerSize(); ++j) + { + Index tmp = positions[j]; + mp_target->outerIndexPtr()[j] = count; + positions[j] = count; + count += tmp; + } + mp_target->makeCompressed(); + mp_target->outerIndexPtr()[mp_target->outerSize()] = count; + mp_target->resizeNonZeros(count); + // pass 2 + for (Index k=0; k<m_outerPackets; ++k) + { + const Index outerOffset = (1<<OuterPacketBits) * k; + typename HashMapType::iterator end = m_hashmaps[k].end(); + for (typename HashMapType::iterator it = m_hashmaps[k].begin(); it!=end; ++it) + { + const Index inner = (it->first >> m_keyBitsOffset) + outerOffset; + const Index outer = it->first & keyBitsMask; + // sorted insertion + // Note that we have to deal with at most 2^OuterPacketBits unsorted coefficients, + // moreover those 2^OuterPacketBits coeffs are likely to be sparse, an so only a + // small fraction of them have to be sorted, whence the following simple procedure: + Index posStart = mp_target->outerIndexPtr()[outer]; + Index i = (positions[outer]++) - 1; + while ( (i >= posStart) && (mp_target->innerIndexPtr()[i] > inner) ) + { + mp_target->valuePtr()[i+1] = mp_target->valuePtr()[i]; + mp_target->innerIndexPtr()[i+1] = mp_target->innerIndexPtr()[i]; + --i; + } + mp_target->innerIndexPtr()[i+1] = inner; + mp_target->valuePtr()[i+1] = it->second.value; + } + } + } + delete[] m_hashmaps; + } + + /** \returns a reference to the coefficient at given coordinates \a row, \a col */ + Scalar& operator() (Index row, Index col) + { + const Index outer = SetterRowMajor ? row : col; + const Index inner = SetterRowMajor ? col : row; + const Index outerMajor = outer >> OuterPacketBits; // index of the packet/map + const Index outerMinor = outer & OuterPacketMask; // index of the inner vector in the packet + const KeyType key = (KeyType(outerMinor)<<m_keyBitsOffset) | inner; + return m_hashmaps[outerMajor][key].value; + } + + /** \returns the number of non zero coefficients + * + * \note According to the underlying map/hash_map implementation, + * this function might be quite expensive. + */ + Index nonZeros() const + { + Index nz = 0; + for (Index k=0; k<m_outerPackets; ++k) + nz += static_cast<Index>(m_hashmaps[k].size()); + return nz; + } + + + protected: + + HashMapType* m_hashmaps; + SparseMatrixType* mp_target; + Index m_outerPackets; + unsigned char m_keyBitsOffset; +}; + +} // end namespace Eigen + +#endif // EIGEN_RANDOMSETTER_H diff --git a/unsupported/Eigen/src/Splines/CMakeLists.txt b/unsupported/Eigen/src/Splines/CMakeLists.txt new file mode 100644 index 000000000..55c6271e9 --- /dev/null +++ b/unsupported/Eigen/src/Splines/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_Splines_SRCS "*.h") + +INSTALL(FILES + ${Eigen_Splines_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/Splines COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/Splines/Spline.h b/unsupported/Eigen/src/Splines/Spline.h new file mode 100644 index 000000000..3680f013a --- /dev/null +++ b/unsupported/Eigen/src/Splines/Spline.h @@ -0,0 +1,464 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.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_SPLINE_H +#define EIGEN_SPLINE_H + +#include "SplineFwd.h" + +namespace Eigen +{ + /** + * \ingroup Splines_Module + * \class Spline class + * \brief A class representing multi-dimensional spline curves. + * + * The class represents B-splines with non-uniform knot vectors. Each control + * point of the B-spline is associated with a basis function + * \f{align*} + * C(u) & = \sum_{i=0}^{n}N_{i,p}(u)P_i + * \f} + * + * \tparam _Scalar The underlying data type (typically float or double) + * \tparam _Dim The curve dimension (e.g. 2 or 3) + * \tparam _Degree Per default set to Dynamic; could be set to the actual desired + * degree for optimization purposes (would result in stack allocation + * of several temporary variables). + **/ + template <typename _Scalar, int _Dim, int _Degree> + class Spline + { + public: + typedef _Scalar Scalar; /*!< The spline curve's scalar type. */ + enum { Dimension = _Dim /*!< The spline curve's dimension. */ }; + enum { Degree = _Degree /*!< The spline curve's degree. */ }; + + /** \brief The point type the spline is representing. */ + typedef typename SplineTraits<Spline>::PointType PointType; + + /** \brief The data type used to store knot vectors. */ + typedef typename SplineTraits<Spline>::KnotVectorType KnotVectorType; + + /** \brief The data type used to store non-zero basis functions. */ + typedef typename SplineTraits<Spline>::BasisVectorType BasisVectorType; + + /** \brief The data type representing the spline's control points. */ + typedef typename SplineTraits<Spline>::ControlPointVectorType ControlPointVectorType; + + /** + * \brief Creates a spline from a knot vector and control points. + * \param knots The spline's knot vector. + * \param ctrls The spline's control point vector. + **/ + template <typename OtherVectorType, typename OtherArrayType> + Spline(const OtherVectorType& knots, const OtherArrayType& ctrls) : m_knots(knots), m_ctrls(ctrls) {} + + /** + * \brief Copy constructor for splines. + * \param spline The input spline. + **/ + template <int OtherDegree> + Spline(const Spline<Scalar, Dimension, OtherDegree>& spline) : + m_knots(spline.knots()), m_ctrls(spline.ctrls()) {} + + /** + * \brief Returns the knots of the underlying spline. + **/ + const KnotVectorType& knots() const { return m_knots; } + + /** + * \brief Returns the knots of the underlying spline. + **/ + const ControlPointVectorType& ctrls() const { return m_ctrls; } + + /** + * \brief Returns the spline value at a given site \f$u\f$. + * + * The function returns + * \f{align*} + * C(u) & = \sum_{i=0}^{n}N_{i,p}P_i + * \f} + * + * \param u Parameter \f$u \in [0;1]\f$ at which the spline is evaluated. + * \return The spline value at the given location \f$u\f$. + **/ + PointType operator()(Scalar u) const; + + /** + * \brief Evaluation of spline derivatives of up-to given order. + * + * The function returns + * \f{align*} + * \frac{d^i}{du^i}C(u) & = \sum_{i=0}^{n} \frac{d^i}{du^i} N_{i,p}(u)P_i + * \f} + * for i ranging between 0 and order. + * + * \param u Parameter \f$u \in [0;1]\f$ at which the spline derivative is evaluated. + * \param order The order up to which the derivatives are computed. + **/ + typename SplineTraits<Spline>::DerivativeType + derivatives(Scalar u, DenseIndex order) const; + + /** + * \copydoc Spline::derivatives + * Using the template version of this function is more efficieent since + * temporary objects are allocated on the stack whenever this is possible. + **/ + template <int DerivativeOrder> + typename SplineTraits<Spline,DerivativeOrder>::DerivativeType + derivatives(Scalar u, DenseIndex order = DerivativeOrder) const; + + /** + * \brief Computes the non-zero basis functions at the given site. + * + * Splines have local support and a point from their image is defined + * by exactly \f$p+1\f$ control points \f$P_i\f$ where \f$p\f$ is the + * spline degree. + * + * This function computes the \f$p+1\f$ non-zero basis function values + * for a given parameter value \f$u\f$. It returns + * \f{align*}{ + * N_{i,p}(u), \hdots, N_{i+p+1,p}(u) + * \f} + * + * \param u Parameter \f$u \in [0;1]\f$ at which the non-zero basis functions + * are computed. + **/ + typename SplineTraits<Spline>::BasisVectorType + basisFunctions(Scalar u) const; + + /** + * \brief Computes the non-zero spline basis function derivatives up to given order. + * + * The function computes + * \f{align*}{ + * \frac{d^i}{du^i} N_{i,p}(u), \hdots, \frac{d^i}{du^i} N_{i+p+1,p}(u) + * \f} + * with i ranging from 0 up to the specified order. + * + * \param u Parameter \f$u \in [0;1]\f$ at which the non-zero basis function + * derivatives are computed. + * \param order The order up to which the basis function derivatives are computes. + **/ + typename SplineTraits<Spline>::BasisDerivativeType + basisFunctionDerivatives(Scalar u, DenseIndex order) const; + + /** + * \copydoc Spline::basisFunctionDerivatives + * Using the template version of this function is more efficieent since + * temporary objects are allocated on the stack whenever this is possible. + **/ + template <int DerivativeOrder> + typename SplineTraits<Spline,DerivativeOrder>::BasisDerivativeType + basisFunctionDerivatives(Scalar u, DenseIndex order = DerivativeOrder) const; + + /** + * \brief Returns the spline degree. + **/ + DenseIndex degree() const; + + /** + * \brief Returns the span within the knot vector in which u is falling. + * \param u The site for which the span is determined. + **/ + DenseIndex span(Scalar u) const; + + /** + * \brief Computes the spang within the provided knot vector in which u is falling. + **/ + static DenseIndex Span(typename SplineTraits<Spline>::Scalar u, DenseIndex degree, const typename SplineTraits<Spline>::KnotVectorType& knots); + + /** + * \brief Returns the spline's non-zero basis functions. + * + * The function computes and returns + * \f{align*}{ + * N_{i,p}(u), \hdots, N_{i+p+1,p}(u) + * \f} + * + * \param u The site at which the basis functions are computed. + * \param degree The degree of the underlying spline. + * \param knots The underlying spline's knot vector. + **/ + static BasisVectorType BasisFunctions(Scalar u, DenseIndex degree, const KnotVectorType& knots); + + + private: + KnotVectorType m_knots; /*!< Knot vector. */ + ControlPointVectorType m_ctrls; /*!< Control points. */ + }; + + template <typename _Scalar, int _Dim, int _Degree> + DenseIndex Spline<_Scalar, _Dim, _Degree>::Span( + typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::Scalar u, + DenseIndex degree, + const typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::KnotVectorType& knots) + { + // Piegl & Tiller, "The NURBS Book", A2.1 (p. 68) + if (u <= knots(0)) return degree; + const Scalar* pos = std::upper_bound(knots.data()+degree-1, knots.data()+knots.size()-degree-1, u); + return static_cast<DenseIndex>( std::distance(knots.data(), pos) - 1 ); + } + + template <typename _Scalar, int _Dim, int _Degree> + typename Spline<_Scalar, _Dim, _Degree>::BasisVectorType + Spline<_Scalar, _Dim, _Degree>::BasisFunctions( + typename Spline<_Scalar, _Dim, _Degree>::Scalar u, + DenseIndex degree, + const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& knots) + { + typedef typename Spline<_Scalar, _Dim, _Degree>::BasisVectorType BasisVectorType; + + const DenseIndex p = degree; + const DenseIndex i = Spline::Span(u, degree, knots); + + const KnotVectorType& U = knots; + + BasisVectorType left(p+1); left(0) = Scalar(0); + BasisVectorType right(p+1); right(0) = Scalar(0); + + VectorBlock<BasisVectorType,Degree>(left,1,p) = u - VectorBlock<const KnotVectorType,Degree>(U,i+1-p,p).reverse(); + VectorBlock<BasisVectorType,Degree>(right,1,p) = VectorBlock<const KnotVectorType,Degree>(U,i+1,p) - u; + + BasisVectorType N(1,p+1); + N(0) = Scalar(1); + for (DenseIndex j=1; j<=p; ++j) + { + Scalar saved = Scalar(0); + for (DenseIndex r=0; r<j; r++) + { + const Scalar tmp = N(r)/(right(r+1)+left(j-r)); + N[r] = saved + right(r+1)*tmp; + saved = left(j-r)*tmp; + } + N(j) = saved; + } + return N; + } + + template <typename _Scalar, int _Dim, int _Degree> + DenseIndex Spline<_Scalar, _Dim, _Degree>::degree() const + { + if (_Degree == Dynamic) + return m_knots.size() - m_ctrls.cols() - 1; + else + return _Degree; + } + + template <typename _Scalar, int _Dim, int _Degree> + DenseIndex Spline<_Scalar, _Dim, _Degree>::span(Scalar u) const + { + return Spline::Span(u, degree(), knots()); + } + + template <typename _Scalar, int _Dim, int _Degree> + typename Spline<_Scalar, _Dim, _Degree>::PointType Spline<_Scalar, _Dim, _Degree>::operator()(Scalar u) const + { + enum { Order = SplineTraits<Spline>::OrderAtCompileTime }; + + const DenseIndex span = this->span(u); + const DenseIndex p = degree(); + const BasisVectorType basis_funcs = basisFunctions(u); + + const Replicate<BasisVectorType,Dimension,1> ctrl_weights(basis_funcs); + const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(ctrls(),0,span-p,Dimension,p+1); + return (ctrl_weights * ctrl_pts).rowwise().sum(); + } + + /* --------------------------------------------------------------------------------------------- */ + + template <typename SplineType, typename DerivativeType> + void derivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& der) + { + enum { Dimension = SplineTraits<SplineType>::Dimension }; + enum { Order = SplineTraits<SplineType>::OrderAtCompileTime }; + enum { DerivativeOrder = DerivativeType::ColsAtCompileTime }; + + typedef typename SplineTraits<SplineType>::Scalar Scalar; + + typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType; + typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType; + + typedef typename SplineTraits<SplineType,DerivativeOrder>::BasisDerivativeType BasisDerivativeType; + typedef typename BasisDerivativeType::ConstRowXpr BasisDerivativeRowXpr; + + const DenseIndex p = spline.degree(); + const DenseIndex span = spline.span(u); + + const DenseIndex n = (std::min)(p, order); + + der.resize(Dimension,n+1); + + // Retrieve the basis function derivatives up to the desired order... + const BasisDerivativeType basis_func_ders = spline.template basisFunctionDerivatives<DerivativeOrder>(u, n+1); + + // ... and perform the linear combinations of the control points. + for (DenseIndex der_order=0; der_order<n+1; ++der_order) + { + const Replicate<BasisDerivativeRowXpr,Dimension,1> ctrl_weights( basis_func_ders.row(der_order) ); + const Block<const ControlPointVectorType,Dimension,Order> ctrl_pts(spline.ctrls(),0,span-p,Dimension,p+1); + der.col(der_order) = (ctrl_weights * ctrl_pts).rowwise().sum(); + } + } + + template <typename _Scalar, int _Dim, int _Degree> + typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::DerivativeType + Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const + { + typename SplineTraits< Spline >::DerivativeType res; + derivativesImpl(*this, u, order, res); + return res; + } + + template <typename _Scalar, int _Dim, int _Degree> + template <int DerivativeOrder> + typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::DerivativeType + Spline<_Scalar, _Dim, _Degree>::derivatives(Scalar u, DenseIndex order) const + { + typename SplineTraits< Spline, DerivativeOrder >::DerivativeType res; + derivativesImpl(*this, u, order, res); + return res; + } + + template <typename _Scalar, int _Dim, int _Degree> + typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisVectorType + Spline<_Scalar, _Dim, _Degree>::basisFunctions(Scalar u) const + { + return Spline::BasisFunctions(u, degree(), knots()); + } + + /* --------------------------------------------------------------------------------------------- */ + + template <typename SplineType, typename DerivativeType> + void basisFunctionDerivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& N_) + { + enum { Order = SplineTraits<SplineType>::OrderAtCompileTime }; + + typedef typename SplineTraits<SplineType>::Scalar Scalar; + typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType; + typedef typename SplineTraits<SplineType>::KnotVectorType KnotVectorType; + typedef typename SplineTraits<SplineType>::ControlPointVectorType ControlPointVectorType; + + const KnotVectorType& U = spline.knots(); + + const DenseIndex p = spline.degree(); + const DenseIndex span = spline.span(u); + + const DenseIndex n = (std::min)(p, order); + + N_.resize(n+1, p+1); + + BasisVectorType left = BasisVectorType::Zero(p+1); + BasisVectorType right = BasisVectorType::Zero(p+1); + + Matrix<Scalar,Order,Order> ndu(p+1,p+1); + + double saved, temp; + + ndu(0,0) = 1.0; + + DenseIndex j; + for (j=1; j<=p; ++j) + { + left[j] = u-U[span+1-j]; + right[j] = U[span+j]-u; + saved = 0.0; + + for (DenseIndex r=0; r<j; ++r) + { + /* Lower triangle */ + ndu(j,r) = right[r+1]+left[j-r]; + temp = ndu(r,j-1)/ndu(j,r); + /* Upper triangle */ + ndu(r,j) = static_cast<Scalar>(saved+right[r+1] * temp); + saved = left[j-r] * temp; + } + + ndu(j,j) = static_cast<Scalar>(saved); + } + + for (j = p; j>=0; --j) + N_(0,j) = ndu(j,p); + + // Compute the derivatives + DerivativeType a(n+1,p+1); + DenseIndex r=0; + for (; r<=p; ++r) + { + DenseIndex s1,s2; + s1 = 0; s2 = 1; // alternate rows in array a + a(0,0) = 1.0; + + // Compute the k-th derivative + for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k) + { + double d = 0.0; + DenseIndex rk,pk,j1,j2; + rk = r-k; pk = p-k; + + if (r>=k) + { + a(s2,0) = a(s1,0)/ndu(pk+1,rk); + d = a(s2,0)*ndu(rk,pk); + } + + if (rk>=-1) j1 = 1; + else j1 = -rk; + + if (r-1 <= pk) j2 = k-1; + else j2 = p-r; + + for (j=j1; j<=j2; ++j) + { + a(s2,j) = (a(s1,j)-a(s1,j-1))/ndu(pk+1,rk+j); + d += a(s2,j)*ndu(rk+j,pk); + } + + if (r<=pk) + { + a(s2,k) = -a(s1,k-1)/ndu(pk+1,r); + d += a(s2,k)*ndu(r,pk); + } + + N_(k,r) = static_cast<Scalar>(d); + j = s1; s1 = s2; s2 = j; // Switch rows + } + } + + /* Multiply through by the correct factors */ + /* (Eq. [2.9]) */ + r = p; + for (DenseIndex k=1; k<=static_cast<DenseIndex>(n); ++k) + { + for (DenseIndex j=p; j>=0; --j) N_(k,j) *= r; + r *= p-k; + } + } + + template <typename _Scalar, int _Dim, int _Degree> + typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType + Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const + { + typename SplineTraits< Spline >::BasisDerivativeType der; + basisFunctionDerivativesImpl(*this, u, order, der); + return der; + } + + template <typename _Scalar, int _Dim, int _Degree> + template <int DerivativeOrder> + typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType + Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const + { + typename SplineTraits< Spline, DerivativeOrder >::BasisDerivativeType der; + basisFunctionDerivativesImpl(*this, u, order, der); + return der; + } +} + +#endif // EIGEN_SPLINE_H diff --git a/unsupported/Eigen/src/Splines/SplineFitting.h b/unsupported/Eigen/src/Splines/SplineFitting.h new file mode 100644 index 000000000..1b566332f --- /dev/null +++ b/unsupported/Eigen/src/Splines/SplineFitting.h @@ -0,0 +1,159 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.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_SPLINE_FITTING_H +#define EIGEN_SPLINE_FITTING_H + +#include <numeric> + +#include "SplineFwd.h" + +#include <Eigen/QR> + +namespace Eigen +{ + /** + * \brief Computes knot averages. + * \ingroup Splines_Module + * + * The knots are computed as + * \f{align*} + * u_0 & = \hdots = u_p = 0 \\ + * u_{m-p} & = \hdots = u_{m} = 1 \\ + * u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p + * \f} + * where \f$p\f$ is the degree and \f$m+1\f$ the number knots + * of the desired interpolating spline. + * + * \param[in] parameters The input parameters. During interpolation one for each data point. + * \param[in] degree The spline degree which is used during the interpolation. + * \param[out] knots The output knot vector. + * + * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data + **/ + template <typename KnotVectorType> + void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots) + { + typedef typename KnotVectorType::Scalar Scalar; + + knots.resize(parameters.size()+degree+1); + + for (DenseIndex j=1; j<parameters.size()-degree; ++j) + knots(j+degree) = parameters.segment(j,degree).mean(); + + knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1); + knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1); + } + + /** + * \brief Computes chord length parameters which are required for spline interpolation. + * \ingroup Splines_Module + * + * \param[in] pts The data points to which a spline should be fit. + * \param[out] chord_lengths The resulting chord lenggth vector. + * + * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data + **/ + template <typename PointArrayType, typename KnotVectorType> + void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths) + { + typedef typename KnotVectorType::Scalar Scalar; + + const DenseIndex n = pts.cols(); + + // 1. compute the column-wise norms + chord_lengths.resize(pts.cols()); + chord_lengths[0] = 0; + chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm(); + + // 2. compute the partial sums + std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data()); + + // 3. normalize the data + chord_lengths /= chord_lengths(n-1); + chord_lengths(n-1) = Scalar(1); + } + + /** + * \brief Spline fitting methods. + * \ingroup Splines_Module + **/ + template <typename SplineType> + struct SplineFitting + { + typedef typename SplineType::KnotVectorType KnotVectorType; + + /** + * \brief Fits an interpolating Spline to the given data points. + * + * \param pts The points for which an interpolating spline will be computed. + * \param degree The degree of the interpolating spline. + * + * \returns A spline interpolating the initially provided points. + **/ + template <typename PointArrayType> + static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree); + + /** + * \brief Fits an interpolating Spline to the given data points. + * + * \param pts The points for which an interpolating spline will be computed. + * \param degree The degree of the interpolating spline. + * \param knot_parameters The knot parameters for the interpolation. + * + * \returns A spline interpolating the initially provided points. + **/ + template <typename PointArrayType> + static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters); + }; + + template <typename SplineType> + template <typename PointArrayType> + SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters) + { + typedef typename SplineType::KnotVectorType::Scalar Scalar; + typedef typename SplineType::BasisVectorType BasisVectorType; + typedef typename SplineType::ControlPointVectorType ControlPointVectorType; + + typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType; + + KnotVectorType knots; + KnotAveraging(knot_parameters, degree, knots); + + DenseIndex n = pts.cols(); + MatrixType A = MatrixType::Zero(n,n); + for (DenseIndex i=1; i<n-1; ++i) + { + const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots); + + // The segment call should somehow be told the spline order at compile time. + A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots); + } + A(0,0) = 1.0; + A(n-1,n-1) = 1.0; + + HouseholderQR<MatrixType> qr(A); + + // Here, we are creating a temporary due to an Eigen issue. + ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose(); + + return SplineType(knots, ctrls); + } + + template <typename SplineType> + template <typename PointArrayType> + SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree) + { + KnotVectorType chord_lengths; // knot parameters + ChordLengths(pts, chord_lengths); + return Interpolate(pts, degree, chord_lengths); + } +} + +#endif // EIGEN_SPLINE_FITTING_H diff --git a/unsupported/Eigen/src/Splines/SplineFwd.h b/unsupported/Eigen/src/Splines/SplineFwd.h new file mode 100644 index 000000000..49db8d35d --- /dev/null +++ b/unsupported/Eigen/src/Splines/SplineFwd.h @@ -0,0 +1,86 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.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_SPLINES_FWD_H +#define EIGEN_SPLINES_FWD_H + +#include <Eigen/Core> + +namespace Eigen +{ + template <typename Scalar, int Dim, int Degree = Dynamic> class Spline; + + template < typename SplineType, int DerivativeOrder = Dynamic > struct SplineTraits {}; + + /** + * \ingroup Splines_Module + * \brief Compile-time attributes of the Spline class for Dynamic degree. + **/ + template <typename _Scalar, int _Dim, int _Degree> + struct SplineTraits< Spline<_Scalar, _Dim, _Degree>, Dynamic > + { + typedef _Scalar Scalar; /*!< The spline curve's scalar type. */ + enum { Dimension = _Dim /*!< The spline curve's dimension. */ }; + enum { Degree = _Degree /*!< The spline curve's degree. */ }; + + enum { OrderAtCompileTime = _Degree==Dynamic ? Dynamic : _Degree+1 /*!< The spline curve's order at compile-time. */ }; + enum { NumOfDerivativesAtCompileTime = OrderAtCompileTime /*!< The number of derivatives defined for the current spline. */ }; + + /** \brief The data type used to store non-zero basis functions. */ + typedef Array<Scalar,1,OrderAtCompileTime> BasisVectorType; + + /** \brief The data type used to store the values of the basis function derivatives. */ + typedef Array<Scalar,Dynamic,Dynamic,RowMajor,NumOfDerivativesAtCompileTime,OrderAtCompileTime> BasisDerivativeType; + + /** \brief The data type used to store the spline's derivative values. */ + typedef Array<Scalar,Dimension,Dynamic,ColMajor,Dimension,NumOfDerivativesAtCompileTime> DerivativeType; + + /** \brief The point type the spline is representing. */ + typedef Array<Scalar,Dimension,1> PointType; + + /** \brief The data type used to store knot vectors. */ + typedef Array<Scalar,1,Dynamic> KnotVectorType; + + /** \brief The data type representing the spline's control points. */ + typedef Array<Scalar,Dimension,Dynamic> ControlPointVectorType; + }; + + /** + * \ingroup Splines_Module + * \brief Compile-time attributes of the Spline class for fixed degree. + * + * The traits class inherits all attributes from the SplineTraits of Dynamic degree. + **/ + template < typename _Scalar, int _Dim, int _Degree, int _DerivativeOrder > + struct SplineTraits< Spline<_Scalar, _Dim, _Degree>, _DerivativeOrder > : public SplineTraits< Spline<_Scalar, _Dim, _Degree> > + { + enum { OrderAtCompileTime = _Degree==Dynamic ? Dynamic : _Degree+1 /*!< The spline curve's order at compile-time. */ }; + enum { NumOfDerivativesAtCompileTime = _DerivativeOrder==Dynamic ? Dynamic : _DerivativeOrder+1 /*!< The number of derivatives defined for the current spline. */ }; + + /** \brief The data type used to store the values of the basis function derivatives. */ + typedef Array<_Scalar,Dynamic,Dynamic,RowMajor,NumOfDerivativesAtCompileTime,OrderAtCompileTime> BasisDerivativeType; + + /** \brief The data type used to store the spline's derivative values. */ + typedef Array<_Scalar,_Dim,Dynamic,ColMajor,_Dim,NumOfDerivativesAtCompileTime> DerivativeType; + }; + + /** \brief 2D float B-spline with dynamic degree. */ + typedef Spline<float,2> Spline2f; + + /** \brief 3D float B-spline with dynamic degree. */ + typedef Spline<float,3> Spline3f; + + /** \brief 2D double B-spline with dynamic degree. */ + typedef Spline<double,2> Spline2d; + + /** \brief 3D double B-spline with dynamic degree. */ + typedef Spline<double,3> Spline3d; +} + +#endif // EIGEN_SPLINES_FWD_H |