Resolve +/-Pi ambiguity

git-svn-id: svn://cook.msm.cam.ac.uk:745/diff-tomo/dtr@107 bf6ca9ba-c028-0410-8290-897cf20841d1

1 files changed, 73 insertions, 48 deletions

diff --git a/src/reproject.c b/src/reproject.c
index faa9433..5594451 100644
--- a/src/reproject.c
+++ b/src/reproject.c
@@ -49,8 +49,8 @@ ImageReflection *reproject_get_reflections(ImageRecord image, size_t *n, Reflect
 		
 		/* Now calculate the (normalised) incident electron wavevector */
 		xt = 0;
-		yt = - sin(tilt);
-		zt = - cos(tilt);
+		yt =  sin(tilt);
+		zt =  cos(tilt);
 		nx = xt*cos(omega) + yt*-sin(omega);
 		ny = xt*sin(omega) + yt*cos(omega);
 		nz = zt;
@@ -71,16 +71,15 @@ ImageReflection *reproject_get_reflections(ImageRecord image, size_t *n, Reflect
 			double kx, ky, kz;
 			double cx, cy, cz;
 			double ux, uy, uz;
-			double uxt, uyt, uzt;
-			double theta, psi;
+			double uyt, uzt;
+			double theta;
 			double x, y;
+			double rx, ry, rz;
 			
 			/* Determine the intersection point */
 			xi = xl + s*nx;  yi = yl + s*ny;  zi = zl + s*nz;
-			reflection_add(ctx->reflectionctx, xl, yl, zl, 1, REFLECTION_CENTRAL);
-			reflection_add(ctx->reflectionctx, xi, yi, zi, 1, REFLECTION_MARKER);
 			
-			/* Calculate Bragg angle and azimuth of point in image */
+			/* Calculate Bragg angle */
 			kx = nx / image.lambda;
 			ky = ny / image.lambda;
 			kz = nz / image.lambda;	/* This is the centre of the Ewald sphere */
@@ -88,54 +87,80 @@ ImageReflection *reproject_get_reflections(ImageRecord image, size_t *n, Reflect
 			gy = yi - ky;
 			gz = zi - kz;	/* This is the vector from the centre of the sphere to the intersection */
 			theta = angle_between(-kx, -ky, -kz, gx, gy, gz);
-			cx = nz*gy - ny*gz;
-			cy = nz*gx - nx*gz;
-			cz = ny*gx - nx*gy;
-			uxt = 0;
-			uyt = cos(tilt);
-			uzt = -sin(tilt);
-			ux = uyt*-sin(omega);
-			uy = uyt*cos(omega);
-			uz = uzt;
-			psi = angle_between(cx, cy, cz, ux, uy, uz) - M_PI_2;
 			
-			if ( image.fmode == FORMULATION_CLEN ) {
-				x = image.camera_len*sin(theta)*cos(psi);
-				y = image.camera_len*sin(theta)*sin(psi);
-				x *= image.resolution;
-				y *= image.resolution;
-			} else if ( image.fmode == FORMULATION_PIXELSIZE ) {
-				x = sin(theta)*cos(psi) / image.lambda;
-				y = sin(theta)*sin(psi) / image.lambda;
-				x /= image.pixel_size;
-				y /= image.pixel_size;
-			} else {
-				fprintf(stderr, "Unrecognised formulation mode in reproject_get_reflections()\n");
-				return NULL;
-			}
+			if ( theta > 0.0 ) {
 			
-			/* Adjust centre */
-			x += image.x_centre;
-			y += image.y_centre;
+				double psi, disc;
 			
-			/* Sanity check */
-			if ( (x>=0) && (x<image.width) && (y>=0) && (y<image.height) ) {
-			
-				/* Record the reflection */
-				refl[i].x = x;
-				refl[i].y = y;
-				i++;
+				/* Determine where "up" is */
+				uyt = cos(tilt);	uzt = -sin(tilt);  /* tilt it (uxt not needed) */
+				ux = uyt*-sin(omega);	uy = uyt*cos(omega);	uz = uzt;  /* rotate it */
+				
+				/* Calculate azimuth of point in image (clockwise from "up", will be changed later) */
+				cx = nz*yi - ny*zi;  cy = nx*zi - nz*xi;  cz = ny*xi - nx*yi;  /* c = (-n) x i */
+				psi = angle_between(cx, cy, cz, ux, uy, uz);
+				
+				/* Finally, resolve the +/- Pi ambiguity from the previous step */
+				rx = nz*cy - ny*cz;  ry = nx*cz - nz*cx;  rz = ny*cx - nx*cy;  /* r = (-n) x [(-n) x i] */
+				disc = angle_between(rx, ry, rz, ux, uy, uz);
+				if ( (psi >= M_PI_2) && (disc >= M_PI_2) ) psi -= M_PI_2;		/* Case 1 */
+				else if ( (psi >= M_PI_2) && (disc < M_PI_2) ) psi = 3*M_PI_2 - psi;	/* Case 2 */
+				else if ( (psi < M_PI_2) && (disc < M_PI_2) ) psi = 3*M_PI_2 - psi;	/* Case 3 */
+				else if ( (psi < M_PI_2) && (disc >= M_PI_2) ) psi = 3*M_PI_2 + psi;	/* Case 4 */
+				psi = M_PI_2 - psi;	/* Anticlockwise from "+x" instead of clockwise from "up" */
+				if ( (reflection->h == 2) && (reflection->k == 2) && (reflection->l == 0) ) {
+					printf("c = %f, %f, %f\n", cx, cy, cz);
+					reflection_add(ctx->reflectionctx, cx, cy, cz, 1, REFLECTION_VECTOR_MARKER_2);
+					printf("r = %f, %f, %f\n", rx, ry, rz);
+					reflection_add(ctx->reflectionctx, rx, ry, rz, 1, REFLECTION_VECTOR_MARKER_3);
+					reflection->type = REFLECTION_MARKER;
+					printf("n = %f, %f, %f\n", kx, ky, kz);
+					reflection_add(ctx->reflectionctx, kx, ky, kz, 1, REFLECTION_VECTOR_MARKER_1);
+					} else {
+					reflection_add(ctx->reflectionctx, xl, yl, zl, 1, REFLECTION_GENERATED);
+					reflection_add(ctx->reflectionctx, xi, yi, zi, 1, REFLECTION_MARKER);
+				}
 				
-				if ( i > MAX_IMAGE_REFLECTIONS ) {
-					fprintf(stderr, "Too many reflections\n");
-					break;
+				/* Calculate image coordinates from polar representation */
+				if ( image.fmode == FORMULATION_CLEN ) {
+					x = image.camera_len*sin(theta)*cos(psi);
+					y = image.camera_len*sin(theta)*sin(psi);
+					x *= image.resolution;
+					y *= image.resolution;
+				} else if ( image.fmode == FORMULATION_PIXELSIZE ) {
+					x = sin(theta)*cos(psi) / image.lambda;
+					y = sin(theta)*sin(psi) / image.lambda;
+					x /= image.pixel_size;
+					y /= image.pixel_size;
+				} else {
+					fprintf(stderr, "Unrecognised formulation mode in reproject_get_reflections()\n");
+					return NULL;
 				}
 				
-				//printf("Reflection %i at %i,%i\n", i, refl[i-1].x, refl[i-1].y);
+				/* Adjust centre */
+				x += image.x_centre;
+				y += image.y_centre;
+				
+				/* Sanity check */
+				if ( (x>=0) && (x<image.width) && (y>=0) && (y<image.height) ) {
+				
+					/* Record the reflection */
+					refl[i].x = x;
+					refl[i].y = y;
+					i++;
+					
+					if ( i > MAX_IMAGE_REFLECTIONS ) {
+						fprintf(stderr, "Too many reflections\n");
+						break;
+					}
+					
+					//printf("Reflection %i at %i,%i\n", i, refl[i-1].x, refl[i-1].y);
+				
+				} else {
+					//fprintf(stderr, "Reflection failed sanity test (x=%f, y=%f)\n", x, y);
+				}
 			
-			} else {
-				//fprintf(stderr, "Reflection failed sanity test (x=%f, y=%f)\n", x, y);
-			}
+			} /* else this is the central beam so don't worry about it */
 		
 		}