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