Debian Codegraph @

lib/util/matrix.c

raw
1 /*=============================================================================
2 matrix
3 ===============================================================================
4
5 Matrix math.
6
7 =============================================================================*/
8
9 #include <assert.h>
10 #include <math.h>
11 #include <stdlib.h>
12 #include <stdio.h>
13
14 #include "pm_c_util.h"
15 #include "mallocvar.h"
16 #include "nstring.h"
17
18 #include "matrix.h"
19
20
21 static double const epsilon = 1e-10;
22
23
24
25 static void
26 swap(double * const aP,
27 double * const bP) {
28
29 double const oldA = *aP;
30
31 *aP = *bP;
32 *bP = oldA;
33 }
34
35
36
37 static void
38 initializeWorkMatrices(unsigned int const n,
39 double ** const aInit,
40 const double * const cInit,
41 double *** const aP,
42 double ** const cP,
43 const char ** const errorP) {
44 /*----------------------------------------------------------------------------
45 Allocate memory for an n x n matrix, initialize it to the value of
46 aInit[], and return it as *aP.
47
48 Allocate memory for an n x 1 matrix, initialize it to the value of
49 cInit[], and return it as *cP.
50 -----------------------------------------------------------------------------*/
51 double ** a;
52 double * c;
53
54 MALLOCARRAY2(a, n, n);
55 if (a == NULL)
56 pm_asprintf(errorP, "Could not get memory for a %u x %u matrix", n, n);
57 else {
58 unsigned int i;
59 for (i = 0; i < n; ++i) {
60 unsigned int j;
61 for (j = 0; j < n; ++j)
62 a[i][j] = aInit[i][j];
63 }
64 MALLOCARRAY(c, n);
65 if (c == NULL)
66 pm_asprintf(errorP, "Could not get memory for a %u x 1 matrix", n);
67 else {
68 unsigned int i;
69 for (i = 0; i < n; ++i)
70 c[i] = cInit[i];
71
72 *errorP = NULL;
73 }
74 if (*errorP)
75 free(a);
76 }
77 *aP = a;
78 *cP = c;
79 }
80
81
82
83 static void
84 findLargestIthCoeff(unsigned int const n,
85 double ** const a,
86 unsigned int const i,
87 unsigned int * const istarP,
88 const char ** const errorP) {
89 /*----------------------------------------------------------------------------
90 Among the 'i'th and following rows in 'a' (which has 'n' total rows),
91 find the one with the largest 'i'th column.
92
93 And it had better be greater than zero; if not, we fail (return *errorP
94 non-null).
95
96 Return its index as *istarP.
97 -----------------------------------------------------------------------------*/
98 double maxSoFar;
99 unsigned int maxIdx;
100 unsigned int ii;
101
102 for (ii = i, maxSoFar = 0.0; ii < n; ++ii) {
103 double const thisA = fabs(a[ii][i]);
104 if (thisA >= maxSoFar) {
105 maxIdx = ii;
106 maxSoFar = thisA;
107 }
108 }
109 if (maxSoFar < epsilon) {
110 const char * const baseMsg = "Matrix equation has no unique solution";
111 if (pm_have_float_format())
112 pm_asprintf(errorP, "%s. (debug: coeff %u %e < %e)",
113 baseMsg, i, maxSoFar, epsilon);
114 else
115 pm_asprintf(errorP, "%s", baseMsg);
116 } else {
117 *istarP = maxIdx;
118 *errorP = NULL;
119 }
120 }
121
122
123
124 static void
125 eliminateOneUnknown(unsigned int const i,
126 unsigned int const n,
127 double ** const a,
128 double * const c,
129 const char ** const errorP) {
130
131 unsigned int maxRow;
132
133 findLargestIthCoeff(n, a, i, &maxRow, errorP);
134
135 if (!*errorP) {
136 /* swap rows 'i' and 'maxRow' in 'a' and 'c', so that the ith
137 row has the largest ith coefficient.
138 */
139 unsigned int j;
140 for (j = 0; j < n; j++)
141 swap(&a[maxRow][j], &a[i][j]);
142
143 swap(&c[maxRow], &c[i]);
144
145 /* Combine rows so that the ith coefficient in every row below
146 the ith is zero.
147 */
148 {
149 unsigned int ii;
150 for (ii = i+1; ii < n; ++ii) {
151 double const multiplier = a[ii][i] / a[i][i];
152 /* This is what we multiply the whole ith row by to make
153 its ith coefficient equal to that in the iith row.
154 */
155 unsigned int j;
156
157 /* Combine ith row into iith row so that the ith coefficient
158 in the iith is zero.
159 */
160 c[ii] = c[ii] - multiplier * c[i];
161
162 for (j = 0; j < n; ++j)
163 a[ii][j] = a[ii][j] - multiplier * a[i][j];
164
165 assert(a[ii][i] < epsilon);
166 }
167 }
168 *errorP = NULL;
169 }
170 }
171
172
173
174 void
175 pm_solvelineareq(double ** const aArg,
176 double * const x,
177 double * const cArg,
178 unsigned int const n,
179 const char ** const errorP) {
180 /*----------------------------------------------------------------------------
181 Solve the matrix equation 'a' * 'x' = 'c' for 'x'.
182
183 'n' is the dimension of the matrices. 'a' is 'n' x 'n',
184 while 'x' and 'c' are 'n' x 1.
185 -----------------------------------------------------------------------------*/
186 /* We use Gaussian reduction. */
187
188 double ** a;
189 double * c;
190
191 initializeWorkMatrices(n, aArg, cArg, &a, &c, errorP);
192
193 if (!*errorP) {
194 unsigned int i;
195
196 for (i = 0, *errorP = NULL; i < n && !*errorP; ++i)
197 eliminateOneUnknown(i, n, a, c, errorP);
198
199 if (!*errorP) {
200 /* a[] now has all zeros in the lower left triangle. */
201 /* Work from the bottom up to solve for the unknowns x[], from
202 the a and c rows in question and all the x[] below it
203 */
204
205 unsigned int k;
206 for (k = 0; k < n; ++k) {
207 unsigned int const m = n - k - 1;
208 unsigned int j;
209 double xwork;
210
211 for (j = m+1, xwork = c[m]; j < n; ++j)
212 xwork -= a[m][j] * x[j];
213
214 x[m] = xwork / a[m][m];
215 }
216 }
217 }
218 pm_freearray2((void**)a);
219 free(c);
220 }
221
222
223