Index: trunk/psLib/src/math/psMinimizePolyFit.c
===================================================================
--- trunk/psLib/src/math/psMinimizePolyFit.c	(revision 7104)
+++ trunk/psLib/src/math/psMinimizePolyFit.c	(revision 7107)
@@ -10,6 +10,6 @@
  *  @author EAM, IfA
  *
- *  @version $Revision: 1.11 $ $Name: not supported by cvs2svn $
- *  @date $Date: 2006-05-10 11:38:55 $
+ *  @version $Revision: 1.12 $ $Name: not supported by cvs2svn $
+ *  @date $Date: 2006-05-10 13:24:57 $
  *
  *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
@@ -1415,18 +1415,14 @@
     }
 
-    psImage    *A = NULL;
-    psVector   *B = NULL;
-    psF64 ***Sums = NULL;
-    psF64 wt;
-    psS32 nTerm;
-
-    psS32 nXterm = 1 + myPoly->nX;
-    psS32 nYterm = 1 + myPoly->nY;
-    psS32 nZterm = 1 + myPoly->nZ;
-    nTerm = nXterm * nYterm * nZterm;
-
-    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
-    B = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    int nXterm = 1 + myPoly->nX;        // Number of x terms
+    int nYterm = 1 + myPoly->nY;        // Number of y terms
+    int nZterm = 1 + myPoly->nZ;        // Number of z terms
+    int nTerm = nXterm * nYterm * nZterm; // Total number of terms
+    int nData = x->n;                   // Number of data points
+    psImage    *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix
+    psVector   *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector
     B->n = B->nalloc;
+
     // Initialize data structures.
     if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) {
@@ -1439,75 +1435,72 @@
     }
 
-    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+    // Dereference points for speed in the loop
+    psF64 **matrix = A->data.F64;       // Least-squares matrix
+    psF64 *vector = B->data.F64;        // Least-squares vector
+    psF64 *xData = x->data.F64;         // x
+    psF64 *yData = y->data.F64;         // y
+    psF64 *zData = z->data.F64;         // z
+    psF64 *fData = f->data.F64;         // f
+    psF64 *fErrData = NULL;             // Error in f
+    if (fErr) {
+        fErrData = fErr->data.F64;
+    }
+    psU8 *dataMask = NULL;              // Mask for data
+    if (mask) {
+        dataMask = mask->data.U8;
+    }
+    psU8 ***termMask = myPoly->mask;    // Mask for polynomial terms
+    int nXYterm = nXterm * nYterm;      // Multiplication of the numbers, to calculate the index
 
     // Build the B and A data structs.
-    for (psS32 k  = 0; k < x->n; k++) {
-        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
+    psF64 ***Sums = NULL;         // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+    for (int k = 0; k < nData; k++) {
+        if (dataMask && dataMask[k] & maskValue) {
             continue;
         }
 
-        Sums = BuildSums3D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], nXterm, nYterm, nZterm);
-
+        Sums = BuildSums3D(Sums, xData[k], yData[k], zData[k], nXterm, nYterm, nZterm);
+
+        double wt;
         if (fErr == NULL) {
             wt = 1.0;
         } else {
             // this filters fErr == 0 values
-            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
-        }
-
-        // we could skip half of the array and assign at the end
-        // we must handle masked orders
-        for (psS32 ix = 0; ix < nXterm; ix++) {
-            for (psS32 iy = 0; iy < nYterm; iy++) {
-                for (psS32 iz = 0; iz < nZterm; iz++) {
-                    if (myPoly->mask[ix][iy][iz]) {
-                        continue;
-                    } else {
-                        psS32 nx = ix + iy*nXterm + iz*nXterm*nYterm;
-                        B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz] * wt;
-                    }
-                }
-            }
-        }
-
-        for (psS32 ix = 0; ix < nXterm; ix++) {
-            for (psS32 iy = 0; iy < nYterm; iy++) {
-                for (psS32 iz = 0; iz < nZterm; iz++) {
-                    if (myPoly->mask[ix][iy][iz])
-                        continue;
-                    psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm;
-                    for (psS32 jx = 0; jx < nXterm; jx++) {
-                        for (psS32 jy = 0; jy < nYterm; jy++) {
-                            for (psS32 jz = 0; jz < nZterm; jz++) {
-                                if (myPoly->mask[jx][jy][jz])
-                                    continue;
-                                psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm;
-                                A->data.F64[nx][ny] += Sums[ix+jx][iy+jy][iz+jz] * wt;
-                            }
-                        }
-                    }
-                }
-            }
-        }
-    }
-
-    for (psS32 ix = 0; ix < nXterm; ix++) {
-        for (psS32 iy = 0; iy < nYterm; iy++) {
-            for (psS32 iz = 0; iz < nZterm; iz++) {
-                if (!myPoly->mask[ix][iy][iz])
+            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]);
+        }
+
+        for (int i = 0; i < nTerm; i++) {
+            int iz = i / nXYterm; // z index
+            int iy = (i % nXYterm) / nXterm; // y index
+            int ix = (i % nXYterm) % nXterm; // x index
+            if (termMask[ix][iy][iz]) {
+                continue;
+            }
+
+            vector[i] += fData[k] * Sums[ix][iy][iz] * wt;
+            matrix[i][i] += Sums[2*ix][2*iy][2*iz] * wt;
+            for (int j = i + 1; j < nTerm; j++) {
+                int jz = j / (nXterm * nYterm); // z index
+                int jy = (j % (nXterm * nYterm)) / nXterm; // y index
+                int jx = (j % (nXterm * nYterm)) % nXterm; // x index
+                if (termMask[jx][jy][jz]) {
                     continue;
-                psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm;
-                B->data.F64[nx] = 0;
-                for (psS32 jx = 0; jx < nXterm; jx++) {
-                    for (psS32 jy = 0; jy < nYterm; jy++) {
-                        for (psS32 jz = 0; jz < nZterm; jz++) {
-                            psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm;
-                            A->data.F64[nx][ny] = (nx == ny) ? 1 : 0;
-                        }
-                    }
-                }
-            }
-        }
-    }
+                }
+                double value = Sums[ix+jx][iy+jy][iz+jz] * wt;
+                matrix[i][j] += value;
+                matrix[j][i] += value;
+            }
+        }
+    }
+
+    // Free the sums
+    for (psS32 ix = 0; ix < 2*nXterm; ix++) {
+        for (psS32 iy = 0; iy < 2*nYterm; iy++) {
+            psFree(Sums[ix][iy]);
+        }
+        psFree(Sums[ix]);
+    }
+    psFree(Sums);
+
 
     // XXX: rel10_ifa used psMatrixGJSolve().  However, this failed tests.  So, I'm using psMatrixLUD().
@@ -1518,12 +1511,4 @@
             psFree(A);
             psFree(B);
-
-            for (psS32 ix = 0; ix < 2*nXterm; ix++) {
-                for (psS32 iy = 0; iy < 2*nYterm; iy++) {
-                    psFree(Sums[ix][iy]);
-                }
-                psFree(Sums[ix]);
-            }
-            psFree(Sums);
             psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n");
             return(NULL);
@@ -1578,12 +1563,4 @@
     psFree(A);
     psFree(B);
-
-    for (psS32 ix = 0; ix < 2*nXterm; ix++) {
-        for (psS32 iy = 0; iy < 2*nYterm; iy++) {
-            psFree(Sums[ix][iy]);
-        }
-        psFree(Sums[ix]);
-    }
-    psFree(Sums);
 
     psTrace(__func__, 4, "---- %s() end ----\n", __func__);
@@ -1961,20 +1938,15 @@
     }
 
-    // I think this is 1 dimension down
-    psImage     *A = NULL;
-    psVector    *B = NULL;
-    psF64 ****Sums = NULL;
-    psF64 wt;
-    psS32 nTerm;
-
-    psS32 nXterm = 1 + myPoly->nX;
-    psS32 nYterm = 1 + myPoly->nY;
-    psS32 nZterm = 1 + myPoly->nZ;
-    psS32 nTterm = 1 + myPoly->nZ;
-    nTerm = nXterm * nYterm * nZterm * nTterm;
-
-    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
-    B = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    int nXterm = 1 + myPoly->nX;        // Number of x terms
+    int nYterm = 1 + myPoly->nY;        // Number of y terms
+    int nZterm = 1 + myPoly->nZ;        // Number of z terms
+    int nTterm = 1 + myPoly->nT;        // Number of t terms
+    int nTerm = nXterm * nYterm * nZterm * nTterm; // Total number of terms
+    int nData = x->n;                   // Number of data points
+    psImage    *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix
+    psVector   *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector
     B->n = B->nalloc;
+
     // Initialize data structures.
     if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) {
@@ -1987,83 +1959,78 @@
     }
 
-    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+    // Dereference points for speed in the loop
+    psF64 **matrix = A->data.F64;       // Least-squares matrix
+    psF64 *vector = B->data.F64;        // Least-squares vector
+    psF64 *xData = x->data.F64;         // x
+    psF64 *yData = y->data.F64;         // y
+    psF64 *zData = z->data.F64;         // z
+    psF64 *tData = t->data.F64;         // t
+    psF64 *fData = f->data.F64;         // f
+    psF64 *fErrData = NULL;             // Error in f
+    if (fErr) {
+        fErrData = fErr->data.F64;
+    }
+    psU8 *dataMask = NULL;              // Mask for data
+    if (mask) {
+        dataMask = mask->data.U8;
+    }
+    psU8 ****termMask = myPoly->mask;    // Mask for polynomial terms
+    int nXYZterm = nXterm * nYterm * nZterm; // Multiplication of the numbers, for calculating the index
+    int nXYterm = nXterm * nYterm;      // Multiplication of the numbers, for calculating the index
 
     // Build the B and A data structs.
-    for (psS32 k  = 0; k < x->n; k++) {
-        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
+    psF64 ****Sums = NULL;        // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+    for (int k = 0; k < nData; k++) {
+        if (dataMask && dataMask[k] & maskValue) {
             continue;
         }
 
-        Sums = BuildSums4D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], t->data.F64[k], nXterm, nYterm, nZterm, nTterm);
-
+        Sums = BuildSums4D(Sums, xData[k], yData[k], zData[k], tData[k], nXterm, nYterm, nZterm, nTterm);
+
+        double wt;
         if (fErr == NULL) {
             wt = 1.0;
         } else {
             // this filters fErr == 0 values
-            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
-        }
-
-        // we could skip half of the array and assign at the end
-        // we must handle masked orders
-        for (psS32 ix = 0; ix < nXterm; ix++) {
-            for (psS32 iy = 0; iy < nYterm; iy++) {
-                for (psS32 iz = 0; iz < nZterm; iz++) {
-                    for (psS32 it = 0; it < nTterm; it++) {
-                        if (myPoly->mask[ix][iy][iz][it])
-                            continue;
-                        psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
-                        B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz][it] * wt;
-                    }
-                }
-            }
-        }
-
-        for (psS32 ix = 0; ix < nXterm; ix++) {
-            for (psS32 iy = 0; iy < nYterm; iy++) {
-                for (psS32 iz = 0; iz < nZterm; iz++) {
-                    for (psS32 it = 0; it < nTterm; it++) {
-                        if (myPoly->mask[ix][iy][iz][it])
-                            continue;
-                        psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
-                        for (psS32 jx = 0; jx < nXterm; jx++) {
-                            for (psS32 jy = 0; jy < nYterm; jy++) {
-                                for (psS32 jz = 0; jz < nZterm; jz++) {
-                                    for (psS32 jt = 0; jt < nTterm; jt++) {
-                                        if (myPoly->mask[jx][jy][jz][jt])
-                                            continue;
-                                        psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm;
-                                        A->data.F64[nx][ny]+= Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt;
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        }
-    }
-
-    for (psS32 ix = 0; ix < nXterm; ix++) {
-        for (psS32 iy = 0; iy < nYterm; iy++) {
-            for (psS32 iz = 0; iz < nZterm; iz++) {
-                for (psS32 it = 0; it < nTterm; it++) {
-                    if (!myPoly->mask[ix][iy][iz][it])
-                        continue;
-                    psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
-                    B->data.F64[nx] = 0;
-                    for (psS32 jx = 0; jx < nXterm; jx++) {
-                        for (psS32 jy = 0; jy < nYterm; jy++) {
-                            for (psS32 jz = 0; jz < nZterm; jz++) {
-                                for (psS32 jt = 0; jt < nTterm; jt++) {
-                                    psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm;
-                                    A->data.F64[nx][ny] = (nx == ny) ? 1 : 0;
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        }
-    }
+            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]);
+        }
+
+        for (int i = 0; i < nTerm; i++) {
+            int it = i / (nXYZterm); // t index
+            int iz = (i % (nXYZterm)) / (nXYterm); // z index
+            int iy = ((i % (nXYZterm)) % (nXYterm)) / nXterm; // y index
+            int ix = ((i % (nXYZterm)) % (nXYterm)) % nXterm; // x index
+            if (termMask[ix][iy][iz][it]) {
+                continue;
+            }
+
+            vector[i] += fData[k] * Sums[ix][iy][iz][it] * wt;
+            matrix[i][i] += Sums[2*ix][2*iy][2*iz][2*it] * wt;
+            for (int j = i + 1; j < nTerm; j++) {
+                int jt = j / nXYZterm; // t index
+                int jz = (j % nXYZterm) / nXYterm; // z index
+                int jy = ((j % nXYZterm) % nXYterm) / nXterm; // y index
+                int jx = ((j % nXYZterm) % nXYterm) % nXterm; // x index
+                if (termMask[jx][jy][jz][jt]) {
+                    continue;
+                }
+                double value = Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt;
+                matrix[i][j] += value;
+                matrix[j][i] += value;
+            }
+        }
+    }
+
+    // Free the sums
+    for (int ix = 0; ix < 2*nXterm; ix++) {
+        for (int iy = 0; iy < 2*nYterm; iy++) {
+            for (int iz = 0; iz < 2*nZterm; iz++) {
+                psFree(Sums[ix][iy][iz]);
+            }
+            psFree(Sums[ix][iy]);
+        }
+        psFree(Sums[ix]);
+    }
+    psFree(Sums);
 
 
@@ -2072,17 +2039,7 @@
         // does the solution in place
         // The GaussJordan version was overflowing, so I'm using LUD.
-        if (psMatrixGJSolve(A, B)) {
+        if (!psMatrixGJSolve(A, B)) {
             psFree(A);
             psFree(B);
-            for (psS32 ix = 0; ix < 2*nXterm; ix++) {
-                for (psS32 iy = 0; iy < 2*nYterm; iy++) {
-                    for (psS32 iz = 0; iz < 2*nZterm; iz++) {
-                        psFree(Sums[ix][iy][iz]);
-                    }
-                    psFree(Sums[ix][iy]);
-                }
-                psFree(Sums[ix]);
-            }
-            psFree(Sums);
             psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n");
             return(NULL);
@@ -2145,15 +2102,4 @@
     psFree(B);
 
-    for (psS32 ix = 0; ix < 2*nXterm; ix++) {
-        for (psS32 iy = 0; iy < 2*nYterm; iy++) {
-            for (psS32 iz = 0; iz < 2*nZterm; iz++) {
-                psFree(Sums[ix][iy][iz]);
-            }
-            psFree(Sums[ix][iy]);
-        }
-        psFree(Sums[ix]);
-    }
-    psFree(Sums);
-
     psTrace(__func__, 4, "---- %s() end ----\n", __func__);
     return (myPoly);