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 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 #ifndef EIGEN_JACOBI_H
00027 #define EIGEN_JACOBI_H
00028 
00029 namespace Eigen { 
00030 
00049 template<typename Scalar> class JacobiRotation
00050 {
00051   public:
00052     typedef typename NumTraits<Scalar>::Real RealScalar;
00053 
00055     JacobiRotation() {}
00056 
00058     JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
00059 
00060     Scalar& c() { return m_c; }
00061     Scalar c() const { return m_c; }
00062     Scalar& s() { return m_s; }
00063     Scalar s() const { return m_s; }
00064 
00066     JacobiRotation operator*(const JacobiRotation& other)
00067     {
00068       return JacobiRotation(m_c * other.m_c - internal::conj(m_s) * other.m_s,
00069                             internal::conj(m_c * internal::conj(other.m_s) + internal::conj(m_s) * internal::conj(other.m_c)));
00070     }
00071 
00073     JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); }
00074 
00076     JacobiRotation adjoint() const { return JacobiRotation(internal::conj(m_c), -m_s); }
00077 
00078     template<typename Derived>
00079     bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
00080     bool makeJacobi(RealScalar x, Scalar y, RealScalar z);
00081 
00082     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
00083 
00084   protected:
00085     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
00086     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
00087 
00088     Scalar m_c, m_s;
00089 };
00090 
00096 template<typename Scalar>
00097 bool JacobiRotation<Scalar>::makeJacobi(RealScalar x, Scalar y, RealScalar z)
00098 {
00099   typedef typename NumTraits<Scalar>::Real RealScalar;
00100   if(y == Scalar(0))
00101   {
00102     m_c = Scalar(1);
00103     m_s = Scalar(0);
00104     return false;
00105   }
00106   else
00107   {
00108     RealScalar tau = (x-z)/(RealScalar(2)*internal::abs(y));
00109     RealScalar w = internal::sqrt(internal::abs2(tau) + RealScalar(1));
00110     RealScalar t;
00111     if(tau>RealScalar(0))
00112     {
00113       t = RealScalar(1) / (tau + w);
00114     }
00115     else
00116     {
00117       t = RealScalar(1) / (tau - w);
00118     }
00119     RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
00120     RealScalar n = RealScalar(1) / internal::sqrt(internal::abs2(t)+RealScalar(1));
00121     m_s = - sign_t * (internal::conj(y) / internal::abs(y)) * internal::abs(t) * n;
00122     m_c = n;
00123     return true;
00124   }
00125 }
00126 
00136 template<typename Scalar>
00137 template<typename Derived>
00138 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
00139 {
00140   return makeJacobi(internal::real(m.coeff(p,p)), m.coeff(p,q), internal::real(m.coeff(q,q)));
00141 }
00142 
00159 template<typename Scalar>
00160 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
00161 {
00162   makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
00163 }
00164 
00165 
00166 // specialization for complexes
00167 template<typename Scalar>
00168 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
00169 {
00170   if(q==Scalar(0))
00171   {
00172     m_c = internal::real(p)<0 ? Scalar(-1) : Scalar(1);
00173     m_s = 0;
00174     if(r) *r = m_c * p;
00175   }
00176   else if(p==Scalar(0))
00177   {
00178     m_c = 0;
00179     m_s = -q/internal::abs(q);
00180     if(r) *r = internal::abs(q);
00181   }
00182   else
00183   {
00184     RealScalar p1 = internal::norm1(p);
00185     RealScalar q1 = internal::norm1(q);
00186     if(p1>=q1)
00187     {
00188       Scalar ps = p / p1;
00189       RealScalar p2 = internal::abs2(ps);
00190       Scalar qs = q / p1;
00191       RealScalar q2 = internal::abs2(qs);
00192 
00193       RealScalar u = internal::sqrt(RealScalar(1) + q2/p2);
00194       if(internal::real(p)<RealScalar(0))
00195         u = -u;
00196 
00197       m_c = Scalar(1)/u;
00198       m_s = -qs*internal::conj(ps)*(m_c/p2);
00199       if(r) *r = p * u;
00200     }
00201     else
00202     {
00203       Scalar ps = p / q1;
00204       RealScalar p2 = internal::abs2(ps);
00205       Scalar qs = q / q1;
00206       RealScalar q2 = internal::abs2(qs);
00207 
00208       RealScalar u = q1 * internal::sqrt(p2 + q2);
00209       if(internal::real(p)<RealScalar(0))
00210         u = -u;
00211 
00212       p1 = internal::abs(p);
00213       ps = p/p1;
00214       m_c = p1/u;
00215       m_s = -internal::conj(ps) * (q/u);
00216       if(r) *r = ps * u;
00217     }
00218   }
00219 }
00220 
00221 // specialization for reals
00222 template<typename Scalar>
00223 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
00224 {
00225 
00226   if(q==Scalar(0))
00227   {
00228     m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
00229     m_s = Scalar(0);
00230     if(r) *r = internal::abs(p);
00231   }
00232   else if(p==Scalar(0))
00233   {
00234     m_c = Scalar(0);
00235     m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
00236     if(r) *r = internal::abs(q);
00237   }
00238   else if(internal::abs(p) > internal::abs(q))
00239   {
00240     Scalar t = q/p;
00241     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00242     if(p<Scalar(0))
00243       u = -u;
00244     m_c = Scalar(1)/u;
00245     m_s = -t * m_c;
00246     if(r) *r = p * u;
00247   }
00248   else
00249   {
00250     Scalar t = p/q;
00251     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00252     if(q<Scalar(0))
00253       u = -u;
00254     m_s = -Scalar(1)/u;
00255     m_c = -t * m_s;
00256     if(r) *r = q * u;
00257   }
00258 
00259 }
00260 
00261 /****************************************************************************************
00262 *   Implementation of MatrixBase methods
00263 ****************************************************************************************/
00264 
00271 namespace internal {
00272 template<typename VectorX, typename VectorY, typename OtherScalar>
00273 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
00274 }
00275 
00282 template<typename Derived>
00283 template<typename OtherScalar>
00284 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00285 {
00286   RowXpr x(this->row(p));
00287   RowXpr y(this->row(q));
00288   internal::apply_rotation_in_the_plane(x, y, j);
00289 }
00290 
00297 template<typename Derived>
00298 template<typename OtherScalar>
00299 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00300 {
00301   ColXpr x(this->col(p));
00302   ColXpr y(this->col(q));
00303   internal::apply_rotation_in_the_plane(x, y, j.transpose());
00304 }
00305 
00306 namespace internal {
00307 template<typename VectorX, typename VectorY, typename OtherScalar>
00308 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
00309 {
00310   typedef typename VectorX::Index Index;
00311   typedef typename VectorX::Scalar Scalar;
00312   enum { PacketSize = packet_traits<Scalar>::size };
00313   typedef typename packet_traits<Scalar>::type Packet;
00314   eigen_assert(_x.size() == _y.size());
00315   Index size = _x.size();
00316   Index incrx = _x.innerStride();
00317   Index incry = _y.innerStride();
00318 
00319   Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
00320   Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
00321 
00322   /*** dynamic-size vectorized paths ***/
00323 
00324   if(VectorX::SizeAtCompileTime == Dynamic &&
00325     (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00326     ((incrx==1 && incry==1) || PacketSize == 1))
00327   {
00328     // both vectors are sequentially stored in memory => vectorization
00329     enum { Peeling = 2 };
00330 
00331     Index alignedStart = internal::first_aligned(y, size);
00332     Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
00333 
00334     const Packet pc = pset1<Packet>(j.c());
00335     const Packet ps = pset1<Packet>(j.s());
00336     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00337 
00338     for(Index i=0; i<alignedStart; ++i)
00339     {
00340       Scalar xi = x[i];
00341       Scalar yi = y[i];
00342       x[i] =  j.c() * xi + conj(j.s()) * yi;
00343       y[i] = -j.s() * xi + conj(j.c()) * yi;
00344     }
00345 
00346     Scalar* EIGEN_RESTRICT px = x + alignedStart;
00347     Scalar* EIGEN_RESTRICT py = y + alignedStart;
00348 
00349     if(internal::first_aligned(x, size)==alignedStart)
00350     {
00351       for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
00352       {
00353         Packet xi = pload<Packet>(px);
00354         Packet yi = pload<Packet>(py);
00355         pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00356         pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00357         px += PacketSize;
00358         py += PacketSize;
00359       }
00360     }
00361     else
00362     {
00363       Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
00364       for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
00365       {
00366         Packet xi   = ploadu<Packet>(px);
00367         Packet xi1  = ploadu<Packet>(px+PacketSize);
00368         Packet yi   = pload <Packet>(py);
00369         Packet yi1  = pload <Packet>(py+PacketSize);
00370         pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00371         pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
00372         pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00373         pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
00374         px += Peeling*PacketSize;
00375         py += Peeling*PacketSize;
00376       }
00377       if(alignedEnd!=peelingEnd)
00378       {
00379         Packet xi = ploadu<Packet>(x+peelingEnd);
00380         Packet yi = pload <Packet>(y+peelingEnd);
00381         pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00382         pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00383       }
00384     }
00385 
00386     for(Index i=alignedEnd; i<size; ++i)
00387     {
00388       Scalar xi = x[i];
00389       Scalar yi = y[i];
00390       x[i] =  j.c() * xi + conj(j.s()) * yi;
00391       y[i] = -j.s() * xi + conj(j.c()) * yi;
00392     }
00393   }
00394 
00395   /*** fixed-size vectorized path ***/
00396   else if(VectorX::SizeAtCompileTime != Dynamic &&
00397           (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00398           (VectorX::Flags & VectorY::Flags & AlignedBit))
00399   {
00400     const Packet pc = pset1<Packet>(j.c());
00401     const Packet ps = pset1<Packet>(j.s());
00402     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00403     Scalar* EIGEN_RESTRICT px = x;
00404     Scalar* EIGEN_RESTRICT py = y;
00405     for(Index i=0; i<size; i+=PacketSize)
00406     {
00407       Packet xi = pload<Packet>(px);
00408       Packet yi = pload<Packet>(py);
00409       pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00410       pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00411       px += PacketSize;
00412       py += PacketSize;
00413     }
00414   }
00415 
00416   /*** non-vectorized path ***/
00417   else
00418   {
00419     for(Index i=0; i<size; ++i)
00420     {
00421       Scalar xi = *x;
00422       Scalar yi = *y;
00423       *x =  j.c() * xi + conj(j.s()) * yi;
00424       *y = -j.s() * xi + conj(j.c()) * yi;
00425       x += incrx;
00426       y += incry;
00427     }
00428   }
00429 }
00430 
00431 } // end namespace internal
00432 
00433 } // end namespace Eigen
00434 
00435 #endif // EIGEN_JACOBI_H