Fix T46848: OpenNL crash on Windows due to uninintialized variables.
[blender.git] / intern / opennl / intern / opennl.cpp
1 /** \file opennl/intern/opennl.c
2  *  \ingroup opennlintern
3  */
4 /*
5  *
6  *  OpenNL: Numerical Library
7  *  Copyright (C) 2004 Bruno Levy
8  *
9  *  This program is free software; you can redistribute it and/or modify
10  *  it under the terms of the GNU General Public License as published by
11  *  the Free Software Foundation; either version 2 of the License, or
12  *  (at your option) any later version.
13  *
14  *  This program is distributed in the hope that it will be useful,
15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  *  GNU General Public License for more details.
18  *
19  *  You should have received a copy of the GNU General Public License
20  *  along with this program; if not, write to the Free Software
21  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
22  *
23  *  If you modify this software, you should include a notice giving the
24  *  name of the person performing the modification, the date of modification,
25  *  and the reason for such modification.
26  *
27  *  Contact: Bruno Levy
28  *
29  *       levy@loria.fr
30  *
31  *       ISA Project
32  *       LORIA, INRIA Lorraine,
33  *       Campus Scientifique, BP 239
34  *       54506 VANDOEUVRE LES NANCY CEDEX
35  *       FRANCE
36  *
37  *  Note that the GNU General Public License does not permit incorporating
38  *  the Software into proprietary programs.
39  */
40
41 #include "ONL_opennl.h"
42
43 #include <Eigen/Sparse>
44
45 #include <algorithm>
46 #include <cassert>
47 #include <cstdlib>
48 #include <iostream>
49 #include <vector>
50
51 /* Eigen data structures */
52
53 typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
54 typedef Eigen::SparseLU<EigenSparseMatrix> EigenSparseSolver;
55 typedef Eigen::VectorXd EigenVectorX;
56 typedef Eigen::Triplet<double> EigenTriplet;
57
58 /* NLContext data structure */
59
60 typedef struct {
61         NLuint index;
62         NLdouble value;
63 } NLCoeff;
64
65 typedef struct {
66         NLdouble value[4];
67         NLboolean locked;
68         NLuint index;
69         std::vector<NLCoeff> a;
70 } NLVariable;
71
72 #define NL_STATE_INITIAL            0
73 #define NL_STATE_SYSTEM             1
74 #define NL_STATE_MATRIX             2
75 #define NL_STATE_MATRIX_CONSTRUCTED 3
76 #define NL_STATE_SYSTEM_CONSTRUCTED 4
77 #define NL_STATE_SYSTEM_SOLVED      5
78
79 struct NLContext {
80    NLContext()
81    {
82       state = NL_STATE_INITIAL;
83       n = 0;
84       m = 0;
85       sparse_solver = NULL;
86       nb_variables = 0;
87       nb_rhs = 1;
88       nb_rows = 0;
89       least_squares = false;
90       solve_again = false;
91    }
92
93    ~NLContext()
94    {
95       delete sparse_solver;
96    }
97
98         NLenum state;
99
100         NLuint n;
101         NLuint m;
102
103         std::vector<EigenTriplet> Mtriplets;
104         EigenSparseMatrix M;
105         EigenSparseMatrix MtM;
106         std::vector<EigenVectorX> b;
107         std::vector<EigenVectorX> Mtb;
108         std::vector<EigenVectorX> x;
109
110         EigenSparseSolver *sparse_solver;
111
112         NLuint nb_variables;
113         std::vector<NLVariable> variable;
114
115         NLuint nb_rows;
116         NLuint nb_rhs;
117
118         NLboolean least_squares;
119         NLboolean solve_again;
120 };
121
122 NLContext *nlNewContext(void)
123 {
124         return new NLContext();
125 }
126
127 void nlDeleteContext(NLContext *context)
128 {
129         delete context;
130 }
131
132 static void __nlCheckState(NLContext *context, NLenum state)
133 {
134         assert(context->state == state);
135 }
136
137 static void __nlTransition(NLContext *context, NLenum from_state, NLenum to_state)
138 {
139         __nlCheckState(context, from_state);
140         context->state = to_state;
141 }
142
143 /* Get/Set parameters */
144
145 void nlSolverParameteri(NLContext *context, NLenum pname, NLint param)
146 {
147         __nlCheckState(context, NL_STATE_INITIAL);
148         switch(pname) {
149         case NL_NB_VARIABLES: {
150                 assert(param > 0);
151                 context->nb_variables = (NLuint)param;
152         } break;
153         case NL_NB_ROWS: {
154                 assert(param > 0);
155                 context->nb_rows = (NLuint)param;
156         } break;
157         case NL_LEAST_SQUARES: {
158                 context->least_squares = (NLboolean)param;
159         } break;
160         case NL_NB_RIGHT_HAND_SIDES: {
161                 context->nb_rhs = (NLuint)param;
162         } break;
163         default: {
164                 assert(0);
165         } break;
166         }
167 }
168
169 /* Get/Set Lock/Unlock variables */
170
171 void nlSetVariable(NLContext *context, NLuint rhsindex, NLuint index, NLdouble value)
172 {
173         __nlCheckState(context, NL_STATE_SYSTEM);
174         context->variable[index].value[rhsindex] = value;
175 }
176
177 NLdouble nlGetVariable(NLContext *context, NLuint rhsindex, NLuint index)
178 {
179         assert(context->state != NL_STATE_INITIAL);
180         return context->variable[index].value[rhsindex];
181 }
182
183 void nlLockVariable(NLContext *context, NLuint index)
184 {
185         __nlCheckState(context, NL_STATE_SYSTEM);
186         context->variable[index].locked = true;
187 }
188
189 void nlUnlockVariable(NLContext *context, NLuint index)
190 {
191         __nlCheckState(context, NL_STATE_SYSTEM);
192         context->variable[index].locked = false;
193 }
194
195 /* System construction */
196
197 static void __nlVariablesToVector(NLContext *context)
198 {
199         NLuint i, j, nb_rhs;
200
201         nb_rhs= context->nb_rhs;
202
203         for(i=0; i<context->nb_variables; i++) {
204                 NLVariable* v = &(context->variable[i]);
205                 if(!v->locked) {
206                         for(j=0; j<nb_rhs; j++)
207                                 context->x[j][v->index] = v->value[j];
208                 }
209         }
210 }
211
212 static void __nlVectorToVariables(NLContext *context)
213 {
214         NLuint i, j, nb_rhs;
215
216         nb_rhs= context->nb_rhs;
217
218         for(i=0; i<context->nb_variables; i++) {
219                 NLVariable* v = &(context->variable[i]);
220                 if(!v->locked) {
221                         for(j=0; j<nb_rhs; j++)
222                                 v->value[j] = context->x[j][v->index];
223                 }
224         }
225 }
226
227 static void __nlBeginSystem(NLContext *context)
228 {
229         assert(context->nb_variables > 0);
230
231         if (context->solve_again)
232                 __nlTransition(context, NL_STATE_SYSTEM_SOLVED, NL_STATE_SYSTEM);
233         else {
234                 __nlTransition(context, NL_STATE_INITIAL, NL_STATE_SYSTEM);
235
236                 context->variable.resize(context->nb_variables);
237         }
238 }
239
240 static void __nlEndSystem(NLContext *context)
241 {
242         __nlTransition(context, NL_STATE_MATRIX_CONSTRUCTED, NL_STATE_SYSTEM_CONSTRUCTED);
243 }
244
245 static void __nlBeginMatrix(NLContext *context)
246 {
247         NLuint i;
248         NLuint m = 0, n = 0;
249
250         __nlTransition(context, NL_STATE_SYSTEM, NL_STATE_MATRIX);
251
252         if (!context->solve_again) {
253                 for(i=0; i<context->nb_variables; i++) {
254                         if(context->variable[i].locked)
255                                 context->variable[i].index = ~0;
256                         else
257                                 context->variable[i].index = n++;
258                 }
259
260                 m = (context->nb_rows == 0)? n: context->nb_rows;
261
262                 context->m = m;
263                 context->n = n;
264
265                 /* reserve reasonable estimate */
266                 context->Mtriplets.clear();
267                 context->Mtriplets.reserve(std::max(m, n)*3);
268
269                 context->b.resize(context->nb_rhs);
270                 context->x.resize(context->nb_rhs);
271
272                 for (i=0; i<context->nb_rhs; i++) {
273                         context->b[i].setZero(m);
274                         context->x[i].setZero(n);
275                 }
276         }
277         else {
278                 /* need to recompute b only, A is not constructed anymore */
279                 for (i=0; i<context->nb_rhs; i++)
280                         context->b[i].setZero(context->m);
281         }
282
283         __nlVariablesToVector(context);
284 }
285
286 static void __nlEndMatrixRHS(NLContext *context, NLuint rhs)
287 {
288         NLVariable *variable;
289         NLuint i, j;
290
291         EigenVectorX& b = context->b[rhs];
292
293         for(i=0; i<context->nb_variables; i++) {
294                 variable = &(context->variable[i]);
295
296                 if(variable->locked) {
297                         std::vector<NLCoeff>& a = variable->a;
298
299                         for(j=0; j<a.size(); j++) {
300                                 b[a[j].index] -= a[j].value*variable->value[rhs];
301                         }
302                 }
303         }
304
305         if(context->least_squares)
306                 context->Mtb[rhs] = context->M.transpose() * b;
307 }
308
309 static void __nlEndMatrix(NLContext *context)
310 {
311         __nlTransition(context, NL_STATE_MATRIX, NL_STATE_MATRIX_CONSTRUCTED);
312
313         if(!context->solve_again) {
314                 context->M.resize(context->m, context->n);
315                 context->M.setFromTriplets(context->Mtriplets.begin(), context->Mtriplets.end());
316                 context->Mtriplets.clear();
317
318                 if(context->least_squares) {
319                         context->MtM = context->M.transpose() * context->M;
320
321                         context->Mtb.resize(context->nb_rhs);
322                         for (NLuint rhs=0; rhs<context->nb_rhs; rhs++)
323                                 context->Mtb[rhs].setZero(context->n);
324                 }
325         }
326
327         for (NLuint rhs=0; rhs<context->nb_rhs; rhs++)
328                 __nlEndMatrixRHS(context, rhs);
329 }
330
331 void nlMatrixAdd(NLContext *context, NLuint row, NLuint col, NLdouble value)
332 {
333         __nlCheckState(context, NL_STATE_MATRIX);
334
335         if(context->solve_again)
336                 return;
337
338         if (!context->least_squares && context->variable[row].locked);
339         else if (context->variable[col].locked) {
340                 if(!context->least_squares)
341                         row = context->variable[row].index;
342
343                 NLCoeff coeff = {row, value};
344                 context->variable[col].a.push_back(coeff);
345         }
346         else {
347                 if(!context->least_squares)
348                         row = context->variable[row].index;
349                 col = context->variable[col].index;
350
351                 // direct insert into matrix is too slow, so use triplets
352                 EigenTriplet triplet(row, col, value);
353                 context->Mtriplets.push_back(triplet);
354         }
355 }
356
357 void nlRightHandSideAdd(NLContext *context, NLuint rhsindex, NLuint index, NLdouble value)
358 {
359         __nlCheckState(context, NL_STATE_MATRIX);
360
361         if(context->least_squares) {
362                 context->b[rhsindex][index] += value;
363         }
364         else {
365                 if(!context->variable[index].locked) {
366                         index = context->variable[index].index;
367                         context->b[rhsindex][index] += value;
368                 }
369         }
370 }
371
372 void nlRightHandSideSet(NLContext *context, NLuint rhsindex, NLuint index, NLdouble value)
373 {
374         __nlCheckState(context, NL_STATE_MATRIX);
375
376         if(context->least_squares) {
377                 context->b[rhsindex][index] = value;
378         }
379         else {
380                 if(!context->variable[index].locked) {
381                         index = context->variable[index].index;
382                         context->b[rhsindex][index] = value;
383                 }
384         }
385 }
386
387 void nlBegin(NLContext *context, NLenum prim)
388 {
389         switch(prim) {
390         case NL_SYSTEM: {
391                 __nlBeginSystem(context);
392         } break;
393         case NL_MATRIX: {
394                 __nlBeginMatrix(context);
395         } break;
396         default: {
397                 assert(0);
398         }
399         }
400 }
401
402 void nlEnd(NLContext *context, NLenum prim)
403 {
404         switch(prim) {
405         case NL_SYSTEM: {
406                 __nlEndSystem(context);
407         } break;
408         case NL_MATRIX: {
409                 __nlEndMatrix(context);
410         } break;
411         default: {
412                 assert(0);
413         }
414         }
415 }
416
417 void nlPrintMatrix(NLContext *context)
418 {
419         std::cout << "A:" << context->M << std::endl;
420
421         for(NLuint rhs=0; rhs<context->nb_rhs; rhs++)
422                 std::cout << "b " << rhs << ":" << context->b[rhs] << std::endl;
423
424         if (context->MtM.rows() && context->MtM.cols())
425                 std::cout << "AtA:" << context->MtM << std::endl;
426 }
427
428 /* Solving */
429
430 NLboolean nlSolve(NLContext *context, NLboolean solveAgain)
431 {
432         NLboolean result = true;
433
434         __nlCheckState(context, NL_STATE_SYSTEM_CONSTRUCTED);
435
436         if (!context->solve_again) {
437                 EigenSparseMatrix& M = (context->least_squares)? context->MtM: context->M;
438
439                 assert(M.rows() == M.cols());
440
441                 /* Convert M to compressed column format */
442                 M.makeCompressed();
443
444                 /* Perform sparse LU factorization */
445                 EigenSparseSolver *sparse_solver = new EigenSparseSolver();
446                 context->sparse_solver = sparse_solver;
447
448                 sparse_solver->analyzePattern(M);
449                 sparse_solver->factorize(M);
450
451                 result = (sparse_solver->info() == Eigen::Success);
452
453                 /* Free M, don't need it anymore at this point */
454                 M.resize(0, 0);
455         }
456
457         if (result) {
458                 /* Solve each right hand side */
459                 for(NLuint rhs=0; rhs<context->nb_rhs; rhs++) {
460                         EigenVectorX& b = (context->least_squares)? context->Mtb[rhs]: context->b[rhs];
461                         context->x[rhs] = context->sparse_solver->solve(b);
462
463                         if (context->sparse_solver->info() != Eigen::Success)
464                                 result = false;
465                 }
466
467                 if (result) {
468                         __nlVectorToVariables(context);
469
470                         if (solveAgain)
471                                 context->solve_again = true;
472
473                         __nlTransition(context, NL_STATE_SYSTEM_CONSTRUCTED, NL_STATE_SYSTEM_SOLVED);
474                 }
475         }
476
477         return result;
478 }
479