00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef EIGEN_JACOBI_H
00027 #define EIGEN_JACOBI_H
00028
00029 namespace Eigen {
00030
00049 template<typename Scalar> class JacobiRotation
00050 {
00051 public:
00052 typedef typename NumTraits<Scalar>::Real RealScalar;
00053
00055 JacobiRotation() {}
00056
00058 JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
00059
00060 Scalar& c() { return m_c; }
00061 Scalar c() const { return m_c; }
00062 Scalar& s() { return m_s; }
00063 Scalar s() const { return m_s; }
00064
00066 JacobiRotation operator*(const JacobiRotation& other)
00067 {
00068 return JacobiRotation(m_c * other.m_c - internal::conj(m_s) * other.m_s,
00069 internal::conj(m_c * internal::conj(other.m_s) + internal::conj(m_s) * internal::conj(other.m_c)));
00070 }
00071
00073 JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); }
00074
00076 JacobiRotation adjoint() const { return JacobiRotation(internal::conj(m_c), -m_s); }
00077
00078 template<typename Derived>
00079 bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
00080 bool makeJacobi(RealScalar x, Scalar y, RealScalar z);
00081
00082 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
00083
00084 protected:
00085 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
00086 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
00087
00088 Scalar m_c, m_s;
00089 };
00090
00096 template<typename Scalar>
00097 bool JacobiRotation<Scalar>::makeJacobi(RealScalar x, Scalar y, RealScalar z)
00098 {
00099 typedef typename NumTraits<Scalar>::Real RealScalar;
00100 if(y == Scalar(0))
00101 {
00102 m_c = Scalar(1);
00103 m_s = Scalar(0);
00104 return false;
00105 }
00106 else
00107 {
00108 RealScalar tau = (x-z)/(RealScalar(2)*internal::abs(y));
00109 RealScalar w = internal::sqrt(internal::abs2(tau) + RealScalar(1));
00110 RealScalar t;
00111 if(tau>RealScalar(0))
00112 {
00113 t = RealScalar(1) / (tau + w);
00114 }
00115 else
00116 {
00117 t = RealScalar(1) / (tau - w);
00118 }
00119 RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
00120 RealScalar n = RealScalar(1) / internal::sqrt(internal::abs2(t)+RealScalar(1));
00121 m_s = - sign_t * (internal::conj(y) / internal::abs(y)) * internal::abs(t) * n;
00122 m_c = n;
00123 return true;
00124 }
00125 }
00126
00136 template<typename Scalar>
00137 template<typename Derived>
00138 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
00139 {
00140 return makeJacobi(internal::real(m.coeff(p,p)), m.coeff(p,q), internal::real(m.coeff(q,q)));
00141 }
00142
00159 template<typename Scalar>
00160 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
00161 {
00162 makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
00163 }
00164
00165
00166
00167 template<typename Scalar>
00168 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
00169 {
00170 if(q==Scalar(0))
00171 {
00172 m_c = internal::real(p)<0 ? Scalar(-1) : Scalar(1);
00173 m_s = 0;
00174 if(r) *r = m_c * p;
00175 }
00176 else if(p==Scalar(0))
00177 {
00178 m_c = 0;
00179 m_s = -q/internal::abs(q);
00180 if(r) *r = internal::abs(q);
00181 }
00182 else
00183 {
00184 RealScalar p1 = internal::norm1(p);
00185 RealScalar q1 = internal::norm1(q);
00186 if(p1>=q1)
00187 {
00188 Scalar ps = p / p1;
00189 RealScalar p2 = internal::abs2(ps);
00190 Scalar qs = q / p1;
00191 RealScalar q2 = internal::abs2(qs);
00192
00193 RealScalar u = internal::sqrt(RealScalar(1) + q2/p2);
00194 if(internal::real(p)<RealScalar(0))
00195 u = -u;
00196
00197 m_c = Scalar(1)/u;
00198 m_s = -qs*internal::conj(ps)*(m_c/p2);
00199 if(r) *r = p * u;
00200 }
00201 else
00202 {
00203 Scalar ps = p / q1;
00204 RealScalar p2 = internal::abs2(ps);
00205 Scalar qs = q / q1;
00206 RealScalar q2 = internal::abs2(qs);
00207
00208 RealScalar u = q1 * internal::sqrt(p2 + q2);
00209 if(internal::real(p)<RealScalar(0))
00210 u = -u;
00211
00212 p1 = internal::abs(p);
00213 ps = p/p1;
00214 m_c = p1/u;
00215 m_s = -internal::conj(ps) * (q/u);
00216 if(r) *r = ps * u;
00217 }
00218 }
00219 }
00220
00221
00222 template<typename Scalar>
00223 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
00224 {
00225
00226 if(q==Scalar(0))
00227 {
00228 m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
00229 m_s = Scalar(0);
00230 if(r) *r = internal::abs(p);
00231 }
00232 else if(p==Scalar(0))
00233 {
00234 m_c = Scalar(0);
00235 m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
00236 if(r) *r = internal::abs(q);
00237 }
00238 else if(internal::abs(p) > internal::abs(q))
00239 {
00240 Scalar t = q/p;
00241 Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00242 if(p<Scalar(0))
00243 u = -u;
00244 m_c = Scalar(1)/u;
00245 m_s = -t * m_c;
00246 if(r) *r = p * u;
00247 }
00248 else
00249 {
00250 Scalar t = p/q;
00251 Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00252 if(q<Scalar(0))
00253 u = -u;
00254 m_s = -Scalar(1)/u;
00255 m_c = -t * m_s;
00256 if(r) *r = q * u;
00257 }
00258
00259 }
00260
00261
00262
00263
00264
00271 namespace internal {
00272 template<typename VectorX, typename VectorY, typename OtherScalar>
00273 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
00274 }
00275
00282 template<typename Derived>
00283 template<typename OtherScalar>
00284 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00285 {
00286 RowXpr x(this->row(p));
00287 RowXpr y(this->row(q));
00288 internal::apply_rotation_in_the_plane(x, y, j);
00289 }
00290
00297 template<typename Derived>
00298 template<typename OtherScalar>
00299 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00300 {
00301 ColXpr x(this->col(p));
00302 ColXpr y(this->col(q));
00303 internal::apply_rotation_in_the_plane(x, y, j.transpose());
00304 }
00305
00306 namespace internal {
00307 template<typename VectorX, typename VectorY, typename OtherScalar>
00308 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
00309 {
00310 typedef typename VectorX::Index Index;
00311 typedef typename VectorX::Scalar Scalar;
00312 enum { PacketSize = packet_traits<Scalar>::size };
00313 typedef typename packet_traits<Scalar>::type Packet;
00314 eigen_assert(_x.size() == _y.size());
00315 Index size = _x.size();
00316 Index incrx = _x.innerStride();
00317 Index incry = _y.innerStride();
00318
00319 Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
00320 Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
00321
00322
00323
00324 if(VectorX::SizeAtCompileTime == Dynamic &&
00325 (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00326 ((incrx==1 && incry==1) || PacketSize == 1))
00327 {
00328
00329 enum { Peeling = 2 };
00330
00331 Index alignedStart = internal::first_aligned(y, size);
00332 Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
00333
00334 const Packet pc = pset1<Packet>(j.c());
00335 const Packet ps = pset1<Packet>(j.s());
00336 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00337
00338 for(Index i=0; i<alignedStart; ++i)
00339 {
00340 Scalar xi = x[i];
00341 Scalar yi = y[i];
00342 x[i] = j.c() * xi + conj(j.s()) * yi;
00343 y[i] = -j.s() * xi + conj(j.c()) * yi;
00344 }
00345
00346 Scalar* EIGEN_RESTRICT px = x + alignedStart;
00347 Scalar* EIGEN_RESTRICT py = y + alignedStart;
00348
00349 if(internal::first_aligned(x, size)==alignedStart)
00350 {
00351 for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
00352 {
00353 Packet xi = pload<Packet>(px);
00354 Packet yi = pload<Packet>(py);
00355 pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00356 pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00357 px += PacketSize;
00358 py += PacketSize;
00359 }
00360 }
00361 else
00362 {
00363 Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
00364 for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
00365 {
00366 Packet xi = ploadu<Packet>(px);
00367 Packet xi1 = ploadu<Packet>(px+PacketSize);
00368 Packet yi = pload <Packet>(py);
00369 Packet yi1 = pload <Packet>(py+PacketSize);
00370 pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00371 pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
00372 pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00373 pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
00374 px += Peeling*PacketSize;
00375 py += Peeling*PacketSize;
00376 }
00377 if(alignedEnd!=peelingEnd)
00378 {
00379 Packet xi = ploadu<Packet>(x+peelingEnd);
00380 Packet yi = pload <Packet>(y+peelingEnd);
00381 pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00382 pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00383 }
00384 }
00385
00386 for(Index i=alignedEnd; i<size; ++i)
00387 {
00388 Scalar xi = x[i];
00389 Scalar yi = y[i];
00390 x[i] = j.c() * xi + conj(j.s()) * yi;
00391 y[i] = -j.s() * xi + conj(j.c()) * yi;
00392 }
00393 }
00394
00395
00396 else if(VectorX::SizeAtCompileTime != Dynamic &&
00397 (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00398 (VectorX::Flags & VectorY::Flags & AlignedBit))
00399 {
00400 const Packet pc = pset1<Packet>(j.c());
00401 const Packet ps = pset1<Packet>(j.s());
00402 conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00403 Scalar* EIGEN_RESTRICT px = x;
00404 Scalar* EIGEN_RESTRICT py = y;
00405 for(Index i=0; i<size; i+=PacketSize)
00406 {
00407 Packet xi = pload<Packet>(px);
00408 Packet yi = pload<Packet>(py);
00409 pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00410 pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00411 px += PacketSize;
00412 py += PacketSize;
00413 }
00414 }
00415
00416
00417 else
00418 {
00419 for(Index i=0; i<size; ++i)
00420 {
00421 Scalar xi = *x;
00422 Scalar yi = *y;
00423 *x = j.c() * xi + conj(j.s()) * yi;
00424 *y = -j.s() * xi + conj(j.c()) * yi;
00425 x += incrx;
00426 y += incry;
00427 }
00428 }
00429 }
00430
00431 }
00432
00433 }
00434
00435 #endif // EIGEN_JACOBI_H