Nicer pattern centering

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

1 files changed, 37 insertions, 4 deletions

diff --git a/src/image.c b/src/image.c
index 5290cb4..8eba5f9 100644
--- a/src/image.c
+++ b/src/image.c
@@ -116,12 +116,11 @@ void image_feature_list_free(ImageFeatureList *flist) {
 
 }
 
-ImageFeature *image_feature_closest(ImageFeatureList *flist, double x, double y, double *d) {
+ImageFeature *image_feature_closest(ImageFeatureList *flist, double x, double y, double *d, int *idx) {
 
 	int i;
 	double dmin = +HUGE_VAL;
-	int closest;
-	closest = 0;
+	int closest = 0;
 	
 	for ( i=0; i<flist->n_features; i++ ) {
 	
@@ -136,11 +135,45 @@ ImageFeature *image_feature_closest(ImageFeatureList *flist, double x, double y,
 	
 	}
 	
-	if ( dmin <= 20.0 ) {
+	if ( dmin < +HUGE_VAL ) {
 		*d = dmin;
+		*idx = closest;
 		return &flist->features[closest];
 	}
+	
+	return NULL;
+
+}
 
+ImageFeature *image_feature_second_closest(ImageFeatureList *flist, double x, double y, double *d, int *idx) {
+
+	int i;
+	double dmin = +HUGE_VAL;
+	int closest = 0;
+	double dfirst;
+	int idxfirst;
+	
+	image_feature_closest(flist, x, y, &dfirst, &idxfirst);
+	
+	for ( i=0; i<flist->n_features; i++ ) {
+	
+		double d;
+		
+		d = distance(flist->features[i].x, flist->features[i].y, x, y);
+	
+		if ( (d < dmin) && (i != idxfirst) ) {
+			dmin = d;
+			closest = i;
+		}
+	
+	}
+	
+	if ( dmin < +HUGE_VAL ) {
+		*d = dmin;
+		*idx = closest;
+		return &flist->features[closest];
+	}
+	
 	return NULL;
 
 }