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  return JacobiRotation(m_c * other.m_c - internal::conj(m_s) * other.m_s,
54  internal::conj(m_c * internal::conj(other.m_s) + internal::conj(m_s) * internal::conj(other.m_c)));
55  }
56 
58  JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); }
59 
61  JacobiRotation adjoint() const { return JacobiRotation(internal::conj(m_c), -m_s); }
62 
63  template<typename Derived>
64  bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
65  bool makeJacobi(RealScalar x, Scalar y, RealScalar z);
66 
67  void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
68 
69  protected:
70  void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
71  void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
72 
73  Scalar m_c, m_s;
74 };
75 
81 template<typename Scalar>
82 bool JacobiRotation<Scalar>::makeJacobi(RealScalar x, Scalar y, RealScalar z)
83 {
84  typedef typename NumTraits<Scalar>::Real RealScalar;
85  if(y == Scalar(0))
86  {
87  m_c = Scalar(1);
88  m_s = Scalar(0);
89  return false;
90  }
91  else
92  {
93  RealScalar tau = (x-z)/(RealScalar(2)*internal::abs(y));
94  RealScalar w = internal::sqrt(internal::abs2(tau) + RealScalar(1));
95  RealScalar t;
96  if(tau>RealScalar(0))
97  {
98  t = RealScalar(1) / (tau + w);
99  }
100  else
101  {
102  t = RealScalar(1) / (tau - w);
103  }
104  RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
105  RealScalar n = RealScalar(1) / internal::sqrt(internal::abs2(t)+RealScalar(1));
106  m_s = - sign_t * (internal::conj(y) / internal::abs(y)) * internal::abs(t) * n;
107  m_c = n;
108  return true;
109  }
110 }
111 
121 template<typename Scalar>
122 template<typename Derived>
123 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
124 {
125  return makeJacobi(internal::real(m.coeff(p,p)), m.coeff(p,q), internal::real(m.coeff(q,q)));
126 }
127 
144 template<typename Scalar>
145 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
146 {
147  makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
148 }
149 
150 
151 // specialization for complexes
152 template<typename Scalar>
153 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
154 {
155  if(q==Scalar(0))
156  {
157  m_c = internal::real(p)<0 ? Scalar(-1) : Scalar(1);
158  m_s = 0;
159  if(r) *r = m_c * p;
160  }
161  else if(p==Scalar(0))
162  {
163  m_c = 0;
164  m_s = -q/internal::abs(q);
165  if(r) *r = internal::abs(q);
166  }
167  else
168  {
169  RealScalar p1 = internal::norm1(p);
170  RealScalar q1 = internal::norm1(q);
171  if(p1>=q1)
172  {
173  Scalar ps = p / p1;
174  RealScalar p2 = internal::abs2(ps);
175  Scalar qs = q / p1;
176  RealScalar q2 = internal::abs2(qs);
177 
178  RealScalar u = internal::sqrt(RealScalar(1) + q2/p2);
179  if(internal::real(p)<RealScalar(0))
180  u = -u;
181 
182  m_c = Scalar(1)/u;
183  m_s = -qs*internal::conj(ps)*(m_c/p2);
184  if(r) *r = p * u;
185  }
186  else
187  {
188  Scalar ps = p / q1;
189  RealScalar p2 = internal::abs2(ps);
190  Scalar qs = q / q1;
191  RealScalar q2 = internal::abs2(qs);
192 
193  RealScalar u = q1 * internal::sqrt(p2 + q2);
194  if(internal::real(p)<RealScalar(0))
195  u = -u;
196 
197  p1 = internal::abs(p);
198  ps = p/p1;
199  m_c = p1/u;
200  m_s = -internal::conj(ps) * (q/u);
201  if(r) *r = ps * u;
202  }
203  }
204 }
205 
206 // specialization for reals
207 template<typename Scalar>
208 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
209 {
210 
211  if(q==Scalar(0))
212  {
213  m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
214  m_s = Scalar(0);
215  if(r) *r = internal::abs(p);
216  }
217  else if(p==Scalar(0))
218  {
219  m_c = Scalar(0);
220  m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
221  if(r) *r = internal::abs(q);
222  }
223  else if(internal::abs(p) > internal::abs(q))
224  {
225  Scalar t = q/p;
226  Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
227  if(p<Scalar(0))
228  u = -u;
229  m_c = Scalar(1)/u;
230  m_s = -t * m_c;
231  if(r) *r = p * u;
232  }
233  else
234  {
235  Scalar t = p/q;
236  Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
237  if(q<Scalar(0))
238  u = -u;
239  m_s = -Scalar(1)/u;
240  m_c = -t * m_s;
241  if(r) *r = q * u;
242  }
243 
244 }
245 
246 /****************************************************************************************
247 * Implementation of MatrixBase methods
248 ****************************************************************************************/
249 
256 namespace internal {
257 template<typename VectorX, typename VectorY, typename OtherScalar>
258 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
259 }
260 
267 template<typename Derived>
268 template<typename OtherScalar>
270 {
271  RowXpr x(this->row(p));
272  RowXpr y(this->row(q));
273  internal::apply_rotation_in_the_plane(x, y, j);
274 }
275 
282 template<typename Derived>
283 template<typename OtherScalar>
285 {
286  ColXpr x(this->col(p));
287  ColXpr y(this->col(q));
288  internal::apply_rotation_in_the_plane(x, y, j.transpose());
289 }
290 
291 namespace internal {
292 template<typename VectorX, typename VectorY, typename OtherScalar>
293 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
294 {
295  typedef typename VectorX::Index Index;
296  typedef typename VectorX::Scalar Scalar;
297  enum { PacketSize = packet_traits<Scalar>::size };
298  typedef typename packet_traits<Scalar>::type Packet;
299  eigen_assert(_x.size() == _y.size());
300  Index size = _x.size();
301  Index incrx = _x.innerStride();
302  Index incry = _y.innerStride();
303 
304  Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
305  Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
306 
307  /*** dynamic-size vectorized paths ***/
308 
309  if(VectorX::SizeAtCompileTime == Dynamic &&
310  (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
311  ((incrx==1 && incry==1) || PacketSize == 1))
312  {
313  // both vectors are sequentially stored in memory => vectorization
314  enum { Peeling = 2 };
315 
316  Index alignedStart = internal::first_aligned(y, size);
317  Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
318 
319  const Packet pc = pset1<Packet>(j.c());
320  const Packet ps = pset1<Packet>(j.s());
321  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
322 
323  for(Index i=0; i<alignedStart; ++i)
324  {
325  Scalar xi = x[i];
326  Scalar yi = y[i];
327  x[i] = j.c() * xi + conj(j.s()) * yi;
328  y[i] = -j.s() * xi + conj(j.c()) * yi;
329  }
330 
331  Scalar* EIGEN_RESTRICT px = x + alignedStart;
332  Scalar* EIGEN_RESTRICT py = y + alignedStart;
333 
334  if(internal::first_aligned(x, size)==alignedStart)
335  {
336  for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
337  {
338  Packet xi = pload<Packet>(px);
339  Packet yi = pload<Packet>(py);
340  pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
341  pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
342  px += PacketSize;
343  py += PacketSize;
344  }
345  }
346  else
347  {
348  Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
349  for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
350  {
351  Packet xi = ploadu<Packet>(px);
352  Packet xi1 = ploadu<Packet>(px+PacketSize);
353  Packet yi = pload <Packet>(py);
354  Packet yi1 = pload <Packet>(py+PacketSize);
355  pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
356  pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
357  pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
358  pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
359  px += Peeling*PacketSize;
360  py += Peeling*PacketSize;
361  }
362  if(alignedEnd!=peelingEnd)
363  {
364  Packet xi = ploadu<Packet>(x+peelingEnd);
365  Packet yi = pload <Packet>(y+peelingEnd);
366  pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
367  pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
368  }
369  }
370 
371  for(Index i=alignedEnd; i<size; ++i)
372  {
373  Scalar xi = x[i];
374  Scalar yi = y[i];
375  x[i] = j.c() * xi + conj(j.s()) * yi;
376  y[i] = -j.s() * xi + conj(j.c()) * yi;
377  }
378  }
379 
380  /*** fixed-size vectorized path ***/
381  else if(VectorX::SizeAtCompileTime != Dynamic &&
382  (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
383  (VectorX::Flags & VectorY::Flags & AlignedBit))
384  {
385  const Packet pc = pset1<Packet>(j.c());
386  const Packet ps = pset1<Packet>(j.s());
387  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
388  Scalar* EIGEN_RESTRICT px = x;
389  Scalar* EIGEN_RESTRICT py = y;
390  for(Index i=0; i<size; i+=PacketSize)
391  {
392  Packet xi = pload<Packet>(px);
393  Packet yi = pload<Packet>(py);
394  pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
395  pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
396  px += PacketSize;
397  py += PacketSize;
398  }
399  }
400 
401  /*** non-vectorized path ***/
402  else
403  {
404  for(Index i=0; i<size; ++i)
405  {
406  Scalar xi = *x;
407  Scalar yi = *y;
408  *x = j.c() * xi + conj(j.s()) * yi;
409  *y = -j.s() * xi + conj(j.c()) * yi;
410  x += incrx;
411  y += incry;
412  }
413  }
414 }
415 
416 } // end namespace internal
417 
418 } // end namespace Eigen
419 
420 #endif // EIGEN_JACOBI_H