![]() |
Eigen
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> 00005 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> 00006 // 00007 // This Source Code Form is subject to the terms of the Mozilla 00008 // Public License v. 2.0. If a copy of the MPL was not distributed 00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00010 00011 #ifndef EIGEN_JACOBI_H 00012 #define EIGEN_JACOBI_H 00013 00014 namespace Eigen { 00015 00034 template<typename Scalar> class JacobiRotation 00035 { 00036 public: 00037 typedef typename NumTraits<Scalar>::Real RealScalar; 00038 00040 JacobiRotation() {} 00041 00043 JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {} 00044 00045 Scalar& c() { return m_c; } 00046 Scalar c() const { return m_c; } 00047 Scalar& s() { return m_s; } 00048 Scalar s() const { return m_s; } 00049 00051 JacobiRotation operator*(const JacobiRotation& other) 00052 { 00053 using numext::conj; 00054 return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s, 00055 conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c))); 00056 } 00057 00059 JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); } 00060 00062 JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); } 00063 00064 template<typename Derived> 00065 bool makeJacobi(const MatrixBase<Derived>&, Index p, Index q); 00066 bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z); 00067 00068 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0); 00069 00070 protected: 00071 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type); 00072 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type); 00073 00074 Scalar m_c, m_s; 00075 }; 00076 00082 template<typename Scalar> 00083 bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z) 00084 { 00085 using std::sqrt; 00086 using std::abs; 00087 typedef typename NumTraits<Scalar>::Real RealScalar; 00088 RealScalar deno = RealScalar(2)*abs(y); 00089 if(deno < (std::numeric_limits<RealScalar>::min)()) 00090 { 00091 m_c = Scalar(1); 00092 m_s = Scalar(0); 00093 return false; 00094 } 00095 else 00096 { 00097 RealScalar tau = (x-z)/deno; 00098 RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1)); 00099 RealScalar t; 00100 if(tau>RealScalar(0)) 00101 { 00102 t = RealScalar(1) / (tau + w); 00103 } 00104 else 00105 { 00106 t = RealScalar(1) / (tau - w); 00107 } 00108 RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1); 00109 RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1)); 00110 m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n; 00111 m_c = n; 00112 return true; 00113 } 00114 } 00115 00125 template<typename Scalar> 00126 template<typename Derived> 00127 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, Index q) 00128 { 00129 return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q))); 00130 } 00131 00148 template<typename Scalar> 00149 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z) 00150 { 00151 makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type()); 00152 } 00153 00154 00155 // specialization for complexes 00156 template<typename Scalar> 00157 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type) 00158 { 00159 using std::sqrt; 00160 using std::abs; 00161 using numext::conj; 00162 00163 if(q==Scalar(0)) 00164 { 00165 m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1); 00166 m_s = 0; 00167 if(r) *r = m_c * p; 00168 } 00169 else if(p==Scalar(0)) 00170 { 00171 m_c = 0; 00172 m_s = -q/abs(q); 00173 if(r) *r = abs(q); 00174 } 00175 else 00176 { 00177 RealScalar p1 = numext::norm1(p); 00178 RealScalar q1 = numext::norm1(q); 00179 if(p1>=q1) 00180 { 00181 Scalar ps = p / p1; 00182 RealScalar p2 = numext::abs2(ps); 00183 Scalar qs = q / p1; 00184 RealScalar q2 = numext::abs2(qs); 00185 00186 RealScalar u = sqrt(RealScalar(1) + q2/p2); 00187 if(numext::real(p)<RealScalar(0)) 00188 u = -u; 00189 00190 m_c = Scalar(1)/u; 00191 m_s = -qs*conj(ps)*(m_c/p2); 00192 if(r) *r = p * u; 00193 } 00194 else 00195 { 00196 Scalar ps = p / q1; 00197 RealScalar p2 = numext::abs2(ps); 00198 Scalar qs = q / q1; 00199 RealScalar q2 = numext::abs2(qs); 00200 00201 RealScalar u = q1 * sqrt(p2 + q2); 00202 if(numext::real(p)<RealScalar(0)) 00203 u = -u; 00204 00205 p1 = abs(p); 00206 ps = p/p1; 00207 m_c = p1/u; 00208 m_s = -conj(ps) * (q/u); 00209 if(r) *r = ps * u; 00210 } 00211 } 00212 } 00213 00214 // specialization for reals 00215 template<typename Scalar> 00216 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type) 00217 { 00218 using std::sqrt; 00219 using std::abs; 00220 if(q==Scalar(0)) 00221 { 00222 m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1); 00223 m_s = Scalar(0); 00224 if(r) *r = abs(p); 00225 } 00226 else if(p==Scalar(0)) 00227 { 00228 m_c = Scalar(0); 00229 m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1); 00230 if(r) *r = abs(q); 00231 } 00232 else if(abs(p) > abs(q)) 00233 { 00234 Scalar t = q/p; 00235 Scalar u = sqrt(Scalar(1) + numext::abs2(t)); 00236 if(p<Scalar(0)) 00237 u = -u; 00238 m_c = Scalar(1)/u; 00239 m_s = -t * m_c; 00240 if(r) *r = p * u; 00241 } 00242 else 00243 { 00244 Scalar t = p/q; 00245 Scalar u = sqrt(Scalar(1) + numext::abs2(t)); 00246 if(q<Scalar(0)) 00247 u = -u; 00248 m_s = -Scalar(1)/u; 00249 m_c = -t * m_s; 00250 if(r) *r = q * u; 00251 } 00252 00253 } 00254 00255 /**************************************************************************************** 00256 * Implementation of MatrixBase methods 00257 ****************************************************************************************/ 00258 00259 namespace internal { 00266 template<typename VectorX, typename VectorY, typename OtherScalar> 00267 void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j); 00268 } 00269 00276 template<typename Derived> 00277 template<typename OtherScalar> 00278 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j) 00279 { 00280 RowXpr x(this->row(p)); 00281 RowXpr y(this->row(q)); 00282 internal::apply_rotation_in_the_plane(x, y, j); 00283 } 00284 00291 template<typename Derived> 00292 template<typename OtherScalar> 00293 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j) 00294 { 00295 ColXpr x(this->col(p)); 00296 ColXpr y(this->col(q)); 00297 internal::apply_rotation_in_the_plane(x, y, j.transpose()); 00298 } 00299 00300 namespace internal { 00301 template<typename VectorX, typename VectorY, typename OtherScalar> 00302 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j) 00303 { 00304 typedef typename VectorX::Scalar Scalar; 00305 enum { PacketSize = packet_traits<Scalar>::size }; 00306 typedef typename packet_traits<Scalar>::type Packet; 00307 eigen_assert(xpr_x.size() == xpr_y.size()); 00308 Index size = xpr_x.size(); 00309 Index incrx = xpr_x.derived().innerStride(); 00310 Index incry = xpr_y.derived().innerStride(); 00311 00312 Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0); 00313 Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0); 00314 00315 OtherScalar c = j.c(); 00316 OtherScalar s = j.s(); 00317 if (c==OtherScalar(1) && s==OtherScalar(0)) 00318 return; 00319 00320 /*** dynamic-size vectorized paths ***/ 00321 00322 if(VectorX::SizeAtCompileTime == Dynamic && 00323 (VectorX::Flags & VectorY::Flags & PacketAccessBit) && 00324 ((incrx==1 && incry==1) || PacketSize == 1)) 00325 { 00326 // both vectors are sequentially stored in memory => vectorization 00327 enum { Peeling = 2 }; 00328 00329 Index alignedStart = internal::first_default_aligned(y, size); 00330 Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize; 00331 00332 const Packet pc = pset1<Packet>(c); 00333 const Packet ps = pset1<Packet>(s); 00334 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj; 00335 00336 for(Index i=0; i<alignedStart; ++i) 00337 { 00338 Scalar xi = x[i]; 00339 Scalar yi = y[i]; 00340 x[i] = c * xi + numext::conj(s) * yi; 00341 y[i] = -s * xi + numext::conj(c) * yi; 00342 } 00343 00344 Scalar* EIGEN_RESTRICT px = x + alignedStart; 00345 Scalar* EIGEN_RESTRICT py = y + alignedStart; 00346 00347 if(internal::first_default_aligned(x, size)==alignedStart) 00348 { 00349 for(Index i=alignedStart; i<alignedEnd; i+=PacketSize) 00350 { 00351 Packet xi = pload<Packet>(px); 00352 Packet yi = pload<Packet>(py); 00353 pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi))); 00354 pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi))); 00355 px += PacketSize; 00356 py += PacketSize; 00357 } 00358 } 00359 else 00360 { 00361 Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize); 00362 for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize) 00363 { 00364 Packet xi = ploadu<Packet>(px); 00365 Packet xi1 = ploadu<Packet>(px+PacketSize); 00366 Packet yi = pload <Packet>(py); 00367 Packet yi1 = pload <Packet>(py+PacketSize); 00368 pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi))); 00369 pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1))); 00370 pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi))); 00371 pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1))); 00372 px += Peeling*PacketSize; 00373 py += Peeling*PacketSize; 00374 } 00375 if(alignedEnd!=peelingEnd) 00376 { 00377 Packet xi = ploadu<Packet>(x+peelingEnd); 00378 Packet yi = pload <Packet>(y+peelingEnd); 00379 pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi))); 00380 pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi))); 00381 } 00382 } 00383 00384 for(Index i=alignedEnd; i<size; ++i) 00385 { 00386 Scalar xi = x[i]; 00387 Scalar yi = y[i]; 00388 x[i] = c * xi + numext::conj(s) * yi; 00389 y[i] = -s * xi + numext::conj(c) * yi; 00390 } 00391 } 00392 00393 /*** fixed-size vectorized path ***/ 00394 else if(VectorX::SizeAtCompileTime != Dynamic && 00395 (VectorX::Flags & VectorY::Flags & PacketAccessBit) && 00396 (EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment)>0)) // FIXME should be compared to the required alignment 00397 { 00398 const Packet pc = pset1<Packet>(c); 00399 const Packet ps = pset1<Packet>(s); 00400 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj; 00401 Scalar* EIGEN_RESTRICT px = x; 00402 Scalar* EIGEN_RESTRICT py = y; 00403 for(Index i=0; i<size; i+=PacketSize) 00404 { 00405 Packet xi = pload<Packet>(px); 00406 Packet yi = pload<Packet>(py); 00407 pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi))); 00408 pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi))); 00409 px += PacketSize; 00410 py += PacketSize; 00411 } 00412 } 00413 00414 /*** non-vectorized path ***/ 00415 else 00416 { 00417 for(Index i=0; i<size; ++i) 00418 { 00419 Scalar xi = *x; 00420 Scalar yi = *y; 00421 *x = c * xi + numext::conj(s) * yi; 00422 *y = -s * xi + numext::conj(c) * yi; 00423 x += incrx; 00424 y += incry; 00425 } 00426 } 00427 } 00428 00429 } // end namespace internal 00430 00431 } // end namespace Eigen 00432 00433 #endif // EIGEN_JACOBI_H