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 static 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_SYSTEM_SOLVED                7
 453
 454 typedef struct {
 455         NLenum             state;
 456         __NLVariable*   variable;
 457         NLuint             n;
 458         __NLSparseMatrix M;
 459         __NLRowColumn   af;
 460         __NLRowColumn   al;
 461         NLfloat*                x;
 462         NLfloat*                b;
 463         NLfloat          right_hand_side;
 464         NLuint             nb_variables;
 465         NLuint             current_row;
 466         NLboolean               least_squares;
 467         NLboolean               symmetric;
 468         NLboolean               solve_again;
 469         NLboolean               alloc_M;
 470         NLboolean               alloc_af;
 471         NLboolean               alloc_al;
 472         NLboolean               alloc_variable;
 473         NLboolean               alloc_x;
 474         NLboolean               alloc_b;
 475         NLfloat          error;
 476         __NLMatrixFunc   matrix_vector_prod;
 477
 478         struct __NLSuperLUContext {
 479                 NLboolean alloc_slu;
 480                 SuperMatrix L, U;
 481                 NLint *perm_c, *perm_r;
 482                 SuperLUStat_t stat;
 483         } slu;
 484 } __NLContext;
 485
 486 static __NLContext* __nlCurrentContext = NULL;
 487
 488 static void __nlMatrixVectorProd_default(NLfloat* x, NLfloat* y) {
 489         __nlSparseMatrixMult(&(__nlCurrentContext->M), x, y);
 490 }
 491
 492
 493 NLContext nlNewContext(void) {
 494         __NLContext* result       = __NL_NEW(__NLContext);
 495         result->state                   = __NL_STATE_INITIAL;
 496         result->right_hand_side  = 0.0;
 497         result->matrix_vector_prod = __nlMatrixVectorProd_default;
 498         nlMakeCurrent(result);
 499         return result;
 500 }
 501
 502 static void __nlFree_SUPERLU(__NLContext *context);
 503
 504 void nlDeleteContext(NLContext context_in) {
 505         __NLContext* context = (__NLContext*)(context_in);
 506         if(__nlCurrentContext == context) {
 507                 __nlCurrentContext = NULL;
 508         }
 509         if(context->alloc_M) {
 510                 __nlSparseMatrixDestroy(&context->M);
 511         }
 512         if(context->alloc_af) {
 513                 __nlRowColumnDestroy(&context->af);
 514         }
 515         if(context->alloc_al) {
 516                 __nlRowColumnDestroy(&context->al);
 517         }
 518         if(context->alloc_variable) {
 519                 __NL_DELETE_ARRAY(context->variable);
 520         }
 521         if(context->alloc_x) {
 522                 __NL_DELETE_ARRAY(context->x);
 523         }
 524         if(context->alloc_b) {
 525                 __NL_DELETE_ARRAY(context->b);
 526         }
 527         if (context->slu.alloc_slu) {
 528                 __nlFree_SUPERLU(context);
 529         }
 530
 531 #ifdef NL_PARANOID
 532         __NL_CLEAR(__NLContext, context);
 533 #endif
 534         __NL_DELETE(context);
 535 }
 536
 537 void nlMakeCurrent(NLContext context) {
 538         __nlCurrentContext = (__NLContext*)(context);
 539 }
 540
 541 NLContext nlGetCurrent(void) {
 542         return __nlCurrentContext;
 543 }
 544
 545 static void __nlCheckState(NLenum state) {
 546         __nl_assert(__nlCurrentContext->state == state);
 547 }
 548
 549 static void __nlTransition(NLenum from_state, NLenum to_state) {
 550         __nlCheckState(from_state);
 551         __nlCurrentContext->state = to_state;
 552 }
 553
 554 /************************************************************************************/
 555 /* Get/Set parameters */
 556
 557 void nlSolverParameterf(NLenum pname, NLfloat param) {
 558         __nlCheckState(__NL_STATE_INITIAL);
 559         switch(pname) {
 560         case NL_NB_VARIABLES: {
 561                 __nl_assert(param > 0);
 562                 __nlCurrentContext->nb_variables = (NLuint)param;
 563         } break;
 564         case NL_LEAST_SQUARES: {
 565                 __nlCurrentContext->least_squares = (NLboolean)param;
 566         } break;
 567         case NL_SYMMETRIC: {
 568                 __nlCurrentContext->symmetric = (NLboolean)param;
 569         }
 570         default: {
 571                 __nl_assert_not_reached;
 572         } break;
 573         }
 574 }
 575
 576 void nlSolverParameteri(NLenum pname, NLint param) {
 577         __nlCheckState(__NL_STATE_INITIAL);
 578         switch(pname) {
 579         case NL_NB_VARIABLES: {
 580                 __nl_assert(param > 0);
 581                 __nlCurrentContext->nb_variables = (NLuint)param;
 582         } break;
 583         case NL_LEAST_SQUARES: {
 584                 __nlCurrentContext->least_squares = (NLboolean)param;
 585         } break;
 586         case NL_SYMMETRIC: {
 587                 __nlCurrentContext->symmetric = (NLboolean)param;
 588         }
 589         default: {
 590                 __nl_assert_not_reached;
 591         } break;
 592         }
 593 }
 594
 595 void nlRowParameterf(NLenum pname, NLfloat param) {
 596         __nlCheckState(__NL_STATE_MATRIX);
 597         switch(pname) {
 598         case NL_RIGHT_HAND_SIDE: {
 599                 __nlCurrentContext->right_hand_side = param;
 600         } break;
 601         }
 602 }
 603
 604 void nlRowParameteri(NLenum pname, NLint param) {
 605         __nlCheckState(__NL_STATE_MATRIX);
 606         switch(pname) {
 607         case NL_RIGHT_HAND_SIDE: {
 608                 __nlCurrentContext->right_hand_side = (NLfloat)param;
 609         } break;
 610         }
 611 }
 612
 613 void nlGetBooleanv(NLenum pname, NLboolean* params) {
 614         switch(pname) {
 615         case NL_LEAST_SQUARES: {
 616                 *params = __nlCurrentContext->least_squares;
 617         } break;
 618         case NL_SYMMETRIC: {
 619                 *params = __nlCurrentContext->symmetric;
 620         } break;
 621         default: {
 622                 __nl_assert_not_reached;
 623         } break;
 624         }
 625 }
 626
 627 void nlGetFloatv(NLenum pname, NLfloat* params) {
 628         switch(pname) {
 629         case NL_NB_VARIABLES: {
 630                 *params = (NLfloat)(__nlCurrentContext->nb_variables);
 631         } break;
 632         case NL_LEAST_SQUARES: {
 633                 *params = (NLfloat)(__nlCurrentContext->least_squares);
 634         } break;
 635         case NL_SYMMETRIC: {
 636                 *params = (NLfloat)(__nlCurrentContext->symmetric);
 637         } break;
 638         case NL_ERROR: {
 639                 *params = (NLfloat)(__nlCurrentContext->error);
 640         } break;
 641         default: {
 642                 __nl_assert_not_reached;
 643         } break;
 644         }
 645 }
 646
 647 void nlGetIntergerv(NLenum pname, NLint* params) {
 648         switch(pname) {
 649         case NL_NB_VARIABLES: {
 650                 *params = (NLint)(__nlCurrentContext->nb_variables);
 651         } break;
 652         case NL_LEAST_SQUARES: {
 653                 *params = (NLint)(__nlCurrentContext->least_squares);
 654         } break;
 655         case NL_SYMMETRIC: {
 656                 *params = (NLint)(__nlCurrentContext->symmetric);
 657         } break;
 658         default: {
 659                 __nl_assert_not_reached;
 660         } break;
 661         }
 662 }
 663
 664 /************************************************************************************/
 665 /* Enable / Disable */
 666
 667 void nlEnable(NLenum pname) {
 668         switch(pname) {
 669         default: {
 670                 __nl_assert_not_reached;
 671         }
 672         }
 673 }
 674
 675 void nlDisable(NLenum pname) {
 676         switch(pname) {
 677         default: {
 678                 __nl_assert_not_reached;
 679         }
 680         }
 681 }
 682
 683 NLboolean nlIsEnabled(NLenum pname) {
 684         switch(pname) {
 685         default: {
 686                 __nl_assert_not_reached;
 687         }
 688         }
 689         return NL_FALSE;
 690 }
 691
 692 /************************************************************************************/
 693 /* Get/Set Lock/Unlock variables */
 694
 695 void nlSetVariable(NLuint index, NLfloat value) {
 696         __nlCheckState(__NL_STATE_SYSTEM);
 697         __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1);
 698         __nlCurrentContext->variable[index].value = value;
 699 }
 700
 701 NLfloat nlGetVariable(NLuint index) {
 702         __nl_assert(__nlCurrentContext->state != __NL_STATE_INITIAL);
 703         __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1);
 704         return __nlCurrentContext->variable[index].value;
 705 }
 706
 707 void nlLockVariable(NLuint index) {
 708         __nlCheckState(__NL_STATE_SYSTEM);
 709         __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1);
 710         __nlCurrentContext->variable[index].locked = NL_TRUE;
 711 }
 712
 713 void nlUnlockVariable(NLuint index) {
 714         __nlCheckState(__NL_STATE_SYSTEM);
 715         __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1);
 716         __nlCurrentContext->variable[index].locked = NL_FALSE;
 717 }
 718
 719 NLboolean nlVariableIsLocked(NLuint index) {
 720         __nl_assert(__nlCurrentContext->state != __NL_STATE_INITIAL);
 721         __nl_parano_range_assert(index, 0, __nlCurrentContext->nb_variables - 1);
 722         return __nlCurrentContext->variable[index].locked;
 723 }
 724
 725 /************************************************************************************/
 726 /* System construction */
 727
 728 static void __nlVariablesToVector() {
 729         NLuint i;
 730         __nl_assert(__nlCurrentContext->alloc_x);
 731         __nl_assert(__nlCurrentContext->alloc_variable);
 732         for(i=0; i<__nlCurrentContext->nb_variables; i++) {
 733                 __NLVariable* v = &(__nlCurrentContext->variable[i]);
 734                 if(!v->locked) {
 735                         __nl_assert(v->index < __nlCurrentContext->n);
 736                         __nlCurrentContext->x[v->index] = v->value;
 737                 }
 738         }
 739 }
 740
 741 static void __nlVectorToVariables() {
 742         NLuint i;
 743         __nl_assert(__nlCurrentContext->alloc_x);
 744         __nl_assert(__nlCurrentContext->alloc_variable);
 745         for(i=0; i<__nlCurrentContext->nb_variables; i++) {
 746                 __NLVariable* v = &(__nlCurrentContext->variable[i]);
 747                 if(!v->locked) {
 748                         __nl_assert(v->index < __nlCurrentContext->n);
 749                         v->value = __nlCurrentContext->x[v->index];
 750                 }
 751         }
 752 }
 753
 754 static void __nlBeginSystem() {
 755         __nl_assert(__nlCurrentContext->nb_variables > 0);
 756
 757         if (__nlCurrentContext->solve_again)
 758                 __nlTransition(__NL_STATE_SYSTEM_SOLVED, __NL_STATE_SYSTEM);
 759         else {
 760                 __nlTransition(__NL_STATE_INITIAL, __NL_STATE_SYSTEM);
 761
 762                 __nlCurrentContext->variable = __NL_NEW_ARRAY(
 763                         __NLVariable, __nlCurrentContext->nb_variables
 764                 );
 765                 __nlCurrentContext->alloc_variable = NL_TRUE;
 766         }
 767 }
 768
 769 static void __nlEndSystem() {
 770         __nlTransition(__NL_STATE_MATRIX_CONSTRUCTED, __NL_STATE_SYSTEM_CONSTRUCTED);
 771 }
 772
 773 static void __nlBeginMatrix() {
 774         NLuint i;
 775         NLuint n = 0;
 776         NLenum storage = __NL_ROWS;
 777
 778         __nlTransition(__NL_STATE_SYSTEM, __NL_STATE_MATRIX);
 779
 780         if (!__nlCurrentContext->solve_again) {
 781                 for(i=0; i<__nlCurrentContext->nb_variables; i++) {
 782                         if(!__nlCurrentContext->variable[i].locked)
 783                                 __nlCurrentContext->variable[i].index = n++;
 784                         else
 785                                 __nlCurrentContext->variable[i].index = ~0;
 786                 }
 787
 788                 __nlCurrentContext->n = n;
 789
 790                 /* a least squares problem results in a symmetric matrix */
 791                 if(__nlCurrentContext->least_squares)
 792                         __nlCurrentContext->symmetric = NL_TRUE;
 793
 794                 if(__nlCurrentContext->symmetric)
 795                         storage = (storage | __NL_SYMMETRIC);
 796
 797                 /* SuperLU storage does not support symmetric storage */
 798                 storage = (storage & ~__NL_SYMMETRIC);
 799
 800                 __nlSparseMatrixConstruct(&__nlCurrentContext->M, n, n, storage);
 801                 __nlCurrentContext->alloc_M = NL_TRUE;
 802
 803                 __nlCurrentContext->x = __NL_NEW_ARRAY(NLfloat, n);
 804                 __nlCurrentContext->alloc_x = NL_TRUE;
 805
 806                 __nlCurrentContext->b = __NL_NEW_ARRAY(NLfloat, n);
 807                 __nlCurrentContext->alloc_b = NL_TRUE;
 808         }
 809         else {
 810                 /* need to recompute b only, A is not constructed anymore */
 811                 __NL_CLEAR_ARRAY(NLfloat, __nlCurrentContext->b, __nlCurrentContext->n);
 812         }
 813
 814         __nlVariablesToVector();
 815
 816         __nlRowColumnConstruct(&__nlCurrentContext->af);
 817         __nlCurrentContext->alloc_af = NL_TRUE;
 818         __nlRowColumnConstruct(&__nlCurrentContext->al);
 819         __nlCurrentContext->alloc_al = NL_TRUE;
 820
 821         __nlCurrentContext->current_row = 0;
 822 }
 823
 824 static void __nlEndMatrix() {
 825         __nlTransition(__NL_STATE_MATRIX, __NL_STATE_MATRIX_CONSTRUCTED);
 826
 827         __nlRowColumnDestroy(&__nlCurrentContext->af);
 828         __nlCurrentContext->alloc_af = NL_FALSE;
 829         __nlRowColumnDestroy(&__nlCurrentContext->al);
 830         __nlCurrentContext->alloc_al = NL_FALSE;
 831
 832 #if 0
 833         if(!__nlCurrentContext->least_squares) {
 834                 __nl_assert(
 835                         __nlCurrentContext->current_row ==
 836                         __nlCurrentContext->n
 837                 );
 838         }
 839 #endif
 840 }
 841
 842 static void __nlBeginRow() {
 843         __nlTransition(__NL_STATE_MATRIX, __NL_STATE_ROW);
 844         __nlRowColumnZero(&__nlCurrentContext->af);
 845         __nlRowColumnZero(&__nlCurrentContext->al);
 846 }
 847
 848 static void __nlEndRow() {
 849         __NLRowColumn*  af = &__nlCurrentContext->af;
 850         __NLRowColumn*  al = &__nlCurrentContext->al;
 851         __NLSparseMatrix* M  = &__nlCurrentContext->M;
 852         NLfloat* b              = __nlCurrentContext->b;
 853         NLuint nf                 = af->size;
 854         NLuint nl                 = al->size;
 855         NLuint current_row = __nlCurrentContext->current_row;
 856         NLuint i;
 857         NLuint j;
 858         NLfloat S;
 859         __nlTransition(__NL_STATE_ROW, __NL_STATE_MATRIX);
 860
 861         if(__nlCurrentContext->least_squares) {
 862                 if (!__nlCurrentContext->solve_again) {
 863                         for(i=0; i<nf; i++) {
 864                                 for(j=0; j<nf; j++) {
 865                                         __nlSparseMatrixAdd(
 866                                                 M, af->coeff[i].index, af->coeff[j].index,
 867                                                 af->coeff[i].value * af->coeff[j].value
 868                                         );
 869                                 }
 870                         }
 871                 }
 872
 873                 S = -__nlCurrentContext->right_hand_side;
 874                 for(j=0; j<nl; j++)
 875                         S += al->coeff[j].value;
 876
 877                 for(i=0; i<nf; i++)
 878                         b[ af->coeff[i].index ] -= af->coeff[i].value * S;
 879         } else {
 880                 if (!__nlCurrentContext->solve_again) {
 881                         for(i=0; i<nf; i++) {
 882                                 __nlSparseMatrixAdd(
 883                                         M, current_row, af->coeff[i].index, af->coeff[i].value
 884                                 );
 885                         }
 886                 }
 887                 b[current_row] = -__nlCurrentContext->right_hand_side;
 888                 for(i=0; i<nl; i++) {
 889                         b[current_row] -= al->coeff[i].value;
 890                 }
 891         }
 892         __nlCurrentContext->current_row++;
 893         __nlCurrentContext->right_hand_side = 0.0;
 894 }
 895
 896 void nlMatrixAdd(NLuint row, NLuint col, NLfloat value)
 897 {
 898         __NLSparseMatrix* M  = &__nlCurrentContext->M;
 899         __nlCheckState(__NL_STATE_MATRIX);
 900         __nl_range_assert(row, 0, __nlCurrentContext->n - 1);
 901         __nl_range_assert(col, 0, __nlCurrentContext->nb_variables - 1);
 902         __nl_assert(!__nlCurrentContext->least_squares);
 903
 904         __nlSparseMatrixAdd(M, row, col, value);
 905 }
 906
 907 void nlRightHandSideAdd(NLuint index, NLfloat value)
 908 {
 909         NLfloat* b = __nlCurrentContext->b;
 910
 911         __nlCheckState(__NL_STATE_MATRIX);
 912         __nl_range_assert(index, 0, __nlCurrentContext->n - 1);
 913         __nl_assert(!__nlCurrentContext->least_squares);
 914
 915         b[index] += value;
 916 }
 917
 918 void nlCoefficient(NLuint index, NLfloat value) {
 919         __NLVariable* v;
 920         unsigned int zero= 0;
 921         __nlCheckState(__NL_STATE_ROW);
 922         __nl_range_assert(index, zero, __nlCurrentContext->nb_variables - 1);
 923         v = &(__nlCurrentContext->variable[index]);
 924         if(v->locked)
 925                 __nlRowColumnAppend(&(__nlCurrentContext->al), 0, value*v->value);
 926         else
 927                 __nlRowColumnAppend(&(__nlCurrentContext->af), v->index, value);
 928 }
 929
 930 void nlBegin(NLenum prim) {
 931         switch(prim) {
 932         case NL_SYSTEM: {
 933                 __nlBeginSystem();
 934         } break;
 935         case NL_MATRIX: {
 936                 __nlBeginMatrix();
 937         } break;
 938         case NL_ROW: {
 939                 __nlBeginRow();
 940         } break;
 941         default: {
 942                 __nl_assert_not_reached;
 943         }
 944         }
 945 }
 946
 947 void nlEnd(NLenum prim) {
 948         switch(prim) {
 949         case NL_SYSTEM: {
 950                 __nlEndSystem();
 951         } break;
 952         case NL_MATRIX: {
 953                 __nlEndMatrix();
 954         } break;
 955         case NL_ROW: {
 956                 __nlEndRow();
 957         } break;
 958         default: {
 959                 __nl_assert_not_reached;
 960         }
 961         }
 962 }
 963
 964 /************************************************************************/
 965 /* SuperLU wrapper */
 966
 967 /* Note: SuperLU is difficult to call, but it is worth it.      */
 968 /* Here is a driver inspired by A. Sheffer's "cow flattener". */
 969 static NLboolean __nlFactorize_SUPERLU(__NLContext *context, NLint *permutation) {
 970
 971         /* OpenNL Context */
 972         __NLSparseMatrix* M = &(context->M);
 973         NLuint n = context->n;
 974         NLuint nnz = __nlSparseMatrixNNZ(M); /* number of non-zero coeffs */
 975
 976         /* Compressed Row Storage matrix representation */
 977         NLint   *xa             = __NL_NEW_ARRAY(NLint, n+1);
 978         NLfloat *rhs    = __NL_NEW_ARRAY(NLfloat, n);
 979         NLfloat *a              = __NL_NEW_ARRAY(NLfloat, nnz);
 980         NLint   *asub   = __NL_NEW_ARRAY(NLint, nnz);
 981         NLint   *etree  = __NL_NEW_ARRAY(NLint, n);
 982
 983         /* SuperLU variables */
 984         SuperMatrix At, AtP;
 985         NLint info, panel_size, relax;
 986         superlu_options_t options;
 987
 988         /* Temporary variables */
 989         __NLRowColumn* Ri = NULL;
 990         NLuint i, jj, count;
 991
 992         __nl_assert(!(M->storage & __NL_SYMMETRIC));
 993         __nl_assert(M->storage & __NL_ROWS);
 994         __nl_assert(M->m == M->n);
 995
 996         /* Convert M to compressed column format */
 997         for(i=0, count=0; i<n; i++) {
 998                 __NLRowColumn *Ri = M->row + i;
 999                 xa[i] = count;
1000
1001                 for(jj=0; jj<Ri->size; jj++, count++) {
1002                         a[count] = Ri->coeff[jj].value;
1003                         asub[count] = Ri->coeff[jj].index;
1004                 }
1005         }
1006         xa[n] = nnz;
1007
1008         /* Free M, don't need it anymore at this point */
1009         __nlSparseMatrixClear(M);
1010
1011         /* Create superlu A matrix transposed */
1012         sCreate_CompCol_Matrix(
1013                 &At, n, n, nnz, a, asub, xa,
1014                 SLU_NC,         /* Colum wise, no supernode */
1015                 SLU_S,          /* floats */
1016                 SLU_GE          /* general storage */
1017         );
1018
1019         /* Set superlu options */
1020         set_default_options(&options);
1021         options.ColPerm = MY_PERMC;
1022         options.Fact = DOFACT;
1023
1024         StatInit(&(context->slu.stat));
1025
1026         panel_size = sp_ienv(1); /* sp_ienv give us the defaults */
1027         relax = sp_ienv(2);
1028
1029         /* Compute permutation and permuted matrix */
1030         context->slu.perm_r = __NL_NEW_ARRAY(NLint, n);
1031         context->slu.perm_c = __NL_NEW_ARRAY(NLint, n);
1032
1033         if ((permutation == NULL) || (*permutation == -1)) {
1034                 get_perm_c(3, &At, context->slu.perm_c);
1035
1036                 if (permutation)
1037                         memcpy(permutation, context->slu.perm_c, sizeof(NLint)*n);
1038         }
1039         else
1040                 memcpy(context->slu.perm_c, permutation, sizeof(NLint)*n);
1041
1042         sp_preorder(&options, &At, context->slu.perm_c, etree, &AtP);
1043
1044         /* Decompose into L and U */
1045         sgstrf(&options, &AtP, relax, panel_size,
1046                 etree, NULL, 0, context->slu.perm_c, context->slu.perm_r,
1047                 &(context->slu.L), &(context->slu.U), &(context->slu.stat), &info);
1048
1049         /* Cleanup */
1050
1051         Destroy_SuperMatrix_Store(&At);
1052         Destroy_SuperMatrix_Store(&AtP);
1053
1054         __NL_DELETE_ARRAY(etree);
1055         __NL_DELETE_ARRAY(xa);
1056         __NL_DELETE_ARRAY(rhs);
1057         __NL_DELETE_ARRAY(a);
1058         __NL_DELETE_ARRAY(asub);
1059
1060         context->slu.alloc_slu = NL_TRUE;
1061
1062         return (info == 0);
1063 }
1064
1065 static NLboolean __nlInvert_SUPERLU(__NLContext *context) {
1066
1067         /* OpenNL Context */
1068         NLfloat* b = context->b;
1069         NLfloat* x = context->x;
1070         NLuint n = context->n;
1071
1072         /* SuperLU variables */
1073         SuperMatrix B;
1074         NLint info;
1075
1076         /* Create superlu array for B */
1077         sCreate_Dense_Matrix(
1078                 &B, n, 1, b, n,
1079                 SLU_DN, /* Fortran-type column-wise storage */
1080                 SLU_S,  /* floats                                                 */
1081                 SLU_GE  /* general                                                */
1082         );
1083
1084         /* Forward/Back substitution to compute x */
1085         sgstrs(TRANS, &(context->slu.L), &(context->slu.U),
1086                 context->slu.perm_c, context->slu.perm_r, &B,
1087                 &(context->slu.stat), &info);
1088
1089         if(info == 0)
1090                 memcpy(x, ((DNformat*)B.Store)->nzval, sizeof(*x)*n);
1091
1092         Destroy_SuperMatrix_Store(&B);
1093
1094         return (info == 0);
1095 }
1096
1097 static void __nlFree_SUPERLU(__NLContext *context) {
1098
1099         Destroy_SuperNode_Matrix(&(context->slu.L));
1100         Destroy_CompCol_Matrix(&(context->slu.U));
1101
1102         StatFree(&(context->slu.stat));
1103
1104         __NL_DELETE_ARRAY(context->slu.perm_r);
1105         __NL_DELETE_ARRAY(context->slu.perm_c);
1106
1107         context->slu.alloc_slu = NL_FALSE;
1108 }
1109
1110 void nlPrintMatrix(void) {
1111         __NLSparseMatrix* M  = &(__nlCurrentContext->M);
1112         float *b = __nlCurrentContext->b;
1113         NLuint i, jj, k;
1114         NLuint n = __nlCurrentContext->n;
1115         __NLRowColumn* Ri = NULL;
1116         float *value = malloc(sizeof(*value)*n);
1117
1118         printf("A:\n");
1119         for(i=0; i<n; i++) {
1120                 Ri = &(M->row[i]);
1121
1122                 memset(value, 0.0, sizeof(*value)*n);
1123                 for(jj=0; jj<Ri->size; jj++)
1124                         value[Ri->coeff[jj].index] = Ri->coeff[jj].value;
1125
1126                 for (k = 0; k<n; k++)
1127                         printf("%.3f ", value[k]);
1128                 printf("\n");
1129         }
1130
1131         printf("b:\n");
1132         for(i=0; i<n; i++)
1133                 printf("%f ", b[i]);
1134         printf("\n");
1135
1136         free(value);
1137 }
1138
1139 /************************************************************************/
1140 /* nlSolve() driver routine */
1141
1142 NLboolean nlSolveAdvanced(NLint *permutation, NLboolean solveAgain) {
1143         NLboolean result = NL_TRUE;
1144
1145         __nlCheckState(__NL_STATE_SYSTEM_CONSTRUCTED);
1146
1147         if (!__nlCurrentContext->solve_again)
1148                 result = __nlFactorize_SUPERLU(__nlCurrentContext, permutation);
1149
1150         if (result) {
1151                 result = __nlInvert_SUPERLU(__nlCurrentContext);
1152
1153                 if (result) {
1154                         __nlVectorToVariables();
1155
1156                         if (solveAgain)
1157                                 __nlCurrentContext->solve_again = NL_TRUE;
1158
1159                         __nlTransition(__NL_STATE_SYSTEM_CONSTRUCTED, __NL_STATE_SYSTEM_SOLVED);
1160                 }
1161         }
1162
1163         return result;
1164 }
1165
1166 NLboolean nlSolve() {
1167         return nlSolveAdvanced(NULL, NL_FALSE);
1168 }
1169