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
16 /** \ingroup Jacobi_Module
17 * \jacobi_module
18 * \class JacobiRotation
19 * \brief Rotation given by a cosine-sine pair.
20 *
21 * This class represents a Jacobi or Givens rotation.
22 * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
23 * its cosine \c c and sine \c s as follow:
24 * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} \right ) \f$
25 *
26 * You can apply the respective counter-clockwise rotation to a column vector \c v by
27 * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
28 * \code
29 * v.applyOnTheLeft(J.adjoint());
30 * \endcode
31 *
32 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
33 */
34 template<typename Scalar> class JacobiRotation
35 {
36 public:
37 typedef typename NumTraits<Scalar>::Real RealScalar;
38
39 /** Default constructor without any initialization. */
JacobiRotation()40 JacobiRotation() {}
41
42 /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
JacobiRotation(const Scalar & c,const Scalar & s)43 JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
44
c()45 Scalar& c() { return m_c; }
c()46 Scalar c() const { return m_c; }
s()47 Scalar& s() { return m_s; }
s()48 Scalar s() const { return m_s; }
49
50 /** Concatenates two planar rotation */
51 JacobiRotation operator*(const JacobiRotation& other)
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
58 /** Returns the transposed transformation */
transpose()59 JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
60
61 /** Returns the adjoint transformation */
adjoint()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
77 /** Makes \c *this as a Jacobi rotation \a J such that applying \a J on both the right and left sides of the selfadjoint 2x2 matrix
78 * \f$ B = \left ( \begin{array}{cc} x & y \\ \overline y & z \end{array} \right )\f$ yields a diagonal matrix \f$ A = J^* B J \f$
79 *
80 * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
81 */
82 template<typename Scalar>
makeJacobi(const RealScalar & x,const Scalar & y,const RealScalar & z)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
115 /** Makes \c *this as a Jacobi rotation \c J such that applying \a J on both the right and left sides of the 2x2 selfadjoint matrix
116 * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
117 * a diagonal matrix \f$ A = J^* B J \f$
118 *
119 * Example: \include Jacobi_makeJacobi.cpp
120 * Output: \verbinclude Jacobi_makeJacobi.out
121 *
122 * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
123 */
124 template<typename Scalar>
125 template<typename Derived>
makeJacobi(const MatrixBase<Derived> & m,typename Derived::Index p,typename Derived::Index q)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
131 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
132 * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
133 * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
134 *
135 * The value of \a z is returned if \a z is not null (the default is null).
136 * Also note that G is built such that the cosine is always real.
137 *
138 * Example: \include Jacobi_makeGivens.cpp
139 * Output: \verbinclude Jacobi_makeGivens.out
140 *
141 * This function implements the continuous Givens rotation generation algorithm
142 * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
143 * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
144 *
145 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
146 */
147 template<typename Scalar>
makeGivens(const Scalar & p,const Scalar & q,Scalar * z)148 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
149 {
150 makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
151 }
152
153
154 // specialization for complexes
155 template<typename Scalar>
makeGivens(const Scalar & p,const Scalar & q,Scalar * r,internal::true_type)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>
makeGivens(const Scalar & p,const Scalar & q,Scalar * r,internal::false_type)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 /** \jacobi_module
259 * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
260 * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
261 *
262 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
263 */
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
269 /** \jacobi_module
270 * Applies the rotation in the plane \a j to the rows \a p and \a q of \c *this, i.e., it computes B = J * B,
271 * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
272 *
273 * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
274 */
275 template<typename Derived>
276 template<typename OtherScalar>
applyOnTheLeft(Index p,Index q,const JacobiRotation<OtherScalar> & j)277 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
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
284 /** \ingroup Jacobi_Module
285 * Applies the rotation in the plane \a j to the columns \a p and \a q of \c *this, i.e., it computes B = B * J
286 * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
287 *
288 * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
289 */
290 template<typename Derived>
291 template<typename OtherScalar>
applyOnTheRight(Index p,Index q,const JacobiRotation<OtherScalar> & j)292 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
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>
apply_rotation_in_the_plane(VectorX & _x,VectorY & _y,const JacobiRotation<OtherScalar> & j)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);
334 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
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);
400 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
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
434