source: git/src/matrix.c

main
Last change on this file was 8048d405, checked in by Olly Betts <olly@…>, 3 months ago

Separate linkcommon reverse and flag bits

We had at least 2 spare bytes of padding, so we can store the reverse
direction separately which seems cleaner (and slightly more efficient
but the difference is so small it's only measureable using cachegrind).
  • Property mode set to 100644
File size: 12.6 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, see
17 * <https://www.gnu.org/licenses/>.
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 "osalloc.h"
35#include "out.h"
36
37#undef PRINT_MATRICES
38#define PRINT_MATRICES 0
39
40#undef DEBUG_MATRIX_BUILD
41#define DEBUG_MATRIX_BUILD 0
42
43#undef DEBUG_MATRIX
44#define DEBUG_MATRIX 0
45
46#if PRINT_MATRICES
47static void print_matrix(real *M, real *B, long n);
48#endif
49
50static void choleski(real *M, real *B, long n);
51
52#ifdef SOR
53static void sor(real *M, real *B, long n);
54#endif
55
56/* for M(row, col) col must be <= row, so Y <= X */
57# define M(X, Y) ((real *)M)[((((size_t)(X)) * ((X) + 1)) >> 1) + (Y)]
58 /* +(Y>X?0*printf("row<col (line %d)\n",__LINE__):0) */
59/*#define M_(X, Y) ((real *)M)[((((size_t)(Y)) * ((Y) + 1)) >> 1) + (X)]*/
60
61static void set_row(node *stn, int row_number) {
62 // We store the matrix row/column index in stn->colour for quick and easy
63 // lookup when copying out the solved station coordinates.
64 stn->colour = row_number;
65 for (int d = 0; d < 3; d++) {
66 linkfor *leg = stn->leg[d];
67 if (!leg) break;
68 node *to = leg->l.to;
69 if (to->colour < 0 && stn->name->pos == to->name->pos) {
70 set_row(to, row_number);
71 }
72 }
73}
74
75#ifdef NO_COVARIANCES
76# define FACTOR 1
77#else
78# define FACTOR 3
79#endif
80
81/* Find positions for a subset of the reduced network by solving a matrix
82 * equation.
83 *
84 * list is a non-empty linked list of unfixed stations to solve for.
85 *
86 * As a pre-condition, all stations in list must have a negative value for
87 * stn->colour. This can be ensured by the caller (which avoids having to
88 * make an extra pass over the list just to set the colours suitably).
89 */
90extern void
91solve_matrix(node *list)
92{
93 // Assign a matrix row/column index to each group of stations with the same
94 // pos.
95 //
96 // We also set listend to the last station in the list while doing so, which
97 // we use after solving to splice list into fixedlist.
98 node *listend = NULL;
99 size_t n = 0;
100 for (node *stn = list; stn; stn = stn->next) {
101 listend = stn;
102 if (stn->colour < 0) {
103 set_row(stn, n++);
104 }
105 }
106 SVX_ASSERT(n > 0);
107
108 // Array to map from row/column index to pos. We fill this in as we build
109 // the matrix, and use it to know where to copy the solved station
110 // coordinates to.
111 pos **stn_tab = osmalloc(n * sizeof(pos*));
112
113 real *M = osmalloc((((n * FACTOR * (n * FACTOR + 1)) >> 1)) * sizeof(real));
114 real *B = osmalloc(n * FACTOR * sizeof(real));
115
116 if (n == 1)
117 out_current_action(msg(/*Solving one equation*/78));
118 else
119 out_current_action1(msg(/*Solving %d simultaneous equations*/75), (int)n);
120
121#ifdef NO_COVARIANCES
122 int dim = 2;
123#else
124 int dim = 0; /* Collapse loop to a single iteration. */
125#endif
126 for ( ; dim >= 0; dim--) {
127 /* Initialise M and B to zero - zeroing "linearly" will minimise
128 * paging when the matrix is large */
129 {
130 int end = n * FACTOR;
131 for (int row = 0; row < end; row++) B[row] = (real)0.0;
132 end = ((size_t)n * FACTOR * (n * FACTOR + 1)) >> 1;
133 for (int row = 0; row < end; row++) M[row] = (real)0.0;
134 }
135
136 /* Construct matrix by going through the stn list.
137 *
138 * All legs between two fixed stations can be ignored here.
139 *
140 * Other legs we want to add exactly once to M. To achieve this we
141 * want to:
142 *
143 * - add forward legs between two unfixed stations,
144 *
145 * - add legs from unfixed stations to fixed stations (we do them from
146 * the unfixed end so we don't need to detect when we're at a fixed
147 * point cut line and determine which side we're currently dealing
148 * with).
149 *
150 * To implement this, we only look at legs from unfixed stations and add
151 * a leg if to a fixed station, or to an unfixed station and it's a
152 * forward leg.
153 */
154 for (node *stn = list; stn; stn = stn->next) {
155 if (dim == 0) {
156 stn_tab[stn->colour] = stn->name->pos;
157 }
158
159#ifdef NO_COVARIANCES
160 real e;
161#else
162 svar e;
163 delta a;
164#endif
165#if DEBUG_MATRIX_BUILD
166 print_prefix(stn->name);
167 printf(" used: %d colour %ld\n",
168 (!!stn->leg[2]) << 2 | (!!stn -> leg[1]) << 1 | (!!stn->leg[0]),
169 stn->colour);
170
171 for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
172 printf("Leg %d, vx=%f, reverse=%d, bits = 0x%02x, to ", dirn,
173 stn->leg[dirn]->v[0], stn->leg[dirn]->l.reverse,
174 stn->leg[dirn]->l.bits);
175 print_prefix(stn->leg[dirn]->l.to->name);
176 putnl();
177 }
178 putnl();
179#endif /* DEBUG_MATRIX_BUILD */
180
181 int f = stn->colour;
182 SVX_ASSERT(f >= 0);
183 {
184 for (int dirn = 0; dirn <= 2 && stn->leg[dirn]; dirn++) {
185 linkfor *leg = stn->leg[dirn];
186 node *to = leg->l.to;
187 if (fixed(to)) {
188 bool fRev = !data_here(leg);
189 if (fRev) leg = reverse_leg(leg);
190 /* Ignore equated nodes */
191#ifdef NO_COVARIANCES
192 e = leg->v[dim];
193 if (e != (real)0.0) {
194 e = ((real)1.0) / e;
195 M(f,f) += e;
196 B[f] += e * POS(to, dim);
197 if (fRev) {
198 B[f] += leg->d[dim];
199 } else {
200 B[f] -= leg->d[dim];
201 }
202 }
203#else
204 if (invert_svar(&e, &leg->v)) {
205 if (fRev) {
206 adddd(&a, &POSD(to), &leg->d);
207 } else {
208 subdd(&a, &POSD(to), &leg->d);
209 }
210 delta b;
211 mulsd(&b, &e, &a);
212 for (int i = 0; i < 3; i++) {
213 M(f * FACTOR + i, f * FACTOR + i) += e[i];
214 B[f * FACTOR + i] += b[i];
215 }
216 M(f * FACTOR + 1, f * FACTOR) += e[3];
217 M(f * FACTOR + 2, f * FACTOR) += e[4];
218 M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
219 }
220#endif
221 } else if (data_here(leg) &&
222 (leg->l.bits & FLAG_ARTICULATION) == 0) {
223 /* forward leg, unfixed -> unfixed */
224 int t = to->colour;
225 SVX_ASSERT(t >= 0);
226#if DEBUG_MATRIX
227# ifdef NO_COVARIANCES
228 printf("Leg %d to %d, var %f, delta %f\n", f, t, e,
229 leg->d[dim]);
230# else
231 printf("Leg %d to %d, var (%f, %f, %f; %f, %f, %f), "
232 "delta %f\n", f, t, e[0], e[1], e[2], e[3], e[4], e[5],
233 leg->d[dim]);
234# endif
235#endif
236 /* Ignore equated nodes & lollipops */
237#ifdef NO_COVARIANCES
238 e = leg->v[dim];
239 if (t != f && e != (real)0.0) {
240 e = ((real)1.0) / e;
241 M(f,f) += e;
242 M(t,t) += e;
243 if (f < t) M(t,f) -= e; else M(f,t) -= e;
244 real a = e * leg->d[dim];
245 B[f] -= a;
246 B[t] += a;
247 }
248#else
249 if (t != f && invert_svar(&e, &leg->v)) {
250 mulsd(&a, &e, &leg->d);
251 for (int i = 0; i < 3; i++) {
252 M(f * FACTOR + i, f * FACTOR + i) += e[i];
253 M(t * FACTOR + i, t * FACTOR + i) += e[i];
254 if (f < t)
255 M(t * FACTOR + i, f * FACTOR + i) -= e[i];
256 else
257 M(f * FACTOR + i, t * FACTOR + i) -= e[i];
258 B[f * FACTOR + i] -= a[i];
259 B[t * FACTOR + i] += a[i];
260 }
261 M(f * FACTOR + 1, f * FACTOR) += e[3];
262 M(t * FACTOR + 1, t * FACTOR) += e[3];
263 M(f * FACTOR + 2, f * FACTOR) += e[4];
264 M(t * FACTOR + 2, t * FACTOR) += e[4];
265 M(f * FACTOR + 2, f * FACTOR + 1) += e[5];
266 M(t * FACTOR + 2, t * FACTOR + 1) += e[5];
267 if (f < t) {
268 M(t * FACTOR + 1, f * FACTOR) -= e[3];
269 M(t * FACTOR, f * FACTOR + 1) -= e[3];
270 M(t * FACTOR + 2, f * FACTOR) -= e[4];
271 M(t * FACTOR, f * FACTOR + 2) -= e[4];
272 M(t * FACTOR + 2, f * FACTOR + 1) -= e[5];
273 M(t * FACTOR + 1, f * FACTOR + 2) -= e[5];
274 } else {
275 M(f * FACTOR + 1, t * FACTOR) -= e[3];
276 M(f * FACTOR, t * FACTOR + 1) -= e[3];
277 M(f * FACTOR + 2, t * FACTOR) -= e[4];
278 M(f * FACTOR, t * FACTOR + 2) -= e[4];
279 M(f * FACTOR + 2, t * FACTOR + 1) -= e[5];
280 M(f * FACTOR + 1, t * FACTOR + 2) -= e[5];
281 }
282 }
283#endif
284 }
285 }
286 }
287 }
288
289#if PRINT_MATRICES
290 print_matrix(M, B, n * FACTOR); /* 'ave a look! */
291#endif
292
293#ifdef SOR
294 /* defined in network.c, may be altered by -z<letters> on command line */
295 if (optimize & BITA('i'))
296 sor(M, B, n * FACTOR);
297 else
298#endif
299 choleski(M, B, n * FACTOR);
300
301 {
302 for (int m = (int)(n - 1); m >= 0; m--) {
303#ifdef NO_COVARIANCES
304 stn_tab[m]->p[dim] = B[m];
305 if (dim == 0) {
306 SVX_ASSERT2(pos_fixed(stn_tab[m]),
307 "setting station coordinates didn't mark pos as fixed");
308 }
309#else
310 for (int i = 0; i < 3; i++) {
311 stn_tab[m]->p[i] = B[m * FACTOR + i];
312 }
313 SVX_ASSERT2(pos_fixed(stn_tab[m]),
314 "setting station coordinates didn't mark pos as fixed");
315#endif
316 }
317 }
318 }
319
320 // Put the solved stations back on fixedlist.
321 listend->next = fixedlist;
322 if (fixedlist) fixedlist->prev = listend;
323 fixedlist = list;
324
325 free(B);
326 free(M);
327 free(stn_tab);
328
329#if DEBUG_MATRIX
330 for (node *stn = list; stn; stn = stn->next) {
331 printf("(%8.2f, %8.2f, %8.2f ) ", POS(stn, 0), POS(stn, 1), POS(stn, 2));
332 print_prefix(stn->name);
333 putnl();
334 }
335#endif
336}
337
338/* Solve MX=B for X by first factoring M into LDL'. This is a modified form
339 * of Choleski factorisation - the original Choleski factorisation is LL',
340 * but this modified version has the advantage of avoiding O(n) square root
341 * calculations.
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 * sizeof(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 sor_delta = (x - X[row]) * SOR_factor;
419 X[row] += sor_delta;
420 real t2 = fabs(sor_delta);
421 if (t2 > t) t = t2;
422 }
423 printf("% 6ld: %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 free(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.