1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Benoit Jacob <[email protected]>
5 // Copyright (C) 2009 Gael Guennebaud <[email protected]>
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. */
40 EIGEN_DEVICE_FUNC
JacobiRotation()41 JacobiRotation() {}
42
43 /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
44 EIGEN_DEVICE_FUNC
JacobiRotation(const Scalar & c,const Scalar & s)45 JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
46
c()47 EIGEN_DEVICE_FUNC Scalar& c() { return m_c; }
c()48 EIGEN_DEVICE_FUNC Scalar c() const { return m_c; }
s()49 EIGEN_DEVICE_FUNC Scalar& s() { return m_s; }
s()50 EIGEN_DEVICE_FUNC Scalar s() const { return m_s; }
51
52 /** Concatenates two planar rotation */
53 EIGEN_DEVICE_FUNC
54 JacobiRotation operator*(const JacobiRotation& other)
55 {
56 using numext::conj;
57 return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s,
58 conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c)));
59 }
60
61 /** Returns the transposed transformation */
62 EIGEN_DEVICE_FUNC
transpose()63 JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
64
65 /** Returns the adjoint transformation */
66 EIGEN_DEVICE_FUNC
adjoint()67 JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); }
68
69 template<typename Derived>
70 EIGEN_DEVICE_FUNC
71 bool makeJacobi(const MatrixBase<Derived>&, Index p, Index q);
72 EIGEN_DEVICE_FUNC
73 bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
74
75 EIGEN_DEVICE_FUNC
76 void makeGivens(const Scalar& p, const Scalar& q, Scalar* r=0);
77
78 protected:
79 EIGEN_DEVICE_FUNC
80 void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
81 EIGEN_DEVICE_FUNC
82 void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type);
83
84 Scalar m_c, m_s;
85 };
86
87 /** 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
88 * \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$
89 *
90 * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
91 */
92 template<typename Scalar>
93 EIGEN_DEVICE_FUNC
makeJacobi(const RealScalar & x,const Scalar & y,const RealScalar & z)94 bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z)
95 {
96 using std::sqrt;
97 using std::abs;
98
99 RealScalar deno = RealScalar(2)*abs(y);
100 if(deno < (std::numeric_limits<RealScalar>::min)())
101 {
102 m_c = Scalar(1);
103 m_s = Scalar(0);
104 return false;
105 }
106 else
107 {
108 RealScalar tau = (x-z)/deno;
109 RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
110 RealScalar t;
111 if(tau>RealScalar(0))
112 {
113 t = RealScalar(1) / (tau + w);
114 }
115 else
116 {
117 t = RealScalar(1) / (tau - w);
118 }
119 RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
120 RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
121 m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
122 m_c = n;
123 return true;
124 }
125 }
126
127 /** 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
128 * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
129 * a diagonal matrix \f$ A = J^* B J \f$
130 *
131 * Example: \include Jacobi_makeJacobi.cpp
132 * Output: \verbinclude Jacobi_makeJacobi.out
133 *
134 * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
135 */
136 template<typename Scalar>
137 template<typename Derived>
138 EIGEN_DEVICE_FUNC
makeJacobi(const MatrixBase<Derived> & m,Index p,Index q)139 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, Index q)
140 {
141 return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
142 }
143
144 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
145 * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
146 * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
147 *
148 * The value of \a r is returned if \a r is not null (the default is null).
149 * Also note that G is built such that the cosine is always real.
150 *
151 * Example: \include Jacobi_makeGivens.cpp
152 * Output: \verbinclude Jacobi_makeGivens.out
153 *
154 * This function implements the continuous Givens rotation generation algorithm
155 * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
156 * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
157 *
158 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
159 */
160 template<typename Scalar>
161 EIGEN_DEVICE_FUNC
makeGivens(const Scalar & p,const Scalar & q,Scalar * r)162 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r)
163 {
164 makeGivens(p, q, r, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
165 }
166
167
168 // specialization for complexes
169 template<typename Scalar>
170 EIGEN_DEVICE_FUNC
makeGivens(const Scalar & p,const Scalar & q,Scalar * r,internal::true_type)171 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
172 {
173 using std::sqrt;
174 using std::abs;
175 using numext::conj;
176
177 if(q==Scalar(0))
178 {
179 m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
180 m_s = 0;
181 if(r) *r = m_c * p;
182 }
183 else if(p==Scalar(0))
184 {
185 m_c = 0;
186 m_s = -q/abs(q);
187 if(r) *r = abs(q);
188 }
189 else
190 {
191 RealScalar p1 = numext::norm1(p);
192 RealScalar q1 = numext::norm1(q);
193 if(p1>=q1)
194 {
195 Scalar ps = p / p1;
196 RealScalar p2 = numext::abs2(ps);
197 Scalar qs = q / p1;
198 RealScalar q2 = numext::abs2(qs);
199
200 RealScalar u = sqrt(RealScalar(1) + q2/p2);
201 if(numext::real(p)<RealScalar(0))
202 u = -u;
203
204 m_c = Scalar(1)/u;
205 m_s = -qs*conj(ps)*(m_c/p2);
206 if(r) *r = p * u;
207 }
208 else
209 {
210 Scalar ps = p / q1;
211 RealScalar p2 = numext::abs2(ps);
212 Scalar qs = q / q1;
213 RealScalar q2 = numext::abs2(qs);
214
215 RealScalar u = q1 * sqrt(p2 + q2);
216 if(numext::real(p)<RealScalar(0))
217 u = -u;
218
219 p1 = abs(p);
220 ps = p/p1;
221 m_c = p1/u;
222 m_s = -conj(ps) * (q/u);
223 if(r) *r = ps * u;
224 }
225 }
226 }
227
228 // specialization for reals
229 template<typename Scalar>
230 EIGEN_DEVICE_FUNC
makeGivens(const Scalar & p,const Scalar & q,Scalar * r,internal::false_type)231 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
232 {
233 using std::sqrt;
234 using std::abs;
235 if(q==Scalar(0))
236 {
237 m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
238 m_s = Scalar(0);
239 if(r) *r = abs(p);
240 }
241 else if(p==Scalar(0))
242 {
243 m_c = Scalar(0);
244 m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
245 if(r) *r = abs(q);
246 }
247 else if(abs(p) > abs(q))
248 {
249 Scalar t = q/p;
250 Scalar u = sqrt(Scalar(1) + numext::abs2(t));
251 if(p<Scalar(0))
252 u = -u;
253 m_c = Scalar(1)/u;
254 m_s = -t * m_c;
255 if(r) *r = p * u;
256 }
257 else
258 {
259 Scalar t = p/q;
260 Scalar u = sqrt(Scalar(1) + numext::abs2(t));
261 if(q<Scalar(0))
262 u = -u;
263 m_s = -Scalar(1)/u;
264 m_c = -t * m_s;
265 if(r) *r = q * u;
266 }
267
268 }
269
270 /****************************************************************************************
271 * Implementation of MatrixBase methods
272 ****************************************************************************************/
273
274 namespace internal {
275 /** \jacobi_module
276 * Applies the clock wise 2D rotation \a j to the set of 2D vectors of coordinates \a x and \a y:
277 * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
278 *
279 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
280 */
281 template<typename VectorX, typename VectorY, typename OtherScalar>
282 EIGEN_DEVICE_FUNC
283 void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j);
284 }
285
286 /** \jacobi_module
287 * 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,
288 * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
289 *
290 * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
291 */
292 template<typename Derived>
293 template<typename OtherScalar>
294 EIGEN_DEVICE_FUNC
applyOnTheLeft(Index p,Index q,const JacobiRotation<OtherScalar> & j)295 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
296 {
297 RowXpr x(this->row(p));
298 RowXpr y(this->row(q));
299 internal::apply_rotation_in_the_plane(x, y, j);
300 }
301
302 /** \ingroup Jacobi_Module
303 * 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
304 * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
305 *
306 * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
307 */
308 template<typename Derived>
309 template<typename OtherScalar>
310 EIGEN_DEVICE_FUNC
applyOnTheRight(Index p,Index q,const JacobiRotation<OtherScalar> & j)311 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
312 {
313 ColXpr x(this->col(p));
314 ColXpr y(this->col(q));
315 internal::apply_rotation_in_the_plane(x, y, j.transpose());
316 }
317
318 namespace internal {
319
320 template<typename Scalar, typename OtherScalar,
321 int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
322 struct apply_rotation_in_the_plane_selector
323 {
324 static EIGEN_DEVICE_FUNC
runapply_rotation_in_the_plane_selector325 inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
326 {
327 for(Index i=0; i<size; ++i)
328 {
329 Scalar xi = *x;
330 Scalar yi = *y;
331 *x = c * xi + numext::conj(s) * yi;
332 *y = -s * xi + numext::conj(c) * yi;
333 x += incrx;
334 y += incry;
335 }
336 }
337 };
338
339 template<typename Scalar, typename OtherScalar,
340 int SizeAtCompileTime, int MinAlignment>
341 struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */>
342 {
343 static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
344 {
345 enum {
346 PacketSize = packet_traits<Scalar>::size,
347 OtherPacketSize = packet_traits<OtherScalar>::size
348 };
349 typedef typename packet_traits<Scalar>::type Packet;
350 typedef typename packet_traits<OtherScalar>::type OtherPacket;
351
352 /*** dynamic-size vectorized paths ***/
353 if(SizeAtCompileTime == Dynamic && ((incrx==1 && incry==1) || PacketSize == 1))
354 {
355 // both vectors are sequentially stored in memory => vectorization
356 enum { Peeling = 2 };
357
358 Index alignedStart = internal::first_default_aligned(y, size);
359 Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
360
361 const OtherPacket pc = pset1<OtherPacket>(c);
362 const OtherPacket ps = pset1<OtherPacket>(s);
363 conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj;
364 conj_helper<OtherPacket,Packet,false,false> pm;
365
366 for(Index i=0; i<alignedStart; ++i)
367 {
368 Scalar xi = x[i];
369 Scalar yi = y[i];
370 x[i] = c * xi + numext::conj(s) * yi;
371 y[i] = -s * xi + numext::conj(c) * yi;
372 }
373
374 Scalar* EIGEN_RESTRICT px = x + alignedStart;
375 Scalar* EIGEN_RESTRICT py = y + alignedStart;
376
377 if(internal::first_default_aligned(x, size)==alignedStart)
378 {
379 for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
380 {
381 Packet xi = pload<Packet>(px);
382 Packet yi = pload<Packet>(py);
383 pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
384 pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
385 px += PacketSize;
386 py += PacketSize;
387 }
388 }
389 else
390 {
391 Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
392 for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
393 {
394 Packet xi = ploadu<Packet>(px);
395 Packet xi1 = ploadu<Packet>(px+PacketSize);
396 Packet yi = pload <Packet>(py);
397 Packet yi1 = pload <Packet>(py+PacketSize);
398 pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
399 pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
400 pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
401 pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
402 px += Peeling*PacketSize;
403 py += Peeling*PacketSize;
404 }
405 if(alignedEnd!=peelingEnd)
406 {
407 Packet xi = ploadu<Packet>(x+peelingEnd);
408 Packet yi = pload <Packet>(y+peelingEnd);
409 pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
410 pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
411 }
412 }
413
414 for(Index i=alignedEnd; i<size; ++i)
415 {
416 Scalar xi = x[i];
417 Scalar yi = y[i];
418 x[i] = c * xi + numext::conj(s) * yi;
419 y[i] = -s * xi + numext::conj(c) * yi;
420 }
421 }
422
423 /*** fixed-size vectorized path ***/
424 else if(SizeAtCompileTime != Dynamic && MinAlignment>0) // FIXME should be compared to the required alignment
425 {
426 const OtherPacket pc = pset1<OtherPacket>(c);
427 const OtherPacket ps = pset1<OtherPacket>(s);
428 conj_helper<OtherPacket,Packet,NumTraits<OtherPacket>::IsComplex,false> pcj;
429 conj_helper<OtherPacket,Packet,false,false> pm;
430 Scalar* EIGEN_RESTRICT px = x;
431 Scalar* EIGEN_RESTRICT py = y;
432 for(Index i=0; i<size; i+=PacketSize)
433 {
434 Packet xi = pload<Packet>(px);
435 Packet yi = pload<Packet>(py);
436 pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
437 pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
438 px += PacketSize;
439 py += PacketSize;
440 }
441 }
442
443 /*** non-vectorized path ***/
444 else
445 {
446 apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,false>::run(x,incrx,y,incry,size,c,s);
447 }
448 }
449 };
450
451 template<typename VectorX, typename VectorY, typename OtherScalar>
452 EIGEN_DEVICE_FUNC
453 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
454 {
455 typedef typename VectorX::Scalar Scalar;
456 const bool Vectorizable = (int(VectorX::Flags) & int(VectorY::Flags) & PacketAccessBit)
457 && (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
458
459 eigen_assert(xpr_x.size() == xpr_y.size());
460 Index size = xpr_x.size();
461 Index incrx = xpr_x.derived().innerStride();
462 Index incry = xpr_y.derived().innerStride();
463
464 Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
465 Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0);
466
467 OtherScalar c = j.c();
468 OtherScalar s = j.s();
469 if (c==OtherScalar(1) && s==OtherScalar(0))
470 return;
471
472 apply_rotation_in_the_plane_selector<
473 Scalar,OtherScalar,
474 VectorX::SizeAtCompileTime,
475 EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment),
476 Vectorizable>::run(x,incrx,y,incry,size,c,s);
477 }
478
479 } // end namespace internal
480
481 } // end namespace Eigen
482
483 #endif // EIGEN_JACOBI_H
484