Add rotation based on quaternions

3 files changed, 91 insertions, 14 deletions

diff --git a/src/ewald.c b/src/ewald.c
index 02974175..7a620c42 100644
--- a/src/ewald.c
+++ b/src/ewald.c
@@ -20,6 +20,37 @@
 #include "ewald.h"
 
 
+static struct threevec quat_rot(struct threevec q, struct quaternion z)
+{
+	struct threevec res;
+	double t01, t02, t03, t11, t12, t13, t22, t23, t33;
+
+	t01 = z.w*z.x;
+	t02 = z.w*z.y;
+	t03 = z.w*z.z;
+	t11 = z.x*z.x;
+	t12 = z.x*z.y;
+	t13 = z.x*z.z;
+	t22 = z.y*z.y;
+	t23 = z.y*z.z;
+	t33 = z.z*z.z;
+
+	res.u = (1.0 - 2.0 * (t22 + t33)) * q.u
+	            + (2.0 * (t12 + t03)) * q.v
+	            + (2.0 * (t13 - t02)) * q.w;
+
+	res.v =       (2.0 * (t12 - t03)) * q.u
+	      + (1.0 - 2.0 * (t11 + t33)) * q.v
+	            + (2.0 * (t01 + t23)) * q.w;
+
+	res.w =       (2.0 * (t02 + t13)) * q.u
+	            + (2.0 * (t23 - t01)) * q.v
+	      + (1.0 - 2.0 * (t11 + t22)) * q.w;
+
+	return res;
+}
+
+
 void get_ewald(struct image *image)
 {
 	int x, y;
@@ -39,6 +70,7 @@ void get_ewald(struct image *image)
 		double rx, ry, r;
 		double twothetax, twothetay, twotheta;
 		double qx, qy, qz;
+		struct threevec q;
 		
 		/* Calculate q vectors for Ewald sphere */
 		rx = ((double)x - image->x_centre) / image->resolution;
@@ -52,9 +84,11 @@ void get_ewald(struct image *image)
 		qx = k * sin(twothetax);
 		qy = k * sin(twothetay);
 		qz = k - k * cos(twotheta);
-		
-		/* FIXME: Rotate vector here */
-		
+
+		q.u = qx;  q.v = qy;  q.w = qz;
+		image->qvecs[x + image->width*y] = quat_rot(q,
+		                                            image->orientation);
+
 		image->qvecs[x + image->width*y].u = qx;
 		image->qvecs[x + image->width*y].v = qy;
 		image->qvecs[x + image->width*y].w = qz;
diff --git a/src/image.h b/src/image.h
index b40f8a57..5206048a 100644
--- a/src/image.h
+++ b/src/image.h
@@ -57,6 +57,15 @@ struct threevec
 };
 
 
+struct quaternion
+{
+	double w;
+	double x;
+	double y;
+	double z;
+};
+
+
 /* Structure describing an image */
 struct image {
 
@@ -67,11 +76,7 @@ struct image {
 	double			*twotheta;
 	struct molecule		*molecule;
 
-	/* Radians.  Defines where the pattern lies in reciprocal space */
-	double			tilt;
-
-	/* Radians.  Defines where the pattern lies in reciprocal space */
-	double			omega;
+	struct quaternion	orientation;
 
 	/* Image parameters can be given as camera length or pixel size.
 	 * If FORMULATION_CLEN, then camera_len and resolution must be given.
diff --git a/src/main.c b/src/main.c
index 86cf93d6..e4b826b7 100644
--- a/src/main.c
+++ b/src/main.c
@@ -40,9 +40,11 @@ static void main_show_help(const char *s)
 
 int main(int argc, char *argv[])
 {
-	int c;
+	int c, done;
 	UnitCell *cell;
 	struct image image;
+	char filename[1024];
+	int number = 1;
 
 	while ((c = getopt(argc, argv, "h")) != -1) {
 
@@ -65,11 +67,44 @@ int main(int argc, char *argv[])
 	                                deg2rad(90.0),
 	                                deg2rad(120.0));
 
+again:
+
+	/* Read quaternion from stdin */
+	done = 0;
+	do {
+
+		int r;
+		float w, x, y, z;
+		char line[1024];
+		char *rval;
+
+		printf("Please input quaternion: w x y z\n");
+		rval = fgets(line, 1023, stdin);
+		if ( rval == NULL ) return 0;
+		chomp(line);
+
+		r = sscanf(line, "%f %f %f %f", &w, &x, &y, &z);
+		if ( r == 4 ) {
+
+			printf("Rotation is: %f %f %f %f (modulus=%f)\n",
+			        w, x, y, z, sqrtf(w*w + x*x + y*y + z*z));
+
+			image.orientation.w = w;
+			image.orientation.x = x;
+			image.orientation.y = y;
+			image.orientation.z = z;
+
+			done = 1;
+
+		} else {
+			fprintf(stderr, "Invalid rotation '%s'\n", line);
+		}
+
+	} while ( !done );
+
 	/* Define image parameters */
 	image.width = 512;
 	image.height = 512;
-	image.omega = deg2rad(40.0);
-	image.tilt = deg2rad(0.0);
 	image.fmode = FORMULATION_CLEN;
 	image.x_centre = 255.5;
 	image.y_centre = 255.5;
@@ -88,8 +123,11 @@ int main(int argc, char *argv[])
 	get_diffraction(&image, cell);
 	record_image(&image);
 
-	/* Write the output file */
-	hdf5_write("results/sim.h5", image.data, image.width, image.height);
+	snprintf(filename, 1023, "results/sim-%i.h5", number);
+	number++;
 
-	return 0;
+	/* Write the output file */
+	hdf5_write(filename, image.data, image.width, image.height);
+	printf("Mock write: %i\n", number-1);
+	goto again;
 }