Implement data_template_rotate_group()

4 files changed, 167 insertions, 6 deletions

diff --git a/libcrystfel/src/datatemplate.c b/libcrystfel/src/datatemplate.c
index fc0e7f0e..207974e3 100644
--- a/libcrystfel/src/datatemplate.c
+++ b/libcrystfel/src/datatemplate.c
@@ -45,7 +45,7 @@
  * \file datatemplate.h
  */
 
-static const struct panel_group_template *find_group(const DataTemplate *dt, const char *name)
+static struct panel_group_template *find_group(const DataTemplate *dt, const char *name)
 {
 	int i;
 
@@ -116,7 +116,7 @@ static int parse_group(const char *name, DataTemplate *dt, const char *val)
 	} else {
 
 		for ( i=0; i<n_members; i++ ) {
-			gt->children[i] =  find_group(dt, members[i]);
+			gt->children[i] = find_group(dt, members[i]);
 			if ( gt->children[i] == NULL ) {
 				ERROR("Unknown panel group '%s'\n", members[i]);
 				fail = 1;
@@ -2121,3 +2121,138 @@ int data_template_translate_group_m(DataTemplate *dtempl, const char *group_name
 	if ( group == NULL ) return 1;
 	return translate_group_contents(dtempl, group, x, y, z, 1);
 }
+
+
+static void add_point(const struct panel_template *p,
+                      int fs, int ss,
+                      double *tx, double *ty, double *tz)
+{
+	*tx += (p->cnx + fs*p->fsx + ss*p->ssx) * p->pixel_pitch;
+	*ty += (p->cny + fs*p->fsy + ss*p->ssy) * p->pixel_pitch;
+	*tz += p->cnz_offset + (fs*p->fsz + ss*p->ssz) * p->pixel_pitch;
+}
+
+
+static int group_center(DataTemplate *dtempl,
+                        const struct panel_group_template *group,
+                        double *cx, double *cy, double *cz)
+{
+	if ( group->n_children == 0 ) {
+
+		const struct panel_template *p = find_panel_by_name(dtempl, group->name);
+		if ( p == NULL ) return 1;
+
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		add_point(p, 0, 0, &tx, &ty, &tz);
+		add_point(p, PANEL_WIDTH(p), 0, &tx, &ty, &tz);
+		add_point(p, 0, PANEL_HEIGHT(p), &tx, &ty, &tz);
+		add_point(p, PANEL_WIDTH(p), PANEL_HEIGHT(p), &tx, &ty, &tz);
+
+		*cx = tx / 4.0;
+		*cy = ty / 4.0;
+		*cz = tz / 4.0;
+
+		return 0;
+
+	} else {
+
+		int i;
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		for ( i=0; i<group->n_children; i++ ) {
+			double gcx, gcy, gcz;
+			group_center(dtempl, group->children[i], &gcx, &gcy, &gcz);
+			tx += gcx;
+			ty += gcy;
+			tz += gcz;
+		}
+
+		*cx = tx / group->n_children;
+		*cy = ty / group->n_children;
+		*cz = tz / group->n_children;
+
+		return 0;
+
+	}
+}
+
+static int rotate_all_panels(DataTemplate *dtempl,
+                             struct panel_group_template *group,
+                             char axis, double ang,
+                             double cx, double cy, double cz)
+{
+	if ( group->n_children == 0 ) {
+
+		struct panel_template *p = find_panel_by_name(dtempl, group->name);
+		if ( p == NULL ) return 1;
+
+		switch ( axis )
+		{
+			case 'x':
+			p->cnz_offset =  p->cnz_offset*cos(ang) + p->cny*sin(ang);
+			p->cny = -p->cnz_offset*sin(ang) + p->cny*cos(ang);
+			p->fsz =  p->fsz*cos(ang) + p->fsy*sin(ang);
+			p->fsy = -p->fsz*sin(ang) + p->fsy*cos(ang);
+			p->ssz =  p->ssz*cos(ang) + p->ssy*sin(ang);
+			p->ssy = -p->ssz*sin(ang) + p->ssy*cos(ang);
+			break;
+
+			case 'y':
+			p->cnx =  p->cnx*cos(ang) + p->cnz_offset*sin(ang);
+			p->cnz_offset = -p->cnx*sin(ang) + p->cnz_offset*cos(ang);
+			p->fsx =  p->fsx*cos(ang) + p->fsz*sin(ang);
+			p->fsz = -p->fsx*sin(ang) + p->fsz*cos(ang);
+			p->ssx =  p->ssx*cos(ang) + p->ssz*sin(ang);
+			p->ssz = -p->ssx*sin(ang) + p->ssz*cos(ang);
+			break;
+
+			case 'z':
+			p->cnx =  p->cnx*cos(ang) + p->cny*sin(ang);
+			p->cny = -p->cnx*sin(ang) + p->cny*cos(ang);
+			p->fsx =  p->fsx*cos(ang) + p->fsy*sin(ang);
+			p->fsy = -p->fsx*sin(ang) + p->fsy*cos(ang);
+			p->ssx =  p->ssx*cos(ang) + p->ssy*sin(ang);
+			p->ssy = -p->ssx*sin(ang) + p->ssy*cos(ang);
+			break;
+		}
+
+		return 0;
+
+	} else {
+
+		int i;
+
+		for ( i=0; i<group->n_children; i++ ) {
+			rotate_all_panels(dtempl, group->children[i],
+			                  axis, ang, cx, cy, cz);
+		}
+
+		return 0;
+
+	}
+}
+
+/**
+ * Alters dtempl by rotating the named panel group by ang (degrees) about its
+ * center.
+ *
+ * \returns zero for success, non-zero on error
+ */
+int data_template_rotate_group(DataTemplate *dtempl, const char *group_name,
+                               double ang, char axis)
+{
+	struct panel_group_template *group;
+	double cx, cy, cz;
+
+	group = find_group(dtempl, group_name);
+	if ( group == NULL ) return 1;
+
+	if ( group_center(dtempl, group, &cx, &cy, &cz) ) return 1;
+
+	return rotate_all_panels(dtempl, group, axis, ang, cx, cy, cz);
+}
diff --git a/libcrystfel/src/datatemplate.h b/libcrystfel/src/datatemplate.h
index cce3eb2d..006b1a63 100644
--- a/libcrystfel/src/datatemplate.h
+++ b/libcrystfel/src/datatemplate.h
@@ -91,6 +91,10 @@ extern int data_template_translate_group_m(DataTemplate *dtempl,
                                            const char *group_name,
                                            double x, double y, double z);
 
+extern int data_template_rotate_group(DataTemplate *dtempl,
+                                      const char *group_name,
+                                      double ang, char axis);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/libcrystfel/src/datatemplate_priv.h b/libcrystfel/src/datatemplate_priv.h
index 39513adb..f7830a49 100644
--- a/libcrystfel/src/datatemplate_priv.h
+++ b/libcrystfel/src/datatemplate_priv.h
@@ -215,7 +215,7 @@ struct panel_group_template
 {
 	char *name;
 	int n_children;
-	const struct panel_group_template *children[MAX_PANEL_GROUP_CHILDREN];
+	struct panel_group_template *children[MAX_PANEL_GROUP_CHILDREN];
 };
 
 
diff --git a/src/adjust_detector.c b/src/adjust_detector.c
index 8c7e6aec..ab46a9df 100644
--- a/src/adjust_detector.c
+++ b/src/adjust_detector.c
@@ -58,9 +58,9 @@ static void show_help(const char *s)
 	       "  -p, --panel=p              Panel (or group) to move\n"
 	       "      --mm                   Interpret shifts as mm, not px\n"
 	       "\n"
-	       "  -a                         Rotation around x-axis\n"
-	       "  -b                         Rotation around y-axis\n"
-	       "  -c                         Rotation around z-axis\n"
+	       "  -a                         Rotation around x-axis (deg)\n"
+	       "  -b                         Rotation around y-axis (deg)\n"
+	       "  -c                         Rotation around z-axis (deg)\n"
 	       "  -x                         Shift in x direction\n"
 	       "  -y                         Shift in y direction\n"
 	       "  -z                         Shift in z direction\n"
@@ -214,5 +214,27 @@ int main(int argc, char *argv[])
 		return 1;
 	}
 
+	if ( data_template_rotate_group(dtempl, group, deg2rad(x_rot), 'x') ) {
+		ERROR("Failed to rotate group around x.\n");
+		return 1;
+	}
+
+	if ( data_template_rotate_group(dtempl, group, deg2rad(y_rot), 'y') ) {
+		ERROR("Failed to rotate group around y.\n");
+		return 1;
+	}
+
+	if ( data_template_rotate_group(dtempl, group, deg2rad(z_rot), 'z') ) {
+		ERROR("Failed to rotate group around z.\n");
+		return 1;
+	}
+
+	if ( data_template_write_to_file(dtempl, out_geom) ) {
+		ERROR("Failed to save geometry file.\n");
+		return 1;
+	}
+
+	data_template_free(dtempl);
+
 	return 0;
 }