| // This file is part of Eigen, a lightweight C++ template library |
| // for linear algebra. |
| // |
| // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> |
| // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> |
| // |
| // This Source Code Form is subject to the terms of the Mozilla |
| // Public License v. 2.0. If a copy of the MPL was not distributed |
| // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
| |
| #ifndef EIGEN_JACOBI_H |
| #define EIGEN_JACOBI_H |
| |
| #include "./InternalHeaderCheck.h" |
| |
| namespace Eigen { |
| |
| /** \ingroup Jacobi_Module |
| * \jacobi_module |
| * \class JacobiRotation |
| * \brief Rotation given by a cosine-sine pair. |
| * |
| * This class represents a Jacobi or Givens rotation. |
| * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by |
| * its cosine \c c and sine \c s as follow: |
| * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} \right ) \f$ |
| * |
| * You can apply the respective counter-clockwise rotation to a column vector \c v by |
| * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code: |
| * \code |
| * v.applyOnTheLeft(J.adjoint()); |
| * \endcode |
| * |
| * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() |
| */ |
| template<typename Scalar> class JacobiRotation |
| { |
| public: |
| typedef typename NumTraits<Scalar>::Real RealScalar; |
| |
| /** Default constructor without any initialization. */ |
| EIGEN_DEVICE_FUNC |
| JacobiRotation() {} |
| |
| /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */ |
| EIGEN_DEVICE_FUNC |
| JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {} |
| |
| EIGEN_DEVICE_FUNC Scalar& c() { return m_c; } |
| EIGEN_DEVICE_FUNC Scalar c() const { return m_c; } |
| EIGEN_DEVICE_FUNC Scalar& s() { return m_s; } |
| EIGEN_DEVICE_FUNC Scalar s() const { return m_s; } |
| |
| /** Concatenates two planar rotation */ |
| EIGEN_DEVICE_FUNC |
| JacobiRotation operator*(const JacobiRotation& other) |
| { |
| using numext::conj; |
| return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s, |
| conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c))); |
| } |
| |
| /** Returns the transposed transformation */ |
| EIGEN_DEVICE_FUNC |
| JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); } |
| |
| /** Returns the adjoint transformation */ |
| EIGEN_DEVICE_FUNC |
| JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); } |
| |
| template<typename Derived> |
| EIGEN_DEVICE_FUNC |
| bool makeJacobi(const MatrixBase<Derived>&, Index p, Index q); |
| EIGEN_DEVICE_FUNC |
| bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z); |
| |
| EIGEN_DEVICE_FUNC |
| void makeGivens(const Scalar& p, const Scalar& q, Scalar* r=0); |
| |
| protected: |
| EIGEN_DEVICE_FUNC |
| void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type); |
| EIGEN_DEVICE_FUNC |
| void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type); |
| |
| Scalar m_c, m_s; |
| }; |
| |
| /** 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 |
| * \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$ |
| * |
| * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() |
| */ |
| template<typename Scalar> |
| EIGEN_DEVICE_FUNC |
| bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z) |
| { |
| using std::sqrt; |
| using std::abs; |
| |
| RealScalar deno = RealScalar(2)*abs(y); |
| if(deno < (std::numeric_limits<RealScalar>::min)()) |
| { |
| m_c = Scalar(1); |
| m_s = Scalar(0); |
| return false; |
| } |
| else |
| { |
| RealScalar tau = (x-z)/deno; |
| RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1)); |
| RealScalar t; |
| if(tau>RealScalar(0)) |
| { |
| t = RealScalar(1) / (tau + w); |
| } |
| else |
| { |
| t = RealScalar(1) / (tau - w); |
| } |
| RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1); |
| RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1)); |
| m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n; |
| m_c = n; |
| return true; |
| } |
| } |
| |
| /** 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 |
| * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields |
| * a diagonal matrix \f$ A = J^* B J \f$ |
| * |
| * Example: \include Jacobi_makeJacobi.cpp |
| * Output: \verbinclude Jacobi_makeJacobi.out |
| * |
| * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() |
| */ |
| template<typename Scalar> |
| template<typename Derived> |
| EIGEN_DEVICE_FUNC |
| inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, Index q) |
| { |
| return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q))); |
| } |
| |
| /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector |
| * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields: |
| * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$. |
| * |
| * The value of \a r is returned if \a r is not null (the default is null). |
| * Also note that G is built such that the cosine is always real. |
| * |
| * Example: \include Jacobi_makeGivens.cpp |
| * Output: \verbinclude Jacobi_makeGivens.out |
| * |
| * This function implements the continuous Givens rotation generation algorithm |
| * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem. |
| * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000. |
| * |
| * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() |
| */ |
| template<typename Scalar> |
| EIGEN_DEVICE_FUNC |
| void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r) |
| { |
| makeGivens(p, q, r, std::conditional_t<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>()); |
| } |
| |
| |
| // specialization for complexes |
| template<typename Scalar> |
| EIGEN_DEVICE_FUNC |
| void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type) |
| { |
| using std::sqrt; |
| using std::abs; |
| using numext::conj; |
| |
| if(q==Scalar(0)) |
| { |
| m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1); |
| m_s = 0; |
| if(r) *r = m_c * p; |
| } |
| else if(p==Scalar(0)) |
| { |
| m_c = 0; |
| m_s = -q/abs(q); |
| if(r) *r = abs(q); |
| } |
| else |
| { |
| RealScalar p1 = numext::norm1(p); |
| RealScalar q1 = numext::norm1(q); |
| if(p1>=q1) |
| { |
| Scalar ps = p / p1; |
| RealScalar p2 = numext::abs2(ps); |
| Scalar qs = q / p1; |
| RealScalar q2 = numext::abs2(qs); |
| |
| RealScalar u = sqrt(RealScalar(1) + q2/p2); |
| if(numext::real(p)<RealScalar(0)) |
| u = -u; |
| |
| m_c = Scalar(1)/u; |
| m_s = -qs*conj(ps)*(m_c/p2); |
| if(r) *r = p * u; |
| } |
| else |
| { |
| Scalar ps = p / q1; |
| RealScalar p2 = numext::abs2(ps); |
| Scalar qs = q / q1; |
| RealScalar q2 = numext::abs2(qs); |
| |
| RealScalar u = q1 * sqrt(p2 + q2); |
| if(numext::real(p)<RealScalar(0)) |
| u = -u; |
| |
| p1 = abs(p); |
| ps = p/p1; |
| m_c = p1/u; |
| m_s = -conj(ps) * (q/u); |
| if(r) *r = ps * u; |
| } |
| } |
| } |
| |
| // specialization for reals |
| template<typename Scalar> |
| EIGEN_DEVICE_FUNC |
| void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type) |
| { |
| using std::sqrt; |
| using std::abs; |
| if(numext::is_exactly_zero(q)) |
| { |
| m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1); |
| m_s = Scalar(0); |
| if(r) *r = abs(p); |
| } |
| else if(numext::is_exactly_zero(p)) |
| { |
| m_c = Scalar(0); |
| m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1); |
| if(r) *r = abs(q); |
| } |
| else if(abs(p) > abs(q)) |
| { |
| Scalar t = q/p; |
| Scalar u = sqrt(Scalar(1) + numext::abs2(t)); |
| if(p<Scalar(0)) |
| u = -u; |
| m_c = Scalar(1)/u; |
| m_s = -t * m_c; |
| if(r) *r = p * u; |
| } |
| else |
| { |
| Scalar t = p/q; |
| Scalar u = sqrt(Scalar(1) + numext::abs2(t)); |
| if(q<Scalar(0)) |
| u = -u; |
| m_s = -Scalar(1)/u; |
| m_c = -t * m_s; |
| if(r) *r = q * u; |
| } |
| |
| } |
| |
| /**************************************************************************************** |
| * Implementation of MatrixBase methods |
| ****************************************************************************************/ |
| |
| namespace internal { |
| /** \jacobi_module |
| * Applies the clock wise 2D rotation \a j to the set of 2D vectors of coordinates \a x and \a y: |
| * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$ |
| * |
| * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() |
| */ |
| template<typename VectorX, typename VectorY, typename OtherScalar> |
| EIGEN_DEVICE_FUNC |
| void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j); |
| } |
| |
| /** \jacobi_module |
| * 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, |
| * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$. |
| * |
| * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane() |
| */ |
| template<typename Derived> |
| template<typename OtherScalar> |
| EIGEN_DEVICE_FUNC |
| inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j) |
| { |
| RowXpr x(this->row(p)); |
| RowXpr y(this->row(q)); |
| internal::apply_rotation_in_the_plane(x, y, j); |
| } |
| |
| /** \ingroup Jacobi_Module |
| * 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 |
| * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$. |
| * |
| * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane() |
| */ |
| template<typename Derived> |
| template<typename OtherScalar> |
| EIGEN_DEVICE_FUNC |
| inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j) |
| { |
| ColXpr x(this->col(p)); |
| ColXpr y(this->col(q)); |
| internal::apply_rotation_in_the_plane(x, y, j.transpose()); |
| } |
| |
| namespace internal { |
| |
| template<typename Scalar, typename OtherScalar, |
| int SizeAtCompileTime, int MinAlignment, bool Vectorizable> |
| struct apply_rotation_in_the_plane_selector |
| { |
| static EIGEN_DEVICE_FUNC |
| inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s) |
| { |
| for(Index i=0; i<size; ++i) |
| { |
| Scalar xi = *x; |
| Scalar yi = *y; |
| *x = c * xi + numext::conj(s) * yi; |
| *y = -s * xi + numext::conj(c) * yi; |
| x += incrx; |
| y += incry; |
| } |
| } |
| }; |
| |
| template<typename Scalar, typename OtherScalar, |
| int SizeAtCompileTime, int MinAlignment> |
| struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */> |
| { |
| static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s) |
| { |
| typedef typename packet_traits<Scalar>::type Packet; |
| typedef typename packet_traits<OtherScalar>::type OtherPacket; |
| |
| enum { |
| RequiredAlignment = plain_enum_max(unpacket_traits<Packet>::alignment, |
| unpacket_traits<OtherPacket>::alignment), |
| PacketSize = packet_traits<Scalar>::size, |
| OtherPacketSize = packet_traits<OtherScalar>::size |
| }; |
| |
| /*** dynamic-size vectorized paths ***/ |
| if(size >= 2 * PacketSize && SizeAtCompileTime == Dynamic && ((incrx == 1 && incry == 1) || PacketSize == 1)) |
| { |
| // both vectors are sequentially stored in memory => vectorization |
| enum { Peeling = 2 }; |
| |
| Index alignedStart = internal::first_default_aligned(y, size); |
| Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize; |
| |
| const OtherPacket pc = pset1<OtherPacket>(c); |
| const OtherPacket ps = pset1<OtherPacket>(s); |
| conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj; |
| conj_helper<OtherPacket,Packet,false,false> pm; |
| |
| for(Index i=0; i<alignedStart; ++i) |
| { |
| Scalar xi = x[i]; |
| Scalar yi = y[i]; |
| x[i] = c * xi + numext::conj(s) * yi; |
| y[i] = -s * xi + numext::conj(c) * yi; |
| } |
| |
| Scalar* EIGEN_RESTRICT px = x + alignedStart; |
| Scalar* EIGEN_RESTRICT py = y + alignedStart; |
| |
| if(internal::first_default_aligned(x, size)==alignedStart) |
| { |
| for(Index i=alignedStart; i<alignedEnd; i+=PacketSize) |
| { |
| Packet xi = pload<Packet>(px); |
| Packet yi = pload<Packet>(py); |
| pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi))); |
| pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi))); |
| px += PacketSize; |
| py += PacketSize; |
| } |
| } |
| else |
| { |
| Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize); |
| for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize) |
| { |
| Packet xi = ploadu<Packet>(px); |
| Packet xi1 = ploadu<Packet>(px+PacketSize); |
| Packet yi = pload <Packet>(py); |
| Packet yi1 = pload <Packet>(py+PacketSize); |
| pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi))); |
| pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1))); |
| pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi))); |
| pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1))); |
| px += Peeling*PacketSize; |
| py += Peeling*PacketSize; |
| } |
| if(alignedEnd!=peelingEnd) |
| { |
| Packet xi = ploadu<Packet>(x+peelingEnd); |
| Packet yi = pload <Packet>(y+peelingEnd); |
| pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi))); |
| pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi))); |
| } |
| } |
| |
| for(Index i=alignedEnd; i<size; ++i) |
| { |
| Scalar xi = x[i]; |
| Scalar yi = y[i]; |
| x[i] = c * xi + numext::conj(s) * yi; |
| y[i] = -s * xi + numext::conj(c) * yi; |
| } |
| } |
| |
| /*** fixed-size vectorized path ***/ |
| else if(SizeAtCompileTime != Dynamic && MinAlignment >= RequiredAlignment) |
| { |
| const OtherPacket pc = pset1<OtherPacket>(c); |
| const OtherPacket ps = pset1<OtherPacket>(s); |
| conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj; |
| conj_helper<OtherPacket,Packet,false,false> pm; |
| Scalar* EIGEN_RESTRICT px = x; |
| Scalar* EIGEN_RESTRICT py = y; |
| for(Index i=0; i<size; i+=PacketSize) |
| { |
| Packet xi = pload<Packet>(px); |
| Packet yi = pload<Packet>(py); |
| pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi))); |
| pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi))); |
| px += PacketSize; |
| py += PacketSize; |
| } |
| } |
| |
| /*** non-vectorized path ***/ |
| else |
| { |
| apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,false>::run(x,incrx,y,incry,size,c,s); |
| } |
| } |
| }; |
| |
| template<typename VectorX, typename VectorY, typename OtherScalar> |
| EIGEN_DEVICE_FUNC |
| void inline apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j) |
| { |
| typedef typename VectorX::Scalar Scalar; |
| constexpr bool Vectorizable = (int(evaluator<VectorX>::Flags) & int(evaluator<VectorY>::Flags) & PacketAccessBit) && |
| (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size)); |
| |
| eigen_assert(xpr_x.size() == xpr_y.size()); |
| Index size = xpr_x.size(); |
| Index incrx = xpr_x.derived().innerStride(); |
| Index incry = xpr_y.derived().innerStride(); |
| |
| Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0); |
| Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0); |
| |
| OtherScalar c = j.c(); |
| OtherScalar s = j.s(); |
| if (numext::is_exactly_one(c) && numext::is_exactly_zero(s)) |
| return; |
| |
| apply_rotation_in_the_plane_selector< |
| Scalar,OtherScalar, |
| VectorX::SizeAtCompileTime, |
| plain_enum_min(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment), |
| Vectorizable>::run(x,incrx,y,incry,size,c,s); |
| } |
| |
| } // end namespace internal |
| |
| } // end namespace Eigen |
| |
| #endif // EIGEN_JACOBI_H |
