Separate gradients into "panel" and "physics" parts

1 files changed, 156 insertions, 81 deletions

diff --git a/libcrystfel/src/predict-refine.c b/libcrystfel/src/predict-refine.c
index 00de8bb8..0e8754ed 100644
--- a/libcrystfel/src/predict-refine.c
+++ b/libcrystfel/src/predict-refine.c
@@ -140,24 +140,18 @@ double r_gradient(int param, Reflection *refl, UnitCell *cell, double wavelength
 }
 
 
-/* Returns dfs_refl/dP and fss_refl/dP, where P = any parameter
- * fs_refl is the fast scan position of refl in panel 'p',
- * in metres (not pixels) */
-int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
-                   struct detgeom_panel *p, gsl_matrix *Minv,
-                   float *fsg, float *ssg)
+/* Spot position gradients for diffraction physics (anything that changes the
+ * diffracted ray direction) */
+int fs_ss_gradient_physics(int param, Reflection *refl, UnitCell *cell,
+                         struct detgeom_panel *p, gsl_matrix *Minv,
+                         double fs, double ss, double mu,
+                         float *fsg, float *ssg)
 {
 	signed int h, k, l;
 	double xl, yl, zl, kpred;
 	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
-	double tta, ctt, phi;
-	gsl_vector *t;
+	gsl_vector *dRdp;
 	gsl_vector *v;
-	gsl_matrix *M;
-	double mu;
-	gsl_matrix *dMdp;
-	gsl_matrix *gM;  /* M^-1 * dM/dx * M^-1 */
-	double fs, ss;
 
 	get_indices(refl, &h, &k, &l);
 	kpred = get_kpred(refl);
@@ -168,91 +162,91 @@ int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
 	yl = h*asy + k*bsy + l*csy;
 	zl = h*asz + k*bsz + l*csz;
 
-	/* Calculate 2theta (scattering angle) and azimuth (phi) */
-	tta = atan2(sqrt(xl*xl+yl*yl), kpred+zl);
-	ctt = cos(tta);
-	phi = atan2(yl, xl);
+	dRdp = gsl_vector_calloc(3);
 
-	/* Set up matrix equation */
-	M = gsl_matrix_alloc(3, 3);
-	v = gsl_vector_alloc(3);
-	t = gsl_vector_alloc(3);
-	if ( (M==NULL) || (v==NULL) || (t==NULL) ) {
-		ERROR("Failed to allocate vectors for gradient calculation\n");
-		return 1;
-	}
-
-	gsl_vector_set(t, 0, sin(tta)*cos(phi));
-	gsl_vector_set(t, 1, sin(tta)*sin(phi));
-	gsl_vector_set(t, 2, ctt);
-
-	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);
-
-	if ( gsl_linalg_HH_solve(M, t, v) ) {
-		ERROR("Failed to solve gradient equation\n");
-		return 1;
-	}
-
-	gsl_matrix_free(M);
-	mu = 1.0 / gsl_vector_get(v, 0);
-	fs = mu*gsl_vector_get(v, 1);
-	ss = mu*gsl_vector_get(v, 2);
-
-	dMdp = gsl_matrix_calloc(3, 3);
 	switch ( param ) {
 
 		case GPARAM_ASX :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_BSX :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_CSX :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_ASY :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_BSY :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_CSY :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_ASZ :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_BSZ :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
 
 		case GPARAM_CSZ :
-		*fsg = 0.0;
-		*ssg = 0.0;
-		return 0;
+		gsl_vector_set(dRdp, 0, 0.0);
+		gsl_vector_set(dRdp, 1, 0.0);
+		gsl_vector_set(dRdp, 2, 0.0);
+		break;
+
+		default :
+		ERROR("Invalid physics gradient %i\n", param);
+		return 1;
+	}
+
+	v = gsl_vector_calloc(3);
+	gsl_blas_dgemv(CblasNoTrans, 1.0, Minv, dRdp, 0.0, v);
+
+	*fsg = mu*(gsl_vector_get(v, 1) - fs*gsl_vector_get(v, 0));
+	*ssg = mu*(gsl_vector_get(v, 2) - ss*gsl_vector_get(v, 0));
+
+	return 0;
+}
+
+
+/* Spot position gradients for panel motions (translation or rotation) */
+int fs_ss_gradient_panel(int param, Reflection *refl, UnitCell *cell,
+                         struct detgeom_panel *p, gsl_matrix *Minv,
+                         double fs, double ss, double mu,
+                         gsl_vector *t,
+                         float *fsg, float *ssg)
+{
+	gsl_vector *v;
+	gsl_matrix *gM;  /* M^-1 * dM/dx * M^-1 */
+	gsl_matrix *dMdp = gsl_matrix_calloc(3, 3);
+
+	switch ( param ) {
 
 		case GPARAM_DET_TX :
 		gsl_matrix_set(dMdp, 0, 0, 1.0);
@@ -282,25 +276,106 @@ int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
 		return 0;
 
 		default:
+		ERROR("Invalid panel gradient %i\n", param);
 		return 1;
 
 	}
 
 	gM = matrix_mult3(Minv, dMdp, Minv);
+	gsl_matrix_free(dMdp);
+
+	v = gsl_vector_calloc(3);
 	gsl_blas_dgemv(CblasNoTrans, -1.0, gM, t, 0.0, v);
+	gsl_vector_free(t);
+	gsl_matrix_free(gM);
 
 	*fsg = mu*(gsl_vector_get(v, 1) - fs*gsl_vector_get(v, 0));
 	*ssg = mu*(gsl_vector_get(v, 2) - ss*gsl_vector_get(v, 0));
 
 	gsl_vector_free(v);
-	gsl_matrix_free(gM);
-	gsl_matrix_free(dMdp);
-	gsl_vector_free(t);
 
 	return 0;
 }
 
 
+/* Returns dfs_refl/dP and fss_refl/dP, where P = any parameter
+ * fs_refl is the fast scan position of refl in panel 'p',
+ * in metres (not pixels) */
+int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
+                   struct detgeom_panel *p, gsl_matrix *Minv,
+                   float *fsg, float *ssg)
+{
+	signed int h, k, l;
+	double xl, yl, zl, kpred;
+	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
+	double tta, ctt, phi;
+	gsl_vector *t;
+	gsl_vector *v;
+	gsl_matrix *M;
+	double mu;
+	double fs, ss;
+
+	get_indices(refl, &h, &k, &l);
+	kpred = get_kpred(refl);
+	cell_get_reciprocal(cell, &asx, &asy, &asz,
+	                          &bsx, &bsy, &bsz,
+	                          &csx, &csy, &csz);
+	xl = h*asx + k*bsx + l*csx;
+	yl = h*asy + k*bsy + l*csy;
+	zl = h*asz + k*bsz + l*csz;
+
+	/* Calculate 2theta (scattering angle) and azimuth (phi) */
+	tta = atan2(sqrt(xl*xl+yl*yl), kpred+zl);
+	ctt = cos(tta);
+	phi = atan2(yl, xl);
+
+	/* Set up matrix equation */
+	M = gsl_matrix_alloc(3, 3);
+	v = gsl_vector_alloc(3);
+	t = gsl_vector_alloc(3);
+	if ( (M==NULL) || (v==NULL) || (t==NULL) ) {
+		ERROR("Failed to allocate vectors for gradient calculation\n");
+		return 1;
+	}
+
+	gsl_vector_set(t, 0, sin(tta)*cos(phi));
+	gsl_vector_set(t, 1, sin(tta)*sin(phi));
+	gsl_vector_set(t, 2, ctt);
+
+	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);
+
+	if ( gsl_linalg_HH_solve(M, t, v) ) {
+		ERROR("Failed to solve gradient equation\n");
+		return 1;
+	}
+	gsl_matrix_free(M);
+
+	mu = 1.0 / gsl_vector_get(v, 0);
+	fs = mu*gsl_vector_get(v, 1);
+	ss = mu*gsl_vector_get(v, 2);
+	gsl_vector_free(v);
+
+	if ( param <= GPARAM_CSZ ) {
+		gsl_vector_free(t);
+		return fs_ss_gradient_physics(param, refl, cell, p,
+		                              Minv, fs, ss, mu,
+		                              fsg, ssg);
+	} else {
+		return fs_ss_gradient_panel(param, refl, cell, p,
+		                            Minv, fs, ss, mu, t,
+		                            fsg, ssg);
+	}
+}
+
+
 static int cmpd2(const void *av, const void *bv)
 {
 	struct reflpeak *a, *b;