1 files changed, 98 insertions, 1 deletions

diff --git a/src/reproject.c b/src/reproject.c
index f62e8f3..c9023af 100644
--- a/src/reproject.c
+++ b/src/reproject.c
@@ -10,21 +10,118 @@
  */
 
 #include <stdlib.h>
+#include <math.h>
 
 #include "control.h"
 #include "reflections.h"
 
 #define MAX_IMAGE_REFLECTIONS 1024
 
-ImageReflection *reproject_get_reflections(ImageRecord *image, size_t *n, ReflectionContext *ref) {
+ImageReflection *reproject_get_reflections(ImageRecord image, size_t *n, ReflectionContext *rctx) {
 
 	ImageReflection *refl;
+	Reflection *reflection;
 	int i;
+	double smax = 1e9;
+	double tilt, omega;
+	
+	tilt = 2*M_PI*(image.tilt/360);
+	omega = 2*M_PI*(image.omega/360);	/* Convert to Radians */
 	
 	refl = malloc(MAX_IMAGE_REFLECTIONS*sizeof(ImageReflection));
 	
 	i = 0;
 	
+	reflection = rctx->reflections;
+	do {
+	
+		double xl, yl, zl;
+		double nx, ny, nz;
+		double xt, yt, zt;
+		double a, b, c;
+		double s1, s2, s;
+		
+		/* Get the coordinates of the reciprocal lattice point */
+		xl = reflection->x;
+		yl = reflection->y;
+		zl = reflection->z;
+		
+		/* Now calculate the (normalised) incident electron wavevector */
+		xt = 0;
+		yt = - sin(tilt);
+		zt = - cos(tilt);
+		nx = xt*cos(omega) + yt*-sin(omega);
+		ny = xt*sin(omega) + yt*cos(omega);
+		nz = zt;
+		
+		/* Next, solve the reprojection equation to calculate the excitation error */
+		a = nx*nx + ny*ny + nz*nz;
+		b = 2*(xl*nx + yl*ny + zl*nz - nz/image.lambda);
+		c = xl*xl + yl*yl + zl*zl - 2*zl/image.lambda;
+		s1 = (-b + sqrt(b*b-4*a*c))/(2*a);
+		s2 = (-b - sqrt(b*b-4*a*c))/(2*a);
+		if ( s1 < s2 ) s = s1; else s = s2;
+		
+		/* Skip this reflection if s is large */
+		if ( s <= smax ) {
+		
+			double xddd, yddd, zddd;
+			double xdd, ydd, zdd;
+			double xd, yd, zd;
+			double theta, psi;
+			double x, y;
+			
+			/* Determine the intersection point */
+			xddd = xl + s*nx;  yddd = yl + s*ny;  zddd = zl + s*nz;
+			
+			/* Invert the image->3D mapping to get the image coordinates */
+			xdd = xddd;
+			ydd = (yddd/cos(tilt) - zddd*tan(tilt)/cos(tilt))/(1+tan(tilt)*tan(tilt));
+			zdd = (-zddd-ydd*sin(tilt))/cos(tilt);
+			
+			yd = (ydd-xdd*tan(omega))/(sin(omega)*tan(omega)+cos(omega));
+			xd = (xdd+yd*sin(omega))/cos(omega);
+			zd = zdd;
+			
+			if ( image.fmode == FORMULATION_CLEN ) {
+				psi = atan2(-yd, xd);
+				theta = acos(1-zd*image.lambda);
+				x = image.camera_len*sin(theta)*cos(psi);
+				y = image.camera_len*sin(theta)*sin(psi);
+				x *= image.resolution;
+			} else if ( image.fmode == FORMULATION_PIXELSIZE ) {
+				x = xd / image.pixel_size;
+				y = yd / image.pixel_size;
+			} else {
+				fprintf(stderr, "Unrecognised formulation mode in reproject_get_reflections()\n");
+				return NULL;
+			}
+			
+			/* 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 ) break;
+				
+				printf("Reflection %i at %i,%i\n", i, refl[i-1].x, refl[i-1].y);
+			
+			} else {
+				fprintf(stderr, "Reflection failed sanity test\n");
+			}
+		
+		}
+		
+		reflection = reflection->next;
+
+	} while ( reflection );
 	
 	*n = i;
 	return refl;