39 #include "sinfo_matrix.h"
50 #define dtiny(a) ((a)<0?(a)> -1.e-30:(a)<1.e-30)
55 gauss_pivot(
double *ptra,
double *ptrc,
int n);
72 sinfo_create_mx(
int nr,
int nc)
75 b = (Matrix) cpl_calloc(1,
sizeof(sinfo_eclipse_matrix));
76 b->m = (
double*) cpl_calloc(nr * nc,
sizeof(
double));
92 sinfo_copy_mx(Matrix a)
94 Matrix b = sinfo_create_mx(a->nr, a->nc);
96 register int s = a->nr * a->nc;
97 register double *mm = b->m + s;
98 register double *am = a->m + s;
115 sinfo_close_mx(Matrix a)
136 sinfo_mul_mx(Matrix a, Matrix b)
139 int n1 = a->nr, n2 = a->nc, n3 = b->nc;
143 register int i, j, k;
147 c = sinfo_create_mx(n1, n3);
148 d = sinfo_transp_mx(b);
150 for (i = 0, c0 = c->m; i < n1; i++)
151 for (j = 0, d0 = d->m; j < n3; j++, c0++)
152 for (k = 0, *c0 = 0, a0 = a->m + i * n2; k < n2; k++)
153 *c0 += *a0++ * *d0++;
170 sinfo_invert_mx(Matrix aa)
175 if (aa->nr != aa->nc)
177 bb = sinfo_create_mx(aa->nr, aa->nc);
188 else if (aa->nr == 2) {
191 register double *mm = aa->m;
192 double a = *(mm++), b = *(mm++);
193 double c = *(mm++), d = *(mm);
199 *(mm++) = d * ted, *(mm++) = -b * ted;
200 *(mm++) = -c * ted, *(mm) = a * ted;
202 else if (aa->nr == 3) {
205 register double *mm = aa->m;
206 double a = *(mm++), b = *(mm++), c = *(mm++);
207 double d = *(mm++), e = *(mm++), f = *(mm++);
208 double g = *(mm++), h = *(mm++), i = *(mm);
209 det = a * e * i - a * h * f - b * d * i + b * g * f + c * d * h
215 *(mm++) = (e * i - f * h) * ted, *(mm++) = (c * h - b * i) * ted, *(mm++) =
216 (b * f - e * c) * ted;
218 *(mm++) = (f * g - d * i) * ted, *(mm++) = (a * i - g * c) * ted, *(mm++) =
219 (d * c - a * f) * ted;
221 *(mm++) = (d * h - g * e) * ted, *(mm++) = (g * b - a * h) * ted, *(mm) =
222 (a * e - d * b) * ted;
225 Matrix temp = sinfo_copy_mx(aa);
226 if (gauss_pivot(temp->m, bb->m, aa->nr) == 0)
228 sinfo_close_mx(temp);
247 sinfo_transp_mx(Matrix a)
249 register int nc = a->nc, nr = a->nr;
253 Matrix b = sinfo_create_mx(nc, nr);
255 if (b == (Matrix) NULL )
257 for (i = 0, b0 = b->m; i < nc; i++)
258 for (j = 0, a0 = a->m + i; j < nr; j++, a0 += nc, b0++)
276 gauss_pivot(
double *ptra,
double *ptrc,
int n)
279 #define SINFO_ABS(a) (((a) > 0) ? (a) : -(a))
281 register int i, j, k, l;
286 ptrb = (
double *) cpl_calloc(n * n,
sizeof(
double));
287 for (i = 0; i < n; i++)
288 ptrb[i * n + i] = 1.0;
290 for (i = 1; i <= n; i++) {
292 max = SINFO_ABS(*(ptra + n*i-n));
294 for (j = i; j <= n; j++)
295 if (SINFO_ABS(*(ptra+n*j+i-n-1)) > max) {
297 max = SINFO_ABS(*(ptra+n*j+i-n-1));
302 for (j = i; j <= n; j++) {
303 r = *(ptra + n * maj + j - n - 1);
304 *(ptra + n * maj + j - n - 1) = *(ptra + n * i + j - n - 1);
305 *(ptra + n * i + j - n - 1) = r;
307 for (l = 0; l < n; l++) {
308 r = *(ptrb + l * n + maj - 1);
309 *(ptrb + l * n + maj - 1) = *(ptrb + l * n + i - 1);
310 *(ptrb + l * n + i - 1) = r;
315 for (j = i + 1; j <= n; j++) {
316 t = (*(ptra + (n + 1) * i - n - 1));
319 r = (*(ptra + n * j + i - n - 1)) / t;
320 for (l = 0; l < n; l++)
321 *(ptrb + l * n + j - 1) -= r * (*(ptrb + l * n + i - 1));
322 for (k = i; k <= n; k++)
323 *(ptra + n * j + k - n - 1) -= r
324 * (*(ptra + n * i + k - n - 1));
329 for (l = 0; l < n; l++)
330 for (i = n; i >= 1; i--) {
331 t = (*(ptra + (n + 1) * i - n - 1));
334 *(ptrc + l + (i - 1) * n) = (*(ptrb + l * n + i - 1)) / t;
336 for (j = i - 1; j > 0; j--)
337 *(ptrb + l * n + j - 1) -= (*(ptra + n * j + i - n - 1))
338 * (*(ptrc + l + (i - 1) * n));
364 sinfo_least_sq_mx(Matrix A, Matrix B)
366 Matrix m1, m2, m3, m4, m5;
368 m1 = sinfo_transp_mx(A);
369 m2 = sinfo_mul_mx(A, m1);
370 m3 = sinfo_invert_mx(m2);
371 m4 = sinfo_mul_mx(B, m1);
372 m5 = sinfo_mul_mx(m4, m3);
395 sinfo_print_mx(Matrix M,
const char * name)
399 fprintf(stdout,
"# sinfo_eclipse_matrix %s is [%d x %d]\n", name, M->nr,
401 for (j = 0; j < M->nr; j++) {
402 for (i = 0; i < M->nc; i++) {
403 fprintf(stdout,
"%g\t", M->m[i + j * M->nc]);
405 fprintf(stdout,
"\n");
407 fprintf(stdout,
"\n");