Jacobi.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
00005 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00006 //
00007 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 #ifndef EIGEN_JACOBI_H
00027 #define EIGEN_JACOBI_H
00028 
00047 template<typename Scalar> class JacobiRotation
00048 {
00049   public:
00050     typedef typename NumTraits<Scalar>::Real RealScalar;
00051 
00053     JacobiRotation() {}
00054 
00056     JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
00057 
00058     Scalar& c() { return m_c; }
00059     Scalar c() const { return m_c; }
00060     Scalar& s() { return m_s; }
00061     Scalar s() const { return m_s; }
00062 
00064     JacobiRotation operator*(const JacobiRotation& other)
00065     {
00066       return JacobiRotation(m_c * other.m_c - internal::conj(m_s) * other.m_s,
00067                             internal::conj(m_c * internal::conj(other.m_s) + internal::conj(m_s) * internal::conj(other.m_c)));
00068     }
00069 
00071     JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); }
00072 
00074     JacobiRotation adjoint() const { return JacobiRotation(internal::conj(m_c), -m_s); }
00075 
00076     template<typename Derived>
00077     bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
00078     bool makeJacobi(RealScalar x, Scalar y, RealScalar z);
00079 
00080     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
00081 
00082   protected:
00083     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
00084     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
00085 
00086     Scalar m_c, m_s;
00087 };
00088 
00094 template<typename Scalar>
00095 bool JacobiRotation<Scalar>::makeJacobi(RealScalar x, Scalar y, RealScalar z)
00096 {
00097   typedef typename NumTraits<Scalar>::Real RealScalar;
00098   if(y == Scalar(0))
00099   {
00100     m_c = Scalar(1);
00101     m_s = Scalar(0);
00102     return false;
00103   }
00104   else
00105   {
00106     RealScalar tau = (x-z)/(RealScalar(2)*internal::abs(y));
00107     RealScalar w = internal::sqrt(internal::abs2(tau) + RealScalar(1));
00108     RealScalar t;
00109     if(tau>RealScalar(0))
00110     {
00111       t = RealScalar(1) / (tau + w);
00112     }
00113     else
00114     {
00115       t = RealScalar(1) / (tau - w);
00116     }
00117     RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
00118     RealScalar n = RealScalar(1) / internal::sqrt(internal::abs2(t)+RealScalar(1));
00119     m_s = - sign_t * (internal::conj(y) / internal::abs(y)) * internal::abs(t) * n;
00120     m_c = n;
00121     return true;
00122   }
00123 }
00124 
00134 template<typename Scalar>
00135 template<typename Derived>
00136 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
00137 {
00138   return makeJacobi(internal::real(m.coeff(p,p)), m.coeff(p,q), internal::real(m.coeff(q,q)));
00139 }
00140 
00157 template<typename Scalar>
00158 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
00159 {
00160   makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
00161 }
00162 
00163 
00164 // specialization for complexes
00165 template<typename Scalar>
00166 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
00167 {
00168   if(q==Scalar(0))
00169   {
00170     m_c = internal::real(p)<0 ? Scalar(-1) : Scalar(1);
00171     m_s = 0;
00172     if(r) *r = m_c * p;
00173   }
00174   else if(p==Scalar(0))
00175   {
00176     m_c = 0;
00177     m_s = -q/internal::abs(q);
00178     if(r) *r = internal::abs(q);
00179   }
00180   else
00181   {
00182     RealScalar p1 = internal::norm1(p);
00183     RealScalar q1 = internal::norm1(q);
00184     if(p1>=q1)
00185     {
00186       Scalar ps = p / p1;
00187       RealScalar p2 = internal::abs2(ps);
00188       Scalar qs = q / p1;
00189       RealScalar q2 = internal::abs2(qs);
00190 
00191       RealScalar u = internal::sqrt(RealScalar(1) + q2/p2);
00192       if(internal::real(p)<RealScalar(0))
00193         u = -u;
00194 
00195       m_c = Scalar(1)/u;
00196       m_s = -qs*internal::conj(ps)*(m_c/p2);
00197       if(r) *r = p * u;
00198     }
00199     else
00200     {
00201       Scalar ps = p / q1;
00202       RealScalar p2 = internal::abs2(ps);
00203       Scalar qs = q / q1;
00204       RealScalar q2 = internal::abs2(qs);
00205 
00206       RealScalar u = q1 * internal::sqrt(p2 + q2);
00207       if(internal::real(p)<RealScalar(0))
00208         u = -u;
00209 
00210       p1 = internal::abs(p);
00211       ps = p/p1;
00212       m_c = p1/u;
00213       m_s = -internal::conj(ps) * (q/u);
00214       if(r) *r = ps * u;
00215     }
00216   }
00217 }
00218 
00219 // specialization for reals
00220 template<typename Scalar>
00221 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
00222 {
00223 
00224   if(q==Scalar(0))
00225   {
00226     m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
00227     m_s = Scalar(0);
00228     if(r) *r = internal::abs(p);
00229   }
00230   else if(p==Scalar(0))
00231   {
00232     m_c = Scalar(0);
00233     m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
00234     if(r) *r = internal::abs(q);
00235   }
00236   else if(internal::abs(p) > internal::abs(q))
00237   {
00238     Scalar t = q/p;
00239     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00240     if(p<Scalar(0))
00241       u = -u;
00242     m_c = Scalar(1)/u;
00243     m_s = -t * m_c;
00244     if(r) *r = p * u;
00245   }
00246   else
00247   {
00248     Scalar t = p/q;
00249     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00250     if(q<Scalar(0))
00251       u = -u;
00252     m_s = -Scalar(1)/u;
00253     m_c = -t * m_s;
00254     if(r) *r = q * u;
00255   }
00256 
00257 }
00258 
00259 /****************************************************************************************
00260 *   Implementation of MatrixBase methods
00261 ****************************************************************************************/
00262 
00269 namespace internal {
00270 template<typename VectorX, typename VectorY, typename OtherScalar>
00271 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
00272 }
00273 
00280 template<typename Derived>
00281 template<typename OtherScalar>
00282 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00283 {
00284   RowXpr x(this->row(p));
00285   RowXpr y(this->row(q));
00286   internal::apply_rotation_in_the_plane(x, y, j);
00287 }
00288 
00295 template<typename Derived>
00296 template<typename OtherScalar>
00297 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00298 {
00299   ColXpr x(this->col(p));
00300   ColXpr y(this->col(q));
00301   internal::apply_rotation_in_the_plane(x, y, j.transpose());
00302 }
00303 
00304 namespace internal {
00305 template<typename VectorX, typename VectorY, typename OtherScalar>
00306 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
00307 {
00308   typedef typename VectorX::Index Index;
00309   typedef typename VectorX::Scalar Scalar;
00310   enum { PacketSize = packet_traits<Scalar>::size };
00311   typedef typename packet_traits<Scalar>::type Packet;
00312   eigen_assert(_x.size() == _y.size());
00313   Index size = _x.size();
00314   Index incrx = _x.innerStride();
00315   Index incry = _y.innerStride();
00316 
00317   Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
00318   Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
00319 
00320   /*** dynamic-size vectorized paths ***/
00321 
00322   if(VectorX::SizeAtCompileTime == Dynamic &&
00323     (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00324     ((incrx==1 && incry==1) || PacketSize == 1))
00325   {
00326     // both vectors are sequentially stored in memory => vectorization
00327     enum { Peeling = 2 };
00328 
00329     Index alignedStart = first_aligned(y, size);
00330     Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
00331 
00332     const Packet pc = pset1<Packet>(j.c());
00333     const Packet ps = pset1<Packet>(j.s());
00334     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00335 
00336     for(Index i=0; i<alignedStart; ++i)
00337     {
00338       Scalar xi = x[i];
00339       Scalar yi = y[i];
00340       x[i] =  j.c() * xi + conj(j.s()) * yi;
00341       y[i] = -j.s() * xi + conj(j.c()) * yi;
00342     }
00343 
00344     Scalar* EIGEN_RESTRICT px = x + alignedStart;
00345     Scalar* EIGEN_RESTRICT py = y + alignedStart;
00346 
00347     if(first_aligned(x, size)==alignedStart)
00348     {
00349       for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
00350       {
00351         Packet xi = pload<Packet>(px);
00352         Packet yi = pload<Packet>(py);
00353         pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00354         pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00355         px += PacketSize;
00356         py += PacketSize;
00357       }
00358     }
00359     else
00360     {
00361       Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
00362       for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
00363       {
00364         Packet xi   = ploadu<Packet>(px);
00365         Packet xi1  = ploadu<Packet>(px+PacketSize);
00366         Packet yi   = pload <Packet>(py);
00367         Packet yi1  = pload <Packet>(py+PacketSize);
00368         pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00369         pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
00370         pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00371         pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
00372         px += Peeling*PacketSize;
00373         py += Peeling*PacketSize;
00374       }
00375       if(alignedEnd!=peelingEnd)
00376       {
00377         Packet xi = ploadu<Packet>(x+peelingEnd);
00378         Packet yi = pload <Packet>(y+peelingEnd);
00379         pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00380         pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00381       }
00382     }
00383 
00384     for(Index i=alignedEnd; i<size; ++i)
00385     {
00386       Scalar xi = x[i];
00387       Scalar yi = y[i];
00388       x[i] =  j.c() * xi + conj(j.s()) * yi;
00389       y[i] = -j.s() * xi + conj(j.c()) * yi;
00390     }
00391   }
00392 
00393   /*** fixed-size vectorized path ***/
00394   else if(VectorX::SizeAtCompileTime != Dynamic &&
00395           (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00396           (VectorX::Flags & VectorY::Flags & AlignedBit))
00397   {
00398     const Packet pc = pset1<Packet>(j.c());
00399     const Packet ps = pset1<Packet>(j.s());
00400     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00401     Scalar* EIGEN_RESTRICT px = x;
00402     Scalar* EIGEN_RESTRICT py = y;
00403     for(Index i=0; i<size; i+=PacketSize)
00404     {
00405       Packet xi = pload<Packet>(px);
00406       Packet yi = pload<Packet>(py);
00407       pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00408       pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00409       px += PacketSize;
00410       py += PacketSize;
00411     }
00412   }
00413 
00414   /*** non-vectorized path ***/
00415   else
00416   {
00417     for(Index i=0; i<size; ++i)
00418     {
00419       Scalar xi = *x;
00420       Scalar yi = *y;
00421       *x =  j.c() * xi + conj(j.s()) * yi;
00422       *y = -j.s() * xi + conj(j.c()) * yi;
00423       x += incrx;
00424       y += incry;
00425     }
00426   }
00427 }
00428 }
00429 
00430 #endif // EIGEN_JACOBI_H


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:51