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 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 57 /** Returns the transposed transformation */ 58 JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); } 59 60 /** Returns the adjoint transformation */ 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 76/** 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 77 * \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$ 78 * 79 * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 80 */ 81template<typename Scalar> 82bool 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 112/** 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 113 * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields 114 * a diagonal matrix \f$ A = J^* B J \f$ 115 * 116 * Example: \include Jacobi_makeJacobi.cpp 117 * Output: \verbinclude Jacobi_makeJacobi.out 118 * 119 * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 120 */ 121template<typename Scalar> 122template<typename Derived> 123inline 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 128/** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector 129 * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields: 130 * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$. 131 * 132 * The value of \a z is returned if \a z is not null (the default is null). 133 * Also note that G is built such that the cosine is always real. 134 * 135 * Example: \include Jacobi_makeGivens.cpp 136 * Output: \verbinclude Jacobi_makeGivens.out 137 * 138 * This function implements the continuous Givens rotation generation algorithm 139 * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem. 140 * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000. 141 * 142 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 143 */ 144template<typename Scalar> 145void 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 152template<typename Scalar> 153void 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 207template<typename Scalar> 208void 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 250/** \jacobi_module 251 * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y: 252 * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$ 253 * 254 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 255 */ 256namespace internal { 257template<typename VectorX, typename VectorY, typename OtherScalar> 258void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j); 259} 260 261/** \jacobi_module 262 * 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, 263 * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$. 264 * 265 * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane() 266 */ 267template<typename Derived> 268template<typename OtherScalar> 269inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j) 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 276/** \ingroup Jacobi_Module 277 * 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 278 * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$. 279 * 280 * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane() 281 */ 282template<typename Derived> 283template<typename OtherScalar> 284inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j) 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 291namespace internal { 292template<typename VectorX, typename VectorY, typename OtherScalar> 293void /*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 421