2 * Matrix building and solving routines
3 * Copyright (C) 1993-2003,2010,2013 Olly Betts
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.
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.
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
23 # define DEBUG_INVALID 1
39 #define PRINT_MATRICES 0
41 #undef DEBUG_MATRIX_BUILD
42 #define DEBUG_MATRIX_BUILD 0
45 #define DEBUG_MATRIX 0
48 static void print_matrix(real
*M
, real
*B
, long n
);
51 static void choleski(real
*M
, real
*B
, long n
);
54 static void sor(real
*M
, real
*B
, long n
);
57 /* for M(row, col) col must be <= row, so Y <= X */
58 # define M(X, Y) ((real *)M)[((((OSSIZE_T)(X)) * ((X) + 1)) >> 1) + (Y)]
59 /* +(Y>X?0*printf("row<col (line %d)\n",__LINE__):0) */
60 /*#define M_(X, Y) ((real *)M)[((((OSSIZE_T)(Y)) * ((Y) + 1)) >> 1) + (X)]*/
62 static int find_stn_in_tab(node
*stn
);
63 static int add_stn_to_tab(node
*stn
);
64 static void build_matrix(node
*list
);
66 static long n_stn_tab
;
71 solve_matrix(node
*list
)
75 FOR_EACH_STN(stn
, list
) {
80 /* we just need n to be a reasonable estimate >= the number
81 * of stations left after reduction. If memory is
82 * plentiful, we can be crass.
84 stn_tab
= osmalloc((OSSIZE_T
)(n
* ossizeof(pos
*)));
87 FOR_EACH_STN(stn
, list
) {
88 if (!fixed(stn
)) add_stn_to_tab(stn
);
92 /* release unused entries in stn_tab */
93 stn_tab
= osrealloc(stn_tab
, n_stn_tab
* ossizeof(pos
*));
98 FOR_EACH_STN(stn
, list
) {
99 printf("(%8.2f, %8.2f, %8.2f ) ", POS(stn
, 0), POS(stn
, 1), POS(stn
, 2));
100 print_prefix(stn
->name
);
108 #ifdef NO_COVARIANCES
115 build_matrix(node
*list
)
121 if (n_stn_tab
== 0) {
123 puts(msg(/*Network solved by reduction - no simultaneous equations to solve.*/74));
126 /* (OSSIZE_T) cast may be needed if n_stn_tab>=181 */
127 M
= osmalloc((OSSIZE_T
)((((OSSIZE_T
)n_stn_tab
* FACTOR
* (n_stn_tab
* FACTOR
+ 1)) >> 1)) * ossizeof(real
));
128 B
= osmalloc((OSSIZE_T
)(n_stn_tab
* FACTOR
* ossizeof(real
)));
132 out_current_action(msg(/*Solving one equation*/78));
134 out_current_action1(msg(/*Solving %d simultaneous equations*/75), n_stn_tab
);
137 #ifdef NO_COVARIANCES
140 dim
= 0; /* fudge next loop for now */
142 for ( ; dim
>= 0; dim
--) {
146 /* Initialise M and B to zero - zeroing "linearly" will minimise
147 * paging when the matrix is large */
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;
155 /* Construct matrix - Go thru' stn list & add all forward legs between
156 * two unfixed stations to M (so each leg goes on exactly once).
158 * All legs between two fixed stations can be ignored here.
160 * All legs between a fixed and an unfixed station are then considered
161 * from the unfixed end (if we consider them from the fixed end we'd
162 * need to somehow detect when we're at a fixed point cut line and work
163 * out which side we're dealing with at this time. */
164 FOR_EACH_STN(stn
, list
) {
165 #ifdef NO_COVARIANCES
173 #if DEBUG_MATRIX_BUILD
174 print_prefix(stn
->name
);
175 printf(" used: %d colour %ld\n",
176 (!!stn
->leg
[2]) << 2 | (!!stn
-> leg
[1]) << 1 | (!!stn
->leg
[0]),
179 for (dirn
= 0; dirn
<= 2 && stn
->leg
[dirn
]; dirn
++) {
180 #ifdef NO_COVARIANCES
181 printf("Leg %d, vx=%f, reverse=%d, to ", dirn
,
182 stn
->leg
[dirn
]->v
[0], stn
->leg
[dirn
]->l
.reverse
);
184 printf("Leg %d, vx=%f, reverse=%d, to ", dirn
,
185 stn
->leg
[dirn
]->v
[0][0], stn
->leg
[dirn
]->l
.reverse
);
187 print_prefix(stn
->leg
[dirn
]->l
.to
->name
);
191 #endif /* DEBUG_MATRIX_BUILD */
194 f
= find_stn_in_tab(stn
);
195 for (dirn
= 0; dirn
<= 2 && stn
->leg
[dirn
]; dirn
++) {
196 linkfor
*leg
= stn
->leg
[dirn
];
197 node
*to
= leg
->l
.to
;
199 bool fRev
= !data_here(leg
);
200 if (fRev
) leg
= reverse_leg(leg
);
201 /* Ignore equated nodes */
202 #ifdef NO_COVARIANCES
204 if (e
!= (real
)0.0) {
207 B
[f
] += e
* POS(to
, dim
);
215 if (invert_svar(&e
, &leg
->v
)) {
219 adddd(&a
, &POSD(to
), &leg
->d
);
221 subdd(&a
, &POSD(to
), &leg
->d
);
224 for (i
= 0; i
< 3; i
++) {
225 M(f
* FACTOR
+ i
, f
* FACTOR
+ i
) += e
[i
];
226 B
[f
* FACTOR
+ i
] += b
[i
];
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];
233 } else if (data_here(leg
)) {
234 /* forward leg, unfixed -> unfixed */
235 t
= find_stn_in_tab(to
);
237 printf("Leg %d to %d, var %f, delta %f\n", f
, t
, e
,
240 /* Ignore equated nodes & lollipops */
241 #ifdef NO_COVARIANCES
243 if (t
!= f
&& e
!= (real
)0.0) {
248 if (f
< t
) M(t
,f
) -= e
; else M(f
,t
) -= e
;
254 if (t
!= f
&& invert_svar(&e
, &leg
->v
)) {
256 mulsd(&a
, &e
, &leg
->d
);
257 for (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
];
261 M(t
* FACTOR
+ i
, f
* FACTOR
+ i
) -= e
[i
];
263 M(f
* FACTOR
+ i
, t
* FACTOR
+ i
) -= e
[i
];
264 B
[f
* FACTOR
+ i
] -= a
[i
];
265 B
[t
* FACTOR
+ i
] += a
[i
];
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];
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];
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];
296 print_matrix(M
, B
, n_stn_tab
* FACTOR
); /* 'ave a look! */
300 /* defined in network.c, may be altered by -z<letters> on command line */
301 if (optimize
& BITA('i'))
302 sor(M
, B
, n_stn_tab
* FACTOR
);
305 choleski(M
, B
, n_stn_tab
* FACTOR
);
309 for (m
= (int)(n_stn_tab
- 1); m
>= 0; m
--) {
310 #ifdef NO_COVARIANCES
311 stn_tab
[m
]->p
[dim
] = B
[m
];
313 SVX_ASSERT2(pos_fixed(stn_tab
[m
]),
314 "setting station coordinates didn't mark pos as fixed");
318 for (i
= 0; i
< 3; i
++) {
319 stn_tab
[m
]->p
[i
] = B
[m
* FACTOR
+ i
];
321 SVX_ASSERT2(pos_fixed(stn_tab
[m
]),
322 "setting station coordinates didn't mark pos as fixed");
325 #if EXPLICIT_FIXED_FLAG
326 for (m
= n_stn_tab
- 1; m
>= 0; m
--) fixpos(stn_tab
[m
]);
335 find_stn_in_tab(node
*stn
)
338 pos
*p
= stn
->name
->pos
;
339 while (stn_tab
[i
] != p
)
340 if (++i
== n_stn_tab
) {
342 fputs("Station ", stderr
);
343 fprint_prefix(stderr
, stn
->name
);
344 fputs(" not in table\n\n", stderr
);
347 print_prefix(stn
->name
);
348 printf(" used: %d colour %d\n",
349 (!!stn
->leg
[2])<<2 | (!!stn
->leg
[1])<<1 | (!!stn
->leg
[0]),
352 fatalerror(/*Bug in program detected! Please report this to the authors*/11);
358 add_stn_to_tab(node
*stn
)
361 pos
*p
= stn
->name
->pos
;
362 for (i
= 0; i
< n_stn_tab
; i
++) {
363 if (stn_tab
[i
] == p
) return i
;
365 stn_tab
[n_stn_tab
++] = p
;
369 /* Solve MX=B for X by Choleski factorisation - modified Choleski actually
370 * since we factor into LDL' while Choleski is just LL'
372 /* Note M must be symmetric positive definite */
373 /* routine is entitled to scribble on M and B if it wishes */
375 choleski(real
*M
, real
*B
, long n
)
379 for (j
= 1; j
< n
; j
++) {
381 for (i
= 0; i
< j
; i
++) {
383 for (k
= 0; k
< i
; k
++) V
+= M(i
,k
) * M(j
,k
) * M(k
,k
);
384 M(j
,i
) = (M(j
,i
) - V
) / M(i
,i
);
387 for (k
= 0; k
< j
; k
++) V
+= M(j
,k
) * M(j
,k
) * M(k
,k
);
388 M(j
,j
) -= V
; /* may be best to add M() last for numerical reasons too */
391 /* Multiply x by L inverse */
392 for (i
= 0; i
< n
- 1; i
++) {
393 for (j
= i
+ 1; j
< n
; j
++) {
394 B
[j
] -= M(j
,i
) * B
[i
];
398 /* Multiply x by D inverse */
399 for (i
= 0; i
< n
; i
++) {
403 /* Multiply x by (L transpose) inverse */
404 for (i
= (int)(n
- 1); i
> 0; i
--) {
405 for (j
= i
- 1; j
>= 0; j
--) {
406 B
[j
] -= M(i
,j
) * B
[i
];
410 /* printf("\n%ld/%ld\n\n",flops,flopsTot); */
414 /* factor to use for SOR (must have 1 <= SOR_factor < 2) */
415 #define SOR_factor 1.93 /* 1.95 */
417 /* Solve MX=B for X by SOR of Gauss-Siedel */
418 /* routine is entitled to scribble on M and B if it wishes */
420 sor(real
*M
, real
*B
, long n
)
422 real t
, x
, delta
, threshold
, t2
;
427 X
= osmalloc(n
* ossizeof(real
));
431 printf("reciprocating diagonal\n"); /* TRANSLATE */
433 /* munge diagonal so we can multiply rather than divide */
434 for (row
= n
- 1; row
>= 0; row
--) {
435 M(row
,row
) = 1 / M(row
,row
);
439 printf("starting iteration\n"); /* TRANSLATE */
445 for (row
= 0; row
< n
; row
++) {
447 for (col
= 0; col
< row
; col
++) x
-= M(row
,col
) * X
[col
];
448 for (col
++; col
< n
; col
++) x
-= M(col
,row
) * X
[col
];
450 delta
= (x
- X
[row
]) * SOR_factor
;
455 printf("% 6d: %8.6f\n", it
, t
);
456 } while (t
>= threshold
&& it
< 100000);
458 if (t
>= threshold
) {
459 fprintf(stderr
, "*not* converged after %ld iterations\n", it
);
460 BUG("iteration stinks");
463 printf("%ld iterations\n", it
); /* TRANSLATE */
467 for (row
= n
- 1; row
>= 0; row
--) {
469 for (col
= 0; col
< row
; col
++) t
+= M(row
, col
) * X
[col
];
470 t
+= X
[row
] / M(row
, row
);
471 for (col
= row
+ 1; col
< n
; col
++)
472 t
+= M(col
, row
) * X
[col
];
473 printf("[ %f %f ]\n", t
, B
[row
]);
477 for (row
= n
- 1; row
>= 0; row
--) B
[row
] = X
[row
];
480 printf("\ndone\n"); /* TRANSLATE */
486 print_matrix(real
*M
, real
*B
, long n
)
489 printf("Matrix, M and vector, B:\n");
490 for (row
= 0; row
< n
; row
++) {
491 for (col
= 0; col
<= row
; col
++) printf("%6.2f\t", M(row
, col
));
492 for (; col
<= n
; col
++) printf(" \t");
493 printf("\t%6.2f\n", B
[row
]);