Index: /branches/eam_branches/ipp-20111122/Ohana/src/opihi/cmd.astro/spex2dgas.c
===================================================================
--- /branches/eam_branches/ipp-20111122/Ohana/src/opihi/cmd.astro/spex2dgas.c	(revision 32921)
+++ /branches/eam_branches/ipp-20111122/Ohana/src/opihi/cmd.astro/spex2dgas.c	(revision 32922)
@@ -78,11 +78,19 @@
     dF = MIN (maxPressure, MAX (-maxPressure, dF));
 
-    float dPdXi = dF * dX / Dcur;
-    float dPdYi = dF * dY / Dcur;
-
-    // if we are too close, then dX/Dcur is too ill-defined, just jump away
+    float dPdXi, dPdYi;
+    if (fabs(Dcur) < 1e-6) {
+      dPdXi = 0.0;
+      dPdYi = 0.0;
+    } else {
+      dPdXi = dF * dX / Dcur;
+      dPdYi = dF * dY / Dcur;
+    }
+
+    if (isnan(Dtgt) || isnan(dX) || isnan(dY) || isnan(Dcur) || isnan(dF) || isnan(dPdXi) || isnan(dPdYi)) abort();
+
+    // if we are too close, then dX/Dcur is too ill-defined, just jump away by 5% of Dtgt
     if (Dcur < 0.01*Dtgt) {
-      dPdXi = drand48() - 0.5;
-      dPdYi = drand48() - 0.5;
+      dPdXi = (drand48() - 0.5)*0.1*Dtgt;
+      dPdYi = (drand48() - 0.5)*0.1*Dtgt;
     }
 
@@ -108,12 +116,15 @@
 int spex2dgas (int argc, char **argv) {
   
-  int i, iter;
+  int i, j, iter, IoMax;
   Vector *index1, *index2, *distance;
-
-  // init random numbers
-  long A, B;
-  A = time(NULL);
-  for (B = 0; A == time(NULL); B++);
-  srand48(B);
+  float XoMax, YoMax;
+
+  { 
+    // init random numbers
+    long A, B;
+    A = time(NULL);
+    for (B = 0; A == time(NULL); B++);
+    srand48(B);
+  }
  
   if (argc != 9) goto usage;
@@ -202,11 +213,80 @@
   // int Ngrid = sqrt(Nobject);
   // float dgrid = 1.5 * Dmax / Ngrid; // XXX remove the fudge factor
-  for (i = 0; i < Nobject; i++) {
-    // object[i].Xo = dgrid * (int) (i % Ngrid);
-    // object[i].Yo = dgrid * (int) (i / Ngrid);
-    object[i].Xo = Dmax*drand48();
-    object[i].Yo = Dmax*drand48();
+  // XXX for (i = 0; i < Nobject; i++) {
+  // XXX   // object[i].Xo = dgrid * (int) (i % Ngrid);
+  // XXX   // object[i].Yo = dgrid * (int) (i / Ngrid);
+  // XXX   object[i].Xo = Dmax*drand48();
+  // XXX   object[i].Yo = Dmax*drand48();
+  // XXX   object[i].dPdX = 0.0;
+  // XXX   object[i].dPdY = 0.0;
+  // XXX }
+
+  // place the objects at the initial guess locations
+  // use object 0 and its most distant friend to constrain:
+  
+  int idx1 = 0;
+  int idx2 = object[idx1].index[object[idx1].Nindex-1];
+  float A = object[idx1].Dtgt[object[idx1].Nindex-1];
+  object[idx1].Xo = 0.0;
+  object[idx1].Yo = 0.0;
+  object[idx1].dPdX = 0.0;
+  object[idx1].dPdY = 0.0;
+
+  object[idx2].Xo = A;
+  object[idx2].Yo = 0.0;
+  object[idx2].dPdX = 0.0;
+  object[idx2].dPdY = 0.0;
+		 
+  // choose the correct Y side by comparing to the distance from the most deviant
+  YoMax = 0;
+  XoMax = 0;
+  IoMax = 0;
+
+  for (i = 0; i < Nobject; i++) {
+    if (i == idx1) continue;
+    if (i == idx2) continue;
+    float B = NAN;
+    float C = NAN;
+    for (j = 0; (isnan(B) || isnan(C)) && (j < object[i].Nindex); j++) {
+      if (object[i].index[j] == idx1) { B = object[i].Dtgt[j]; }
+      if (object[i].index[j] == idx2) { C = object[i].Dtgt[j]; }
+    }
+    if (isnan(B) || isnan(C)) abort();
+
+    float Xo = (SQ(A) + SQ(B) - SQ(C)) / (2*A);
+    float Y2 = SQ(B) - SQ(Xo);
+
+    float Yo = (Y2 < 0) ? 0.0 : sqrt(Y2);
+    if (isnan(Yo)) abort();
+
+    object[i].Xo = Xo;
+    object[i].Yo = Yo;
     object[i].dPdX = 0.0;
     object[i].dPdY = 0.0;
+    if (Yo > YoMax) {
+      YoMax = Yo;
+      XoMax = Xo;
+      IoMax = i;
+    }
+  }
+
+  for (i = 0; i < Nobject; i++) {
+    if (i == idx1) continue;
+    if (i == idx2) continue;
+    if (i == IoMax) continue;
+    for (j = 0; j < object[i].Nindex; j++) {
+      if (object[i].index[j] == IoMax) { 
+	float d02 = SQ(object[i].Dtgt[j]);
+	float dX  = XoMax - object[i].Xo;
+	float dY1 = YoMax - object[i].Yo;
+	float dY2 = YoMax + object[i].Yo;
+	float d12 = SQ(dX) + SQ(dY1);
+	float d22 = SQ(dX) + SQ(dY2);
+	if (fabs(d12 - d02) > fabs(d22 - d02)) {
+	  object[i].Yo = -object[i].Yo;
+	  break;
+	}
+      }
+    }
   }
 
@@ -215,4 +295,13 @@
 
     int nearNeighbors = (iter < nCloseIter);
+
+    // save the result
+    char name[64];
+    snprintf (name, 64, "output.%02d.dat", iter);
+    FILE *output = fopen (name, "w");
+    for (i = 0; i < Nobject; i++) {
+      fprintf (output, "%f %f : %f %f\n", object[i].Xo, object[i].Yo, object[i].dPdX, object[i].dPdY);
+    }
+    fclose (output);
 
     // measure (dP/dX),(dP/dY) for all objects