compbio
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 {
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));
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 template<typename VectorX, typename VectorY, typename OtherScalar>
303 {
304  typedef typename VectorX::Scalar Scalar;
305  enum { PacketSize = packet_traits<Scalar>::size };
306  typedef typename packet_traits<Scalar>::type Packet;
307  eigen_assert(xpr_x.size() == xpr_y.size());
308  Index size = xpr_x.size();
309  Index incrx = xpr_x.derived().innerStride();
310  Index incry = xpr_y.derived().innerStride();
311 
312  Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
313  Scalar* EIGEN_RESTRICT y = &xpr_y.derived().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_default_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_default_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  (EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment)>0)) // FIXME should be compared to the required alignment
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
JacobiRotation operator*(const JacobiRotation &other)
Concatenates two planar rotation.
Definition: Jacobi.h:51
Definition: BlasUtil.h:61
Definition: Meta.h:55
void applyOnTheLeft(const EigenBase< OtherDerived > &other)
replaces *this by other * *this.
Definition: MatrixBase.h:523
void makeGivens(const Scalar &p, const Scalar &q, Scalar *z=0)
Makes *this as a Givens rotation G such that applying to the left of the vector yields: ...
Definition: Jacobi.h:149
Definition: CoreEvaluators.h:90
JacobiRotation(const Scalar &c, const Scalar &s)
Construct a planar rotation from a cosine-sine pair (c, s).
Definition: Jacobi.h:43
void applyOnTheRight(const EigenBase< OtherDerived > &other)
replaces *this by *this * other.
Definition: MatrixBase.h:511
Namespace containing all symbols from the Eigen library.
Definition: bench_norm.cpp:85
Definition: ForwardDeclarations.h:263
Holds information about the various numeric (i.e.
Definition: NumTraits.h:150
Definition: Meta.h:58
bool makeJacobi(const MatrixBase< Derived > &, Index p, Index q)
Makes *this as a Jacobi rotation J such that applying J on both the right and left sides of the 2x2 s...
Definition: Jacobi.h:127
Base class for all dense matrices, vectors, and arrays.
Definition: DenseBase.h:41
const unsigned int PacketAccessBit
Short version: means the expression might be vectorized.
Definition: Constants.h:89
Definition: GenericPacketMath.h:96
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:33
Definition: PacketMath.h:48
Definition: BandTriangularSolver.h:13
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
JacobiRotation transpose() const
Returns the transposed transformation.
Definition: Jacobi.h:59
void apply_rotation_in_the_plane(DenseBase< VectorX > &xpr_x, DenseBase< VectorY > &xpr_y, const JacobiRotation< OtherScalar > &j)
Applies the clock wise 2D rotation j to the set of 2D vectors of cordinates x and y: ...
Definition: Jacobi.h:302
const int Dynamic
This value means that a positive quantity (e.g., a size) is not known at compile-time, and that instead the value is stored in some runtime variable.
Definition: Constants.h:21
Definition: Meta.h:54
JacobiRotation()
Default constructor without any initialization.
Definition: Jacobi.h:40
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
JacobiRotation adjoint() const
Returns the adjoint transformation.
Definition: Jacobi.h:62