Jacobi.h
Go to the documentation of this file.
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:
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 
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>&, 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 
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, 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 
147 template<typename Scalar>
148 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
149 {
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 
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 
275 template<typename Derived>
276 template<typename OtherScalar>
278 {
279  RowXpr x(this->row(p));
280  RowXpr y(this->row(q));
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(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);
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);
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
Scalar & c()
Definition: Jacobi.h:45
const AutoDiffScalar< DerType > & conj(const AutoDiffScalar< DerType > &x)
IntermediateState sqrt(const Expression &arg)
JacobiRotation operator*(const JacobiRotation &other)
Definition: Jacobi.h:51
Scalar c() const
Definition: Jacobi.h:46
void applyOnTheLeft(const EigenBase< OtherDerived > &other)
Definition: EigenBase.h:154
internal::traits< Derived >::Index Index
The type of indices.
Definition: DenseBase.h:61
void makeGivens(const Scalar &p, const Scalar &q, Scalar *z=0)
Definition: Jacobi.h:148
#define EIGEN_RESTRICT
JacobiRotation(const Scalar &c, const Scalar &s)
Definition: Jacobi.h:43
void applyOnTheRight(const EigenBase< OtherDerived > &other)
Definition: EigenBase.h:146
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
Rotation given by a cosine-sine pair.
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:88
JacobiRotation transpose() const
Definition: Jacobi.h:59
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs2_op< Scalar >, const Derived > abs2() const
const unsigned int PacketAccessBit
void pstore(Scalar *to, const Packet &from)
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs_op< Scalar >, const Derived > abs() const
RealReturnType real() const
const unsigned int AlignedBit
void pstoreu(Scalar *to, const Packet &from)
Packet psub(const Packet &a, const Packet &b)
bool makeJacobi(const MatrixBase< Derived > &, typename Derived::Index p, typename Derived::Index q)
Definition: Jacobi.h:126
JacobiRotation adjoint() const
Definition: Jacobi.h:62
NumTraits< Scalar >::Real RealScalar
Definition: Jacobi.h:37
Expression of a fixed-size or dynamic-size block.
Definition: Core/Block.h:102
RowXpr row(Index i)
Definition: BlockMethods.h:725
void apply_rotation_in_the_plane(VectorX &_x, VectorY &_y, const JacobiRotation< OtherScalar > &j)
Definition: Jacobi.h:301
Scalar & s()
Definition: Jacobi.h:47
Packet pmul(const Packet &a, const Packet &b)
ColXpr col(Index i)
Definition: BlockMethods.h:708
static Derived::Index first_aligned(const Derived &m)
#define eigen_assert(x)
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Packet padd(const Packet &a, const Packet &b)
const T & y
Scalar s() const
Definition: Jacobi.h:48


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:44