/src/Geometry_Eigen/Eigen/src/Jacobi/Jacobi.h

  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@gmail.com>
  5// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
  6//
  7// Eigen is free software; you can redistribute it and/or
  8// modify it under the terms of the GNU Lesser General Public
  9// License as published by the Free Software Foundation; either
 10// version 3 of the License, or (at your option) any later version.
 11//
 12// Alternatively, you can redistribute it and/or
 13// modify it under the terms of the GNU General Public License as
 14// published by the Free Software Foundation; either version 2 of
 15// the License, or (at your option) any later version.
 16//
 17// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
 18// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 19// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
 20// GNU General Public License for more details.
 21//
 22// You should have received a copy of the GNU Lesser General Public
 23// License and a copy of the GNU General Public License along with
 24// Eigen. If not, see <http://www.gnu.org/licenses/>.
 25
 26#ifndef EIGEN_JACOBI_H
 27#define EIGEN_JACOBI_H
 28
 29/** \ingroup Jacobi_Module
 30  * \jacobi_module
 31  * \class JacobiRotation
 32  * \brief Rotation given by a cosine-sine pair.
 33  *
 34  * This class represents a Jacobi or Givens rotation.
 35  * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
 36  * its cosine \c c and sine \c s as follow:
 37  * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s  & \overline c \end{array} \right ) \f$
 38  *
 39  * You can apply the respective counter-clockwise rotation to a column vector \c v by
 40  * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
 41  * \code
 42  * v.applyOnTheLeft(J.adjoint());
 43  * \endcode
 44  *
 45  * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
 46  */
 47template<typename Scalar> class JacobiRotation
 48{
 49  public:
 50    typedef typename NumTraits<Scalar>::Real RealScalar;
 51
 52    /** Default constructor without any initialization. */
 53    JacobiRotation() {}
 54
 55    /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
 56    JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
 57
 58    Scalar& c() { return m_c; }
 59    Scalar c() const { return m_c; }
 60    Scalar& s() { return m_s; }
 61    Scalar s() const { return m_s; }
 62
 63    /** Concatenates two planar rotation */
 64    JacobiRotation operator*(const JacobiRotation& other)
 65    {
 66      return JacobiRotation(m_c * other.m_c - internal::conj(m_s) * other.m_s,
 67                            internal::conj(m_c * internal::conj(other.m_s) + internal::conj(m_s) * internal::conj(other.m_c)));
 68    }
 69
 70    /** Returns the transposed transformation */
 71    JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); }
 72
 73    /** Returns the adjoint transformation */
 74    JacobiRotation adjoint() const { return JacobiRotation(internal::conj(m_c), -m_s); }
 75
 76    template<typename Derived>
 77    bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
 78    bool makeJacobi(RealScalar x, Scalar y, RealScalar z);
 79
 80    void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
 81
 82  protected:
 83    void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
 84    void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
 85
 86    Scalar m_c, m_s;
 87};
 88
 89/** 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
 90  * \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$
 91  *
 92  * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
 93  */
 94template<typename Scalar>
 95bool JacobiRotation<Scalar>::makeJacobi(RealScalar x, Scalar y, RealScalar z)
 96{
 97  typedef typename NumTraits<Scalar>::Real RealScalar;
 98  if(y == Scalar(0))
 99  {
100    m_c = Scalar(1);
101    m_s = Scalar(0);
102    return false;
103  }
104  else
105  {
106    RealScalar tau = (x-z)/(RealScalar(2)*internal::abs(y));
107    RealScalar w = internal::sqrt(internal::abs2(tau) + RealScalar(1));
108    RealScalar t;
109    if(tau>RealScalar(0))
110    {
111      t = RealScalar(1) / (tau + w);
112    }
113    else
114    {
115      t = RealScalar(1) / (tau - w);
116    }
117    RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
118    RealScalar n = RealScalar(1) / internal::sqrt(internal::abs2(t)+RealScalar(1));
119    m_s = - sign_t * (internal::conj(y) / internal::abs(y)) * internal::abs(t) * n;
120    m_c = n;
121    return true;
122  }
123}
124
125/** 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
126  * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
127  * a diagonal matrix \f$ A = J^* B J \f$
128  *
129  * Example: \include Jacobi_makeJacobi.cpp
130  * Output: \verbinclude Jacobi_makeJacobi.out
131  *
132  * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
133  */
134template<typename Scalar>
135template<typename Derived>
136inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
137{
138  return makeJacobi(internal::real(m.coeff(p,p)), m.coeff(p,q), internal::real(m.coeff(q,q)));
139}
140
141/** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
142  * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
143  * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
144  *
145  * The value of \a z is returned if \a z is not null (the default is null).
146  * Also note that G is built such that the cosine is always real.
147  *
148  * Example: \include Jacobi_makeGivens.cpp
149  * Output: \verbinclude Jacobi_makeGivens.out
150  *
151  * This function implements the continuous Givens rotation generation algorithm
152  * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
153  * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
154  *
155  * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
156  */
157template<typename Scalar>
158void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
159{
160  makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
161}
162
163
164// specialization for complexes
165template<typename Scalar>
166void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
167{
168  if(q==Scalar(0))
169  {
170    m_c = internal::real(p)<0 ? Scalar(-1) : Scalar(1);
171    m_s = 0;
172    if(r) *r = m_c * p;
173  }
174  else if(p==Scalar(0))
175  {
176    m_c = 0;
177    m_s = -q/internal::abs(q);
178    if(r) *r = internal::abs(q);
179  }
180  else
181  {
182    RealScalar p1 = internal::norm1(p);
183    RealScalar q1 = internal::norm1(q);
184    if(p1>=q1)
185    {
186      Scalar ps = p / p1;
187      RealScalar p2 = internal::abs2(ps);
188      Scalar qs = q / p1;
189      RealScalar q2 = internal::abs2(qs);
190
191      RealScalar u = internal::sqrt(RealScalar(1) + q2/p2);
192      if(internal::real(p)<RealScalar(0))
193        u = -u;
194
195      m_c = Scalar(1)/u;
196      m_s = -qs*internal::conj(ps)*(m_c/p2);
197      if(r) *r = p * u;
198    }
199    else
200    {
201      Scalar ps = p / q1;
202      RealScalar p2 = internal::abs2(ps);
203      Scalar qs = q / q1;
204      RealScalar q2 = internal::abs2(qs);
205
206      RealScalar u = q1 * internal::sqrt(p2 + q2);
207      if(internal::real(p)<RealScalar(0))
208        u = -u;
209
210      p1 = internal::abs(p);
211      ps = p/p1;
212      m_c = p1/u;
213      m_s = -internal::conj(ps) * (q/u);
214      if(r) *r = ps * u;
215    }
216  }
217}
218
219// specialization for reals
220template<typename Scalar>
221void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
222{
223
224  if(q==Scalar(0))
225  {
226    m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
227    m_s = Scalar(0);
228    if(r) *r = internal::abs(p);
229  }
230  else if(p==Scalar(0))
231  {
232    m_c = Scalar(0);
233    m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
234    if(r) *r = internal::abs(q);
235  }
236  else if(internal::abs(p) > internal::abs(q))
237  {
238    Scalar t = q/p;
239    Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
240    if(p<Scalar(0))
241      u = -u;
242    m_c = Scalar(1)/u;
243    m_s = -t * m_c;
244    if(r) *r = p * u;
245  }
246  else
247  {
248    Scalar t = p/q;
249    Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
250    if(q<Scalar(0))
251      u = -u;
252    m_s = -Scalar(1)/u;
253    m_c = -t * m_s;
254    if(r) *r = q * u;
255  }
256
257}
258
259/****************************************************************************************
260*   Implementation of MatrixBase methods
261****************************************************************************************/
262
263/** \jacobi_module
264  * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
265  * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right )  =  J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
266  *
267  * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
268  */
269namespace internal {
270template<typename VectorX, typename VectorY, typename OtherScalar>
271void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
272}
273
274/** \jacobi_module
275  * 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,
276  * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
277  *
278  * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
279  */
280template<typename Derived>
281template<typename OtherScalar>
282inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
283{
284  RowXpr x(this->row(p));
285  RowXpr y(this->row(q));
286  internal::apply_rotation_in_the_plane(x, y, j);
287}
288
289/** \ingroup Jacobi_Module
290  * 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
291  * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
292  *
293  * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
294  */
295template<typename Derived>
296template<typename OtherScalar>
297inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
298{
299  ColXpr x(this->col(p));
300  ColXpr y(this->col(q));
301  internal::apply_rotation_in_the_plane(x, y, j.transpose());
302}
303
304namespace internal {
305template<typename VectorX, typename VectorY, typename OtherScalar>
306void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
307{
308  typedef typename VectorX::Index Index;
309  typedef typename VectorX::Scalar Scalar;
310  enum { PacketSize = packet_traits<Scalar>::size };
311  typedef typename packet_traits<Scalar>::type Packet;
312  eigen_assert(_x.size() == _y.size());
313  Index size = _x.size();
314  Index incrx = _x.innerStride();
315  Index incry = _y.innerStride();
316
317  Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
318  Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
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 = first_aligned(y, size);
330    Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
331
332    const Packet pc = pset1<Packet>(j.c());
333    const Packet ps = pset1<Packet>(j.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] =  j.c() * xi + conj(j.s()) * yi;
341      y[i] = -j.s() * xi + conj(j.c()) * yi;
342    }
343
344    Scalar* EIGEN_RESTRICT px = x + alignedStart;
345    Scalar* EIGEN_RESTRICT py = y + alignedStart;
346
347    if(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] =  j.c() * xi + conj(j.s()) * yi;
389      y[i] = -j.s() * xi + conj(j.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>(j.c());
399    const Packet ps = pset1<Packet>(j.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 =  j.c() * xi + conj(j.s()) * yi;
422      *y = -j.s() * xi + conj(j.c()) * yi;
423      x += incrx;
424      y += incry;
425    }
426  }
427}
428}
429
430#endif // EIGEN_JACOBI_H