geoptimiser: Update avg_displ_{x,y} straight after adjusting panel

1 files changed, 31 insertions, 58 deletions

diff --git a/src/geoptimiser.c b/src/geoptimiser.c
index 96ce4028..a84d0d56 100644
--- a/src/geoptimiser.c
+++ b/src/geoptimiser.c
@@ -1092,6 +1092,7 @@ static void correct_corner_coordinates(struct rg_collection *connected,
 
 static void correct_rotation_and_stretch(struct rg_collection *connected,
                                          struct detector *det,
+                                         struct gpanel *gpanels,
                                          struct connected_data *conn_data,
                                          double clen_to_use,
                                          double stretch_coeff,
@@ -1102,12 +1103,21 @@ static void correct_rotation_and_stretch(struct rg_collection *connected,
 
 	STATUS("Applying rotation and stretch corrections.\n");
 	for ( di=0; di<connected->n_rigid_groups; di++ ) {
+
+		int npp = conn_data[di].num_peaks_per_pixel;
+		double c_stretch = conn_data[di].cstr;
+
+		if ( fabs(c_stretch)<FLT_EPSILON ) c_stretch = stretch_coeff;
+
 		for ( ip=0; ip<connected->rigid_groups[di]->n_panels; ip++ ) {
 
 			struct panel *p;
 			double new_fsx, new_fsy, new_ssx, new_ssy;
+			int fs, ss;
+			struct gpanel *gp;
 
 			p = connected->rigid_groups[di]->panels[ip];
+			gp = &gpanels[panel_number(det, p)];
 
 			new_fsx = p->fsx*cos(conn_data[di].cang)-
 			          p->fsy*sin(conn_data[di].cang);
@@ -1117,14 +1127,33 @@ static void correct_rotation_and_stretch(struct rg_collection *connected,
 			          p->ssy*sin(conn_data[di].cang);
 			new_ssy = p->ssx*sin(conn_data[di].cang)+
 			          p->ssy*cos(conn_data[di].cang);
+
+			new_fsx /= c_stretch;
+			new_ssx /= c_stretch;
+			new_fsy /= c_stretch;
+			new_ssy /= c_stretch;
+
+			/* The average displacements now need to be updated
+			 * (stretch and angle here) */
+			for ( ss=0; ss<p->h; ss++ ) {
+			for ( fs=0; fs<p->w; fs++ ) {
+				int idx = fs + p->w*ss;
+				if ( gp->num_pix_displ[idx] < npp ) continue;
+				gp->avg_displ_x[idx] += fs*(new_fsx - p->fsx);
+				gp->avg_displ_x[idx] += ss*(new_ssx - p->ssx);
+				gp->avg_displ_y[idx] += fs*(new_fsy - p->fsy);
+				gp->avg_displ_y[idx] += ss*(new_ssy - p->ssy);
+			}
+			}
+
 			p->fsx = new_fsx;
 			p->fsy = new_fsy;
 			p->ssx = new_ssx;
 			p->ssy = new_ssy;
+
 		}
 	}
 
-
 	if ( gparams->individual_coffset ) {
 
 		STATUS("Using individual distances for rigid panels.\n");
@@ -1159,59 +1188,6 @@ static void correct_rotation_and_stretch(struct rg_collection *connected,
 }
 
 
-static void adjust_panel(struct connected_data *conn_data,
-                         struct rg_collection *connected, double c_stretch,
-                         double stretch_coeff, int num_peaks_per_pixel,
-                         struct panel *p, struct gpanel *gp)
-{
-	int ifs, iss;
-
-	/* FIXME: What does "TODO" mean? */
-	//TODO
-
-	if ( fabs(c_stretch)<FLT_EPSILON ) c_stretch = stretch_coeff;
-
-	for ( iss=0; iss<p->h; iss++ ) {
-	for ( ifs=0; ifs<p->w; ifs++ ) {
-
-		double x, y;
-		int idx = ifs+gp->p->w*iss;
-
-		if ( gp->num_pix_displ[idx] < num_peaks_per_pixel) continue;
-
-		compute_x_y(ifs, iss, p, &x, &y);
-		gp->avg_displ_x[idx] -= x - x/c_stretch;
-		gp->avg_displ_y[idx] -= y - y/c_stretch;
-
-	}
-	}
-}
-
-
-static void adjust_displ_for_stretch(struct rg_collection *connected,
-                                     struct connected_data *conn_data,
-                                     double stretch_coeff, struct detector *det,
-                                     struct gpanel *gpanels)
-{
-	int di, ip;
-
-	for ( di=0; di<connected->n_rigid_groups; di++ ) {
-		for (ip=0; ip<connected->rigid_groups[di]->n_panels; ip++) {
-
-			struct panel *p;
-			struct gpanel *gp;
-
-			p = connected->rigid_groups[di]->panels[ip];
-			gp = &gpanels[panel_number(det, p)];
-
-			adjust_panel(conn_data, connected, conn_data[di].cstr,
-			             stretch_coeff,
-			             conn_data[di].num_peaks_per_pixel, p, gp);
-		}
-	}
-}
-
-
 /* Collect together all the offsets for each group in "connected"
  * Only offsets which have enough peaks per pixel will be used. */
 static int collate_offsets_for_rg(struct rigid_group *group,
@@ -2407,13 +2383,10 @@ int optimize_geometry(struct geoptimiser_params *gparams,
 		return 1;
 	}
 
-	correct_rotation_and_stretch(connected, det, conn_data,
+	correct_rotation_and_stretch(connected, det, gpanels, conn_data,
 	                             clen_to_use, stretch_coeff,
 	                             gparams);
 
-	adjust_displ_for_stretch(connected, conn_data, stretch_coeff,
-	                         det, gpanels);
-
 	ret = compute_shift(connected, conn_data, det, gparams, gpanels);
 	if ( ret != 0 ) {
 		free(conn_data);