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