Add rotation based on quaternions

1 files changed, 37 insertions, 3 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;