Blender  V3.3
linear_solver.cc
Go to the documentation of this file.
1 /* SPDX-License-Identifier: GPL-2.0-or-later
2  * Copyright 2004 Bruno Levy.
3  * 2005-2015 Blender Foundation. */
4 
10 #include "linear_solver.h"
11 
12 #include <Eigen/Sparse>
13 
14 #include <algorithm>
15 #include <cassert>
16 #include <cstdlib>
17 #include <iostream>
18 #include <vector>
19 
20 /* Eigen data structures */
21 
22 typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
23 typedef Eigen::SparseLU<EigenSparseMatrix> EigenSparseLU;
24 typedef Eigen::VectorXd EigenVectorX;
25 typedef Eigen::Triplet<double> EigenTriplet;
26 
27 /* Linear Solver data structure */
28 
29 struct LinearSolver {
30  struct Coeff {
32  {
33  index = 0;
34  value = 0.0;
35  }
36 
37  int index;
38  double value;
39  };
40 
41  struct Variable {
43  {
44  memset(value, 0, sizeof(value));
45  locked = false;
46  index = 0;
47  }
48 
49  double value[4];
50  bool locked;
51  int index;
52  std::vector<Coeff> a;
53  };
54 
56 
57  LinearSolver(int num_rows_, int num_variables_, int num_rhs_, bool lsq_)
58  {
59  assert(num_variables_ > 0);
60  assert(num_rhs_ <= 4);
61 
63  m = 0;
64  n = 0;
65  sparseLU = NULL;
66  num_variables = num_variables_;
67  num_rhs = num_rhs_;
68  num_rows = num_rows_;
69  least_squares = lsq_;
70 
71  variable.resize(num_variables);
72  }
73 
75  {
76  delete sparseLU;
77  }
78 
80 
81  int n;
82  int m;
83 
84  std::vector<EigenTriplet> Mtriplets;
87  std::vector<EigenVectorX> b;
88  std::vector<EigenVectorX> x;
89 
91 
93  std::vector<Variable> variable;
94 
95  int num_rows;
96  int num_rhs;
97 
99 };
100 
101 LinearSolver *EIG_linear_solver_new(int num_rows, int num_columns, int num_rhs)
102 {
103  return new LinearSolver(num_rows, num_columns, num_rhs, false);
104 }
105 
106 LinearSolver *EIG_linear_least_squares_solver_new(int num_rows, int num_columns, int num_rhs)
107 {
108  return new LinearSolver(num_rows, num_columns, num_rhs, true);
109 }
110 
112 {
113  delete solver;
114 }
115 
116 /* Variables */
117 
118 void EIG_linear_solver_variable_set(LinearSolver *solver, int rhs, int index, double value)
119 {
120  solver->variable[index].value[rhs] = value;
121 }
122 
123 double EIG_linear_solver_variable_get(LinearSolver *solver, int rhs, int index)
124 {
125  return solver->variable[index].value[rhs];
126 }
127 
129 {
130  if (!solver->variable[index].locked) {
132  solver->variable[index].locked = true;
133  }
134 }
135 
137 {
138  if (solver->variable[index].locked) {
140  solver->variable[index].locked = false;
141  }
142 }
143 
145 {
146  int num_rhs = solver->num_rhs;
147 
148  for (int i = 0; i < solver->num_variables; i++) {
149  LinearSolver::Variable *v = &solver->variable[i];
150  if (!v->locked) {
151  for (int j = 0; j < num_rhs; j++)
152  solver->x[j][v->index] = v->value[j];
153  }
154  }
155 }
156 
158 {
159  int num_rhs = solver->num_rhs;
160 
161  for (int i = 0; i < solver->num_variables; i++) {
162  LinearSolver::Variable *v = &solver->variable[i];
163  if (!v->locked) {
164  for (int j = 0; j < num_rhs; j++)
165  v->value[j] = solver->x[j][v->index];
166  }
167  }
168 }
169 
170 /* Matrix */
171 
173 {
174  /* transition to matrix construction if necessary */
176  int n = 0;
177 
178  for (int i = 0; i < solver->num_variables; i++) {
179  if (solver->variable[i].locked)
180  solver->variable[i].index = ~0;
181  else
182  solver->variable[i].index = n++;
183  }
184 
185  int m = (solver->num_rows == 0) ? n : solver->num_rows;
186 
187  solver->m = m;
188  solver->n = n;
189 
190  assert(solver->least_squares || m == n);
191 
192  /* reserve reasonable estimate */
193  solver->Mtriplets.clear();
194  solver->Mtriplets.reserve(std::max(m, n) * 3);
195 
196  solver->b.resize(solver->num_rhs);
197  solver->x.resize(solver->num_rhs);
198 
199  for (int i = 0; i < solver->num_rhs; i++) {
200  solver->b[i].setZero(m);
201  solver->x[i].setZero(n);
202  }
203 
205 
207  }
208 }
209 
210 void EIG_linear_solver_matrix_add(LinearSolver *solver, int row, int col, double value)
211 {
213  return;
214 
216 
217  if (!solver->least_squares && solver->variable[row].locked)
218  ;
219  else if (solver->variable[col].locked) {
220  if (!solver->least_squares)
221  row = solver->variable[row].index;
222 
223  LinearSolver::Coeff coeff;
224  coeff.index = row;
225  coeff.value = value;
226  solver->variable[col].a.push_back(coeff);
227  }
228  else {
229  if (!solver->least_squares)
230  row = solver->variable[row].index;
231  col = solver->variable[col].index;
232 
233  /* direct insert into matrix is too slow, so use triplets */
234  EigenTriplet triplet(row, col, value);
235  solver->Mtriplets.push_back(triplet);
236  }
237 }
238 
239 /* Right hand side */
240 
241 void EIG_linear_solver_right_hand_side_add(LinearSolver *solver, int rhs, int index, double value)
242 {
244 
245  if (solver->least_squares) {
246  solver->b[rhs][index] += value;
247  }
248  else if (!solver->variable[index].locked) {
249  index = solver->variable[index].index;
250  solver->b[rhs][index] += value;
251  }
252 }
253 
254 /* Solve */
255 
257 {
258  /* nothing to solve, perhaps all variables were locked */
259  if (solver->m == 0 || solver->n == 0)
260  return true;
261 
262  bool result = true;
263 
265 
267  /* create matrix from triplets */
268  solver->M.resize(solver->m, solver->n);
269  solver->M.setFromTriplets(solver->Mtriplets.begin(), solver->Mtriplets.end());
270  solver->Mtriplets.clear();
271 
272  /* create least squares matrix */
273  if (solver->least_squares)
274  solver->MtM = solver->M.transpose() * solver->M;
275 
276  /* convert M to compressed column format */
277  EigenSparseMatrix &M = (solver->least_squares) ? solver->MtM : solver->M;
278  M.makeCompressed();
279 
280  /* perform sparse LU factorization */
281  EigenSparseLU *sparseLU = new EigenSparseLU();
282  solver->sparseLU = sparseLU;
283 
284  sparseLU->compute(M);
285  result = (sparseLU->info() == Eigen::Success);
286 
288  }
289 
290  if (result) {
291  /* solve for each right hand side */
292  for (int rhs = 0; rhs < solver->num_rhs; rhs++) {
293  /* modify for locked variables */
294  EigenVectorX &b = solver->b[rhs];
295 
296  for (int i = 0; i < solver->num_variables; i++) {
297  LinearSolver::Variable *variable = &solver->variable[i];
298 
299  if (variable->locked) {
300  std::vector<LinearSolver::Coeff> &a = variable->a;
301 
302  for (int j = 0; j < a.size(); j++)
303  b[a[j].index] -= a[j].value * variable->value[rhs];
304  }
305  }
306 
307  /* solve */
308  if (solver->least_squares) {
309  EigenVectorX Mtb = solver->M.transpose() * b;
310  solver->x[rhs] = solver->sparseLU->solve(Mtb);
311  }
312  else {
313  EigenVectorX &b = solver->b[rhs];
314  solver->x[rhs] = solver->sparseLU->solve(b);
315  }
316 
317  if (solver->sparseLU->info() != Eigen::Success)
318  result = false;
319  }
320 
321  if (result)
323  }
324 
325  /* clear for next solve */
326  for (int rhs = 0; rhs < solver->num_rhs; rhs++)
327  solver->b[rhs].setZero(solver->m);
328 
329  return result;
330 }
331 
332 /* Debugging */
333 
335 {
336  std::cout << "A:" << solver->M << std::endl;
337 
338  for (int rhs = 0; rhs < solver->num_rhs; rhs++)
339  std::cout << "b " << rhs << ":" << solver->b[rhs] << std::endl;
340 
341  if (solver->MtM.rows() && solver->MtM.cols())
342  std::cout << "AtA:" << solver->MtM << std::endl;
343 }
ATTR_WARN_UNUSED_RESULT const BMVert * v
uint col
void EIG_linear_solver_print_matrix(LinearSolver *solver)
Eigen::VectorXd EigenVectorX
void EIG_linear_solver_variable_set(LinearSolver *solver, int rhs, int index, double value)
LinearSolver * EIG_linear_solver_new(int num_rows, int num_columns, int num_rhs)
void EIG_linear_solver_right_hand_side_add(LinearSolver *solver, int rhs, int index, double value)
void EIG_linear_solver_variable_unlock(LinearSolver *solver, int index)
Eigen::SparseMatrix< double, Eigen::ColMajor > EigenSparseMatrix
Eigen::SparseLU< EigenSparseMatrix > EigenSparseLU
LinearSolver * EIG_linear_least_squares_solver_new(int num_rows, int num_columns, int num_rhs)
void EIG_linear_solver_delete(LinearSolver *solver)
Eigen::Triplet< double > EigenTriplet
static void linear_solver_variables_to_vector(LinearSolver *solver)
double EIG_linear_solver_variable_get(LinearSolver *solver, int rhs, int index)
static void linear_solver_vector_to_variables(LinearSolver *solver)
void EIG_linear_solver_matrix_add(LinearSolver *solver, int row, int col, double value)
bool EIG_linear_solver_solve(LinearSolver *solver)
static void linear_solver_ensure_matrix_construct(LinearSolver *solver)
void EIG_linear_solver_variable_lock(LinearSolver *solver, int index)
struct LinearSolver LinearSolver
Definition: linear_solver.h:20
#define M
static unsigned a[3]
Definition: RandGen.cpp:78
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
std::vector< Coeff > a
EigenSparseMatrix MtM
EigenSparseLU * sparseLU
@ STATE_VARIABLES_CONSTRUCT
std::vector< Variable > variable
EigenSparseMatrix M
LinearSolver(int num_rows_, int num_variables_, int num_rhs_, bool lsq_)
std::vector< EigenVectorX > b
std::vector< EigenVectorX > x
std::vector< EigenTriplet > Mtriplets
float max