Jacobi.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) 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       return JacobiRotation(m_c * other.m_c - internal::conj(m_s) * other.m_s,
00054                             internal::conj(m_c * internal::conj(other.m_s) + internal::conj(m_s) * internal::conj(other.m_c)));
00055     }
00056 
00058     JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); }
00059 
00061     JacobiRotation adjoint() const { return JacobiRotation(internal::conj(m_c), -m_s); }
00062 
00063     template<typename Derived>
00064     bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
00065     bool makeJacobi(RealScalar x, Scalar y, RealScalar z);
00066 
00067     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
00068 
00069   protected:
00070     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
00071     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
00072 
00073     Scalar m_c, m_s;
00074 };
00075 
00081 template<typename Scalar>
00082 bool JacobiRotation<Scalar>::makeJacobi(RealScalar x, Scalar y, RealScalar z)
00083 {
00084   typedef typename NumTraits<Scalar>::Real RealScalar;
00085   if(y == Scalar(0))
00086   {
00087     m_c = Scalar(1);
00088     m_s = Scalar(0);
00089     return false;
00090   }
00091   else
00092   {
00093     RealScalar tau = (x-z)/(RealScalar(2)*internal::abs(y));
00094     RealScalar w = internal::sqrt(internal::abs2(tau) + RealScalar(1));
00095     RealScalar t;
00096     if(tau>RealScalar(0))
00097     {
00098       t = RealScalar(1) / (tau + w);
00099     }
00100     else
00101     {
00102       t = RealScalar(1) / (tau - w);
00103     }
00104     RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
00105     RealScalar n = RealScalar(1) / internal::sqrt(internal::abs2(t)+RealScalar(1));
00106     m_s = - sign_t * (internal::conj(y) / internal::abs(y)) * internal::abs(t) * n;
00107     m_c = n;
00108     return true;
00109   }
00110 }
00111 
00121 template<typename Scalar>
00122 template<typename Derived>
00123 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
00124 {
00125   return makeJacobi(internal::real(m.coeff(p,p)), m.coeff(p,q), internal::real(m.coeff(q,q)));
00126 }
00127 
00144 template<typename Scalar>
00145 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
00146 {
00147   makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
00148 }
00149 
00150 
00151 // specialization for complexes
00152 template<typename Scalar>
00153 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
00154 {
00155   if(q==Scalar(0))
00156   {
00157     m_c = internal::real(p)<0 ? Scalar(-1) : Scalar(1);
00158     m_s = 0;
00159     if(r) *r = m_c * p;
00160   }
00161   else if(p==Scalar(0))
00162   {
00163     m_c = 0;
00164     m_s = -q/internal::abs(q);
00165     if(r) *r = internal::abs(q);
00166   }
00167   else
00168   {
00169     RealScalar p1 = internal::norm1(p);
00170     RealScalar q1 = internal::norm1(q);
00171     if(p1>=q1)
00172     {
00173       Scalar ps = p / p1;
00174       RealScalar p2 = internal::abs2(ps);
00175       Scalar qs = q / p1;
00176       RealScalar q2 = internal::abs2(qs);
00177 
00178       RealScalar u = internal::sqrt(RealScalar(1) + q2/p2);
00179       if(internal::real(p)<RealScalar(0))
00180         u = -u;
00181 
00182       m_c = Scalar(1)/u;
00183       m_s = -qs*internal::conj(ps)*(m_c/p2);
00184       if(r) *r = p * u;
00185     }
00186     else
00187     {
00188       Scalar ps = p / q1;
00189       RealScalar p2 = internal::abs2(ps);
00190       Scalar qs = q / q1;
00191       RealScalar q2 = internal::abs2(qs);
00192 
00193       RealScalar u = q1 * internal::sqrt(p2 + q2);
00194       if(internal::real(p)<RealScalar(0))
00195         u = -u;
00196 
00197       p1 = internal::abs(p);
00198       ps = p/p1;
00199       m_c = p1/u;
00200       m_s = -internal::conj(ps) * (q/u);
00201       if(r) *r = ps * u;
00202     }
00203   }
00204 }
00205 
00206 // specialization for reals
00207 template<typename Scalar>
00208 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
00209 {
00210 
00211   if(q==Scalar(0))
00212   {
00213     m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
00214     m_s = Scalar(0);
00215     if(r) *r = internal::abs(p);
00216   }
00217   else if(p==Scalar(0))
00218   {
00219     m_c = Scalar(0);
00220     m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
00221     if(r) *r = internal::abs(q);
00222   }
00223   else if(internal::abs(p) > internal::abs(q))
00224   {
00225     Scalar t = q/p;
00226     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00227     if(p<Scalar(0))
00228       u = -u;
00229     m_c = Scalar(1)/u;
00230     m_s = -t * m_c;
00231     if(r) *r = p * u;
00232   }
00233   else
00234   {
00235     Scalar t = p/q;
00236     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00237     if(q<Scalar(0))
00238       u = -u;
00239     m_s = -Scalar(1)/u;
00240     m_c = -t * m_s;
00241     if(r) *r = q * u;
00242   }
00243 
00244 }
00245 
00246 /****************************************************************************************
00247 *   Implementation of MatrixBase methods
00248 ****************************************************************************************/
00249 
00256 namespace internal {
00257 template<typename VectorX, typename VectorY, typename OtherScalar>
00258 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
00259 }
00260 
00267 template<typename Derived>
00268 template<typename OtherScalar>
00269 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00270 {
00271   RowXpr x(this->row(p));
00272   RowXpr y(this->row(q));
00273   internal::apply_rotation_in_the_plane(x, y, j);
00274 }
00275 
00282 template<typename Derived>
00283 template<typename OtherScalar>
00284 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00285 {
00286   ColXpr x(this->col(p));
00287   ColXpr y(this->col(q));
00288   internal::apply_rotation_in_the_plane(x, y, j.transpose());
00289 }
00290 
00291 namespace internal {
00292 template<typename VectorX, typename VectorY, typename OtherScalar>
00293 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
00294 {
00295   typedef typename VectorX::Index Index;
00296   typedef typename VectorX::Scalar Scalar;
00297   enum { PacketSize = packet_traits<Scalar>::size };
00298   typedef typename packet_traits<Scalar>::type Packet;
00299   eigen_assert(_x.size() == _y.size());
00300   Index size = _x.size();
00301   Index incrx = _x.innerStride();
00302   Index incry = _y.innerStride();
00303 
00304   Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
00305   Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
00306 
00307   /*** dynamic-size vectorized paths ***/
00308 
00309   if(VectorX::SizeAtCompileTime == Dynamic &&
00310     (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00311     ((incrx==1 && incry==1) || PacketSize == 1))
00312   {
00313     // both vectors are sequentially stored in memory => vectorization
00314     enum { Peeling = 2 };
00315 
00316     Index alignedStart = internal::first_aligned(y, size);
00317     Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
00318 
00319     const Packet pc = pset1<Packet>(j.c());
00320     const Packet ps = pset1<Packet>(j.s());
00321     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00322 
00323     for(Index i=0; i<alignedStart; ++i)
00324     {
00325       Scalar xi = x[i];
00326       Scalar yi = y[i];
00327       x[i] =  j.c() * xi + conj(j.s()) * yi;
00328       y[i] = -j.s() * xi + conj(j.c()) * yi;
00329     }
00330 
00331     Scalar* EIGEN_RESTRICT px = x + alignedStart;
00332     Scalar* EIGEN_RESTRICT py = y + alignedStart;
00333 
00334     if(internal::first_aligned(x, size)==alignedStart)
00335     {
00336       for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
00337       {
00338         Packet xi = pload<Packet>(px);
00339         Packet yi = pload<Packet>(py);
00340         pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00341         pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00342         px += PacketSize;
00343         py += PacketSize;
00344       }
00345     }
00346     else
00347     {
00348       Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
00349       for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
00350       {
00351         Packet xi   = ploadu<Packet>(px);
00352         Packet xi1  = ploadu<Packet>(px+PacketSize);
00353         Packet yi   = pload <Packet>(py);
00354         Packet yi1  = pload <Packet>(py+PacketSize);
00355         pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00356         pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
00357         pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00358         pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
00359         px += Peeling*PacketSize;
00360         py += Peeling*PacketSize;
00361       }
00362       if(alignedEnd!=peelingEnd)
00363       {
00364         Packet xi = ploadu<Packet>(x+peelingEnd);
00365         Packet yi = pload <Packet>(y+peelingEnd);
00366         pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00367         pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00368       }
00369     }
00370 
00371     for(Index i=alignedEnd; i<size; ++i)
00372     {
00373       Scalar xi = x[i];
00374       Scalar yi = y[i];
00375       x[i] =  j.c() * xi + conj(j.s()) * yi;
00376       y[i] = -j.s() * xi + conj(j.c()) * yi;
00377     }
00378   }
00379 
00380   /*** fixed-size vectorized path ***/
00381   else if(VectorX::SizeAtCompileTime != Dynamic &&
00382           (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00383           (VectorX::Flags & VectorY::Flags & AlignedBit))
00384   {
00385     const Packet pc = pset1<Packet>(j.c());
00386     const Packet ps = pset1<Packet>(j.s());
00387     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00388     Scalar* EIGEN_RESTRICT px = x;
00389     Scalar* EIGEN_RESTRICT py = y;
00390     for(Index i=0; i<size; i+=PacketSize)
00391     {
00392       Packet xi = pload<Packet>(px);
00393       Packet yi = pload<Packet>(py);
00394       pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00395       pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00396       px += PacketSize;
00397       py += PacketSize;
00398     }
00399   }
00400 
00401   /*** non-vectorized path ***/
00402   else
00403   {
00404     for(Index i=0; i<size; ++i)
00405     {
00406       Scalar xi = *x;
00407       Scalar yi = *y;
00408       *x =  j.c() * xi + conj(j.s()) * yi;
00409       *y = -j.s() * xi + conj(j.c()) * yi;
00410       x += incrx;
00411       y += incry;
00412     }
00413   }
00414 }
00415 
00416 } // end namespace internal
00417 
00418 } // end namespace Eigen
00419 
00420 #endif // EIGEN_JACOBI_H


win_eigen
Author(s): Daniel Stonier
autogenerated on Mon Oct 6 2014 12:24:50