Changeset 2897 for trunk/stac/src/stacInvertMaps.c

Timestamp:

Jan 4, 2005, 3:55:40 PM (22 years ago)

Author:

Paul Price

Message:

Updated to handle higher polynomial orders than linear, and to calculate the output size itself

File:

• trunk/stac/src/stacInvertMaps.c (modified) (3 diffs)

trunk/stac/src/stacInvertMaps.c

@@ -5 +5 @@
 
 #define MAX(x,y) ((x) > (y) ? (x) : (y))
-
-psArray *stacInvertMaps(const psArray *maps // Array of maps to invert
+#define NUM_GRID 20
+
+psArray *stacInvertMaps(const psArray *maps, // Array of maps to invert
+                        const stacConfig *config // Configuration
     )
 {
@@ -14 +16 @@
     psTrace("stac.invertMaps", 1, "Inverting maps....\n");
 
-    for (int i = 0; i < nMaps; i++) {
-        // Can't handle higher order than linear yet
-        if (((psPlaneTransform*)maps->data[i])->x->nX != 2 ||
-            ((psPlaneTransform*)maps->data[i])->x->nY != 2 ||
-            ((psPlaneTransform*)maps->data[i])->y->nX != 2 ||
-            ((psPlaneTransform*)maps->data[i])->y->nY != 2) {
-            psError("stac.invertMaps",
-                    "STAC cannot currently support orders other than linear.\n");
-            psFree(inverted);
-            return NULL;
-        }
-
-        psPlaneTransform *newMap = psPlaneTransformAlloc(2, 2); // Inverted map
-        psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[i]; // Uninverted map
-
-        // Now, simply do a 2x2 matrix inversion
-
-        double a = oldMap->x->coeff[1][0];
-        double b = oldMap->x->coeff[0][1];
-        double c = oldMap->y->coeff[1][0];
-        double d = oldMap->y->coeff[0][1];
-        double e = oldMap->x->coeff[0][0];
-        double f = oldMap->y->coeff[0][0];
-
-        double invDet = 1.0 / (a * d - b * c); // Inverse of the determinant
-
-        // Not entirely sure why this works, but it appears to do so.......................................
-        newMap->x->coeff[1][0] = invDet * a;
-        newMap->x->coeff[0][1] = - invDet * b;
-        newMap->y->coeff[1][0] = - invDet * c;
-        newMap->y->coeff[0][1] = invDet * d;
-
-        newMap->x->coeff[0][0] = - invDet * (d * e + c * f);
-        newMap->y->coeff[0][0] = - invDet * (b * e + a * f);
+    // Coordinates for the transformations
+    psPlane *inCoord = psAlloc(sizeof(psPlane));
+    psPlane *outCoord = psAlloc(sizeof(psPlane));
+
+    for (int mapNum = 0; mapNum < nMaps; mapNum++) {
+
+        psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[mapNum]; // Uninverted map
+        // Check input
+        if (oldMap->x->nX != oldMap->x->nY) {
+            psError("stac.invertMaps", "Polynomial order in x and y don't match for map %d\n", mapNum);
+            return NULL;
+        }
+        if (oldMap->y->nX != oldMap->y->nY) {
+            psError("stac.invertMaps", "Polynomial order in x and y don't match for map %d\n", mapNum);
+            return NULL;
+        }
+        if (oldMap->x->nX != oldMap->y->nX) {
+            psError("stac.invertMaps", "Polynomial order in x and y don't match for map %d\n", mapNum);
+            return NULL;
+        }
+        int order = oldMap->x->nX;      // Polynomial order
+        psTrace("stac.invertMaps", 4, "Generating order %d polynomial inverse transformation.\n", order);
+        psPlaneTransform *newMap = psPlaneTransformAlloc(order, order); // Inverted map
+
+        // Create fake polynomial to use in evaluation
+        psDPolynomial2D *fakePoly = psDPolynomial2DAlloc(order, order, PS_POLYNOMIAL_ORD);
+        for (int i = 0; i < order; i++) {
+            for (int j = 0; j < order; j++) {
+                fakePoly->coeff[i][j] = 1.0; // Set all coeffecients to 1
+                fakePoly->mask[i][j] = 1; // Mask all coefficients; unmask to evaluate
+            }
+        }
+
+        // A grid of xin,yin --> xout,yout
+        psVector *xIn = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+        psVector *yIn = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+        psVector *xOut = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+        psVector *yOut = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+
+        // Create grid of points
+        for (int yint = 0; yint < NUM_GRID; yint++) {
+            inCoord->y = (float)(yint * config->outny) / (float)(NUM_GRID - 1);
+            for (int xint = 0; xint < NUM_GRID; xint++) {
+                inCoord->x = (float)(xint * config->outnx) / (float)(NUM_GRID - 1);
+
+                (void)psPlaneTransformApply(outCoord, oldMap, inCoord);
+
+                xOut->data.F32[yint*NUM_GRID + xint] = inCoord->x;
+                yOut->data.F32[yint*NUM_GRID + xint] = inCoord->y;
+                xIn->data.F32[yint*NUM_GRID + xint] = outCoord->x;
+                yIn->data.F32[yint*NUM_GRID + xint] = outCoord->y;
+            }
+        }
+
+        // Initialise the matrix and vectors
+        int nCoeff = order * (order + 1) / 2; // Number of polynomial coefficients
+        psImage *matrix = psImageAlloc(nCoeff, nCoeff, PS_TYPE_F64); // Matrix for solution
+        psVector *xVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in x
+        psVector *yVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in y
+        for (int i = 0; i < nCoeff; i++) {
+            for (int j = 0; j < nCoeff; j++) {
+                matrix->data.F64[i][j] = 0.0;
+            }
+            xVector->data.F64[i] = 0.0;
+            yVector->data.F64[i] = 0.0;
+        }
+
+        // Iterate over the grid points
+        for (int g = 0; g < NUM_GRID*NUM_GRID; g++) {
+
+            // Iterate over the polynomial coefficients, accumulating the matrix and vectors
+            for (int i = 0, ijIndex = 0; i < order; i++) {
+                for (int j = 0; j < order - i; j++, ijIndex++) {
+
+                    fakePoly->mask[i][j] = 0;
+                    double ijPoly = psDPolynomial2DEval(xIn->data.F32[g], yIn->data.F32[g], fakePoly);
+                    fakePoly->mask[i][j] = 1;
+
+                    for (int m = 0, mnIndex = 0; m < order; m++) {
+                        for (int n = 0; n < order - m; n++, mnIndex++) {
+                            
+                            fakePoly->mask[m][n] = 0;
+                            double mnPoly = psDPolynomial2DEval(xIn->data.F32[g], yIn->data.F32[g], fakePoly);
+                            fakePoly->mask[m][n] = 1;
+
+                            matrix->data.F64[ijIndex][mnIndex] += ijPoly * mnPoly;
+                        }
+                    }
+                    
+                    xVector->data.F64[ijIndex] += ijPoly * (double)xOut->data.F32[g];
+                    yVector->data.F64[ijIndex] += ijPoly * (double)yOut->data.F32[g];
+                }
+            } // Iterating over coefficients
+        } // Iterating over grid points
+
+        // Solution via LU Decomposition
+        psVector *permutation = psVectorAlloc(nCoeff, PS_TYPE_F64); // Permutation vector for LU Decomposition
+        psImage *luMatrix = psMatrixLUD(NULL, permutation, matrix); // LU decomposed matrix
+        psVector *xSolution = psMatrixLUSolve(NULL, luMatrix, xVector, permutation); // Solution in x
+        psVector *ySolution = psMatrixLUSolve(NULL, luMatrix, yVector, permutation); // Solution in y
+
+        // Stuff coefficients into transformation
+        for (int i = 0, ijIndex = 0; i < order; i++) {
+            for (int j = 0; j < order - i; j++, ijIndex++) {
+                newMap->x->coeff[i][j] = xSolution->data.F64[ijIndex];
+                newMap->y->coeff[i][j] = ySolution->data.F64[ijIndex];
+            }
+        }
+        inverted->data[mapNum] = newMap;
+
+#ifdef TESTING
+        // Print x coefficients
+        psTrace("stac.invertMaps", 7, "x' = \n");
+        for (int i = 0; i < order; i++) {
+            for (int j = 0; j < order - i; j++) {
+                psTrace("stac.invertMaps", 7, "      %f x^%d y^%d\n", newMap->x->coeff[i][j], i, j);
+            }
+        }
+        // Print y coefficients
+        psTrace("stac.invertMaps", 7, "y' = \n");
+        for (int i = 0; i < order; i++) {
+            for (int j = 0; j < order - i; j++) {
+                psTrace("stac.invertMaps", 7, "      %f x^%d y^%d\n", newMap->y->coeff[i][j], i, j);
+            }
+        }
+#endif
 
 #ifdef TESTING
@@ -64 +158 @@
 #endif
 
-        inverted->data[i] = newMap;     // Stuff into the array
+        psFree(permutation);
+        psFree(luMatrix);
+        psFree(matrix);
+        psFree(xVector);
+        psFree(yVector);
+        psFree(xSolution);
+        psFree(ySolution);
+        psFree(fakePoly);
+        psFree(xIn);
+        psFree(yIn);
+        psFree(xOut);
+        psFree(yOut);
     }
+
+    psFree(inCoord);
+    psFree(outCoord);
+                
+
+#if 0
+        // Can't handle higher order than linear yet
+        if (((psPlaneTransform*)maps->data[i])->x->nX != 2 ||
+            ((psPlaneTransform*)maps->data[i])->x->nY != 2 ||
+            ((psPlaneTransform*)maps->data[i])->y->nX != 2 ||
+            ((psPlaneTransform*)maps->data[i])->y->nY != 2) {
+            psError("stac.invertMaps",
+                    "STAC cannot currently support orders other than linear.\n");
+            psFree(inverted);
+            return NULL;
+        }
+
+        psPlaneTransform *newMap = psPlaneTransformAlloc(2, 2); // Inverted map
+        psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[i]; // Uninverted map
+
+        // Now, simply do a 2x2 matrix inversion
+
+        double a = oldMap->x->coeff[1][0];
+        double b = oldMap->x->coeff[0][1];
+        double c = oldMap->y->coeff[1][0];
+        double d = oldMap->y->coeff[0][1];
+        double e = oldMap->x->coeff[0][0];
+        double f = oldMap->y->coeff[0][0];
+
+        double invDet = 1.0 / (a * d - b * c); // Inverse of the determinant
+
+        // Not entirely sure why this works, but it appears to do so.......................................
+        newMap->x->coeff[1][0] = invDet * a;
+        newMap->x->coeff[0][1] = - invDet * b;
+        newMap->y->coeff[1][0] = - invDet * c;
+        newMap->y->coeff[0][1] = invDet * d;
+
+        newMap->x->coeff[0][0] = - invDet * (d * e + c * f);
+        newMap->y->coeff[0][0] = - invDet * (b * e + a * f);
+#endif
+
 
     return inverted;