Eigen  3.3.7
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* r=0);
69 
70  protected:
71  void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
72  void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, 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  RealScalar deno = RealScalar(2)*abs(y);
88  if(deno < (std::numeric_limits<RealScalar>::min)())
89  {
90  m_c = Scalar(1);
91  m_s = Scalar(0);
92  return false;
93  }
94  else
95  {
96  RealScalar tau = (x-z)/deno;
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>
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* r)
149 {
150  makeGivens(p, q, r, 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 
301 template<typename Scalar, typename OtherScalar,
302  int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
303 struct apply_rotation_in_the_plane_selector
304 {
305  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
306  {
307  for(Index i=0; i<size; ++i)
308  {
309  Scalar xi = *x;
310  Scalar yi = *y;
311  *x = c * xi + numext::conj(s) * yi;
312  *y = -s * xi + numext::conj(c) * yi;
313  x += incrx;
314  y += incry;
315  }
316  }
317 };
318 
319 template<typename Scalar, typename OtherScalar,
320  int SizeAtCompileTime, int MinAlignment>
321 struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */>
322 {
323  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
324  {
325  enum {
326  PacketSize = packet_traits<Scalar>::size,
327  OtherPacketSize = packet_traits<OtherScalar>::size
328  };
329  typedef typename packet_traits<Scalar>::type Packet;
330  typedef typename packet_traits<OtherScalar>::type OtherPacket;
331 
332  /*** dynamic-size vectorized paths ***/
333  if(SizeAtCompileTime == Dynamic && ((incrx==1 && incry==1) || PacketSize == 1))
334  {
335  // both vectors are sequentially stored in memory => vectorization
336  enum { Peeling = 2 };
337 
338  Index alignedStart = internal::first_default_aligned(y, size);
339  Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
340 
341  const OtherPacket pc = pset1<OtherPacket>(c);
342  const OtherPacket ps = pset1<OtherPacket>(s);
343  conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj;
344  conj_helper<OtherPacket,Packet,false,false> pm;
345 
346  for(Index i=0; i<alignedStart; ++i)
347  {
348  Scalar xi = x[i];
349  Scalar yi = y[i];
350  x[i] = c * xi + numext::conj(s) * yi;
351  y[i] = -s * xi + numext::conj(c) * yi;
352  }
353 
354  Scalar* EIGEN_RESTRICT px = x + alignedStart;
355  Scalar* EIGEN_RESTRICT py = y + alignedStart;
356 
357  if(internal::first_default_aligned(x, size)==alignedStart)
358  {
359  for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
360  {
361  Packet xi = pload<Packet>(px);
362  Packet yi = pload<Packet>(py);
363  pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
364  pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
365  px += PacketSize;
366  py += PacketSize;
367  }
368  }
369  else
370  {
371  Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
372  for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
373  {
374  Packet xi = ploadu<Packet>(px);
375  Packet xi1 = ploadu<Packet>(px+PacketSize);
376  Packet yi = pload <Packet>(py);
377  Packet yi1 = pload <Packet>(py+PacketSize);
378  pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
379  pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
380  pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
381  pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
382  px += Peeling*PacketSize;
383  py += Peeling*PacketSize;
384  }
385  if(alignedEnd!=peelingEnd)
386  {
387  Packet xi = ploadu<Packet>(x+peelingEnd);
388  Packet yi = pload <Packet>(y+peelingEnd);
389  pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
390  pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
391  }
392  }
393 
394  for(Index i=alignedEnd; i<size; ++i)
395  {
396  Scalar xi = x[i];
397  Scalar yi = y[i];
398  x[i] = c * xi + numext::conj(s) * yi;
399  y[i] = -s * xi + numext::conj(c) * yi;
400  }
401  }
402 
403  /*** fixed-size vectorized path ***/
404  else if(SizeAtCompileTime != Dynamic && MinAlignment>0) // FIXME should be compared to the required alignment
405  {
406  const OtherPacket pc = pset1<OtherPacket>(c);
407  const OtherPacket ps = pset1<OtherPacket>(s);
408  conj_helper<OtherPacket,Packet,NumTraits<OtherPacket>::IsComplex,false> pcj;
409  conj_helper<OtherPacket,Packet,false,false> pm;
410  Scalar* EIGEN_RESTRICT px = x;
411  Scalar* EIGEN_RESTRICT py = y;
412  for(Index i=0; i<size; i+=PacketSize)
413  {
414  Packet xi = pload<Packet>(px);
415  Packet yi = pload<Packet>(py);
416  pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
417  pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
418  px += PacketSize;
419  py += PacketSize;
420  }
421  }
422 
423  /*** non-vectorized path ***/
424  else
425  {
426  apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,false>::run(x,incrx,y,incry,size,c,s);
427  }
428  }
429 };
430 
431 template<typename VectorX, typename VectorY, typename OtherScalar>
432 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
433 {
434  typedef typename VectorX::Scalar Scalar;
435  const bool Vectorizable = (VectorX::Flags & VectorY::Flags & PacketAccessBit)
436  && (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
437 
438  eigen_assert(xpr_x.size() == xpr_y.size());
439  Index size = xpr_x.size();
440  Index incrx = xpr_x.derived().innerStride();
441  Index incry = xpr_y.derived().innerStride();
442 
443  Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
444  Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0);
445 
446  OtherScalar c = j.c();
447  OtherScalar s = j.s();
448  if (c==OtherScalar(1) && s==OtherScalar(0))
449  return;
450 
451  apply_rotation_in_the_plane_selector<
452  Scalar,OtherScalar,
453  VectorX::SizeAtCompileTime,
454  EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment),
455  Vectorizable>::run(x,incrx,y,incry,size,c,s);
456 }
457 
458 } // end namespace internal
459 
460 } // end namespace Eigen
461 
462 #endif // EIGEN_JACOBI_H
JacobiRotation operator*(const JacobiRotation &other)
Definition: Jacobi.h:51
void applyOnTheLeft(const EigenBase< OtherDerived > &other)
Definition: MatrixBase.h:522
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_conjugate_op< typename Derived::Scalar >, const Derived > conj(const Eigen::ArrayBase< Derived > &x)
JacobiRotation(const Scalar &c, const Scalar &s)
Definition: Jacobi.h:43
void applyOnTheRight(const EigenBase< OtherDerived > &other)
Definition: MatrixBase.h:510
Namespace containing all symbols from the Eigen library.
Definition: Core:306
Rotation given by a cosine-sine pair.
Definition: ForwardDeclarations.h:259
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:150
bool makeJacobi(const MatrixBase< Derived > &, Index p, Index q)
Definition: Jacobi.h:126
Eigen::Index Index
The interface type of indices.
Definition: EigenBase.h:38
CoeffReturnType coeff(Index row, Index col) const
Definition: DenseCoeffsBase.h:96
const unsigned int PacketAccessBit
Definition: Constants.h:89
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:33
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_abs_op< typename Derived::Scalar >, const Derived > abs(const Eigen::ArrayBase< Derived > &x)
Definition: Eigen_Colamd.h:50
JacobiRotation transpose() const
Definition: Jacobi.h:59
const int Dynamic
Definition: Constants.h:21
JacobiRotation()
Definition: Jacobi.h:40
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
JacobiRotation adjoint() const
Definition: Jacobi.h:62
void makeGivens(const Scalar &p, const Scalar &q, Scalar *r=0)
Definition: Jacobi.h:148