Add MainFrm::GetCSProj() getter method
[survex.git] / src / netbits.c
blobe9551a5981e9aa65444cb6308c0edafa77fc25c4
1 /* netbits.c
2 * Miscellaneous primitive network routines for Survex
3 * Copyright (C) 1992-2003,2006,2011,2013,2014,2015 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
20 #ifdef HAVE_CONFIG_H
21 # include <config.h>
22 #endif
24 #if 0
25 # define DEBUG_INVALID 1
26 #endif
28 #include "debug.h"
29 #include "cavern.h"
30 #include "filename.h"
31 #include "message.h"
32 #include "netbits.h"
33 #include "datain.h" /* for compile_error */
34 #include "validate.h" /* for compile_error */
36 #define THRESHOLD (REAL_EPSILON * 1000) /* 100 was too small */
38 node *stn_iter = NULL; /* for FOR_EACH_STN */
40 static struct {
41 prefix * to_name;
42 prefix * fr_name;
43 linkfor * leg;
44 int n;
45 } last_leg = { NULL, NULL, NULL, 0 };
47 void clear_last_leg(void) {
48 last_leg.to_name = NULL;
51 static char freeleg(node **stnptr);
53 #ifdef NO_COVARIANCES
54 static void check_var(/*const*/ var *v) {
55 int bad = 0;
56 int i;
58 for (i = 0; i < 3; i++) {
59 char buf[32];
60 sprintf(buf, "%6.3f", v[i]);
61 if (strstr(buf, "NaN") || strstr(buf, "nan"))
62 printf("*** NaN!!!\n"), bad = 1;
64 if (bad) print_var(v);
65 return;
67 #else
68 #define V(A,B) ((*v)[A][B])
69 static void check_var(/*const*/ var *v) {
70 int bad = 0;
71 int ok = 0;
72 int i, j;
73 #if DEBUG_INVALID
74 real det = 0.0;
75 #endif
77 for (i = 0; i < 3; i++) {
78 for (j = 0; j < 3; j++) {
79 char buf[32];
80 sprintf(buf, "%6.3f", V(i, j));
81 if (strstr(buf, "NaN") || strstr(buf, "nan"))
82 printf("*** NaN!!!\n"), bad = 1, ok = 1;
83 if (V(i, j) != 0.0) ok = 1;
86 if (!ok) return; /* ignore all-zero matrices */
88 #if DEBUG_INVALID
89 for (i = 0; i < 3; i++) {
90 det += V(i, 0) * (V((i + 1) % 3, 1) * V((i + 2) % 3, 2) -
91 V((i + 1) % 3, 2) * V((i + 2) % 3, 1));
94 if (fabs(det) < THRESHOLD)
95 printf("*** Singular!!!\n"), bad = 1;
96 #endif
98 #if 0
99 /* don't check this - it isn't always the case! */
100 if (fabs(V(0,1) - V(1,0)) > THRESHOLD ||
101 fabs(V(0,2) - V(2,0)) > THRESHOLD ||
102 fabs(V(1,2) - V(2,1)) > THRESHOLD)
103 printf("*** Not symmetric!!!\n"), bad = 1;
104 if (V(0,0) <= 0.0 || V(1,1) <= 0.0 || V(2,2) <= 0.0)
105 printf("*** Not positive definite (diag <= 0)!!!\n"), bad = 1;
106 if (sqrd(V(0,1)) >= V(0,0)*V(1,1) || sqrd(V(0,2)) >= V(0,0)*V(2,2) ||
107 sqrd(V(1,0)) >= V(0,0)*V(1,1) || sqrd(V(2,0)) >= V(0,0)*V(2,2) ||
108 sqrd(V(1,2)) >= V(2,2)*V(1,1) || sqrd(V(2,1)) >= V(2,2)*V(1,1))
109 printf("*** Not positive definite (off diag^2 >= diag product)!!!\n"), bad = 1;
110 #endif
111 if (bad) print_var(*v);
114 #define SN(V,A,B) ((*(V))[(A)==(B)?(A):2+(A)+(B)])
115 #define S(A,B) SN(v,A,B)
117 static void check_svar(/*const*/ svar *v) {
118 int bad = 0;
119 int ok = 0;
120 int i;
121 #if DEBUG_INVALID
122 real det = 0.0;
123 #endif
125 for (i = 0; i < 6; i++) {
126 char buf[32];
127 sprintf(buf, "%6.3f", (*v)[i]);
128 if (strstr(buf, "NaN") || strstr(buf, "nan"))
129 printf("*** NaN!!!\n"), bad = 1, ok = 1;
130 if ((*v)[i] != 0.0) ok = 1;
132 if (!ok) return; /* ignore all-zero matrices */
134 #if DEBUG_INVALID
135 for (i = 0; i < 3; i++) {
136 det += S(i, 0) * (S((i + 1) % 3, 1) * S((i + 2) % 3, 2) -
137 S((i + 1) % 3, 2) * S((i + 2) % 3, 1));
140 if (fabs(det) < THRESHOLD)
141 printf("*** Singular!!!\n"), bad = 1;
142 #endif
144 #if 0
145 /* don't check this - it isn't always the case! */
146 if ((*v)[0] <= 0.0 || (*v)[1] <= 0.0 || (*v)[2] <= 0.0)
147 printf("*** Not positive definite (diag <= 0)!!!\n"), bad = 1;
148 if (sqrd((*v)[3]) >= (*v)[0]*(*v)[1] ||
149 sqrd((*v)[4]) >= (*v)[0]*(*v)[2] ||
150 sqrd((*v)[5]) >= (*v)[1]*(*v)[2])
151 printf("*** Not positive definite (off diag^2 >= diag product)!!!\n"), bad = 1;
152 #endif
153 if (bad) print_svar(*v);
155 #endif
157 static void check_d(/*const*/ delta *d) {
158 int bad = 0;
159 int i;
161 for (i = 0; i < 3; i++) {
162 char buf[32];
163 sprintf(buf, "%6.3f", (*d)[i]);
164 if (strstr(buf, "NaN") || strstr(buf, "nan"))
165 printf("*** NaN!!!\n"), bad = 1;
168 if (bad) printf("(%4.2f,%4.2f,%4.2f)\n", (*d)[0], (*d)[1], (*d)[2]);
171 /* insert at head of double-linked list */
172 void
173 add_stn_to_list(node **list, node *stn) {
174 SVX_ASSERT(list);
175 SVX_ASSERT(stn);
176 SVX_ASSERT(stn_iter != stn); /* if it does, we're still on a list... */
177 #if 0
178 printf("add_stn_to_list(%p, [%p] ", list, stn);
179 if (stn->name) print_prefix(stn->name);
180 printf(")\n");
181 #endif
182 stn->next = *list;
183 stn->prev = NULL;
184 if (*list) (*list)->prev = stn;
185 *list = stn;
188 /* remove from double-linked list */
189 void
190 remove_stn_from_list(node **list, node *stn) {
191 SVX_ASSERT(list);
192 SVX_ASSERT(stn);
193 #if 0
194 printf("remove_stn_from_list(%p, [%p] ", list, stn);
195 if (stn->name) print_prefix(stn->name);
196 printf(")\n");
197 #endif
198 #if DEBUG_INVALID
200 /* check station is actually in this list */
201 node *stn_to_remove_is_in_list = *list;
202 validate();
203 while (stn_to_remove_is_in_list != stn) {
204 SVX_ASSERT(stn_to_remove_is_in_list);
205 stn_to_remove_is_in_list = stn_to_remove_is_in_list->next;
208 #endif
209 /* adjust the iterator if it points to the element we're deleting */
210 if (stn_iter == stn) stn_iter = stn_iter->next;
211 /* need a special case if we're removing the list head */
212 if (stn->prev == NULL) {
213 *list = stn->next;
214 if (*list) (*list)->prev = NULL;
215 } else {
216 stn->prev->next = stn->next;
217 if (stn->next) stn->next->prev = stn->prev;
221 /* Create (uses osmalloc) a forward leg containing the data in leg, or
222 * the reversed data in the reverse of leg, if leg doesn't hold data
224 linkfor *
225 copy_link(linkfor *leg)
227 linkfor *legOut;
228 int d;
229 legOut = osnew(linkfor);
230 if (data_here(leg)) {
231 for (d = 2; d >= 0; d--) legOut->d[d] = leg->d[d];
232 } else {
233 leg = reverse_leg(leg);
234 SVX_ASSERT(data_here(leg));
235 for (d = 2; d >= 0; d--) legOut->d[d] = -leg->d[d];
237 #if 1
238 # ifndef NO_COVARIANCES
239 check_svar(&(leg->v));
241 int i;
242 for (i = 0; i < 6; i++) legOut->v[i] = leg->v[i];
244 # else
245 for (d = 2; d >= 0; d--) legOut->v[d] = leg->v[d];
246 # endif
247 #else
248 memcpy(legOut->v, leg->v, sizeof(svar));
249 #endif
250 legOut->meta = pcs->meta;
251 if (pcs->meta) ++pcs->meta->ref_count;
252 return legOut;
255 /* Adds to the forward leg “leg”, the data in leg2, or the reversed data
256 * in the reverse of leg2, if leg2 doesn't hold data
258 linkfor *
259 addto_link(linkfor *leg, const linkfor *leg2)
261 if (data_here(leg2)) {
262 adddd(&leg->d, &leg->d, &((linkfor *)leg2)->d);
263 } else {
264 leg2 = reverse_leg(leg2);
265 SVX_ASSERT(data_here(leg2));
266 subdd(&leg->d, &leg->d, &((linkfor *)leg2)->d);
268 addss(&leg->v, &leg->v, &((linkfor *)leg2)->v);
269 return leg;
272 static linkfor *
273 addleg_(node *fr, node *to,
274 real dx, real dy, real dz,
275 real vx, real vy, real vz,
276 #ifndef NO_COVARIANCES
277 real cyz, real czx, real cxy,
278 #endif
279 int leg_flags)
281 int i, j;
282 linkfor *leg, *leg2;
283 /* we have been asked to add a leg with the same node at both ends
284 * - this should be trapped by the caller */
285 SVX_ASSERT(fr->name != to->name);
287 leg = osnew(linkfor);
288 leg2 = (linkfor*)osnew(linkrev);
290 i = freeleg(&fr);
291 j = freeleg(&to);
293 leg->l.to = to;
294 leg2->l.to = fr;
295 leg->d[0] = dx;
296 leg->d[1] = dy;
297 leg->d[2] = dz;
298 #ifndef NO_COVARIANCES
299 leg->v[0] = vx;
300 leg->v[1] = vy;
301 leg->v[2] = vz;
302 leg->v[3] = cxy;
303 leg->v[4] = czx;
304 leg->v[5] = cyz;
305 check_svar(&(leg->v));
306 #else
307 leg->v[0] = vx;
308 leg->v[1] = vy;
309 leg->v[2] = vz;
310 #endif
311 leg2->l.reverse = i;
312 leg->l.reverse = j | FLAG_DATAHERE | leg_flags;
314 leg->l.flags = pcs->flags | (pcs->style << FLAGS_STYLE_BIT0);
315 leg->meta = pcs->meta;
316 if (pcs->meta) ++pcs->meta->ref_count;
318 fr->leg[i] = leg;
319 to->leg[j] = leg2;
321 ++fr->name->shape;
322 ++to->name->shape;
324 return leg;
327 /* Add a leg between names *fr_name and *to_name
328 * If either is a three node, then it is split into two
329 * and the data structure adjusted as necessary.
331 void
332 addlegbyname(prefix *fr_name, prefix *to_name, bool fToFirst,
333 real dx, real dy, real dz,
334 real vx, real vy, real vz
335 #ifndef NO_COVARIANCES
336 , real cyz, real czx, real cxy
337 #endif
340 node *to, *fr;
341 if (to_name == fr_name) {
342 /* TRANSLATORS: Here a "survey leg" is a set of measurements between two
343 * "survey stations".
345 * %s is replaced by the name of the station. */
346 compile_diagnostic(DIAG_ERR, /*Survey leg with same station (“%s”) at both ends - typing error?*/50,
347 sprint_prefix(to_name));
348 return;
350 if (fToFirst) {
351 to = StnFromPfx(to_name);
352 fr = StnFromPfx(fr_name);
353 } else {
354 fr = StnFromPfx(fr_name);
355 to = StnFromPfx(to_name);
357 if (last_leg.to_name) {
358 if (last_leg.to_name == to_name && last_leg.fr_name == fr_name) {
359 /* FIXME: Not the right way to average... */
360 linkfor * leg = last_leg.leg;
361 int n = last_leg.n++;
362 leg->d[0] = (leg->d[0] * n + dx) / (n + 1);
363 leg->d[1] = (leg->d[1] * n + dy) / (n + 1);
364 leg->d[2] = (leg->d[2] * n + dz) / (n + 1);
365 #ifndef NO_COVARIANCES
366 leg->v[0] = (leg->v[0] * n + vx) / (n + 1);
367 leg->v[1] = (leg->v[1] * n + vy) / (n + 1);
368 leg->v[2] = (leg->v[2] * n + vz) / (n + 1);
369 leg->v[3] = (leg->v[3] * n + cxy) / (n + 1);
370 leg->v[4] = (leg->v[4] * n + czx) / (n + 1);
371 leg->v[5] = (leg->v[5] * n + cyz) / (n + 1);
372 check_svar(&(leg->v));
373 #else
374 leg->v[0] = (leg->v[0] * n + vx) / (n + 1);
375 leg->v[1] = (leg->v[1] * n + vy) / (n + 1);
376 leg->v[2] = (leg->v[2] * n + vz) / (n + 1);
377 #endif
378 return;
381 cLegs++;
383 last_leg.to_name = to_name;
384 last_leg.fr_name = fr_name;
385 last_leg.n = 1;
386 last_leg.leg = addleg_(fr, to, dx, dy, dz, vx, vy, vz,
387 #ifndef NO_COVARIANCES
388 cyz, czx, cxy,
389 #endif
393 /* helper function for replace_pfx */
394 static void
395 replace_pfx_(node *stn, node *from, pos *pos_replace, pos *pos_with)
397 int d;
398 stn->name->pos = pos_with;
399 for (d = 0; d < 3; d++) {
400 linkfor *leg = stn->leg[d];
401 node *to;
402 if (!leg) break;
403 to = leg->l.to;
404 if (to == from) continue;
406 if (fZeros(data_here(leg) ? &leg->v : &reverse_leg(leg)->v))
407 replace_pfx_(to, stn, pos_replace, pos_with);
411 /* We used to iterate over the whole station list (inefficient) - now we
412 * just look at any neighbouring nodes to see if they are equated */
413 static void
414 replace_pfx(const prefix *pfx_replace, const prefix *pfx_with)
416 pos *pos_replace;
417 SVX_ASSERT(pfx_replace);
418 SVX_ASSERT(pfx_with);
419 pos_replace = pfx_replace->pos;
420 SVX_ASSERT(pos_replace != pfx_with->pos);
422 replace_pfx_(pfx_replace->stn, NULL, pos_replace, pfx_with->pos);
424 #if DEBUG_INVALID
426 node *stn;
427 FOR_EACH_STN(stn, stnlist) {
428 SVX_ASSERT(stn->name->pos != pos_replace);
431 #endif
433 /* free the (now-unused) old pos */
434 osfree(pos_replace);
437 /* Add an equating leg between existing stations *fr and *to (whose names are
438 * name1 and name2).
440 void
441 process_equate(prefix *name1, prefix *name2)
443 node *stn1, *stn2;
444 clear_last_leg();
445 if (name1 == name2) {
446 /* catch something like *equate "fred fred" */
447 /* TRANSLATORS: Here "station" is a survey station, not a train station.
449 compile_diagnostic(DIAG_WARN, /*Station “%s” equated to itself*/13,
450 sprint_prefix(name1));
451 return;
453 stn1 = StnFromPfx(name1);
454 stn2 = StnFromPfx(name2);
455 /* equate nodes if not already equated */
456 if (name1->pos != name2->pos) {
457 if (pfx_fixed(name1)) {
458 if (pfx_fixed(name2)) {
459 /* both are fixed, but let them off iff their coordinates match */
460 char *s = osstrdup(sprint_prefix(name1));
461 int d;
462 for (d = 2; d >= 0; d--) {
463 if (name1->pos->p[d] != name2->pos->p[d]) {
464 compile_diagnostic(DIAG_ERR, /*Tried to equate two non-equal fixed stations: “%s” and “%s”*/52,
465 s, sprint_prefix(name2));
466 osfree(s);
467 return;
470 /* TRANSLATORS: "equal" as in:
472 * *fix a 1 2 3
473 * *fix b 1 2 3
474 * *equate a b */
475 compile_diagnostic(DIAG_WARN, /*Equating two equal fixed points: “%s” and “%s”*/53,
476 s, sprint_prefix(name2));
477 osfree(s);
480 /* name1 is fixed, so replace all refs to name2's pos with name1's */
481 replace_pfx(name2, name1);
482 } else {
483 /* name1 isn't fixed, so replace all refs to its pos with name2's */
484 replace_pfx(name1, name2);
487 /* count equates as legs for now... */
488 cLegs++;
489 addleg_(stn1, stn2,
490 (real)0.0, (real)0.0, (real)0.0,
491 (real)0.0, (real)0.0, (real)0.0,
492 #ifndef NO_COVARIANCES
493 (real)0.0, (real)0.0, (real)0.0,
494 #endif
495 FLAG_FAKE);
499 /* Add a 'fake' leg (not counted) between existing stations *fr and *to
500 * (which *must* be different)
501 * If either node is a three node, then it is split into two
502 * and the data structure adjusted as necessary
504 void
505 addfakeleg(node *fr, node *to,
506 real dx, real dy, real dz,
507 real vx, real vy, real vz
508 #ifndef NO_COVARIANCES
509 , real cyz, real czx, real cxy
510 #endif
513 clear_last_leg();
514 addleg_(fr, to, dx, dy, dz, vx, vy, vz,
515 #ifndef NO_COVARIANCES
516 cyz, czx, cxy,
517 #endif
518 FLAG_FAKE);
521 static char
522 freeleg(node **stnptr)
524 node *stn, *oldstn;
525 linkfor *leg, *leg2;
526 #ifndef NO_COVARIANCES
527 int i;
528 #endif
530 stn = *stnptr;
532 if (stn->leg[0] == NULL) return 0; /* leg[0] unused */
533 if (stn->leg[1] == NULL) return 1; /* leg[1] unused */
534 if (stn->leg[2] == NULL) return 2; /* leg[2] unused */
536 /* All legs used, so split node in two */
537 oldstn = stn;
538 stn = osnew(node);
539 leg = osnew(linkfor);
540 leg2 = (linkfor*)osnew(linkrev);
542 *stnptr = stn;
544 add_stn_to_list(&stnlist, stn);
545 stn->name = oldstn->name;
547 leg->l.to = stn;
548 leg->d[0] = leg->d[1] = leg->d[2] = (real)0.0;
550 #ifndef NO_COVARIANCES
551 for (i = 0; i < 6; i++) leg->v[i] = (real)0.0;
552 #else
553 leg->v[0] = leg->v[1] = leg->v[2] = (real)0.0;
554 #endif
555 leg->l.reverse = 1 | FLAG_DATAHERE | FLAG_FAKE;
556 leg->l.flags = pcs->flags | (pcs->style << FLAGS_STYLE_BIT0);
558 leg2->l.to = oldstn;
559 leg2->l.reverse = 0;
561 /* NB this preserves pos->stn->leg[0] to point to the "real" fixed point
562 * for stations fixed with error estimates
564 stn->leg[0] = oldstn->leg[0];
565 /* correct reverse leg */
566 reverse_leg(stn->leg[0])->l.to = stn;
567 stn->leg[1] = leg2;
569 oldstn->leg[0] = leg;
571 stn->leg[2] = NULL; /* needed as stn->leg[dirn]==NULL indicates unused */
573 return(2); /* leg[2] unused */
576 node *
577 StnFromPfx(prefix *name)
579 node *stn;
580 if (name->stn != NULL) return (name->stn);
581 stn = osnew(node);
582 stn->name = name;
583 if (name->pos == NULL) {
584 name->pos = osnew(pos);
585 unfix(stn);
587 stn->leg[0] = stn->leg[1] = stn->leg[2] = NULL;
588 add_stn_to_list(&stnlist, stn);
589 name->stn = stn;
590 cStns++;
591 return stn;
594 extern void
595 fprint_prefix(FILE *fh, const prefix *ptr)
597 SVX_ASSERT(ptr);
598 if (TSTBIT(ptr->sflags, SFLAGS_ANON)) {
599 /* We release the stations, so ptr->stn is NULL late on, so we can't
600 * use that to print "anonymous station surveyed from somesurvey.12"
601 * here. FIXME */
602 fputs("anonymous station", fh);
603 /* FIXME: if ident is set, show it? */
604 return;
606 if (ptr->up != NULL) {
607 fprint_prefix(fh, ptr->up);
608 if (ptr->up->up != NULL) fputc('.', fh);
609 SVX_ASSERT(ptr->ident);
610 fputs(ptr->ident, fh);
614 static char *buffer = NULL;
615 static OSSIZE_T buffer_len = 256;
617 static OSSIZE_T
618 sprint_prefix_(const prefix *ptr)
620 OSSIZE_T len = 1;
621 if (ptr->up != NULL) {
622 SVX_ASSERT(ptr->ident);
623 len = sprint_prefix_(ptr->up) + strlen(ptr->ident);
624 if (ptr->up->up != NULL) len++;
625 if (len > buffer_len) {
626 buffer = osrealloc(buffer, len);
627 buffer_len = len;
629 if (ptr->up->up != NULL) strcat(buffer, ".");
630 strcat(buffer, ptr->ident);
632 return len;
635 extern char *
636 sprint_prefix(const prefix *ptr)
638 SVX_ASSERT(ptr);
639 if (!buffer) buffer = osmalloc(buffer_len);
640 if (TSTBIT(ptr->sflags, SFLAGS_ANON)) {
641 /* We release the stations, so ptr->stn is NULL late on, so we can't
642 * use that to print "anonymous station surveyed from somesurvey.12"
643 * here. FIXME */
644 sprintf(buffer, "anonymous station");
645 /* FIXME: if ident is set, show it? */
646 return buffer;
648 *buffer = '\0';
649 sprint_prefix_(ptr);
650 return buffer;
653 /* r = ab ; r,a,b are variance matrices */
654 void
655 mulss(var *r, /*const*/ svar *a, /*const*/ svar *b)
657 #ifdef NO_COVARIANCES
658 /* variance-only version */
659 (*r)[0] = (*a)[0] * (*b)[0];
660 (*r)[1] = (*a)[1] * (*b)[1];
661 (*r)[2] = (*a)[2] * (*b)[2];
662 #else
663 int i, j, k;
664 real tot;
666 #if 0
667 SVX_ASSERT((/*const*/ var *)r != a);
668 SVX_ASSERT((/*const*/ var *)r != b);
669 #endif
671 check_svar(a);
672 check_svar(b);
674 for (i = 0; i < 3; i++) {
675 for (j = 0; j < 3; j++) {
676 tot = 0;
677 for (k = 0; k < 3; k++) {
678 tot += SN(a,i,k) * SN(b,k,j);
680 (*r)[i][j] = tot;
683 check_var(r);
684 #endif
687 #ifndef NO_COVARIANCES
688 /* r = ab ; r,a,b are variance matrices */
689 void
690 smulvs(svar *r, /*const*/ var *a, /*const*/ svar *b)
692 int i, j, k;
693 real tot;
695 #if 0
696 SVX_ASSERT((/*const*/ var *)r != a);
697 #endif
698 SVX_ASSERT((/*const*/ svar *)r != b);
700 check_var(a);
701 check_svar(b);
703 (*r)[3]=(*r)[4]=(*r)[5]=-999;
704 for (i = 0; i < 3; i++) {
705 for (j = 0; j < 3; j++) {
706 tot = 0;
707 for (k = 0; k < 3; k++) {
708 tot += (*a)[i][k] * SN(b,k,j);
710 if (i <= j)
711 SN(r,i,j) = tot;
712 else if (fabs(SN(r,j,i) - tot) > THRESHOLD) {
713 printf("not sym - %d,%d = %f, %d,%d was %f\n",
714 i,j,tot,j,i,SN(r,j,i));
715 BUG("smulvs didn't produce a sym mx\n");
719 check_svar(r);
721 #endif
723 /* r = vb ; r,b delta vectors; a variance matrix */
724 void
725 mulsd(delta *r, /*const*/ svar *v, /*const*/ delta *b)
727 #ifdef NO_COVARIANCES
728 /* variance-only version */
729 (*r)[0] = (*v)[0] * (*b)[0];
730 (*r)[1] = (*v)[1] * (*b)[1];
731 (*r)[2] = (*v)[2] * (*b)[2];
732 #else
733 int i, j;
734 real tot;
736 SVX_ASSERT((/*const*/ delta*)r != b);
737 check_svar(v);
738 check_d(b);
740 for (i = 0; i < 3; i++) {
741 tot = 0;
742 for (j = 0; j < 3; j++) tot += S(i,j) * (*b)[j];
743 (*r)[i] = tot;
745 check_d(r);
746 #endif
749 /* r = ca ; r,a variance matrices; c real scaling factor */
750 void
751 mulsc(svar *r, /*const*/ svar *a, real c)
753 #ifdef NO_COVARIANCES
754 /* variance-only version */
755 (*r)[0] = (*a)[0] * c;
756 (*r)[1] = (*a)[1] * c;
757 (*r)[2] = (*a)[2] * c;
758 #else
759 int i;
761 check_svar(a);
762 for (i = 0; i < 6; i++) (*r)[i] = (*a)[i] * c;
763 check_svar(r);
764 #endif
767 /* r = a + b ; r,a,b delta vectors */
768 void
769 adddd(delta *r, /*const*/ delta *a, /*const*/ delta *b)
771 check_d(a);
772 check_d(b);
773 (*r)[0] = (*a)[0] + (*b)[0];
774 (*r)[1] = (*a)[1] + (*b)[1];
775 (*r)[2] = (*a)[2] + (*b)[2];
776 check_d(r);
779 /* r = a - b ; r,a,b delta vectors */
780 void
781 subdd(delta *r, /*const*/ delta *a, /*const*/ delta *b) {
782 check_d(a);
783 check_d(b);
784 (*r)[0] = (*a)[0] - (*b)[0];
785 (*r)[1] = (*a)[1] - (*b)[1];
786 (*r)[2] = (*a)[2] - (*b)[2];
787 check_d(r);
790 /* r = a + b ; r,a,b variance matrices */
791 void
792 addss(svar *r, /*const*/ svar *a, /*const*/ svar *b)
794 #ifdef NO_COVARIANCES
795 /* variance-only version */
796 (*r)[0] = (*a)[0] + (*b)[0];
797 (*r)[1] = (*a)[1] + (*b)[1];
798 (*r)[2] = (*a)[2] + (*b)[2];
799 #else
800 int i;
802 check_svar(a);
803 check_svar(b);
804 for (i = 0; i < 6; i++) (*r)[i] = (*a)[i] + (*b)[i];
805 check_svar(r);
806 #endif
809 /* r = a - b ; r,a,b variance matrices */
810 void
811 subss(svar *r, /*const*/ svar *a, /*const*/ svar *b)
813 #ifdef NO_COVARIANCES
814 /* variance-only version */
815 (*r)[0] = (*a)[0] - (*b)[0];
816 (*r)[1] = (*a)[1] - (*b)[1];
817 (*r)[2] = (*a)[2] - (*b)[2];
818 #else
819 int i;
821 check_svar(a);
822 check_svar(b);
823 for (i = 0; i < 6; i++) (*r)[i] = (*a)[i] - (*b)[i];
824 check_svar(r);
825 #endif
828 /* inv = v^-1 ; inv,v variance matrices */
829 extern int
830 invert_svar(svar *inv, /*const*/ svar *v)
832 #ifdef NO_COVARIANCES
833 int i;
834 for (i = 0; i < 3; i++) {
835 if ((*v)[i] == 0.0) return 0; /* matrix is singular */
836 (*inv)[i] = 1.0 / (*v)[i];
838 #else
839 real det, a, b, c, d, e, f, bcff, efcd, dfbe;
841 #if 0
842 SVX_ASSERT((/*const*/ var *)inv != v);
843 #endif
845 check_svar(v);
846 /* a d e
847 * d b f
848 * e f c
850 a = (*v)[0], b = (*v)[1], c = (*v)[2];
851 d = (*v)[3], e = (*v)[4], f = (*v)[5];
852 bcff = b * c - f * f;
853 efcd = e * f - c * d;
854 dfbe = d * f - b * e;
855 det = a * bcff + d * efcd + e * dfbe;
857 if (det == 0.0) {
858 /* printf("det=%.20f\n", det); */
859 return 0; /* matrix is singular */
862 det = 1 / det;
864 (*inv)[0] = det * bcff;
865 (*inv)[1] = det * (c * a - e * e);
866 (*inv)[2] = det * (a * b - d * d);
867 (*inv)[3] = det * efcd;
868 (*inv)[4] = det * dfbe;
869 (*inv)[5] = det * (e * d - a * f);
871 #if 0
872 /* This test fires very occasionally, and there's not much point in
873 * it anyhow - the matrix inversion algorithm is simple enough that
874 * we can be confident it's correctly implemented, so we might as
875 * well save the cycles and not perform this check.
877 { /* check that original * inverse = identity matrix */
878 int i;
879 var p;
880 real D = 0;
881 mulss(&p, v, inv);
882 for (i = 0; i < 3; i++) {
883 int j;
884 for (j = 0; j < 3; j++) D += fabs(p[i][j] - (real)(i==j));
886 if (D > THRESHOLD) {
887 printf("original * inverse=\n");
888 print_svar(*v);
889 printf("*\n");
890 print_svar(*inv);
891 printf("=\n");
892 print_var(p);
893 BUG("matrix didn't invert");
895 check_svar(inv);
897 #endif
898 #endif
899 return 1;
902 /* r = (b^-1)a ; r,a delta vectors; b variance matrix */
903 #ifndef NO_COVARIANCES
904 void
905 divds(delta *r, /*const*/ delta *a, /*const*/ svar *b)
907 #ifdef NO_COVARIANCES
908 /* variance-only version */
909 (*r)[0] = (*a)[0] / (*b)[0];
910 (*r)[1] = (*a)[1] / (*b)[1];
911 (*r)[2] = (*a)[2] / (*b)[2];
912 #else
913 svar b_inv;
914 if (!invert_svar(&b_inv, b)) {
915 print_svar(*b);
916 BUG("covariance matrix is singular");
918 mulsd(r, &b_inv, a);
919 #endif
921 #endif
923 bool
924 fZeros(/*const*/ svar *v) {
925 #ifdef NO_COVARIANCES
926 /* variance-only version */
927 return ((*v)[0] == 0.0 && (*v)[1] == 0.0 && (*v)[2] == 0.0);
928 #else
929 int i;
931 check_svar(v);
932 for (i = 0; i < 6; i++) if ((*v)[i] != 0.0) return fFalse;
934 return fTrue;
935 #endif