← Back to branch summary

~ojwb/survex/master

« back to all changes in this revision

Viewing changes to src/netbits.c

  • Committer: Olly Betts
  • Date: 2010-06-18 07:16:42 UTC
  • mfrom: (2003.1.853)
  • Revision ID: git-v1:75fe355c16c77b1090c4426a3dc0b8dabca31904
Rename branches/survex-1_1 to trunk.

git-svn-id: file:///home/survex-svn/survex/trunk@3454 4b37db11-9a0c-4f06-9ece-9ab7cdaee568

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/netbits.c
1
1
/* netbits.c
2
2
 * Miscellaneous primitive network routines for Survex
3
 
 * Copyright (C) 1992-2002 Olly Betts
 
3
 * Copyright (C) 1992-2003,2006 Olly Betts
4
4
 *
5
5
 * This program is free software; you can redistribute it and/or modify
6
6
 * it under the terms of the GNU General Public License as published by
14
14
 *
15
15
 * You should have received a copy of the GNU General Public License
16
16
 * along with this program; if not, write to the Free Software
17
 
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
17
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
18
18
 */
19
19
 
20
20
#ifdef HAVE_CONFIG_H
33
33
#include "datain.h" /* for compile_error */
34
34
#include "validate.h" /* for compile_error */
35
35
 
36
 
#ifdef CHASM3DX
37
 
#include <stddef.h> /* for offsetof */
38
 
#endif
39
 
 
40
36
#define THRESHOLD (REAL_EPSILON * 1000) /* 100 was too small */
41
37
 
42
38
node *stn_iter = NULL; /* for FOR_EACH_STN */
43
39
 
 
40
static char freeleg(node **stnptr);
 
41
 
44
42
#ifdef NO_COVARIANCES
45
43
static void check_var(/*const*/ var *v) {
46
44
   int bad = 0;
238
236
#else
239
237
   memcpy(legOut->v, leg->v, sizeof(svar));
240
238
#endif
 
239
   legOut->meta = pcs->meta;
 
240
   if (pcs->meta) ++pcs->meta->ref_count;
241
241
   return legOut;
242
242
}
243
243
 
302
302
   leg->l.reverse = j | FLAG_DATAHERE | leg_flags;
303
303
 
304
304
   leg->l.flags = pcs->flags;
 
305
   leg->meta = pcs->meta;
 
306
   if (pcs->meta) ++pcs->meta->ref_count;
305
307
 
306
308
   fr->leg[i] = leg;
307
309
   to->leg[j] = leg2;
468
470
           FLAG_FAKE);
469
471
}
470
472
 
471
 
char
 
473
static char
472
474
freeleg(node **stnptr)
473
475
{
474
476
   node *stn, *oldstn;
531
533
   stn = osnew(node);
532
534
   stn->name = name;
533
535
   if (name->pos == NULL) {
534
 
#ifdef CHASM3DX
535
 
      if (fUseNewFormat) {
536
 
         name->pos = osnew(pos);
537
 
         name->pos->id = 0;
538
 
      } else {
539
 
         /* only allocate the part of the structure we need... */
540
 
         name->pos = (pos *)osmalloc(offsetof(pos, id));
541
 
      }
542
 
#else
543
536
      name->pos = osnew(pos);
544
 
#endif
545
537
      unfix(stn);
546
538
   }
547
539
   stn->leg[0] = stn->leg[1] = stn->leg[2] = NULL;
596
588
 
597
589
/* r = ab ; r,a,b are variance matrices */
598
590
void
599
 
mulvv(var *r, /*const*/ var *a, /*const*/ var *b)
600
 
{
601
 
#ifdef NO_COVARIANCES
602
 
   /* variance-only version */
603
 
   (*r)[0] = (*a)[0] * (*b)[0];
604
 
   (*r)[1] = (*a)[1] * (*b)[1];
605
 
   (*r)[2] = (*a)[2] * (*b)[2];
606
 
#else
607
 
   int i, j, k;
608
 
   real tot;
609
 
 
610
 
   SVX_ASSERT((/*const*/ var *)r != a);
611
 
   SVX_ASSERT((/*const*/ var *)r != b);
612
 
 
613
 
   check_var(a);
614
 
   check_var(b);
615
 
 
616
 
   for (i = 0; i < 3; i++) {
617
 
      for (j = 0; j < 3; j++) {
618
 
         tot = 0;
619
 
         for (k = 0; k < 3; k++) {
620
 
            tot += (*a)[i][k] * (*b)[k][j];
621
 
         }
622
 
         (*r)[i][j] = tot;
623
 
      }
624
 
   }
625
 
   check_var(r);
626
 
#endif
627
 
}
628
 
 
629
 
/* r = ab ; r,a,b are variance matrices */
630
 
void
631
591
mulss(var *r, /*const*/ svar *a, /*const*/ svar *b)
632
592
{
633
593
#ifdef NO_COVARIANCES
660
620
#endif
661
621
}
662
622
 
 
623
#ifndef NO_COVARIANCES
663
624
/* r = ab ; r,a,b are variance matrices */
664
625
void
665
626
smulvs(svar *r, /*const*/ var *a, /*const*/ svar *b)
666
627
{
667
 
#ifdef NO_COVARIANCES
668
 
   /* variance-only version */
669
 
   (*r)[0] = (*a)[0] * (*b)[0];
670
 
   (*r)[1] = (*a)[1] * (*b)[1];
671
 
   (*r)[2] = (*a)[2] * (*b)[2];
672
 
#else
673
628
   int i, j, k;
674
629
   real tot;
675
630
 
698
653
      }
699
654
   }
700
655
   check_svar(r);
701
 
#endif
702
 
}
703
 
 
704
 
/* r = ab ; r,b delta vectors; a variance matrix */
705
 
void
706
 
mulvd(delta *r, /*const*/ var *a, /*const*/ delta *b)
707
 
{
708
 
#ifdef NO_COVARIANCES
709
 
   /* variance-only version */
710
 
   (*r)[0] = (*a)[0] * (*b)[0];
711
 
   (*r)[1] = (*a)[1] * (*b)[1];
712
 
   (*r)[2] = (*a)[2] * (*b)[2];
713
 
#else
714
 
   int i, k;
715
 
   real tot;
716
 
 
717
 
   SVX_ASSERT((/*const*/ delta*)r != b);
718
 
   check_var(a);
719
 
   check_d(b);
720
 
 
721
 
   for (i = 0; i < 3; i++) {
722
 
      tot = 0;
723
 
      for (k = 0; k < 3; k++) tot += (*a)[i][k] * (*b)[k];
724
 
      (*r)[i] = tot;
725
 
   }
726
 
   check_d(r);
727
 
#endif
728
 
}
 
656
}
 
657
#endif
729
658
 
730
659
/* r = vb ; r,b delta vectors; a variance matrix */
731
660
void
753
682
#endif
754
683
}
755
684
 
756
 
/* r = ca ; r,a delta vectors; c real scaling factor  */
757
 
void
758
 
muldc(delta *r, /*const*/ delta *a, real c) {
759
 
   check_d(a);
760
 
   (*r)[0] = (*a)[0] * c;
761
 
   (*r)[1] = (*a)[1] * c;
762
 
   (*r)[2] = (*a)[2] * c;
763
 
   check_d(r);
764
 
}
765
 
 
766
 
/* r = ca ; r,a variance matrices; c real scaling factor  */
767
 
void
768
 
mulvc(var *r, /*const*/ var *a, real c)
769
 
{
770
 
#ifdef NO_COVARIANCES
771
 
   /* variance-only version */
772
 
   (*r)[0] = (*a)[0] * c;
773
 
   (*r)[1] = (*a)[1] * c;
774
 
   (*r)[2] = (*a)[2] * c;
775
 
#else
776
 
   int i, j;
777
 
 
778
 
   check_var(a);
779
 
   for (i = 0; i < 3; i++) {
780
 
      for (j = 0; j < 3; j++) (*r)[i][j] = (*a)[i][j] * c;
781
 
   }
782
 
   check_var(r);
783
 
#endif
784
 
}
785
 
 
786
685
/* r = ca ; r,a variance matrices; c real scaling factor  */
787
686
void
788
687
mulsc(svar *r, /*const*/ svar *a, real c)
826
725
 
827
726
/* r = a + b ; r,a,b variance matrices */
828
727
void
829
 
addvv(var *r, /*const*/ var *a, /*const*/ var *b)
830
 
{
831
 
#ifdef NO_COVARIANCES
832
 
   /* variance-only version */
833
 
   (*r)[0] = (*a)[0] + (*b)[0];
834
 
   (*r)[1] = (*a)[1] + (*b)[1];
835
 
   (*r)[2] = (*a)[2] + (*b)[2];
836
 
#else
837
 
   int i, j;
838
 
 
839
 
   check_var(a);
840
 
   check_var(b);
841
 
   for (i = 0; i < 3; i++) {
842
 
      for (j = 0; j < 3; j++) (*r)[i][j] = (*a)[i][j] + (*b)[i][j];
843
 
   }
844
 
   check_var(r);
845
 
#endif
846
 
}
847
 
 
848
 
/* r = a + b ; r,a,b variance matrices */
849
 
void
850
728
addss(svar *r, /*const*/ svar *a, /*const*/ svar *b)
851
729
{
852
730
#ifdef NO_COVARIANCES
866
744
 
867
745
/* r = a - b ; r,a,b variance matrices */
868
746
void
869
 
subvv(var *r, /*const*/ var *a, /*const*/ var *b)
870
 
{
871
 
#ifdef NO_COVARIANCES
872
 
   /* variance-only version */
873
 
   (*r)[0] = (*a)[0] - (*b)[0];
874
 
   (*r)[1] = (*a)[1] - (*b)[1];
875
 
   (*r)[2] = (*a)[2] - (*b)[2];
876
 
#else
877
 
   int i, j;
878
 
 
879
 
   check_var(a);
880
 
   check_var(b);
881
 
   for (i = 0; i < 3; i++) {
882
 
      for (j = 0; j < 3; j++) (*r)[i][j] = (*a)[i][j] - (*b)[i][j];
883
 
   }
884
 
   check_var(r);
885
 
#endif
886
 
}
887
 
 
888
 
/* r = a - b ; r,a,b variance matrices */
889
 
void
890
747
subss(svar *r, /*const*/ svar *a, /*const*/ svar *b)
891
748
{
892
749
#ifdef NO_COVARIANCES
905
762
}
906
763
 
907
764
/* inv = v^-1 ; inv,v variance matrices */
908
 
#ifdef NO_COVARIANCES
909
765
extern int
910
 
invert_var(var *inv, /*const*/ var *v)
 
766
invert_svar(svar *inv, /*const*/ svar *v)
911
767
{
 
768
#ifdef NO_COVARIANCES
912
769
   int i;
913
770
   for (i = 0; i < 3; i++) {
914
771
      if ((*v)[i] == 0.0) return 0; /* matrix is singular */
915
772
      (*inv)[i] = 1.0 / (*v)[i];
916
773
   }
917
 
   return 1;
918
 
}
919
774
#else
920
 
extern int
921
 
invert_var(var *inv, /*const*/ var *v)
922
 
{
923
 
   int i, j;
924
 
   real det = 0;
925
 
 
926
 
   SVX_ASSERT((/*const*/ var *)inv != v);
927
 
 
928
 
   check_var(v);
929
 
   for (i = 0; i < 3; i++) {
930
 
      det += V(i, 0) * (V((i + 1) % 3, 1) * V((i + 2) % 3, 2) -
931
 
                        V((i + 1) % 3, 2) * V((i + 2) % 3, 1));
932
 
   }
933
 
 
934
 
   if (det == 0.0) {
935
 
      /* printf("det=%.20f\n", det); */
936
 
      return 0; /* matrix is singular */
937
 
   }
938
 
 
939
 
   det = 1 / det;
940
 
 
941
 
#define B(I,J) ((*v)[(J)%3][(I)%3])
942
 
   for (i = 0; i < 3; i++) {
943
 
      for (j = 0; j < 3; j++) {
944
 
         (*inv)[i][j] = det * (B(i+1,j+1)*B(i+2,j+2) - B(i+2,j+1)*B(i+1,j+2));
945
 
      }
946
 
   }
947
 
#undef B
948
 
 
949
 
#if 0
950
 
   /* This test fires very occasionally, and there's not much point in
951
 
    * it anyhow - the matrix inversion algorithm is simple enough that
952
 
    * we can be confident it's correctly implemented, so we might as
953
 
    * well save the cycles and not perform this check.
954
 
    */
955
 
     { /* check that original * inverse = identity matrix */
956
 
        var p;
957
 
        real d = 0;
958
 
        mulvv(&p, v, inv);
959
 
        for (i = 0; i < 3; i++) {
960
 
           for (j = 0; j < 3; j++) d += fabs(p[i][j] - (real)(i==j));
961
 
        }
962
 
        if (d > THRESHOLD) {
963
 
           printf("original * inverse=\n");
964
 
           print_var(*v);
965
 
           printf("*\n");
966
 
           print_var(*inv);
967
 
           printf("=\n");
968
 
           print_var(p);
969
 
           BUG("matrix didn't invert");
970
 
        }
971
 
        check_var(inv);
972
 
     }
973
 
#endif
974
 
 
975
 
   return 1;
976
 
}
977
 
#endif
978
 
 
979
 
/* inv = v^-1 ; inv,v variance matrices */
980
 
#ifndef NO_COVARIANCES
981
 
extern int
982
 
invert_svar(svar *inv, /*const*/ svar *v)
983
 
{
984
775
   real det, a, b, c, d, e, f, bcff, efcd, dfbe;
985
776
 
986
777
#if 0
1040
831
        check_svar(inv);
1041
832
     }
1042
833
#endif
 
834
#endif
1043
835
   return 1;
1044
836
}
1045
 
#endif
1046
 
 
1047
 
/* r = (b^-1)a ; r,a delta vectors; b variance matrix */
1048
 
void
1049
 
divdv(delta *r, /*const*/ delta *a, /*const*/ var *b)
1050
 
{
1051
 
#ifdef NO_COVARIANCES
1052
 
   /* variance-only version */
1053
 
   (*r)[0] = (*a)[0] / (*b)[0];
1054
 
   (*r)[1] = (*a)[1] / (*b)[1];
1055
 
   (*r)[2] = (*a)[2] / (*b)[2];
1056
 
#else
1057
 
   var b_inv;
1058
 
   if (!invert_var(&b_inv, b)) {
1059
 
      print_var(*b);
1060
 
      BUG("covariance matrix is singular");
1061
 
   }
1062
 
   mulvd(r, &b_inv, a);
1063
 
#endif
1064
 
}
1065
837
 
1066
838
/* r = (b^-1)a ; r,a delta vectors; b variance matrix */
1067
839
#ifndef NO_COVARIANCES
1068
840
void
1069
841
divds(delta *r, /*const*/ delta *a, /*const*/ svar *b)
1070
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
1071
849
   svar b_inv;
1072
850
   if (!invert_svar(&b_inv, b)) {
1073
851
      print_svar(*b);
1074
852
      BUG("covariance matrix is singular");
1075
853
   }
1076
854
   mulsd(r, &b_inv, a);
1077
 
}
1078
 
#endif
1079
 
 
1080
 
/* f = a(b^-1) ; r,a,b variance matrices */
1081
 
void
1082
 
divvv(var *r, /*const*/ var *a, /*const*/ var *b)
1083
 
{
1084
 
#ifdef NO_COVARIANCES
1085
 
   /* variance-only version */
1086
 
   (*r)[0] = (*a)[0] / (*b)[0];
1087
 
   (*r)[1] = (*a)[1] / (*b)[1];
1088
 
   (*r)[2] = (*a)[2] / (*b)[2];
1089
 
#else
1090
 
   var b_inv;
1091
 
   check_var(a);
1092
 
   check_var(b);
1093
 
   if (!invert_var(&b_inv, b)) {
1094
 
      print_var(*b);
1095
 
      BUG("covariance matrix is singular");
1096
 
   }
1097
 
   mulvv(r, a, &b_inv);
1098
 
   check_var(r);
1099
 
#endif
1100
 
}
 
855
#endif
 
856
}
 
857
#endif
1101
858
 
1102
859
bool
1103
860
fZeros(/*const*/ svar *v) {