nl-matrix.c

   1 /* nl-matrix.c --- matrix functions for newLISP
   2
   3     Copyright (C) 2008 Lutz Mueller
   4
   5     This program is free software: you can redistribute it and/or modify
   6     it under the terms of the GNU General Public License as published by
   7     the Free Software Foundation, either version 3 of the License, or
   8     (at your option) any later version.
   9
  10     This program is distributed in the hope that it will be useful,
  11     but WITHOUT ANY WARRANTY; without even the implied warranty of
  12     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13     GNU General Public License for more details.
  14
  15     You should have received a copy of the GNU General Public License
  16     along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17
  18 */
  19
  20 /* Since version 8.9.9 all matrix operations work on lists and
  21    arrays. Previously only lists where supported.
  22 */
  23
  24 #include "newlisp.h"
  25 #include "protos.h"
  26
  27 #define TINY 1.0e-20
  28
  29
  30 double * * multiply(double ** A, double ** B, int m, int n, int k, int l);
  31 double * * invert(double * * A, int n, int * err);
  32 int ludcmp(double * * a, int n, int * indx, double * d);
  33 void lubksb(double * * a, int n, int * indx, double * b);
  34 double * * getMatrix(CELL * params, int * type, int * n, int * m, int * err, int evalFlag);
  35 double * * makeMatrix(CELL * number, int n, int m);
  36 int getDimensions(CELL * mat, int * n, int * m);
  37 double * * data2matrix(CELL * list, int n, int m);
  38 CELL * matrix2data(double * * matrix, int type, int n, int m);
  39 double * * allocateMatrix(int rows, int cols);
  40 void freeMatrix(double * * m, int rows);
  41
  42 /* since version 7.4.8 'transpose' tramspose any matrix not only
  43    matrices containing numbers.
  44 */
  45
  46 CELL * p_matTranspose(CELL * params)
  47 {
  48 CELL * A;
  49 CELL * B;
  50 CELL * rowA = NULL;
  51 CELL * cell = NULL;
  52 CELL * conCell;
  53 CELL * new;
  54 int n, m, i, j;
  55 CELL * * Brows;
  56
  57 A = evaluateExpression(params);
  58
  59 if(A->type == CELL_ARRAY)
  60         return(arrayTranspose(A));
  61
  62 if(A->type != CELL_EXPRESSION)
  63         return(errorProcExt(ERR_NOT_MATRIX, params));
  64
  65 if(getDimensions(A, &n, &m) == FALSE)
  66     return(errorProcExt(ERR_NOT_MATRIX, params));
  67
  68 if(n == 0 || m == 0)
  69     return(errorProcExt(ERR_WRONG_DIMENSIONS, params));
  70
  71 Brows = allocMemory(sizeof(CELL *) * m);
  72 for(j = 0; j < m; j++)
  73         {
  74         Brows[j] = getCell(CELL_EXPRESSION);
  75         if(j > 0) Brows[j-1]->next = Brows[j];
  76         }
  77
  78 B = getCell(CELL_EXPRESSION);
  79 B->contents = (UINT)Brows[0];
  80
  81 for(i = 0; i < n; i++)
  82         {
  83         if(i == 0)
  84                 rowA = (CELL*)A->contents;
  85         else
  86                 rowA = rowA->next;
  87
  88         if(rowA->type != CELL_EXPRESSION)
  89                 conCell = rowA;
  90         else
  91                 conCell = NULL;
  92
  93         for(j = 0; j < m; j++)
  94                 {
  95                 if(conCell == NULL)
  96                         {
  97                         if(j == 0)
  98                                 cell = (CELL*)rowA->contents;
  99                         else
 100                                 cell = cell->next;
 101                         new = copyCell(cell);
 102                         }
 103                 else
 104                         new = copyCell(conCell);
 105
 106                 if( i == 0)
 107                         Brows[j]->contents = (UINT)new;
 108                 else
 109                         Brows[j]->next = new;
 110
 111                 Brows[j] = new;
 112                 }
 113         }
 114
 115 free(Brows);
 116
 117 return(B);
 118 }
 119
 120
 121 CELL * p_matMultiply(CELL * params)
 122 {
 123 CELL * C;
 124 double * * X = NULL;
 125 double * * Y = NULL;
 126 int typeX, typeY, typeM;
 127 double * * M = NULL;
 128 int n, m, k, l;
 129 int err = 0;
 130
 131 if((X = getMatrix(params, &typeX, &n, &m, &err, TRUE)) == NULL)
 132         return(errorProcExt(err, params));
 133
 134 if((Y = getMatrix(params->next, &typeY, &k, &l, &err, TRUE)) == NULL)
 135         {
 136         freeMatrix(X, n);
 137         return(errorProcExt(err, params->next));
 138         }
 139
 140 typeM = (typeX == CELL_ARRAY || typeY == CELL_ARRAY) ? CELL_ARRAY : CELL_EXPRESSION;
 141
 142 if(m != k)
 143         err = ERR_WRONG_DIMENSIONS;
 144 else if((M = multiply(X, Y, n, m, k, l)) == NULL)
 145         err = ERR_NOT_ENOUGH_MEMORY;
 146
 147 freeMatrix(X, n);
 148 freeMatrix(Y, k);
 149
 150 if(err) return(errorProc(err));
 151
 152 C = matrix2data(M, typeM, n, l);
 153 freeMatrix(M, n);
 154
 155 return(C);
 156 }
 157
 158 CELL * p_matInvert(CELL * params)
 159 {
 160 CELL * dataY;
 161 int n, m, err = 0;
 162 int typeA;
 163 double * * A;
 164 double * * Y;
 165
 166 if((A = getMatrix(params, &typeA, &n, &m, &err, TRUE)) == NULL)
 167         return(errorProcExt(err, params));
 168
 169 if(n != m)
 170         {
 171         freeMatrix(A, n);
 172         return(errorProcExt(ERR_WRONG_DIMENSIONS, params));
 173         }
 174
 175 if( (Y = invert(A, n, &err)) == NULL)
 176         {
 177         freeMatrix(A, n);
 178         if(err) return(errorProc(err));
 179         return(nilCell);
 180         }
 181
 182 dataY = matrix2data(Y, typeA, n, n);
 183
 184 freeMatrix(A, n);
 185 freeMatrix(Y, n);
 186
 187 return(dataY);
 188 }
 189
 190
 191 CELL * p_determinant(CELL * params)
 192 {
 193 double * * M;
 194 double d;
 195 int typeM, n, m, i, err;
 196 int * indx;
 197
 198 if( (M = getMatrix(params, &typeM, &m, &n, &err, TRUE)) == NULL)
 199         return(errorProcExt(err, params));
 200
 201 if(n != m)
 202         {
 203         freeMatrix(M, n);
 204         return(errorProcExt(ERR_WRONG_DIMENSIONS, params));
 205         }
 206
 207 indx = (int *)calloc((n + 1), sizeof(int));
 208
 209 if(ludcmp(M, n, indx, &d) == FALSE)
 210         return(nilCell);
 211
 212 for(i = 1; i <= n; i++)
 213         d *= M[i][i];
 214
 215 return(stuffFloat(&d));
 216 }
 217
 218
 219 CELL * p_matScalar(CELL * params)
 220 {
 221 CELL * param;
 222 double * * A = NULL;
 223 double * * B = NULL;
 224 double * * M = NULL;
 225 int typeA, typeB, typeC = 0;
 226 int type;
 227 int n, m, k, l, err = 0;
 228
 229 CELL * result;
 230 param = params;
 231 param = evaluateExpression(param);
 232 if(param->type == CELL_SYMBOL)
 233         type = *((SYMBOL *)param->contents)->name;
 234 else if(param->type == CELL_PRIMITIVE)
 235         type = *(char *)param->aux;
 236 else
 237         return(errorProcExt(ERR_ILLEGAL_TYPE, params));
 238
 239 params = params->next;
 240
 241 if( (A = getMatrix(params, &typeA, &n, &m, &err, TRUE)) == NULL)
 242         return(errorProcExt(err, params));
 243 result = evaluateExpression(params->next);
 244 if(isNumber(result->type))
 245         {
 246         k = n, l = m;
 247         if((B = makeMatrix(result, k, l)) == NULL)
 248                 {
 249                 err = ERR_NOT_ENOUGH_MEMORY;
 250                 goto MAT_SCALAR_FIN;
 251                 }
 252         typeB = CELL_EXPRESSION;
 253         }
 254 else if( (B = getMatrix(result, &typeB, &k, &l, &err, FALSE)) == NULL)
 255         {
 256         freeMatrix(A, n);
 257         return(errorProc(err));
 258         }
 259
 260 typeC = (typeA == CELL_ARRAY || typeB == CELL_ARRAY) ? CELL_ARRAY : CELL_EXPRESSION;
 261
 262 if(n != k || m != l)
 263         err = ERR_WRONG_DIMENSIONS;
 264 else if((M = allocateMatrix(n, m)) == NULL)
 265         err = ERR_NOT_ENOUGH_MEMORY;
 266 else
 267         {
 268         for(k = 1; k <= n; k++)
 269          for(l = 1; l <= m; l++)
 270                 {
 271                 switch(type)
 272                         {
 273                         case '+': M[k][l] = A[k][l] + B[k][l]; break;
 274                         case '-': M[k][l] = A[k][l] - B[k][l]; break;
 275                         case '*': M[k][l] = A[k][l] * B[k][l]; break;
 276                         case '/':
 277                                 {
 278                                 if(B[k][l] == 0)
 279                                         return(errorProc(ERR_MATH));
 280                                 else
 281                                         M[k][l] = A[k][l] / B[k][l];
 282                                 }
 283                                 break;
 284                         default:
 285             return(errorProcExt(ERR_ILLEGAL_TYPE, params));
 286                         }
 287                 }
 288 }
 289
 290 MAT_SCALAR_FIN:
 291 freeMatrix(A, n);
 292 freeMatrix(B, n);
 293 if(err) return (errorProc(err));
 294
 295 result = matrix2data(M, typeC, n, m);
 296 freeMatrix(M, n);
 297
 298 return(result);
 299 }
 300
 301
 302 /* ------------------- C = A*B, A and B unchanged --------------------- */
 303
 304 double * * multiply(double ** A, double ** B, int n, int m, int k, int l)
 305 {
 306 double * * C;
 307 int i, j, s;
 308 double sum;
 309
 310 if((C = allocateMatrix(n, l)) == NULL)
 311         return(NULL);
 312
 313 if(m != k)
 314         {
 315         errorProc(ERR_WRONG_DIMENSIONS);
 316         return(NULL);
 317         }
 318
 319 for(i = 1; i <= n; i++)
 320         {
 321         for(j = 1; j <= l; j++)
 322                 {
 323                 sum = 0.0;
 324                 for(s = 1; s <= m; s++)
 325                         sum += A[i][s] * B[s][j];
 326
 327                 C[i][j] = sum;
 328                 }
 329         }
 330
 331 return(C);
 332 }
 333
 334
 335 /* ----- return inverse of A in Y, A will contain LU decomposition --- */
 336
 337 double * * invert(double * * A, int n, int * err)
 338 {
 339 double * * Y = NULL;
 340 double * col;
 341 double d;
 342 int i, j;
 343 int * indx;
 344
 345
 346 col = (double *)calloc(n + 4, sizeof(double));
 347 indx = (int *)calloc((n + 1), sizeof(int));
 348
 349 if(ludcmp(A, n, indx, &d) == FALSE)
 350         goto INVERT_FIN;
 351
 352 if((Y = allocateMatrix(n, n)) == NULL)
 353         {
 354         *err = ERR_NOT_ENOUGH_MEMORY;
 355         goto INVERT_FIN;
 356         }
 357
 358 for(j = 1; j <= n; j++)
 359         {
 360         for(i = 1; i <= n; i++) col[i] = 0.0;
 361         col[j] = 1.0;
 362         lubksb(A, n, indx, col);
 363         for(i = 1; i <= n; i++) Y[i][j] = col[i];
 364         }
 365
 366 INVERT_FIN:
 367 free(col);
 368 free(indx);
 369
 370 return(Y);
 371 }
 372
 373
 374 /* ------------------------- LU decomposition ---------------------------
 375 // algorithms ludcmp() and lubskb() adapted from:
 376 // Numerical Recipes in 'C', 2nd Edition
 377 // W.H. Press, S.A. Teukolsky
 378 // W.T. Vettering, B.P. Flannery
 379 */
 380
 381 int ludcmp(double * * a, int n, int * indx, double * d)
 382 {
 383 int i, imax = 0, j, k;
 384 double big, dum, sum, temp;
 385 double * vv;
 386
 387 vv = (double *)calloc(n + 4, sizeof(double));
 388 *d = 1.0;
 389
 390 /* find abs biggest number in each row and put 1/big in vector */
 391 for (i = 1; i <= n; i++)
 392         {
 393         big = 0.0;
 394         for(j = 1;j <= n; j++)
 395                 if ((temp = fabs(a[i][j])) > big) big = temp;
 396
 397         if(big == 0.0)
 398                 {
 399                 free(vv);
 400                 return(FALSE);
 401                 }
 402
 403         vv[i] = 1.0 / big;
 404         }
 405
 406 for (j = 1; j <= n; j++)
 407         {
 408         for (i = 1; i < j; i++)
 409                 {
 410                 sum = a[i][j];
 411                 for(k = 1; k < i; k++) sum -= a[i][k] * a[k][j];
 412                 a[i][j] = sum;
 413                 }
 414         big = 0.0;
 415
 416         for (i = j; i <= n; i++)
 417                 {
 418                 sum = a[i][j];
 419                 for(k = 1; k < j; k++)
 420                         sum -= a[i][k] * a[k][j];
 421                 a[i][j] = sum;
 422
 423                 if ( (dum = vv[i] * fabs(sum)) >= big)
 424                         {
 425                         big = dum;
 426                         imax = i;
 427                         }
 428                 }
 429
 430         if (j != imax)
 431                 {
 432                 for (k = 1; k <= n; k++)
 433                         {
 434                         dum = a[imax][k];
 435                         a[imax][k] = a[j][k];
 436                         a[j][k] = dum;
 437                         }
 438
 439                 *d = -(*d);
 440                 vv[imax] = vv[j];
 441                 }
 442
 443
 444         indx[j] = imax;
 445         if (a[j][j] == 0.0) a[j][j] = TINY;
 446
 447         if (j != n)
 448                 {
 449                 dum = 1.0 / (a[j][j]);
 450                 for(i = j+1; i <= n; i++) a[i][j] *= dum;
 451                 }
 452         }
 453
 454 free(vv);
 455 return(TRUE);
 456 }
 457
 458
 459 void lubksb(double * * a, int n, int * indx, double * b)
 460 {
 461 int i, ii = 0, ip, j;
 462 double sum;
 463
 464 for (i = 1; i <= n; i++)
 465         {
 466         ip = indx[i];
 467         sum = b[ip];
 468         b[ip] = b[i];
 469         if (ii)
 470                 for(j = ii; j <= i-1; j++) sum -= a[i][j] * b[j];
 471         else
 472                 if(sum) ii = i;
 473         b[i] = sum;
 474         }
 475
 476 for (i = n; i >= 1; i--)
 477         {
 478         sum = b[i];
 479         for(j = i+1; j <= n; j++) sum -= a[i][j] * b[j];
 480         b[i] = sum / a[i][i];
 481         }
 482 }
 483
 484 /* ------------ make a matrix from list or array expr ----------- */
 485
 486 double * * getMatrix(CELL * params, int * type, int * n, int * m, int *err, int evalFlag)
 487 {
 488 CELL * data;
 489 double * * M;
 490
 491 if(evalFlag)
 492         data = evaluateExpression(params);
 493 else
 494         data = params;
 495
 496 if(data->type != CELL_EXPRESSION && data->type != CELL_ARRAY)
 497         {
 498         *err = ERR_NOT_MATRIX;
 499         return(NULL);
 500         }
 501
 502 *type = data->type;
 503
 504 if(getDimensions(data, n, m) == FALSE)
 505         {
 506     *err = ERR_NOT_MATRIX;
 507         return(NULL);
 508         }
 509
 510 if( (M = data2matrix(data, *n, *m)) == NULL)
 511         {
 512         *err = ERR_NOT_ENOUGH_MEMORY;
 513         return(NULL);
 514         }
 515
 516 return(M);
 517 }
 518
 519 double * * makeMatrix(CELL * number, int n, int m)
 520 {
 521 double s;
 522 double * * matrix;
 523 int i, j;
 524
 525 #ifndef NEWLISP64
 526 if(number->type == CELL_FLOAT)
 527         s = *(double *)&number->aux;
 528 else if(number->type == CELL_INT64)
 529         s = *(INT64 *)&number->aux;
 530 #else
 531 if(number->type == CELL_FLOAT)
 532         s = *(double *)&number->contents;
 533 #endif
 534 else
 535         s = number->contents;
 536
 537 if((matrix = allocateMatrix(n, m)) == NULL)
 538         return(NULL);
 539
 540 for(i = 1; i <= n; i++)
 541         for(j = 1; j <= m; j++)
 542                 matrix[i][j] = s;
 543
 544 return(matrix);
 545 }
 546
 547 /* --------------------------- get dimensons or a matrix --------------- */
 548
 549 int getDimensions(CELL * mat, int * n, int * m)
 550 {
 551 CELL * row;
 552 CELL * cell;
 553 int rows, cols;
 554 CELL * * addr;
 555
 556 if(mat->type == CELL_ARRAY)
 557         {
 558         addr = (CELL * *)mat->contents;
 559         *n = (mat->aux - 1) / sizeof(CELL *);
 560         cell = *addr;
 561         if(cell->type != CELL_ARRAY)
 562                 {
 563                 errorProcExt(ERR_WRONG_DIMENSIONS, mat);
 564                 return(FALSE);
 565                 }
 566         *m = (cell->aux - 1) / sizeof(CELL *);
 567         return(TRUE);
 568         }
 569
 570 /* mat->type is CELL_EXPRESSSION */
 571
 572 row = (CELL *)mat->contents;
 573 if(row->type != CELL_EXPRESSION)
 574         {
 575         errorProcExt(ERR_NOT_MATRIX, mat);
 576         return(FALSE);
 577         }
 578
 579 cell = (CELL *)row->contents;
 580
 581 rows = cols = 0;
 582 while(row != nilCell)
 583         {
 584         ++rows;
 585         row = row->next;
 586         }
 587
 588 while(cell != nilCell)
 589         {
 590         ++cols;
 591         cell = cell->next;
 592         }
 593
 594 if(rows == 0 || cols == 0)
 595         {
 596         errorProcExt(ERR_WRONG_DIMENSIONS, mat);
 597         return(FALSE);
 598         }
 599
 600 *n = rows;
 601 *m = cols;
 602
 603 return(TRUE);
 604 }
 605
 606
 607 /* ------------ copy array/list into a matrix of doubles -------- */
 608
 609
 610 /* xlate lists or arrays into C arrays with 1-offest for 1st element
 611 wrong row type result and missing row elelements result on 0.0 */
 612 double * * data2matrix(CELL * list, int n, int m)
 613 {
 614 double ** matrix;
 615 CELL * row;
 616 CELL * cell;
 617 int i, j, size;
 618 CELL * * addr;
 619 CELL * * rowAddr;
 620
 621 if((matrix = allocateMatrix(n, m)) == NULL)
 622         return(NULL);
 623
 624 if(list->type == CELL_ARRAY)
 625         {
 626         addr = (CELL * *)list->contents;
 627         for(i = 1; i <= n; i++)
 628                 {
 629                 row = *(addr + i - 1);
 630                 if(row->type != CELL_ARRAY)
 631                         {
 632                         for(j = 1; j <= m; j++) matrix[i][j] = 0.0;
 633                         continue;
 634                         }
 635                 rowAddr = (CELL * *)row->contents;
 636                 size = (row->aux - 1)/sizeof(CELL *);
 637                 for(j = 1; j <= m; j++)
 638                         {
 639                         if(j <= size)
 640                                 matrix[i][j] = getDirectFloat((CELL *)*(rowAddr + j - 1));
 641                         else matrix[i][j] = 0.0;
 642                         }
 643                 }
 644         return(matrix);
 645         }
 646
 647 /* list->type == CELL_EXPRESSION */
 648
 649 row = (CELL *)list->contents;
 650 for(i = 1; i <= n; i++)
 651         {
 652         if(row->type != CELL_EXPRESSION)
 653                 for(j = 1; j <= m; j++) matrix[i][j] = 0.0;
 654         else
 655                 {
 656                 cell = (CELL *)row->contents;
 657                 for(j = 1; j <= m; j++)
 658                         {
 659                         matrix[i][j] = getDirectFloat(cell);
 660                         cell = cell->next;
 661                         }
 662                 }
 663         row = row->next;
 664         }
 665
 666 return(matrix);
 667 }
 668
 669
 670 /* ------- allocate an array/list and copy matrix into it ---- */
 671
 672 CELL * matrix2data(double ** matrix, int type, int n, int m)
 673 {
 674 CELL * list;
 675 CELL * row;
 676 CELL * cell;
 677 int i, j;
 678 double fnum;
 679 CELL * * addr;
 680 CELL * * rowAddr;
 681
 682 list = getCell(type);
 683
 684 if(type == CELL_EXPRESSION)
 685         {
 686         row = getCell(CELL_EXPRESSION);
 687         list->contents = (UINT)row;
 688         for(i = 1; i <= n; i++)
 689                 {
 690                 cell = getCell(CELL_FLOAT);
 691                 row->contents = (UINT)cell;
 692                 for(j = 1; j <= m; j++)
 693                         {
 694                         fnum = matrix[i][j];
 695 #ifndef NEWLISP64
 696                         *(double *)&cell->aux = fnum;
 697 #else
 698                         *(double *)&cell->contents = fnum;
 699 #endif
 700                         if(j == m) break;
 701                         cell->next = getCell(CELL_FLOAT);
 702                         cell = cell->next;
 703                         }
 704                 if(i == n) break;
 705                 row->next = getCell(CELL_EXPRESSION);
 706                 row = row->next;
 707                 }
 708         }
 709
 710 else /* type is CELL_ARRAY */
 711         {
 712         list->aux = n * sizeof(CELL *) + 1;
 713         addr = (CELL * *)allocMemory(list->aux);
 714         list->contents = (UINT)addr;
 715
 716         for(i = 1; i <= n; i++)
 717                 {
 718                 cell = getCell(CELL_ARRAY);
 719                 cell->aux = m * sizeof(CELL *) + 1;
 720                 rowAddr = (CELL * *)allocMemory(cell->aux);
 721                 cell->contents = (UINT)rowAddr;
 722                 *(addr + i -1) = cell;
 723                 for(j = 1; j <= m; j++)
 724                         {
 725                         fnum = matrix[i][j];
 726                         *(rowAddr + j - 1) = stuffFloat(&fnum);
 727                         }
 728                 }
 729         }
 730
 731 return(list);
 732 }
 733
 734
 735 /* ------------------------ allocate/free a matrix ----------------- */
 736
 737 double * * allocateMatrix(int rows, int cols)
 738 {
 739 int i;
 740 double * * m;
 741
 742 m = (double **) calloc(rows + 1, sizeof(double *));
 743 if (!m)
 744         return(NULL);
 745
 746 for(i = 1; i <= rows; i++)
 747         {
 748         m[i] = (double *)calloc(cols + 1, sizeof(double));
 749         if (!m[i])
 750                 return(NULL);
 751         }
 752
 753 return(m);
 754 }
 755
 756
 757 void freeMatrix(double * * m, int rows)
 758 {
 759 int i;
 760
 761 if(m == NULL) return;
 762
 763 for(i = 1; i <= rows; i++)
 764         free(m[i]);
 765
 766 free(m);
 767 }
 768
 769 /* end of file */