← Back to branch summary

~ubuntu-branches/ubuntu/vivid/grass/vivid-proposed

« back to all changes in this revision

Viewing changes to lib/imagery/georef.c

  • Committer: Package Import Robot
  • Author(s): Bas Couwenberg
  • Date: 2015-02-20 23:12:08 UTC
  • mfrom: (8.2.6 experimental)
  • Revision ID: package-import@ubuntu.com-20150220231208-1u6qvqm84v430b10
Tags: 7.0.0-1~exp1
* New upstream release.
* Update python-ctypes-ternary.patch to use if/else instead of and/or.
* Drop check4dev patch, rely on upstream check.
* Add build dependency on libpq-dev to grass-dev for libpq-fe.h.
* Drop patches applied upstream, refresh remaining patches.
* Update symlinks for images switched from jpg to png.

Show diffs side-by-side

added added

removed removed

Lines of Context:
lib/imagery/georef.c
3
3
 *
4
4
 * MODULE:       imagery library
5
5
 * AUTHOR(S):    Original author(s) name(s) unknown - written by CERL
 
6
 *               Written By: Brian J. Buckley
 
7
 *
 
8
 *               At: The Center for Remote Sensing
 
9
 *               Michigan State University
 
10
 *
6
11
 * PURPOSE:      Image processing library
7
12
 * COPYRIGHT:    (C) 1999, 2005 by the GRASS Development Team
8
13
 *
12
17
 *
13
18
 *****************************************************************************/
14
19
 
15
 
#include <grass/config.h>
 
20
/*
 
21
 *  Written: 12/19/91
 
22
 *
 
23
 *  Last Update: 12/26/91 Brian J. Buckley
 
24
 *  Last Update:  1/24/92 Brian J. Buckley
 
25
 *  Added printout of trnfile. Triggered by BDEBUG.
 
26
 *  Last Update:  1/27/92 Brian J. Buckley
 
27
 *  Fixed bug so that only the active control points were used.
 
28
 * 
 
29
 */
 
30
 
 
31
 
 
32
#include <stdlib.h>
 
33
#include <math.h>
 
34
#include <grass/gis.h>
16
35
#include <grass/imagery.h>
17
36
#include <signal.h>
18
37
 
19
 
static int floating_exception;
20
 
static void catch(int);
21
 
static double determinant(double, double,
22
 
                          double, double, double, double, double, double,
23
 
                          double);
24
 
 
25
 
/* find coefficients A,B,C for e2 = A + B*e1 + C*n1
26
 
 * also compute the reverse equations
27
 
 *
28
 
 * return 0 if no points
29
 
 *       -1 if not solvable
30
 
 *        1 if ok
31
 
 *
32
 
 * method is least squares.
33
 
 * the least squares problem reduces to solving the following
34
 
 * system of equations for A,B,C
35
 
 *
36
 
 *   s0*A + s1*B + s2*C = x0
37
 
 *   s1*A + s3*B + s4*C = x1
38
 
 *   s2*A + s4*B + s5*C = x2
39
 
 *
40
 
 * use Cramer's rule
41
 
 *
42
 
 *     | x0 s1 s2 |      | s0 x0 s2 |      | s0 s1 x0 |
43
 
 *     | x1 s3 s4 |      | s1 x1 s4 |      | s1 s3 x1 |
44
 
 *     | x2 s4 s5 |      | s2 x2 s5 |      | s2 s4 x2 |
45
 
 * A = ------------  B = ------------  C = ------------ 
46
 
 *     | s0 s1 s2 |      | s0 s1 s2 |      | s0 s1 s2 |
47
 
 *     | s1 s3 s4 |      | s1 s3 s4 |      | s1 s3 s4 |
48
 
 *     | s2 s4 s5 |      | s2 s4 s5 |      | s2 s4 s5 |
49
 
 *
50
 
 */
51
 
 
52
 
int I_compute_georef_equations(struct Control_Points *cp,
53
 
                               double E12[3], double N12[3], double E21[3],
54
 
                               double N21[3])
55
 
{
56
 
    RETSIGTYPE(*sigfpe) (int);
57
 
    double s0, s1, s2, s3, s4, s5;
58
 
    double x0, x1, x2;
59
 
    double det;
60
 
    int i;
61
 
 
62
 
 
63
 
    s0 = s1 = s2 = s3 = s4 = s5 = 0.0;
64
 
    for (i = 0; i < cp->count; i++) {
65
 
        if (cp->status[i] <= 0)
66
 
            continue;
67
 
        s0 += 1.0;
68
 
        s1 += cp->e1[i];
69
 
        s2 += cp->n1[i];
70
 
        s3 += cp->e1[i] * cp->e1[i];
71
 
        s4 += cp->e1[i] * cp->n1[i];
72
 
        s5 += cp->n1[i] * cp->n1[i];
73
 
    }
74
 
    if (s0 < 0.5)
75
 
        return 0;
76
 
 
77
 
    floating_exception = 0;
78
 
    sigfpe = signal(SIGFPE, catch);
79
 
 
80
 
    /* eastings */
81
 
    x0 = x1 = x2 = 0.0;
82
 
    for (i = 0; i < cp->count; i++) {
83
 
        if (cp->status[i] <= 0)
84
 
            continue;
85
 
        x0 += cp->e2[i];
86
 
        x1 += cp->e1[i] * cp->e2[i];
87
 
        x2 += cp->n1[i] * cp->e2[i];
88
 
    }
89
 
 
90
 
    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
91
 
    if (det == 0.0) {
92
 
        signal(SIGFPE, sigfpe);
93
 
        return -1;
94
 
    }
95
 
    E12[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
96
 
    E12[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
97
 
    E12[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
98
 
 
99
 
    /* northings */
100
 
    x0 = x1 = x2 = 0.0;
101
 
    for (i = 0; i < cp->count; i++) {
102
 
        if (cp->status[i] <= 0)
103
 
            continue;
104
 
        x0 += cp->n2[i];
105
 
        x1 += cp->e1[i] * cp->n2[i];
106
 
        x2 += cp->n1[i] * cp->n2[i];
107
 
    }
108
 
 
109
 
    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
110
 
    if (det == 0.0) {
111
 
        signal(SIGFPE, sigfpe);
112
 
        return -1;
113
 
    }
114
 
    N12[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
115
 
    N12[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
116
 
    N12[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
117
 
 
118
 
    /* the inverse equations */
119
 
 
120
 
    s0 = s1 = s2 = s3 = s4 = s5 = 0.0;
121
 
    for (i = 0; i < cp->count; i++) {
122
 
        if (cp->status[i] <= 0)
123
 
            continue;
124
 
        s0 += 1.0;
125
 
        s1 += cp->e2[i];
126
 
        s2 += cp->n2[i];
127
 
        s3 += cp->e2[i] * cp->e2[i];
128
 
        s4 += cp->e2[i] * cp->n2[i];
129
 
        s5 += cp->n2[i] * cp->n2[i];
130
 
    }
131
 
 
132
 
    /* eastings */
133
 
    x0 = x1 = x2 = 0.0;
134
 
    for (i = 0; i < cp->count; i++) {
135
 
        if (cp->status[i] <= 0)
136
 
            continue;
137
 
        x0 += cp->e1[i];
138
 
        x1 += cp->e2[i] * cp->e1[i];
139
 
        x2 += cp->n2[i] * cp->e1[i];
140
 
    }
141
 
 
142
 
    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
143
 
    if (det == 0.0) {
144
 
        signal(SIGFPE, sigfpe);
145
 
        return -1;
146
 
    }
147
 
    E21[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
148
 
    E21[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
149
 
    E21[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
150
 
 
151
 
    /* northings */
152
 
    x0 = x1 = x2 = 0.0;
153
 
    for (i = 0; i < cp->count; i++) {
154
 
        if (cp->status[i] <= 0)
155
 
            continue;
156
 
        x0 += cp->n1[i];
157
 
        x1 += cp->e2[i] * cp->n1[i];
158
 
        x2 += cp->n2[i] * cp->n1[i];
159
 
    }
160
 
 
161
 
    det = determinant(s0, s1, s2, s1, s3, s4, s2, s4, s5);
162
 
    if (det == 0.0) {
163
 
        signal(SIGFPE, sigfpe);
164
 
        return -1;
165
 
    }
166
 
    N21[0] = determinant(x0, s1, s2, x1, s3, s4, x2, s4, s5) / det;
167
 
    N21[1] = determinant(s0, x0, s2, s1, x1, s4, s2, x2, s5) / det;
168
 
    N21[2] = determinant(s0, s1, x0, s1, s3, x1, s2, s4, x2) / det;
169
 
 
170
 
    signal(SIGFPE, sigfpe);
171
 
    return floating_exception ? -1 : 1;
172
 
}
173
 
 
174
 
static double determinant(double a, double b, double c, double d, double e,
175
 
                          double f, double g, double h, double i)
176
 
{
177
 
    /* compute determinant of 3x3 matrix
178
 
     *     | a b c |
179
 
     *     | d e f |
180
 
     *     | g h i |
181
 
     */
182
 
    return a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
183
 
}
184
 
 
185
 
static void catch(int n)
186
 
{
187
 
    floating_exception = 1;
188
 
    signal(n, catch);
189
 
}
190
 
 
191
 
int I_georef(double e1, double n1,
192
 
             double *e2, double *n2, double E[3], double N[3])
193
 
{
194
 
    *e2 = E[0] + E[1] * e1 + E[2] * n1;
195
 
    *n2 = N[0] + N[1] * e1 + N[2] * n1;
196
 
 
197
 
    return 0;
 
38
/* STRUCTURE FOR USE INTERNALLY WITH THESE FUNCTIONS.  THESE FUNCTIONS EXPECT
 
39
   SQUARE MATRICES SO ONLY ONE VARIABLE IS GIVEN (N) FOR THE MATRIX SIZE */
 
40
 
 
41
struct MATRIX
 
42
{
 
43
    int n;                      /* SIZE OF THIS MATRIX (N x N) */
 
44
    double *v;
 
45
};
 
46
 
 
47
/* CALCULATE OFFSET INTO ARRAY BASED ON R/C */
 
48
 
 
49
#define M(row,col) m->v[(((row)-1)*(m->n))+(col)-1]
 
50
 
 
51
#define MSUCCESS     1          /* SUCCESS */
 
52
#define MNPTERR      0          /* NOT ENOUGH POINTS */
 
53
#define MUNSOLVABLE -1          /* NOT SOLVABLE */
 
54
#define MMEMERR     -2          /* NOT ENOUGH MEMORY */
 
55
#define MPARMERR    -3          /* PARAMETER ERROR */
 
56
#define MINTERR     -4          /* INTERNAL ERROR */
 
57
 
 
58
#define MAXORDER 3    /* HIGHEST SUPPORTED ORDER OF TRANSFORMATION */
 
59
 
 
60
/***********************************************************************
 
61
 
 
62
  FUNCTION PROTOTYPES FOR STATIC (INTERNAL) FUNCTIONS
 
63
 
 
64
************************************************************************/
 
65
 
 
66
static int calccoef(struct Control_Points *, double *, double *, int);
 
67
static int calcls(struct Control_Points *, struct MATRIX *, double *,
 
68
                  double *, double *, double *);
 
69
static int exactdet(struct Control_Points *, struct MATRIX *, double *,
 
70
                    double *, double *, double *);
 
71
static int solvemat(struct MATRIX *, double *, double *, double *, double *);
 
72
static double term(int, double, double);
 
73
 
 
74
/***********************************************************************
 
75
 
 
76
  TRANSFORM A SINGLE COORDINATE PAIR.
 
77
 
 
78
************************************************************************/
 
79
 
 
80
int I_georef(double e1,    /* EASTING TO BE TRANSFORMED */
 
81
             double n1,    /* NORTHING TO BE TRANSFORMED */
 
82
             double *e,    /* EASTING, TRANSFORMED */
 
83
             double *n,    /* NORTHING, TRANSFORMED */
 
84
             double E[],   /* EASTING COEFFICIENTS */
 
85
             double N[],   /* NORTHING COEFFICIENTS */
 
86
             int order     /* ORDER OF TRANSFORMATION TO BE PERFORMED, MUST MATCH THE
 
87
                              ORDER USED TO CALCULATE THE COEFFICIENTS */
 
88
    )
 
89
{
 
90
    double e3, e2n, en2, n3, e2, en, n2;
 
91
 
 
92
    switch (order) {
 
93
    case 1:
 
94
        *e = E[0] + E[1] * e1 + E[2] * n1;
 
95
        *n = N[0] + N[1] * e1 + N[2] * n1;
 
96
        break;
 
97
 
 
98
    case 2:
 
99
        e2 = e1 * e1;
 
100
        n2 = n1 * n1;
 
101
        en = e1 * n1;
 
102
 
 
103
        *e = E[0] + E[1] * e1 + E[2] * n1 + E[3] * e2 + E[4] * en + E[5] * n2;
 
104
        *n = N[0] + N[1] * e1 + N[2] * n1 + N[3] * e2 + N[4] * en + N[5] * n2;
 
105
        break;
 
106
 
 
107
    case 3:
 
108
        e2 = e1 * e1;
 
109
        en = e1 * n1;
 
110
        n2 = n1 * n1;
 
111
        e3 = e1 * e2;
 
112
        e2n = e2 * n1;
 
113
        en2 = e1 * n2;
 
114
        n3 = n1 * n2;
 
115
 
 
116
        *e = E[0] +
 
117
            E[1] * e1 + E[2] * n1 +
 
118
            E[3] * e2 + E[4] * en + E[5] * n2 +
 
119
            E[6] * e3 + E[7] * e2n + E[8] * en2 + E[9] * n3;
 
120
        *n = N[0] +
 
121
            N[1] * e1 + N[2] * n1 +
 
122
            N[3] * e2 + N[4] * en + N[5] * n2 +
 
123
            N[6] * e3 + N[7] * e2n + N[8] * en2 + N[9] * n3;
 
124
        break;
 
125
 
 
126
    default:
 
127
        return MPARMERR;
 
128
    }
 
129
 
 
130
    return MSUCCESS;
 
131
}
 
132
 
 
133
/***********************************************************************
 
134
 
 
135
  COMPUTE THE FORWARD AND BACKWARD GEOREFFERENCING COEFFICIENTS
 
136
  BASED ON A SET OF CONTROL POINTS
 
137
 
 
138
************************************************************************/
 
139
 
 
140
int I_compute_georef_equations(struct Control_Points *cp, double E12[],
 
141
                               double N12[], double E21[], double N21[],
 
142
                               int order)
 
143
{
 
144
    double *tempptr;
 
145
    int status;
 
146
 
 
147
    if (order < 1 || order > MAXORDER)
 
148
        return MPARMERR;
 
149
 
 
150
    /* CALCULATE THE FORWARD TRANSFORMATION COEFFICIENTS */
 
151
 
 
152
    status = calccoef(cp, E12, N12, order);
 
153
 
 
154
    if (status != MSUCCESS)
 
155
        return status;
 
156
 
 
157
    /* SWITCH THE 1 AND 2 EASTING AND NORTHING ARRAYS */
 
158
 
 
159
    tempptr = cp->e1;
 
160
    cp->e1 = cp->e2;
 
161
    cp->e2 = tempptr;
 
162
    tempptr = cp->n1;
 
163
    cp->n1 = cp->n2;
 
164
    cp->n2 = tempptr;
 
165
 
 
166
    /* CALCULATE THE BACKWARD TRANSFORMATION COEFFICIENTS */
 
167
 
 
168
    status = calccoef(cp, E21, N21, order);
 
169
 
 
170
    /* SWITCH THE 1 AND 2 EASTING AND NORTHING ARRAYS BACK */
 
171
 
 
172
    tempptr = cp->e1;
 
173
    cp->e1 = cp->e2;
 
174
    cp->e2 = tempptr;
 
175
    tempptr = cp->n1;
 
176
    cp->n1 = cp->n2;
 
177
    cp->n2 = tempptr;
 
178
 
 
179
    return status;
 
180
}
 
181
 
 
182
/***********************************************************************
 
183
 
 
184
  COMPUTE THE GEOREFFERENCING COEFFICIENTS
 
185
  BASED ON A SET OF CONTROL POINTS
 
186
 
 
187
************************************************************************/
 
188
 
 
189
static int calccoef(struct Control_Points *cp, double E[], double N[],
 
190
                    int order)
 
191
{
 
192
    struct MATRIX m;
 
193
    double *a;
 
194
    double *b;
 
195
    int numactive;              /* NUMBER OF ACTIVE CONTROL POINTS */
 
196
    int status, i;
 
197
 
 
198
    /* CALCULATE THE NUMBER OF VALID CONTROL POINTS */
 
199
 
 
200
    for (i = numactive = 0; i < cp->count; i++) {
 
201
        if (cp->status[i] > 0)
 
202
            numactive++;
 
203
    }
 
204
 
 
205
    /* CALCULATE THE MINIMUM NUMBER OF CONTROL POINTS NEEDED TO DETERMINE
 
206
       A TRANSFORMATION OF THIS ORDER */
 
207
 
 
208
    m.n = ((order + 1) * (order + 2)) / 2;
 
209
 
 
210
    if (numactive < m.n)
 
211
        return MNPTERR;
 
212
 
 
213
    /* INITIALIZE MATRIX */
 
214
 
 
215
    m.v = G_calloc(m.n * m.n, sizeof(double));
 
216
    a = G_calloc(m.n, sizeof(double));
 
217
    b = G_calloc(m.n, sizeof(double));
 
218
 
 
219
    if (numactive == m.n)
 
220
        status = exactdet(cp, &m, a, b, E, N);
 
221
    else
 
222
        status = calcls(cp, &m, a, b, E, N);
 
223
 
 
224
    G_free(m.v);
 
225
    G_free(a);
 
226
    G_free(b);
 
227
 
 
228
    return status;
 
229
}
 
230
 
 
231
/***********************************************************************
 
232
 
 
233
  CALCULATE THE TRANSFORMATION COEFFICIENTS WITH EXACTLY THE MINIMUM
 
234
  NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.
 
235
 
 
236
************************************************************************/
 
237
 
 
238
static int exactdet(struct Control_Points *cp, struct MATRIX *m,
 
239
                    double a[], double b[],
 
240
                    double E[], /* EASTING COEFFICIENTS */
 
241
                    double N[]  /* NORTHING COEFFICIENTS */
 
242
    )
 
243
{
 
244
    int pntnow, currow, j;
 
245
 
 
246
    currow = 1;
 
247
    for (pntnow = 0; pntnow < cp->count; pntnow++) {
 
248
        if (cp->status[pntnow] > 0) {
 
249
            /* POPULATE MATRIX M */
 
250
 
 
251
            for (j = 1; j <= m->n; j++)
 
252
                M(currow, j) = term(j, cp->e1[pntnow], cp->n1[pntnow]);
 
253
 
 
254
            /* POPULATE MATRIX A AND B */
 
255
 
 
256
            a[currow - 1] = cp->e2[pntnow];
 
257
            b[currow - 1] = cp->n2[pntnow];
 
258
 
 
259
            currow++;
 
260
        }
 
261
    }
 
262
 
 
263
    if (currow - 1 != m->n)
 
264
        return MINTERR;
 
265
 
 
266
    return solvemat(m, a, b, E, N);
 
267
}
 
268
 
 
269
/***********************************************************************
 
270
 
 
271
  CALCULATE THE TRANSFORMATION COEFFICIENTS WITH MORE THAN THE MINIMUM
 
272
  NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.  THIS
 
273
  ROUTINE USES THE LEAST SQUARES METHOD TO COMPUTE THE COEFFICIENTS.
 
274
 
 
275
************************************************************************/
 
276
 
 
277
static int calcls(struct Control_Points *cp, struct MATRIX *m,
 
278
                  double a[], double b[],
 
279
                  double E[],   /* EASTING COEFFICIENTS */
 
280
                  double N[]    /* NORTHING COEFFICIENTS */
 
281
    )
 
282
{
 
283
    int i, j, n, numactive = 0;
 
284
 
 
285
    /* INITIALIZE THE UPPER HALF OF THE MATRIX AND THE TWO COLUMN VECTORS */
 
286
 
 
287
    for (i = 1; i <= m->n; i++) {
 
288
        for (j = i; j <= m->n; j++)
 
289
            M(i, j) = 0.0;
 
290
        a[i - 1] = b[i - 1] = 0.0;
 
291
    }
 
292
 
 
293
    /* SUM THE UPPER HALF OF THE MATRIX AND THE COLUMN VECTORS ACCORDING TO
 
294
       THE LEAST SQUARES METHOD OF SOLVING OVER DETERMINED SYSTEMS */
 
295
 
 
296
    for (n = 0; n < cp->count; n++) {
 
297
        if (cp->status[n] > 0) {
 
298
            numactive++;
 
299
            for (i = 1; i <= m->n; i++) {
 
300
                for (j = i; j <= m->n; j++)
 
301
                    M(i, j) +=
 
302
                        term(i, cp->e1[n], cp->n1[n]) * term(j, cp->e1[n],
 
303
                                                             cp->n1[n]);
 
304
 
 
305
                a[i - 1] += cp->e2[n] * term(i, cp->e1[n], cp->n1[n]);
 
306
                b[i - 1] += cp->n2[n] * term(i, cp->e1[n], cp->n1[n]);
 
307
            }
 
308
        }
 
309
    }
 
310
 
 
311
    if (numactive <= m->n)
 
312
        return MINTERR;
 
313
 
 
314
    /* TRANSPOSE VALUES IN UPPER HALF OF M TO OTHER HALF */
 
315
 
 
316
    for (i = 2; i <= m->n; i++)
 
317
        for (j = 1; j < i; j++)
 
318
            M(i, j) = M(j, i);
 
319
 
 
320
    return solvemat(m, a, b, E, N);
 
321
}
 
322
 
 
323
/***********************************************************************
 
324
 
 
325
  CALCULATE THE X/Y TERM BASED ON THE TERM NUMBER
 
326
 
 
327
  ORDER\TERM   1    2    3    4    5    6    7    8    9   10
 
328
  1            e0n0 e1n0 e0n1
 
329
  2            e0n0 e1n0 e0n1 e2n0 e1n1 e0n2
 
330
  3            e0n0 e1n0 e0n1 e2n0 e1n1 e0n2 e3n0 e2n1 e1n2 e0n3
 
331
 
 
332
************************************************************************/
 
333
 
 
334
static double term(int term, double e, double n)
 
335
{
 
336
    switch (term) {
 
337
    case 1:
 
338
        return 1.0;
 
339
    case 2:
 
340
        return e;
 
341
    case 3:
 
342
        return n;
 
343
    case 4:
 
344
        return e * e;
 
345
    case 5:
 
346
        return e * n;
 
347
    case 6:
 
348
        return n * n;
 
349
    case 7:
 
350
        return e * e * e;
 
351
    case 8:
 
352
        return e * e * n;
 
353
    case 9:
 
354
        return e * n * n;
 
355
    case 10:
 
356
        return n * n * n;
 
357
    }
 
358
 
 
359
    return 0.0;
 
360
}
 
361
 
 
362
/***********************************************************************
 
363
 
 
364
  SOLVE FOR THE 'E' AND 'N' COEFFICIENTS BY USING A SOMEWHAT MODIFIED
 
365
  GAUSSIAN ELIMINATION METHOD.
 
366
 
 
367
  | M11 M12 ... M1n | | E0   |   | a0   |
 
368
  | M21 M22 ... M2n | | E1   | = | a1   |
 
369
  |  .   .   .   .  | | .    |   | .    |
 
370
  | Mn1 Mn2 ... Mnn | | En-1 |   | an-1 |
 
371
 
 
372
  and
 
373
 
 
374
  | M11 M12 ... M1n | | N0   |   | b0   |
 
375
  | M21 M22 ... M2n | | N1   | = | b1   |
 
376
  |  .   .   .   .  | | .    |   | .    |
 
377
  | Mn1 Mn2 ... Mnn | | Nn-1 |   | bn-1 |
 
378
 
 
379
************************************************************************/
 
380
 
 
381
static int solvemat(struct MATRIX *m, double a[], double b[], double E[],
 
382
                    double N[])
 
383
{
 
384
    int i, j, i2, j2, imark;
 
385
    double factor, temp;
 
386
    double pivot;               /* ACTUAL VALUE OF THE LARGEST PIVOT CANDIDATE */
 
387
 
 
388
    for (i = 1; i <= m->n; i++) {
 
389
        j = i;
 
390
 
 
391
        /* find row with largest magnitude value for pivot value */
 
392
 
 
393
        pivot = M(i, j);
 
394
        imark = i;
 
395
        for (i2 = i + 1; i2 <= m->n; i2++) {
 
396
            temp = fabs(M(i2, j));
 
397
            if (temp > fabs(pivot)) {
 
398
                pivot = M(i2, j);
 
399
                imark = i2;
 
400
            }
 
401
        }
 
402
 
 
403
        /* if the pivot is very small then the points are nearly co-linear */
 
404
        /* co-linear points result in an undefined matrix, and nearly */
 
405
        /* co-linear points results in a solution with rounding error */
 
406
 
 
407
        if (pivot == 0.0)
 
408
            return MUNSOLVABLE;
 
409
 
 
410
        /* if row with highest pivot is not the current row, switch them */
 
411
 
 
412
        if (imark != i) {
 
413
            for (j2 = 1; j2 <= m->n; j2++) {
 
414
                temp = M(imark, j2);
 
415
                M(imark, j2) = M(i, j2);
 
416
                M(i, j2) = temp;
 
417
            }
 
418
 
 
419
            temp = a[imark - 1];
 
420
            a[imark - 1] = a[i - 1];
 
421
            a[i - 1] = temp;
 
422
 
 
423
            temp = b[imark - 1];
 
424
            b[imark - 1] = b[i - 1];
 
425
            b[i - 1] = temp;
 
426
        }
 
427
 
 
428
        /* compute zeros above and below the pivot, and compute
 
429
           values for the rest of the row as well */
 
430
 
 
431
        for (i2 = 1; i2 <= m->n; i2++) {
 
432
            if (i2 != i) {
 
433
                factor = M(i2, j) / pivot;
 
434
                for (j2 = j; j2 <= m->n; j2++)
 
435
                    M(i2, j2) -= factor * M(i, j2);
 
436
                a[i2 - 1] -= factor * a[i - 1];
 
437
                b[i2 - 1] -= factor * b[i - 1];
 
438
            }
 
439
        }
 
440
    }
 
441
 
 
442
    /* SINCE ALL OTHER VALUES IN THE MATRIX ARE ZERO NOW, CALCULATE THE
 
443
       COEFFICIENTS BY DIVIDING THE COLUMN VECTORS BY THE DIAGONAL VALUES. */
 
444
 
 
445
    for (i = 1; i <= m->n; i++) {
 
446
        E[i - 1] = a[i - 1] / M(i, i);
 
447
        N[i - 1] = b[i - 1] / M(i, i);
 
448
    }
 
449
 
 
450
    return MSUCCESS;
198
451
}