source: git/src/matrix.c @ 3b8b342

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

Fix warnings in SOR code

This is some old experimental code to solve the matrix iteratively.
It still works if enabled (but seems to be much slower) but there
were some compiler warnings.
  • Property mode set to 100644
File size: 12.1 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   SVX_ASSERT(n_stn_tab > 0);
126   /* (OSSIZE_T) cast may be needed if n_stn_tab>=181 */
127   real *M = osmalloc((OSSIZE_T)((((OSSIZE_T)n_stn_tab * FACTOR * (n_stn_tab * FACTOR + 1)) >> 1)) * ossizeof(real));
128   real *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   int dim = 2;
139#else
140   int 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 by going through the stn list.
156       *
157       * All legs between two fixed stations can be ignored here.
158       *
159       * Other legs we want to add exactly once to M.  To achieve this we
160       * wan to:
161       *
162       * - add forward legs between two unfixed stations,
163       *
164       * - add legs from unfixed stations to fixed stations (we do them from
165       *   the unfixed end so we don't need to detect when we're at a fixed
166       *   point cut line and determine which side we're currently dealing
167       *   with).
168       *
169       * To implement this, we only look at legs from unfixed stations and add
170       * a leg if to a fixed station, or to an unfixed station and it's a
171       * forward leg.
172       */
173      FOR_EACH_STN(stn, list) {
174#ifdef NO_COVARIANCES
175         real e;
176#else
177         svar e;
178         delta a;
179#endif
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 (int 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            int f = stn->colour;
202            for (int 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                     if (fRev) {
224                        adddd(&a, &POSD(to), &leg->d);
225                     } else {
226                        subdd(&a, &POSD(to), &leg->d);
227                     }
228                     delta b;
229                     mulsd(&b, &e, &a);
230                     for (int i = 0; i < 3; i++) {
231                        M(f * FACTOR + i, f * FACTOR + i) += e[i];
232                        B[f * FACTOR + i] += b[i];
233                     }
234                     M(f * FACTOR + 1, f * FACTOR) += e[3];
235                     M(f * FACTOR + 2, f * FACTOR) += e[4];
236                     M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
237                  }
238#endif
239               } else if (data_here(leg)) {
240                  /* forward leg, unfixed -> unfixed */
241                  int t = to->colour;
242#if DEBUG_MATRIX
243                  printf("Leg %d to %d, var %f, delta %f\n", f, t, e,
244                         leg->d[dim]);
245#endif
246                  /* Ignore equated nodes & lollipops */
247#ifdef NO_COVARIANCES
248                  e = leg->v[dim];
249                  if (t != f && e != (real)0.0) {
250                     e = ((real)1.0) / e;
251                     M(f,f) += e;
252                     M(t,t) += e;
253                     if (f < t) M(t,f) -= e; else M(f,t) -= e;
254                     real a = e * leg->d[dim];
255                     B[f] -= a;
256                     B[t] += a;
257                  }
258#else
259                  if (t != f && invert_svar(&e, &leg->v)) {
260                     mulsd(&a, &e, &leg->d);
261                     for (int i = 0; i < 3; i++) {
262                        M(f * FACTOR + i, f * FACTOR + i) += e[i];
263                        M(t * FACTOR + i, t * FACTOR + i) += e[i];
264                        if (f < t)
265                           M(t * FACTOR + i, f * FACTOR + i) -= e[i];
266                        else
267                           M(f * FACTOR + i, t * FACTOR + i) -= e[i];
268                        B[f * FACTOR + i] -= a[i];
269                        B[t * FACTOR + i] += a[i];
270                     }
271                     M(f * FACTOR + 1, f * FACTOR) += e[3];
272                     M(t * FACTOR + 1, t * FACTOR) += e[3];
273                     M(f * FACTOR + 2, f * FACTOR) += e[4];
274                     M(t * FACTOR + 2, t * FACTOR) += e[4];
275                     M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
276                     M(t * FACTOR + 2, t * FACTOR + 1) += e[5];
277                     if (f < t) {
278                        M(t * FACTOR + 1, f * FACTOR) -= e[3];
279                        M(t * FACTOR, f * FACTOR + 1) -= e[3];
280                        M(t * FACTOR + 2, f * FACTOR) -= e[4];
281                        M(t * FACTOR, f * FACTOR + 2) -= e[4];
282                        M(t * FACTOR + 2, f * FACTOR + 1) -= e[5];
283                        M(t * FACTOR + 1, f * FACTOR + 2) -= e[5];
284                     } else {
285                        M(f * FACTOR + 1, t * FACTOR) -= e[3];
286                        M(f * FACTOR, t * FACTOR + 1) -= e[3];
287                        M(f * FACTOR + 2, t * FACTOR) -= e[4];
288                        M(f * FACTOR, t * FACTOR + 2) -= e[4];
289                        M(f * FACTOR + 2, t * FACTOR + 1) -= e[5];
290                        M(f * FACTOR + 1, t * FACTOR + 2) -= e[5];
291                     }
292                  }
293#endif
294               }
295            }
296         }
297      }
298
299#if PRINT_MATRICES
300      print_matrix(M, B, n_stn_tab * FACTOR); /* 'ave a look! */
301#endif
302
303#ifdef SOR
304      /* defined in network.c, may be altered by -z<letters> on command line */
305      if (optimize & BITA('i'))
306         sor(M, B, n_stn_tab * FACTOR);
307      else
308#endif
309         choleski(M, B, n_stn_tab * FACTOR);
310
311      {
312         for (int m = (int)(n_stn_tab - 1); m >= 0; m--) {
313#ifdef NO_COVARIANCES
314            stn_tab[m]->p[dim] = B[m];
315            if (dim == 0) {
316               SVX_ASSERT2(pos_fixed(stn_tab[m]),
317                       "setting station coordinates didn't mark pos as fixed");
318            }
319#else
320            for (int i = 0; i < 3; i++) {
321               stn_tab[m]->p[i] = B[m * FACTOR + i];
322            }
323            SVX_ASSERT2(pos_fixed(stn_tab[m]),
324                    "setting station coordinates didn't mark pos as fixed");
325#endif
326         }
327#if EXPLICIT_FIXED_FLAG
328         for (int m = n_stn_tab - 1; m >= 0; m--) fixpos(stn_tab[m]);
329#endif
330      }
331   }
332   osfree(B);
333   osfree(M);
334}
335
336/* Solve MX=B for X by Choleski factorisation - modified Choleski actually
337 * since we factor into LDL' while Choleski is just LL'
338 */
339/* Note M must be symmetric positive definite */
340/* routine is entitled to scribble on M and B if it wishes */
341static void
342choleski(real *M, real *B, long n)
343{
344   for (int j = 1; j < n; j++) {
345      real V;
346      for (int i = 0; i < j; i++) {
347         V = (real)0.0;
348         for (int k = 0; k < i; k++) V += M(i,k) * M(j,k) * M(k,k);
349         M(j,i) = (M(j,i) - V) / M(i,i);
350      }
351      V = (real)0.0;
352      for (int k = 0; k < j; k++) V += M(j,k) * M(j,k) * M(k,k);
353      M(j,j) -= V; /* may be best to add M() last for numerical reasons too */
354   }
355
356   /* Multiply x by L inverse */
357   for (int i = 0; i < n - 1; i++) {
358      for (int j = i + 1; j < n; j++) {
359         B[j] -= M(j,i) * B[i];
360      }
361   }
362
363   /* Multiply x by D inverse */
364   for (int i = 0; i < n; i++) {
365      B[i] /= M(i,i);
366   }
367
368   /* Multiply x by (L transpose) inverse */
369   for (int i = (int)(n - 1); i > 0; i--) {
370      for (int j = i - 1; j >= 0; j--) {
371         B[j] -= M(i,j) * B[i];
372      }
373   }
374
375   /* printf("\n%ld/%ld\n\n",flops,flopsTot); */
376}
377
378#ifdef SOR
379/* factor to use for SOR (must have 1 <= SOR_factor < 2) */
380#define SOR_factor 1.93 /* 1.95 */
381
382/* Solve MX=B for X by SOR of Gauss-Siedel */
383/* routine is entitled to scribble on M and B if it wishes */
384static void
385sor(real *M, real *B, long n)
386{
387   long it = 0;
388
389   real *X = osmalloc(n * ossizeof(real));
390
391   const real threshold = 0.00001;
392
393   printf("reciprocating diagonal\n"); /* TRANSLATE */
394
395   /* munge diagonal so we can multiply rather than divide */
396   for (int row = n - 1; row >= 0; row--) {
397      M(row,row) = 1 / M(row,row);
398      X[row] = 0;
399   }
400
401   printf("starting iteration\n"); /* TRANSLATE */
402
403   real t;
404   do {
405      /*printf("*");*/
406      it++;
407      t = 0.0;
408      for (int row = 0; row < n; row++) {
409         real x = B[row];
410         int col;
411         for (col = 0; col < row; col++) x -= M(row,col) * X[col];
412         for (col++; col < n; col++) x -= M(col,row) * X[col];
413         x *= M(row,row);
414         real sor_delta = (x - X[row]) * SOR_factor;
415         X[row] += sor_delta;
416         real t2 = fabs(sor_delta);
417         if (t2 > t) t = t2;
418      }
419      printf("% 6ld: %8.6f\n", it, t);
420   } while (t >= threshold && it < 100000);
421
422   if (t >= threshold) {
423      fprintf(stderr, "*not* converged after %ld iterations\n", it);
424      BUG("iteration stinks");
425   }
426
427   printf("%ld iterations\n", it); /* TRANSLATE */
428
429#if 0
430   putnl();
431   for (int row = n - 1; row >= 0; row--) {
432      t = 0.0;
433      for (int col = 0; col < row; col++) t += M(row, col) * X[col];
434      t += X[row] / M(row, row);
435      for (col = row + 1; col < n; col++)
436         t += M(col, row) * X[col];
437      printf("[ %f %f ]\n", t, B[row]);
438   }
439#endif
440
441   for (int row = n - 1; row >= 0; row--) B[row] = X[row];
442
443   osfree(X);
444   printf("\ndone\n"); /* TRANSLATE */
445}
446#endif
447
448#if PRINT_MATRICES
449static void
450print_matrix(real *M, real *B, long n)
451{
452   printf("Matrix, M and vector, B:\n");
453   for (long row = 0; row < n; row++) {
454      long col;
455      for (col = 0; col <= row; col++) printf("%6.2f\t", M(row, col));
456      for (; col <= n; col++) printf(" \t");
457      printf("\t%6.2f\n", B[row]);
458   }
459   putnl();
460   return;
461}
462#endif
Note: See TracBrowser for help on using the repository browser.