source: git/src/netbits.c @ 6d6f4e3

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

Make from==to only a warning for Compass DAT

Compass itself seems to quietly ignore such legs.

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