source: git/src/matrix.c @ 9965b2b

RELEASE/1.2debug-cidebug-ci-sanitisersstereowalls-data
Last change on this file since 9965b2b was 9965b2b, checked in by Olly Betts <olly@…>, 14 years ago

src/: Strip out "Far" and "Huge" which we no longer need now that
DOS memory models are no longer relevant.

git-svn-id: ​file:///home/survex-svn/survex/trunk@3503 4b37db11-9a0c-4f06-9ece-9ab7cdaee568
  • Property mode set to 100644
File size: 13.5 KB
Line 
1/* matrix.c
2 * Matrix building and solving routines
3 * Copyright (C) 1993-2003,2010 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
48static void print_matrix(real *M, real *B, long n);
49#endif
50
51static void choleski(real *M, real *B, long n);
52
53#ifdef SOR
54static 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
62static int find_stn_in_tab(node *stn);
63static int add_stn_to_tab(node *stn);
64static void build_matrix(node *list);
65
66static long n_stn_tab;
67
68static pos **stn_tab;
69
70extern void
71solve_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
114static void
115build_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     putnl();
132     if (n_stn_tab == 1)
133       puts(msg(/*Solving one equation*/78));
134     else {
135       printf(msg(/*Solving %d simultaneous equations*/75), n_stn_tab);
136       putnl();
137     }
138   }
139
140#ifdef NO_COVARIANCES
141   dim = 2;
142#else
143   dim = 0; /* fudge next loop for now */
144#endif
145   for ( ; dim >= 0; dim--) {
146      char buf[256];
147      node *stn;
148      int row;
149
150      sprintf(buf, msg(/*Solving to find %c coordinates*/76), (char)('x'+dim));
151      out_current_action(buf);
152
153      /* Initialise M and B to zero - zeroing "linearly" will minimise
154       * paging when the matrix is large */
155      {
156         int end = n_stn_tab * FACTOR;
157         for (row = 0; row < end; row++) B[row] = (real)0.0;
158         end = ((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1;
159         for (row = 0; row < end; row++) M[row] = (real)0.0;
160      }
161
162      /* Construct matrix - Go thru' stn list & add all forward legs between
163       * two unfixed stations to M (so each leg goes on exactly once).
164       *
165       * All legs between two fixed stations can be ignored here.
166       *
167       * All legs between a fixed and an unfixed station are then considered
168       * from the unfixed end (if we consider them from the fixed end we'd
169       * need to somehow detect when we're at a fixed point cut line and work
170       * out which side we're dealing with at this time. */
171      FOR_EACH_STN(stn, list) {
172#ifdef NO_COVARIANCES
173         real e;
174#else
175         svar e;
176         delta a;
177#endif
178         int f, t;
179         int dirn;
180#if DEBUG_MATRIX_BUILD
181         print_prefix(stn->name);
182         printf(" used: %d colour %ld\n",
183                (!!stn->leg[2]) << 2 | (!!stn -> leg[1]) << 1 | (!!stn->leg[0]),
184                stn->colour);
185
186         for (dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
187#ifdef NO_COVARIANCES
188            printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
189                   stn->leg[dirn]->v[0], stn->leg[dirn]->l.reverse);
190#else
191            printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
192                   stn->leg[dirn]->v[0][0], stn->leg[dirn]->l.reverse);
193#endif
194            print_prefix(stn->leg[dirn]->l.to->name);
195            putnl();
196         }
197         putnl();
198#endif /* DEBUG_MATRIX_BUILD */
199
200         if (!fixed(stn)) {
201            f = find_stn_in_tab(stn);
202            for (dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
203               linkfor *leg = stn->leg[dirn];
204               node *to = leg->l.to;
205               if (fixed(to)) {
206                  bool fRev = !data_here(leg);
207                  if (fRev) leg = reverse_leg(leg);
208                  /* Ignore equated nodes */
209#ifdef NO_COVARIANCES
210                  e = leg->v[dim];
211                  if (e != (real)0.0) {
212                     e = ((real)1.0) / e;
213                     M(f,f) += e;
214                     B[f] += e * POS(to, dim);
215                     if (fRev) {
216                        B[f] += leg->d[dim];
217                     } else {
218                        B[f] -= leg->d[dim];
219                     }
220                  }
221#else
222                  if (invert_svar(&e, &leg->v)) {
223                     delta b;
224                     int i;
225                     if (fRev) {
226                        adddd(&a, &POSD(to), &leg->d);
227                     } else {
228                        subdd(&a, &POSD(to), &leg->d);
229                     }
230                     mulsd(&b, &e, &a);
231                     for (i = 0; i < 3; i++) {
232                        M(f * FACTOR + i, f * FACTOR + i) += e[i];
233                        B[f * FACTOR + i] += b[i];
234                     }
235                     M(f * FACTOR + 1, f * FACTOR) += e[3];
236                     M(f * FACTOR + 2, f * FACTOR) += e[4];
237                     M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
238                  }
239#endif
240               } else if (data_here(leg)) {
241                  /* forward leg, unfixed -> unfixed */
242                  t = find_stn_in_tab(to);
243#if DEBUG_MATRIX
244                  printf("Leg %d to %d, var %f, delta %f\n", f, t, e,
245                         leg->d[dim]);
246#endif
247                  /* Ignore equated nodes & lollipops */
248#ifdef NO_COVARIANCES
249                  e = leg->v[dim];
250                  if (t != f && e != (real)0.0) {
251                     real a;
252                     e = ((real)1.0) / e;
253                     M(f,f) += e;
254                     M(t,t) += e;
255                     if (f < t) M(t,f) -= e; else M(f,t) -= e;
256                     a = e * leg->d[dim];
257                     B[f] -= a;
258                     B[t] += a;
259                  }
260#else
261                  if (t != f && invert_svar(&e, &leg->v)) {
262                     int i;
263                     mulsd(&a, &e, &leg->d);
264                     for (i = 0; i < 3; i++) {
265                        M(f * FACTOR + i, f * FACTOR + i) += e[i];
266                        M(t * FACTOR + i, t * FACTOR + i) += e[i];
267                        if (f < t)
268                           M(t * FACTOR + i, f * FACTOR + i) -= e[i];
269                        else
270                           M(f * FACTOR + i, t * FACTOR + i) -= e[i];
271                        B[f * FACTOR + i] -= a[i];
272                        B[t * FACTOR + i] += a[i];
273                     }
274                     M(f * FACTOR + 1, f * FACTOR) += e[3];
275                     M(t * FACTOR + 1, t * FACTOR) += e[3];
276                     M(f * FACTOR + 2, f * FACTOR) += e[4];
277                     M(t * FACTOR + 2, t * FACTOR) += e[4];
278                     M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
279                     M(t * FACTOR + 2, t * FACTOR + 1) += e[5];
280                     if (f < t) {
281                        M(t * FACTOR + 1, f * FACTOR) -= e[3];
282                        M(t * FACTOR, f * FACTOR + 1) -= e[3];
283                        M(t * FACTOR + 2, f * FACTOR) -= e[4];
284                        M(t * FACTOR, f * FACTOR + 2) -= e[4];
285                        M(t * FACTOR + 2, f * FACTOR + 1) -= e[5];
286                        M(t * FACTOR + 1, f * FACTOR + 2) -= e[5];
287                     } else {
288                        M(f * FACTOR + 1, t * FACTOR) -= e[3];
289                        M(f * FACTOR, t * FACTOR + 1) -= e[3];
290                        M(f * FACTOR + 2, t * FACTOR) -= e[4];
291                        M(f * FACTOR, t * FACTOR + 2) -= e[4];
292                        M(f * FACTOR + 2, t * FACTOR + 1) -= e[5];
293                        M(f * FACTOR + 1, t * FACTOR + 2) -= e[5];
294                     }
295                  }
296#endif
297               }
298            }
299         }
300      }
301
302#if PRINT_MATRICES
303      print_matrix(M, B, n_stn_tab * FACTOR); /* 'ave a look! */
304#endif
305
306#ifdef SOR
307      /* defined in network.c, may be altered by -z<letters> on command line */
308      if (optimize & BITA('i'))
309         sor(M, B, n_stn_tab * FACTOR);
310      else
311#endif
312         choleski(M, B, n_stn_tab * FACTOR);
313
314      {
315         int m;
316         for (m = (int)(n_stn_tab - 1); m >= 0; m--) {
317#ifdef NO_COVARIANCES
318            stn_tab[m]->p[dim] = B[m];
319            if (dim == 0) {
320               SVX_ASSERT2(pos_fixed(stn_tab[m]),
321                       "setting station coordinates didn't mark pos as fixed");
322            }
323#else
324            int i;
325            for (i = 0; i < 3; i++) {
326               stn_tab[m]->p[i] = B[m * FACTOR + i];
327            }
328            SVX_ASSERT2(pos_fixed(stn_tab[m]),
329                    "setting station coordinates didn't mark pos as fixed");
330#endif
331         }
332#if EXPLICIT_FIXED_FLAG
333         for (m = n_stn_tab - 1; m >= 0; m--) fixpos(stn_tab[m]);
334#endif
335      }
336   }
337   osfree(B);
338   osfree(M);
339}
340
341static int
342find_stn_in_tab(node *stn)
343{
344   int i = 0;
345   pos *p = stn->name->pos;
346   while (stn_tab[i] != p)
347      if (++i == n_stn_tab) {
348#if DEBUG_INVALID
349         fputs("Station ", stderr);
350         fprint_prefix(stderr, stn->name);
351         fputs(" not in table\n\n", stderr);
352#endif
353#if 0
354         print_prefix(stn->name);
355         printf(" used: %d colour %d\n",
356                (!!stn->leg[2])<<2 | (!!stn->leg[1])<<1 | (!!stn->leg[0]),
357                stn->colour);
358#endif
359         fatalerror(/*Bug in program detected! Please report this to the authors*/11);
360      }
361   return i;
362}
363
364static int
365add_stn_to_tab(node *stn)
366{
367   int i;
368   pos *p = stn->name->pos;
369   for (i = 0; i < n_stn_tab; i++) {
370      if (stn_tab[i] == p) return i;
371   }
372   stn_tab[n_stn_tab++] = p;
373   return i;
374}
375
376/* Solve MX=B for X by Choleski factorisation - modified Choleski actually
377 * since we factor into LDL' while Choleski is just LL'
378 */
379/* Note M must be symmetric positive definite */
380/* routine is entitled to scribble on M and B if it wishes */
381static void
382choleski(real *M, real *B, long n)
383{
384   int i, j, k;
385#ifndef NO_PERCENTAGE
386   unsigned long flopsTot, flops = 0, temp = 0;
387#define do_percent(N) BLK(flops += (N); printf("%d%%\r", (int)((100.0 * flops) / flopsTot));)
388
389   /* calc as double so we don't overflow a unsigned long with intermediate results */
390   flopsTot = (unsigned long)(n * (2.0 * n * n + 9.0 * n - 5.0) / 6.0);
391   /* 3*n*(n-1)/2 + n*(n-1)*(n-2)/3 + n*(n-1)/2 + n + n*(n-1)/2; */
392   /* n*(9*n-5 + 2*n*n )/6 ; */
393#endif
394
395   for (j = 1; j < n; j++) {
396      real V;
397      for (i = 0; i < j; i++) {
398         V = (real)0.0;
399         for (k = 0; k < i; k++) V += M(i,k) * M(j,k) * M(k,k);
400         M(j,i) = (M(j,i) - V) / M(i,i);
401      }
402      V = (real)0.0;
403      for (k = 0; k < j; k++) V += M(j,k) * M(j,k) * M(k,k);
404      M(j,j) -= V; /* may be best to add M() last for numerical reasons too */
405
406#ifndef NO_PERCENTAGE
407      if (fPercent) {
408         temp += ((unsigned long)j + j) + 1ul; /* avoid multiplies */
409         do_percent(temp);
410      }
411#endif
412   }
413
414   /* Multiply x by L inverse */
415   for (i = 0; i < n - 1; i++) {
416      for (j = i + 1; j < n; j++) {
417         B[j] -= M(j,i) * B[i];
418      }
419   }
420
421#ifndef NO_PERCENTAGE
422   if (fPercent) {
423      temp = (unsigned long)n * (n - 1ul) / 2ul; /* needed again lower down */
424      do_percent(temp);
425   }
426#endif
427
428   /* Multiply x by D inverse */
429   for (i = 0; i < n; i++) {
430      B[i] /= M(i,i);
431   }
432
433#ifndef NO_PERCENTAGE
434   if (fPercent) do_percent((unsigned long)n);
435#endif
436
437   /* Multiply x by (L transpose) inverse */
438   for (i = (int)(n - 1); i > 0; i--) {
439      for (j = i - 1; j >= 0; j--) {
440         B[j] -= M(i,j) * B[i];
441      }
442   }
443
444#ifndef NO_PERCENTAGE
445   if (fPercent) do_percent(temp);
446# undef do_percent
447#endif
448
449   /* printf("\n%ld/%ld\n\n",flops,flopsTot); */
450}
451
452#ifdef SOR
453/* factor to use for SOR (must have 1 <= SOR_factor < 2) */
454#define SOR_factor 1.93 /* 1.95 */
455
456/* Solve MX=B for X by SOR of Gauss-Siedel */
457/* routine is entitled to scribble on M and B if it wishes */
458static void
459sor(real *M, real *B, long n)
460{
461   real t, x, delta, threshold, t2;
462   int row, col;
463   real *X;
464   long it = 0;
465
466   X = osmalloc(n * ossizeof(real));
467
468   threshold = 0.00001;
469
470   printf("reciprocating diagonal\n"); /* TRANSLATE */
471
472   /* munge diagonal so we can multiply rather than divide */
473   for (row = n - 1; row >= 0; row--) {
474      M(row,row) = 1 / M(row,row);
475      X[row] = 0;
476   }
477
478   printf("starting iteration\n"); /* TRANSLATE */
479
480   do {
481      /*printf("*");*/
482      it++;
483      t = 0.0;
484      for (row = 0; row < n; row++) {
485         x = B[row];
486         for (col = 0; col < row; col++) x -= M(row,col) * X[col];
487         for (col++; col < n; col++) x -= M(col,row) * X[col];
488         x *= M(row,row);
489         delta = (x - X[row]) * SOR_factor;
490         X[row] += delta;
491         t2 = fabs(delta);
492         if (t2 > t) t = t2;
493      }
494      printf("% 6d: %8.6f\n", it, t);
495   } while (t >= threshold && it < 100000);
496
497   if (t >= threshold) {
498      fprintf(stderr, "*not* converged after %ld iterations\n", it);
499      BUG("iteration stinks");
500   }
501
502   printf("%ld iterations\n", it); /* TRANSLATE */
503
504#if 0
505   putnl();
506   for (row = n - 1; row >= 0; row--) {
507      t = 0.0;
508      for (col = 0; col < row; col++) t += M(row, col) * X[col];
509      t += X[row] / M(row, row);
510      for (col = row + 1; col < n; col++)
511         t += M(col, row) * X[col];
512      printf("[ %f %f ]\n", t, B[row]);
513   }
514#endif
515
516   for (row = n - 1; row >= 0; row--) B[row] = X[row];
517
518   osfree(X);
519   printf("\ndone\n"); /* TRANSLATE */
520}
521#endif
522
523#if PRINT_MATRICES
524static void
525print_matrix(real *M, real *B, long n)
526{
527   long row, col;
528   printf("Matrix, M and vector, B:\n");
529   for (row = 0; row < n; row++) {
530      for (col = 0; col <= row; col++) printf("%6.2f\t", M(row, col));
531      for (; col <= n; col++) printf(" \t");
532      printf("\t%6.2f\n", B[row]);
533   }
534   putnl();
535   return;
536}
537#endif
Note: See TracBrowser for help on using the repository browser.