Yoji KURODA / Eigen

Eigne Matrix Class Library

Dependents:   Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more

Eigen Matrix Class Library for mbed.

Finally, you can use Eigen on your mbed!!!

src/Jacobi/Jacobi.h@0:13a5d365ba16, 2016-10-13 (annotated)

Committer:
ykuroda
Date:
Thu Oct 13 04:07:23 2016 +0000
Revision:
0:13a5d365ba16
First commint, Eigne Matrix Class Library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ykuroda 0:13a5d365ba16 1 // This file is part of Eigen, a lightweight C++ template library
ykuroda 0:13a5d365ba16 2 // for linear algebra.
ykuroda 0:13a5d365ba16 3 //
ykuroda 0:13a5d365ba16 4 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
ykuroda 0:13a5d365ba16 5 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
ykuroda 0:13a5d365ba16 6 //
ykuroda 0:13a5d365ba16 7 // This Source Code Form is subject to the terms of the Mozilla
ykuroda 0:13a5d365ba16 8 // Public License v. 2.0. If a copy of the MPL was not distributed
ykuroda 0:13a5d365ba16 9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
ykuroda 0:13a5d365ba16 10
ykuroda 0:13a5d365ba16 11 #ifndef EIGEN_JACOBI_H
ykuroda 0:13a5d365ba16 12 #define EIGEN_JACOBI_H
ykuroda 0:13a5d365ba16 13
ykuroda 0:13a5d365ba16 14 namespace Eigen {
ykuroda 0:13a5d365ba16 15
ykuroda 0:13a5d365ba16 16 /** \ingroup Jacobi_Module
ykuroda 0:13a5d365ba16 17 * \jacobi_module
ykuroda 0:13a5d365ba16 18 * \class JacobiRotation
ykuroda 0:13a5d365ba16 19 * \brief Rotation given by a cosine-sine pair.
ykuroda 0:13a5d365ba16 20 *
ykuroda 0:13a5d365ba16 21 * This class represents a Jacobi or Givens rotation.
ykuroda 0:13a5d365ba16 22 * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
ykuroda 0:13a5d365ba16 23 * its cosine \c c and sine \c s as follow:
ykuroda 0:13a5d365ba16 24 * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} \right ) \f$
ykuroda 0:13a5d365ba16 25 *
ykuroda 0:13a5d365ba16 26 * You can apply the respective counter-clockwise rotation to a column vector \c v by
ykuroda 0:13a5d365ba16 27 * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
ykuroda 0:13a5d365ba16 28 * \code
ykuroda 0:13a5d365ba16 29 * v.applyOnTheLeft(J.adjoint());
ykuroda 0:13a5d365ba16 30 * \endcode
ykuroda 0:13a5d365ba16 31 *
ykuroda 0:13a5d365ba16 32 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
ykuroda 0:13a5d365ba16 33 */
ykuroda 0:13a5d365ba16 34 template<typename Scalar> class JacobiRotation
ykuroda 0:13a5d365ba16 35 {
ykuroda 0:13a5d365ba16 36 public:
ykuroda 0:13a5d365ba16 37 typedef typename NumTraits<Scalar>::Real RealScalar;
ykuroda 0:13a5d365ba16 38
ykuroda 0:13a5d365ba16 39 /** Default constructor without any initialization. */
ykuroda 0:13a5d365ba16 40 JacobiRotation() {}
ykuroda 0:13a5d365ba16 41
ykuroda 0:13a5d365ba16 42 /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
ykuroda 0:13a5d365ba16 43 JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
ykuroda 0:13a5d365ba16 44
ykuroda 0:13a5d365ba16 45 Scalar& c() { return m_c; }
ykuroda 0:13a5d365ba16 46 Scalar c() const { return m_c; }
ykuroda 0:13a5d365ba16 47 Scalar& s() { return m_s; }
ykuroda 0:13a5d365ba16 48 Scalar s() const { return m_s; }
ykuroda 0:13a5d365ba16 49
ykuroda 0:13a5d365ba16 50 /** Concatenates two planar rotation */
ykuroda 0:13a5d365ba16 51 JacobiRotation operator*(const JacobiRotation& other)
ykuroda 0:13a5d365ba16 52 {
ykuroda 0:13a5d365ba16 53 using numext::conj;
ykuroda 0:13a5d365ba16 54 return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s,
ykuroda 0:13a5d365ba16 55 conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c)));
ykuroda 0:13a5d365ba16 56 }
ykuroda 0:13a5d365ba16 57
ykuroda 0:13a5d365ba16 58 /** Returns the transposed transformation */
ykuroda 0:13a5d365ba16 59 JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
ykuroda 0:13a5d365ba16 60
ykuroda 0:13a5d365ba16 61 /** Returns the adjoint transformation */
ykuroda 0:13a5d365ba16 62 JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); }
ykuroda 0:13a5d365ba16 63
ykuroda 0:13a5d365ba16 64 template<typename Derived>
ykuroda 0:13a5d365ba16 65 bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
ykuroda 0:13a5d365ba16 66 bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
ykuroda 0:13a5d365ba16 67
ykuroda 0:13a5d365ba16 68 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
ykuroda 0:13a5d365ba16 69
ykuroda 0:13a5d365ba16 70 protected:
ykuroda 0:13a5d365ba16 71 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
ykuroda 0:13a5d365ba16 72 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
ykuroda 0:13a5d365ba16 73
ykuroda 0:13a5d365ba16 74 Scalar m_c, m_s;
ykuroda 0:13a5d365ba16 75 };
ykuroda 0:13a5d365ba16 76
ykuroda 0:13a5d365ba16 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
ykuroda 0:13a5d365ba16 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$
ykuroda 0:13a5d365ba16 79 *
ykuroda 0:13a5d365ba16 80 * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
ykuroda 0:13a5d365ba16 81 */
ykuroda 0:13a5d365ba16 82 template<typename Scalar>
ykuroda 0:13a5d365ba16 83 bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z)
ykuroda 0:13a5d365ba16 84 {
ykuroda 0:13a5d365ba16 85 using std::sqrt;
ykuroda 0:13a5d365ba16 86 using std::abs;
ykuroda 0:13a5d365ba16 87 typedef typename NumTraits<Scalar>::Real RealScalar;
ykuroda 0:13a5d365ba16 88 if(y == Scalar(0))
ykuroda 0:13a5d365ba16 89 {
ykuroda 0:13a5d365ba16 90 m_c = Scalar(1);
ykuroda 0:13a5d365ba16 91 m_s = Scalar(0);
ykuroda 0:13a5d365ba16 92 return false;
ykuroda 0:13a5d365ba16 93 }
ykuroda 0:13a5d365ba16 94 else
ykuroda 0:13a5d365ba16 95 {
ykuroda 0:13a5d365ba16 96 RealScalar tau = (x-z)/(RealScalar(2)*abs(y));
ykuroda 0:13a5d365ba16 97 RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
ykuroda 0:13a5d365ba16 98 RealScalar t;
ykuroda 0:13a5d365ba16 99 if(tau>RealScalar(0))
ykuroda 0:13a5d365ba16 100 {
ykuroda 0:13a5d365ba16 101 t = RealScalar(1) / (tau + w);
ykuroda 0:13a5d365ba16 102 }
ykuroda 0:13a5d365ba16 103 else
ykuroda 0:13a5d365ba16 104 {
ykuroda 0:13a5d365ba16 105 t = RealScalar(1) / (tau - w);
ykuroda 0:13a5d365ba16 106 }
ykuroda 0:13a5d365ba16 107 RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
ykuroda 0:13a5d365ba16 108 RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
ykuroda 0:13a5d365ba16 109 m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
ykuroda 0:13a5d365ba16 110 m_c = n;
ykuroda 0:13a5d365ba16 111 return true;
ykuroda 0:13a5d365ba16 112 }
ykuroda 0:13a5d365ba16 113 }
ykuroda 0:13a5d365ba16 114
ykuroda 0:13a5d365ba16 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
ykuroda 0:13a5d365ba16 116 * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
ykuroda 0:13a5d365ba16 117 * a diagonal matrix \f$ A = J^* B J \f$
ykuroda 0:13a5d365ba16 118 *
ykuroda 0:13a5d365ba16 119 * Example: \include Jacobi_makeJacobi.cpp
ykuroda 0:13a5d365ba16 120 * Output: \verbinclude Jacobi_makeJacobi.out
ykuroda 0:13a5d365ba16 121 *
ykuroda 0:13a5d365ba16 122 * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
ykuroda 0:13a5d365ba16 123 */
ykuroda 0:13a5d365ba16 124 template<typename Scalar>
ykuroda 0:13a5d365ba16 125 template<typename Derived>
ykuroda 0:13a5d365ba16 126 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
ykuroda 0:13a5d365ba16 127 {
ykuroda 0:13a5d365ba16 128 return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
ykuroda 0:13a5d365ba16 129 }
ykuroda 0:13a5d365ba16 130
ykuroda 0:13a5d365ba16 131 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
ykuroda 0:13a5d365ba16 132 * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
ykuroda 0:13a5d365ba16 133 * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
ykuroda 0:13a5d365ba16 134 *
ykuroda 0:13a5d365ba16 135 * The value of \a z is returned if \a z is not null (the default is null).
ykuroda 0:13a5d365ba16 136 * Also note that G is built such that the cosine is always real.
ykuroda 0:13a5d365ba16 137 *
ykuroda 0:13a5d365ba16 138 * Example: \include Jacobi_makeGivens.cpp
ykuroda 0:13a5d365ba16 139 * Output: \verbinclude Jacobi_makeGivens.out
ykuroda 0:13a5d365ba16 140 *
ykuroda 0:13a5d365ba16 141 * This function implements the continuous Givens rotation generation algorithm
ykuroda 0:13a5d365ba16 142 * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
ykuroda 0:13a5d365ba16 143 * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
ykuroda 0:13a5d365ba16 144 *
ykuroda 0:13a5d365ba16 145 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
ykuroda 0:13a5d365ba16 146 */
ykuroda 0:13a5d365ba16 147 template<typename Scalar>
ykuroda 0:13a5d365ba16 148 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
ykuroda 0:13a5d365ba16 149 {
ykuroda 0:13a5d365ba16 150 makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
ykuroda 0:13a5d365ba16 151 }
ykuroda 0:13a5d365ba16 152
ykuroda 0:13a5d365ba16 153
ykuroda 0:13a5d365ba16 154 // specialization for complexes
ykuroda 0:13a5d365ba16 155 template<typename Scalar>
ykuroda 0:13a5d365ba16 156 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
ykuroda 0:13a5d365ba16 157 {
ykuroda 0:13a5d365ba16 158 using std::sqrt;
ykuroda 0:13a5d365ba16 159 using std::abs;
ykuroda 0:13a5d365ba16 160 using numext::conj;
ykuroda 0:13a5d365ba16 161
ykuroda 0:13a5d365ba16 162 if(q==Scalar(0))
ykuroda 0:13a5d365ba16 163 {
ykuroda 0:13a5d365ba16 164 m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
ykuroda 0:13a5d365ba16 165 m_s = 0;
ykuroda 0:13a5d365ba16 166 if(r) *r = m_c * p;
ykuroda 0:13a5d365ba16 167 }
ykuroda 0:13a5d365ba16 168 else if(p==Scalar(0))
ykuroda 0:13a5d365ba16 169 {
ykuroda 0:13a5d365ba16 170 m_c = 0;
ykuroda 0:13a5d365ba16 171 m_s = -q/abs(q);
ykuroda 0:13a5d365ba16 172 if(r) *r = abs(q);
ykuroda 0:13a5d365ba16 173 }
ykuroda 0:13a5d365ba16 174 else
ykuroda 0:13a5d365ba16 175 {
ykuroda 0:13a5d365ba16 176 RealScalar p1 = numext::norm1(p);
ykuroda 0:13a5d365ba16 177 RealScalar q1 = numext::norm1(q);
ykuroda 0:13a5d365ba16 178 if(p1>=q1)
ykuroda 0:13a5d365ba16 179 {
ykuroda 0:13a5d365ba16 180 Scalar ps = p / p1;
ykuroda 0:13a5d365ba16 181 RealScalar p2 = numext::abs2(ps);
ykuroda 0:13a5d365ba16 182 Scalar qs = q / p1;
ykuroda 0:13a5d365ba16 183 RealScalar q2 = numext::abs2(qs);
ykuroda 0:13a5d365ba16 184
ykuroda 0:13a5d365ba16 185 RealScalar u = sqrt(RealScalar(1) + q2/p2);
ykuroda 0:13a5d365ba16 186 if(numext::real(p)<RealScalar(0))
ykuroda 0:13a5d365ba16 187 u = -u;
ykuroda 0:13a5d365ba16 188
ykuroda 0:13a5d365ba16 189 m_c = Scalar(1)/u;
ykuroda 0:13a5d365ba16 190 m_s = -qs*conj(ps)*(m_c/p2);
ykuroda 0:13a5d365ba16 191 if(r) *r = p * u;
ykuroda 0:13a5d365ba16 192 }
ykuroda 0:13a5d365ba16 193 else
ykuroda 0:13a5d365ba16 194 {
ykuroda 0:13a5d365ba16 195 Scalar ps = p / q1;
ykuroda 0:13a5d365ba16 196 RealScalar p2 = numext::abs2(ps);
ykuroda 0:13a5d365ba16 197 Scalar qs = q / q1;
ykuroda 0:13a5d365ba16 198 RealScalar q2 = numext::abs2(qs);
ykuroda 0:13a5d365ba16 199
ykuroda 0:13a5d365ba16 200 RealScalar u = q1 * sqrt(p2 + q2);
ykuroda 0:13a5d365ba16 201 if(numext::real(p)<RealScalar(0))
ykuroda 0:13a5d365ba16 202 u = -u;
ykuroda 0:13a5d365ba16 203
ykuroda 0:13a5d365ba16 204 p1 = abs(p);
ykuroda 0:13a5d365ba16 205 ps = p/p1;
ykuroda 0:13a5d365ba16 206 m_c = p1/u;
ykuroda 0:13a5d365ba16 207 m_s = -conj(ps) * (q/u);
ykuroda 0:13a5d365ba16 208 if(r) *r = ps * u;
ykuroda 0:13a5d365ba16 209 }
ykuroda 0:13a5d365ba16 210 }
ykuroda 0:13a5d365ba16 211 }
ykuroda 0:13a5d365ba16 212
ykuroda 0:13a5d365ba16 213 // specialization for reals
ykuroda 0:13a5d365ba16 214 template<typename Scalar>
ykuroda 0:13a5d365ba16 215 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
ykuroda 0:13a5d365ba16 216 {
ykuroda 0:13a5d365ba16 217 using std::sqrt;
ykuroda 0:13a5d365ba16 218 using std::abs;
ykuroda 0:13a5d365ba16 219 if(q==Scalar(0))
ykuroda 0:13a5d365ba16 220 {
ykuroda 0:13a5d365ba16 221 m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
ykuroda 0:13a5d365ba16 222 m_s = Scalar(0);
ykuroda 0:13a5d365ba16 223 if(r) *r = abs(p);
ykuroda 0:13a5d365ba16 224 }
ykuroda 0:13a5d365ba16 225 else if(p==Scalar(0))
ykuroda 0:13a5d365ba16 226 {
ykuroda 0:13a5d365ba16 227 m_c = Scalar(0);
ykuroda 0:13a5d365ba16 228 m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
ykuroda 0:13a5d365ba16 229 if(r) *r = abs(q);
ykuroda 0:13a5d365ba16 230 }
ykuroda 0:13a5d365ba16 231 else if(abs(p) > abs(q))
ykuroda 0:13a5d365ba16 232 {
ykuroda 0:13a5d365ba16 233 Scalar t = q/p;
ykuroda 0:13a5d365ba16 234 Scalar u = sqrt(Scalar(1) + numext::abs2(t));
ykuroda 0:13a5d365ba16 235 if(p<Scalar(0))
ykuroda 0:13a5d365ba16 236 u = -u;
ykuroda 0:13a5d365ba16 237 m_c = Scalar(1)/u;
ykuroda 0:13a5d365ba16 238 m_s = -t * m_c;
ykuroda 0:13a5d365ba16 239 if(r) *r = p * u;
ykuroda 0:13a5d365ba16 240 }
ykuroda 0:13a5d365ba16 241 else
ykuroda 0:13a5d365ba16 242 {
ykuroda 0:13a5d365ba16 243 Scalar t = p/q;
ykuroda 0:13a5d365ba16 244 Scalar u = sqrt(Scalar(1) + numext::abs2(t));
ykuroda 0:13a5d365ba16 245 if(q<Scalar(0))
ykuroda 0:13a5d365ba16 246 u = -u;
ykuroda 0:13a5d365ba16 247 m_s = -Scalar(1)/u;
ykuroda 0:13a5d365ba16 248 m_c = -t * m_s;
ykuroda 0:13a5d365ba16 249 if(r) *r = q * u;
ykuroda 0:13a5d365ba16 250 }
ykuroda 0:13a5d365ba16 251
ykuroda 0:13a5d365ba16 252 }
ykuroda 0:13a5d365ba16 253
ykuroda 0:13a5d365ba16 254 /****************************************************************************************
ykuroda 0:13a5d365ba16 255 * Implementation of MatrixBase methods
ykuroda 0:13a5d365ba16 256 ****************************************************************************************/
ykuroda 0:13a5d365ba16 257
ykuroda 0:13a5d365ba16 258 /** \jacobi_module
ykuroda 0:13a5d365ba16 259 * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
ykuroda 0:13a5d365ba16 260 * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
ykuroda 0:13a5d365ba16 261 *
ykuroda 0:13a5d365ba16 262 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
ykuroda 0:13a5d365ba16 263 */
ykuroda 0:13a5d365ba16 264 namespace internal {
ykuroda 0:13a5d365ba16 265 template<typename VectorX, typename VectorY, typename OtherScalar>
ykuroda 0:13a5d365ba16 266 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
ykuroda 0:13a5d365ba16 267 }
ykuroda 0:13a5d365ba16 268
ykuroda 0:13a5d365ba16 269 /** \jacobi_module
ykuroda 0:13a5d365ba16 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,
ykuroda 0:13a5d365ba16 271 * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
ykuroda 0:13a5d365ba16 272 *
ykuroda 0:13a5d365ba16 273 * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
ykuroda 0:13a5d365ba16 274 */
ykuroda 0:13a5d365ba16 275 template<typename Derived>
ykuroda 0:13a5d365ba16 276 template<typename OtherScalar>
ykuroda 0:13a5d365ba16 277 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
ykuroda 0:13a5d365ba16 278 {
ykuroda 0:13a5d365ba16 279 RowXpr x(this->row(p));
ykuroda 0:13a5d365ba16 280 RowXpr y(this->row(q));
ykuroda 0:13a5d365ba16 281 internal::apply_rotation_in_the_plane(x, y, j);
ykuroda 0:13a5d365ba16 282 }
ykuroda 0:13a5d365ba16 283
ykuroda 0:13a5d365ba16 284 /** \ingroup Jacobi_Module
ykuroda 0:13a5d365ba16 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
ykuroda 0:13a5d365ba16 286 * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
ykuroda 0:13a5d365ba16 287 *
ykuroda 0:13a5d365ba16 288 * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
ykuroda 0:13a5d365ba16 289 */
ykuroda 0:13a5d365ba16 290 template<typename Derived>
ykuroda 0:13a5d365ba16 291 template<typename OtherScalar>
ykuroda 0:13a5d365ba16 292 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
ykuroda 0:13a5d365ba16 293 {
ykuroda 0:13a5d365ba16 294 ColXpr x(this->col(p));
ykuroda 0:13a5d365ba16 295 ColXpr y(this->col(q));
ykuroda 0:13a5d365ba16 296 internal::apply_rotation_in_the_plane(x, y, j.transpose());
ykuroda 0:13a5d365ba16 297 }
ykuroda 0:13a5d365ba16 298
ykuroda 0:13a5d365ba16 299 namespace internal {
ykuroda 0:13a5d365ba16 300 template<typename VectorX, typename VectorY, typename OtherScalar>
ykuroda 0:13a5d365ba16 301 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
ykuroda 0:13a5d365ba16 302 {
ykuroda 0:13a5d365ba16 303 typedef typename VectorX::Index Index;
ykuroda 0:13a5d365ba16 304 typedef typename VectorX::Scalar Scalar;
ykuroda 0:13a5d365ba16 305 enum { PacketSize = packet_traits<Scalar>::size };
ykuroda 0:13a5d365ba16 306 typedef typename packet_traits<Scalar>::type Packet;
ykuroda 0:13a5d365ba16 307 eigen_assert(_x.size() == _y.size());
ykuroda 0:13a5d365ba16 308 Index size = _x.size();
ykuroda 0:13a5d365ba16 309 Index incrx = _x.innerStride();
ykuroda 0:13a5d365ba16 310 Index incry = _y.innerStride();
ykuroda 0:13a5d365ba16 311
ykuroda 0:13a5d365ba16 312 Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
ykuroda 0:13a5d365ba16 313 Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
ykuroda 0:13a5d365ba16 314
ykuroda 0:13a5d365ba16 315 OtherScalar c = j.c();
ykuroda 0:13a5d365ba16 316 OtherScalar s = j.s();
ykuroda 0:13a5d365ba16 317 if (c==OtherScalar(1) && s==OtherScalar(0))
ykuroda 0:13a5d365ba16 318 return;
ykuroda 0:13a5d365ba16 319
ykuroda 0:13a5d365ba16 320 /*** dynamic-size vectorized paths ***/
ykuroda 0:13a5d365ba16 321
ykuroda 0:13a5d365ba16 322 if(VectorX::SizeAtCompileTime == Dynamic &&
ykuroda 0:13a5d365ba16 323 (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
ykuroda 0:13a5d365ba16 324 ((incrx==1 && incry==1) || PacketSize == 1))
ykuroda 0:13a5d365ba16 325 {
ykuroda 0:13a5d365ba16 326 // both vectors are sequentially stored in memory => vectorization
ykuroda 0:13a5d365ba16 327 enum { Peeling = 2 };
ykuroda 0:13a5d365ba16 328
ykuroda 0:13a5d365ba16 329 Index alignedStart = internal::first_aligned(y, size);
ykuroda 0:13a5d365ba16 330 Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
ykuroda 0:13a5d365ba16 331
ykuroda 0:13a5d365ba16 332 const Packet pc = pset1<Packet>(c);
ykuroda 0:13a5d365ba16 333 const Packet ps = pset1<Packet>(s);
ykuroda 0:13a5d365ba16 334 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
ykuroda 0:13a5d365ba16 335
ykuroda 0:13a5d365ba16 336 for(Index i=0; i<alignedStart; ++i)
ykuroda 0:13a5d365ba16 337 {
ykuroda 0:13a5d365ba16 338 Scalar xi = x[i];
ykuroda 0:13a5d365ba16 339 Scalar yi = y[i];
ykuroda 0:13a5d365ba16 340 x[i] = c * xi + numext::conj(s) * yi;
ykuroda 0:13a5d365ba16 341 y[i] = -s * xi + numext::conj(c) * yi;
ykuroda 0:13a5d365ba16 342 }
ykuroda 0:13a5d365ba16 343
ykuroda 0:13a5d365ba16 344 Scalar* EIGEN_RESTRICT px = x + alignedStart;
ykuroda 0:13a5d365ba16 345 Scalar* EIGEN_RESTRICT py = y + alignedStart;
ykuroda 0:13a5d365ba16 346
ykuroda 0:13a5d365ba16 347 if(internal::first_aligned(x, size)==alignedStart)
ykuroda 0:13a5d365ba16 348 {
ykuroda 0:13a5d365ba16 349 for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
ykuroda 0:13a5d365ba16 350 {
ykuroda 0:13a5d365ba16 351 Packet xi = pload<Packet>(px);
ykuroda 0:13a5d365ba16 352 Packet yi = pload<Packet>(py);
ykuroda 0:13a5d365ba16 353 pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
ykuroda 0:13a5d365ba16 354 pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
ykuroda 0:13a5d365ba16 355 px += PacketSize;
ykuroda 0:13a5d365ba16 356 py += PacketSize;
ykuroda 0:13a5d365ba16 357 }
ykuroda 0:13a5d365ba16 358 }
ykuroda 0:13a5d365ba16 359 else
ykuroda 0:13a5d365ba16 360 {
ykuroda 0:13a5d365ba16 361 Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
ykuroda 0:13a5d365ba16 362 for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
ykuroda 0:13a5d365ba16 363 {
ykuroda 0:13a5d365ba16 364 Packet xi = ploadu<Packet>(px);
ykuroda 0:13a5d365ba16 365 Packet xi1 = ploadu<Packet>(px+PacketSize);
ykuroda 0:13a5d365ba16 366 Packet yi = pload <Packet>(py);
ykuroda 0:13a5d365ba16 367 Packet yi1 = pload <Packet>(py+PacketSize);
ykuroda 0:13a5d365ba16 368 pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
ykuroda 0:13a5d365ba16 369 pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
ykuroda 0:13a5d365ba16 370 pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
ykuroda 0:13a5d365ba16 371 pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
ykuroda 0:13a5d365ba16 372 px += Peeling*PacketSize;
ykuroda 0:13a5d365ba16 373 py += Peeling*PacketSize;
ykuroda 0:13a5d365ba16 374 }
ykuroda 0:13a5d365ba16 375 if(alignedEnd!=peelingEnd)
ykuroda 0:13a5d365ba16 376 {
ykuroda 0:13a5d365ba16 377 Packet xi = ploadu<Packet>(x+peelingEnd);
ykuroda 0:13a5d365ba16 378 Packet yi = pload <Packet>(y+peelingEnd);
ykuroda 0:13a5d365ba16 379 pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
ykuroda 0:13a5d365ba16 380 pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
ykuroda 0:13a5d365ba16 381 }
ykuroda 0:13a5d365ba16 382 }
ykuroda 0:13a5d365ba16 383
ykuroda 0:13a5d365ba16 384 for(Index i=alignedEnd; i<size; ++i)
ykuroda 0:13a5d365ba16 385 {
ykuroda 0:13a5d365ba16 386 Scalar xi = x[i];
ykuroda 0:13a5d365ba16 387 Scalar yi = y[i];
ykuroda 0:13a5d365ba16 388 x[i] = c * xi + numext::conj(s) * yi;
ykuroda 0:13a5d365ba16 389 y[i] = -s * xi + numext::conj(c) * yi;
ykuroda 0:13a5d365ba16 390 }
ykuroda 0:13a5d365ba16 391 }
ykuroda 0:13a5d365ba16 392
ykuroda 0:13a5d365ba16 393 /*** fixed-size vectorized path ***/
ykuroda 0:13a5d365ba16 394 else if(VectorX::SizeAtCompileTime != Dynamic &&
ykuroda 0:13a5d365ba16 395 (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
ykuroda 0:13a5d365ba16 396 (VectorX::Flags & VectorY::Flags & AlignedBit))
ykuroda 0:13a5d365ba16 397 {
ykuroda 0:13a5d365ba16 398 const Packet pc = pset1<Packet>(c);
ykuroda 0:13a5d365ba16 399 const Packet ps = pset1<Packet>(s);
ykuroda 0:13a5d365ba16 400 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
ykuroda 0:13a5d365ba16 401 Scalar* EIGEN_RESTRICT px = x;
ykuroda 0:13a5d365ba16 402 Scalar* EIGEN_RESTRICT py = y;
ykuroda 0:13a5d365ba16 403 for(Index i=0; i<size; i+=PacketSize)
ykuroda 0:13a5d365ba16 404 {
ykuroda 0:13a5d365ba16 405 Packet xi = pload<Packet>(px);
ykuroda 0:13a5d365ba16 406 Packet yi = pload<Packet>(py);
ykuroda 0:13a5d365ba16 407 pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
ykuroda 0:13a5d365ba16 408 pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
ykuroda 0:13a5d365ba16 409 px += PacketSize;
ykuroda 0:13a5d365ba16 410 py += PacketSize;
ykuroda 0:13a5d365ba16 411 }
ykuroda 0:13a5d365ba16 412 }
ykuroda 0:13a5d365ba16 413
ykuroda 0:13a5d365ba16 414 /*** non-vectorized path ***/
ykuroda 0:13a5d365ba16 415 else
ykuroda 0:13a5d365ba16 416 {
ykuroda 0:13a5d365ba16 417 for(Index i=0; i<size; ++i)
ykuroda 0:13a5d365ba16 418 {
ykuroda 0:13a5d365ba16 419 Scalar xi = *x;
ykuroda 0:13a5d365ba16 420 Scalar yi = *y;
ykuroda 0:13a5d365ba16 421 *x = c * xi + numext::conj(s) * yi;
ykuroda 0:13a5d365ba16 422 *y = -s * xi + numext::conj(c) * yi;
ykuroda 0:13a5d365ba16 423 x += incrx;
ykuroda 0:13a5d365ba16 424 y += incry;
ykuroda 0:13a5d365ba16 425 }
ykuroda 0:13a5d365ba16 426 }
ykuroda 0:13a5d365ba16 427 }
ykuroda 0:13a5d365ba16 428
ykuroda 0:13a5d365ba16 429 } // end namespace internal
ykuroda 0:13a5d365ba16 430
ykuroda 0:13a5d365ba16 431 } // end namespace Eigen
ykuroda 0:13a5d365ba16 432
ykuroda 0:13a5d365ba16 433 #endif // EIGEN_JACOBI_H