| 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 |
|