Index: trunk/psLib/src/math/psMinimizePolyFit.c
===================================================================
--- trunk/psLib/src/math/psMinimizePolyFit.c	(revision 41532)
+++ trunk/psLib/src/math/psMinimizePolyFit.c	(revision 41896)
@@ -35,4 +35,5 @@
 
 #include "psMinimizePolyFit.h"
+#include "psAbort.h"
 #include "psAssert.h"
 #include "psMinimizeLMM.h"  // For Gauss-Jordan routines
@@ -1225,4 +1226,153 @@
 
 /******************************************************************************
+VectorFitPolynomial2DCheb(myPoly, *mask, maskValue, *f, *fErr, *x, *y): This is
+a private routine which will fit a 2-D polynomial to a set of (x, y)-(f)
+pairs.  All non-NULL vectors must be of type PS_TYPE_F64.
+ 
+ *****************************************************************************/
+static bool VectorFitPolynomial2DCheb(
+    psPolynomial2D* myPoly,
+    const psVector *f,
+    const psVector *x,
+    const psVector *y)
+{
+    psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__);
+    PS_ASSERT_POLY_NON_NULL(myPoly, false);
+    PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false);
+    PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false);
+    PS_ASSERT_VECTOR_NON_NULL(f, false);
+    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false);
+    PS_ASSERT_VECTOR_NON_NULL(x, false);
+    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false);
+    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false);
+    PS_ASSERT_VECTOR_NON_NULL(y, false);
+    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false);
+    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false);
+
+    // Number of polynomial terms
+    int nXterm = 1 + myPoly->nX;      // Number of terms in x
+    int nYterm = 1 + myPoly->nY;      // Number of terms in y
+    int nTerm = nXterm * nYterm;      // Total number of terms
+    if (nXterm > 9) {
+	psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit: orders higher than 9 are not yet coded\n");
+	return false;
+    }
+    if (nYterm > 9) {
+	psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit: orders higher than 9 are not yet coded\n");
+	return false;
+    }
+
+    // determine scale factors
+    if (!psChebyshevSetScale (myPoly, x, 0)) { psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit.\n"); return false; }
+    if (!psChebyshevSetScale (myPoly, y, 1)) { psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit.\n"); return false; }
+
+    // generate normalized vectors
+    psVector *xNorm = psChebyshevNormVector (myPoly, x, 0);
+    psVector *yNorm = psChebyshevNormVector (myPoly, y, 1);
+
+    // generate the N cheb polynomials based on xNorm, yNorm
+    psArray *xPolySet = psArrayAlloc (nXterm);
+    for (int i = 0; i < nXterm; i++) {
+	xPolySet->data[i] = psChebyshevPolyVector (xNorm, i);
+    }
+    psArray *yPolySet = psArrayAlloc (nYterm);
+    for (int i = 0; i < nYterm; i++) {
+	yPolySet->data[i] = psChebyshevPolyVector (yNorm, i);
+    }
+
+    psF64 *fData = f->data.F64;         // Dereference f
+
+    psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix
+    psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector
+
+    // Initialize data structures (should not be able to fail)
+    psAssert (psImageInit(A, 0.0), "Could initialize data structures A");
+    psAssert (psVectorInit(B, 0.0), "Could initialize data structures B");
+
+    // Dereference stuff, to make the loop go faster
+    psF64 **matrix = A->data.F64;       // Dereference the least-squares matrix
+    psF64 *vector = B->data.F64;        // Dereference the least-squares vector
+
+    // loop over all elements of the data vector
+    for (int k = 0; k < x->n; k++) {
+
+	if (!finite(fData[k])) continue;
+    
+	// XXX can we only calculate the upper diagonal?
+	int nelem = 0;
+	for (int jx = 0; jx < nXterm; jx++) {
+	    psVector *jxCheb = xPolySet->data[jx];
+	    for (int jy = 0; jy < nYterm; jy++) {
+		psVector *jyCheb = yPolySet->data[jy];
+		psF64 chebValue = jxCheb->data.F64[k] * jyCheb->data.F64[k];
+		
+		vector[nelem] += fData[k] * chebValue;
+
+		int melem = 0;
+		for (int kx = 0; kx < nXterm; kx++) {
+		    psVector *kxCheb = xPolySet->data[kx];
+		    for (int ky = 0; ky < nYterm; ky++) {
+			psVector *kyCheb = yPolySet->data[ky];
+			matrix[nelem][melem] += chebValue * kxCheb->data.F64[k]*kyCheb->data.F64[k];
+			melem++;
+		    }
+		}
+		nelem++;
+	    }
+	}
+    }
+
+    if (psTraceGetLevel("psLib.math") >= 4) {
+        printf("Least-squares vector:\n");
+        for (int i = 0; i < nTerm; i++) {
+            printf("%f ", B->data.F64[i]);
+        }
+        printf("\n");
+        printf("Least-squares matrix:\n");
+        for (int i = 0; i < nTerm; i++) {
+            for (int j = 0; j < nTerm; j++) {
+                printf("%f ", A->data.F64[i][j]);
+            }
+            printf("\n");
+        }
+    }
+
+    bool status = false;
+    if (USE_GAUSS_JORDAN) {
+        status = psMatrixGJSolve(A, B);
+    } else {
+        status = psMatrixLUSolve(A, B);
+    }
+    if (!status) {
+	psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n");
+	goto escape;
+    } 
+
+    // unroll the result:
+    int nelem = 0;
+    for (int jx = 0; jx < nXterm; jx++) {
+	for (int jy = 0; jy < nYterm; jy++) {
+	    myPoly->coeff[jx][jy]    = B->data.F64[nelem];
+	    myPoly->coeffErr[jx][jy] = sqrt(A->data.F64[nelem][nelem]);
+	    nelem ++;
+	}
+    }
+    psFree(A);
+    psFree(B);
+
+    psFree (xNorm);
+    psFree (yNorm);
+    psFree (xPolySet);
+    psFree (yPolySet);
+
+    return true;
+
+escape:
+    psFree (A);
+    psFree (B);
+    return false;
+}
+
+/******************************************************************************
 psVectorFitPolynomial2D():  This routine fits a 2D polynomial of arbitrary
 degree (specified in poly) to the data points (x, y)-(f) and returns that
@@ -1240,5 +1390,5 @@
 {
     PS_ASSERT_POLY_NON_NULL(poly, false);
-    PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false);
+    // PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false);
 
     PS_ASSERT_VECTOR_NON_NULL(f, false);
@@ -1277,10 +1427,15 @@
         break;
     case PS_POLYNOMIAL_CHEB:
-        if (mask != NULL) {
-            psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n");
-        }
-        psError(PS_ERR_UNKNOWN, true, "2-D Chebyshev polynomial vector fitting has not been implemented.  Returning NULL.\n");
-        result = false;
-        break;
+      if (mask != NULL) {
+	  psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n");
+      }
+      if (fErr != NULL) {
+	  psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring error values for Chebyshev polynomials.\n");
+      }
+      result = VectorFitPolynomial2DCheb(poly, f64, x64, y64);
+      if (!result) {
+	  psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial.  Returning NULL.\n");
+      }
+      break;
     default:
         psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type.  Returning NULL.\n");