TriangularMatrixVector.h

00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_TRIANGULARMATRIXVECTOR_H
00011 #define EIGEN_TRIANGULARMATRIXVECTOR_H
00012 
00013 namespace Eigen { 
00014 
00015 namespace internal {
00016 
00017 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int StorageOrder, int Version=Specialized>
00018 struct triangular_matrix_vector_product;
00019 
00020 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
00021 struct triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,ColMajor,Version>
00022 {
00023   typedef typename scalar_product_traits<LhsScalar, RhsScalar>::ReturnType ResScalar;
00024   enum {
00025     IsLower = ((Mode&Lower)==Lower),
00026     HasUnitDiag = (Mode & UnitDiag)==UnitDiag,
00027     HasZeroDiag = (Mode & ZeroDiag)==ZeroDiag
00028   };
00029   static EIGEN_DONT_INLINE  void run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
00030                                      const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha);
00031 };
00032 
00033 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs, int Version>
00034 EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,ColMajor,Version>
00035   ::run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
00036         const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha)
00037   {
00038     static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
00039     Index size = (std::min)(_rows,_cols);
00040     Index rows = IsLower ? _rows : (std::min)(_rows,_cols);
00041     Index cols = IsLower ? (std::min)(_rows,_cols) : _cols;
00042 
00043     typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > LhsMap;
00044     const LhsMap lhs(_lhs,rows,cols,OuterStride<>(lhsStride));
00045     typename conj_expr_if<ConjLhs,LhsMap>::type cjLhs(lhs);
00046     
00047     typedef Map<const Matrix<RhsScalar,Dynamic,1>, 0, InnerStride<> > RhsMap;
00048     const RhsMap rhs(_rhs,cols,InnerStride<>(rhsIncr));
00049     typename conj_expr_if<ConjRhs,RhsMap>::type cjRhs(rhs);
00050 
00051     typedef Map<Matrix<ResScalar,Dynamic,1> > ResMap;
00052     ResMap res(_res,rows);
00053 
00054     for (Index pi=0; pi<size; pi+=PanelWidth)
00055     {
00056       Index actualPanelWidth = (std::min)(PanelWidth, size-pi);
00057       for (Index k=0; k<actualPanelWidth; ++k)
00058       {
00059         Index i = pi + k;
00060         Index s = IsLower ? ((HasUnitDiag||HasZeroDiag) ? i+1 : i ) : pi;
00061         Index r = IsLower ? actualPanelWidth-k : k+1;
00062         if ((!(HasUnitDiag||HasZeroDiag)) || (--r)>0)
00063           res.segment(s,r) += (alpha * cjRhs.coeff(i)) * cjLhs.col(i).segment(s,r);
00064         if (HasUnitDiag)
00065           res.coeffRef(i) += alpha * cjRhs.coeff(i);
00066       }
00067       Index r = IsLower ? rows - pi - actualPanelWidth : pi;
00068       if (r>0)
00069       {
00070         Index s = IsLower ? pi+actualPanelWidth : 0;
00071         general_matrix_vector_product<Index,LhsScalar,ColMajor,ConjLhs,RhsScalar,ConjRhs,BuiltIn>::run(
00072             r, actualPanelWidth,
00073             &lhs.coeffRef(s,pi), lhsStride,
00074             &rhs.coeffRef(pi), rhsIncr,
00075             &res.coeffRef(s), resIncr, alpha);
00076       }
00077     }
00078     if((!IsLower) && cols>size)
00079     {
00080       general_matrix_vector_product<Index,LhsScalar,ColMajor,ConjLhs,RhsScalar,ConjRhs>::run(
00081           rows, cols-size,
00082           &lhs.coeffRef(0,size), lhsStride,
00083           &rhs.coeffRef(size), rhsIncr,
00084           _res, resIncr, alpha);
00085     }
00086   }
00087 
00088 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs,int Version>
00089 struct triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,RowMajor,Version>
00090 {
00091   typedef typename scalar_product_traits<LhsScalar, RhsScalar>::ReturnType ResScalar;
00092   enum {
00093     IsLower = ((Mode&Lower)==Lower),
00094     HasUnitDiag = (Mode & UnitDiag)==UnitDiag,
00095     HasZeroDiag = (Mode & ZeroDiag)==ZeroDiag
00096   };
00097   static EIGEN_DONT_INLINE void run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
00098                                     const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha);
00099 };
00100 
00101 template<typename Index, int Mode, typename LhsScalar, bool ConjLhs, typename RhsScalar, bool ConjRhs,int Version>
00102 EIGEN_DONT_INLINE void triangular_matrix_vector_product<Index,Mode,LhsScalar,ConjLhs,RhsScalar,ConjRhs,RowMajor,Version>
00103   ::run(Index _rows, Index _cols, const LhsScalar* _lhs, Index lhsStride,
00104         const RhsScalar* _rhs, Index rhsIncr, ResScalar* _res, Index resIncr, const ResScalar& alpha)
00105   {
00106     static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
00107     Index diagSize = (std::min)(_rows,_cols);
00108     Index rows = IsLower ? _rows : diagSize;
00109     Index cols = IsLower ? diagSize : _cols;
00110 
00111     typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,RowMajor>, 0, OuterStride<> > LhsMap;
00112     const LhsMap lhs(_lhs,rows,cols,OuterStride<>(lhsStride));
00113     typename conj_expr_if<ConjLhs,LhsMap>::type cjLhs(lhs);
00114 
00115     typedef Map<const Matrix<RhsScalar,Dynamic,1> > RhsMap;
00116     const RhsMap rhs(_rhs,cols);
00117     typename conj_expr_if<ConjRhs,RhsMap>::type cjRhs(rhs);
00118 
00119     typedef Map<Matrix<ResScalar,Dynamic,1>, 0, InnerStride<> > ResMap;
00120     ResMap res(_res,rows,InnerStride<>(resIncr));
00121     
00122     for (Index pi=0; pi<diagSize; pi+=PanelWidth)
00123     {
00124       Index actualPanelWidth = (std::min)(PanelWidth, diagSize-pi);
00125       for (Index k=0; k<actualPanelWidth; ++k)
00126       {
00127         Index i = pi + k;
00128         Index s = IsLower ? pi  : ((HasUnitDiag||HasZeroDiag) ? i+1 : i);
00129         Index r = IsLower ? k+1 : actualPanelWidth-k;
00130         if ((!(HasUnitDiag||HasZeroDiag)) || (--r)>0)
00131           res.coeffRef(i) += alpha * (cjLhs.row(i).segment(s,r).cwiseProduct(cjRhs.segment(s,r).transpose())).sum();
00132         if (HasUnitDiag)
00133           res.coeffRef(i) += alpha * cjRhs.coeff(i);
00134       }
00135       Index r = IsLower ? pi : cols - pi - actualPanelWidth;
00136       if (r>0)
00137       {
00138         Index s = IsLower ? 0 : pi + actualPanelWidth;
00139         general_matrix_vector_product<Index,LhsScalar,RowMajor,ConjLhs,RhsScalar,ConjRhs,BuiltIn>::run(
00140             actualPanelWidth, r,
00141             &lhs.coeffRef(pi,s), lhsStride,
00142             &rhs.coeffRef(s), rhsIncr,
00143             &res.coeffRef(pi), resIncr, alpha);
00144       }
00145     }
00146     if(IsLower && rows>diagSize)
00147     {
00148       general_matrix_vector_product<Index,LhsScalar,RowMajor,ConjLhs,RhsScalar,ConjRhs>::run(
00149             rows-diagSize, cols,
00150             &lhs.coeffRef(diagSize,0), lhsStride,
00151             &rhs.coeffRef(0), rhsIncr,
00152             &res.coeffRef(diagSize), resIncr, alpha);
00153     }
00154   }
00155 
00156 /***************************************************************************
00157 * Wrapper to product_triangular_vector
00158 ***************************************************************************/
00159 
00160 template<int Mode, bool LhsIsTriangular, typename Lhs, typename Rhs>
00161 struct traits<TriangularProduct<Mode,LhsIsTriangular,Lhs,false,Rhs,true> >
00162  : traits<ProductBase<TriangularProduct<Mode,LhsIsTriangular,Lhs,false,Rhs,true>, Lhs, Rhs> >
00163 {};
00164 
00165 template<int Mode, bool LhsIsTriangular, typename Lhs, typename Rhs>
00166 struct traits<TriangularProduct<Mode,LhsIsTriangular,Lhs,true,Rhs,false> >
00167  : traits<ProductBase<TriangularProduct<Mode,LhsIsTriangular,Lhs,true,Rhs,false>, Lhs, Rhs> >
00168 {};
00169 
00170 
00171 template<int StorageOrder>
00172 struct trmv_selector;
00173 
00174 } // end namespace internal
00175 
00176 template<int Mode, typename Lhs, typename Rhs>
00177 struct TriangularProduct<Mode,true,Lhs,false,Rhs,true>
00178   : public ProductBase<TriangularProduct<Mode,true,Lhs,false,Rhs,true>, Lhs, Rhs >
00179 {
00180   EIGEN_PRODUCT_PUBLIC_INTERFACE(TriangularProduct)
00181 
00182   TriangularProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {}
00183 
00184   template<typename Dest> void scaleAndAddTo(Dest& dst, const Scalar& alpha) const
00185   {
00186     eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols());
00187   
00188     internal::trmv_selector<(int(internal::traits<Lhs>::Flags)&RowMajorBit) ? RowMajor : ColMajor>::run(*this, dst, alpha);
00189   }
00190 };
00191 
00192 template<int Mode, typename Lhs, typename Rhs>
00193 struct TriangularProduct<Mode,false,Lhs,true,Rhs,false>
00194   : public ProductBase<TriangularProduct<Mode,false,Lhs,true,Rhs,false>, Lhs, Rhs >
00195 {
00196   EIGEN_PRODUCT_PUBLIC_INTERFACE(TriangularProduct)
00197 
00198   TriangularProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {}
00199 
00200   template<typename Dest> void scaleAndAddTo(Dest& dst, const Scalar& alpha) const
00201   {
00202     eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols());
00203 
00204     typedef TriangularProduct<(Mode & (UnitDiag|ZeroDiag)) | ((Mode & Lower) ? Upper : Lower),true,Transpose<const Rhs>,false,Transpose<const Lhs>,true> TriangularProductTranspose;
00205     Transpose<Dest> dstT(dst);
00206     internal::trmv_selector<(int(internal::traits<Rhs>::Flags)&RowMajorBit) ? ColMajor : RowMajor>::run(
00207       TriangularProductTranspose(m_rhs.transpose(),m_lhs.transpose()), dstT, alpha);
00208   }
00209 };
00210 
00211 namespace internal {
00212 
00213 // TODO: find a way to factorize this piece of code with gemv_selector since the logic is exactly the same.
00214   
00215 template<> struct trmv_selector<ColMajor>
00216 {
00217   template<int Mode, typename Lhs, typename Rhs, typename Dest>
00218   static void run(const TriangularProduct<Mode,true,Lhs,false,Rhs,true>& prod, Dest& dest, const typename TriangularProduct<Mode,true,Lhs,false,Rhs,true>::Scalar& alpha)
00219   {
00220     typedef TriangularProduct<Mode,true,Lhs,false,Rhs,true> ProductType;
00221     typedef typename ProductType::Index Index;
00222     typedef typename ProductType::LhsScalar   LhsScalar;
00223     typedef typename ProductType::RhsScalar   RhsScalar;
00224     typedef typename ProductType::Scalar      ResScalar;
00225     typedef typename ProductType::RealScalar  RealScalar;
00226     typedef typename ProductType::ActualLhsType ActualLhsType;
00227     typedef typename ProductType::ActualRhsType ActualRhsType;
00228     typedef typename ProductType::LhsBlasTraits LhsBlasTraits;
00229     typedef typename ProductType::RhsBlasTraits RhsBlasTraits;
00230     typedef Map<Matrix<ResScalar,Dynamic,1>, Aligned> MappedDest;
00231 
00232     typename internal::add_const_on_value_type<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(prod.lhs());
00233     typename internal::add_const_on_value_type<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(prod.rhs());
00234 
00235     ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs())
00236                                   * RhsBlasTraits::extractScalarFactor(prod.rhs());
00237 
00238     enum {
00239       // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
00240       // on, the other hand it is good for the cache to pack the vector anyways...
00241       EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime==1,
00242       ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex),
00243       MightCannotUseDest = (Dest::InnerStrideAtCompileTime!=1) || ComplexByReal
00244     };
00245 
00246     gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
00247 
00248     bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0));
00249     bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible;
00250     
00251     RhsScalar compatibleAlpha = get_factor<ResScalar,RhsScalar>::run(actualAlpha);
00252 
00253     ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(),
00254                                                   evalToDest ? dest.data() : static_dest.data());
00255 
00256     if(!evalToDest)
00257     {
00258       #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00259       Index size = dest.size();
00260       EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00261       #endif
00262       if(!alphaIsCompatible)
00263       {
00264         MappedDest(actualDestPtr, dest.size()).setZero();
00265         compatibleAlpha = RhsScalar(1);
00266       }
00267       else
00268         MappedDest(actualDestPtr, dest.size()) = dest;
00269     }
00270     
00271     internal::triangular_matrix_vector_product
00272       <Index,Mode,
00273        LhsScalar, LhsBlasTraits::NeedToConjugate,
00274        RhsScalar, RhsBlasTraits::NeedToConjugate,
00275        ColMajor>
00276       ::run(actualLhs.rows(),actualLhs.cols(),
00277             actualLhs.data(),actualLhs.outerStride(),
00278             actualRhs.data(),actualRhs.innerStride(),
00279             actualDestPtr,1,compatibleAlpha);
00280 
00281     if (!evalToDest)
00282     {
00283       if(!alphaIsCompatible)
00284         dest += actualAlpha * MappedDest(actualDestPtr, dest.size());
00285       else
00286         dest = MappedDest(actualDestPtr, dest.size());
00287     }
00288   }
00289 };
00290 
00291 template<> struct trmv_selector<RowMajor>
00292 {
00293   template<int Mode, typename Lhs, typename Rhs, typename Dest>
00294   static void run(const TriangularProduct<Mode,true,Lhs,false,Rhs,true>& prod, Dest& dest, const typename TriangularProduct<Mode,true,Lhs,false,Rhs,true>::Scalar& alpha)
00295   {
00296     typedef TriangularProduct<Mode,true,Lhs,false,Rhs,true> ProductType;
00297     typedef typename ProductType::LhsScalar LhsScalar;
00298     typedef typename ProductType::RhsScalar RhsScalar;
00299     typedef typename ProductType::Scalar    ResScalar;
00300     typedef typename ProductType::Index Index;
00301     typedef typename ProductType::ActualLhsType ActualLhsType;
00302     typedef typename ProductType::ActualRhsType ActualRhsType;
00303     typedef typename ProductType::_ActualRhsType _ActualRhsType;
00304     typedef typename ProductType::LhsBlasTraits LhsBlasTraits;
00305     typedef typename ProductType::RhsBlasTraits RhsBlasTraits;
00306 
00307     typename add_const<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(prod.lhs());
00308     typename add_const<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(prod.rhs());
00309 
00310     ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs())
00311                                   * RhsBlasTraits::extractScalarFactor(prod.rhs());
00312 
00313     enum {
00314       DirectlyUseRhs = _ActualRhsType::InnerStrideAtCompileTime==1
00315     };
00316 
00317     gemv_static_vector_if<RhsScalar,_ActualRhsType::SizeAtCompileTime,_ActualRhsType::MaxSizeAtCompileTime,!DirectlyUseRhs> static_rhs;
00318 
00319     ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,actualRhs.size(),
00320         DirectlyUseRhs ? const_cast<RhsScalar*>(actualRhs.data()) : static_rhs.data());
00321 
00322     if(!DirectlyUseRhs)
00323     {
00324       #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00325       int size = actualRhs.size();
00326       EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00327       #endif
00328       Map<typename _ActualRhsType::PlainObject>(actualRhsPtr, actualRhs.size()) = actualRhs;
00329     }
00330     
00331     internal::triangular_matrix_vector_product
00332       <Index,Mode,
00333        LhsScalar, LhsBlasTraits::NeedToConjugate,
00334        RhsScalar, RhsBlasTraits::NeedToConjugate,
00335        RowMajor>
00336       ::run(actualLhs.rows(),actualLhs.cols(),
00337             actualLhs.data(),actualLhs.outerStride(),
00338             actualRhsPtr,1,
00339             dest.data(),dest.innerStride(),
00340             actualAlpha);
00341   }
00342 };
00343 
00344 } // end namespace internal
00345 
00346 } // end namespace Eigen
00347 
00348 #endif // EIGEN_TRIANGULARMATRIXVECTOR_H