Eigen  3.2.92
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 // 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 
34 template<typename Scalar> class JacobiRotation
35 {
36  public:
37  typedef typename NumTraits<Scalar>::Real RealScalar;
38 
41 
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 
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 
59  JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
60 
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>&, Index p, 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 
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 
124 template<typename Scalar>
125 template<typename Derived>
126 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, Index q)
127 {
128  return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
129 }
130 
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 namespace internal {
265 template<typename VectorX, typename VectorY, typename OtherScalar>
266 void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j);
267 }
268 
275 template<typename Derived>
276 template<typename OtherScalar>
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 
290 template<typename Derived>
291 template<typename OtherScalar>
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(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
302 {
303  typedef typename VectorX::Scalar Scalar;
304  enum { PacketSize = packet_traits<Scalar>::size };
305  typedef typename packet_traits<Scalar>::type Packet;
306  eigen_assert(xpr_x.size() == xpr_y.size());
307  Index size = xpr_x.size();
308  Index incrx = xpr_x.derived().innerStride();
309  Index incry = xpr_y.derived().innerStride();
310 
311  Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
312  Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0);
313 
314  OtherScalar c = j.c();
315  OtherScalar s = j.s();
316  if (c==OtherScalar(1) && s==OtherScalar(0))
317  return;
318 
319  /*** dynamic-size vectorized paths ***/
320 
321  if(VectorX::SizeAtCompileTime == Dynamic &&
322  (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
323  ((incrx==1 && incry==1) || PacketSize == 1))
324  {
325  // both vectors are sequentially stored in memory => vectorization
326  enum { Peeling = 2 };
327 
328  Index alignedStart = internal::first_default_aligned(y, size);
329  Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
330 
331  const Packet pc = pset1<Packet>(c);
332  const Packet ps = pset1<Packet>(s);
333  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
334 
335  for(Index i=0; i<alignedStart; ++i)
336  {
337  Scalar xi = x[i];
338  Scalar yi = y[i];
339  x[i] = c * xi + numext::conj(s) * yi;
340  y[i] = -s * xi + numext::conj(c) * yi;
341  }
342 
343  Scalar* EIGEN_RESTRICT px = x + alignedStart;
344  Scalar* EIGEN_RESTRICT py = y + alignedStart;
345 
346  if(internal::first_default_aligned(x, size)==alignedStart)
347  {
348  for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
349  {
350  Packet xi = pload<Packet>(px);
351  Packet yi = pload<Packet>(py);
352  pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
353  pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
354  px += PacketSize;
355  py += PacketSize;
356  }
357  }
358  else
359  {
360  Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
361  for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
362  {
363  Packet xi = ploadu<Packet>(px);
364  Packet xi1 = ploadu<Packet>(px+PacketSize);
365  Packet yi = pload <Packet>(py);
366  Packet yi1 = pload <Packet>(py+PacketSize);
367  pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
368  pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
369  pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
370  pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
371  px += Peeling*PacketSize;
372  py += Peeling*PacketSize;
373  }
374  if(alignedEnd!=peelingEnd)
375  {
376  Packet xi = ploadu<Packet>(x+peelingEnd);
377  Packet yi = pload <Packet>(y+peelingEnd);
378  pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
379  pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
380  }
381  }
382 
383  for(Index i=alignedEnd; i<size; ++i)
384  {
385  Scalar xi = x[i];
386  Scalar yi = y[i];
387  x[i] = c * xi + numext::conj(s) * yi;
388  y[i] = -s * xi + numext::conj(c) * yi;
389  }
390  }
391 
392  /*** fixed-size vectorized path ***/
393  else if(VectorX::SizeAtCompileTime != Dynamic &&
394  (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
395  (EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment)>0)) // FIXME should be compared to the required alignment
396  {
397  const Packet pc = pset1<Packet>(c);
398  const Packet ps = pset1<Packet>(s);
399  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
400  Scalar* EIGEN_RESTRICT px = x;
401  Scalar* EIGEN_RESTRICT py = y;
402  for(Index i=0; i<size; i+=PacketSize)
403  {
404  Packet xi = pload<Packet>(px);
405  Packet yi = pload<Packet>(py);
406  pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
407  pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
408  px += PacketSize;
409  py += PacketSize;
410  }
411  }
412 
413  /*** non-vectorized path ***/
414  else
415  {
416  for(Index i=0; i<size; ++i)
417  {
418  Scalar xi = *x;
419  Scalar yi = *y;
420  *x = c * xi + numext::conj(s) * yi;
421  *y = -s * xi + numext::conj(c) * yi;
422  x += incrx;
423  y += incry;
424  }
425  }
426 }
427 
428 } // end namespace internal
429 
430 } // end namespace Eigen
431 
432 #endif // EIGEN_JACOBI_H
internal::traits< Derived >::Scalar Scalar
Definition: DenseBase.h:68
JacobiRotation operator*(const JacobiRotation &other)
Definition: Jacobi.h:51
void applyOnTheLeft(const EigenBase< OtherDerived > &other)
Definition: MatrixBase.h:520
void makeGivens(const Scalar &p, const Scalar &q, Scalar *z=0)
Definition: Jacobi.h:148
JacobiRotation(const Scalar &c, const Scalar &s)
Definition: Jacobi.h:43
void applyOnTheRight(const EigenBase< OtherDerived > &other)
Definition: MatrixBase.h:508
Definition: LDLT.h:16
Rotation given by a cosine-sine pair.
Definition: ForwardDeclarations.h:260
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:107
bool makeJacobi(const MatrixBase< Derived > &, Index p, Index q)
Definition: Jacobi.h:126
Base class for all dense matrices, vectors, and arrays.
Definition: DenseBase.h:41
JacobiRotation transpose() const
Definition: Jacobi.h:59
const unsigned int PacketAccessBit
Definition: Constants.h:88
JacobiRotation adjoint() const
Definition: Jacobi.h:62
Definition: Eigen_Colamd.h:54
JacobiRotation()
Definition: Jacobi.h:40
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48