Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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 | 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 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 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))); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 56 | } |
| 57 | |
| 58 | /** Returns the transposed transformation */ |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 59 | JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); } |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 60 | |
| 61 | /** Returns the adjoint transformation */ |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 62 | JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); } |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 63 | |
| 64 | template<typename Derived> |
| 65 | bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q); |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 66 | bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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> |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 83 | bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z) |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 84 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 85 | using std::sqrt; |
| 86 | using std::abs; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 87 | typedef typename NumTraits<Scalar>::Real RealScalar; |
| 88 | if(y == Scalar(0)) |
| 89 | { |
| 90 | m_c = Scalar(1); |
| 91 | m_s = Scalar(0); |
| 92 | return false; |
| 93 | } |
| 94 | else |
| 95 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 96 | RealScalar tau = (x-z)/(RealScalar(2)*abs(y)); |
| 97 | RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1)); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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); |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 108 | RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1)); |
| 109 | m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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 | */ |
| 124 | template<typename Scalar> |
| 125 | template<typename Derived> |
| 126 | inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q) |
| 127 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 128 | return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q))); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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 z is returned if \a z 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 | */ |
| 147 | template<typename Scalar> |
| 148 | void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z) |
| 149 | { |
| 150 | makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type()); |
| 151 | } |
| 152 | |
| 153 | |
| 154 | // specialization for complexes |
| 155 | template<typename Scalar> |
| 156 | void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type) |
| 157 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 158 | using std::sqrt; |
| 159 | using std::abs; |
| 160 | using numext::conj; |
| 161 | |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 162 | if(q==Scalar(0)) |
| 163 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 164 | m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 165 | m_s = 0; |
| 166 | if(r) *r = m_c * p; |
| 167 | } |
| 168 | else if(p==Scalar(0)) |
| 169 | { |
| 170 | m_c = 0; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 171 | m_s = -q/abs(q); |
| 172 | if(r) *r = abs(q); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 173 | } |
| 174 | else |
| 175 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 176 | RealScalar p1 = numext::norm1(p); |
| 177 | RealScalar q1 = numext::norm1(q); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 178 | if(p1>=q1) |
| 179 | { |
| 180 | Scalar ps = p / p1; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 181 | RealScalar p2 = numext::abs2(ps); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 182 | Scalar qs = q / p1; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 183 | RealScalar q2 = numext::abs2(qs); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 184 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 185 | RealScalar u = sqrt(RealScalar(1) + q2/p2); |
| 186 | if(numext::real(p)<RealScalar(0)) |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 187 | u = -u; |
| 188 | |
| 189 | m_c = Scalar(1)/u; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 190 | m_s = -qs*conj(ps)*(m_c/p2); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 191 | if(r) *r = p * u; |
| 192 | } |
| 193 | else |
| 194 | { |
| 195 | Scalar ps = p / q1; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 196 | RealScalar p2 = numext::abs2(ps); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 197 | Scalar qs = q / q1; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 198 | RealScalar q2 = numext::abs2(qs); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 199 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 200 | RealScalar u = q1 * sqrt(p2 + q2); |
| 201 | if(numext::real(p)<RealScalar(0)) |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 202 | u = -u; |
| 203 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 204 | p1 = abs(p); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 205 | ps = p/p1; |
| 206 | m_c = p1/u; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 207 | m_s = -conj(ps) * (q/u); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 208 | if(r) *r = ps * u; |
| 209 | } |
| 210 | } |
| 211 | } |
| 212 | |
| 213 | // specialization for reals |
| 214 | template<typename Scalar> |
| 215 | void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type) |
| 216 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 217 | using std::sqrt; |
| 218 | using std::abs; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 219 | if(q==Scalar(0)) |
| 220 | { |
| 221 | m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1); |
| 222 | m_s = Scalar(0); |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 223 | if(r) *r = abs(p); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 224 | } |
| 225 | else if(p==Scalar(0)) |
| 226 | { |
| 227 | m_c = Scalar(0); |
| 228 | m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1); |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 229 | if(r) *r = abs(q); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 230 | } |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 231 | else if(abs(p) > abs(q)) |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 232 | { |
| 233 | Scalar t = q/p; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 234 | Scalar u = sqrt(Scalar(1) + numext::abs2(t)); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 244 | Scalar u = sqrt(Scalar(1) + numext::abs2(t)); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 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 | |
| 258 | /** \jacobi_module |
| 259 | * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y: |
| 260 | * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$ |
| 261 | * |
| 262 | * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() |
| 263 | */ |
| 264 | namespace internal { |
| 265 | template<typename VectorX, typename VectorY, typename OtherScalar> |
| 266 | void apply_rotation_in_the_plane(VectorX& _x, VectorY& _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 | */ |
| 275 | template<typename Derived> |
| 276 | template<typename OtherScalar> |
| 277 | inline 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 | */ |
| 290 | template<typename Derived> |
| 291 | template<typename OtherScalar> |
| 292 | inline 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 | |
| 299 | namespace internal { |
| 300 | template<typename VectorX, typename VectorY, typename OtherScalar> |
| 301 | void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j) |
| 302 | { |
| 303 | typedef typename VectorX::Index Index; |
| 304 | typedef typename VectorX::Scalar Scalar; |
| 305 | enum { PacketSize = packet_traits<Scalar>::size }; |
| 306 | typedef typename packet_traits<Scalar>::type Packet; |
| 307 | eigen_assert(_x.size() == _y.size()); |
| 308 | Index size = _x.size(); |
| 309 | Index incrx = _x.innerStride(); |
| 310 | Index incry = _y.innerStride(); |
| 311 | |
| 312 | Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0); |
| 313 | Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0); |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 314 | |
| 315 | OtherScalar c = j.c(); |
| 316 | OtherScalar s = j.s(); |
| 317 | if (c==OtherScalar(1) && s==OtherScalar(0)) |
| 318 | return; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 319 | |
| 320 | /*** dynamic-size vectorized paths ***/ |
| 321 | |
| 322 | if(VectorX::SizeAtCompileTime == Dynamic && |
| 323 | (VectorX::Flags & VectorY::Flags & PacketAccessBit) && |
| 324 | ((incrx==1 && incry==1) || PacketSize == 1)) |
| 325 | { |
| 326 | // both vectors are sequentially stored in memory => vectorization |
| 327 | enum { Peeling = 2 }; |
| 328 | |
| 329 | Index alignedStart = internal::first_aligned(y, size); |
| 330 | Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize; |
| 331 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 332 | const Packet pc = pset1<Packet>(c); |
| 333 | const Packet ps = pset1<Packet>(s); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 334 | conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj; |
| 335 | |
| 336 | for(Index i=0; i<alignedStart; ++i) |
| 337 | { |
| 338 | Scalar xi = x[i]; |
| 339 | Scalar yi = y[i]; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 340 | x[i] = c * xi + numext::conj(s) * yi; |
| 341 | y[i] = -s * xi + numext::conj(c) * yi; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 342 | } |
| 343 | |
| 344 | Scalar* EIGEN_RESTRICT px = x + alignedStart; |
| 345 | Scalar* EIGEN_RESTRICT py = y + alignedStart; |
| 346 | |
| 347 | if(internal::first_aligned(x, size)==alignedStart) |
| 348 | { |
| 349 | for(Index i=alignedStart; i<alignedEnd; i+=PacketSize) |
| 350 | { |
| 351 | Packet xi = pload<Packet>(px); |
| 352 | Packet yi = pload<Packet>(py); |
| 353 | pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi))); |
| 354 | pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi))); |
| 355 | px += PacketSize; |
| 356 | py += PacketSize; |
| 357 | } |
| 358 | } |
| 359 | else |
| 360 | { |
| 361 | Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize); |
| 362 | for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize) |
| 363 | { |
| 364 | Packet xi = ploadu<Packet>(px); |
| 365 | Packet xi1 = ploadu<Packet>(px+PacketSize); |
| 366 | Packet yi = pload <Packet>(py); |
| 367 | Packet yi1 = pload <Packet>(py+PacketSize); |
| 368 | pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi))); |
| 369 | pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1))); |
| 370 | pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi))); |
| 371 | pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1))); |
| 372 | px += Peeling*PacketSize; |
| 373 | py += Peeling*PacketSize; |
| 374 | } |
| 375 | if(alignedEnd!=peelingEnd) |
| 376 | { |
| 377 | Packet xi = ploadu<Packet>(x+peelingEnd); |
| 378 | Packet yi = pload <Packet>(y+peelingEnd); |
| 379 | pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi))); |
| 380 | pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi))); |
| 381 | } |
| 382 | } |
| 383 | |
| 384 | for(Index i=alignedEnd; i<size; ++i) |
| 385 | { |
| 386 | Scalar xi = x[i]; |
| 387 | Scalar yi = y[i]; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 388 | x[i] = c * xi + numext::conj(s) * yi; |
| 389 | y[i] = -s * xi + numext::conj(c) * yi; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 390 | } |
| 391 | } |
| 392 | |
| 393 | /*** fixed-size vectorized path ***/ |
| 394 | else if(VectorX::SizeAtCompileTime != Dynamic && |
| 395 | (VectorX::Flags & VectorY::Flags & PacketAccessBit) && |
| 396 | (VectorX::Flags & VectorY::Flags & AlignedBit)) |
| 397 | { |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 398 | const Packet pc = pset1<Packet>(c); |
| 399 | const Packet ps = pset1<Packet>(s); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 400 | conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj; |
| 401 | Scalar* EIGEN_RESTRICT px = x; |
| 402 | Scalar* EIGEN_RESTRICT py = y; |
| 403 | for(Index i=0; i<size; i+=PacketSize) |
| 404 | { |
| 405 | Packet xi = pload<Packet>(px); |
| 406 | Packet yi = pload<Packet>(py); |
| 407 | pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi))); |
| 408 | pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi))); |
| 409 | px += PacketSize; |
| 410 | py += PacketSize; |
| 411 | } |
| 412 | } |
| 413 | |
| 414 | /*** non-vectorized path ***/ |
| 415 | else |
| 416 | { |
| 417 | for(Index i=0; i<size; ++i) |
| 418 | { |
| 419 | Scalar xi = *x; |
| 420 | Scalar yi = *y; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 421 | *x = c * xi + numext::conj(s) * yi; |
| 422 | *y = -s * xi + numext::conj(c) * yi; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 423 | x += incrx; |
| 424 | y += incry; |
| 425 | } |
| 426 | } |
| 427 | } |
| 428 | |
| 429 | } // end namespace internal |
| 430 | |
| 431 | } // end namespace Eigen |
| 432 | |
| 433 | #endif // EIGEN_JACOBI_H |