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 // This Source Code Form is subject to the terms of the Mozilla
00008 // Public License v. 2.0. If a copy of the MPL was not distributed
00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00010 
00011 #ifndef EIGEN_JACOBI_H
00012 #define EIGEN_JACOBI_H
00013 
00014 namespace Eigen { 
00015 
00034 template<typename Scalar> class JacobiRotation
00035 {
00036   public:
00037     typedef typename NumTraits<Scalar>::Real RealScalar;
00038 
00040     JacobiRotation() {}
00041 
00043     JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
00044 
00045     Scalar& c() { return m_c; }
00046     Scalar c() const { return m_c; }
00047     Scalar& s() { return m_s; }
00048     Scalar s() const { return m_s; }
00049 
00051     JacobiRotation operator*(const JacobiRotation& other)
00052     {
00053       using numext::conj;
00054       return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s,
00055                             conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c)));
00056     }
00057 
00059     JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
00060 
00062     JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); }
00063 
00064     template<typename Derived>
00065     bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
00066     bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
00067 
00068     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
00069 
00070   protected:
00071     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
00072     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
00073 
00074     Scalar m_c, m_s;
00075 };
00076 
00082 template<typename Scalar>
00083 bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z)
00084 {
00085   using std::sqrt;
00086   using std::abs;
00087   typedef typename NumTraits<Scalar>::Real RealScalar;
00088   if(y == Scalar(0))
00089   {
00090     m_c = Scalar(1);
00091     m_s = Scalar(0);
00092     return false;
00093   }
00094   else
00095   {
00096     RealScalar tau = (x-z)/(RealScalar(2)*abs(y));
00097     RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
00098     RealScalar t;
00099     if(tau>RealScalar(0))
00100     {
00101       t = RealScalar(1) / (tau + w);
00102     }
00103     else
00104     {
00105       t = RealScalar(1) / (tau - w);
00106     }
00107     RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
00108     RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
00109     m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
00110     m_c = n;
00111     return true;
00112   }
00113 }
00114 
00124 template<typename Scalar>
00125 template<typename Derived>
00126 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
00127 {
00128   return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
00129 }
00130 
00147 template<typename Scalar>
00148 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
00149 {
00150   makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
00151 }
00152 
00153 
00154 // specialization for complexes
00155 template<typename Scalar>
00156 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
00157 {
00158   using std::sqrt;
00159   using std::abs;
00160   using numext::conj;
00161   
00162   if(q==Scalar(0))
00163   {
00164     m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
00165     m_s = 0;
00166     if(r) *r = m_c * p;
00167   }
00168   else if(p==Scalar(0))
00169   {
00170     m_c = 0;
00171     m_s = -q/abs(q);
00172     if(r) *r = abs(q);
00173   }
00174   else
00175   {
00176     RealScalar p1 = numext::norm1(p);
00177     RealScalar q1 = numext::norm1(q);
00178     if(p1>=q1)
00179     {
00180       Scalar ps = p / p1;
00181       RealScalar p2 = numext::abs2(ps);
00182       Scalar qs = q / p1;
00183       RealScalar q2 = numext::abs2(qs);
00184 
00185       RealScalar u = sqrt(RealScalar(1) + q2/p2);
00186       if(numext::real(p)<RealScalar(0))
00187         u = -u;
00188 
00189       m_c = Scalar(1)/u;
00190       m_s = -qs*conj(ps)*(m_c/p2);
00191       if(r) *r = p * u;
00192     }
00193     else
00194     {
00195       Scalar ps = p / q1;
00196       RealScalar p2 = numext::abs2(ps);
00197       Scalar qs = q / q1;
00198       RealScalar q2 = numext::abs2(qs);
00199 
00200       RealScalar u = q1 * sqrt(p2 + q2);
00201       if(numext::real(p)<RealScalar(0))
00202         u = -u;
00203 
00204       p1 = abs(p);
00205       ps = p/p1;
00206       m_c = p1/u;
00207       m_s = -conj(ps) * (q/u);
00208       if(r) *r = ps * u;
00209     }
00210   }
00211 }
00212 
00213 // specialization for reals
00214 template<typename Scalar>
00215 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
00216 {
00217   using std::sqrt;
00218   using std::abs;
00219   if(q==Scalar(0))
00220   {
00221     m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
00222     m_s = Scalar(0);
00223     if(r) *r = abs(p);
00224   }
00225   else if(p==Scalar(0))
00226   {
00227     m_c = Scalar(0);
00228     m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
00229     if(r) *r = abs(q);
00230   }
00231   else if(abs(p) > abs(q))
00232   {
00233     Scalar t = q/p;
00234     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
00235     if(p<Scalar(0))
00236       u = -u;
00237     m_c = Scalar(1)/u;
00238     m_s = -t * m_c;
00239     if(r) *r = p * u;
00240   }
00241   else
00242   {
00243     Scalar t = p/q;
00244     Scalar u = sqrt(Scalar(1) + numext::abs2(t));
00245     if(q<Scalar(0))
00246       u = -u;
00247     m_s = -Scalar(1)/u;
00248     m_c = -t * m_s;
00249     if(r) *r = q * u;
00250   }
00251 
00252 }
00253 
00254 /****************************************************************************************
00255 *   Implementation of MatrixBase methods
00256 ****************************************************************************************/
00257 
00264 namespace internal {
00265 template<typename VectorX, typename VectorY, typename OtherScalar>
00266 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
00267 }
00268 
00275 template<typename Derived>
00276 template<typename OtherScalar>
00277 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00278 {
00279   RowXpr x(this->row(p));
00280   RowXpr y(this->row(q));
00281   internal::apply_rotation_in_the_plane(x, y, j);
00282 }
00283 
00290 template<typename Derived>
00291 template<typename OtherScalar>
00292 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00293 {
00294   ColXpr x(this->col(p));
00295   ColXpr y(this->col(q));
00296   internal::apply_rotation_in_the_plane(x, y, j.transpose());
00297 }
00298 
00299 namespace internal {
00300 template<typename VectorX, typename VectorY, typename OtherScalar>
00301 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
00302 {
00303   typedef typename VectorX::Index Index;
00304   typedef typename VectorX::Scalar Scalar;
00305   enum { PacketSize = packet_traits<Scalar>::size };
00306   typedef typename packet_traits<Scalar>::type Packet;
00307   eigen_assert(_x.size() == _y.size());
00308   Index size = _x.size();
00309   Index incrx = _x.innerStride();
00310   Index incry = _y.innerStride();
00311 
00312   Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
00313   Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
00314   
00315   OtherScalar c = j.c();
00316   OtherScalar s = j.s();
00317   if (c==OtherScalar(1) && s==OtherScalar(0))
00318     return;
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 = internal::first_aligned(y, size);
00330     Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
00331 
00332     const Packet pc = pset1<Packet>(c);
00333     const Packet ps = pset1<Packet>(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] =  c * xi + numext::conj(s) * yi;
00341       y[i] = -s * xi + numext::conj(c) * yi;
00342     }
00343 
00344     Scalar* EIGEN_RESTRICT px = x + alignedStart;
00345     Scalar* EIGEN_RESTRICT py = y + alignedStart;
00346 
00347     if(internal::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] =  c * xi + numext::conj(s) * yi;
00389       y[i] = -s * xi + numext::conj(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>(c);
00399     const Packet ps = pset1<Packet>(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 =  c * xi + numext::conj(s) * yi;
00422       *y = -s * xi + numext::conj(c) * yi;
00423       x += incrx;
00424       y += incry;
00425     }
00426   }
00427 }
00428 
00429 } // end namespace internal
00430 
00431 } // end namespace Eigen
00432 
00433 #endif // EIGEN_JACOBI_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:58:42