1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10/* NOTE The functions of this file have been adapted from the GMM++ library */
11
12//========================================================================
13//
14// Copyright (C) 2002-2007 Yves Renard
15//
16// This file is a part of GETFEM++
17//
18// Getfem++ is free software; you can redistribute it and/or modify
19// it under the terms of the GNU Lesser General Public License as
20// published by the Free Software Foundation; version 2.1 of the License.
21//
22// This program is distributed in the hope that it will be useful,
23// but WITHOUT ANY WARRANTY; without even the implied warranty of
24// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
25// GNU Lesser General Public License for more details.
26// You should have received a copy of the GNU Lesser General Public
27// License along with this program; if not, write to the Free Software
28// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301,
29// USA.
30//
31//========================================================================
32
33#include "../../../../Eigen/src/Core/util/NonMPL2.h"
34
35#ifndef EIGEN_CONSTRAINEDCG_H
36#define EIGEN_CONSTRAINEDCG_H
37
38#include <Eigen/Core>
39
40namespace Eigen {
41
42namespace internal {
43
44/** \ingroup IterativeSolvers_Module
45  * Compute the pseudo inverse of the non-square matrix C such that
46  * \f$ CINV = (C * C^T)^{-1} * C \f$ based on a conjugate gradient method.
47  *
48  * This function is internally used by constrained_cg.
49  */
50template <typename CMatrix, typename CINVMatrix>
51void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
52{
53  // optimisable : copie de la ligne, precalcul de C * trans(C).
54  typedef typename CMatrix::Scalar Scalar;
55  typedef typename CMatrix::Index Index;
56  // FIXME use sparse vectors ?
57  typedef Matrix<Scalar,Dynamic,1> TmpVec;
58
59  Index rows = C.rows(), cols = C.cols();
60
61  TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows);
62  Scalar rho, rho_1, alpha;
63  d.setZero();
64
65  CINV.startFill(); // FIXME estimate the number of non-zeros
66  for (Index i = 0; i < rows; ++i)
67  {
68    d[i] = 1.0;
69    rho = 1.0;
70    e.setZero();
71    r = d;
72    p = d;
73
74    while (rho >= 1e-38)
75    { /* conjugate gradient to compute e             */
76      /* which is the i-th row of inv(C * trans(C))  */
77      l = C.transpose() * p;
78      q = C * l;
79      alpha = rho / p.dot(q);
80      e +=  alpha * p;
81      r += -alpha * q;
82      rho_1 = rho;
83      rho = r.dot(r);
84      p = (rho/rho_1) * p + r;
85    }
86
87    l = C.transpose() * e; // l is the i-th row of CINV
88    // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse
89    for (Index j=0; j<l.size(); ++j)
90      if (l[j]<1e-15)
91        CINV.fill(i,j) = l[j];
92
93    d[i] = 0.0;
94  }
95  CINV.endFill();
96}
97
98
99
100/** \ingroup IterativeSolvers_Module
101  * Constrained conjugate gradient
102  *
103  * Computes the minimum of \f$ 1/2((Ax).x) - bx \f$ under the contraint \f$ Cx \le f \f$
104  */
105template<typename TMatrix, typename CMatrix,
106         typename VectorX, typename VectorB, typename VectorF>
107void constrained_cg(const TMatrix& A, const CMatrix& C, VectorX& x,
108                       const VectorB& b, const VectorF& f, IterationController &iter)
109{
110  typedef typename TMatrix::Scalar Scalar;
111  typedef typename TMatrix::Index Index;
112  typedef Matrix<Scalar,Dynamic,1>  TmpVec;
113
114  Scalar rho = 1.0, rho_1, lambda, gamma;
115  Index xSize = x.size();
116  TmpVec  p(xSize), q(xSize), q2(xSize),
117          r(xSize), old_z(xSize), z(xSize),
118          memox(xSize);
119  std::vector<bool> satured(C.rows());
120  p.setZero();
121  iter.setRhsNorm(sqrt(b.dot(b))); // gael vect_sp(PS, b, b)
122  if (iter.rhsNorm() == 0.0) iter.setRhsNorm(1.0);
123
124  SparseMatrix<Scalar,RowMajor> CINV(C.rows(), C.cols());
125  pseudo_inverse(C, CINV);
126
127  while(true)
128  {
129    // computation of residual
130    old_z = z;
131    memox = x;
132    r = b;
133    r += A * -x;
134    z = r;
135    bool transition = false;
136    for (Index i = 0; i < C.rows(); ++i)
137    {
138      Scalar al = C.row(i).dot(x) - f.coeff(i);
139      if (al >= -1.0E-15)
140      {
141        if (!satured[i])
142        {
143          satured[i] = true;
144          transition = true;
145        }
146        Scalar bb = CINV.row(i).dot(z);
147        if (bb > 0.0)
148          // FIXME: we should allow that: z += -bb * C.row(i);
149          for (typename CMatrix::InnerIterator it(C,i); it; ++it)
150            z.coeffRef(it.index()) -= bb*it.value();
151      }
152      else
153        satured[i] = false;
154    }
155
156    // descent direction
157    rho_1 = rho;
158    rho = r.dot(z);
159
160    if (iter.finished(rho)) break;
161
162    if (iter.noiseLevel() > 0 && transition) std::cerr << "CCG: transition\n";
163    if (transition || iter.first()) gamma = 0.0;
164    else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
165    p = z + gamma*p;
166
167    ++iter;
168    // one dimensionnal optimization
169    q = A * p;
170    lambda = rho / q.dot(p);
171    for (Index i = 0; i < C.rows(); ++i)
172    {
173      if (!satured[i])
174      {
175        Scalar bb = C.row(i).dot(p) - f[i];
176        if (bb > 0.0)
177          lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb);
178      }
179    }
180    x += lambda * p;
181    memox -= x;
182  }
183}
184
185} // end namespace internal
186
187} // end namespace Eigen
188
189#endif // EIGEN_CONSTRAINEDCG_H
190