source: git/src/matrix.c @ 103c026

stereo-2025
Last change on this file since 103c026 was 103c026, checked in by Olly Betts <olly@…>, 12 months ago

Store stn_tab index in stn->colour for matrix build

This avoids a linear search for every station to find the index
while building the matrix (though this doesn't change doesn't
improve the O() behaviour of the matrix build).
  • Property mode set to 100644
File size: 12.2 KB
Line 
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
46static void print_matrix(real *M, real *B, long n);
47#endif
48
49static void choleski(real *M, real *B, long n);
50
51#ifdef SOR
52static 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
60static void build_matrix(node *list);
61
62static long n_stn_tab;
63
64static pos **stn_tab;
65
66extern void
67solve_matrix(node *list)
68{
69   node *stn;
70   long n = 0;
71   FOR_EACH_STN(stn, list) {
72      if (!fixed(stn))
73          n++;
74   }
75   if (n == 0) return;
76
77   /* We need to allocate stn_tab with one entry per unfixed cluster of equated
78    * stations, but it's much simpler to count the number of unfixed nodes.
79    * This will over-count stations with more than 3 legs and equated stations
80    * but in a typical survey that's a small minority of stations.
81    */
82   stn_tab = osmalloc((OSSIZE_T)(n * ossizeof(pos*)));
83   n_stn_tab = 0;
84
85   /* We store the stn_tab index in stn->colour for quick and easy lookup in
86    * build_matrix().
87    */
88   FOR_EACH_STN(stn, list) {
89      if (!fixed(stn)) {
90          int i;
91          pos *p = stn->name->pos;
92          for (i = 0; i < n_stn_tab; i++) {
93              if (stn_tab[i] == p)
94                  break;
95          }
96          if (i == n_stn_tab)
97              stn_tab[n_stn_tab++] = p;
98          stn->colour = i;
99      } else {
100          stn->colour = -1;
101      }
102   }
103
104   build_matrix(list);
105#if DEBUG_MATRIX
106   FOR_EACH_STN(stn, list) {
107      printf("(%8.2f, %8.2f, %8.2f ) ", POS(stn, 0), POS(stn, 1), POS(stn, 2));
108      print_prefix(stn->name);
109      putnl();
110   }
111#endif
112
113   osfree(stn_tab);
114}
115
116#ifdef NO_COVARIANCES
117# define FACTOR 1
118#else
119# define FACTOR 3
120#endif
121
122static void
123build_matrix(node *list)
124{
125   if (n_stn_tab == 0) {
126      if (!fQuiet)
127         puts(msg(/*Network solved by reduction - no simultaneous equations to solve.*/74));
128      return;
129   }
130   /* (OSSIZE_T) cast may be needed if n_stn_tab>=181 */
131   real *M = osmalloc((OSSIZE_T)((((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1)) * ossizeof(real));
132   real *B = osmalloc((OSSIZE_T)(n_stn_tab * FACTOR * ossizeof(real)));
133
134   if (!fQuiet) {
135      if (n_stn_tab == 1)
136         out_current_action(msg(/*Solving one equation*/78));
137      else
138         out_current_action1(msg(/*Solving %d simultaneous equations*/75), n_stn_tab);
139   }
140
141#ifdef NO_COVARIANCES
142   int dim = 2;
143#else
144   int dim = 0; /* fudge next loop for now */
145#endif
146   for ( ; dim >= 0; dim--) {
147      node *stn;
148      int row;
149
150      /* Initialise M and B to zero - zeroing "linearly" will minimise
151       * paging when the matrix is large */
152      {
153         int end = n_stn_tab * FACTOR;
154         for (row = 0; row < end; row++) B[row] = (real)0.0;
155         end = ((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1;
156         for (row = 0; row < end; row++) M[row] = (real)0.0;
157      }
158
159      /* Construct matrix by going through the stn list.
160       *
161       * All legs between two fixed stations can be ignored here.
162       *
163       * Other legs we want to add exactly once to M.  To achieve this we
164       * wan to:
165       *
166       * - add forward legs between two unfixed stations,
167       *
168       * - add legs from unfixed stations to fixed stations (we do them from
169       *   the unfixed end so we don't need to detect when we're at a fixed
170       *   point cut line and determine which side we're currently dealing
171       *   with).
172       *
173       * To implement this, we only look at legs from unfixed stations and add
174       * a leg if to a fixed station, or to an unfixed station and it's a
175       * forward leg.
176       */
177      FOR_EACH_STN(stn, list) {
178#ifdef NO_COVARIANCES
179         real e;
180#else
181         svar e;
182         delta a;
183#endif
184#if DEBUG_MATRIX_BUILD
185         print_prefix(stn->name);
186         printf(" used: %d colour %ld\n",
187                (!!stn->leg[2]) << 2 | (!!stn -> leg[1]) << 1 | (!!stn->leg[0]),
188                stn->colour);
189
190         for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
191#ifdef NO_COVARIANCES
192            printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
193                   stn->leg[dirn]->v[0], stn->leg[dirn]->l.reverse);
194#else
195            printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
196                   stn->leg[dirn]->v[0][0], stn->leg[dirn]->l.reverse);
197#endif
198            print_prefix(stn->leg[dirn]->l.to->name);
199            putnl();
200         }
201         putnl();
202#endif /* DEBUG_MATRIX_BUILD */
203
204         if (!fixed(stn)) {
205            int f = stn->colour;
206            for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
207               linkfor *leg = stn->leg[dirn];
208               node *to = leg->l.to;
209               if (fixed(to)) {
210                  bool fRev = !data_here(leg);
211                  if (fRev) leg = reverse_leg(leg);
212                  /* Ignore equated nodes */
213#ifdef NO_COVARIANCES
214                  e = leg->v[dim];
215                  if (e != (real)0.0) {
216                     e = ((real)1.0) / e;
217                     M(f,f) += e;
218                     B[f] += e * POS(to, dim);
219                     if (fRev) {
220                        B[f] += leg->d[dim];
221                     } else {
222                        B[f] -= leg->d[dim];
223                     }
224                  }
225#else
226                  if (invert_svar(&e, &leg->v)) {
227                     if (fRev) {
228                        adddd(&a, &POSD(to), &leg->d);
229                     } else {
230                        subdd(&a, &POSD(to), &leg->d);
231                     }
232                     delta b;
233                     mulsd(&b, &e, &a);
234                     for (int i = 0; i < 3; i++) {
235                        M(f * FACTOR + i, f * FACTOR + i) += e[i];
236                        B[f * FACTOR + i] += b[i];
237                     }
238                     M(f * FACTOR + 1, f * FACTOR) += e[3];
239                     M(f * FACTOR + 2, f * FACTOR) += e[4];
240                     M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
241                  }
242#endif
243               } else if (data_here(leg)) {
244                  /* forward leg, unfixed -> unfixed */
245                  int t = to->colour;
246#if DEBUG_MATRIX
247                  printf("Leg %d to %d, var %f, delta %f\n", f, t, e,
248                         leg->d[dim]);
249#endif
250                  /* Ignore equated nodes & lollipops */
251#ifdef NO_COVARIANCES
252                  e = leg->v[dim];
253                  if (t != f && e != (real)0.0) {
254                     e = ((real)1.0) / e;
255                     M(f,f) += e;
256                     M(t,t) += e;
257                     if (f < t) M(t,f) -= e; else M(f,t) -= e;
258                     real a = e * leg->d[dim];
259                     B[f] -= a;
260                     B[t] += a;
261                  }
262#else
263                  if (t != f && invert_svar(&e, &leg->v)) {
264                     mulsd(&a, &e, &leg->d);
265                     for (int i = 0; i < 3; i++) {
266                        M(f * FACTOR + i, f * FACTOR + i) += e[i];
267                        M(t * FACTOR + i, t * FACTOR + i) += e[i];
268                        if (f < t)
269                           M(t * FACTOR + i, f * FACTOR + i) -= e[i];
270                        else
271                           M(f * FACTOR + i, t * FACTOR + i) -= e[i];
272                        B[f * FACTOR + i] -= a[i];
273                        B[t * FACTOR + i] += a[i];
274                     }
275                     M(f * FACTOR + 1, f * FACTOR) += e[3];
276                     M(t * FACTOR + 1, t * FACTOR) += e[3];
277                     M(f * FACTOR + 2, f * FACTOR) += e[4];
278                     M(t * FACTOR + 2, t * FACTOR) += e[4];
279                     M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
280                     M(t * FACTOR + 2, t * FACTOR + 1) += e[5];
281                     if (f < t) {
282                        M(t * FACTOR + 1, f * FACTOR) -= e[3];
283                        M(t * FACTOR, f * FACTOR + 1) -= e[3];
284                        M(t * FACTOR + 2, f * FACTOR) -= e[4];
285                        M(t * FACTOR, f * FACTOR + 2) -= e[4];
286                        M(t * FACTOR + 2, f * FACTOR + 1) -= e[5];
287                        M(t * FACTOR + 1, f * FACTOR + 2) -= e[5];
288                     } else {
289                        M(f * FACTOR + 1, t * FACTOR) -= e[3];
290                        M(f * FACTOR, t * FACTOR + 1) -= e[3];
291                        M(f * FACTOR + 2, t * FACTOR) -= e[4];
292                        M(f * FACTOR, t * FACTOR + 2) -= e[4];
293                        M(f * FACTOR + 2, t * FACTOR + 1) -= e[5];
294                        M(f * FACTOR + 1, t * FACTOR + 2) -= e[5];
295                     }
296                  }
297#endif
298               }
299            }
300         }
301      }
302
303#if PRINT_MATRICES
304      print_matrix(M, B, n_stn_tab * FACTOR); /* 'ave a look! */
305#endif
306
307#ifdef SOR
308      /* defined in network.c, may be altered by -z<letters> on command line */
309      if (optimize & BITA('i'))
310         sor(M, B, n_stn_tab * FACTOR);
311      else
312#endif
313         choleski(M, B, n_stn_tab * FACTOR);
314
315      {
316         for (int 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            for (int i = 0; i < 3; i++) {
325               stn_tab[m]->p[i] = B[m * FACTOR + i];
326            }
327            SVX_ASSERT2(pos_fixed(stn_tab[m]),
328                    "setting station coordinates didn't mark pos as fixed");
329#endif
330         }
331#if EXPLICIT_FIXED_FLAG
332         for (int m = n_stn_tab - 1; m >= 0; m--) fixpos(stn_tab[m]);
333#endif
334      }
335   }
336   osfree(B);
337   osfree(M);
338}
339
340/* Solve MX=B for X by Choleski factorisation - modified Choleski actually
341 * since we factor into LDL' while Choleski is just LL'
342 */
343/* Note M must be symmetric positive definite */
344/* routine is entitled to scribble on M and B if it wishes */
345static void
346choleski(real *M, real *B, long n)
347{
348   for (int j = 1; j < n; j++) {
349      real V;
350      for (int i = 0; i < j; i++) {
351         V = (real)0.0;
352         for (int k = 0; k < i; k++) V += M(i,k) * M(j,k) * M(k,k);
353         M(j,i) = (M(j,i) - V) / M(i,i);
354      }
355      V = (real)0.0;
356      for (int k = 0; k < j; k++) V += M(j,k) * M(j,k) * M(k,k);
357      M(j,j) -= V; /* may be best to add M() last for numerical reasons too */
358   }
359
360   /* Multiply x by L inverse */
361   for (int i = 0; i < n - 1; i++) {
362      for (int j = i + 1; j < n; j++) {
363         B[j] -= M(j,i) * B[i];
364      }
365   }
366
367   /* Multiply x by D inverse */
368   for (int i = 0; i < n; i++) {
369      B[i] /= M(i,i);
370   }
371
372   /* Multiply x by (L transpose) inverse */
373   for (int i = (int)(n - 1); i > 0; i--) {
374      for (int j = i - 1; j >= 0; j--) {
375         B[j] -= M(i,j) * B[i];
376      }
377   }
378
379   /* printf("\n%ld/%ld\n\n",flops,flopsTot); */
380}
381
382#ifdef SOR
383/* factor to use for SOR (must have 1 <= SOR_factor < 2) */
384#define SOR_factor 1.93 /* 1.95 */
385
386/* Solve MX=B for X by SOR of Gauss-Siedel */
387/* routine is entitled to scribble on M and B if it wishes */
388static void
389sor(real *M, real *B, long n)
390{
391   long it = 0;
392
393   real *X = osmalloc(n * ossizeof(real));
394
395   const real threshold = 0.00001;
396
397   printf("reciprocating diagonal\n"); /* TRANSLATE */
398
399   /* munge diagonal so we can multiply rather than divide */
400   for (int row = n - 1; row >= 0; row--) {
401      M(row,row) = 1 / M(row,row);
402      X[row] = 0;
403   }
404
405   printf("starting iteration\n"); /* TRANSLATE */
406
407   real t;
408   do {
409      /*printf("*");*/
410      it++;
411      t = 0.0;
412      for (int row = 0; row < n; row++) {
413         real x = B[row];
414         int col;
415         for (col = 0; col < row; col++) x -= M(row,col) * X[col];
416         for (col++; col < n; col++) x -= M(col,row) * X[col];
417         x *= M(row,row);
418         real delta = (x - X[row]) * SOR_factor;
419         X[row] += delta;
420         real t2 = fabs(delta);
421         if (t2 > t) t = t2;
422      }
423      printf("% 6d: %8.6f\n", it, t);
424   } while (t >= threshold && it < 100000);
425
426   if (t >= threshold) {
427      fprintf(stderr, "*not* converged after %ld iterations\n", it);
428      BUG("iteration stinks");
429   }
430
431   printf("%ld iterations\n", it); /* TRANSLATE */
432
433#if 0
434   putnl();
435   for (int row = n - 1; row >= 0; row--) {
436      t = 0.0;
437      for (int col = 0; col < row; col++) t += M(row, col) * X[col];
438      t += X[row] / M(row, row);
439      for (col = row + 1; col < n; col++)
440         t += M(col, row) * X[col];
441      printf("[ %f %f ]\n", t, B[row]);
442   }
443#endif
444
445   for (int row = n - 1; row >= 0; row--) B[row] = X[row];
446
447   osfree(X);
448   printf("\ndone\n"); /* TRANSLATE */
449}
450#endif
451
452#if PRINT_MATRICES
453static void
454print_matrix(real *M, real *B, long n)
455{
456   printf("Matrix, M and vector, B:\n");
457   for (long row = 0; row < n; row++) {
458      long col;
459      for (col = 0; col <= row; col++) printf("%6.2f\t", M(row, col));
460      for (; col <= n; col++) printf(" \t");
461      printf("\t%6.2f\n", B[row]);
462   }
463   putnl();
464   return;
465}
466#endif
Note: See TracBrowser for help on using the repository browser.