src/matrix.c

   1 /* matrix.c
   2  * Matrix building and solving routines
   3  * Copyright (C) 1993-2003,2010,2013 Olly Betts
   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 2 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, write to the Free Software
  17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  18  */
  19
  20 /*#define SOR 1*/
  21
  22 #if 0
  23 # define DEBUG_INVALID 1
  24 #endif
  25
  26 #include <config.h>
  27
  28 #include "debug.h"
  29 #include "cavern.h"
  30 #include "filename.h"
  31 #include "message.h"
  32 #include "netbits.h"
  33 #include "matrix.h"
  34 #include "out.h"
  35
  36 #undef PRINT_MATRICES
  37 #define PRINT_MATRICES 0
  38
  39 #undef DEBUG_MATRIX_BUILD
  40 #define DEBUG_MATRIX_BUILD 0
  41
  42 #undef DEBUG_MATRIX
  43 #define DEBUG_MATRIX 0
  44
  45 #if PRINT_MATRICES
  46 static void print_matrix(real *M, real *B, long n);
  47 #endif
  48
  49 static void choleski(real *M, real *B, long n);
  50
  51 #ifdef SOR
  52 static void sor(real *M, real *B, long n);
  53 #endif
  54
  55 /* for M(row, col) col must be <= row, so Y <= X */
  56 # define M(X, Y) ((real *)M)[((((OSSIZE_T)(X)) * ((X) + 1)) >> 1) + (Y)]
  57               /* +(Y>X?0*printf("row<col (line %d)\n",__LINE__):0) */
  58 /*#define M_(X, Y) ((real *)M)[((((OSSIZE_T)(Y)) * ((Y) + 1)) >> 1) + (X)]*/
  59
  60 static int find_stn_in_tab(node *stn);
  61 static int add_stn_to_tab(node *stn);
  62 static void build_matrix(node *list);
  63
  64 static long n_stn_tab;
  65
  66 static pos **stn_tab;
  67
  68 extern void
  69 solve_matrix(node *list)
  70 {
  71    node *stn;
  72    long n = 0;
  73    FOR_EACH_STN(stn, list) {
  74       if (!fixed(stn)) n++;
  75    }
  76    if (n == 0) return;
  77
  78    /* we just need n to be a reasonable estimate >= the number
  79     * of stations left after reduction. If memory is
  80     * plentiful, we can be crass.
  81     */
  82    stn_tab = osmalloc((OSSIZE_T)(n * ossizeof(pos*)));
  83    n_stn_tab = 0;
  84
  85    FOR_EACH_STN(stn, list) {
  86       if (!fixed(stn)) add_stn_to_tab(stn);
  87    }
  88
  89    if (n_stn_tab < n) {
  90       /* release unused entries in stn_tab */
  91       stn_tab = osrealloc(stn_tab, n_stn_tab * ossizeof(pos*));
  92    }
  93
  94    build_matrix(list);
  95 #if DEBUG_MATRIX
  96    FOR_EACH_STN(stn, list) {
  97       printf("(%8.2f, %8.2f, %8.2f ) ", POS(stn, 0), POS(stn, 1), POS(stn, 2));
  98       print_prefix(stn->name);
  99       putnl();
 100    }
 101 #endif
 102
 103    osfree(stn_tab);
 104 }
 105
 106 #ifdef NO_COVARIANCES
 107 # define FACTOR 1
 108 #else
 109 # define FACTOR 3
 110 #endif
 111
 112 static void
 113 build_matrix(node *list)
 114 {
 115    real *M;
 116    real *B;
 117    int dim;
 118
 119    if (n_stn_tab == 0) {
 120       if (!fQuiet)
 121          puts(msg(/*Network solved by reduction - no simultaneous equations to solve.*/74));
 122       return;
 123    }
 124    /* (OSSIZE_T) cast may be needed if n_stn_tab>=181 */
 125    M = osmalloc((OSSIZE_T)((((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1)) * ossizeof(real));
 126    B = osmalloc((OSSIZE_T)(n_stn_tab * FACTOR * ossizeof(real)));
 127
 128    if (!fQuiet) {
 129       if (n_stn_tab == 1)
 130          out_current_action(msg(/*Solving one equation*/78));
 131       else
 132          out_current_action1(msg(/*Solving %d simultaneous equations*/75), n_stn_tab);
 133    }
 134
 135 #ifdef NO_COVARIANCES
 136    dim = 2;
 137 #else
 138    dim = 0; /* fudge next loop for now */
 139 #endif
 140    for ( ; dim >= 0; dim--) {
 141       node *stn;
 142       int row;
 143
 144       /* Initialise M and B to zero - zeroing "linearly" will minimise
 145        * paging when the matrix is large */
 146       {
 147          int end = n_stn_tab * FACTOR;
 148          for (row = 0; row < end; row++) B[row] = (real)0.0;
 149          end = ((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1;
 150          for (row = 0; row < end; row++) M[row] = (real)0.0;
 151       }
 152
 153       /* Construct matrix - Go thru' stn list & add all forward legs between
 154        * two unfixed stations to M (so each leg goes on exactly once).
 155        *
 156        * All legs between two fixed stations can be ignored here.
 157        *
 158        * All legs between a fixed and an unfixed station are then considered
 159        * from the unfixed end (if we consider them from the fixed end we'd
 160        * need to somehow detect when we're at a fixed point cut line and work
 161        * out which side we're dealing with at this time. */
 162       FOR_EACH_STN(stn, list) {
 163 #ifdef NO_COVARIANCES
 164          real e;
 165 #else
 166          svar e;
 167          delta a;
 168 #endif
 169          int f, t;
 170          int dirn;
 171 #if DEBUG_MATRIX_BUILD
 172          print_prefix(stn->name);
 173          printf(" used: %d colour %ld\n",
 174                 (!!stn->leg[2]) << 2 | (!!stn -> leg[1]) << 1 | (!!stn->leg[0]),
 175                 stn->colour);
 176
 177          for (dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
 178 #ifdef NO_COVARIANCES
 179             printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
 180                    stn->leg[dirn]->v[0], stn->leg[dirn]->l.reverse);
 181 #else
 182             printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
 183                    stn->leg[dirn]->v[0][0], stn->leg[dirn]->l.reverse);
 184 #endif
 185             print_prefix(stn->leg[dirn]->l.to->name);
 186             putnl();
 187          }
 188          putnl();
 189 #endif /* DEBUG_MATRIX_BUILD */
 190
 191          if (!fixed(stn)) {
 192             f = find_stn_in_tab(stn);
 193             for (dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
 194                linkfor *leg = stn->leg[dirn];
 195                node *to = leg->l.to;
 196                if (fixed(to)) {
 197                   bool fRev = !data_here(leg);
 198                   if (fRev) leg = reverse_leg(leg);
 199                   /* Ignore equated nodes */
 200 #ifdef NO_COVARIANCES
 201                   e = leg->v[dim];
 202                   if (e != (real)0.0) {
 203                      e = ((real)1.0) / e;
 204                      M(f,f) += e;
 205                      B[f] += e * POS(to, dim);
 206                      if (fRev) {
 207                         B[f] += leg->d[dim];
 208                      } else {
 209                         B[f] -= leg->d[dim];
 210                      }
 211                   }
 212 #else
 213                   if (invert_svar(&e, &leg->v)) {
 214                      delta b;
 215                      int i;
 216                      if (fRev) {
 217                         adddd(&a, &POSD(to), &leg->d);
 218                      } else {
 219                         subdd(&a, &POSD(to), &leg->d);
 220                      }
 221                      mulsd(&b, &e, &a);
 222                      for (i = 0; i < 3; i++) {
 223                         M(f * FACTOR + i, f * FACTOR + i) += e[i];
 224                         B[f * FACTOR + i] += b[i];
 225                      }
 226                      M(f * FACTOR + 1, f * FACTOR) += e[3];
 227                      M(f * FACTOR + 2, f * FACTOR) += e[4];
 228                      M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
 229                   }
 230 #endif
 231                } else if (data_here(leg)) {
 232                   /* forward leg, unfixed -> unfixed */
 233                   t = find_stn_in_tab(to);
 234 #if DEBUG_MATRIX
 235                   printf("Leg %d to %d, var %f, delta %f\n", f, t, e,
 236                          leg->d[dim]);
 237 #endif
 238                   /* Ignore equated nodes & lollipops */
 239 #ifdef NO_COVARIANCES
 240                   e = leg->v[dim];
 241                   if (t != f && e != (real)0.0) {
 242                      real a;
 243                      e = ((real)1.0) / e;
 244                      M(f,f) += e;
 245                      M(t,t) += e;
 246                      if (f < t) M(t,f) -= e; else M(f,t) -= e;
 247                      a = e * leg->d[dim];
 248                      B[f] -= a;
 249                      B[t] += a;
 250                   }
 251 #else
 252                   if (t != f && invert_svar(&e, &leg->v)) {
 253                      int i;
 254                      mulsd(&a, &e, &leg->d);
 255                      for (i = 0; i < 3; i++) {
 256                         M(f * FACTOR + i, f * FACTOR + i) += e[i];
 257                         M(t * FACTOR + i, t * FACTOR + i) += e[i];
 258                         if (f < t)
 259                            M(t * FACTOR + i, f * FACTOR + i) -= e[i];
 260                         else
 261                            M(f * FACTOR + i, t * FACTOR + i) -= e[i];
 262                         B[f * FACTOR + i] -= a[i];
 263                         B[t * FACTOR + i] += a[i];
 264                      }
 265                      M(f * FACTOR + 1, f * FACTOR) += e[3];
 266                      M(t * FACTOR + 1, t * FACTOR) += e[3];
 267                      M(f * FACTOR + 2, f * FACTOR) += e[4];
 268                      M(t * FACTOR + 2, t * FACTOR) += e[4];
 269                      M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
 270                      M(t * FACTOR + 2, t * FACTOR + 1) += e[5];
 271                      if (f < t) {
 272                         M(t * FACTOR + 1, f * FACTOR) -= e[3];
 273                         M(t * FACTOR, f * FACTOR + 1) -= e[3];
 274                         M(t * FACTOR + 2, f * FACTOR) -= e[4];
 275                         M(t * FACTOR, f * FACTOR + 2) -= e[4];
 276                         M(t * FACTOR + 2, f * FACTOR + 1) -= e[5];
 277                         M(t * FACTOR + 1, f * FACTOR + 2) -= e[5];
 278                      } else {
 279                         M(f * FACTOR + 1, t * FACTOR) -= e[3];
 280                         M(f * FACTOR, t * FACTOR + 1) -= e[3];
 281                         M(f * FACTOR + 2, t * FACTOR) -= e[4];
 282                         M(f * FACTOR, t * FACTOR + 2) -= e[4];
 283                         M(f * FACTOR + 2, t * FACTOR + 1) -= e[5];
 284                         M(f * FACTOR + 1, t * FACTOR + 2) -= e[5];
 285                      }
 286                   }
 287 #endif
 288                }
 289             }
 290          }
 291       }
 292
 293 #if PRINT_MATRICES
 294       print_matrix(M, B, n_stn_tab * FACTOR); /* 'ave a look! */
 295 #endif
 296
 297 #ifdef SOR
 298       /* defined in network.c, may be altered by -z<letters> on command line */
 299       if (optimize & BITA('i'))
 300          sor(M, B, n_stn_tab * FACTOR);
 301       else
 302 #endif
 303          choleski(M, B, n_stn_tab * FACTOR);
 304
 305       {
 306          int m;
 307          for (m = (int)(n_stn_tab - 1); m >= 0; m--) {
 308 #ifdef NO_COVARIANCES
 309             stn_tab[m]->p[dim] = B[m];
 310             if (dim == 0) {
 311                SVX_ASSERT2(pos_fixed(stn_tab[m]),
 312                        "setting station coordinates didn't mark pos as fixed");
 313             }
 314 #else
 315             int i;
 316             for (i = 0; i < 3; i++) {
 317                stn_tab[m]->p[i] = B[m * FACTOR + i];
 318             }
 319             SVX_ASSERT2(pos_fixed(stn_tab[m]),
 320                     "setting station coordinates didn't mark pos as fixed");
 321 #endif
 322          }
 323 #if EXPLICIT_FIXED_FLAG
 324          for (m = n_stn_tab - 1; m >= 0; m--) fixpos(stn_tab[m]);
 325 #endif
 326       }
 327    }
 328    osfree(B);
 329    osfree(M);
 330 }
 331
 332 static int
 333 find_stn_in_tab(node *stn)
 334 {
 335    int i = 0;
 336    pos *p = stn->name->pos;
 337    while (stn_tab[i] != p)
 338       if (++i == n_stn_tab) {
 339 #if DEBUG_INVALID
 340          fputs("Station ", stderr);
 341          fprint_prefix(stderr, stn->name);
 342          fputs(" not in table\n\n", stderr);
 343 #endif
 344 #if 0
 345          print_prefix(stn->name);
 346          printf(" used: %d colour %d\n",
 347                 (!!stn->leg[2])<<2 | (!!stn->leg[1])<<1 | (!!stn->leg[0]),
 348                 stn->colour);
 349 #endif
 350          fatalerror(/*Bug in program detected! Please report this to the authors*/11);
 351       }
 352    return i;
 353 }
 354
 355 static int
 356 add_stn_to_tab(node *stn)
 357 {
 358    int i;
 359    pos *p = stn->name->pos;
 360    for (i = 0; i < n_stn_tab; i++) {
 361       if (stn_tab[i] == p) return i;
 362    }
 363    stn_tab[n_stn_tab++] = p;
 364    return i;
 365 }
 366
 367 /* Solve MX=B for X by Choleski factorisation - modified Choleski actually
 368  * since we factor into LDL' while Choleski is just LL'
 369  */
 370 /* Note M must be symmetric positive definite */
 371 /* routine is entitled to scribble on M and B if it wishes */
 372 static void
 373 choleski(real *M, real *B, long n)
 374 {
 375    int i, j, k;
 376
 377    for (j = 1; j < n; j++) {
 378       real V;
 379       for (i = 0; i < j; i++) {
 380          V = (real)0.0;
 381          for (k = 0; k < i; k++) V += M(i,k) * M(j,k) * M(k,k);
 382          M(j,i) = (M(j,i) - V) / M(i,i);
 383       }
 384       V = (real)0.0;
 385       for (k = 0; k < j; k++) V += M(j,k) * M(j,k) * M(k,k);
 386       M(j,j) -= V; /* may be best to add M() last for numerical reasons too */
 387    }
 388
 389    /* Multiply x by L inverse */
 390    for (i = 0; i < n - 1; i++) {
 391       for (j = i + 1; j < n; j++) {
 392          B[j] -= M(j,i) * B[i];
 393       }
 394    }
 395
 396    /* Multiply x by D inverse */
 397    for (i = 0; i < n; i++) {
 398       B[i] /= M(i,i);
 399    }
 400
 401    /* Multiply x by (L transpose) inverse */
 402    for (i = (int)(n - 1); i > 0; i--) {
 403       for (j = i - 1; j >= 0; j--) {
 404          B[j] -= M(i,j) * B[i];
 405       }
 406    }
 407
 408    /* printf("\n%ld/%ld\n\n",flops,flopsTot); */
 409 }
 410
 411 #ifdef SOR
 412 /* factor to use for SOR (must have 1 <= SOR_factor < 2) */
 413 #define SOR_factor 1.93 /* 1.95 */
 414
 415 /* Solve MX=B for X by SOR of Gauss-Siedel */
 416 /* routine is entitled to scribble on M and B if it wishes */
 417 static void
 418 sor(real *M, real *B, long n)
 419 {
 420    real t, x, delta, threshold, t2;
 421    int row, col;
 422    real *X;
 423    long it = 0;
 424
 425    X = osmalloc(n * ossizeof(real));
 426
 427    threshold = 0.00001;
 428
 429    printf("reciprocating diagonal\n"); /* TRANSLATE */
 430
 431    /* munge diagonal so we can multiply rather than divide */
 432    for (row = n - 1; row >= 0; row--) {
 433       M(row,row) = 1 / M(row,row);
 434       X[row] = 0;
 435    }
 436
 437    printf("starting iteration\n"); /* TRANSLATE */
 438
 439    do {
 440       /*printf("*");*/
 441       it++;
 442       t = 0.0;
 443       for (row = 0; row < n; row++) {
 444          x = B[row];
 445          for (col = 0; col < row; col++) x -= M(row,col) * X[col];
 446          for (col++; col < n; col++) x -= M(col,row) * X[col];
 447          x *= M(row,row);
 448          delta = (x - X[row]) * SOR_factor;
 449          X[row] += delta;
 450          t2 = fabs(delta);
 451          if (t2 > t) t = t2;
 452       }
 453       printf("% 6d: %8.6f\n", it, t);
 454    } while (t >= threshold && it < 100000);
 455
 456    if (t >= threshold) {
 457       fprintf(stderr, "*not* converged after %ld iterations\n", it);
 458       BUG("iteration stinks");
 459    }
 460
 461    printf("%ld iterations\n", it); /* TRANSLATE */
 462
 463 #if 0
 464    putnl();
 465    for (row = n - 1; row >= 0; row--) {
 466       t = 0.0;
 467       for (col = 0; col < row; col++) t += M(row, col) * X[col];
 468       t += X[row] / M(row, row);
 469       for (col = row + 1; col < n; col++)
 470          t += M(col, row) * X[col];
 471       printf("[ %f %f ]\n", t, B[row]);
 472    }
 473 #endif
 474
 475    for (row = n - 1; row >= 0; row--) B[row] = X[row];
 476
 477    osfree(X);
 478    printf("\ndone\n"); /* TRANSLATE */
 479 }
 480 #endif
 481
 482 #if PRINT_MATRICES
 483 static void
 484 print_matrix(real *M, real *B, long n)
 485 {
 486    long row, col;
 487    printf("Matrix, M and vector, B:\n");
 488    for (row = 0; row < n; row++) {
 489       for (col = 0; col <= row; col++) printf("%6.2f\t", M(row, col));
 490       for (; col <= n; col++) printf(" \t");
 491       printf("\t%6.2f\n", B[row]);
 492    }
 493    putnl();
 494    return;
 495 }
 496 #endif