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 
00016 /** \ingroup Jacobi_Module
00017   * \jacobi_module
00018   * \class JacobiRotation
00019   * \brief Rotation given by a cosine-sine pair.
00020   *
00021   * This class represents a Jacobi or Givens rotation.
00022   * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
00023   * its cosine \c c and sine \c s as follow:
00024   * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s  & \overline c \end{array} \right ) \f$
00025   *
00026   * You can apply the respective counter-clockwise rotation to a column vector \c v by
00027   * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
00028   * \code
00029   * v.applyOnTheLeft(J.adjoint());
00030   * \endcode
00031   *
00032   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00033   */
00034 template<typename Scalar> class JacobiRotation 
00035 {
00036   public:
00037     typedef typename NumTraits<Scalar>::Real RealScalar;
00038 
00039     /** Default constructor without any initialization. */
00040     JacobiRotation() {}
00041 
00042     /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
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 
00050     /** Concatenates two planar rotation */
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 
00058     /** Returns the transposed transformation */
00059     JacobiRotation  transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
00060 
00061     /** Returns the adjoint transformation */
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>&, typename Derived::Index p, typename Derived::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 
00077 /** Makes \c *this as a Jacobi rotation \a J such that applying \a J on both the right and left sides of the selfadjoint 2x2 matrix
00078   * \f$ B = \left ( \begin{array}{cc} x & y \\ \overline y & z \end{array} \right )\f$ yields a diagonal matrix \f$ A = J^* B J \f$
00079   *
00080   * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00081   */
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   if(y == Scalar(0))
00089   {
00090     m_c = Scalar(1);
00091     m_s = Scalar(0);
00092     return false;
00093   }
00094   else
00095   {
00096     RealScalar tau = (x-z)/(RealScalar(2)*abs(y));
00097     RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
00098     RealScalar t;
00099     if(tau>RealScalar(0))
00100     {
00101       t = RealScalar(1) / (tau + w);
00102     }
00103     else
00104     {
00105       t = RealScalar(1) / (tau - w);
00106     }
00107     RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
00108     RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
00109     m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
00110     m_c = n;
00111     return true;
00112   }
00113 }
00114 
00115 /** Makes \c *this as a Jacobi rotation \c J such that applying \a J on both the right and left sides of the 2x2 selfadjoint matrix
00116   * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
00117   * a diagonal matrix \f$ A = J^* B J \f$
00118   *
00119   * Example: \include Jacobi_makeJacobi.cpp
00120   * Output: \verbinclude Jacobi_makeJacobi.out
00121   *
00122   * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00123   */
00124 template<typename Scalar>
00125 template<typename Derived>
00126 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
00127 {
00128   return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
00129 }
00130 
00131 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
00132   * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
00133   * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
00134   *
00135   * The value of \a z is returned if \a z is not null (the default is null).
00136   * Also note that G is built such that the cosine is always real.
00137   *
00138   * Example: \include Jacobi_makeGivens.cpp
00139   * Output: \verbinclude Jacobi_makeGivens.out
00140   *
00141   * This function implements the continuous Givens rotation generation algorithm
00142   * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
00143   * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
00144   *
00145   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00146   */
00147 template<typename Scalar>
00148 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
00149 {
00150   makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
00151 }
00152 
00153 
00154 // specialization for complexes
00155 template<typename Scalar>
00156 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
00157 {
00158   using std::sqrt;
00159   using std::abs;
00160   using numext::conj;
00161   
00162   if(q==Scalar(0))
00163   {
00164     m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
00165     m_s = 0;
00166     if(r) *r = m_c * p;
00167   }
00168   else if(p==Scalar(0))
00169   {
00170     m_c = 0;
00171     m_s = -q/abs(q);
00172     if(r) *r = abs(q);
00173   }
00174   else
00175   {
00176     RealScalar p1 = numext::norm1(p);
00177     RealScalar q1 = numext::norm1(q);
00178     if(p1>=q1)
00179     {
00180       Scalar ps = p / p1;
00181       RealScalar p2 = numext::abs2(ps);
00182       Scalar qs = q / p1;
00183       RealScalar q2 = numext::abs2(qs);
00184 
00185       RealScalar u = sqrt(RealScalar(1) + q2/p2);
00186       if(numext::real(p)<RealScalar(0))
00187         u = -u;
00188 
00189       m_c = Scalar(1)/u;
00190       m_s = -qs*conj(ps)*(m_c/p2);
00191       if(r) *r = p * u;
00192     }
00193     else
00194     {
00195       Scalar ps = p / q1;
00196       RealScalar p2 = numext::abs2(ps);
00197       Scalar qs = q / q1;
00198       RealScalar q2 = numext::abs2(qs);
00199 
00200       RealScalar u = q1 * sqrt(p2 + q2);
00201       if(numext::real(p)<RealScalar(0))
00202         u = -u;
00203 
00204       p1 = abs(p);
00205       ps = p/p1;
00206       m_c = p1/u;
00207       m_s = -conj(ps) * (q/u);
00208       if(r) *r = ps * u;
00209     }
00210   }
00211 }
00212 
00213 // specialization for reals
00214 template<typename Scalar>
00215 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
00216 {
00217   using std::sqrt;
00218   using std::abs;
00219   if(q==Scalar(0))
00220   {
00221     m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
00222     m_s = Scalar(0);
00223     if(r) *r = abs(p);
00224   }
00225   else if(p==Scalar(0))
00226   {
00227     m_c = Scalar(0);
00228     m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
00229     if(r) *r = abs(q);
00230   }
00231   else if(abs(p) > abs(q))
00232   {
00233     Scalar t = q/p;
00234     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
00235     if(p<Scalar(0))
00236       u = -u;
00237     m_c = Scalar(1)/u;
00238     m_s = -t * m_c;
00239     if(r) *r = p * u;
00240   }
00241   else
00242   {
00243     Scalar t = p/q;
00244     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
00245     if(q<Scalar(0))
00246       u = -u;
00247     m_s = -Scalar(1)/u;
00248     m_c = -t * m_s;
00249     if(r) *r = q * u;
00250   }
00251 
00252 }
00253 
00254 /****************************************************************************************
00255 *   Implementation of MatrixBase methods
00256 ****************************************************************************************/
00257 
00258 /** \jacobi_module
00259   * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
00260   * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right )  =  J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
00261   *
00262   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00263   */
00264 namespace internal {
00265 template<typename VectorX, typename VectorY, typename OtherScalar>
00266 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
00267 }
00268 
00269 /** \jacobi_module
00270   * Applies the rotation in the plane \a j to the rows \a p and \a q of \c *this, i.e., it computes B = J * B,
00271   * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
00272   *
00273   * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
00274   */
00275 template<typename Derived>
00276 template<typename OtherScalar>
00277 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar> & j)
00278 {
00279   RowXpr x(this->row(p));
00280   RowXpr y(this->row(q));
00281   internal::apply_rotation_in_the_plane(x, y, j);
00282 }
00283 
00284 /** \ingroup Jacobi_Module
00285   * Applies the rotation in the plane \a j to the columns \a p and \a q of \c *this, i.e., it computes B = B * J
00286   * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
00287   *
00288   * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
00289   */
00290 template<typename Derived>
00291 template<typename OtherScalar>
00292 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar> & j)
00293 {
00294   ColXpr x(this->col(p));
00295   ColXpr y(this->col(q));
00296   internal::apply_rotation_in_the_plane(x, y, j.transpose());
00297 }
00298 
00299 namespace internal {
00300 template<typename VectorX, typename VectorY, typename OtherScalar>
00301 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar> & j)
00302 {
00303   typedef typename VectorX::Index Index;
00304   typedef typename VectorX::Scalar Scalar;
00305   enum { PacketSize = packet_traits<Scalar>::size };
00306   typedef typename packet_traits<Scalar>::type Packet;
00307   eigen_assert(_x.size() == _y.size());
00308   Index size = _x.size();
00309   Index incrx = _x.innerStride();
00310   Index incry = _y.innerStride();
00311 
00312   Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
00313   Scalar* EIGEN_RESTRICT y = &_y.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_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_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           (VectorX::Flags & VectorY::Flags & AlignedBit))
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