distrib/sources/mbk/mbk_matrix.c

   1
   2 #include <stdio.h>
   3 #include <math.h>
   4 #include <limits.h>
   5 #ifdef Linux
   6 #include <values.h>
   7 #else
   8 #include <float.h>
   9 #endif
  10 #include MUT_H
  11 #include AVT_H
  12 //#include "mbk_matrix.h"
  13
  14 //#define FINEST_PIVOT
  15 //#define GAUSS
  16
  17 // ----- optimizaions ------
  18 #define OPTIM_REDUCE
  19 #define OPTIM_MULTIPLY
  20
  21 // -------------------------
  22 void mbk_FreeMatrix(mbk_matrix *mm)
  23 {
  24   DeleteHeap(&mm->mbk_matrix_elem_heap);
  25   mbkfree(mm->line);
  26   mbkfree(mm->col);
  27   if (mm->resolve_line_order!=NULL) mbkfree(mm->resolve_line_order);
  28   if (mm->resolve_unk_order!=NULL) mbkfree(mm->resolve_unk_order);
  29   mbkfree(mm);
  30 }
  31
  32 mbk_matrix *mbk_CreateMatrix(int nbx, int nby)
  33 {
  34   int i;
  35   mbk_matrix *mm;
  36   mm=(mbk_matrix *)mbkalloc(sizeof(mbk_matrix));
  37   mm->tracker0=mm->tracker1=NULL;
  38   mm->line=(elem_index_tab *)mbkalloc(sizeof(elem_index_tab)*nby);
  39   mm->col=(elem_index_tab *)mbkalloc(sizeof(elem_index_tab)*nbx);
  40   for (i=0;i<nbx;i++)
  41     {
  42       mm->col[i].first_elem=NULL;
  43       mm->col[i].last_elem=NULL;
  44       mm->col[i].tracker=NULL;
  45       mm->col[i].NZ=0;
  46     }
  47   for (i=0;i<nby;i++)
  48     {
  49       mm->line[i].first_elem=NULL;
  50       mm->line[i].last_elem=NULL;
  51       mm->line[i].tracker=NULL;
  52       mm->line[i].NZ=0;
  53     }
  54   mm->nbx=nbx;
  55   mm->nby=nby;
  56   mm->resolve_line_order=NULL;
  57   mm->resolve_unk_order=NULL;
  58   CreateHeap(sizeof(mbk_matrix_elem), nbx>4096?nbx:4096, &mm->mbk_matrix_elem_heap);
  59   return mm;
  60 }
  61
  62
  63 static int from_line_border(mbk_matrix *mm, int x, int y,mbk_matrix_elem **found, mbk_matrix_elem **last)
  64 {
  65   if (mm->col[x].first_elem==NULL) { *last=NULL; *found=NULL; return 1; }
  66   if (y==mm->col[x].first_elem->y) { *last=mm->col[x].first_elem; *found=*last; return 1;}
  67   else if (y<mm->col[x].first_elem->y) { *last=mm->col[x].first_elem; *found=NULL; return 1;}
  68   if (y==mm->col[x].last_elem->y) { *last=mm->col[x].last_elem; *found=*last; return 1;}
  69   else if (y>mm->col[x].last_elem->y) { *last=mm->col[x].last_elem; *found=NULL; return 1;}
  70   return 0;
  71 }
  72 static int from_col_border(mbk_matrix *mm, int x, int y, mbk_matrix_elem **found, mbk_matrix_elem **last)
  73 {
  74   if (mm->line[y].first_elem==NULL) { *last=NULL; *found=NULL; return 1; }
  75   if (x==mm->line[y].first_elem->x) { *last=mm->line[y].first_elem; *found=*last; return 1;}
  76   else if (x<mm->line[y].first_elem->x) { *last=mm->line[y].first_elem; *found=NULL; return 1;}
  77   if (x==mm->line[y].last_elem->x) { *last=mm->line[y].last_elem; *found=*last; return 1;}
  78   else if (x>mm->line[y].last_elem->x) { *last=mm->line[y].last_elem; *found=NULL; return 1;}
  79   return 0;
  80 }
  81
  82 static mbk_matrix_elem *goto_line(mbk_matrix *mm, mbk_matrix_elem *start, int y, mbk_matrix_elem **last)
  83 {
  84   mbk_matrix_elem *res;
  85
  86   *last=start;
  87   if (start==NULL) return NULL;
  88
  89   if (start->y==y) return start;
  90
  91   if (from_line_border(mm, start->x, y, &res, last)==1) return res;
  92
  93   if (y>start->y)
  94     {
  95       while (start!=NULL && y>start->y)
  96         {
  97           *last=start;
  98           start=start->DOWN;
  99         }
 100     }
 101   else
 102     {
 103       while (start!=NULL && y<start->y)
 104         {
 105           *last=start;
 106           start=start->UP;
 107         }
 108     }
 109   if (start!=NULL && start->y==y) return start;
 110   return NULL;
 111 }
 112
 113 static mbk_matrix_elem *goto_col(mbk_matrix *mm, mbk_matrix_elem *start, int x, mbk_matrix_elem **last)
 114 {
 115   mbk_matrix_elem *res;
 116
 117   *last=start;
 118   if (start==NULL) return NULL;
 119
 120   if (start->x==x) return start;
 121
 122   if (from_col_border(mm, x, start->y, &res, last)==1) return res;
 123
 124   if (x>start->x)
 125     {
 126       while (start!=NULL && x>start->x)
 127         {
 128           *last=start;
 129           start=start->RIGHT;
 130         }
 131     }
 132   else
 133     {
 134       while (start!=NULL && x<start->x)
 135         {
 136           *last=start;
 137           start=start->LEFT;
 138         }
 139     }
 140   if (start!=NULL && start->x==x) return start;
 141   return NULL;
 142 }
 143
 144 static mbk_matrix_elem *matrix_get(mbk_matrix *mm, int x, int y, mbk_matrix_elem **last)
 145 {
 146   mbk_matrix_elem *res=NULL;
 147   *last=NULL;
 148
 149   if (from_col_border(mm, x, y, &res, last)==1
 150       ||
 151       from_line_border(mm, x, y, &res, last)==1) return res;
 152
 153   if (mm->tracker0!=NULL)
 154     {
 155       if (mm->tracker0->y==y)
 156         res=goto_col(mm, mm->tracker0, x, last);
 157       else
 158         if (mm->tracker0->x==x)
 159           res=goto_line(mm, mm->tracker0, y, last);
 160       if (res) mm->tracker0=res;
 161       if (*last!=NULL) return res;
 162     }
 163   if (mm->tracker1!=NULL && res==NULL)
 164     {
 165       if (mm->tracker1->y==y)
 166         res=goto_col(mm, mm->tracker1, x, last);
 167       else
 168         if (mm->tracker1->x==x)
 169           res=goto_line(mm, mm->tracker1, y, last);
 170       if (res) mm->tracker1=res;
 171       if (*last!=NULL) return res;
 172     }
 173   if (res==NULL)
 174     res=goto_col(mm, mm->line[y].first_elem, x, last);
 175
 176   return res;
 177 }
 178
 179 static inline void get_up_down(mbk_matrix_elem *mme, int y, mbk_matrix_elem **up, mbk_matrix_elem **down)
 180 {
 181   if (mme->y>y) { *down=mme; *up=(*down)->UP; }
 182   else { *up=mme; *down=(*up)->DOWN; }
 183 }
 184
 185 static inline void get_left_right(mbk_matrix_elem *mme, int x, mbk_matrix_elem **left, mbk_matrix_elem **right)
 186 {
 187   if (mme->x>x) { *right=mme; *left=(*right)->LEFT; }
 188   else { *left=mme; *right=(*left)->RIGHT; }
 189 }
 190
 191 static mbk_matrix_elem *matrix_create_elem(mbk_matrix *mm, mbk_matrix_elem *left, mbk_matrix_elem *right, mbk_matrix_elem *up, mbk_matrix_elem *down, int x, int y)
 192 {
 193   mbk_matrix_elem *mme;
 194
 195   mme=AddHeapItem(&mm->mbk_matrix_elem_heap);
 196   mme->x=x;
 197   mme->y=y;
 198   mme->LEFT=left;
 199   mme->RIGHT=right;
 200   mme->UP=up;
 201   mme->DOWN=down;
 202   if (left==NULL) mm->line[mme->y].first_elem=mme;
 203   else left->RIGHT=mme;
 204   if (up==NULL) mm->col[mme->x].first_elem=mme;
 205   else up->DOWN=mme;
 206   if (right==NULL) mm->line[mme->y].last_elem=mme;
 207   else right->LEFT=mme;
 208   if (down==NULL) mm->col[mme->x].last_elem=mme;
 209   else down->UP=mme;
 210   mm->line[y].NZ++;
 211   mm->col[x].NZ++;
 212   return mme;
 213 }
 214
 215 // a utiliser que si l'element n'existe pas
 216 static mbk_matrix_elem *matrix_create_for_set(mbk_matrix *mm, int x, int y)
 217 {
 218   mbk_matrix_elem *mme, *last;
 219   mbk_matrix_elem *up=NULL, *down=NULL, *left=NULL, *right=NULL;
 220
 221   if (mm->tracker0!=NULL)
 222     {
 223       if (mm->tracker0->y==y)
 224         {
 225           goto_col(mm, mm->tracker0, x, &last);
 226           get_left_right(last, x, &left, &right);
 227         }
 228       else
 229         if (mm->tracker0->x==x)
 230           {
 231             goto_line(mm, mm->tracker0, y, &last);
 232             get_up_down(last, y, &up, &down);
 233           }
 234     }
 235   if (mm->tracker1!=NULL)
 236     {
 237       if (mm->tracker1->y==y && (left==NULL && right==NULL))
 238         {
 239           goto_col(mm, mm->tracker1, x, &last);
 240           get_left_right(last, x, &left, &right);
 241         }
 242       else
 243         if (mm->tracker1->x==x && (up==NULL && down==NULL))
 244           {
 245             goto_line(mm, mm->tracker1, y, &last);
 246             get_up_down(last, y, &up, &down);
 247           }
 248     }
 249
 250   if (left==NULL && right==NULL)
 251     {
 252       goto_col(mm, mm->line[y].first_elem, x, &last);
 253       if (last!=NULL)
 254         get_left_right(last, x, &left, &right);
 255     }
 256   if (up==NULL && down==NULL)
 257     {
 258       goto_line(mm, mm->col[x].first_elem, y, &last);
 259       if (last!=NULL)
 260         get_up_down(last, y, &up, &down);
 261     }
 262
 263   mme=matrix_create_elem(mm, left, right, up, down, x, y);
 264
 265   return mme;
 266 }
 267
 268
 269 static inline void settracker(mbk_matrix *mm, mbk_matrix_elem *mme)
 270 {
 271   int d1, d2;
 272   if (mm->tracker0==mme || mm->tracker1==mme) return;
 273   if (mm->tracker0==NULL) d1=0;
 274   else d1=(mm->tracker0->x-mme->x)+(mm->tracker0->y-mme->y);
 275   if (mm->tracker1==NULL) d2=0;
 276   else d2=(mm->tracker1->x-mme->x)+(mm->tracker1->y-mme->y);
 277   if (d1<0) d1=-d1;
 278   if (d2<0) d2=-d2;
 279   if (d1<d2) mm->tracker0=mme;
 280   else mm->tracker1=mme;
 281 }
 282
 283 double mbk_GetMatrixValue(mbk_matrix *mm, int x, int y)
 284 {
 285   mbk_matrix_elem *res, *last;
 286   res=matrix_get(mm, x, y, &last);
 287   if (res==NULL)
 288     {
 289       if (last!=NULL) settracker(mm, last);
 290       return 0;
 291     }
 292   settracker(mm, res);
 293   return res->value;
 294 }
 295
 296 static void matrix_remove(mbk_matrix *mm, mbk_matrix_elem *mme)
 297 {
 298   if (mme->LEFT==NULL) mm->line[mme->y].first_elem=mme->RIGHT;
 299   else mme->LEFT->RIGHT=mme->RIGHT;
 300
 301   if (mme->RIGHT==NULL) mm->line[mme->y].last_elem=mme->LEFT;
 302   else mme->RIGHT->LEFT=mme->LEFT;
 303
 304   if (mme->UP==NULL) mm->col[mme->x].first_elem=mme->DOWN;
 305   else mme->UP->DOWN=mme->DOWN;
 306
 307   if (mme->DOWN==NULL) mm->col[mme->x].last_elem=mme->UP;
 308   else mme->DOWN->UP=mme->UP;
 309
 310   if (mm->tracker0==mme) mm->tracker0=mme->LEFT;
 311   if (mm->tracker1==mme) mm->tracker1=mme->LEFT;
 312
 313   if (mm->col[mme->x].tracker==mme) mm->col[mme->x].tracker=mme->DOWN;
 314   if (mm->line[mme->y].tracker==mme) mm->line[mme->y].tracker=mme->LEFT;
 315
 316   mm->line[mme->y].NZ--;
 317   mm->col[mme->x].NZ--;
 318
 319   DelHeapItem(&mm->mbk_matrix_elem_heap, mme);
 320 }
 321
 322 void matrix_clearline(mbk_matrix *mm, int i)
 323 {
 324   mbk_matrix_elem *res, *nres;
 325   for (res=mm->line[i].first_elem; res!=NULL; res=nres)
 326     {
 327       nres=res->RIGHT;
 328       matrix_remove(mm, res);
 329     }
 330 }
 331
 332 void matrix_clearcol(mbk_matrix *mm, int i)
 333 {
 334   mbk_matrix_elem *res, *nres;
 335   for (res=mm->col[i].first_elem; res!=NULL; res=nres)
 336     {
 337       nres=res->DOWN;
 338       matrix_remove(mm, res);
 339     }
 340 }
 341
 342 void mbk_SetMatrixValue(mbk_matrix *mm, int x, int y, double val)
 343 {
 344   mbk_matrix_elem *res, *last;
 345   res=matrix_get(mm, x, y, &last);
 346   if (res!=NULL)
 347     {
 348       if (val==0)
 349         {
 350           matrix_remove(mm, res);
 351         }
 352       else
 353         {
 354           res->value=val;
 355           settracker(mm, res);
 356         }
 357     }
 358   else if (val!=0)
 359     {
 360       res=matrix_create_for_set(mm, x, y);
 361       res->value=val;
 362       settracker(mm, res);
 363     }
 364 }
 365
 366 inline int mbk_GetMatrixNZUR(mbk_matrix *mm, int x)
 367 {
 368   return mm->col[x].NZ;
 369 }
 370
 371 inline int mbk_GetMatrixNZLC(mbk_matrix *mm, int y)
 372 {
 373   return mm->line[y].NZ;
 374 }
 375
 376 void mbk_DisplayMatrix(char *filename, mbk_matrix *mm)
 377 {
 378   int i, j;
 379   char buf[20];
 380   FILE *f=stdout;
 381
 382   if (filename!=NULL)
 383     {
 384       f=fopen(filename,"wt");
 385     }
 386
 387   for (j=0; j<mm->nby; j++)
 388     {
 389       for (i=0; i<mm->nbx; i++)
 390         {
 391           sprintf(buf, "%.3g", mbk_GetMatrixValue(mm, i, j));
 392           fprintf(f," %9s", buf);
 393         }
 394       fprintf(f,"\n");
 395     }
 396
 397   fprintf(f,"NZUR:");
 398   for (i=0; i<mm->nbx; i++)
 399     {
 400       sprintf(buf, "%d", mbk_GetMatrixNZUR(mm, i));
 401       fprintf(f," %2s", buf);
 402     }
 403   fprintf(f,"\n");
 404   fprintf(f,"NZLC:");
 405   for (j=0; j<mm->nby; j++)
 406     {
 407       sprintf(buf, "%d", mbk_GetMatrixNZLC(mm, j));
 408       fprintf(f," %2s", buf);
 409     }
 410   fprintf(f,"\n");
 411   if (mm->resolve_line_order!=NULL)
 412     {
 413       fprintf(f,"RESOLVE LINE ORDER:");
 414       for (i=0; i<mm->nby; i++)
 415         fprintf(f," %d", mm->resolve_line_order[i]);
 416       fprintf(f,"\n");
 417     }
 418   if (mm->resolve_unk_order!=NULL)
 419     {
 420       fprintf(f,"RESOLVE UNK  ORDER:");
 421       for (i=0; i<mm->nby; i++)
 422         fprintf(f," %d", mm->resolve_unk_order[i]);
 423       fprintf(f,"\n");
 424     }
 425   if (f!=stdout) fclose(f);
 426 }
 427
 428
 429 static chain_list *find_lowestNZ(elem_index_tab *eit, chain_list **unlocked)
 430 {
 431   int i, lowest=INT_MAX;
 432   chain_list *cl=NULL, **prev, *ch;
 433
 434   prev=unlocked;
 435   ch=*unlocked;
 436   while (ch!=NULL)
 437     {
 438       i=(int)(long)ch->DATA;
 439       if (eit[i].NZ<lowest)
 440         {
 441           freechain(cl);
 442           cl=addchain(NULL, prev);
 443           lowest=eit[i].NZ;
 444         }
 445 #ifdef FINEST_PIVOT
 446       else if (eit[i].NZ==lowest) { cl=addchain(cl, prev); }
 447 #endif
 448       prev=&ch->NEXT;
 449       ch=ch->NEXT;
 450     }
 451   return cl;
 452 }
 453
 454 #if 0
 455 static int findPivot(mbk_matrix *u, chain_list **unlockedlines, chain_list **unlockedcols, int *x, int *y)
 456 {
 457   chain_list *colcandidates, *linecandidates;
 458   chain_list *clx, *cly, **tmp, *tmpfree, **bestx=NULL, **besty=NULL;
 459   double highest=-DBL_MAX, val;
 460
 461   colcandidates=find_lowestNZ(u->col, unlockedcols);
 462   linecandidates=find_lowestNZ(u->line, unlockedlines);
 463   if (colcandidates->NEXT==NULL && linecandidates->NEXT==NULL)
 464     {
 465       tmp=(chain_list **)colcandidates->DATA;
 466       tmpfree=*tmp;
 467       *x=(int)(long)tmpfree->DATA;
 468       *tmp=(*tmp)->NEXT;
 469       tmpfree->NEXT=NULL; freechain(tmpfree);
 470
 471       tmp=(chain_list **)linecandidates->DATA;
 472       tmpfree=*tmp;
 473       *y=(int)(long)tmpfree->DATA;
 474       *tmp=(*tmp)->NEXT;
 475       tmpfree->NEXT=NULL; freechain(tmpfree);
 476       // ligne et column du pivot sont enleves des listes des col et ligne non bloque
 477     }
 478   else
 479     {
 480       int i, j;
 481       for (cly=linecandidates; cly!=NULL; cly=cly->NEXT)
 482         {
 483           tmp=(chain_list **)cly->DATA;
 484           j=(int)(long)(*tmp)->DATA;
 485           for (clx=colcandidates; clx!=NULL; clx=clx->NEXT)
 486             {
 487               tmp=(chain_list **)clx->DATA;
 488               i=(int)(long)(*tmp)->DATA;
 489               val=mbk_GetMatrixValue(u, i, j);
 490               if (val<0) val=-val;
 491               if (val>highest)
 492                 {
 493                   highest=val;
 494                   *x=i;
 495                   *y=j;
 496                   bestx=(chain_list **)clx->DATA;
 497                   besty=(chain_list **)cly->DATA;
 498                 }
 499             }
 500         }
 501       if (bestx==NULL || besty==NULL)
 502         {
 503           avt_error("mbk_matrix", 2, AVT_ERR, "<NaN> or <Inf> value found in matrix\n");
 504           return 1;
 505         }
 506       // enleve ligne et column du pivot des listes des col et ligne non bloque
 507       tmpfree=*bestx; *bestx=(*bestx)->NEXT;
 508       tmpfree->NEXT=NULL; freechain(tmpfree);
 509       tmpfree=*besty; *besty=(*besty)->NEXT;
 510       tmpfree->NEXT=NULL; freechain(tmpfree);
 511     }
 512   freechain(colcandidates);
 513   freechain(linecandidates);
 514   return 0;
 515 }
 516 #endif
 517 /*
 518 static chain_list **find_MAXPivot(mbk_matrix *u, int y, chain_list **unlockedcols, char *locking, double *highest, chain_list **who)
 519 {
 520   mbk_matrix_elem *mme;
 521   chain_list *cl;
 522   int x;
 523
 524   mme=u->line[y];
 525
 526   for (cl=unlockedcols; cl!=NULL; cl=cl->NEXT)
 527     {
 528       x=(*cl)->DATA;
 529       val=mbk_GetMatrixValue(u, x, y);
 530       if (val<0) val=-val;
 531       if (val>*highest)
 532         {
 533           who=cl;
 534           *highest=val;
 535         }
 536     }
 537   retur who;
 538 }
 539 */
 540 static int findPivot(mbk_matrix *u, chain_list **unlockedlines, chain_list **unlockedcols, int *x, int *y)
 541 {
 542   chain_list **colfound, *linecandidates, **linefound, *cl, *ch, **prev;
 543   chain_list **tmp, *tmpfree;
 544   double highest=-DBL_MAX, val;
 545   int xx, yy;
 546
 547 //  colcandidates=find_lowestNZ(u->col, unlockedcols);
 548   colfound=NULL; linefound=NULL;
 549   linecandidates=find_lowestNZ(u->line, unlockedlines);
 550
 551 // version en dessous, on parcourt quasiment toute la matrice dans notre cas
 552 #if 0
 553   if (linecandidates->NEXT!=NULL)
 554     {
 555       colcandidates=find_lowestNZ(u->col, unlockedcols);
 556       for (cl=colcandidates; cl!=NULL; cl=cl->NEXT)
 557         {
 558           tmp=(chain_list **)ch->DATA;
 559           xx=(int)(long)(*tmp)->DATA;
 560           for (ch=linecandidates; ch!=NULL; ch=ch->NEXT)
 561         {
 562           tmp=(chain_list **)ch->DATA;
 563           yy=(int)(long)(*tmp)->DATA;
 564
 565         }
 566         }
 567       freechain(colcandidates);
 568     }
 569 #endif
 570
 571
 572   for (ch=linecandidates; ch!=NULL; ch=ch->NEXT)
 573     {
 574       tmp=(chain_list **)ch->DATA;
 575       yy=(int)(long)(*tmp)->DATA;
 576       prev=unlockedcols;
 577       for (cl=*unlockedcols; cl!=NULL; prev=&cl->NEXT, cl=cl->NEXT)
 578         {
 579           xx=(int)(long)cl->DATA;
 580           val=mbk_GetMatrixValue(u, xx, yy);
 581           if (val<0) val=-val;
 582           if (val>highest)
 583             {
 584               colfound=prev;
 585               linefound=(chain_list **)ch->DATA;
 586               highest=val;
 587             }
 588         }
 589 #ifndef FINEST_PIVOT
 590       break;
 591 #endif
 592     }
 593
 594   if (colfound==NULL) return 1;
 595
 596   *x=(int)(long)(*colfound)->DATA;
 597   *y=(int)(long)(*linefound)->DATA;
 598
 599   // enleve ligne et column du pivot des listes des col et ligne non bloque
 600   tmpfree=*colfound; *colfound=(*colfound)->NEXT;
 601   tmpfree->NEXT=NULL; freechain(tmpfree);
 602   tmpfree=*linefound; *linefound=(*linefound)->NEXT;
 603   tmpfree->NEXT=NULL; freechain(tmpfree);
 604
 605   freechain(linecandidates);
 606   return 0;
 607 }
 608
 609 void mbk_MatrixCopyCol(mbk_matrix *u, mbk_matrix *l, int xu, int xl, char *locking)
 610 {
 611   mbk_matrix_elem *res, *nres;
 612   for (res=l->col[xl].first_elem; res!=NULL; res=nres)
 613     {
 614       nres=res->DOWN;
 615       matrix_remove(l, res);
 616     }
 617   for (res=u->col[xu].first_elem; res!=NULL; res=res->DOWN)
 618     {
 619       if (locking==NULL || locking[res->y]==0)
 620         mbk_SetMatrixValue(l, xl, res->y, res->value);
 621     }
 622 }
 623
 624 void mbk_MatrixCopyCol__notfinished(mbk_matrix *u, mbk_matrix *l, int xu, int xl, char *locking)
 625 {
 626   mbk_matrix_elem *res, *nres, *mme;
 627   for (res=l->col[xl].first_elem; res!=NULL; res=nres)
 628     {
 629       nres=res->DOWN;
 630       matrix_remove(l, res);
 631     }
 632   nres=NULL; mme=NULL;
 633   for (res=u->col[xu].first_elem; res!=NULL; res=res->DOWN)
 634     {
 635       if (locking==NULL || locking[res->y]==0)
 636         {
 637           mme=matrix_create_elem(l, NULL, NULL, nres, NULL, xl, res->y);
 638           mme->value=res->value;
 639           if (nres==NULL) l->col[xl].first_elem=mme;
 640           else nres->DOWN=mme;
 641           nres=mme;
 642         }
 643     }
 644   l->col[xl].last_elem=mme;
 645 }
 646
 647 void mbk_MatrixCopyCol__notfinished_linkline(mbk_matrix *l)
 648 {
 649   mbk_matrix_elem *res;
 650   int xl;
 651
 652   for (xl=0; xl<l->nby; xl++) l->line[xl].tracker=NULL;
 653
 654   for (xl=0; xl<l->nbx; xl++)
 655     {
 656       for (res=l->col[xl].first_elem; res!=NULL; res=res->DOWN)
 657         {
 658           res->LEFT=l->line[res->y].tracker;
 659           if (res->LEFT!=NULL) res->LEFT->RIGHT=res;
 660           else l->line[res->y].first_elem=res;
 661           l->line[res->y].tracker=res;
 662         }
 663     }
 664   for (xl=0; xl<l->nby; xl++) l->line[xl].last_elem=l->line[xl].tracker;
 665 }
 666
 667
 668
 669 void mbk_MatrixMultiplyLineBy(mbk_matrix *u, int y, char *locking, double mult)
 670 {
 671   mbk_matrix_elem *res, *nres;
 672
 673   for (res=u->line[y].first_elem; res!=NULL; res=nres)
 674     {
 675       nres=res->RIGHT;
 676       if (locking==NULL || locking[res->x]==0)
 677         {
 678           res->value*=mult;
 679           if (res->value==0)
 680             {
 681               matrix_remove(u, res);
 682             }
 683         }
 684     }
 685 }
 686
 687 void mbk_MatrixDivideLineBy(mbk_matrix *u, int y, char *locking, double mult)
 688 {
 689   mbk_matrix_elem *res, *nres;
 690
 691   for (res=u->line[y].first_elem; res!=NULL; res=nres)
 692     {
 693       nres=res->RIGHT;
 694       if (locking==NULL || locking[res->x]==0)
 695         {
 696           res->value/=mult;
 697           if (res->value==0)
 698             {
 699               matrix_remove(u, res);
 700             }
 701         }
 702     }
 703 }
 704
 705 mbk_matrix_elem *tracker_goto_line(mbk_matrix *mm, int x, int y)
 706 {
 707   mbk_matrix_elem *res, *last;
 708
 709   if (from_line_border(mm, x, y, &res, &last)==1)
 710     {
 711       mm->col[x].tracker=last;
 712     }
 713   else
 714     {
 715       last=NULL;
 716       if (mm->col[x].tracker==NULL) mm->col[x].tracker=mm->col[x].first_elem;
 717       res=mm->col[x].tracker;
 718       if (res!=NULL)
 719         {
 720           if (y>res->y)
 721             {
 722               while (res!=NULL && y>res->y)
 723                 {
 724                   last=res;
 725                   res=res->DOWN;
 726                 }
 727               mm->col[x].tracker=last;
 728             }
 729           else
 730             {
 731               while (res!=NULL && y<res->y)
 732                 {
 733                   last=res;
 734                   res=res->UP;
 735                 }
 736             }
 737         }
 738       mm->col[x].tracker=last;
 739     }
 740   return  mm->col[x].tracker;
 741 }
 742
 743 void followline(mbk_matrix *mm, int dy, int sy, char *locking, double mult)
 744 {
 745   mbk_matrix_elem *a, *b, *na, *preva=NULL, *last;
 746   mbk_matrix_elem *left, *right, *up, *down;
 747   double val;
 748   a=mm->line[dy].first_elem;
 749   b=mm->line[sy].first_elem;
 750
 751   while (a!=NULL || b!=NULL)
 752     {
 753       if (a!=NULL && b!=NULL && a->x==b->x)
 754         {
 755           na=a->RIGHT;
 756           if (locking[a->x]==0)
 757             {
 758               val=a->value=a->value-(b->value*mult);
 759               if (val==0) matrix_remove(mm, a);
 760               else
 761                 {
 762                   a->value=val;
 763                   preva=a;
 764                 }
 765             }
 766           else preva=a;
 767           a=na; b=b->RIGHT;
 768         }
 769       else if (b==NULL || (a!=NULL && a->x<b->x))
 770         {
 771           na=a->RIGHT;
 772           preva=a; a=na;
 773         }
 774       else //if (a->x>b->x)
 775         {
 776           //              na=a->RIGHT;
 777           val=0-(b->value*mult);
 778           if (locking[b->x]==0)
 779             {
 780               last=tracker_goto_line(mm, b->x, dy);
 781               if (last==NULL)
 782                 left=right=up=down=NULL;
 783               else
 784                 {
 785                   get_up_down(last, dy, &up, &down);
 786                 }
 787               a=matrix_create_elem(mm, preva, a, up, down, b->x, dy);
 788               a->value=val;
 789               preva=a;
 790             }
 791           b=b->RIGHT;
 792         }
 793     }
 794 }
 795
 796
 797
 798 // elem(dy)(x) = elem(dy)(x) - elem(sy)(x) * mult
 799 void mbk_MatrixLineMultThenSub(mbk_matrix *u, int dy, int sy, char *locking, double mult)
 800 {
 801 /*  double sval, dval;
 802   int i;*/
 803   followline(u, dy, sy, locking, mult);
 804
 805 /*  for (i=0; i<u->nbx; i++)
 806     {
 807       if (locking==NULL || locking[i]==0)
 808         {
 809           sval=mbk_GetMatrixValue(u, i, sy);
 810           dval=mbk_GetMatrixValue(u, i, dy);
 811           dval=dval-(sval*mult);
 812           mbk_SetMatrixValue(u, i, dy, dval);
 813         }
 814     }
 815 */
 816 }
 817
 818 void mbkMatrixSwapCols(mbk_matrix *u, int x0, int x1)
 819 {
 820   int j;
 821   double val0, val1;
 822   for (j=0; j<u->nby; j++)
 823     {
 824       val0=mbk_GetMatrixValue(u, x0, j);
 825       val1=mbk_GetMatrixValue(u, x1, j);
 826       mbk_SetMatrixValue(u, x1, j, val0);
 827       mbk_SetMatrixValue(u, x0, j, val1);
 828     }
 829 }
 830
 831 void mbkMatrixSwapLines(mbk_matrix *u, int y0, int y1)
 832 {
 833   int j;
 834   double val0, val1;
 835   for (j=0; j<u->nbx; j++)
 836     {
 837       val0=mbk_GetMatrixValue(u, j, y0);
 838       val1=mbk_GetMatrixValue(u, j, y1);
 839       mbk_SetMatrixValue(u, j, y1, val0);
 840       mbk_SetMatrixValue(u, j, y0, val1);
 841     }
 842 }
 843
 844 int mbk_CreateLUMatrix(mbk_matrix *u, mbk_matrix *l, mbk_matrix *sol)
 845 {
 846   char *lockedCOL, *lockedLINE;
 847   int i, j, pivot_x, pivot_y;
 848   double pivot_value, val;
 849   mbk_matrix_elem *mme, *last;
 850   chain_list *unlockedCOL, *unlockedLINE, *cl;
 851
 852   if (u->nbx!=u->nby)
 853     {
 854       avt_errmsg (MBK_ERRMSG, "036", AVT_ERROR);
 855       return 1;
 856     }
 857   if (l!=NULL && (u->nbx!=l->nbx || l->nbx!=l->nby))
 858     {
 859       avt_errmsg (MBK_ERRMSG, "037", AVT_ERROR);
 860       return 1;
 861     }
 862
 863   if (u->resolve_line_order==NULL)
 864     {
 865       u->resolve_line_order=mbkalloc(sizeof(int)*u->nby);
 866       u->resolve_unk_order=mbkalloc(sizeof(int)*u->nby);
 867     }
 868   if (l!=NULL && l->resolve_line_order==NULL)
 869     {
 870       l->resolve_line_order=mbkalloc(sizeof(int)*l->nby);
 871       l->resolve_unk_order=mbkalloc(sizeof(int)*l->nby);
 872     }
 873   lockedCOL=mbkalloc(sizeof(char)*u->nbx);
 874   lockedLINE=mbkalloc(sizeof(char)*u->nby);
 875   unlockedCOL=unlockedLINE=NULL;
 876
 877   for (i=u->nbx-1;i>=0;i--)
 878     {
 879       lockedCOL[i]=0;
 880       lockedLINE[i]=0;
 881       unlockedCOL=addchain(unlockedCOL, (void *)(long)i);
 882       unlockedLINE=addchain(unlockedLINE, (void *)(long)i);
 883     }
 884
 885   for (i=0; i<u->nbx; i++)
 886     {
 887 #ifndef GAUSS
 888       if (findPivot(u, &unlockedLINE, &unlockedCOL, &pivot_x, &pivot_y))
 889         {
 890           avt_errmsg (MBK_ERRMSG, "038", AVT_ERROR);
 891           return 1;
 892         }
 893 #else
 894       pivot_x=pivot_y=i;
 895       unlockedLINE=unlockedLINE->NEXT;
 896       unlockedCOL=unlockedCOL->NEXT;
 897
 898       if (i!=pivot_x)
 899         {
 900           mbkMatrixSwapCols(u, i, pivot_x);
 901         }
 902       if (i!=pivot_y)
 903         {
 904           mbkMatrixSwapLines(u, i, pivot_y);
 905         }
 906 #endif
 907
 908       if (l!=NULL)
 909         mbk_MatrixCopyCol__notfinished(u, l, pivot_x, pivot_y, lockedLINE);
 910
 911       pivot_value=mbk_GetMatrixValue(u, pivot_x, pivot_y);
 912       if (pivot_value==0)
 913         {
 914           avt_errmsg (MBK_ERRMSG, "039", AVT_ERROR);
 915           return 1;
 916         }
 917
 918       if (l!=NULL)
 919         {
 920           l->resolve_line_order[i]=pivot_y;
 921           l->resolve_unk_order[i]=pivot_y;
 922         }
 923       u->resolve_line_order[u->nby-i-1]=pivot_y;
 924       u->resolve_unk_order[u->nby-i-1]=pivot_x;
 925
 926       mbk_MatrixDivideLineBy(u, pivot_y, lockedCOL, pivot_value);
 927       if (sol!=NULL)
 928         mbk_MatrixDivideLineBy(sol, pivot_y, NULL, pivot_value);
 929
 930       for (cl=unlockedLINE; cl!=NULL; cl=cl->NEXT)
 931         {
 932           j=(int)(long)cl->DATA;
 933 #if 1
 934           if (cl==unlockedLINE)
 935             {
 936               mme=matrix_get(u, pivot_x, j, &last);
 937               if (mme==NULL) { val=0; mme=u->col[pivot_x].first_elem; }
 938               else { val=mme->value; mme=mme->DOWN; }
 939             }
 940           else
 941             {
 942               while (mme!=NULL && mme->y<j) { mme=mme->DOWN; }
 943               if (mme==NULL || mme->y!=j) val=0;
 944               else { val=mme->value; mme=mme->DOWN; }
 945             }
 946 #else
 947           val=mbk_GetMatrixValue(u, pivot_x, j);
 948 #endif
 949           if (val!=0)
 950             {
 951               mbk_MatrixLineMultThenSub(u, j, pivot_y, lockedCOL, val);
 952               if (sol!=NULL)
 953                 mbk_MatrixLineMultThenSub(sol, j, pivot_y, NULL, val);
 954             }
 955         }
 956
 957       lockedCOL[pivot_x]=1;
 958       lockedLINE[pivot_y]=1;
 959 #if 0
 960       printf("U' *************\n");
 961       mbk_DisplayMatrix(stdout,u);
 962 #endif
 963     }
 964
 965   if (l!=NULL) mbk_MatrixCopyCol__notfinished_linkline(l);
 966
 967   mbkfree(lockedLINE);
 968   mbkfree(lockedCOL);
 969   return 0;
 970 }
 971
 972 mbk_matrix *mbk_MatrixMultiplyMatrix(mbk_matrix *a, mbk_matrix *b)
 973 {
 974   mbk_matrix *res;
 975   int /*i,*/ j, k;
 976   double cumul;
 977   mbk_matrix_elem *mmx, *mmy, *mmxstart;
 978   if (a->nbx!=b->nby)
 979     {
 980       avt_errmsg (MBK_ERRMSG, "040", AVT_ERROR);
 981       return NULL;
 982     }
 983   res=mbk_CreateMatrix(b->nbx, a->nby);
 984
 985 #ifndef OPTIM_MULTIPLY
 986   for (j=0; j<a->nby; j++)
 987     {
 988       for (k=0; k<b->nbx; k++)
 989         {
 990           cumul=0;
 991           for (i=0; i<b->nby; i++)
 992             {
 993               cumul+=mbk_GetMatrixValue(a, i, j)*mbk_GetMatrixValue(b, k, i);
 994             }
 995           mbk_SetMatrixValue(res, k, j, cumul);
 996         }
 997     }
 998 #else
 999   for (j=0; j<a->nby; j++)
1000     {
1001       mmxstart=a->line[j].first_elem;
1002       for (k=0; k<b->nbx; k++)
1003         {
1004           mmy=b->col[k].first_elem;
1005           mmx=mmxstart;
1006           cumul=0;
1007           while (mmx!=NULL && mmy!=NULL)
1008             {
1009               if (mmx->x<mmy->y) mmx=mmx->RIGHT;
1010               else if (mmy->y<mmx->x) mmy=mmy->DOWN;
1011               else
1012                 {
1013                   cumul+=mmx->value*mmy->value;
1014                   mmx=mmx->RIGHT;
1015                   mmy=mmy->DOWN;
1016                 }
1017             }
1018           if (cumul!=0)
1019             mbk_SetMatrixValue(res, k, j, cumul);
1020         }
1021     }
1022 #endif
1023   return res;
1024 }
1025
1026 mbk_matrix *mbk_MatrixSubstractMatrix(mbk_matrix *a, mbk_matrix *b)
1027 {
1028   mbk_matrix *res;
1029   int j, k;
1030   double cumul;
1031   if (a->nbx!=b->nbx || a->nby!=b->nby)
1032     {
1033       avt_errmsg (MBK_ERRMSG, "041", AVT_ERROR);
1034       return NULL;
1035     }
1036
1037   res=mbk_CreateMatrix(a->nbx, a->nby);
1038   for (j=0; j<a->nby; j++)
1039     {
1040       for (k=0; k<a->nbx; k++)
1041         {
1042           cumul=mbk_GetMatrixValue(a, k, j)-mbk_GetMatrixValue(b, k, j);
1043           mbk_SetMatrixValue(res, k, j, cumul);
1044         }
1045     }
1046   return res;
1047 }
1048
1049 mbk_matrix *mbk_MatrixDuplicateMatrix(mbk_matrix *a)
1050 {
1051   mbk_matrix *res;
1052   int j, k;
1053   double cumul;
1054   res=mbk_CreateMatrix(a->nbx, a->nby);
1055   for (j=0; j<a->nby; j++)
1056     {
1057       for (k=0; k<a->nbx; k++)
1058         {
1059           cumul=mbk_GetMatrixValue(a, k, j);
1060           mbk_SetMatrixValue(res, k, j, cumul);
1061         }
1062     }
1063   return res;
1064 }
1065
1066 void mbk_MatrixAddHalfLinkedElem(mbk_matrix *mm, int x, int y, double value)
1067 {
1068   mbk_matrix_elem *mme;
1069   if (value==0) return;
1070   mme=matrix_create_elem(mm, NULL, NULL, mm->col[x].tracker, NULL, x, y);
1071   mme->value=value;
1072   if (mm->col[x].tracker!=NULL) mm->col[x].tracker->DOWN=mme;
1073   mm->col[x].tracker=mme;
1074 }
1075
1076 void mbk_MatrixFinishElemLink(mbk_matrix *mm)
1077 {
1078   mbk_MatrixCopyCol__notfinished_linkline(mm);
1079 }
1080
1081 double *mbk_MatrixSolveUsingArray(mbk_matrix *a, double *solval, double *unkval)
1082 {
1083   int o, i, u;
1084   double divval, cumul;
1085   mbk_matrix_elem *mme;
1086
1087   if (a->resolve_line_order==NULL)
1088     {
1089       avt_errmsg (MBK_ERRMSG, "042", AVT_ERROR);
1090       return NULL;
1091     }
1092   if (unkval==NULL) unkval=mbkalloc(sizeof(double)*a->nbx);
1093
1094   for (o=0; o<a->nby; o++)
1095     {
1096       i=a->resolve_line_order[o];
1097       u=a->resolve_unk_order[o];
1098       cumul=0; divval=0;
1099       for (mme=a->line[i].first_elem; mme!=NULL; mme=mme->RIGHT)
1100         {
1101           if (mme->x==u) divval=mme->value;
1102           else cumul=cumul+(mme->value*unkval[mme->x]);
1103         }
1104       unkval[u]=(solval[i]-cumul)/divval;
1105     }
1106   return unkval;
1107 }
1108
1109 mbk_matrix *mbk_MatrixSolve(mbk_matrix *a, mbk_matrix *sol)
1110 {
1111   int i;
1112   double *unkval, *solval;
1113   mbk_matrix *res;
1114
1115   if (sol->nbx!=1 || sol->nby!=a->nby)
1116     {
1117       avt_errmsg (MBK_ERRMSG, "043", AVT_ERROR);
1118       return NULL;
1119     }
1120   solval=mbkalloc(sizeof(double)*sol->nby);
1121   for (i=0;i<sol->nby;i++) solval[i]=mbk_GetMatrixValue(sol, 0, i);
1122
1123   unkval=mbk_MatrixSolveUsingArray(a, solval, NULL);
1124
1125   mbkfree(solval);
1126
1127   if ((res=mbk_CreateMatrix(1, a->nbx))==NULL) return res;
1128
1129   for (i=0;i<a->nbx;i++)
1130     if (unkval[i]!=0) mbk_SetMatrixValue(res, 0, i, unkval[i]);
1131
1132   return res;
1133 }
1134
1135 int mbk_MatrixReduce(mbk_matrix *a, int limit_index)
1136 {
1137   int i, j;
1138   double pval, lval, sval;
1139   mbk_matrix_elem *mme;
1140
1141 #ifdef OPTIM_REDUCE
1142   for (i=a->nby-1; i>=limit_index; i--)
1143     {
1144       if ((mme=a->line[i].last_elem)==NULL || mme->x!=i)
1145         {
1146           pval=0;
1147           avt_errmsg (MBK_ERRMSG, "044", AVT_ERROR);
1148           return 1;
1149         }
1150       else
1151         {
1152           pval=mme->value;
1153         }
1154       mbk_MatrixDivideLineBy(a, i, NULL, -pval);
1155       for (j=0; j<i; j++)
1156         {
1157           if ((mme=a->line[j].last_elem)==NULL || mme->x!=i)
1158             lval=0;
1159           else
1160             {
1161               lval=mme->value;
1162             }
1163           if (lval!=0)
1164             {
1165               for (mme=a->line[i].first_elem; mme!=NULL && mme->x<i; mme=mme->RIGHT)
1166                 {
1167                   sval=mbk_GetMatrixValue(a, mme->x, j);
1168                   mbk_SetMatrixValue(a, mme->x, j, sval+lval*mme->value);
1169                 }
1170             }
1171         }
1172       matrix_clearcol(a, i);
1173       matrix_clearline(a, i);
1174     }
1175 #else
1176   {
1177     int k;
1178   for (i=a->nby-1; i>=limit_index; i--)
1179     {
1180       pval=mbk_GetMatrixValue(a, i, i);
1181       mbk_MatrixDivideLineBy(a, i, NULL, -pval);
1182       for (k=0; k<i; k++)
1183         {
1184           pval=mbk_GetMatrixValue(a, k, i);
1185           for (j=0; j<i; j++)
1186             {
1187               lval=mbk_GetMatrixValue(a, i, j);
1188               if (lval!=0)
1189                 {
1190                   sval=mbk_GetMatrixValue(a, k, j);
1191                   mbk_SetMatrixValue(a, k, j,
1192                                      sval+lval*pval);
1193                 }
1194             }
1195         }
1196     }
1197   }
1198 #endif
1199   return 0;
1200 }
1201
1202 int mbk_MatrixTraverse(mbk_matrix *a, matrix_traverse_func mtf, void *data)
1203 {
1204   int i, ret;
1205   mbk_matrix_elem *mme, *nextmme;
1206
1207   for (i=0; i<a->nby; i++)
1208     {
1209       for (mme=a->line[i].first_elem; mme!=NULL; mme=nextmme)
1210         {
1211           nextmme=mme->RIGHT;
1212           ret=mtf(mme->x, mme->y, &mme->value, data);
1213           if (mme->value==0) matrix_remove(a, mme);
1214           if (ret!=0) return 1;
1215         }
1216     }
1217   return 0;
1218 }
1219
1220 static int pp(int x, int y, double *val, void *data)
1221 {
1222   if (*val!=0)
1223     fprintf((FILE *)data, "mbk_SetMatrixValue(u,%d,%d,%e);\n",x, y, *val);
1224   return 0;
1225 }
1226
1227 void matdrive(mbk_matrix *a, char *name)
1228 {
1229   FILE *f;
1230   f=fopen("toto.c","wt");
1231   fprintf(f,"u=mbk_CreateMatrix(%d, %d);\n",a->nbx, a->nby);
1232   fprintf(f,"l=mbk_CreateMatrix(%d, %d);\n",a->nbx, a->nby);
1233   mbk_MatrixTraverse(a, pp, f);
1234   fclose(f);
1235   name=NULL;
1236 }