Jacobi.h (16383B)
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. */ 40 EIGEN_DEVICE_FUNC 41 JacobiRotation() {} 42 43 /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */ 44 EIGEN_DEVICE_FUNC 45 JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {} 46 47 EIGEN_DEVICE_FUNC Scalar& c() { return m_c; } 48 EIGEN_DEVICE_FUNC Scalar c() const { return m_c; } 49 EIGEN_DEVICE_FUNC Scalar& s() { return m_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 63 JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); } 64 65 /** Returns the adjoint transformation */ 66 EIGEN_DEVICE_FUNC 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 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 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 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 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 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 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 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 325 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