source: git/src/matrix.c @ 16a78e0

stereo-2025
Last change on this file since 16a78e0 was 16a78e0, checked in by Olly Betts <olly@…>, 9 months ago

Fix to build with DEBUG_MATRIX set to 1
  • Property mode set to 100644
File size: 12.5 KB
Line 
1/* matrix.c
2 * Matrix building and solving routines
3 * Copyright (C) 1993-2003,2010,2013,2024 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
60#define COLOUR_FIXED -1
61#define COLOUR_TODO -2
62
63static void set_row(node *stn, node *from, int row_number) {
64    // We store the matrix row/column index in stn->colour for quick and easy
65    // lookup when copying out the solved station coordinates.
66    stn->colour = row_number;
67    for (int d = 0; d < 3; d++) {
68        linkfor *leg = stn->leg[d];
69        if (!leg) break;
70        node *to = leg->l.to;
71        if (to == from || to->colour != COLOUR_TODO) {
72            continue;
73        }
74        if (fZeros(data_here(leg) ? &leg->v : &reverse_leg(leg)->v)) {
75            set_row(to, stn, row_number);
76        }
77    }
78}
79
80#ifdef NO_COVARIANCES
81# define FACTOR 1
82#else
83# define FACTOR 3
84#endif
85
86extern void
87solve_matrix(node *list)
88{
89   node *stn;
90   bool unfixed_stations = false;
91   FOR_EACH_STN(stn, list) {
92      if (!fixed(stn)) {
93          unfixed_stations = true;
94          stn->colour = COLOUR_TODO;
95      } else {
96          stn->colour = COLOUR_FIXED;
97      }
98   }
99   if (!unfixed_stations) {
100       return;
101   }
102
103   // Assign a matrix row/column index to each group of stations with the same
104   // pos.
105   long n = 0;
106   FOR_EACH_STN(stn, list) {
107      if (stn->colour == COLOUR_TODO) {
108          set_row(stn, NULL, n++);
109      }
110   }
111   SVX_ASSERT(n > 0);
112
113   // Array to map from row/column index to pos.  We fill this in as we build
114   // the matrix, and use it to know where to copy the solved station
115   // coordinates to.
116   pos **stn_tab = osmalloc((OSSIZE_T)(n * ossizeof(pos*)));
117
118   /* (OSSIZE_T) cast may be needed if n >= 181 */
119   real *M = osmalloc((OSSIZE_T)((((OSSIZE_T)n * FACTOR * (n * FACTOR + 1)) >> 1)) * ossizeof(real));
120   real *B = osmalloc((OSSIZE_T)(n * FACTOR * ossizeof(real)));
121
122   if (!fQuiet) {
123      if (n == 1)
124         out_current_action(msg(/*Solving one equation*/78));
125      else
126         out_current_action1(msg(/*Solving %d simultaneous equations*/75), n);
127   }
128
129#ifdef NO_COVARIANCES
130   int dim = 2;
131#else
132   int dim = 0; /* Collapse loop to a single iteration. */
133#endif
134   for ( ; dim >= 0; dim--) {
135      /* Initialise M and B to zero - zeroing "linearly" will minimise
136       * paging when the matrix is large */
137      {
138         int end = n * FACTOR;
139         for (int row = 0; row < end; row++) B[row] = (real)0.0;
140         end = ((OSSIZE_T)n * FACTOR * (n * FACTOR + 1)) >> 1;
141         for (int row = 0; row < end; row++) M[row] = (real)0.0;
142      }
143
144      /* Construct matrix by going through the stn list.
145       *
146       * All legs between two fixed stations can be ignored here.
147       *
148       * Other legs we want to add exactly once to M.  To achieve this we
149       * want to:
150       *
151       * - add forward legs between two unfixed stations,
152       *
153       * - add legs from unfixed stations to fixed stations (we do them from
154       *   the unfixed end so we don't need to detect when we're at a fixed
155       *   point cut line and determine which side we're currently dealing
156       *   with).
157       *
158       * To implement this, we only look at legs from unfixed stations and add
159       * a leg if to a fixed station, or to an unfixed station and it's a
160       * forward leg.
161       */
162      FOR_EACH_STN(stn, list) {
163         if (dim == 0) {
164             if (stn->colour != COLOUR_FIXED) {
165                 stn_tab[stn->colour] = stn->name->pos;
166             }
167         }
168
169#ifdef NO_COVARIANCES
170         real e;
171#else
172         svar e;
173         delta a;
174#endif
175#if DEBUG_MATRIX_BUILD
176         print_prefix(stn->name);
177         printf(" used: %d colour %ld\n",
178                (!!stn->leg[2]) << 2 | (!!stn -> leg[1]) << 1 | (!!stn->leg[0]),
179                stn->colour);
180
181         for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
182            printf("Leg %d, vx=%f, reverse=%d, to ", dirn,
183                   stn->leg[dirn]->v[0], stn->leg[dirn]->l.reverse);
184            print_prefix(stn->leg[dirn]->l.to->name);
185            putnl();
186         }
187         putnl();
188#endif /* DEBUG_MATRIX_BUILD */
189
190         int f = stn->colour;
191         if (f != COLOUR_FIXED) {
192            for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
193               linkfor *leg = stn->leg[dirn];
194               node *to = leg->l.to;
195               int t = to->colour;
196               if (t == COLOUR_FIXED) {
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                     if (fRev) {
215                        adddd(&a, &POSD(to), &leg->d);
216                     } else {
217                        subdd(&a, &POSD(to), &leg->d);
218                     }
219                     delta b;
220                     mulsd(&b, &e, &a);
221                     for (int i = 0; i < 3; i++) {
222                        M(f * FACTOR + i, f * FACTOR + i) += e[i];
223                        B[f * FACTOR + i] += b[i];
224                     }
225                     M(f * FACTOR + 1, f * FACTOR) += e[3];
226                     M(f * FACTOR + 2, f * FACTOR) += e[4];
227                     M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
228                  }
229#endif
230               } else if (data_here(leg)) {
231                  /* forward leg, unfixed -> unfixed */
232#if DEBUG_MATRIX
233# ifdef NO_COVARIANCES
234                  printf("Leg %d to %d, var %f, delta %f\n", f, t, e,
235                         leg->d[dim]);
236# else
237                  printf("Leg %d to %d, var (%f, %f, %f; %f, %f, %f), "
238                         "delta %f\n", f, t, e[0], e[1], e[2], e[3], e[4], e[5],
239                         leg->d[dim]);
240# endif
241#endif
242                  /* Ignore equated nodes & lollipops */
243#ifdef NO_COVARIANCES
244                  e = leg->v[dim];
245                  if (t != f && e != (real)0.0) {
246                     e = ((real)1.0) / e;
247                     M(f,f) += e;
248                     M(t,t) += e;
249                     if (f < t) M(t,f) -= e; else M(f,t) -= e;
250                     real a = e * leg->d[dim];
251                     B[f] -= a;
252                     B[t] += a;
253                  }
254#else
255                  if (t != f && invert_svar(&e, &leg->v)) {
256                     mulsd(&a, &e, &leg->d);
257                     for (int 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 * 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 * FACTOR);
303      else
304#endif
305         choleski(M, B, n * FACTOR);
306
307      {
308         for (int m = (int)(n - 1); m >= 0; m--) {
309#ifdef NO_COVARIANCES
310            stn_tab[m]->p[dim] = B[m];
311# if !EXPLICIT_FIXED_FLAG
312            if (dim == 0) {
313               SVX_ASSERT2(pos_fixed(stn_tab[m]),
314                       "setting station coordinates didn't mark pos as fixed");
315            }
316# endif
317#else
318            for (int i = 0; i < 3; i++) {
319               stn_tab[m]->p[i] = B[m * FACTOR + i];
320            }
321# if !EXPLICIT_FIXED_FLAG
322            SVX_ASSERT2(pos_fixed(stn_tab[m]),
323                    "setting station coordinates didn't mark pos as fixed");
324# endif
325#endif
326#if EXPLICIT_FIXED_FLAG && !defined NO_COVARIANCES
327            fixpos(stn_tab[m]);
328#endif
329         }
330      }
331   }
332#if EXPLICIT_FIXED_FLAG && defined NO_COVARIANCES
333   for (int m = n - 1; m >= 0; m--) fixpos(stn_tab[m]);
334#endif
335   osfree(B);
336   osfree(M);
337   osfree(stn_tab);
338
339#if DEBUG_MATRIX
340   FOR_EACH_STN(stn, list) {
341      printf("(%8.2f, %8.2f, %8.2f ) ", POS(stn, 0), POS(stn, 1), POS(stn, 2));
342      print_prefix(stn->name);
343      putnl();
344   }
345#endif
346}
347
348/* Solve MX=B for X by Choleski factorisation - modified Choleski actually
349 * since we factor into LDL' while Choleski is just LL'
350 */
351/* Note M must be symmetric positive definite */
352/* routine is entitled to scribble on M and B if it wishes */
353static void
354choleski(real *M, real *B, long n)
355{
356   for (int j = 1; j < n; j++) {
357      real V;
358      for (int i = 0; i < j; i++) {
359         V = (real)0.0;
360         for (int k = 0; k < i; k++) V += M(i,k) * M(j,k) * M(k,k);
361         M(j,i) = (M(j,i) - V) / M(i,i);
362      }
363      V = (real)0.0;
364      for (int k = 0; k < j; k++) V += M(j,k) * M(j,k) * M(k,k);
365      M(j,j) -= V; /* may be best to add M() last for numerical reasons too */
366   }
367
368   /* Multiply x by L inverse */
369   for (int i = 0; i < n - 1; i++) {
370      for (int j = i + 1; j < n; j++) {
371         B[j] -= M(j,i) * B[i];
372      }
373   }
374
375   /* Multiply x by D inverse */
376   for (int i = 0; i < n; i++) {
377      B[i] /= M(i,i);
378   }
379
380   /* Multiply x by (L transpose) inverse */
381   for (int i = (int)(n - 1); i > 0; i--) {
382      for (int j = i - 1; j >= 0; j--) {
383         B[j] -= M(i,j) * B[i];
384      }
385   }
386
387   /* printf("\n%ld/%ld\n\n",flops,flopsTot); */
388}
389
390#ifdef SOR
391/* factor to use for SOR (must have 1 <= SOR_factor < 2) */
392#define SOR_factor 1.93 /* 1.95 */
393
394/* Solve MX=B for X by SOR of Gauss-Siedel */
395/* routine is entitled to scribble on M and B if it wishes */
396static void
397sor(real *M, real *B, long n)
398{
399   long it = 0;
400
401   real *X = osmalloc(n * ossizeof(real));
402
403   const real threshold = 0.00001;
404
405   printf("reciprocating diagonal\n"); /* TRANSLATE */
406
407   /* munge diagonal so we can multiply rather than divide */
408   for (int row = n - 1; row >= 0; row--) {
409      M(row,row) = 1 / M(row,row);
410      X[row] = 0;
411   }
412
413   printf("starting iteration\n"); /* TRANSLATE */
414
415   real t;
416   do {
417      /*printf("*");*/
418      it++;
419      t = 0.0;
420      for (int row = 0; row < n; row++) {
421         real x = B[row];
422         int col;
423         for (col = 0; col < row; col++) x -= M(row,col) * X[col];
424         for (col++; col < n; col++) x -= M(col,row) * X[col];
425         x *= M(row,row);
426         real sor_delta = (x - X[row]) * SOR_factor;
427         X[row] += sor_delta;
428         real t2 = fabs(sor_delta);
429         if (t2 > t) t = t2;
430      }
431      printf("% 6ld: %8.6f\n", it, t);
432   } while (t >= threshold && it < 100000);
433
434   if (t >= threshold) {
435      fprintf(stderr, "*not* converged after %ld iterations\n", it);
436      BUG("iteration stinks");
437   }
438
439   printf("%ld iterations\n", it); /* TRANSLATE */
440
441#if 0
442   putnl();
443   for (int row = n - 1; row >= 0; row--) {
444      t = 0.0;
445      for (int col = 0; col < row; col++) t += M(row, col) * X[col];
446      t += X[row] / M(row, row);
447      for (col = row + 1; col < n; col++)
448         t += M(col, row) * X[col];
449      printf("[ %f %f ]\n", t, B[row]);
450   }
451#endif
452
453   for (int row = n - 1; row >= 0; row--) B[row] = X[row];
454
455   osfree(X);
456   printf("\ndone\n"); /* TRANSLATE */
457}
458#endif
459
460#if PRINT_MATRICES
461static void
462print_matrix(real *M, real *B, long n)
463{
464   printf("Matrix, M and vector, B:\n");
465   for (long row = 0; row < n; row++) {
466      long col;
467      for (col = 0; col <= row; col++) printf("%6.2f\t", M(row, col));
468      for (; col <= n; col++) printf(" \t");
469      printf("\t%6.2f\n", B[row]);
470   }
471   putnl();
472   return;
473}
474#endif
Note: See TracBrowser for help on using the repository browser.