Eigen  3.3.5
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  RealScalar deno = RealScalar(2)*abs(y);
89  if(deno < (std::numeric_limits<RealScalar>::min)())
90  {
91  m_c = Scalar(1);
92  m_s = Scalar(0);
93  return false;
94  }
95  else
96  {
97  RealScalar tau = (x-z)/deno;
98  RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
99  RealScalar t;
100  if(tau>RealScalar(0))
101  {
102  t = RealScalar(1) / (tau + w);
103  }
104  else
105  {
106  t = RealScalar(1) / (tau - w);
107  }
108  RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
109  RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
110  m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
111  m_c = n;
112  return true;
113  }
114 }
115 
125 template<typename Scalar>
126 template<typename Derived>
128 {
129  return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
130 }
131 
148 template<typename Scalar>
149 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
150 {
151  makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
152 }
153 
154 
155 // specialization for complexes
156 template<typename Scalar>
157 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
158 {
159  using std::sqrt;
160  using std::abs;
161  using numext::conj;
162 
163  if(q==Scalar(0))
164  {
165  m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
166  m_s = 0;
167  if(r) *r = m_c * p;
168  }
169  else if(p==Scalar(0))
170  {
171  m_c = 0;
172  m_s = -q/abs(q);
173  if(r) *r = abs(q);
174  }
175  else
176  {
177  RealScalar p1 = numext::norm1(p);
178  RealScalar q1 = numext::norm1(q);
179  if(p1>=q1)
180  {
181  Scalar ps = p / p1;
182  RealScalar p2 = numext::abs2(ps);
183  Scalar qs = q / p1;
184  RealScalar q2 = numext::abs2(qs);
185 
186  RealScalar u = sqrt(RealScalar(1) + q2/p2);
187  if(numext::real(p)<RealScalar(0))
188  u = -u;
189 
190  m_c = Scalar(1)/u;
191  m_s = -qs*conj(ps)*(m_c/p2);
192  if(r) *r = p * u;
193  }
194  else
195  {
196  Scalar ps = p / q1;
197  RealScalar p2 = numext::abs2(ps);
198  Scalar qs = q / q1;
199  RealScalar q2 = numext::abs2(qs);
200 
201  RealScalar u = q1 * sqrt(p2 + q2);
202  if(numext::real(p)<RealScalar(0))
203  u = -u;
204 
205  p1 = abs(p);
206  ps = p/p1;
207  m_c = p1/u;
208  m_s = -conj(ps) * (q/u);
209  if(r) *r = ps * u;
210  }
211  }
212 }
213 
214 // specialization for reals
215 template<typename Scalar>
216 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
217 {
218  using std::sqrt;
219  using std::abs;
220  if(q==Scalar(0))
221  {
222  m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
223  m_s = Scalar(0);
224  if(r) *r = abs(p);
225  }
226  else if(p==Scalar(0))
227  {
228  m_c = Scalar(0);
229  m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
230  if(r) *r = abs(q);
231  }
232  else if(abs(p) > abs(q))
233  {
234  Scalar t = q/p;
235  Scalar u = sqrt(Scalar(1) + numext::abs2(t));
236  if(p<Scalar(0))
237  u = -u;
238  m_c = Scalar(1)/u;
239  m_s = -t * m_c;
240  if(r) *r = p * u;
241  }
242  else
243  {
244  Scalar t = p/q;
245  Scalar u = sqrt(Scalar(1) + numext::abs2(t));
246  if(q<Scalar(0))
247  u = -u;
248  m_s = -Scalar(1)/u;
249  m_c = -t * m_s;
250  if(r) *r = q * u;
251  }
252 
253 }
254 
255 /****************************************************************************************
256 * Implementation of MatrixBase methods
257 ****************************************************************************************/
258 
259 namespace internal {
266 template<typename VectorX, typename VectorY, typename OtherScalar>
267 void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j);
268 }
269 
276 template<typename Derived>
277 template<typename OtherScalar>
279 {
280  RowXpr x(this->row(p));
281  RowXpr y(this->row(q));
282  internal::apply_rotation_in_the_plane(x, y, j);
283 }
284 
291 template<typename Derived>
292 template<typename OtherScalar>
294 {
295  ColXpr x(this->col(p));
296  ColXpr y(this->col(q));
297  internal::apply_rotation_in_the_plane(x, y, j.transpose());
298 }
299 
300 namespace internal {
301 
302 template<typename Scalar, typename OtherScalar,
303  int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
304 struct apply_rotation_in_the_plane_selector
305 {
306  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
307  {
308  for(Index i=0; i<size; ++i)
309  {
310  Scalar xi = *x;
311  Scalar yi = *y;
312  *x = c * xi + numext::conj(s) * yi;
313  *y = -s * xi + numext::conj(c) * yi;
314  x += incrx;
315  y += incry;
316  }
317  }
318 };
319 
320 template<typename Scalar, typename OtherScalar,
321  int SizeAtCompileTime, int MinAlignment>
322 struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */>
323 {
324  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
325  {
326  enum {
327  PacketSize = packet_traits<Scalar>::size,
328  OtherPacketSize = packet_traits<OtherScalar>::size
329  };
330  typedef typename packet_traits<Scalar>::type Packet;
331  typedef typename packet_traits<OtherScalar>::type OtherPacket;
332 
333  /*** dynamic-size vectorized paths ***/
334  if(SizeAtCompileTime == Dynamic && ((incrx==1 && incry==1) || PacketSize == 1))
335  {
336  // both vectors are sequentially stored in memory => vectorization
337  enum { Peeling = 2 };
338 
339  Index alignedStart = internal::first_default_aligned(y, size);
340  Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
341 
342  const OtherPacket pc = pset1<OtherPacket>(c);
343  const OtherPacket ps = pset1<OtherPacket>(s);
344  conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj;
345  conj_helper<OtherPacket,Packet,false,false> pm;
346 
347  for(Index i=0; i<alignedStart; ++i)
348  {
349  Scalar xi = x[i];
350  Scalar yi = y[i];
351  x[i] = c * xi + numext::conj(s) * yi;
352  y[i] = -s * xi + numext::conj(c) * yi;
353  }
354 
355  Scalar* EIGEN_RESTRICT px = x + alignedStart;
356  Scalar* EIGEN_RESTRICT py = y + alignedStart;
357 
358  if(internal::first_default_aligned(x, size)==alignedStart)
359  {
360  for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
361  {
362  Packet xi = pload<Packet>(px);
363  Packet yi = pload<Packet>(py);
364  pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
365  pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
366  px += PacketSize;
367  py += PacketSize;
368  }
369  }
370  else
371  {
372  Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
373  for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
374  {
375  Packet xi = ploadu<Packet>(px);
376  Packet xi1 = ploadu<Packet>(px+PacketSize);
377  Packet yi = pload <Packet>(py);
378  Packet yi1 = pload <Packet>(py+PacketSize);
379  pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
380  pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
381  pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
382  pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
383  px += Peeling*PacketSize;
384  py += Peeling*PacketSize;
385  }
386  if(alignedEnd!=peelingEnd)
387  {
388  Packet xi = ploadu<Packet>(x+peelingEnd);
389  Packet yi = pload <Packet>(y+peelingEnd);
390  pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
391  pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
392  }
393  }
394 
395  for(Index i=alignedEnd; i<size; ++i)
396  {
397  Scalar xi = x[i];
398  Scalar yi = y[i];
399  x[i] = c * xi + numext::conj(s) * yi;
400  y[i] = -s * xi + numext::conj(c) * yi;
401  }
402  }
403 
404  /*** fixed-size vectorized path ***/
405  else if(SizeAtCompileTime != Dynamic && MinAlignment>0) // FIXME should be compared to the required alignment
406  {
407  const OtherPacket pc = pset1<OtherPacket>(c);
408  const OtherPacket ps = pset1<OtherPacket>(s);
409  conj_helper<OtherPacket,Packet,NumTraits<OtherPacket>::IsComplex,false> pcj;
410  conj_helper<OtherPacket,Packet,false,false> pm;
411  Scalar* EIGEN_RESTRICT px = x;
412  Scalar* EIGEN_RESTRICT py = y;
413  for(Index i=0; i<size; i+=PacketSize)
414  {
415  Packet xi = pload<Packet>(px);
416  Packet yi = pload<Packet>(py);
417  pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
418  pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
419  px += PacketSize;
420  py += PacketSize;
421  }
422  }
423 
424  /*** non-vectorized path ***/
425  else
426  {
427  apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,false>::run(x,incrx,y,incry,size,c,s);
428  }
429  }
430 };
431 
432 template<typename VectorX, typename VectorY, typename OtherScalar>
433 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
434 {
435  typedef typename VectorX::Scalar Scalar;
436  const bool Vectorizable = (VectorX::Flags & VectorY::Flags & PacketAccessBit)
437  && (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
438 
439  eigen_assert(xpr_x.size() == xpr_y.size());
440  Index size = xpr_x.size();
441  Index incrx = xpr_x.derived().innerStride();
442  Index incry = xpr_y.derived().innerStride();
443 
444  Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
445  Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0);
446 
447  OtherScalar c = j.c();
448  OtherScalar s = j.s();
449  if (c==OtherScalar(1) && s==OtherScalar(0))
450  return;
451 
452  apply_rotation_in_the_plane_selector<
453  Scalar,OtherScalar,
454  VectorX::SizeAtCompileTime,
455  EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment),
456  Vectorizable>::run(x,incrx,y,incry,size,c,s);
457 }
458 
459 } // end namespace internal
460 
461 } // end namespace Eigen
462 
463 #endif // EIGEN_JACOBI_H
JacobiRotation operator*(const JacobiRotation &other)
Definition: Jacobi.h:51
void applyOnTheLeft(const EigenBase< OtherDerived > &other)
Definition: MatrixBase.h:514
void makeGivens(const Scalar &p, const Scalar &q, Scalar *z=0)
Definition: Jacobi.h:149
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:502
Namespace containing all symbols from the Eigen library.
Definition: Core:306
Rotation given by a cosine-sine pair.
Definition: ForwardDeclarations.h:263
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:127
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