Index: trunk/Ohana/src/libdvo/src/coordops.c
===================================================================
--- trunk/Ohana/src/libdvo/src/coordops.c	(revision 27024)
+++ trunk/Ohana/src/libdvo/src/coordops.c	(revision 27025)
@@ -20,5 +20,5 @@
 int XY_to_LM (double *L, double *M, double x, double y, Coords *coords) {
 
-  double X, Y;
+  double X, Y, X2, XY, Y2, X3, Y3;
 
   /** convert pixel coordinates to cartesian system **/
@@ -31,10 +31,15 @@
   /** extra polynomial terms **/
   if (coords[0].Npolyterms > 1) {
-    *L += X*X*coords[0].polyterms[0][0] + X*Y*coords[0].polyterms[1][0] + Y*Y*coords[0].polyterms[2][0];
-    *M += X*X*coords[0].polyterms[0][1] + X*Y*coords[0].polyterms[1][1] + Y*Y*coords[0].polyterms[2][1];
+    X2 = X*X;
+    Y2 = Y*Y;
+    XY = X*Y;
+    *L += X2*coords[0].polyterms[0][0] + XY*coords[0].polyterms[1][0] + Y2*coords[0].polyterms[2][0];
+    *M += X2*coords[0].polyterms[0][1] + XY*coords[0].polyterms[1][1] + Y2*coords[0].polyterms[2][1];
   }
   if (coords[0].Npolyterms > 2) {
-    *L += X*X*X*coords[0].polyterms[3][0] + X*X*Y*coords[0].polyterms[4][0] + X*Y*Y*coords[0].polyterms[5][0] + Y*Y*Y*coords[0].polyterms[6][0];
-    *M += X*X*X*coords[0].polyterms[3][1] + X*X*Y*coords[0].polyterms[4][1] + X*Y*Y*coords[0].polyterms[5][1] + Y*Y*Y*coords[0].polyterms[6][1];
+    X3 = X2*X;
+    Y3 = Y2*Y;
+    *L += X3*coords[0].polyterms[3][0] + X2*Y*coords[0].polyterms[4][0] + X*Y2*coords[0].polyterms[5][0] + Y3*coords[0].polyterms[6][0];
+    *M += X3*coords[0].polyterms[3][1] + X2*Y*coords[0].polyterms[4][1] + X*Y2*coords[0].polyterms[5][1] + Y3*coords[0].polyterms[6][1];
   }
 
@@ -301,39 +306,61 @@
 
   int i;
-  double determ;
-  double X, Y, Lo, Mo, dL, dM;
+  double Ro, Xo, Yo;
+  double dX, dY, X, Y, Lo, Mo, dL, dM;
+  double dLdX, dLdY, dMdX, dMdY, Do;
 
   *x = 0;
   *y = 0;
 
-  /* convert L,M to X,Y */
-  determ = 1.0 / (coords[0].pc1_1*coords[0].pc2_2 - coords[0].pc1_2*coords[0].pc2_1);
-  X = determ * (coords[0].pc2_2*L - coords[0].pc1_2*M);
-  Y = determ * (coords[0].pc1_1*M - coords[0].pc2_1*L);
+  /* start with linear solution for X,Y */
+  Ro = (coords[0].pc1_1*coords[0].pc2_2 - coords[0].pc1_2*coords[0].pc2_1);
+  Xo = (coords[0].pc2_2*L - coords[0].pc1_2*M) / Ro;
+  Yo = (coords[0].pc1_1*M - coords[0].pc2_1*L) / Ro;
 
   /** extra polynomial terms **/
   if (coords[0].Npolyterms > 1) {
     for (i = 0; i < 10; i++) {
-      Lo = (X*coords[0].pc1_1 + Y*coords[0].pc1_2);
-      Mo = (X*coords[0].pc2_1 + Y*coords[0].pc2_2);
+      // find derivatives at Xo, Yo
+      dLdX = coords[0].pc1_1 + 2.0*Xo*coords[0].polyterms[0][0] + Yo*coords[0].polyterms[1][0];
+      dLdY = coords[0].pc1_2 + 2.0*Yo*coords[0].polyterms[2][0] + Xo*coords[0].polyterms[1][0];
+      dMdX = coords[0].pc2_1 + 2.0*Xo*coords[0].polyterms[0][1] + Yo*coords[0].polyterms[1][1];
+      dMdY = coords[0].pc2_2 + 2.0*Yo*coords[0].polyterms[2][1] + Yo*coords[0].polyterms[1][1];
+
+      if (coords[0].Npolyterms > 2) {
+	dLdX += 3.0*Xo*Xo*coords[0].polyterms[3][0] + 2*Xo*Yo*coords[0].polyterms[4][0] + Yo*Yo*coords[0].polyterms[5][0];
+	dLdY += 3.0*Yo*Yo*coords[0].polyterms[6][0] + 2*Xo*Yo*coords[0].polyterms[5][0] + Xo*Xo*coords[0].polyterms[4][0];
+	dMdX += 3.0*Xo*Xo*coords[0].polyterms[3][1] + 2*Xo*Yo*coords[0].polyterms[4][1] + Yo*Yo*coords[0].polyterms[5][1];
+	dMdY += 3.0*Yo*Yo*coords[0].polyterms[6][1] + 2*Xo*Yo*coords[0].polyterms[5][1] + Xo*Xo*coords[0].polyterms[4][1];
+      }
+
+      // find Lo,Mo for Xo,Yo:
+      Lo = (Xo*coords[0].pc1_1 + Yo*coords[0].pc1_2);
+      Mo = (Xo*coords[0].pc2_1 + Yo*coords[0].pc2_2);
       if (coords[0].Npolyterms > 1) {
-	Lo += X*X*coords[0].polyterms[0][0] + X*Y*coords[0].polyterms[1][0] + Y*Y*coords[0].polyterms[2][0];
-	Mo += X*X*coords[0].polyterms[0][1] + X*Y*coords[0].polyterms[1][1] + Y*Y*coords[0].polyterms[2][1];
+	Lo += Xo*Xo*coords[0].polyterms[0][0] + Xo*Yo*coords[0].polyterms[1][0] + Yo*Yo*coords[0].polyterms[2][0];
+	Mo += Xo*Xo*coords[0].polyterms[0][1] + Xo*Yo*coords[0].polyterms[1][1] + Yo*Yo*coords[0].polyterms[2][1];
       }
       if (coords[0].Npolyterms > 2) {
-	Lo += X*X*X*coords[0].polyterms[3][0] + X*X*Y*coords[0].polyterms[4][0] + X*Y*Y*coords[0].polyterms[5][0] + Y*Y*Y*coords[0].polyterms[6][0];
-	Mo += X*X*X*coords[0].polyterms[3][1] + X*X*Y*coords[0].polyterms[4][1] + X*Y*Y*coords[0].polyterms[5][1] + Y*Y*Y*coords[0].polyterms[6][1];
+	Lo += Xo*Xo*Xo*coords[0].polyterms[3][0] + Xo*Xo*Yo*coords[0].polyterms[4][0] + Xo*Yo*Yo*coords[0].polyterms[5][0] + Yo*Yo*Yo*coords[0].polyterms[6][0];
+	Mo += Xo*Xo*Xo*coords[0].polyterms[3][1] + Xo*Xo*Yo*coords[0].polyterms[4][1] + Xo*Yo*Yo*coords[0].polyterms[5][1] + Yo*Yo*Yo*coords[0].polyterms[6][1];
       }
+
       dL = (L - Lo);
       dM = (M - Mo);
 
-      X += determ * (coords[0].pc2_2*dL - coords[0].pc1_2*dM);
-      Y += determ * (coords[0].pc1_1*dM - coords[0].pc2_1*dM);
+      Do = 1.0 / (dLdX * dMdY - dLdY * dMdX);
+      dX = (dL*dMdY - dM*dLdY) * Do;
+      dY = (dM*dLdX - dL*dMdX) * Do;
+
+      // fprintf (stderr, "X,Y + dX,dY : %f, %f : %f, %f\n", X, Y, dX, dY);
+
+      Xo += dX;
+      Yo += dY;
     }
   }
   /* check for correct size (iterate?) */
 
-  *x = X / coords[0].cdelt1 + coords[0].crpix1;
-  *y = Y / coords[0].cdelt2 + coords[0].crpix2;
+  *x = Xo / coords[0].cdelt1 + coords[0].crpix1;
+  *y = Yo / coords[0].cdelt2 + coords[0].crpix2;
 
   return (TRUE);