diff options
author | 2009-10-28 19:06:45 -0400 | |
---|---|---|
committer | 2009-10-28 19:06:45 -0400 | |
commit | e8dd552257f0e886ee281aa84b7094fc489608d0 (patch) | |
tree | 596633c4d3e721b6d4fad31520423b9c0e03bf8a /unsupported/Eigen/src | |
parent | 2840ac7e948ecb3c7b19ba8f581f829a4a4e1fea (diff) | |
parent | 6219f9acfa61e54baf266f816b7eaf9ffbd9841e (diff) |
sync with mainline
Diffstat (limited to 'unsupported/Eigen/src')
-rw-r--r-- | unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h | 4 | ||||
-rw-r--r-- | unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h | 97 | ||||
-rw-r--r-- | unsupported/Eigen/src/FFT/ei_fftw_impl.h | 224 | ||||
-rw-r--r-- | unsupported/Eigen/src/FFT/ei_kissfft_impl.h | 414 |
4 files changed, 702 insertions, 37 deletions
diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h index a5e881487..b3983f8a6 100644 --- a/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h @@ -50,10 +50,12 @@ public: typedef typename Functor::InputType InputType; typedef typename Functor::ValueType ValueType; typedef typename Functor::JacobianType JacobianType; + typedef typename JacobianType::Scalar Scalar; - typedef Matrix<double,InputsAtCompileTime,1> DerivativeType; + typedef Matrix<Scalar,InputsAtCompileTime,1> DerivativeType; typedef AutoDiffScalar<DerivativeType> ActiveScalar; + typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput; typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue; diff --git a/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h index 888aa5c8c..2fb733a99 100644 --- a/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h +++ b/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h @@ -42,9 +42,17 @@ void ei_make_coherent(const A& a, const B&b) /** \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) + * \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. + * 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, @@ -56,10 +64,11 @@ void ei_make_coherent(const A& a, const B&b) * while derivatives are computed right away. * */ -template<typename DerType> +template<typename _DerType> class AutoDiffScalar { public: + typedef typename ei_cleantype<_DerType>::type DerType; typedef typename ei_traits<DerType>::Scalar Scalar; inline AutoDiffScalar() {} @@ -108,12 +117,28 @@ class AutoDiffScalar inline const DerType& derivatives() const { return m_derivatives; } inline DerType& derivatives() { return m_derivatives; } + 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 AutoDiffScalar& operator+=(const Scalar& other) + { + value() += other; + return *this; + } + template<typename OtherDerType> - inline const AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>,DerType,OtherDerType> > + inline const AutoDiffScalar<typename MakeCwiseBinaryOp<ei_scalar_sum_op<Scalar>,DerType,typename ei_cleantype<OtherDerType>::type>::Type > operator+(const AutoDiffScalar<OtherDerType>& other) const { ei_make_coherent(m_derivatives, other.derivatives()); - return AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>,DerType,OtherDerType> >( + return AutoDiffScalar<typename MakeCwiseBinaryOp<ei_scalar_sum_op<Scalar>,DerType,typename ei_cleantype<OtherDerType>::type>::Type >( m_value + other.value(), m_derivatives + other.derivatives()); } @@ -127,11 +152,11 @@ class AutoDiffScalar } template<typename OtherDerType> - inline const AutoDiffScalar<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, DerType,OtherDerType> > + inline const AutoDiffScalar<typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>, DerType,typename ei_cleantype<OtherDerType>::type>::Type > operator-(const AutoDiffScalar<OtherDerType>& other) const { ei_make_coherent(m_derivatives, other.derivatives()); - return AutoDiffScalar<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, DerType,OtherDerType> >( + return AutoDiffScalar<typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>, DerType,typename ei_cleantype<OtherDerType>::type>::Type >( m_value - other.value(), m_derivatives - other.derivatives()); } @@ -145,73 +170,73 @@ class AutoDiffScalar } template<typename OtherDerType> - inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_opposite_op<Scalar>, DerType> > + inline const AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_opposite_op<Scalar>, DerType>::Type > operator-() const { - return AutoDiffScalar<CwiseUnaryOp<ei_scalar_opposite_op<Scalar>, DerType> >( + return AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_opposite_op<Scalar>, DerType>::Type >( -m_value, -m_derivatives); } - inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > + inline const AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type > operator*(const Scalar& other) const { - return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >( + return AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type >( m_value * other, (m_derivatives * other)); } - friend inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > + friend inline const AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type > operator*(const Scalar& other, const AutoDiffScalar& a) { - return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >( + return AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type >( a.value() * other, a.derivatives() * other); } - inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > + inline const AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type > operator/(const Scalar& other) const { - return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >( + return AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type >( m_value / other, (m_derivatives * (Scalar(1)/other))); } - friend inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > + friend inline const AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type > operator/(const Scalar& other, const AutoDiffScalar& a) { - return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >( + return AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type >( other / a.value(), a.derivatives() * (-Scalar(1)/other)); } template<typename OtherDerType> - inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, - NestByValue<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, OtherDerType> > > > > > + inline const AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, + typename MakeNestByValue<typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type>::Type, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, typename ei_cleantype<OtherDerType>::type>::Type>::Type >::Type >::Type >::Type > operator/(const AutoDiffScalar<OtherDerType>& other) const { ei_make_coherent(m_derivatives, other.derivatives()); - return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, - NestByValue<CwiseBinaryOp<ei_scalar_difference_op<Scalar>, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, OtherDerType> > > > > >( + return AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, + typename MakeNestByValue<typename MakeCwiseBinaryOp<ei_scalar_difference_op<Scalar>, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type>::Type, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, typename ei_cleantype<OtherDerType>::type>::Type>::Type >::Type >::Type >::Type >( m_value / other.value(), ((m_derivatives * other.value()).nestByValue() - (m_value * other.derivatives()).nestByValue()).nestByValue() * (Scalar(1)/(other.value()*other.value()))); } template<typename OtherDerType> - inline const AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, OtherDerType> > > > + inline const AutoDiffScalar<typename MakeCwiseBinaryOp<ei_scalar_sum_op<Scalar>, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type>::Type, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, typename ei_cleantype<OtherDerType>::type>::Type>::Type >::Type > operator*(const AutoDiffScalar<OtherDerType>& other) const { ei_make_coherent(m_derivatives, other.derivatives()); - return AutoDiffScalar<CwiseBinaryOp<ei_scalar_sum_op<Scalar>, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >, - NestByValue<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, OtherDerType> > > >( + return AutoDiffScalar<typename MakeCwiseBinaryOp<ei_scalar_sum_op<Scalar>, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type>::Type, + typename MakeNestByValue<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, typename ei_cleantype<OtherDerType>::type>::Type>::Type >::Type >( m_value * other.value(), (m_derivatives * other.value()).nestByValue() + (m_value * other.derivatives()).nestByValue()); } @@ -283,11 +308,11 @@ struct ei_make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRo #define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \ template<typename DerType> \ - inline const Eigen::AutoDiffScalar<Eigen::CwiseUnaryOp<Eigen::ei_scalar_multiple_op<typename Eigen::ei_traits<DerType>::Scalar>, DerType> > \ + inline const Eigen::AutoDiffScalar<typename Eigen::MakeCwiseUnaryOp<Eigen::ei_scalar_multiple_op<typename Eigen::ei_traits<DerType>::Scalar>, DerType>::Type > \ FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \ using namespace Eigen; \ typedef typename ei_traits<DerType>::Scalar Scalar; \ - typedef AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> > ReturnType; \ + typedef AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type > ReturnType; \ CODE; \ } @@ -314,12 +339,12 @@ namespace std return ReturnType(std::log(x.value),x.derivatives() * (Scalar(1).x.value()));) template<typename DerType> - inline const Eigen::AutoDiffScalar<Eigen::CwiseUnaryOp<Eigen::ei_scalar_multiple_op<typename Eigen::ei_traits<DerType>::Scalar>, DerType> > + inline const Eigen::AutoDiffScalar<typename Eigen::MakeCwiseUnaryOp<Eigen::ei_scalar_multiple_op<typename Eigen::ei_traits<DerType>::Scalar>, DerType>::Type > pow(const Eigen::AutoDiffScalar<DerType>& x, typename Eigen::ei_traits<DerType>::Scalar y) { using namespace Eigen; typedef typename ei_traits<DerType>::Scalar Scalar; - return AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType> >( + return AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, DerType>::Type >( std::pow(x.value(),y), x.derivatives() * (y * std::pow(x.value(),y-1))); } @@ -359,7 +384,7 @@ EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(ei_log, return ReturnType(ei_log(x.value),x.derivatives() * (Scalar(1).x.value()));) template<typename DerType> -inline const AutoDiffScalar<CwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<DerType>::Scalar>, DerType> > +inline const AutoDiffScalar<typename MakeCwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<DerType>::Scalar>, DerType>::Type > ei_pow(const AutoDiffScalar<DerType>& x, typename ei_traits<DerType>::Scalar y) { return std::pow(x,y);} 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..e1f67f334 --- /dev/null +++ b/unsupported/Eigen/src/FFT/ei_fftw_impl.h @@ -0,0 +1,224 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Mark Borgerding mark a borgerding net +// +// Eigen 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; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen 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 or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see <http://www.gnu.org/licenses/>. + + + + // 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 * ei_fftw_cast(const T* p) + { + return const_cast<T*>( p); + } + + inline + fftw_complex * ei_fftw_cast( const std::complex<double> * p) + { + return const_cast<fftw_complex*>( reinterpret_cast<const fftw_complex*>(p) ); + } + + inline + fftwf_complex * ei_fftw_cast( const std::complex<float> * p) + { + return const_cast<fftwf_complex*>( reinterpret_cast<const fftwf_complex*>(p) ); + } + + inline + fftwl_complex * ei_fftw_cast( const std::complex<long double> * p) + { + return const_cast<fftwl_complex*>( reinterpret_cast<const fftwl_complex*>(p) ); + } + + template <typename T> + struct ei_fftw_plan {}; + + template <> + struct ei_fftw_plan<float> + { + typedef float scalar_type; + typedef fftwf_complex complex_type; + fftwf_plan m_plan; + ei_fftw_plan() :m_plan(NULL) {} + ~ei_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); + 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); + 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); + 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); + fftwf_execute_dft_c2r( m_plan, src,dst); + } + }; + template <> + struct ei_fftw_plan<double> + { + typedef double scalar_type; + typedef fftw_complex complex_type; + fftw_plan m_plan; + ei_fftw_plan() :m_plan(NULL) {} + ~ei_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_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_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_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_execute_dft_c2r( m_plan, src,dst); + } + }; + template <> + struct ei_fftw_plan<long double> + { + typedef long double scalar_type; + typedef fftwl_complex complex_type; + fftwl_plan m_plan; + ei_fftw_plan() :m_plan(NULL) {} + ~ei_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); + 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); + 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); + 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); + fftwl_execute_dft_c2r( m_plan, src,dst); + } + }; + + template <typename _Scalar> + struct ei_fftw_impl + { + typedef _Scalar Scalar; + typedef std::complex<Scalar> Complex; + + inline + void clear() + { + m_plans.clear(); + } + + inline + void fwd( Complex * dst,const Complex *src,int nfft) + { + get_plan(nfft,false,dst,src).fwd(ei_fftw_cast(dst), ei_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(ei_fftw_cast(dst), ei_fftw_cast(src) ,nfft); + int nhbins=(nfft>>1)+1; + for (int k=nhbins;k < nfft; ++k ) + dst[k] = conj(dst[nfft-k]); + } + + // inverse complex-to-complex + inline + void inv(Complex * dst,const Complex *src,int nfft) + { + get_plan(nfft,true,dst,src).inv(ei_fftw_cast(dst), ei_fftw_cast(src),nfft ); + + //TODO move scaling to Eigen::FFT + // scaling + Scalar s = Scalar(1.)/nfft; + for (int k=0;k<nfft;++k) + dst[k] *= s; + } + + // half-complex to scalar + inline + void inv( Scalar * dst,const Complex * src,int nfft) + { + get_plan(nfft,true,dst,src).inv(ei_fftw_cast(dst), ei_fftw_cast(src),nfft ); + + //TODO move scaling to Eigen::FFT + Scalar s = Scalar(1.)/nfft; + for (int k=0;k<nfft;++k) + dst[k] *= s; + } + + protected: + typedef ei_fftw_plan<Scalar> PlanData; + typedef std::map<int,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; + int key = (nfft<<3 ) | (inverse<<2) | (inplace<<1) | aligned; + return m_plans[key]; + } + }; 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..c068d8765 --- /dev/null +++ b/unsupported/Eigen/src/FFT/ei_kissfft_impl.h @@ -0,0 +1,414 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Mark Borgerding mark a borgerding net +// +// Eigen 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; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen 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 or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see <http://www.gnu.org/licenses/>. + + + + // This FFT implementation was derived from kissfft http:sourceforge.net/projects/kissfft + // Copyright 2003-2009 Mark Borgerding + + template <typename _Scalar> + struct ei_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; + + 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> + 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() - .5*scratch[3].real() , Fout->imag() - .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 = 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 += fstride * k; + if (twidx>=Norig) twidx-=Norig; + t=scratchbuf[q] * twiddles[twidx]; + Fout[ k ] += t; + } + k += m; + } + } + } + }; + + template <typename _Scalar> + struct ei_kissfft_impl + { + typedef _Scalar Scalar; + typedef std::complex<Scalar> Complex; + + void clear() + { + m_plans.clear(); + m_realTwiddles.clear(); + } + + template <typename _Src> + inline + void fwd( Complex * dst,const _Src *src,int nfft) + { + get_plan(nfft,false).work(0, dst, src, 1,1); + } + + // 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 + get_plan(nfft,false).work(0, dst, src, 1,1); + }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); + } + + // place conjugate-symmetric half at the end for completeness + // TODO: make this configurable ( opt-out ) + for ( k=1;k < ncfft ; ++k ) + dst[nfft-k] = conj(dst[k]); + 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); + scale(dst, nfft, Scalar(1)/nfft ); + } + + // half-complex to scalar + inline + void inv( Scalar * dst,const Complex * src,int nfft) + { + if (nfft&3) { + m_tmpBuf.resize(nfft); + inv(&m_tmpBuf[0],src,nfft); + for (int k=0;k<nfft;++k) + dst[k] = m_tmpBuf[k].real(); + }else{ + // optimized version for multiple of 4 + int ncfft = nfft>>1; + int ncfft2 = nfft>>2; + Complex * rtw = real_twiddles(ncfft2); + m_tmpBuf.resize(ncfft); + m_tmpBuf[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_tmpBuf[k] = fek + fok; + m_tmpBuf[ncfft-k] = conj(fek - fok); + } + scale(&m_tmpBuf[0], ncfft, Scalar(1)/nfft ); + get_plan(ncfft,true).work(0, reinterpret_cast<Complex*>(dst), &m_tmpBuf[0], 1,1); + } + } + + protected: + typedef ei_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_tmpBuf; + + inline + int PlanKey(int nfft,bool isinverse) const { return (nfft<<1) | 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 * ((double) (k) / ncfft + .5) ) ); + } + return &twidref[0]; + } + + // TODO move scaling up into Eigen::FFT + inline + void scale(Complex *dst,int n,Scalar s) + { + for (int k=0;k<n;++k) + dst[k] *= s; + } + }; |