source: git/src/matrix.c @ 5bb3dc4

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

Declare some variables at point of initialisation
  • Property mode set to 100644
File size: 12.7 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 int find_stn_in_tab(node *stn);
61static int add_stn_to_tab(node *stn);
62static void build_matrix(node *list);
63
64static long n_stn_tab;
65
66static pos **stn_tab;
67
68extern void
69solve_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
112static void
113build_matrix(node *list)
114{
115   if (n_stn_tab == 0) {
116      if (!fQuiet)
117         puts(msg(/*Network solved by reduction - no simultaneous equations to solve.*/74));
118      return;
119   }
120   /* (OSSIZE_T) cast may be needed if n_stn_tab>=181 */
121   real *M = osmalloc((OSSIZE_T)((((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1)) * ossizeof(real));
122   real *B = osmalloc((OSSIZE_T)(n_stn_tab * FACTOR * ossizeof(real)));
123
124   if (!fQuiet) {
125      if (n_stn_tab == 1)
126         out_current_action(msg(/*Solving one equation*/78));
127      else
128         out_current_action1(msg(/*Solving %d simultaneous equations*/75), n_stn_tab);
129   }
130
131#ifdef NO_COVARIANCES
132   int dim = 2;
133#else
134   int dim = 0; /* fudge next loop for now */
135#endif
136   for ( ; dim >= 0; dim--) {
137      node *stn;
138      int row;
139
140      /* Initialise M and B to zero - zeroing "linearly" will minimise
141       * paging when the matrix is large */
142      {
143         int end = n_stn_tab * FACTOR;
144         for (row = 0; row < end; row++) B[row] = (real)0.0;
145         end = ((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1;
146         for (row = 0; row < end; row++) M[row] = (real)0.0;
147      }
148
149      /* Construct matrix by going through the stn list.
150       *
151       * All legs between two fixed stations can be ignored here.
152       *
153       * Other legs we want to add exactly once to M.  To achieve this we
154       * wan to:
155       *
156       * - add forward legs between two unfixed stations,
157       *
158       * - add legs from unfixed stations to fixed stations (we do them from
159       *   the unfixed end so we don't need to detect when we're at a fixed
160       *   point cut line and determine which side we're currently dealing
161       *   with).
162       *
163       * To implement this, we only look at legs from unfixed stations and add
164       * a leg if to a fixed station, or to an unfixed station and it's a
165       * forward leg.
166       */
167      FOR_EACH_STN(stn, list) {
168#ifdef NO_COVARIANCES
169         real e;
170#else
171         svar e;
172         delta a;
173#endif
174#if DEBUG_MATRIX_BUILD
175         print_prefix(stn->name);
176         printf(" used: %d colour %ld\n",
177                (!!stn->leg[2]) << 2 | (!!stn -> leg[1]) << 1 | (!!stn->leg[0]),
178                stn->colour);
179
180         for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
181#ifdef NO_COVARIANCES
182            printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
183                   stn->leg[dirn]->v[0], stn->leg[dirn]->l.reverse);
184#else
185            printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
186                   stn->leg[dirn]->v[0][0], stn->leg[dirn]->l.reverse);
187#endif
188            print_prefix(stn->leg[dirn]->l.to->name);
189            putnl();
190         }
191         putnl();
192#endif /* DEBUG_MATRIX_BUILD */
193
194         if (!fixed(stn)) {
195            int f = find_stn_in_tab(stn);
196            for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
197               linkfor *leg = stn->leg[dirn];
198               node *to = leg->l.to;
199               if (fixed(to)) {
200                  bool fRev = !data_here(leg);
201                  if (fRev) leg = reverse_leg(leg);
202                  /* Ignore equated nodes */
203#ifdef NO_COVARIANCES
204                  e = leg->v[dim];
205                  if (e != (real)0.0) {
206                     e = ((real)1.0) / e;
207                     M(f,f) += e;
208                     B[f] += e * POS(to, dim);
209                     if (fRev) {
210                        B[f] += leg->d[dim];
211                     } else {
212                        B[f] -= leg->d[dim];
213                     }
214                  }
215#else
216                  if (invert_svar(&e, &leg->v)) {
217                     if (fRev) {
218                        adddd(&a, &POSD(to), &leg->d);
219                     } else {
220                        subdd(&a, &POSD(to), &leg->d);
221                     }
222                     delta b;
223                     mulsd(&b, &e, &a);
224                     for (int 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                  int 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                     e = ((real)1.0) / e;
245                     M(f,f) += e;
246                     M(t,t) += e;
247                     if (f < t) M(t,f) -= e; else M(f,t) -= e;
248                     real a = e * leg->d[dim];
249                     B[f] -= a;
250                     B[t] += a;
251                  }
252#else
253                  if (t != f && invert_svar(&e, &leg->v)) {
254                     mulsd(&a, &e, &leg->d);
255                     for (int 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         for (int m = (int)(n_stn_tab - 1); m >= 0; m--) {
307#ifdef NO_COVARIANCES
308            stn_tab[m]->p[dim] = B[m];
309            if (dim == 0) {
310               SVX_ASSERT2(pos_fixed(stn_tab[m]),
311                       "setting station coordinates didn't mark pos as fixed");
312            }
313#else
314            for (int i = 0; i < 3; i++) {
315               stn_tab[m]->p[i] = B[m * FACTOR + i];
316            }
317            SVX_ASSERT2(pos_fixed(stn_tab[m]),
318                    "setting station coordinates didn't mark pos as fixed");
319#endif
320         }
321#if EXPLICIT_FIXED_FLAG
322         for (int m = n_stn_tab - 1; m >= 0; m--) fixpos(stn_tab[m]);
323#endif
324      }
325   }
326   osfree(B);
327   osfree(M);
328}
329
330static int
331find_stn_in_tab(node *stn)
332{
333   int i = 0;
334   pos *p = stn->name->pos;
335   while (stn_tab[i] != p)
336      if (++i == n_stn_tab) {
337#if DEBUG_INVALID
338         fputs("Station ", stderr);
339         fprint_prefix(stderr, stn->name);
340         fputs(" not in table\n\n", stderr);
341#endif
342#if 0
343         print_prefix(stn->name);
344         printf(" used: %d colour %d\n",
345                (!!stn->leg[2])<<2 | (!!stn->leg[1])<<1 | (!!stn->leg[0]),
346                stn->colour);
347#endif
348         fatalerror(/*Bug in program detected! Please report this to the authors*/11);
349      }
350   return i;
351}
352
353static int
354add_stn_to_tab(node *stn)
355{
356   int i;
357   pos *p = stn->name->pos;
358   for (i = 0; i < n_stn_tab; i++) {
359      if (stn_tab[i] == p) return i;
360   }
361   stn_tab[n_stn_tab++] = p;
362   return i;
363}
364
365/* Solve MX=B for X by Choleski factorisation - modified Choleski actually
366 * since we factor into LDL' while Choleski is just LL'
367 */
368/* Note M must be symmetric positive definite */
369/* routine is entitled to scribble on M and B if it wishes */
370static void
371choleski(real *M, real *B, long n)
372{
373   for (int j = 1; j < n; j++) {
374      real V;
375      for (int i = 0; i < j; i++) {
376         V = (real)0.0;
377         for (int k = 0; k < i; k++) V += M(i,k) * M(j,k) * M(k,k);
378         M(j,i) = (M(j,i) - V) / M(i,i);
379      }
380      V = (real)0.0;
381      for (int k = 0; k < j; k++) V += M(j,k) * M(j,k) * M(k,k);
382      M(j,j) -= V; /* may be best to add M() last for numerical reasons too */
383   }
384
385   /* Multiply x by L inverse */
386   for (int i = 0; i < n - 1; i++) {
387      for (int j = i + 1; j < n; j++) {
388         B[j] -= M(j,i) * B[i];
389      }
390   }
391
392   /* Multiply x by D inverse */
393   for (int i = 0; i < n; i++) {
394      B[i] /= M(i,i);
395   }
396
397   /* Multiply x by (L transpose) inverse */
398   for (int i = (int)(n - 1); i > 0; i--) {
399      for (int j = i - 1; j >= 0; j--) {
400         B[j] -= M(i,j) * B[i];
401      }
402   }
403
404   /* printf("\n%ld/%ld\n\n",flops,flopsTot); */
405}
406
407#ifdef SOR
408/* factor to use for SOR (must have 1 <= SOR_factor < 2) */
409#define SOR_factor 1.93 /* 1.95 */
410
411/* Solve MX=B for X by SOR of Gauss-Siedel */
412/* routine is entitled to scribble on M and B if it wishes */
413static void
414sor(real *M, real *B, long n)
415{
416   long it = 0;
417
418   real *X = osmalloc(n * ossizeof(real));
419
420   const real threshold = 0.00001;
421
422   printf("reciprocating diagonal\n"); /* TRANSLATE */
423
424   /* munge diagonal so we can multiply rather than divide */
425   for (int row = n - 1; row >= 0; row--) {
426      M(row,row) = 1 / M(row,row);
427      X[row] = 0;
428   }
429
430   printf("starting iteration\n"); /* TRANSLATE */
431
432   real t;
433   do {
434      /*printf("*");*/
435      it++;
436      t = 0.0;
437      for (int row = 0; row < n; row++) {
438         real x = B[row];
439         int col;
440         for (col = 0; col < row; col++) x -= M(row,col) * X[col];
441         for (col++; col < n; col++) x -= M(col,row) * X[col];
442         x *= M(row,row);
443         real delta = (x - X[row]) * SOR_factor;
444         X[row] += delta;
445         real t2 = fabs(delta);
446         if (t2 > t) t = t2;
447      }
448      printf("% 6d: %8.6f\n", it, t);
449   } while (t >= threshold && it < 100000);
450
451   if (t >= threshold) {
452      fprintf(stderr, "*not* converged after %ld iterations\n", it);
453      BUG("iteration stinks");
454   }
455
456   printf("%ld iterations\n", it); /* TRANSLATE */
457
458#if 0
459   putnl();
460   for (int row = n - 1; row >= 0; row--) {
461      t = 0.0;
462      for (int col = 0; col < row; col++) t += M(row, col) * X[col];
463      t += X[row] / M(row, row);
464      for (col = row + 1; col < n; col++)
465         t += M(col, row) * X[col];
466      printf("[ %f %f ]\n", t, B[row]);
467   }
468#endif
469
470   for (int row = n - 1; row >= 0; row--) B[row] = X[row];
471
472   osfree(X);
473   printf("\ndone\n"); /* TRANSLATE */
474}
475#endif
476
477#if PRINT_MATRICES
478static void
479print_matrix(real *M, real *B, long n)
480{
481   printf("Matrix, M and vector, B:\n");
482   for (long row = 0; row < n; row++) {
483      long col;
484      for (col = 0; col <= row; col++) printf("%6.2f\t", M(row, col));
485      for (; col <= n; col++) printf(" \t");
486      printf("\t%6.2f\n", B[row]);
487   }
488   putnl();
489   return;
490}
491#endif
Note: See TracBrowser for help on using the repository browser.