source: git/src/netbits.c @ ecbc6c18

RELEASE/1.1RELEASE/1.2debug-cidebug-ci-sanitisersstereowalls-data
Last change on this file since ecbc6c18 was ecbc6c18, checked in by Olly Betts <olly@…>, 14 years ago

src/: Update FSF address in licence notices.

git-svn-id: file:///home/survex-svn/survex/branches/survex-1_1@3417 4b37db11-9a0c-4f06-9ece-9ab7cdaee568

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