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