00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_SELFADJOINT_MATRIX_VECTOR_H
00026 #define EIGEN_SELFADJOINT_MATRIX_VECTOR_H
00027
00028 namespace Eigen {
00029
00030 namespace internal {
00031
00032
00033
00034
00035
00036
00037
00038 template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version=Specialized>
00039 struct selfadjoint_matrix_vector_product;
00040
00041 template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version>
00042 struct selfadjoint_matrix_vector_product
00043
00044 {
00045 static EIGEN_DONT_INLINE void run(
00046 Index size,
00047 const Scalar* lhs, Index lhsStride,
00048 const Scalar* _rhs, Index rhsIncr,
00049 Scalar* res,
00050 Scalar alpha)
00051 {
00052 typedef typename packet_traits<Scalar>::type Packet;
00053 typedef typename NumTraits<Scalar>::Real RealScalar;
00054 const Index PacketSize = sizeof(Packet)/sizeof(Scalar);
00055
00056 enum {
00057 IsRowMajor = StorageOrder==RowMajor ? 1 : 0,
00058 IsLower = UpLo == Lower ? 1 : 0,
00059 FirstTriangular = IsRowMajor == IsLower
00060 };
00061
00062 conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, IsRowMajor), ConjugateRhs> cj0;
00063 conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> cj1;
00064 conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex, ConjugateRhs> cjd;
00065
00066 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, IsRowMajor), ConjugateRhs> pcj0;
00067 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> pcj1;
00068
00069 Scalar cjAlpha = ConjugateRhs ? conj(alpha) : alpha;
00070
00071
00072
00073
00074 ei_declare_aligned_stack_constructed_variable(Scalar,rhs,size,rhsIncr==1 ? const_cast<Scalar*>(_rhs) : 0);
00075 if (rhsIncr!=1)
00076 {
00077 const Scalar* it = _rhs;
00078 for (Index i=0; i<size; ++i, it+=rhsIncr)
00079 rhs[i] = *it;
00080 }
00081
00082 Index bound = (std::max)(Index(0),size-8) & 0xfffffffe;
00083 if (FirstTriangular)
00084 bound = size - bound;
00085
00086 for (Index j=FirstTriangular ? bound : 0;
00087 j<(FirstTriangular ? size : bound);j+=2)
00088 {
00089 register const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
00090 register const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride;
00091
00092 Scalar t0 = cjAlpha * rhs[j];
00093 Packet ptmp0 = pset1<Packet>(t0);
00094 Scalar t1 = cjAlpha * rhs[j+1];
00095 Packet ptmp1 = pset1<Packet>(t1);
00096
00097 Scalar t2(0);
00098 Packet ptmp2 = pset1<Packet>(t2);
00099 Scalar t3(0);
00100 Packet ptmp3 = pset1<Packet>(t3);
00101
00102 size_t starti = FirstTriangular ? 0 : j+2;
00103 size_t endi = FirstTriangular ? j : size;
00104 size_t alignedStart = (starti) + internal::first_aligned(&res[starti], endi-starti);
00105 size_t alignedEnd = alignedStart + ((endi-alignedStart)/(PacketSize))*(PacketSize);
00106
00107
00108 res[j] += cjd.pmul(internal::real(A0[j]), t0);
00109 res[j+1] += cjd.pmul(internal::real(A1[j+1]), t1);
00110 if(FirstTriangular)
00111 {
00112 res[j] += cj0.pmul(A1[j], t1);
00113 t3 += cj1.pmul(A1[j], rhs[j]);
00114 }
00115 else
00116 {
00117 res[j+1] += cj0.pmul(A0[j+1],t0);
00118 t2 += cj1.pmul(A0[j+1], rhs[j+1]);
00119 }
00120
00121 for (size_t i=starti; i<alignedStart; ++i)
00122 {
00123 res[i] += t0 * A0[i] + t1 * A1[i];
00124 t2 += conj(A0[i]) * rhs[i];
00125 t3 += conj(A1[i]) * rhs[i];
00126 }
00127
00128
00129 const Scalar* EIGEN_RESTRICT a0It = A0 + alignedStart;
00130 const Scalar* EIGEN_RESTRICT a1It = A1 + alignedStart;
00131 const Scalar* EIGEN_RESTRICT rhsIt = rhs + alignedStart;
00132 Scalar* EIGEN_RESTRICT resIt = res + alignedStart;
00133 for (size_t i=alignedStart; i<alignedEnd; i+=PacketSize)
00134 {
00135 Packet A0i = ploadu<Packet>(a0It); a0It += PacketSize;
00136 Packet A1i = ploadu<Packet>(a1It); a1It += PacketSize;
00137 Packet Bi = ploadu<Packet>(rhsIt); rhsIt += PacketSize;
00138 Packet Xi = pload <Packet>(resIt);
00139
00140 Xi = pcj0.pmadd(A0i,ptmp0, pcj0.pmadd(A1i,ptmp1,Xi));
00141 ptmp2 = pcj1.pmadd(A0i, Bi, ptmp2);
00142 ptmp3 = pcj1.pmadd(A1i, Bi, ptmp3);
00143 pstore(resIt,Xi); resIt += PacketSize;
00144 }
00145 for (size_t i=alignedEnd; i<endi; i++)
00146 {
00147 res[i] += cj0.pmul(A0[i], t0) + cj0.pmul(A1[i],t1);
00148 t2 += cj1.pmul(A0[i], rhs[i]);
00149 t3 += cj1.pmul(A1[i], rhs[i]);
00150 }
00151
00152 res[j] += alpha * (t2 + predux(ptmp2));
00153 res[j+1] += alpha * (t3 + predux(ptmp3));
00154 }
00155 for (Index j=FirstTriangular ? 0 : bound;j<(FirstTriangular ? bound : size);j++)
00156 {
00157 register const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
00158
00159 Scalar t1 = cjAlpha * rhs[j];
00160 Scalar t2(0);
00161
00162 res[j] += cjd.pmul(internal::real(A0[j]), t1);
00163 for (Index i=FirstTriangular ? 0 : j+1; i<(FirstTriangular ? j : size); i++)
00164 {
00165 res[i] += cj0.pmul(A0[i], t1);
00166 t2 += cj1.pmul(A0[i], rhs[i]);
00167 }
00168 res[j] += alpha * t2;
00169 }
00170 }
00171 };
00172
00173 }
00174
00175
00176
00177
00178
00179 namespace internal {
00180 template<typename Lhs, int LhsMode, typename Rhs>
00181 struct traits<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true> >
00182 : traits<ProductBase<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>, Lhs, Rhs> >
00183 {};
00184 }
00185
00186 template<typename Lhs, int LhsMode, typename Rhs>
00187 struct SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>
00188 : public ProductBase<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>, Lhs, Rhs >
00189 {
00190 EIGEN_PRODUCT_PUBLIC_INTERFACE(SelfadjointProductMatrix)
00191
00192 enum {
00193 LhsUpLo = LhsMode&(Upper|Lower)
00194 };
00195
00196 SelfadjointProductMatrix(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {}
00197
00198 template<typename Dest> void scaleAndAddTo(Dest& dest, Scalar alpha) const
00199 {
00200 typedef typename Dest::Scalar ResScalar;
00201 typedef typename Base::RhsScalar RhsScalar;
00202 typedef Map<Matrix<ResScalar,Dynamic,1>, Aligned> MappedDest;
00203
00204 eigen_assert(dest.rows()==m_lhs.rows() && dest.cols()==m_rhs.cols());
00205
00206 typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(m_lhs);
00207 typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(m_rhs);
00208
00209 Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(m_lhs)
00210 * RhsBlasTraits::extractScalarFactor(m_rhs);
00211
00212 enum {
00213 EvalToDest = (Dest::InnerStrideAtCompileTime==1),
00214 UseRhs = (_ActualRhsType::InnerStrideAtCompileTime==1)
00215 };
00216
00217 internal::gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,!EvalToDest> static_dest;
00218 internal::gemv_static_vector_if<RhsScalar,_ActualRhsType::SizeAtCompileTime,_ActualRhsType::MaxSizeAtCompileTime,!UseRhs> static_rhs;
00219
00220 ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(),
00221 EvalToDest ? dest.data() : static_dest.data());
00222
00223 ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,rhs.size(),
00224 UseRhs ? const_cast<RhsScalar*>(rhs.data()) : static_rhs.data());
00225
00226 if(!EvalToDest)
00227 {
00228 #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00229 int size = dest.size();
00230 EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00231 #endif
00232 MappedDest(actualDestPtr, dest.size()) = dest;
00233 }
00234
00235 if(!UseRhs)
00236 {
00237 #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00238 int size = rhs.size();
00239 EIGEN_DENSE_STORAGE_CTOR_PLUGIN
00240 #endif
00241 Map<typename _ActualRhsType::PlainObject>(actualRhsPtr, rhs.size()) = rhs;
00242 }
00243
00244
00245 internal::selfadjoint_matrix_vector_product<Scalar, Index, (internal::traits<_ActualLhsType>::Flags&RowMajorBit) ? RowMajor : ColMajor, int(LhsUpLo), bool(LhsBlasTraits::NeedToConjugate), bool(RhsBlasTraits::NeedToConjugate)>::run
00246 (
00247 lhs.rows(),
00248 &lhs.coeffRef(0,0), lhs.outerStride(),
00249 actualRhsPtr, 1,
00250 actualDestPtr,
00251 actualAlpha
00252 );
00253
00254 if(!EvalToDest)
00255 dest = MappedDest(actualDestPtr, dest.size());
00256 }
00257 };
00258
00259 namespace internal {
00260 template<typename Lhs, typename Rhs, int RhsMode>
00261 struct traits<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false> >
00262 : traits<ProductBase<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>, Lhs, Rhs> >
00263 {};
00264 }
00265
00266 template<typename Lhs, typename Rhs, int RhsMode>
00267 struct SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>
00268 : public ProductBase<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>, Lhs, Rhs >
00269 {
00270 EIGEN_PRODUCT_PUBLIC_INTERFACE(SelfadjointProductMatrix)
00271
00272 enum {
00273 RhsUpLo = RhsMode&(Upper|Lower)
00274 };
00275
00276 SelfadjointProductMatrix(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {}
00277
00278 template<typename Dest> void scaleAndAddTo(Dest& dest, Scalar alpha) const
00279 {
00280
00281 Transpose<Dest> destT(dest);
00282 SelfadjointProductMatrix<Transpose<const Rhs>, int(RhsUpLo)==Upper ? Lower : Upper, false,
00283 Transpose<const Lhs>, 0, true>(m_rhs.transpose(), m_lhs.transpose()).scaleAndAddTo(destT, alpha);
00284 }
00285 };
00286
00287 }
00288
00289 #endif // EIGEN_SELFADJOINT_MATRIX_VECTOR_H