Cleanup: style
[blender.git] / source / blender / physics / intern / ConstrainedConjugateGradient.h
1
2 #ifndef __CONSTRAINEDCONJUGATEGRADIENT_H__
3 #define __CONSTRAINEDCONJUGATEGRADIENT_H__
4
5 #include <Eigen/Core>
6
7 namespace Eigen { 
8
9 namespace internal {
10
11 /** \internal Low-level conjugate gradient algorithm
12   * \param mat The matrix A
13   * \param rhs The right hand side vector b
14   * \param x On input and initial solution, on output the computed solution.
15   * \param precond A preconditioner being able to efficiently solve for an
16   *                approximation of Ax=b (regardless of b)
17   * \param iters On input the max number of iteration, on output the number of performed iterations.
18   * \param tol_error On input the tolerance error, on output an estimation of the relative error.
19   */
20 template<typename MatrixType, typename Rhs, typename Dest, typename FilterMatrixType, typename Preconditioner>
21 EIGEN_DONT_INLINE
22 void constrained_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
23                                     const FilterMatrixType &filter,
24                                     const Preconditioner& precond, int& iters,
25                                     typename Dest::RealScalar& tol_error)
26 {
27   using std::sqrt;
28   using std::abs;
29   typedef typename Dest::RealScalar RealScalar;
30   typedef typename Dest::Scalar Scalar;
31   typedef Matrix<Scalar,Dynamic,1> VectorType;
32   
33   RealScalar tol = tol_error;
34   int maxIters = iters;
35   
36   int n = mat.cols();
37
38   VectorType residual = filter * (rhs - mat * x); //initial residual
39
40   RealScalar rhsNorm2 = (filter * rhs).squaredNorm();
41   if(rhsNorm2 == 0) 
42   {
43     /* XXX TODO set constrained result here */
44     x.setZero();
45     iters = 0;
46     tol_error = 0;
47     return;
48   }
49   RealScalar threshold = tol*tol*rhsNorm2;
50   RealScalar residualNorm2 = residual.squaredNorm();
51   if (residualNorm2 < threshold)
52   {
53     iters = 0;
54     tol_error = sqrt(residualNorm2 / rhsNorm2);
55     return;
56   }
57   
58   VectorType p(n);
59   p = filter * precond.solve(residual);          //initial search direction
60
61   VectorType z(n), tmp(n);
62   RealScalar absNew = numext::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
63   int i = 0;
64   while(i < maxIters)
65   {
66     tmp.noalias() = filter * (mat * p);            // the bottleneck of the algorithm
67
68     Scalar alpha = absNew / p.dot(tmp);         // the amount we travel on dir
69     x += alpha * p;                             // update solution
70     residual -= alpha * tmp;                    // update residue
71     
72     residualNorm2 = residual.squaredNorm();
73     if(residualNorm2 < threshold)
74       break;
75     
76     z = precond.solve(residual);                // approximately solve for "A z = residual"
77
78     RealScalar absOld = absNew;
79     absNew = numext::real(residual.dot(z));     // update the absolute value of r
80     RealScalar beta = absNew / absOld;          // calculate the Gram-Schmidt value used to create the new search direction
81     p = filter * (z + beta * p);             // update search direction
82     i++;
83   }
84   tol_error = sqrt(residualNorm2 / rhsNorm2);
85   iters = i;
86 }
87
88 }
89
90 #if 0 /* unused */
91 template<typename MatrixType>
92 struct MatrixFilter
93 {
94         MatrixFilter() :
95             m_cmat(NULL)
96         {
97         }
98         
99         MatrixFilter(const MatrixType &cmat) :
100             m_cmat(&cmat)
101         {
102         }
103         
104         void setMatrix(const MatrixType &cmat) { m_cmat = &cmat; }
105         
106         template <typename VectorType>
107         void apply(VectorType v) const
108         {
109                 v = (*m_cmat) * v;
110         }
111         
112 protected:
113         const MatrixType *m_cmat;
114 };
115 #endif
116
117 template< typename _MatrixType, int _UpLo=Lower,
118           typename _FilterMatrixType = _MatrixType,
119           typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
120 class ConstrainedConjugateGradient;
121
122 namespace internal {
123
124 template< typename _MatrixType, int _UpLo, typename _FilterMatrixType, typename _Preconditioner>
125 struct traits<ConstrainedConjugateGradient<_MatrixType,_UpLo,_FilterMatrixType,_Preconditioner> >
126 {
127   typedef _MatrixType MatrixType;
128   typedef _FilterMatrixType FilterMatrixType;
129   typedef _Preconditioner Preconditioner;
130 };
131
132 }
133
134 /** \ingroup IterativeLinearSolvers_Module
135   * \brief A conjugate gradient solver for sparse self-adjoint problems with additional constraints
136   *
137   * This class allows to solve for A.x = b sparse linear problems using a conjugate gradient algorithm.
138   * The sparse matrix A must be selfadjoint. The vectors x and b can be either dense or sparse.
139   *
140   * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix.
141   * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
142   *               or Upper. Default is Lower.
143   * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
144   *
145   * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
146   * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
147   * and NumTraits<Scalar>::epsilon() for the tolerance.
148   * 
149   * This class can be used as the direct solver classes. Here is a typical usage example:
150   * \code
151   * int n = 10000;
152   * VectorXd x(n), b(n);
153   * SparseMatrix<double> A(n,n);
154   * // fill A and b
155   * ConjugateGradient<SparseMatrix<double> > cg;
156   * cg.compute(A);
157   * x = cg.solve(b);
158   * std::cout << "#iterations:     " << cg.iterations() << std::endl;
159   * std::cout << "estimated error: " << cg.error()      << std::endl;
160   * // update b, and solve again
161   * x = cg.solve(b);
162   * \endcode
163   * 
164   * By default the iterations start with x=0 as an initial guess of the solution.
165   * One can control the start using the solveWithGuess() method. Here is a step by
166   * step execution example starting with a random guess and printing the evolution
167   * of the estimated error:
168   * * \code
169   * x = VectorXd::Random(n);
170   * cg.setMaxIterations(1);
171   * int i = 0;
172   * do {
173   *   x = cg.solveWithGuess(b,x);
174   *   std::cout << i << " : " << cg.error() << std::endl;
175   *   ++i;
176   * } while (cg.info()!=Success && i<100);
177   * \endcode
178   * Note that such a step by step excution is slightly slower.
179   * 
180   * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
181   */
182 template< typename _MatrixType, int _UpLo, typename _FilterMatrixType, typename _Preconditioner>
183 class ConstrainedConjugateGradient : public IterativeSolverBase<ConstrainedConjugateGradient<_MatrixType,_UpLo,_FilterMatrixType,_Preconditioner> >
184 {
185   typedef IterativeSolverBase<ConstrainedConjugateGradient> Base;
186   using Base::mp_matrix;
187   using Base::m_error;
188   using Base::m_iterations;
189   using Base::m_info;
190   using Base::m_isInitialized;
191 public:
192   typedef _MatrixType MatrixType;
193   typedef typename MatrixType::Scalar Scalar;
194   typedef typename MatrixType::Index Index;
195   typedef typename MatrixType::RealScalar RealScalar;
196   typedef _FilterMatrixType FilterMatrixType;
197   typedef _Preconditioner Preconditioner;
198
199   enum {
200     UpLo = _UpLo
201   };
202
203 public:
204
205   /** Default constructor. */
206   ConstrainedConjugateGradient() : Base() {}
207
208   /** Initialize the solver with matrix \a A for further \c Ax=b solving.
209     * 
210     * This constructor is a shortcut for the default constructor followed
211     * by a call to compute().
212     * 
213     * \warning this class stores a reference to the matrix A as well as some
214     * precomputed values that depend on it. Therefore, if \a A is changed
215     * this class becomes invalid. Call compute() to update it with the new
216     * matrix A, or modify a copy of A.
217     */
218   ConstrainedConjugateGradient(const MatrixType& A) : Base(A) {}
219
220   ~ConstrainedConjugateGradient() {}
221
222   FilterMatrixType &filter() { return m_filter; }
223   const FilterMatrixType &filter() const { return m_filter; }
224
225   /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A
226     * \a x0 as an initial solution.
227     *
228     * \sa compute()
229     */
230   template<typename Rhs,typename Guess>
231   inline const internal::solve_retval_with_guess<ConstrainedConjugateGradient, Rhs, Guess>
232   solveWithGuess(const MatrixBase<Rhs>& b, const Guess& x0) const
233   {
234     eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
235     eigen_assert(Base::rows()==b.rows()
236               && "ConjugateGradient::solve(): invalid number of rows of the right hand side matrix b");
237     return internal::solve_retval_with_guess
238             <ConstrainedConjugateGradient, Rhs, Guess>(*this, b.derived(), x0);
239   }
240
241   /** \internal */
242   template<typename Rhs,typename Dest>
243   void _solveWithGuess(const Rhs& b, Dest& x) const
244   {
245     m_iterations = Base::maxIterations();
246     m_error = Base::m_tolerance;
247
248     for(int j=0; j<b.cols(); ++j)
249     {
250       m_iterations = Base::maxIterations();
251       m_error = Base::m_tolerance;
252
253       typename Dest::ColXpr xj(x,j);
254       internal::constrained_conjugate_gradient(mp_matrix->template selfadjointView<UpLo>(), b.col(j), xj, m_filter,
255                                                    Base::m_preconditioner, m_iterations, m_error);
256     }
257
258     m_isInitialized = true;
259     m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
260   }
261   
262   /** \internal */
263   template<typename Rhs,typename Dest>
264   void _solve(const Rhs& b, Dest& x) const
265   {
266     x.setOnes();
267     _solveWithGuess(b,x);
268   }
269
270 protected:
271   FilterMatrixType m_filter;
272 };
273
274
275 namespace internal {
276
277 template<typename _MatrixType, int _UpLo, typename _Filter, typename _Preconditioner, typename Rhs>
278 struct solve_retval<ConstrainedConjugateGradient<_MatrixType,_UpLo,_Filter,_Preconditioner>, Rhs>
279   : solve_retval_base<ConstrainedConjugateGradient<_MatrixType,_UpLo,_Filter,_Preconditioner>, Rhs>
280 {
281   typedef ConstrainedConjugateGradient<_MatrixType,_UpLo,_Filter,_Preconditioner> Dec;
282   EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs)
283
284   template<typename Dest> void evalTo(Dest& dst) const
285   {
286     dec()._solve(rhs(),dst);
287   }
288 };
289
290 } // end namespace internal
291
292 } // end namespace Eigen
293
294 #endif // __CONSTRAINEDCONJUGATEGRADIENT_H__