Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
TriangularSolverVector.h
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
Generated on Tue Jul 12 2022 17:47:01 by 1.7.2