TriangularSolverVector.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2010 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_TRIANGULAR_SOLVER_VECTOR_H
00011 #define EIGEN_TRIANGULAR_SOLVER_VECTOR_H
00012 
00013 namespace Eigen { 
00014 
00015 namespace internal {
00016 
00017 template<typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate, int StorageOrder>
00018 struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheRight, Mode, Conjugate, StorageOrder>
00019 {
00020   static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs)
00021   {
00022     triangular_solve_vector<LhsScalar,RhsScalar,Index,OnTheLeft,
00023         ((Mode&Upper)==Upper ? Lower : Upper) | (Mode&UnitDiag),
00024         Conjugate,StorageOrder==RowMajor?ColMajor:RowMajor
00025       >::run(size, _lhs, lhsStride, rhs);
00026   }
00027 };
00028     
00029 // forward and backward substitution, row-major, rhs is a vector
00030 template<typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate>
00031 struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheLeft, Mode, Conjugate, RowMajor>
00032 {
00033   enum {
00034     IsLower = ((Mode&Lower)==Lower)
00035   };
00036   static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs)
00037   {
00038     typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,RowMajor>, 0, OuterStride<> > LhsMap;
00039     const LhsMap lhs(_lhs,size,size,OuterStride<>(lhsStride));
00040     typename internal::conditional<
00041                           Conjugate,
00042                           const CwiseUnaryOp<typename internal::scalar_conjugate_op<LhsScalar>,LhsMap>,
00043                           const LhsMap&>
00044                         ::type cjLhs(lhs);
00045     static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
00046     for(Index pi=IsLower ? 0 : size;
00047         IsLower ? pi<size : pi>0;
00048         IsLower ? pi+=PanelWidth : pi-=PanelWidth)
00049     {
00050       Index actualPanelWidth = (std::min)(IsLower ? size - pi : pi, PanelWidth);
00051 
00052       Index r = IsLower ? pi : size - pi; // remaining size
00053       if (r > 0)
00054       {
00055         // let's directly call the low level product function because:
00056         // 1 - it is faster to compile
00057         // 2 - it is slighlty faster at runtime
00058         Index startRow = IsLower ? pi : pi-actualPanelWidth;
00059         Index startCol = IsLower ? 0 : pi;
00060 
00061         general_matrix_vector_product<Index,LhsScalar,RowMajor,Conjugate,RhsScalar,false>::run(
00062           actualPanelWidth, r,
00063           &lhs.coeffRef(startRow,startCol), lhsStride,
00064           rhs + startCol, 1,
00065           rhs + startRow, 1,
00066           RhsScalar(-1));
00067       }
00068 
00069       for(Index k=0; k<actualPanelWidth; ++k)
00070       {
00071         Index i = IsLower ? pi+k : pi-k-1;
00072         Index s = IsLower ? pi   : i+1;
00073         if (k>0)
00074           rhs[i] -= (cjLhs.row(i).segment(s,k).transpose().cwiseProduct(Map<const Matrix<RhsScalar,Dynamic,1> >(rhs+s,k))).sum();
00075         
00076         if(!(Mode & UnitDiag))
00077           rhs[i] /= cjLhs(i,i);
00078       }
00079     }
00080   }
00081 };
00082 
00083 // forward and backward substitution, column-major, rhs is a vector
00084 template<typename LhsScalar, typename RhsScalar, typename Index, int Mode, bool Conjugate>
00085 struct triangular_solve_vector<LhsScalar, RhsScalar, Index, OnTheLeft, Mode, Conjugate, ColMajor>
00086 {
00087   enum {
00088     IsLower = ((Mode&Lower)==Lower)
00089   };
00090   static void run(Index size, const LhsScalar* _lhs, Index lhsStride, RhsScalar* rhs)
00091   {
00092     typedef Map<const Matrix<LhsScalar,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > LhsMap;
00093     const LhsMap lhs(_lhs,size,size,OuterStride<>(lhsStride));
00094     typename internal::conditional<Conjugate,
00095                                    const CwiseUnaryOp<typename internal::scalar_conjugate_op<LhsScalar>,LhsMap>,
00096                                    const LhsMap&
00097                                   >::type cjLhs(lhs);
00098     static const Index PanelWidth = EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH;
00099 
00100     for(Index pi=IsLower ? 0 : size;
00101         IsLower ? pi<size : pi>0;
00102         IsLower ? pi+=PanelWidth : pi-=PanelWidth)
00103     {
00104       Index actualPanelWidth = (std::min)(IsLower ? size - pi : pi, PanelWidth);
00105       Index startBlock = IsLower ? pi : pi-actualPanelWidth;
00106       Index endBlock = IsLower ? pi + actualPanelWidth : 0;
00107 
00108       for(Index k=0; k<actualPanelWidth; ++k)
00109       {
00110         Index i = IsLower ? pi+k : pi-k-1;
00111         if(!(Mode & UnitDiag))
00112           rhs[i] /= cjLhs.coeff(i,i);
00113 
00114         Index r = actualPanelWidth - k - 1; // remaining size
00115         Index s = IsLower ? i+1 : i-r;
00116         if (r>0)
00117           Map<Matrix<RhsScalar,Dynamic,1> >(rhs+s,r) -= rhs[i] * cjLhs.col(i).segment(s,r);
00118       }
00119       Index r = IsLower ? size - endBlock : startBlock; // remaining size
00120       if (r > 0)
00121       {
00122         // let's directly call the low level product function because:
00123         // 1 - it is faster to compile
00124         // 2 - it is slighlty faster at runtime
00125         general_matrix_vector_product<Index,LhsScalar,ColMajor,Conjugate,RhsScalar,false>::run(
00126             r, actualPanelWidth,
00127             &lhs.coeffRef(endBlock,startBlock), lhsStride,
00128             rhs+startBlock, 1,
00129             rhs+endBlock, 1, RhsScalar(-1));
00130       }
00131     }
00132   }
00133 };
00134 
00135 } // end namespace internal
00136 
00137 } // end namespace Eigen
00138 
00139 #endif // EIGEN_TRIANGULAR_SOLVER_VECTOR_H


turtlebot_exploration_3d
Author(s): Bona , Shawn
autogenerated on Thu Jun 6 2019 21:00:26