Eigen  3.3.3
Jacobi.h
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
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends