Home | History | Annotate | Download | only in Jacobi 
      1 // This file is part of Eigen, a lightweight C++ template library
      2 // for linear algebra.
      3 //
      4 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1 (at) gmail.com>
      5 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud (at) inria.fr>
      6 //
      7 // This Source Code Form is subject to the terms of the Mozilla
      8 // Public License v. 2.0. If a copy of the MPL was not distributed
      9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
     10 
     11 #ifndef EIGEN_JACOBI_H
     12 #define EIGEN_JACOBI_H
     13 
     14 namespace Eigen {
     15 
     16 /** \ingroup Jacobi_Module
     17   * \jacobi_module
     18   * \class JacobiRotation
     19   * \brief Rotation given by a cosine-sine pair.
     20   *
     21   * This class represents a Jacobi or Givens rotation.
     22   * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
     23   * its cosine \c c and sine \c s as follow:
     24   * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s  & \overline c \end{array} \right ) \f$
     25   *
     26   * You can apply the respective counter-clockwise rotation to a column vector \c v by
     27   * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
     28   * \code
     29   * v.applyOnTheLeft(J.adjoint());
     30   * \endcode
     31   *
     32   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
     33   */
     34 template<typename Scalar> class JacobiRotation
     35 {
     36   public:
     37     typedef typename NumTraits<Scalar>::Real RealScalar;
     38 
     39     /** Default constructor without any initialization. */
     40     JacobiRotation() {}
     41 
     42     /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
     43     JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
     44 
     45     Scalar& c() { return m_c; }
     46     Scalar c() const { return m_c; }
     47     Scalar& s() { return m_s; }
     48     Scalar s() const { return m_s; }
     49 
     50     /** Concatenates two planar rotation */
     51     JacobiRotation operator*(const JacobiRotation& other)
     52     {
     53       using numext::conj;
     54       return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s,
     55                             conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c)));
     56     }
     57 
     58     /** Returns the transposed transformation */
     59     JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
     60 
     61     /** Returns the adjoint transformation */
     62     JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); }
     63 
     64     template<typename Derived>
     65     bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
     66     bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
     67 
     68     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
     69 
     70   protected:
     71     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
     72     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
     73 
     74     Scalar m_c, m_s;
     75 };
     76 
     77 /** 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
     78   * \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$
     79   *
     80   * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
     81   */
     82 template<typename Scalar>
     83 bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z)
     84 {
     85   using std::sqrt;
     86   using std::abs;
     87   typedef typename NumTraits<Scalar>::Real RealScalar;
     88   if(y == Scalar(0))
     89   {
     90     m_c = Scalar(1);
     91     m_s = Scalar(0);
     92     return false;
     93   }
     94   else
     95   {
     96     RealScalar tau = (x-z)/(RealScalar(2)*abs(y));
     97     RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
     98     RealScalar t;
     99     if(tau>RealScalar(0))
    100     {
    101       t = RealScalar(1) / (tau + w);
    102     }
    103     else
    104     {
    105       t = RealScalar(1) / (tau - w);
    106     }
    107     RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
    108     RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
    109     m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
    110     m_c = n;
    111     return true;
    112   }
    113 }
    114 
    115 /** 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
    116   * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
    117   * a diagonal matrix \f$ A = J^* B J \f$
    118   *
    119   * Example: \include Jacobi_makeJacobi.cpp
    120   * Output: \verbinclude Jacobi_makeJacobi.out
    121   *
    122   * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
    123   */
    124 template<typename Scalar>
    125 template<typename Derived>
    126 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
    127 {
    128   return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
    129 }
    130 
    131 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
    132   * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
    133   * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
    134   *
    135   * The value of \a z is returned if \a z is not null (the default is null).
    136   * Also note that G is built such that the cosine is always real.
    137   *
    138   * Example: \include Jacobi_makeGivens.cpp
    139   * Output: \verbinclude Jacobi_makeGivens.out
    140   *
    141   * This function implements the continuous Givens rotation generation algorithm
    142   * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
    143   * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
    144   *
    145   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
    146   */
    147 template<typename Scalar>
    148 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
    149 {
    150   makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
    151 }
    152 
    153 
    154 // specialization for complexes
    155 template<typename Scalar>
    156 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
    157 {
    158   using std::sqrt;
    159   using std::abs;
    160   using numext::conj;
    161 
    162   if(q==Scalar(0))
    163   {
    164     m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
    165     m_s = 0;
    166     if(r) *r = m_c * p;
    167   }
    168   else if(p==Scalar(0))
    169   {
    170     m_c = 0;
    171     m_s = -q/abs(q);
    172     if(r) *r = abs(q);
    173   }
    174   else
    175   {
    176     RealScalar p1 = numext::norm1(p);
    177     RealScalar q1 = numext::norm1(q);
    178     if(p1>=q1)
    179     {
    180       Scalar ps = p / p1;
    181       RealScalar p2 = numext::abs2(ps);
    182       Scalar qs = q / p1;
    183       RealScalar q2 = numext::abs2(qs);
    184 
    185       RealScalar u = sqrt(RealScalar(1) + q2/p2);
    186       if(numext::real(p)<RealScalar(0))
    187         u = -u;
    188 
    189       m_c = Scalar(1)/u;
    190       m_s = -qs*conj(ps)*(m_c/p2);
    191       if(r) *r = p * u;
    192     }
    193     else
    194     {
    195       Scalar ps = p / q1;
    196       RealScalar p2 = numext::abs2(ps);
    197       Scalar qs = q / q1;
    198       RealScalar q2 = numext::abs2(qs);
    199 
    200       RealScalar u = q1 * sqrt(p2 + q2);
    201       if(numext::real(p)<RealScalar(0))
    202         u = -u;
    203 
    204       p1 = abs(p);
    205       ps = p/p1;
    206       m_c = p1/u;
    207       m_s = -conj(ps) * (q/u);
    208       if(r) *r = ps * u;
    209     }
    210   }
    211 }
    212 
    213 // specialization for reals
    214 template<typename Scalar>
    215 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
    216 {
    217   using std::sqrt;
    218   using std::abs;
    219   if(q==Scalar(0))
    220   {
    221     m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
    222     m_s = Scalar(0);
    223     if(r) *r = abs(p);
    224   }
    225   else if(p==Scalar(0))
    226   {
    227     m_c = Scalar(0);
    228     m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
    229     if(r) *r = abs(q);
    230   }
    231   else if(abs(p) > abs(q))
    232   {
    233     Scalar t = q/p;
    234     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
    235     if(p<Scalar(0))
    236       u = -u;
    237     m_c = Scalar(1)/u;
    238     m_s = -t * m_c;
    239     if(r) *r = p * u;
    240   }
    241   else
    242   {
    243     Scalar t = p/q;
    244     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
    245     if(q<Scalar(0))
    246       u = -u;
    247     m_s = -Scalar(1)/u;
    248     m_c = -t * m_s;
    249     if(r) *r = q * u;
    250   }
    251 
    252 }
    253 
    254 /****************************************************************************************
    255 *   Implementation of MatrixBase methods
    256 ****************************************************************************************/
    257 
    258 /** \jacobi_module
    259   * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
    260   * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right )  =  J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
    261   *
    262   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
    263   */
    264 namespace internal {
    265 template<typename VectorX, typename VectorY, typename OtherScalar>
    266 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
    267 }
    268 
    269 /** \jacobi_module
    270   * 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,
    271   * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
    272   *
    273   * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
    274   */
    275 template<typename Derived>
    276 template<typename OtherScalar>
    277 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
    278 {
    279   RowXpr x(this->row(p));
    280   RowXpr y(this->row(q));
    281   internal::apply_rotation_in_the_plane(x, y, j);
    282 }
    283 
    284 /** \ingroup Jacobi_Module
    285   * 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
    286   * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
    287   *
    288   * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
    289   */
    290 template<typename Derived>
    291 template<typename OtherScalar>
    292 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
    293 {
    294   ColXpr x(this->col(p));
    295   ColXpr y(this->col(q));
    296   internal::apply_rotation_in_the_plane(x, y, j.transpose());
    297 }
    298 
    299 namespace internal {
    300 template<typename VectorX, typename VectorY, typename OtherScalar>
    301 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
    302 {
    303   typedef typename VectorX::Index Index;
    304   typedef typename VectorX::Scalar Scalar;
    305   enum { PacketSize = packet_traits<Scalar>::size };
    306   typedef typename packet_traits<Scalar>::type Packet;
    307   eigen_assert(_x.size() == _y.size());
    308   Index size = _x.size();
    309   Index incrx = _x.innerStride();
    310   Index incry = _y.innerStride();
    311 
    312   Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
    313   Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
    314 
    315   OtherScalar c = j.c();
    316   OtherScalar s = j.s();
    317   if (c==OtherScalar(1) && s==OtherScalar(0))
    318     return;
    319 
    320   /*** dynamic-size vectorized paths ***/
    321 
    322   if(VectorX::SizeAtCompileTime == Dynamic &&
    323     (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
    324     ((incrx==1 && incry==1) || PacketSize == 1))
    325   {
    326     // both vectors are sequentially stored in memory => vectorization
    327     enum { Peeling = 2 };
    328 
    329     Index alignedStart = internal::first_aligned(y, size);
    330     Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
    331 
    332     const Packet pc = pset1<Packet>(c);
    333     const Packet ps = pset1<Packet>(s);
    334     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
    335 
    336     for(Index i=0; i<alignedStart; ++i)
    337     {
    338       Scalar xi = x[i];
    339       Scalar yi = y[i];
    340       x[i] =  c * xi + numext::conj(s) * yi;
    341       y[i] = -s * xi + numext::conj(c) * yi;
    342     }
    343 
    344     Scalar* EIGEN_RESTRICT px = x + alignedStart;
    345     Scalar* EIGEN_RESTRICT py = y + alignedStart;
    346 
    347     if(internal::first_aligned(x, size)==alignedStart)
    348     {
    349       for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
    350       {
    351         Packet xi = pload<Packet>(px);
    352         Packet yi = pload<Packet>(py);
    353         pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
    354         pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
    355         px += PacketSize;
    356         py += PacketSize;
    357       }
    358     }
    359     else
    360     {
    361       Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
    362       for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
    363       {
    364         Packet xi   = ploadu<Packet>(px);
    365         Packet xi1  = ploadu<Packet>(px+PacketSize);
    366         Packet yi   = pload <Packet>(py);
    367         Packet yi1  = pload <Packet>(py+PacketSize);
    368         pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
    369         pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
    370         pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
    371         pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
    372         px += Peeling*PacketSize;
    373         py += Peeling*PacketSize;
    374       }
    375       if(alignedEnd!=peelingEnd)
    376       {
    377         Packet xi = ploadu<Packet>(x+peelingEnd);
    378         Packet yi = pload <Packet>(y+peelingEnd);
    379         pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
    380         pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
    381       }
    382     }
    383 
    384     for(Index i=alignedEnd; i<size; ++i)
    385     {
    386       Scalar xi = x[i];
    387       Scalar yi = y[i];
    388       x[i] =  c * xi + numext::conj(s) * yi;
    389       y[i] = -s * xi + numext::conj(c) * yi;
    390     }
    391   }
    392 
    393   /*** fixed-size vectorized path ***/
    394   else if(VectorX::SizeAtCompileTime != Dynamic &&
    395           (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
    396           (VectorX::Flags & VectorY::Flags & AlignedBit))
    397   {
    398     const Packet pc = pset1<Packet>(c);
    399     const Packet ps = pset1<Packet>(s);
    400     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
    401     Scalar* EIGEN_RESTRICT px = x;
    402     Scalar* EIGEN_RESTRICT py = y;
    403     for(Index i=0; i<size; i+=PacketSize)
    404     {
    405       Packet xi = pload<Packet>(px);
    406       Packet yi = pload<Packet>(py);
    407       pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
    408       pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
    409       px += PacketSize;
    410       py += PacketSize;
    411     }
    412   }
    413 
    414   /*** non-vectorized path ***/
    415   else
    416   {
    417     for(Index i=0; i<size; ++i)
    418     {
    419       Scalar xi = *x;
    420       Scalar yi = *y;
    421       *x =  c * xi + numext::conj(s) * yi;
    422       *y = -s * xi + numext::conj(c) * yi;
    423       x += incrx;
    424       y += incry;
    425     }
    426   }
    427 }
    428 
    429 } // end namespace internal
    430 
    431 } // end namespace Eigen
    432 
    433 #endif // EIGEN_JACOBI_H
    434