Index: /branches/czw_branch/20160809/Ohana/src/opihi/cmd.data/match2d.c
===================================================================
--- /branches/czw_branch/20160809/Ohana/src/opihi/cmd.data/match2d.c	(revision 39732)
+++ /branches/czw_branch/20160809/Ohana/src/opihi/cmd.data/match2d.c	(revision 39733)
@@ -1,6 +1,9 @@
 # include "data.h"
 
-int find_matches2d (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2);
+int find_matches2d (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2, Vector *radiusMatch);
 int find_matches2d_closest (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index);
+
+int find_matches2d_sphere (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2, Vector *radiusMatch);
+int find_matches2d_sphere_closest (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index);
 
 // match2d (X1) (Y1) (X2) (Y2) (Radius) [-index1 (index1)] [-index2 (index2)] [-nomatch1 nomatch1] [-nomatch2 nomatch2]
@@ -23,4 +26,14 @@
   }
 
+  int SPHERE_DISTANCE = FALSE;
+  if ((N = get_argument (argc, argv, "-sphere"))) {
+    remove_argument (N, &argc, argv);
+    SPHERE_DISTANCE = TRUE;
+  }
+  if ((N = get_argument (argc, argv, "-sky"))) {
+    remove_argument (N, &argc, argv);
+    SPHERE_DISTANCE = TRUE;
+  }
+
   if ((N = get_argument (argc, argv, "-index1"))) {
     remove_argument (N, &argc, argv);
@@ -37,4 +50,16 @@
   } else {
     if ((index2 = SelectVector ("index2", ANYVECTOR, TRUE)) == NULL) return (FALSE);    
+  }
+
+  
+  Vector *radiusMatch = NULL;
+  if ((N = get_argument (argc, argv, "-radius"))) {
+    if (CLOSEST) {
+      gprint (GP_ERR, "error: -radius and -closest are currently incompatible\n");
+      return (FALSE);
+    }
+    remove_argument (N, &argc, argv);
+    if ((radiusMatch = SelectVector (argv[N], ANYVECTOR, TRUE)) == NULL) return (FALSE);    
+    remove_argument (N, &argc, argv);
   }
 
@@ -84,11 +109,19 @@
   }
 
-  if (CLOSEST) {
+  if (SPHERE_DISTANCE) {
+    if (CLOSEST) {
+      find_matches2d_sphere_closest (X1vec, Y1vec, X2vec, Y2vec, Radius, index1);
+      find_matches2d_sphere_closest (X2vec, Y2vec, X1vec, Y1vec, Radius, index2);
+    } else {
+      find_matches2d_sphere (X1vec, Y1vec, X2vec, Y2vec, Radius, index1, index2, radiusMatch);
+    }
+  } else {
+    if (CLOSEST) {
       find_matches2d_closest (X1vec, Y1vec, X2vec, Y2vec, Radius, index1);
       find_matches2d_closest (X2vec, Y2vec, X1vec, Y1vec, Radius, index2);
-  } else {
-      find_matches2d (X1vec, Y1vec, X2vec, Y2vec, Radius, index1, index2);
-  }
-
+    } else {
+      find_matches2d (X1vec, Y1vec, X2vec, Y2vec, Radius, index1, index2, radiusMatch);
+    }
+  }
   return (TRUE);
 
@@ -108,9 +141,13 @@
   gprint (GP_ERR, "use 'reindex' to generate new vectors based on these index vectors\n");
 
+  gprint (GP_ERR, "if -sphere or -sky is supplied, (x1,y1) and (x2,y2) are treaded as (ra,dec) or (long,lat) pairs in degrees\n");
+
+  gprint (GP_ERR, "if -radius (vector) is supplied, the vector will be filled with the distance between the matched pairs\n");
+  gprint (GP_ERR, "  not valid with -closest\n");
   return FALSE;
 }
 
 // we are not defining a relative offset DX,DY for now
-int find_matches2d (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2) {
+int find_matches2d (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2, Vector *radiusMatch) {
   
   off_t i, j, first_j, I, J, *N1, *N2, Nmatch, NMATCH, DMATCH;
@@ -122,4 +159,5 @@
   ResetVector (index1, OPIHI_INT, NMATCH);
   ResetVector (index2, OPIHI_INT, NMATCH);
+  if (radiusMatch) ResetVector (radiusMatch, OPIHI_FLT, NMATCH);
 
   ALLOCATE (N1, off_t, X1->Nelements);
@@ -154,4 +192,5 @@
 	index1->elements.Int[Nmatch] = I;
 	index2->elements.Int[Nmatch] = J;
+	if (radiusMatch) radiusMatch->elements.Flt[Nmatch] = dR;
 
 	// XXX track matches 1 and 2 with internal vector, save new nomatch index vectors
@@ -163,4 +202,5 @@
 	  REALLOCATE (index1->elements.Int, opihi_int, NMATCH);
 	  REALLOCATE (index2->elements.Int, opihi_int, NMATCH);
+	  if (radiusMatch) { REALLOCATE (radiusMatch->elements.Flt, opihi_flt, NMATCH); }
 	}
       }
@@ -171,4 +211,5 @@
   index1->Nelements = Nmatch;
   index2->Nelements = Nmatch;
+  if (radiusMatch) radiusMatch->Nelements = Nmatch;
 
   free (N1);
@@ -246,2 +287,231 @@
   return (TRUE);
 }
+
+double gcdist (double r1, double d1, double r2, double d2) {
+  double num,den;
+  r1 *= RAD_DEG;
+  d1 *= RAD_DEG;
+  r2 *= RAD_DEG;
+  d2 *= RAD_DEG;
+
+  num = sqrt(pow((cos(d2) * sin(r2 - r1)),2) +
+	     pow((cos(d1) * sin(d2) -
+		  sin(d1) * cos(d2) * cos(r2 - r1)),2));
+  den = (sin(d1) * sin(d2) + cos(d1) * cos(d2) * cos(r2 - r1));
+  return(atan2(num,den) * (180 / M_PI));
+}
+
+typedef struct {
+  double sD;
+  double cD;
+  double sR;
+  double cR;
+} Match2D_PreCalc;
+
+double gcdist_PreCalc_v0 (Match2D_PreCalc *p1, Match2D_PreCalc *p2) {
+  double num,den;
+
+  num = sqrt(pow((p2->cD * (p2->sR*p1->cR - p1->sR*p2->cR)),2) +
+	     pow((p1->cD * p2->sD -
+		  p1->sD * p2->cD * (p2->cR*p1->cR + p2->sR*p1->sR)),2));
+  den = (p1->sD * p2->sD + p1->cD * p2->cD * (p2->cR*p1->cR + p2->sR*p1->sR));
+  return(atan2(num,den) * (180 / M_PI));
+}
+
+// we are not defining a relative offset DX,DY for now
+double gcdist_PreCalc (Match2D_PreCalc *p1, Match2D_PreCalc *p2) {
+  double num,den;
+
+  // double Qd = p1->sD * p1->cD * p2->sD * p2->cD;
+  // double Qr = p1->sR * p1->cR * p2->sR * p2->cR;
+
+  //  double Xa 
+  //    = SQ(p2->cD * p1->cR * p2->sR) 
+  //    + SQ(p2->cD * p1->sR * p2->cR) -
+  //    - 2 * SQ(p2->cD) * Qr;
+
+  double cdR = (p2->cR*p1->cR + p2->sR*p1->sR);
+
+  double Xa = SQ(p2->cD * p2->sR * p1->cR - p2->cD * p1->sR * p2->cR);
+  double Xb = SQ(p1->cD * p2->sD - p1->sD * p2->cD * cdR);
+
+  num = sqrt(Xa + Xb);
+  den = (p1->sD * p2->sD + p1->cD * p2->cD * cdR);
+  return(atan2(num,den) * (180 / M_PI));
+}
+
+// we are not defining a relative offset DX,DY for now
+int find_matches2d_sphere (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2, Vector *radiusMatch) {
+  
+  off_t i, j, first_j, I, J, *N1, *N2, Nmatch, NMATCH, DMATCH;
+  double dY, dR;
+
+  NMATCH = MAX(MAX(0.05*X1->Nelements, 0.05*X2->Nelements), 1000);
+  DMATCH = NMATCH;
+
+  ResetVector (index1, OPIHI_INT, NMATCH);
+  ResetVector (index2, OPIHI_INT, NMATCH);
+  if (radiusMatch) ResetVector (radiusMatch, OPIHI_FLT, NMATCH);
+
+  ALLOCATE (N1, off_t, X1->Nelements);
+  ALLOCATE (N2, off_t, X2->Nelements);
+
+  ALLOCATE_PTR (A1, Match2D_PreCalc, X1->Nelements);
+  ALLOCATE_PTR (A2, Match2D_PreCalc, X2->Nelements);
+
+  for (i = 0; i < X1->Nelements; i++) { 
+    A1[i].sR = sin(RAD_DEG*X1->elements.Flt[i]);
+    A1[i].cR = cos(RAD_DEG*X1->elements.Flt[i]);
+    A1[i].sD = sin(RAD_DEG*Y1->elements.Flt[i]);
+    A1[i].cD = cos(RAD_DEG*Y1->elements.Flt[i]);
+    N1[i] = i; 
+  }
+  for (i = 0; i < X2->Nelements; i++) { 
+    A2[i].sR = sin(RAD_DEG*X2->elements.Flt[i]);
+    A2[i].cR = cos(RAD_DEG*X2->elements.Flt[i]);
+    A2[i].sD = sin(RAD_DEG*Y2->elements.Flt[i]);
+    A2[i].cD = cos(RAD_DEG*Y2->elements.Flt[i]);
+    N2[i] = i; 
+  }
+
+  // sort from one pole to the other
+  sort_coords_indexonly (Y1->elements.Flt, X1->elements.Flt, N1, X1->Nelements);
+  sort_coords_indexonly (Y2->elements.Flt, X2->elements.Flt, N2, X2->Nelements);
+
+  Nmatch = 0;
+  for (i = j = 0; (i < X1->Nelements) && (j < X2->Nelements);) {
+    I = N1[i];
+    J = N2[j];
+
+    // we can use dY as minimal requirement: if dY > Radius, we are too far apart
+    dY = Y1->elements.Flt[I] - Y2->elements.Flt[J];
+
+    if (dY <= -1.02*Radius) { i++; continue; }
+    if (dY >= +1.02*Radius) { j++; continue; }
+
+    // look for all matches of list2() to list1(i)
+    first_j = j;
+    for (j = first_j; (dY > -1.02*Radius) && (j < X2->Nelements); j++) {
+      J = N2[j];
+
+      dR = gcdist_PreCalc (&A1[I], &A2[J]);
+      // dR = gcdist (X1->elements.Flt[I], Y1->elements.Flt[I], X2->elements.Flt[J], Y2->elements.Flt[J]);
+
+      if (dR < Radius) {
+	index1->elements.Int[Nmatch] = I;
+	index2->elements.Int[Nmatch] = J;
+	if (radiusMatch) radiusMatch->elements.Flt[Nmatch] = dR;
+
+	// XXX track matches 1 and 2 with internal vector, save new nomatch index vectors
+	// after this loop
+
+	Nmatch ++;
+	if (Nmatch >= NMATCH) {
+	  NMATCH += DMATCH;
+	  REALLOCATE (index1->elements.Int, opihi_int, NMATCH);
+	  REALLOCATE (index2->elements.Int, opihi_int, NMATCH);
+	  if (radiusMatch) { REALLOCATE (radiusMatch->elements.Flt, opihi_flt, NMATCH); }
+	}
+      }
+    }
+    j = first_j;
+    i++;
+  }
+  index1->Nelements = Nmatch;
+  index2->Nelements = Nmatch;
+  if (radiusMatch) radiusMatch->Nelements = Nmatch;
+
+  free (A1);
+  free (A2);
+
+  free (N1);
+  free (N2);
+
+  return (TRUE);
+}
+
+// we are not defining a relative offset DX,DY for now
+int find_matches2d_sphere_closest (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index) {
+  
+  off_t i, j, Jmin, Ji, I, J, *N1, *N2;
+  double dY, dR, Rmin;
+
+  ResetVector (index, OPIHI_INT, X1->Nelements);
+  for (i = 0; i < index->Nelements; i++) { index->elements.Int[i] = -1; }
+
+  ALLOCATE (N1, off_t, X1->Nelements);
+  ALLOCATE (N2, off_t, X2->Nelements);
+
+  ALLOCATE_PTR (A1, Match2D_PreCalc, X1->Nelements);
+  ALLOCATE_PTR (A2, Match2D_PreCalc, X2->Nelements);
+
+  for (i = 0; i < X1->Nelements; i++) { 
+    A1[i].sR = sin(RAD_DEG*X1->elements.Flt[i]);
+    A1[i].cR = cos(RAD_DEG*X1->elements.Flt[i]);
+    A1[i].sD = sin(RAD_DEG*Y1->elements.Flt[i]);
+    A1[i].cD = cos(RAD_DEG*Y1->elements.Flt[i]);
+    N1[i] = i; 
+  }
+  for (i = 0; i < X2->Nelements; i++) { 
+    A2[i].sR = sin(RAD_DEG*X2->elements.Flt[i]);
+    A2[i].cR = cos(RAD_DEG*X2->elements.Flt[i]);
+    A2[i].sD = sin(RAD_DEG*Y2->elements.Flt[i]);
+    A2[i].cD = cos(RAD_DEG*Y2->elements.Flt[i]);
+    N2[i] = i; 
+  }
+
+  // sort from one pole to the other
+  sort_coords_indexonly (Y1->elements.Flt, X1->elements.Flt, N1, X1->Nelements);
+  sort_coords_indexonly (Y2->elements.Flt, X2->elements.Flt, N2, X2->Nelements);
+
+  for (i = j = 0; (i < X1->Nelements) && (j < X2->Nelements);) {
+    I = N1[i];
+    J = N2[j];
+
+    // we can use dY as minimal requirement: if dY > Radius, we are too far apart
+    dY = Y1->elements.Flt[I] - Y2->elements.Flt[J];
+
+    if (dY <= -1.02*Radius) { 
+      // no match in list 2 to this entry
+      index->elements.Int[I] = -1; // (probably not needed --- index is init'ed above)o
+      i++; 
+      continue; 
+    }
+    if (dY >= +1.02*Radius) { j++; continue; }
+
+    // look for all matches of list2() to list1(i)
+    Jmin = -1;
+    Rmin = Radius;
+    for (Ji = j; (dY > -1.02*Radius) && (Ji < X2->Nelements); Ji++) {
+      J = N2[Ji];
+
+      dR = gcdist_PreCalc (&A1[I], &A2[J]);
+      // dR = gcdist (X1->elements.Flt[I], Y1->elements.Flt[I], X2->elements.Flt[J], Y2->elements.Flt[J]);
+
+      if (dR < Rmin) {
+	Rmin = dR;
+	Jmin = J;
+      }
+    }
+    
+
+    // no match in list 2 to this entry
+    if (Jmin == -1) {
+      index->elements.Int[I] = -1;
+      i++;
+      continue;
+    }
+    index->elements.Int[I] = Jmin;
+    i++;
+  }
+
+  free (A1);
+  free (A2);
+
+  free (N1);
+  free (N2);
+
+  return (TRUE);
+}
+
+