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