diff options
author | Thomas White <taw@physics.org> | 2016-10-19 11:32:04 +0200 |
---|---|---|
committer | Thomas White <taw@physics.org> | 2016-10-19 11:32:04 +0200 |
commit | f3e5525294b00206ac953f1966eeb0a740829282 (patch) | |
tree | 247fd5e3f2b0878ea9e6fd3518d6e136f8394083 /libcrystfel | |
parent | 59a2678647cd5df865a72c83543a4b1bc5e46be8 (diff) |
rot_mats_are_similar
Diffstat (limited to 'libcrystfel')
-rw-r--r-- | libcrystfel/src/taketwo.c | 31 |
1 files changed, 28 insertions, 3 deletions
diff --git a/libcrystfel/src/taketwo.c b/libcrystfel/src/taketwo.c index 90a32891..e4b7c8a8 100644 --- a/libcrystfel/src/taketwo.c +++ b/libcrystfel/src/taketwo.c @@ -31,6 +31,7 @@ #include <gsl/gsl_matrix.h> #include <gsl/gsl_blas.h> #include <math.h> +#include <assert.h> #include "cell-utils.h" #include "index.h" @@ -245,12 +246,36 @@ static gsl_matrix *closest_rot_mat(struct rvec vec1, struct rvec vec2, } +static double matrix_trace(gsl_matrix *a) +{ + int i; + double tr = 0.0; + + assert(a->size1 == a->size2); + for ( i=0; i<a->size1; a++ ) { + tr += gsl_matrix_get(a, i, i); + } + return tr; +} + + static int rot_mats_are_similar(gsl_matrix *rot1, gsl_matrix *rot2) { - /* FIXME: write me! - * Write code for that fancy algorithm here from XPLOR */ + double tr; + gsl_matrix *sub; + gsl_matrix *mul; - return 0; + sub = gsl_matrix_calloc(3, 3); + gsl_matrix_memcpy(sub, rot1); + gsl_matrix_sub(sub, rot2); /* sub = rot1 - rot2 */ + + mul = gsl_matrix_calloc(3, 3); + gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, sub, sub, 0.0, mul); + + tr = matrix_trace(mul); + gsl_matrix_free(mul); + + return tr < sqrt(4.0*(1.0-cos(ANGLE_TOLERANCE)));; } |