Calculate panel Minvs

5 files changed, 100 insertions, 9 deletions

diff --git a/libcrystfel/src/crystfel-mille.c b/libcrystfel/src/crystfel-mille.c
index 84240fc4..1bc126ed 100644
--- a/libcrystfel/src/crystfel-mille.c
+++ b/libcrystfel/src/crystfel-mille.c
@@ -75,7 +75,7 @@ int mille_label(int hierarchy_level, int member_index, enum gparam param)
 
 void write_mille(Mille *mille, int n, UnitCell *cell,
                  struct reflpeak *rps, struct image *image,
-                 gsl_matrix **panel_matrices)
+                 gsl_matrix **Minvs)
 {
 	int i;
 
@@ -93,7 +93,7 @@ void write_mille(Mille *mille, int n, UnitCell *cell,
 		for ( j=0; j<9; j++ ) {
 			fs_ss_gradient(rv[j], rps[i].refl, cell,
 			               &image->detgeom->panels[rps[i].peak->pn],
-			               panel_matrices[rps[i].peak->pn],
+			               Minvs[rps[i].peak->pn],
 			               &local_gradients_fs[j],
 			               &local_gradients_ss[j]);
 		}
@@ -105,7 +105,7 @@ void write_mille(Mille *mille, int n, UnitCell *cell,
 
 			fs_ss_gradient(GPARAM_DET_TX, rps[i].refl, cell,
 			               &image->detgeom->panels[rps[i].peak->pn],
-			               panel_matrices[rps[i].peak->pn],
+			               Minvs[rps[i].peak->pn],
 			               &global_gradients_fs[j],
 			               &global_gradients_ss[j]);
 
diff --git a/libcrystfel/src/crystfel-mille.h b/libcrystfel/src/crystfel-mille.h
index 865d453c..fc290cbb 100644
--- a/libcrystfel/src/crystfel-mille.h
+++ b/libcrystfel/src/crystfel-mille.h
@@ -52,7 +52,7 @@ extern int mille_label(int hierarchy_level, int member_index, enum gparam param)
 
 extern void write_mille(Mille *mille, int n, UnitCell *cell,
                         struct reflpeak *rps, struct image *image,
-                        gsl_matrix **panel_matrices);
+                        gsl_matrix **Minvs);
 
 extern void crystfel_mille_delete_last_record(Mille *m);
 
diff --git a/libcrystfel/src/detgeom.c b/libcrystfel/src/detgeom.c
index a8f8b079..8387dff4 100644
--- a/libcrystfel/src/detgeom.c
+++ b/libcrystfel/src/detgeom.c
@@ -29,6 +29,9 @@
 #include <libcrystfel-config.h>
 #include <math.h>
 #include <stdlib.h>
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_blas.h>
+#include <gsl/gsl_linalg.h>
 
 #include "detgeom.h"
 #include "utils.h"
@@ -225,3 +228,81 @@ void detgeom_translate_detector_m(struct detgeom *dg, double x, double y, double
 		p->cnz += z / p->pixel_pitch;
 	}
 }
+
+
+static gsl_matrix *invert(gsl_matrix *m)
+{
+	gsl_permutation *perm;
+	gsl_matrix *inv;
+	int s;
+
+	perm = gsl_permutation_alloc(m->size1);
+	if ( perm == NULL ) {
+		ERROR("Couldn't allocate permutation\n");
+		gsl_matrix_free(m);
+		return NULL;
+	}
+
+	inv = gsl_matrix_alloc(m->size1, m->size2);
+	if ( inv == NULL ) {
+		ERROR("Couldn't allocate inverse\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	if ( gsl_linalg_LU_decomp(m, perm, &s) ) {
+		ERROR("Couldn't decompose matrix\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	if ( gsl_linalg_LU_invert(m, perm, inv)  ) {
+		ERROR("Couldn't invert cell matrix:\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	gsl_permutation_free(perm);
+	gsl_matrix_free(m);
+
+	return inv;
+}
+
+
+gsl_matrix **make_panel_minvs(struct detgeom *dg)
+{
+	int i;
+	gsl_matrix **Minvs;
+
+	Minvs = malloc(dg->n_panels * sizeof(gsl_matrix *));
+	if ( Minvs == NULL ) return NULL;
+
+	for ( i=0; i<dg->n_panels; i++ ) {
+
+		struct detgeom_panel *p = &dg->panels[i];
+		gsl_matrix *M = gsl_matrix_calloc(3, 3);
+
+		gsl_matrix_set(M, 0, 0, p->cnx);
+		gsl_matrix_set(M, 0, 1, p->fsx);
+		gsl_matrix_set(M, 0, 2, p->ssx);
+		gsl_matrix_set(M, 1, 0, p->cny);
+		gsl_matrix_set(M, 1, 1, p->fsy);
+		gsl_matrix_set(M, 1, 2, p->ssy);
+		gsl_matrix_set(M, 2, 0, p->cnz);
+		gsl_matrix_set(M, 2, 1, p->fsz);
+		gsl_matrix_set(M, 2, 2, p->ssz);
+
+		Minvs[i] = invert(M);
+		if ( Minvs[i] == NULL ) {
+			ERROR("Failed to calculate inverse panel matrix for %s\n",
+			      p->name);
+			return NULL;
+		}
+
+	}
+
+	return Minvs;
+}
diff --git a/libcrystfel/src/detgeom.h b/libcrystfel/src/detgeom.h
index cf4968f8..4df67c0c 100644
--- a/libcrystfel/src/detgeom.h
+++ b/libcrystfel/src/detgeom.h
@@ -42,6 +42,7 @@ extern "C" {
  * Detector geometry structure and related functions.
  */
 
+#include <gsl/gsl_matrix.h>
 
 /**
  * Represents one panel of a detector
@@ -142,6 +143,8 @@ extern void detgeom_show_hierarchy(const struct detgeom *dg);
 
 extern void detgeom_translate_detector_m(struct detgeom *dg, double x, double y, double z);
 
+extern gsl_matrix **make_panel_minvs(struct detgeom *dg);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/libcrystfel/src/predict-refine.c b/libcrystfel/src/predict-refine.c
index 87a7ce9b..2201170b 100644
--- a/libcrystfel/src/predict-refine.c
+++ b/libcrystfel/src/predict-refine.c
@@ -471,7 +471,7 @@ int refine_radius(Crystal *cr, struct image *image)
 
 
 static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
-                   struct image *image, gsl_matrix **panel_matrices)
+                   struct image *image, gsl_matrix **Minvs)
 {
 	int i;
 	gsl_matrix *M;
@@ -516,7 +516,7 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 			                            image->lambda);
 			fs_ss_gradient(rv[k], rps[i].refl, cell,
 			               &image->detgeom->panels[rps[i].peak->pn],
-			               panel_matrices[rps[i].peak->pn],
+			               Minvs[rps[i].peak->pn],
 			               &fs_gradients[k], &ss_gradients[k]);
 
 		}
@@ -650,7 +650,7 @@ int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 	double max_I;
 	RefList *reflist;
 	char tmp[256];
-	gsl_matrix *panel_matrices[64];
+	gsl_matrix **Minvs;
 
 	rps = malloc(image_feature_count(image->features)
 	                       * sizeof(struct reflpeak));
@@ -665,6 +665,8 @@ int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 	}
 	crystal_set_reflections(cr, reflist);
 
+	Minvs = make_panel_minvs(image->detgeom);
+
 	/* Normalise the intensities to max 1 */
 	max_I = -INFINITY;
 	for ( i=0; i<n; i++ ) {
@@ -691,7 +693,7 @@ int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 	/* Refine (max 10 cycles) */
 	for ( i=0; i<10; i++ ) {
 		update_predictions(cr);
-		if ( iterate(rps, n, crystal_get_cell(cr), image, panel_matrices) )
+		if ( iterate(rps, n, crystal_get_cell(cr), image, Minvs) )
 		{
 			crystal_set_reflections(cr, NULL);
 			return 1;
@@ -709,11 +711,16 @@ int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 	#ifdef HAVE_MILLEPEDE
 	if ( mille != NULL ) {
 		profile_start("mille-calc");
-		write_mille(mille, n, crystal_get_cell(cr), rps, image, panel_matrices);
+		write_mille(mille, n, crystal_get_cell(cr), rps, image, Minvs);
 		profile_end("mille-calc");
 	}
 	#endif
 
+	for ( i=0; i<image->detgeom->n_panels; i++ ) {
+		gsl_matrix_free(Minvs[i]);
+	}
+	free(Minvs);
+
 	crystal_set_reflections(cr, NULL);
 	reflist_free(reflist);