diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/align_detector.c | 96 |
1 files changed, 96 insertions, 0 deletions
diff --git a/src/align_detector.c b/src/align_detector.c index e8c9e1be..ec2b16b7 100644 --- a/src/align_detector.c +++ b/src/align_detector.c @@ -78,6 +78,98 @@ static const char *group_serial_to_name(int serial, } +static struct dg_group_info *find_group(struct dg_group_info *groups, + int n_groups, const char *name) +{ + int i; + + for ( i=0; i<n_groups; i++ ) { + if ( strcmp(name, groups[i].name) == 0 ) return &groups[i]; + } + + return NULL; +} + +static int ipow(int base, int ex) +{ + int i; + int v = 1; + for ( i=0; i<ex; i++ ) { + v *= base; + } + return v; +} + + +static int is_child(struct dg_group_info *parent, struct dg_group_info *child) +{ + int parent_serial; + + if ( 1+parent->hierarchy_level != child->hierarchy_level ) return 0; + + parent_serial = child->serial % ipow(100, child->hierarchy_level); + if ( parent->serial != parent_serial ) return 0; + + return 1; +} + + +static void write_zero_sum(FILE *fh, struct dg_group_info *g, + struct dg_group_info *groups, int n_groups, + enum gparam p) +{ + int i; + int found = 0; + + for ( i=0; i<n_groups; i++ ) { + if ( is_child(g, &groups[i]) ) { + if ( !found ) { + fprintf(fh, "Constraint 0\n"); + found = 1; + } + fprintf(fh, "%i 1\n", mille_label(groups[i].serial, p)); + } + } + + if ( found ) { + fprintf(fh, "\n"); + } +} + + +static int make_zero_sum(FILE *fh, struct dg_group_info *groups, int n_groups, + const char *group_name, int level) +{ + int i; + struct dg_group_info *g = find_group(groups, n_groups, group_name); + + if ( g == NULL ) { + ERROR("Couldn't find group '%s'\n", group_name); + return 1; + } + + /* Millepede doesn't like excessive constraints */ + if ( g->hierarchy_level >= level ) return 0; + + fprintf(fh, "! Hierarchy constraints for group %s\n", group_name); + write_zero_sum(fh, g, groups, n_groups, GPARAM_DET_TX); + write_zero_sum(fh, g, groups, n_groups, GPARAM_DET_TY); + write_zero_sum(fh, g, groups, n_groups, GPARAM_DET_TZ); + write_zero_sum(fh, g, groups, n_groups, GPARAM_DET_RX); + write_zero_sum(fh, g, groups, n_groups, GPARAM_DET_RY); + write_zero_sum(fh, g, groups, n_groups, GPARAM_DET_RZ); + fprintf(fh, "\n"); + + for ( i=0; i<n_groups; i++ ) { + if ( is_child(g, &groups[i]) ) { + if ( make_zero_sum(fh, groups, n_groups, groups[i].name, level) ) return 1; + } + } + + return 0; +} + + int main(int argc, char *argv[]) { int c; @@ -184,6 +276,7 @@ int main(int argc, char *argv[]) for ( i=0; i<n_groups; i++ ) { int f = (groups[i].hierarchy_level > level) ? -1 : 0; + if ( groups[i].hierarchy_level == 0 ) continue; fprintf(fh, "%i 0 %i\n", mille_label(groups[i].serial, GPARAM_DET_TX), f); fprintf(fh, "%i 0 %i\n", mille_label(groups[i].serial, GPARAM_DET_TY), f); fprintf(fh, "%i 0 %i\n", mille_label(groups[i].serial, GPARAM_DET_TZ), f); @@ -193,6 +286,9 @@ int main(int argc, char *argv[]) } fprintf(fh, "\n"); + /* All corrections must sum to zero at each level of hierarchy */ + if ( make_zero_sum(fh, groups, n_groups, "all", level) ) return 1; + fprintf(fh, "method inversion 5 0.1\n"); fprintf(fh, "end\n"); fclose(fh); |