Added SuperLU 3.0:
[blender.git] / intern / opennl / intern / opennl.c
1 /*
2  *  $Id$
3  *
4  *  OpenNL: Numerical Library
5  *  Copyright (C) 2004 Bruno Levy
6  *
7  *  This program is free software; you can redistribute it and/or modify
8  *  it under the terms of the GNU General Public License as published by
9  *  the Free Software Foundation; either version 2 of the License, or
10  *  (at your option) any later version.
11  *
12  *  This program is distributed in the hope that it will be useful,
13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  *  GNU General Public License for more details.
16  *
17  *  You should have received a copy of the GNU General Public License
18  *  along with this program; if not, write to the Free Software
19  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
20  *
21  *  If you modify this software, you should include a notice giving the
22  *  name of the person performing the modification, the date of modification,
23  *  and the reason for such modification.
24  *
25  *  Contact: Bruno Levy
26  *
27  *     levy@loria.fr
28  *
29  *     ISA Project
30  *     LORIA, INRIA Lorraine, 
31  *     Campus Scientifique, BP 239
32  *     54506 VANDOEUVRE LES NANCY CEDEX 
33  *     FRANCE
34  *
35  *  Note that the GNU General Public License does not permit incorporating
36  *  the Software into proprietary programs. 
37  */
38
39 #include "ONL_opennl.h"
40
41 #include <stdlib.h>
42 #include <stdio.h>
43 #include <string.h>
44 #include <math.h>
45
46 #ifdef NL_PARANOID
47 #ifndef NL_DEBUG
48 #define NL_DEBUG
49 #endif
50 #endif
51
52 /* SuperLU includes */
53 #include <ssp_defs.h>
54 #include <util.h>
55
56 /************************************************************************************/
57 /* Assertions */
58
59
60 static void __nl_assertion_failed(char* cond, char* file, int line) {
61     fprintf(
62         stderr, 
63         "OpenNL assertion failed: %s, file:%s, line:%d\n",
64         cond,file,line
65     ) ;
66     abort() ;
67 }
68
69 static void __nl_range_assertion_failed(
70     float x, float min_val, float max_val, char* file, int line
71 ) {
72     fprintf(
73         stderr, 
74         "OpenNL range assertion failed: %f in [ %f ... %f ], file:%s, line:%d\n",
75         x, min_val, max_val, file,line
76     ) ;
77     abort() ;
78 }
79
80 static void __nl_should_not_have_reached(char* file, int line) {
81     fprintf(
82         stderr, 
83         "OpenNL should not have reached this point: file:%s, line:%d\n",
84         file,line
85     ) ;
86     abort() ;
87 }
88
89
90 #define __nl_assert(x) {                                        \
91     if(!(x)) {                                                  \
92         __nl_assertion_failed(#x,__FILE__, __LINE__) ;          \
93     }                                                           \
94
95
96 #define __nl_range_assert(x,min_val,max_val) {                  \
97     if(((x) < (min_val)) || ((x) > (max_val))) {                \
98         __nl_range_assertion_failed(x, min_val, max_val,        \
99             __FILE__, __LINE__                                  \
100         ) ;                                                     \
101     }                                                           \
102 }
103
104 #define __nl_assert_not_reached {                               \
105     __nl_should_not_have_reached(__FILE__, __LINE__) ;          \
106 }
107
108 #ifdef NL_DEBUG
109 #define __nl_debug_assert(x) __nl_assert(x)
110 #define __nl_debug_range_assert(x,min_val,max_val) __nl_range_assert(x,min_val,max_val)
111 #else
112 #define __nl_debug_assert(x) 
113 #define __nl_debug_range_assert(x,min_val,max_val) 
114 #endif
115
116 #ifdef NL_PARANOID
117 #define __nl_parano_assert(x) __nl_assert(x)
118 #define __nl_parano_range_assert(x,min_val,max_val) __nl_range_assert(x,min_val,max_val)
119 #else
120 #define __nl_parano_assert(x) 
121 #define __nl_parano_range_assert(x,min_val,max_val) 
122 #endif
123
124 /************************************************************************************/
125 /* classic macros */
126
127 #ifndef MIN
128 #define MIN(x,y) (((x) < (y)) ? (x) : (y)) 
129 #endif
130
131 #ifndef MAX
132 #define MAX(x,y) (((x) > (y)) ? (x) : (y)) 
133 #endif
134
135 /************************************************************************************/
136 /* memory management */
137
138 #define __NL_NEW(T)                (T*)(calloc(1, sizeof(T))) 
139 #define __NL_NEW_ARRAY(T,NB)       (T*)(calloc((NB),sizeof(T))) 
140 #define __NL_RENEW_ARRAY(T,x,NB)   (T*)(realloc(x,(NB)*sizeof(T))) 
141 #define __NL_DELETE(x)             free(x); x = NULL 
142 #define __NL_DELETE_ARRAY(x)       free(x); x = NULL
143
144 #define __NL_CLEAR(T, x)           memset(x, 0, sizeof(T)) 
145 #define __NL_CLEAR_ARRAY(T,x,NB)   memset(x, 0, (NB)*sizeof(T)) 
146
147 /************************************************************************************/
148 /* Dynamic arrays for sparse row/columns */
149
150 typedef struct {
151     NLuint   index ;
152     NLfloat value ;
153 } __NLCoeff ;
154
155 typedef struct {
156     NLuint size ;
157     NLuint capacity ;
158     __NLCoeff* coeff ;
159 } __NLRowColumn ;
160
161 static void __nlRowColumnConstruct(__NLRowColumn* c) {
162     c->size     = 0 ;
163     c->capacity = 0 ;
164     c->coeff    = NULL ;
165 }
166
167 static void __nlRowColumnDestroy(__NLRowColumn* c) {
168     __NL_DELETE_ARRAY(c->coeff) ;
169 #ifdef NL_PARANOID
170     __NL_CLEAR(__NLRowColumn, c) ; 
171 #endif
172 }
173
174 static void __nlRowColumnGrow(__NLRowColumn* c) {
175     if(c->capacity != 0) {
176         c->capacity = 2 * c->capacity ;
177         c->coeff = __NL_RENEW_ARRAY(__NLCoeff, c->coeff, c->capacity) ;
178     } else {
179         c->capacity = 4 ;
180         c->coeff = __NL_NEW_ARRAY(__NLCoeff, c->capacity) ;
181     }
182 }
183
184 static void __nlRowColumnAdd(__NLRowColumn* c, NLint index, NLfloat value) {
185     NLuint i ;
186     for(i=0; i<c->size; i++) {
187         if(c->coeff[i].index == (NLuint)index) {
188             c->coeff[i].value += value ;
189             return ;
190         }
191     }
192     if(c->size == c->capacity) {
193         __nlRowColumnGrow(c) ;
194     }
195     c->coeff[c->size].index = index ;
196     c->coeff[c->size].value = value ;
197     c->size++ ;
198 }
199
200 /* Does not check whether the index already exists */
201 static void __nlRowColumnAppend(__NLRowColumn* c, NLint index, NLfloat value) {
202     if(c->size == c->capacity) {
203         __nlRowColumnGrow(c) ;
204     }
205     c->coeff[c->size].index = index ;
206     c->coeff[c->size].value = value ;
207     c->size++ ;
208 }
209
210 static void __nlRowColumnZero(__NLRowColumn* c) {
211     c->size = 0 ;
212 }
213
214 static void __nlRowColumnClear(__NLRowColumn* c) {
215     c->size     = 0 ;
216     c->capacity = 0 ;
217     __NL_DELETE_ARRAY(c->coeff) ;
218 }
219
220 /************************************************************************************/
221 /* SparseMatrix data structure */
222
223 #define __NL_ROWS      1
224 #define __NL_COLUMNS   2
225 #define __NL_SYMMETRIC 4
226
227 typedef struct {
228     NLuint m ;
229     NLuint n ;
230     NLuint diag_size ;
231     NLenum storage ;
232     __NLRowColumn* row ;
233     __NLRowColumn* column ;
234     NLfloat*      diag ;
235 } __NLSparseMatrix ;
236
237
238 static void __nlSparseMatrixConstruct(
239     __NLSparseMatrix* M, NLuint m, NLuint n, NLenum storage
240 ) {
241     NLuint i ;
242     M->m = m ;
243     M->n = n ;
244     M->storage = storage ;
245     if(storage & __NL_ROWS) {
246         M->row = __NL_NEW_ARRAY(__NLRowColumn, m) ;
247         for(i=0; i<n; i++) {
248             __nlRowColumnConstruct(&(M->row[i])) ;
249         }
250     } else {
251         M->row = NULL ;
252     }
253
254     if(storage & __NL_COLUMNS) {
255         M->column = __NL_NEW_ARRAY(__NLRowColumn, n) ;
256         for(i=0; i<n; i++) {
257             __nlRowColumnConstruct(&(M->column[i])) ;
258         }
259     } else {
260         M->column = NULL ;
261     }
262
263     M->diag_size = MIN(m,n) ;
264     M->diag = __NL_NEW_ARRAY(NLfloat, M->diag_size) ;
265 }
266
267 static void __nlSparseMatrixDestroy(__NLSparseMatrix* M) {
268     NLuint i ;
269     __NL_DELETE_ARRAY(M->diag) ;
270     if(M->storage & __NL_ROWS) {
271         for(i=0; i<M->m; i++) {
272             __nlRowColumnDestroy(&(M->row[i])) ;
273         }
274         __NL_DELETE_ARRAY(M->row) ;
275     }
276     if(M->storage & __NL_COLUMNS) {
277         for(i=0; i<M->n; i++) {
278             __nlRowColumnDestroy(&(M->column[i])) ;
279         }
280         __NL_DELETE_ARRAY(M->column) ;
281     }
282 #ifdef NL_PARANOID
283     __NL_CLEAR(__NLSparseMatrix,M) ;
284 #endif
285 }
286
287 static void __nlSparseMatrixAdd(
288     __NLSparseMatrix* M, NLuint i, NLuint j, NLfloat value
289 ) {
290     __nl_parano_range_assert(i, 0, M->m - 1) ;
291     __nl_parano_range_assert(j, 0, M->n - 1) ;
292     if((M->storage & __NL_SYMMETRIC) && (j > i)) {
293         return ;
294     }
295     if(i == j) {
296         M->diag[i] += value ;
297     }
298     if(M->storage & __NL_ROWS) {
299         __nlRowColumnAdd(&(M->row[i]), j, value) ;
300     }
301     if(M->storage & __NL_COLUMNS) {
302         __nlRowColumnAdd(&(M->column[j]), i, value) ;
303     }
304 }
305
306 static void __nlSparseMatrixClear( __NLSparseMatrix* M) {
307     NLuint i ;
308     if(M->storage & __NL_ROWS) {
309         for(i=0; i<M->m; i++) {
310             __nlRowColumnClear(&(M->row[i])) ;
311         }
312     }
313     if(M->storage & __NL_COLUMNS) {
314         for(i=0; i<M->n; i++) {
315             __nlRowColumnClear(&(M->column[i])) ;
316         }
317     }
318     __NL_CLEAR_ARRAY(NLfloat, M->diag, M->diag_size) ;    
319 }
320
321 /* Returns the number of non-zero coefficients */
322 static NLuint __nlSparseMatrixNNZ( __NLSparseMatrix* M) {
323     NLuint nnz = 0 ;
324     NLuint i ;
325     if(M->storage & __NL_ROWS) {
326         for(i = 0; i<M->m; i++) {
327             nnz += M->row[i].size ;
328         }
329     } else if (M->storage & __NL_COLUMNS) {
330         for(i = 0; i<M->n; i++) {
331             nnz += M->column[i].size ;
332         }
333     } else {
334         __nl_assert_not_reached ;
335     }
336     return nnz ;
337 }
338
339 /************************************************************************************/
340 /* SparseMatrix x Vector routines, internal helper routines */
341
342 static void __nlSparseMatrix_mult_rows_symmetric(
343     __NLSparseMatrix* A, NLfloat* x, NLfloat* y
344 ) {
345     NLuint m = A->m ;
346     NLuint i,ij ;
347     __NLRowColumn* Ri = NULL ;
348     __NLCoeff* c = NULL ;
349     for(i=0; i<m; i++) {
350         y[i] = 0 ;
351         Ri = &(A->row[i]) ;
352         for(ij=0; ij<Ri->size; ij++) {
353             c = &(Ri->coeff[ij]) ;
354             y[i] += c->value * x[c->index] ;
355             if(i != c->index) {
356                 y[c->index] += c->value * x[i] ;
357             }
358         }
359     }
360 }
361
362 static void __nlSparseMatrix_mult_rows(
363     __NLSparseMatrix* A, NLfloat* x, NLfloat* y
364 ) {
365     NLuint m = A->m ;
366     NLuint i,ij ;
367     __NLRowColumn* Ri = NULL ;
368     __NLCoeff* c = NULL ;
369     for(i=0; i<m; i++) {
370         y[i] = 0 ;
371         Ri = &(A->row[i]) ;
372         for(ij=0; ij<Ri->size; ij++) {
373             c = &(Ri->coeff[ij]) ;
374             y[i] += c->value * x[c->index] ;
375         }
376     }
377 }
378
379 static void __nlSparseMatrix_mult_cols_symmetric(
380     __NLSparseMatrix* A, NLfloat* x, NLfloat* y
381 ) {
382     NLuint n = A->n ;
383     NLuint j,ii ;
384     __NLRowColumn* Cj = NULL ;
385     __NLCoeff* c = NULL ;
386     for(j=0; j<n; j++) {
387         y[j] = 0 ;
388         Cj = &(A->column[j]) ;
389         for(ii=0; ii<Cj->size; ii++) {
390             c = &(Cj->coeff[ii]) ;
391             y[c->index] += c->value * x[j] ;
392             if(j != c->index) {
393                 y[j] += c->value * x[c->index] ;
394             }
395         }
396     }
397 }
398
399 static void __nlSparseMatrix_mult_cols(
400     __NLSparseMatrix* A, NLfloat* x, NLfloat* y
401 ) {
402     NLuint n = A->n ;
403     NLuint j,ii ; 
404     __NLRowColumn* Cj = NULL ;
405     __NLCoeff* c = NULL ;
406     __NL_CLEAR_ARRAY(NLfloat, y, A->m) ;
407     for(j=0; j<n; j++) {
408         Cj = &(A->column[j]) ;
409         for(ii=0; ii<Cj->size; ii++) {
410             c = &(Cj->coeff[ii]) ;
411             y[c->index] += c->value * x[j] ;
412         }
413     }
414 }
415
416 /************************************************************************************/
417 /* SparseMatrix x Vector routines, main driver routine */
418
419 void __nlSparseMatrixMult(__NLSparseMatrix* A, NLfloat* x, NLfloat* y) {
420     if(A->storage & __NL_ROWS) {
421         if(A->storage & __NL_SYMMETRIC) {
422             __nlSparseMatrix_mult_rows_symmetric(A, x, y) ;
423         } else {
424             __nlSparseMatrix_mult_rows(A, x, y) ;
425         }
426     } else {
427         if(A->storage & __NL_SYMMETRIC) {
428             __nlSparseMatrix_mult_cols_symmetric(A, x, y) ;
429         } else {
430             __nlSparseMatrix_mult_cols(A, x, y) ;
431         }
432     }
433 }
434
435 /************************************************************************************/
436 /* NLContext data structure */
437
438 typedef void(*__NLMatrixFunc)(float* x, float* y) ;
439
440 typedef struct {
441     NLfloat  value ;
442     NLboolean locked ;
443     NLuint    index ;
444 } __NLVariable ;
445
446 #define __NL_STATE_INITIAL            0
447 #define __NL_STATE_SYSTEM             1
448 #define __NL_STATE_MATRIX             2
449 #define __NL_STATE_ROW                3
450 #define __NL_STATE_MATRIX_CONSTRUCTED 4
451 #define __NL_STATE_SYSTEM_CONSTRUCTED 5
452 #define __NL_STATE_SOLVED             6
453
454 typedef struct {
455     NLenum           state ;
456     __NLVariable*    variable ;
457     NLuint           n ;
458     __NLSparseMatrix M ;
459     __NLRowColumn    af ;
460     __NLRowColumn    al ;
461     __NLRowColumn    xl ;
462     NLfloat*        x ;
463     NLfloat*        b ;
464     NLfloat         right_hand_side ;
465     NLfloat         row_scaling ;
466     NLuint           nb_variables ;
467     NLuint           current_row ;
468     NLboolean        least_squares ;
469     NLboolean        symmetric ;
470     NLboolean        normalize_rows ;
471     NLboolean        alloc_M ;
472     NLboolean        alloc_af ;
473     NLboolean        alloc_al ;
474     NLboolean        alloc_xl ;
475     NLboolean        alloc_variable ;
476     NLboolean        alloc_x ;
477     NLboolean        alloc_b ;
478     NLfloat         error ;
479     __NLMatrixFunc   matrix_vector_prod ;
480 } __NLContext ;
481
482 static __NLContext* __nlCurrentContext = NULL ;
483
484 void __nlMatrixVectorProd_default(NLfloat* x, NLfloat* y) {
485     __nlSparseMatrixMult(&(__nlCurrentContext->M), x, y) ;
486 }
487
488
489 NLContext nlNewContext() {
490     __NLContext* result      = __NL_NEW(__NLContext) ;
491     result->state            = __NL_STATE_INITIAL ;
492     result->row_scaling      = 1.0 ;
493     result->right_hand_side  = 0.0 ;
494     result->matrix_vector_prod = __nlMatrixVectorProd_default ;
495     nlMakeCurrent(result) ;
496     return result ;
497 }
498
499 void nlDeleteContext(NLContext context_in) {
500     __NLContext* context = (__NLContext*)(context_in) ;
501     if(__nlCurrentContext == context) {
502         __nlCurrentContext = NULL ;
503     }
504     if(context->alloc_M) {
505         __nlSparseMatrixDestroy(&context->M) ;
506     }
507     if(context->alloc_af) {
508         __nlRowColumnDestroy(&context->af) ;
509     }
510     if(context->alloc_al) {
511         __nlRowColumnDestroy(&context->al) ;
512     }
513     if(context->alloc_xl) {
514         __nlRowColumnDestroy(&context->xl) ;
515     }
516     if(context->alloc_variable) {
517         __NL_DELETE_ARRAY(context->variable) ;
518     }
519     if(context->alloc_x) {
520         __NL_DELETE_ARRAY(context->x) ;
521     }
522     if(context->alloc_b) {
523         __NL_DELETE_ARRAY(context->b) ;
524     }
525
526 #ifdef NL_PARANOID
527     __NL_CLEAR(__NLContext, context) ;
528 #endif
529     __NL_DELETE(context) ;
530 }
531
532 void nlMakeCurrent(NLContext context) {
533     __nlCurrentContext = (__NLContext*)(context) ;
534 }
535
536 NLContext nlGetCurrent() {
537     return __nlCurrentContext ;
538 }
539
540 void __nlCheckState(NLenum state) {
541     __nl_assert(__nlCurrentContext->state == state) ;
542 }
543
544 void __nlTransition(NLenum from_state, NLenum to_state) {
545     __nlCheckState(from_state) ;
546     __nlCurrentContext->state = to_state ;
547 }
548
549 /************************************************************************************/
550 /* Get/Set parameters */
551
552 void nlSolverParameterf(NLenum pname, NLfloat param) {
553     __nlCheckState(__NL_STATE_INITIAL) ;
554     switch(pname) {
555     case NL_NB_VARIABLES: {
556         __nl_assert(param > 0) ;
557         __nlCurrentContext->nb_variables = (NLuint)param ;
558     } break ;
559     case NL_LEAST_SQUARES: {
560         __nlCurrentContext->least_squares = (NLboolean)param ;
561     } break ;
562     case NL_SYMMETRIC: {
563         __nlCurrentContext->symmetric = (NLboolean)param ;        
564     }
565     default: {
566         __nl_assert_not_reached ;
567     } break ;
568     }
569 }
570
571 void nlSolverParameteri(NLenum pname, NLint param) {
572     __nlCheckState(__NL_STATE_INITIAL) ;
573     switch(pname) {
574     case NL_NB_VARIABLES: {
575         __nl_assert(param > 0) ;
576         __nlCurrentContext->nb_variables = (NLuint)param ;
577     } break ;
578     case NL_LEAST_SQUARES: {
579         __nlCurrentContext->least_squares = (NLboolean)param ;
580     } break ;
581     case NL_SYMMETRIC: {
582         __nlCurrentContext->symmetric = (NLboolean)param ;        
583     }
584     default: {
585         __nl_assert_not_reached ;
586     } break ;
587     }
588 }
589
590 void nlRowParameterf(NLenum pname, NLfloat param) {
591     __nlCheckState(__NL_STATE_MATRIX) ;
592     switch(pname) {
593     case NL_RIGHT_HAND_SIDE: {
594         __nlCurrentContext->right_hand_side = param ;
595     } break ;
596     case NL_ROW_SCALING: {
597         __nlCurrentContext->row_scaling = param ;
598     } break ;
599     }
600 }
601
602 void nlRowParameteri(NLenum pname, NLint param) {
603     __nlCheckState(__NL_STATE_MATRIX) ;
604     switch(pname) {
605     case NL_RIGHT_HAND_SIDE: {
606         __nlCurrentContext->right_hand_side = (NLfloat)param ;
607     } break ;
608     case NL_ROW_SCALING: {
609         __nlCurrentContext->row_scaling = (NLfloat)param ;
610     } break ;
611     }
612 }
613
614 void nlGetBooleanv(NLenum pname, NLboolean* params) {
615     switch(pname) {
616     case NL_LEAST_SQUARES: {
617         *params = __nlCurrentContext->least_squares ;
618     } break ;
619     case NL_SYMMETRIC: {
620         *params = __nlCurrentContext->symmetric ;
621     } break ;
622     default: {
623         __nl_assert_not_reached ;
624     } break ;
625     }
626 }
627
628 void nlGetFloatv(NLenum pname, NLfloat* params) {
629     switch(pname) {
630     case NL_NB_VARIABLES: {
631         *params = (NLfloat)(__nlCurrentContext->nb_variables) ;
632     } break ;
633     case NL_LEAST_SQUARES: {
634         *params = (NLfloat)(__nlCurrentContext->least_squares) ;
635     } break ;
636     case NL_SYMMETRIC: {
637         *params = (NLfloat)(__nlCurrentContext->symmetric) ;
638     } break ;
639     case NL_ERROR: {
640         *params = (NLfloat)(__nlCurrentContext->error) ;
641     } break ;
642     default: {
643         __nl_assert_not_reached ;
644     } break ;
645     }
646 }
647
648 void nlGetIntergerv(NLenum pname, NLint* params) {
649     switch(pname) {
650     case NL_NB_VARIABLES: {
651         *params = (NLint)(__nlCurrentContext->nb_variables) ;
652     } break ;
653     case NL_LEAST_SQUARES: {
654         *params = (NLint)(__nlCurrentContext->least_squares) ;
655     } break ;
656     case NL_SYMMETRIC: {
657         *params = (NLint)(__nlCurrentContext->symmetric) ;
658     } break ;
659     default: {
660         __nl_assert_not_reached ;
661     } break ;
662     }
663 }
664
665 /************************************************************************************/
666 /* Enable / Disable */
667
668 void nlEnable(NLenum pname) {
669     switch(pname) {
670     case NL_NORMALIZE_ROWS: {
671         __nl_assert(__nlCurrentContext->state != __NL_STATE_ROW) ;
672         __nlCurrentContext->normalize_rows = NL_TRUE ;
673     } break ;
674     default: {
675         __nl_assert_not_reached ;
676     }
677     }
678 }
679
680 void nlDisable(NLenum pname) {
681     switch(pname) {
682     case NL_NORMALIZE_ROWS: {
683         __nl_assert(__nlCurrentContext->state != __NL_STATE_ROW) ;
684         __nlCurrentContext->normalize_rows = NL_FALSE ;
685     } break ;
686     default: {
687         __nl_assert_not_reached ;
688     }
689     }
690 }
691
692 NLboolean nlIsEnabled(NLenum pname) {
693     switch(pname) {
694     case NL_NORMALIZE_ROWS: {
695         return __nlCurrentContext->normalize_rows ;
696     } break ;
697     default: {
698         __nl_assert_not_reached ;
699     }
700     }
701     return NL_FALSE ;
702 }
703
704 /************************************************************************************/
705 /* Get/Set Lock/Unlock variables */
706
707 void nlSetVariable(NLuint index, NLfloat value) {
708     __nlCheckState(__NL_STATE_SYSTEM) ;
709     __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1) ;
710     __nlCurrentContext->variable[index].value = value ;    
711 }
712
713 NLfloat nlGetVariable(NLuint index) {
714     __nl_assert(__nlCurrentContext->state != __NL_STATE_INITIAL) ;
715     __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1) ;
716     return __nlCurrentContext->variable[index].value ;
717 }
718
719 void nlLockVariable(NLuint index) {
720     __nlCheckState(__NL_STATE_SYSTEM) ;
721     __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1) ;
722     __nlCurrentContext->variable[index].locked = NL_TRUE ;
723 }
724
725 void nlUnlockVariable(NLuint index) {
726     __nlCheckState(__NL_STATE_SYSTEM) ;
727     __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1) ;
728     __nlCurrentContext->variable[index].locked = NL_FALSE ;
729 }
730
731 NLboolean nlVariableIsLocked(NLuint index) {
732     __nl_assert(__nlCurrentContext->state != __NL_STATE_INITIAL) ;
733     __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1) ;
734     return __nlCurrentContext->variable[index].locked  ;
735 }
736
737 /************************************************************************************/
738 /* System construction */
739
740 void __nlVariablesToVector() {
741     NLuint i ;
742     __nl_assert(__nlCurrentContext->alloc_x) ;
743     __nl_assert(__nlCurrentContext->alloc_variable) ;
744     for(i=0; i<__nlCurrentContext->nb_variables; i++) {
745         __NLVariable* v = &(__nlCurrentContext->variable[i]) ;
746         if(!v->locked) {
747             __nl_assert(v->index < __nlCurrentContext->n) ;
748             __nlCurrentContext->x[v->index] = v->value ;
749         }
750     }
751 }
752
753 void __nlVectorToVariables() {
754     NLuint i ;
755     __nl_assert(__nlCurrentContext->alloc_x) ;
756     __nl_assert(__nlCurrentContext->alloc_variable) ;
757     for(i=0; i<__nlCurrentContext->nb_variables; i++) {
758         __NLVariable* v = &(__nlCurrentContext->variable[i]) ;
759         if(!v->locked) {
760             __nl_assert(v->index < __nlCurrentContext->n) ;
761             v->value = __nlCurrentContext->x[v->index] ;
762         }
763     }
764 }
765
766
767 void __nlBeginSystem() {
768     __nlTransition(__NL_STATE_INITIAL, __NL_STATE_SYSTEM) ;
769     __nl_assert(__nlCurrentContext->nb_variables > 0) ;
770     __nlCurrentContext->variable = __NL_NEW_ARRAY(
771         __NLVariable, __nlCurrentContext->nb_variables
772     ) ;
773     __nlCurrentContext->alloc_variable = NL_TRUE ;
774 }
775
776 void __nlEndSystem() {
777     __nlTransition(__NL_STATE_MATRIX_CONSTRUCTED, __NL_STATE_SYSTEM_CONSTRUCTED) ;    
778 }
779
780 void __nlBeginMatrix() {
781     NLuint i ;
782     NLuint n = 0 ;
783     NLenum storage = __NL_ROWS ;
784
785     __nlTransition(__NL_STATE_SYSTEM, __NL_STATE_MATRIX) ;
786
787     for(i=0; i<__nlCurrentContext->nb_variables; i++) {
788         if(!__nlCurrentContext->variable[i].locked) {
789             __nlCurrentContext->variable[i].index = n ;
790             n++ ;
791         } else {
792             __nlCurrentContext->variable[i].index = ~0 ;
793         }
794     }
795
796     __nlCurrentContext->n = n ;
797
798     /* a least squares problem results in a symmetric matrix */
799     if(__nlCurrentContext->least_squares) {
800         __nlCurrentContext->symmetric = NL_TRUE ;
801     }
802
803     if(__nlCurrentContext->symmetric) {
804         storage = (storage | __NL_SYMMETRIC) ;
805     }
806
807     /* SuperLU storage does not support symmetric storage */
808     storage = (storage & ~__NL_SYMMETRIC) ;
809
810     __nlSparseMatrixConstruct(&__nlCurrentContext->M, n, n, storage) ;
811     __nlCurrentContext->alloc_M = NL_TRUE ;
812
813     __nlCurrentContext->x = __NL_NEW_ARRAY(NLfloat, n) ;
814     __nlCurrentContext->alloc_x = NL_TRUE ;
815     
816     __nlCurrentContext->b = __NL_NEW_ARRAY(NLfloat, n) ;
817     __nlCurrentContext->alloc_b = NL_TRUE ;
818
819     __nlVariablesToVector() ;
820
821     __nlRowColumnConstruct(&__nlCurrentContext->af) ;
822     __nlCurrentContext->alloc_af = NL_TRUE ;
823     __nlRowColumnConstruct(&__nlCurrentContext->al) ;
824     __nlCurrentContext->alloc_al = NL_TRUE ;
825     __nlRowColumnConstruct(&__nlCurrentContext->xl) ;
826     __nlCurrentContext->alloc_xl = NL_TRUE ;
827
828     __nlCurrentContext->current_row = 0 ;
829 }
830
831 void __nlEndMatrix() {
832     __nlTransition(__NL_STATE_MATRIX, __NL_STATE_MATRIX_CONSTRUCTED) ;    
833     
834     __nlRowColumnDestroy(&__nlCurrentContext->af) ;
835     __nlCurrentContext->alloc_af = NL_FALSE ;
836     __nlRowColumnDestroy(&__nlCurrentContext->al) ;
837     __nlCurrentContext->alloc_al = NL_FALSE ;
838     __nlRowColumnDestroy(&__nlCurrentContext->xl) ;
839     __nlCurrentContext->alloc_al = NL_FALSE ;
840     
841     if(!__nlCurrentContext->least_squares) {
842         __nl_assert(
843             __nlCurrentContext->current_row == 
844             __nlCurrentContext->n
845         ) ;
846     }
847 }
848
849 void __nlBeginRow() {
850     __nlTransition(__NL_STATE_MATRIX, __NL_STATE_ROW) ;
851     __nlRowColumnZero(&__nlCurrentContext->af) ;
852     __nlRowColumnZero(&__nlCurrentContext->al) ;
853     __nlRowColumnZero(&__nlCurrentContext->xl) ;
854 }
855
856 void __nlScaleRow(NLfloat s) {
857     __NLRowColumn*    af = &__nlCurrentContext->af ;
858     __NLRowColumn*    al = &__nlCurrentContext->al ;
859     NLuint nf            = af->size ;
860     NLuint nl            = al->size ;
861     NLuint i ;
862     for(i=0; i<nf; i++) {
863         af->coeff[i].value *= s ;
864     }
865     for(i=0; i<nl; i++) {
866         al->coeff[i].value *= s ;
867     }
868     __nlCurrentContext->right_hand_side *= s ;
869 }
870
871 void __nlNormalizeRow(NLfloat weight) {
872     __NLRowColumn*    af = &__nlCurrentContext->af ;
873     __NLRowColumn*    al = &__nlCurrentContext->al ;
874     NLuint nf            = af->size ;
875     NLuint nl            = al->size ;
876     NLuint i ;
877     NLfloat norm = 0.0 ;
878     for(i=0; i<nf; i++) {
879         norm += af->coeff[i].value * af->coeff[i].value ;
880     }
881     for(i=0; i<nl; i++) {
882         norm += al->coeff[i].value * al->coeff[i].value ;
883     }
884     norm = sqrt(norm) ;
885     __nlScaleRow(weight / norm) ;
886 }
887
888 void __nlEndRow() {
889     __NLRowColumn*    af = &__nlCurrentContext->af ;
890     __NLRowColumn*    al = &__nlCurrentContext->al ;
891     __NLRowColumn*    xl = &__nlCurrentContext->xl ;
892     __NLSparseMatrix* M  = &__nlCurrentContext->M  ;
893     NLfloat* b        = __nlCurrentContext->b ;
894     NLuint nf          = af->size ;
895     NLuint nl          = al->size ;
896     NLuint current_row = __nlCurrentContext->current_row ;
897     NLuint i ;
898     NLuint j ;
899     NLfloat S ;
900     __nlTransition(__NL_STATE_ROW, __NL_STATE_MATRIX) ;
901
902     if(__nlCurrentContext->normalize_rows) {
903         __nlNormalizeRow(__nlCurrentContext->row_scaling) ;
904     } else {
905         __nlScaleRow(__nlCurrentContext->row_scaling) ;
906     }
907
908     if(__nlCurrentContext->least_squares) {
909         for(i=0; i<nf; i++) {
910             for(j=0; j<nf; j++) {
911                 __nlSparseMatrixAdd(
912                     M, af->coeff[i].index, af->coeff[j].index,
913                     af->coeff[i].value * af->coeff[j].value
914                 ) ;
915             }
916         }
917         S = -__nlCurrentContext->right_hand_side ;
918         for(j=0; j<nl; j++) {
919             S += al->coeff[j].value * xl->coeff[j].value ;
920         }
921         for(i=0; i<nf; i++) {
922             b[ af->coeff[i].index ] -= af->coeff[i].value * S ;
923         }
924     } else {
925         for(i=0; i<nf; i++) {
926             __nlSparseMatrixAdd(
927                 M, current_row, af->coeff[i].index, af->coeff[i].value
928             ) ;
929         }
930         b[current_row] = -__nlCurrentContext->right_hand_side ;
931         for(i=0; i<nl; i++) {
932             b[current_row] -= al->coeff[i].value * xl->coeff[i].value ;
933         }
934     }
935     __nlCurrentContext->current_row++ ;
936     __nlCurrentContext->right_hand_side = 0.0 ;    
937     __nlCurrentContext->row_scaling     = 1.0 ;
938 }
939
940 void nlCoefficient(NLuint index, NLfloat value) {
941     __NLVariable* v;
942         unsigned int zero= 0;
943     __nlCheckState(__NL_STATE_ROW) ;
944     __nl_range_assert(index, zero, __nlCurrentContext->nb_variables - 1) ;
945     v = &(__nlCurrentContext->variable[index]) ;
946     if(v->locked) {
947         __nlRowColumnAppend(&(__nlCurrentContext->al), 0, value) ;
948         __nlRowColumnAppend(&(__nlCurrentContext->xl), 0, v->value) ;
949     } else {
950         __nlRowColumnAppend(&(__nlCurrentContext->af), v->index, value) ;
951     }
952 }
953
954 void nlBegin(NLenum prim) {
955     switch(prim) {
956     case NL_SYSTEM: {
957         __nlBeginSystem() ;
958     } break ;
959     case NL_MATRIX: {
960         __nlBeginMatrix() ;
961     } break ;
962     case NL_ROW: {
963         __nlBeginRow() ;
964     } break ;
965     default: {
966         __nl_assert_not_reached ;
967     }
968     }
969 }
970
971 void nlEnd(NLenum prim) {
972     switch(prim) {
973     case NL_SYSTEM: {
974         __nlEndSystem() ;
975     } break ;
976     case NL_MATRIX: {
977         __nlEndMatrix() ;
978     } break ;
979     case NL_ROW: {
980         __nlEndRow() ;
981     } break ;
982     default: {
983         __nl_assert_not_reached ;
984     }
985     }
986 }
987
988 /************************************************************************/
989 /* SuperLU wrapper */
990
991 /* Note: SuperLU is difficult to call, but it is worth it.    */
992 /* Here is a driver inspired by A. Sheffer's "cow flattener". */
993 static NLboolean __nlSolve_SUPERLU( NLboolean do_perm) {
994
995     /* OpenNL Context */
996     __NLSparseMatrix* M  = &(__nlCurrentContext->M) ;
997     NLfloat* b          = __nlCurrentContext->b ;
998     NLfloat* x          = __nlCurrentContext->x ;
999
1000     /* Compressed Row Storage matrix representation */
1001     NLuint    n      = __nlCurrentContext->n ;
1002     NLuint    nnz    = __nlSparseMatrixNNZ(M) ; /* Number of Non-Zero coeffs */
1003     NLint*    xa     = __NL_NEW_ARRAY(NLint, n+1) ;
1004     NLfloat* rhs    = __NL_NEW_ARRAY(NLfloat, n) ;
1005     NLfloat* a      = __NL_NEW_ARRAY(NLfloat, nnz) ;
1006     NLint*    asub   = __NL_NEW_ARRAY(NLint, nnz) ;
1007
1008     /* Permutation vector */
1009     NLint*    perm_r  = __NL_NEW_ARRAY(NLint, n) ;
1010     NLint*    perm    = __NL_NEW_ARRAY(NLint, n) ;
1011
1012     /* SuperLU variables */
1013     SuperMatrix A, B ; /* System       */
1014     SuperMatrix L, U ; /* Inverse of A */
1015     NLint info ;       /* status code  */
1016     DNformat *vals = NULL ; /* access to result */
1017     float *rvals  = NULL ; /* access to result */
1018
1019     /* SuperLU options and stats */
1020     superlu_options_t options ;
1021     SuperLUStat_t     stat ;
1022
1023
1024     /* Temporary variables */
1025     __NLRowColumn* Ri = NULL ;
1026     NLuint         i,jj,count ;
1027     
1028     __nl_assert(!(M->storage & __NL_SYMMETRIC)) ;
1029     __nl_assert(M->storage & __NL_ROWS) ;
1030     __nl_assert(M->m == M->n) ;
1031     
1032     
1033     /*
1034      * Step 1: convert matrix M into SuperLU compressed column 
1035      *   representation.
1036      * -------------------------------------------------------
1037      */
1038
1039     count = 0 ;
1040     for(i=0; i<n; i++) {
1041         Ri = &(M->row[i]) ;
1042         xa[i] = count ;
1043         for(jj=0; jj<Ri->size; jj++) {
1044             a[count]    = Ri->coeff[jj].value ;
1045             asub[count] = Ri->coeff[jj].index ;
1046             count++ ;
1047         }
1048     }
1049     xa[n] = nnz ;
1050
1051     /* Save memory for SuperLU */
1052     __nlSparseMatrixClear(M) ;
1053
1054
1055     /*
1056      * Rem: symmetric storage does not seem to work with
1057      * SuperLU ... (->deactivated in main SLS::Solver driver)
1058      */
1059     sCreate_CompCol_Matrix(
1060         &A, n, n, nnz, a, asub, xa, 
1061         SLU_NR,              /* Row_wise, no supernode */
1062         SLU_S,               /* floats                */ 
1063         SLU_GE               /* general storage        */
1064     );
1065
1066     /* Step 2: create vector */
1067     sCreate_Dense_Matrix(
1068         &B, n, 1, b, n, 
1069         SLU_DN, /* Fortran-type column-wise storage */
1070         SLU_S,  /* floats                          */
1071         SLU_GE  /* general                          */
1072     );
1073             
1074
1075     /* Step 3: get permutation matrix 
1076      * ------------------------------
1077      * com_perm: 0 -> no re-ordering
1078      *           1 -> re-ordering for A^t.A
1079      *           2 -> re-ordering for A^t+A
1080      *           3 -> approximate minimum degree ordering
1081      */
1082     get_perm_c(do_perm ? 3 : 0, &A, perm) ;
1083
1084     /* Step 4: call SuperLU main routine
1085      * ---------------------------------
1086      */
1087
1088     set_default_options(&options) ;
1089     options.ColPerm = MY_PERMC ;
1090     StatInit(&stat) ;
1091
1092     sgssv(&options, &A, perm, perm_r, &L, &U, &B, &stat, &info);
1093
1094     /* Step 5: get the solution
1095      * ------------------------
1096      * Fortran-type column-wise storage
1097      */
1098     vals = (DNformat*)B.Store;
1099     rvals = (float*)(vals->nzval);
1100     if(info == 0) {
1101         for(i = 0; i <  n; i++){
1102             x[i] = rvals[i];
1103         }
1104     }
1105
1106     /* Step 6: cleanup
1107      * ---------------
1108      */
1109
1110     /*
1111      *  For these two ones, only the "store" structure
1112      * needs to be deallocated (the arrays have been allocated
1113      * by us).
1114      */
1115     Destroy_SuperMatrix_Store(&A) ;
1116     Destroy_SuperMatrix_Store(&B) ;
1117
1118     
1119     /*
1120      *   These ones need to be fully deallocated (they have been
1121      * allocated by SuperLU).
1122      */
1123     Destroy_SuperNode_Matrix(&L);
1124     Destroy_CompCol_Matrix(&U);
1125
1126     __NL_DELETE_ARRAY(xa) ;
1127     __NL_DELETE_ARRAY(rhs) ;
1128     __NL_DELETE_ARRAY(a) ;
1129     __NL_DELETE_ARRAY(asub) ;
1130     __NL_DELETE_ARRAY(perm_r) ;
1131     __NL_DELETE_ARRAY(perm) ;
1132
1133     return (info == 0) ;
1134 }
1135
1136
1137 /************************************************************************/
1138 /* nlSolve() driver routine */
1139
1140 NLboolean nlSolve() {
1141     NLboolean result = NL_TRUE ;
1142
1143     __nlCheckState(__NL_STATE_SYSTEM_CONSTRUCTED) ;
1144     result = __nlSolve_SUPERLU(NL_TRUE) ;
1145
1146     __nlVectorToVariables() ;
1147     __nlTransition(__NL_STATE_SYSTEM_CONSTRUCTED, __NL_STATE_SOLVED) ;
1148
1149     return result ;
1150 }
1151