Cycles: Fix missing type declaration in OpenCL image
[blender.git] / intern / eigen / intern / linear_solver.cc
1 /*
2  *  Sparse linear solver.
3  *  Copyright (C) 2004 Bruno Levy
4  *  Copyright (C) 2005-2015 Blender Foundation
5  *
6  *  This program is free software; you can redistribute it and/or modify
7  *  it under the terms of the GNU General Public License as published by
8  *  the Free Software Foundation; either version 2 of the License, or
9  *  (at your option) any later version.
10  *
11  *  This program is distributed in the hope that it will be useful,
12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  *  GNU General Public License for more details.
15  *
16  *  You should have received a copy of the GNU General Public License
17  *  along with this program; if not, write to the Free Software
18  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19  *
20  *  If you modify this software, you should include a notice giving the
21  *  name of the person performing the modification, the date of modification,
22  *  and the reason for such modification.
23  */
24
25 #include "linear_solver.h"
26
27 #include <Eigen/Sparse>
28
29 #include <algorithm>
30 #include <cassert>
31 #include <cstdlib>
32 #include <iostream>
33 #include <vector>
34
35 /* Eigen data structures */
36
37 typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
38 typedef Eigen::SparseLU<EigenSparseMatrix> EigenSparseLU;
39 typedef Eigen::VectorXd EigenVectorX;
40 typedef Eigen::Triplet<double> EigenTriplet;
41
42 /* Linear Solver data structure */
43
44 struct LinearSolver
45 {
46         struct Coeff
47         {
48                 Coeff()
49                 {
50                         index = 0;
51                         value = 0.0;
52                 }
53
54                 int index;
55                 double value;
56         };
57
58         struct Variable
59         {
60                 Variable()
61                 {
62                         memset(value, 0, sizeof(value));
63                         locked = false;
64                         index = 0;
65                 }
66
67                 double value[4];
68                 bool locked;
69                 int index;
70                 std::vector<Coeff> a;
71         };
72
73         enum State
74         {
75                 STATE_VARIABLES_CONSTRUCT,
76                 STATE_MATRIX_CONSTRUCT,
77                 STATE_MATRIX_SOLVED
78         };
79
80         LinearSolver(int num_rows_, int num_variables_, int num_rhs_, bool lsq_)
81         {
82                 assert(num_variables_ > 0);
83                 assert(num_rhs_ <= 4);
84
85                 state = STATE_VARIABLES_CONSTRUCT;
86                 m = 0;
87                 n = 0;
88                 sparseLU = NULL;
89                 num_variables = num_variables_;
90                 num_rhs = num_rhs_;
91                 num_rows = num_rows_;
92                 least_squares = lsq_;
93
94                 variable.resize(num_variables);
95         }
96
97         ~LinearSolver()
98         {
99                 delete sparseLU;
100         }
101
102         State state;
103
104         int n;
105         int m;
106
107         std::vector<EigenTriplet> Mtriplets;
108         EigenSparseMatrix M;
109         EigenSparseMatrix MtM;
110         std::vector<EigenVectorX> b;
111         std::vector<EigenVectorX> x;
112
113         EigenSparseLU *sparseLU;
114
115         int num_variables;
116         std::vector<Variable> variable;
117
118         int num_rows;
119         int num_rhs;
120
121         bool least_squares;
122 };
123
124 LinearSolver *EIG_linear_solver_new(int num_rows, int num_columns, int num_rhs)
125 {
126         return new LinearSolver(num_rows, num_columns, num_rhs, false);
127 }
128
129 LinearSolver *EIG_linear_least_squares_solver_new(int num_rows, int num_columns, int num_rhs)
130 {
131         return new LinearSolver(num_rows, num_columns, num_rhs, true);
132 }
133
134 void EIG_linear_solver_delete(LinearSolver *solver)
135 {
136         delete solver;
137 }
138
139 /* Variables */
140
141 void EIG_linear_solver_variable_set(LinearSolver *solver, int rhs, int index, double value)
142 {
143         solver->variable[index].value[rhs] = value;
144 }
145
146 double EIG_linear_solver_variable_get(LinearSolver *solver, int rhs, int index)
147 {
148         return solver->variable[index].value[rhs];
149 }
150
151 void EIG_linear_solver_variable_lock(LinearSolver *solver, int index)
152 {
153         if (!solver->variable[index].locked) {
154                 assert(solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT);
155                 solver->variable[index].locked = true;
156         }
157 }
158
159 void EIG_linear_solver_variable_unlock(LinearSolver *solver, int index)
160 {
161         if (solver->variable[index].locked) {
162                 assert(solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT);
163                 solver->variable[index].locked = false;
164         }
165 }
166
167 static void linear_solver_variables_to_vector(LinearSolver *solver)
168 {
169         int num_rhs = solver->num_rhs;
170
171         for (int i = 0; i < solver->num_variables; i++) {
172                 LinearSolver::Variable* v = &solver->variable[i];
173                 if (!v->locked) {
174                         for (int j = 0; j < num_rhs; j++)
175                                 solver->x[j][v->index] = v->value[j];
176                 }
177         }
178 }
179
180 static void linear_solver_vector_to_variables(LinearSolver *solver)
181 {
182         int num_rhs = solver->num_rhs;
183
184         for (int i = 0; i < solver->num_variables; i++) {
185                 LinearSolver::Variable* v = &solver->variable[i];
186                 if (!v->locked) {
187                         for (int j = 0; j < num_rhs; j++)
188                                 v->value[j] = solver->x[j][v->index];
189                 }
190         }
191 }
192
193 /* Matrix */
194
195 static void linear_solver_ensure_matrix_construct(LinearSolver *solver)
196 {
197         /* transition to matrix construction if necessary */
198         if (solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT) {
199                 int n = 0;
200
201                 for (int i = 0; i < solver->num_variables; i++) {
202                         if (solver->variable[i].locked)
203                                 solver->variable[i].index = ~0;
204                         else
205                                 solver->variable[i].index = n++;
206                 }
207
208                 int m = (solver->num_rows == 0)? n: solver->num_rows;
209
210                 solver->m = m;
211                 solver->n = n;
212
213                 assert(solver->least_squares || m == n);
214
215                 /* reserve reasonable estimate */
216                 solver->Mtriplets.clear();
217                 solver->Mtriplets.reserve(std::max(m, n)*3);
218
219                 solver->b.resize(solver->num_rhs);
220                 solver->x.resize(solver->num_rhs);
221
222                 for (int i = 0; i < solver->num_rhs; i++) {
223                         solver->b[i].setZero(m);
224                         solver->x[i].setZero(n);
225                 }
226
227                 linear_solver_variables_to_vector(solver);
228
229                 solver->state = LinearSolver::STATE_MATRIX_CONSTRUCT;
230         }
231 }
232
233 void EIG_linear_solver_matrix_add(LinearSolver *solver, int row, int col, double value)
234 {
235         if (solver->state == LinearSolver::STATE_MATRIX_SOLVED)
236                 return;
237
238         linear_solver_ensure_matrix_construct(solver);
239
240         if (!solver->least_squares && solver->variable[row].locked);
241         else if (solver->variable[col].locked) {
242                 if (!solver->least_squares)
243                         row = solver->variable[row].index;
244
245                 LinearSolver::Coeff coeff;
246                 coeff.index = row;
247                 coeff.value = value;
248                 solver->variable[col].a.push_back(coeff);
249         }
250         else {
251                 if (!solver->least_squares)
252                         row = solver->variable[row].index;
253                 col = solver->variable[col].index;
254
255                 /* direct insert into matrix is too slow, so use triplets */
256                 EigenTriplet triplet(row, col, value);
257                 solver->Mtriplets.push_back(triplet);
258         }
259 }
260
261 /* Right hand side */
262
263 void EIG_linear_solver_right_hand_side_add(LinearSolver *solver, int rhs, int index, double value)
264 {
265         linear_solver_ensure_matrix_construct(solver);
266
267         if (solver->least_squares) {
268                 solver->b[rhs][index] += value;
269         }
270         else if (!solver->variable[index].locked) {
271                 index = solver->variable[index].index;
272                 solver->b[rhs][index] += value;
273         }
274 }
275
276 /* Solve */
277
278 bool EIG_linear_solver_solve(LinearSolver *solver)
279 {
280         /* nothing to solve, perhaps all variables were locked */
281         if (solver->m == 0 || solver->n == 0)
282                 return true;
283
284         bool result = true;
285
286         assert(solver->state != LinearSolver::STATE_VARIABLES_CONSTRUCT);
287
288         if (solver->state == LinearSolver::STATE_MATRIX_CONSTRUCT) {
289                 /* create matrix from triplets */
290                 solver->M.resize(solver->m, solver->n);
291                 solver->M.setFromTriplets(solver->Mtriplets.begin(), solver->Mtriplets.end());
292                 solver->Mtriplets.clear();
293
294                 /* create least squares matrix */
295                 if (solver->least_squares)
296                         solver->MtM = solver->M.transpose() * solver->M;
297
298                 /* convert M to compressed column format */
299                 EigenSparseMatrix& M = (solver->least_squares)? solver->MtM: solver->M;
300                 M.makeCompressed();
301
302                 /* perform sparse LU factorization */
303                 EigenSparseLU *sparseLU = new EigenSparseLU();
304                 solver->sparseLU = sparseLU;
305
306                 sparseLU->compute(M);
307                 result = (sparseLU->info() == Eigen::Success);
308
309                 solver->state = LinearSolver::STATE_MATRIX_SOLVED;
310         }
311
312         if (result) {
313                 /* solve for each right hand side */
314                 for (int rhs = 0; rhs < solver->num_rhs; rhs++) {
315                         /* modify for locked variables */
316                         EigenVectorX& b = solver->b[rhs];
317
318                         for (int i = 0; i < solver->num_variables; i++) {
319                                 LinearSolver::Variable *variable = &solver->variable[i];
320
321                                 if (variable->locked) {
322                                         std::vector<LinearSolver::Coeff>& a = variable->a;
323
324                                         for (int j = 0; j < a.size(); j++)
325                                                 b[a[j].index] -= a[j].value*variable->value[rhs];
326                                 }
327                         }
328
329                         /* solve */
330                         if (solver->least_squares) {
331                                 EigenVectorX Mtb = solver->M.transpose() * b;
332                                 solver->x[rhs] = solver->sparseLU->solve(Mtb);
333                         }
334                         else {
335                                 EigenVectorX& b = solver->b[rhs];
336                                 solver->x[rhs] = solver->sparseLU->solve(b);
337                         }
338
339                         if (solver->sparseLU->info() != Eigen::Success)
340                                 result = false;
341                 }
342
343                 if (result)
344                         linear_solver_vector_to_variables(solver);
345         }
346
347         /* clear for next solve */
348         for (int rhs = 0; rhs < solver->num_rhs; rhs++)
349                 solver->b[rhs].setZero(solver->m);
350
351         return result;
352 }
353
354 /* Debugging */
355
356 void EIG_linear_solver_print_matrix(LinearSolver *solver)
357 {
358         std::cout << "A:" << solver->M << std::endl;
359
360         for (int rhs = 0; rhs < solver->num_rhs; rhs++)
361                 std::cout << "b " << rhs << ":" << solver->b[rhs] << std::endl;
362
363         if (solver->MtM.rows() && solver->MtM.cols())
364                 std::cout << "AtA:" << solver->MtM << std::endl;
365 }
366