src/netbits.c

   1 /* netbits.c
   2  * Miscellaneous primitive network routines for Survex
   3  * Copyright (C) 1992-2003,2006,2011,2013,2014,2015 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
  38 node *stn_iter = NULL; /* for FOR_EACH_STN */
  39
  40 static struct {
  41    prefix * to_name;
  42    prefix * fr_name;
  43    linkfor * leg;
  44    int n;
  45 } last_leg = { NULL, NULL, NULL, 0 };
  46
  47 void clear_last_leg(void) {
  48    last_leg.to_name = NULL;
  49 }
  50
  51 static char freeleg(node **stnptr);
  52
  53 #ifdef NO_COVARIANCES
  54 static void check_var(/*const*/ var *v) {
  55    int bad = 0;
  56    int i;
  57
  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;
  63    }
  64    if (bad) print_var(v);
  65    return;
  66 }
  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
  76
  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;
  84       }
  85    }
  86    if (!ok) return; /* ignore all-zero matrices */
  87
  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));
  92    }
  93
  94    if (fabs(det) < THRESHOLD)
  95       printf("*** Singular!!!\n"), bad = 1;
  96 #endif
  97
  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);
 112 }
 113
 114 #define SN(V,A,B) ((*(V))[(A)==(B)?(A):2+(A)+(B)])
 115 #define S(A,B) SN(v,A,B)
 116
 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
 124
 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;
 131    }
 132    if (!ok) return; /* ignore all-zero matrices */
 133
 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));
 138    }
 139
 140    if (fabs(det) < THRESHOLD)
 141       printf("*** Singular!!!\n"), bad = 1;
 142 #endif
 143
 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);
 154 }
 155 #endif
 156
 157 static void check_d(/*const*/ delta *d) {
 158    int bad = 0;
 159    int i;
 160
 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;
 166    }
 167
 168    if (bad) printf("(%4.2f,%4.2f,%4.2f)\n", (*d)[0], (*d)[1], (*d)[2]);
 169 }
 170
 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;
 186 }
 187
 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
 199      {
 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;
 206         }
 207      }
 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;
 218    }
 219 }
 220
 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
 223  */
 224 linkfor *
 225 copy_link(linkfor *leg)
 226 {
 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];
 236    }
 237 #if 1
 238 # ifndef NO_COVARIANCES
 239    check_svar(&(leg->v));
 240      {
 241         int i;
 242         for (i = 0; i < 6; i++) legOut->v[i] = leg->v[i];
 243      }
 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;
 253 }
 254
 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
 257  */
 258 linkfor *
 259 addto_link(linkfor *leg, const linkfor *leg2)
 260 {
 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);
 267    }
 268    addss(&leg->v, &leg->v, &((linkfor *)leg2)->v);
 269    return leg;
 270 }
 271
 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)
 280 {
 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);
 286
 287    leg = osnew(linkfor);
 288    leg2 = (linkfor*)osnew(linkrev);
 289
 290    i = freeleg(&fr);
 291    j = freeleg(&to);
 292
 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;
 313
 314    leg->l.flags = pcs->flags | (pcs->style << FLAGS_STYLE_BIT0);
 315    leg->meta = pcs->meta;
 316    if (pcs->meta) ++pcs->meta->ref_count;
 317
 318    fr->leg[i] = leg;
 319    to->leg[j] = leg2;
 320
 321    ++fr->name->shape;
 322    ++to->name->shape;
 323
 324    return leg;
 325 }
 326
 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.
 330  */
 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
 338              )
 339 {
 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".
 344        *
 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;
 349    }
 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);
 356    }
 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;
 379       }
 380    }
 381    cLegs++;
 382
 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
 390                           0);
 391 }
 392
 393 /* helper function for replace_pfx */
 394 static void
 395 replace_pfx_(node *stn, node *from, pos *pos_replace, pos *pos_with)
 396 {
 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;
 405
 406       if (fZeros(data_here(leg) ? &leg->v : &reverse_leg(leg)->v))
 407          replace_pfx_(to, stn, pos_replace, pos_with);
 408    }
 409 }
 410
 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)
 415 {
 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);
 421
 422    replace_pfx_(pfx_replace->stn, NULL, pos_replace, pfx_with->pos);
 423
 424 #if DEBUG_INVALID
 425    {
 426       node *stn;
 427       FOR_EACH_STN(stn, stnlist) {
 428          SVX_ASSERT(stn->name->pos != pos_replace);
 429       }
 430    }
 431 #endif
 432
 433    /* free the (now-unused) old pos */
 434    osfree(pos_replace);
 435 }
 436
 437 /* Add an equating leg between existing stations *fr and *to (whose names are
 438  * name1 and name2).
 439  */
 440 void
 441 process_equate(prefix *name1, prefix *name2)
 442 {
 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.
 448        */
 449       compile_diagnostic(DIAG_WARN, /*Station “%s” equated to itself*/13,
 450                          sprint_prefix(name1));
 451       return;
 452    }
 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;
 468                }
 469             }
 470             /* TRANSLATORS: "equal" as in:
 471              *
 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);
 478          }
 479
 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);
 485       }
 486
 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);
 496    }
 497 }
 498
 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
 503  */
 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
 511            )
 512 {
 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);
 519 }
 520
 521 static char
 522 freeleg(node **stnptr)
 523 {
 524    node *stn, *oldstn;
 525    linkfor *leg, *leg2;
 526 #ifndef NO_COVARIANCES
 527    int i;
 528 #endif
 529
 530    stn = *stnptr;
 531
 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 */
 535
 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);
 541
 542    *stnptr = stn;
 543
 544    add_stn_to_list(&stnlist, stn);
 545    stn->name = oldstn->name;
 546
 547    leg->l.to = stn;
 548    leg->d[0] = leg->d[1] = leg->d[2] = (real)0.0;
 549
 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);
 557
 558    leg2->l.to = oldstn;
 559    leg2->l.reverse = 0;
 560
 561    /* NB this preserves pos->stn->leg[0] to point to the "real" fixed point
 562     * for stations fixed with error estimates
 563     */
 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;
 568
 569    oldstn->leg[0] = leg;
 570
 571    stn->leg[2] = NULL; /* needed as stn->leg[dirn]==NULL indicates unused */
 572
 573    return(2); /* leg[2] unused */
 574 }
 575
 576 node *
 577 StnFromPfx(prefix *name)
 578 {
 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);
 586    }
 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;
 592 }
 593
 594 extern void
 595 fprint_prefix(FILE *fh, const prefix *ptr)
 596 {
 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;
 605    }
 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);
 611    }
 612 }
 613
 614 static char *buffer = NULL;
 615 static OSSIZE_T buffer_len = 256;
 616
 617 static OSSIZE_T
 618 sprint_prefix_(const prefix *ptr)
 619 {
 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;
 628       }
 629       if (ptr->up->up != NULL) strcat(buffer, ".");
 630       strcat(buffer, ptr->ident);
 631    }
 632    return len;
 633 }
 634
 635 extern char *
 636 sprint_prefix(const prefix *ptr)
 637 {
 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;
 647    }
 648    *buffer = '\0';
 649    sprint_prefix_(ptr);
 650    return buffer;
 651 }
 652
 653 /* r = ab ; r,a,b are variance matrices */
 654 void
 655 mulss(var *r, /*const*/ svar *a, /*const*/ svar *b)
 656 {
 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;
 665
 666 #if 0
 667    SVX_ASSERT((/*const*/ var *)r != a);
 668    SVX_ASSERT((/*const*/ var *)r != b);
 669 #endif
 670
 671    check_svar(a);
 672    check_svar(b);
 673
 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);
 679          }
 680          (*r)[i][j] = tot;
 681       }
 682    }
 683    check_var(r);
 684 #endif
 685 }
 686
 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)
 691 {
 692    int i, j, k;
 693    real tot;
 694
 695 #if 0
 696    SVX_ASSERT((/*const*/ var *)r != a);
 697 #endif
 698    SVX_ASSERT((/*const*/ svar *)r != b);
 699
 700    check_var(a);
 701    check_svar(b);
 702
 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);
 709          }
 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");
 716          }
 717       }
 718    }
 719    check_svar(r);
 720 }
 721 #endif
 722
 723 /* r = vb ; r,b delta vectors; a variance matrix */
 724 void
 725 mulsd(delta *r, /*const*/ svar *v, /*const*/ delta *b)
 726 {
 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;
 735
 736    SVX_ASSERT((/*const*/ delta*)r != b);
 737    check_svar(v);
 738    check_d(b);
 739
 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;
 744    }
 745    check_d(r);
 746 #endif
 747 }
 748
 749 /* r = ca ; r,a variance matrices; c real scaling factor  */
 750 void
 751 mulsc(svar *r, /*const*/ svar *a, real c)
 752 {
 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;
 760
 761    check_svar(a);
 762    for (i = 0; i < 6; i++) (*r)[i] = (*a)[i] * c;
 763    check_svar(r);
 764 #endif
 765 }
 766
 767 /* r = a + b ; r,a,b delta vectors */
 768 void
 769 adddd(delta *r, /*const*/ delta *a, /*const*/ delta *b)
 770 {
 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);
 777 }
 778
 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);
 788 }
 789
 790 /* r = a + b ; r,a,b variance matrices */
 791 void
 792 addss(svar *r, /*const*/ svar *a, /*const*/ svar *b)
 793 {
 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;
 801
 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
 807 }
 808
 809 /* r = a - b ; r,a,b variance matrices */
 810 void
 811 subss(svar *r, /*const*/ svar *a, /*const*/ svar *b)
 812 {
 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;
 820
 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
 826 }
 827
 828 /* inv = v^-1 ; inv,v variance matrices */
 829 extern int
 830 invert_svar(svar *inv, /*const*/ svar *v)
 831 {
 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];
 837    }
 838 #else
 839    real det, a, b, c, d, e, f, bcff, efcd, dfbe;
 840
 841 #if 0
 842    SVX_ASSERT((/*const*/ var *)inv != v);
 843 #endif
 844
 845    check_svar(v);
 846    /* a d e
 847     * d b f
 848     * e f c
 849     */
 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;
 856
 857    if (det == 0.0) {
 858       /* printf("det=%.20f\n", det); */
 859       return 0; /* matrix is singular */
 860    }
 861
 862    det = 1 / det;
 863
 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);
 870
 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.
 876     */
 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));
 885         }
 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");
 894         }
 895         check_svar(inv);
 896      }
 897 #endif
 898 #endif
 899    return 1;
 900 }
 901
 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)
 906 {
 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");
 917    }
 918    mulsd(r, &b_inv, a);
 919 #endif
 920 }
 921 #endif
 922
 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;
 930
 931    check_svar(v);
 932    for (i = 0; i < 6; i++) if ((*v)[i] != 0.0) return fFalse;
 933
 934    return fTrue;
 935 #endif
 936 }