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
14namespace 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 */
34template<typename Scalar> class JacobiRotation
35{
36 public:
37 typedef typename NumTraits<Scalar>::Real RealScalar;
38
39 /** Default constructor without any initialization. */
40 JacobiRotation() {}
41
42 /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
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
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 */
59 JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
60
61 /** Returns the adjoint transformation */
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* r=0);
69
70 protected:
71 void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
72 void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, 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 */
82template<typename Scalar>
83bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z)
84{
85 using std::sqrt;
86 using std::abs;
87 RealScalar deno = RealScalar(2)*abs(y);
88 if(deno < (std::numeric_limits<RealScalar>::min)())
89 {
90 m_c = Scalar(1);
91 m_s = Scalar(0);
92 return false;
93 }
94 else
95 {
96 RealScalar tau = (x-z)/deno;
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 */
124template<typename Scalar>
125template<typename Derived>
126inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, 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 r is returned if \a r 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 */
147template<typename Scalar>
148void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r)
149{
150 makeGivens(p, q, r, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
151}
152
153
154// specialization for complexes
155template<typename Scalar>
156void 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
214template<typename Scalar>
215void 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
258namespace internal {
259/** \jacobi_module
260 * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
261 * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
262 *
263 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
264 */
265template<typename VectorX, typename VectorY, typename OtherScalar>
266void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_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 */
275template<typename Derived>
276template<typename OtherScalar>
277inline 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 */
290template<typename Derived>
291template<typename OtherScalar>
292inline 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
299namespace internal {
300
301template<typename Scalar, typename OtherScalar,
302 int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
303struct apply_rotation_in_the_plane_selector
304{
305 static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
306 {
307 for(Index i=0; i<size; ++i)
308 {
309 Scalar xi = *x;
310 Scalar yi = *y;
311 *x = c * xi + numext::conj(s) * yi;
312 *y = -s * xi + numext::conj(c) * yi;
313 x += incrx;
314 y += incry;
315 }
316 }
317};
318
319template<typename Scalar, typename OtherScalar,
320 int SizeAtCompileTime, int MinAlignment>
321struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */>
322{
323 static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
324 {
325 enum {
326 PacketSize = packet_traits<Scalar>::size,
327 OtherPacketSize = packet_traits<OtherScalar>::size
328 };
329 typedef typename packet_traits<Scalar>::type Packet;
330 typedef typename packet_traits<OtherScalar>::type OtherPacket;
331
332 /*** dynamic-size vectorized paths ***/
333 if(SizeAtCompileTime == Dynamic && ((incrx==1 && incry==1) || PacketSize == 1))
334 {
335 // both vectors are sequentially stored in memory => vectorization
336 enum { Peeling = 2 };
337
338 Index alignedStart = internal::first_default_aligned(y, size);
339 Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
340
341 const OtherPacket pc = pset1<OtherPacket>(c);
342 const OtherPacket ps = pset1<OtherPacket>(s);
343 conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj;
344 conj_helper<OtherPacket,Packet,false,false> pm;
345
346 for(Index i=0; i<alignedStart; ++i)
347 {
348 Scalar xi = x[i];
349 Scalar yi = y[i];
350 x[i] = c * xi + numext::conj(s) * yi;
351 y[i] = -s * xi + numext::conj(c) * yi;
352 }
353
354 Scalar* EIGEN_RESTRICT px = x + alignedStart;
355 Scalar* EIGEN_RESTRICT py = y + alignedStart;
356
357 if(internal::first_default_aligned(x, size)==alignedStart)
358 {
359 for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
360 {
361 Packet xi = pload<Packet>(px);
362 Packet yi = pload<Packet>(py);
363 pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
364 pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
365 px += PacketSize;
366 py += PacketSize;
367 }
368 }
369 else
370 {
371 Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
372 for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
373 {
374 Packet xi = ploadu<Packet>(px);
375 Packet xi1 = ploadu<Packet>(px+PacketSize);
376 Packet yi = pload <Packet>(py);
377 Packet yi1 = pload <Packet>(py+PacketSize);
378 pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
379 pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
380 pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
381 pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
382 px += Peeling*PacketSize;
383 py += Peeling*PacketSize;
384 }
385 if(alignedEnd!=peelingEnd)
386 {
387 Packet xi = ploadu<Packet>(x+peelingEnd);
388 Packet yi = pload <Packet>(y+peelingEnd);
389 pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
390 pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
391 }
392 }
393
394 for(Index i=alignedEnd; i<size; ++i)
395 {
396 Scalar xi = x[i];
397 Scalar yi = y[i];
398 x[i] = c * xi + numext::conj(s) * yi;
399 y[i] = -s * xi + numext::conj(c) * yi;
400 }
401 }
402
403 /*** fixed-size vectorized path ***/
404 else if(SizeAtCompileTime != Dynamic && MinAlignment>0) // FIXME should be compared to the required alignment
405 {
406 const OtherPacket pc = pset1<OtherPacket>(c);
407 const OtherPacket ps = pset1<OtherPacket>(s);
408 conj_helper<OtherPacket,Packet,NumTraits<OtherPacket>::IsComplex,false> pcj;
409 conj_helper<OtherPacket,Packet,false,false> pm;
410 Scalar* EIGEN_RESTRICT px = x;
411 Scalar* EIGEN_RESTRICT py = y;
412 for(Index i=0; i<size; i+=PacketSize)
413 {
414 Packet xi = pload<Packet>(px);
415 Packet yi = pload<Packet>(py);
416 pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
417 pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
418 px += PacketSize;
419 py += PacketSize;
420 }
421 }
422
423 /*** non-vectorized path ***/
424 else
425 {
426 apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,false>::run(x,incrx,y,incry,size,c,s);
427 }
428 }
429};
430
431template<typename VectorX, typename VectorY, typename OtherScalar>
432void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
433{
434 typedef typename VectorX::Scalar Scalar;
435 const bool Vectorizable = (VectorX::Flags & VectorY::Flags & PacketAccessBit)
436 && (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
437
438 eigen_assert(xpr_x.size() == xpr_y.size());
439 Index size = xpr_x.size();
440 Index incrx = xpr_x.derived().innerStride();
441 Index incry = xpr_y.derived().innerStride();
442
443 Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
444 Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0);
445
446 OtherScalar c = j.c();
447 OtherScalar s = j.s();
448 if (c==OtherScalar(1) && s==OtherScalar(0))
449 return;
450
451 apply_rotation_in_the_plane_selector<
452 Scalar,OtherScalar,
453 VectorX::SizeAtCompileTime,
454 EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment),
455 Vectorizable>::run(x,incrx,y,incry,size,c,s);
456}
457
458} // end namespace internal
459
460} // end namespace Eigen
461
462#endif // EIGEN_JACOBI_H
463