?? matrix.c
字號:
/***********************************************************************matrix.c - simple matrix operationsCopyright (C) 1991 Dean RubineThis program is free software; you can redistribute it and/or modifyit under the terms of the GNU General Public License. See ../COPYING forthe full agreement.**********************************************************************//* Simple matrix operations Why I am writing this stuff over is beyond me*/#undef PIQ_DEBUG#include <stdio.h>#include <stdlib.h>#include <math.h>#include "util.h"#include "matrix.h"typedef struct array_header *Array;#define EPSILON (1.0e-10) /* zero range *//* Allocation functions*/VectorNewVector(r)int r;{ register struct array_header *a; register Vector v; a = (struct array_header *) allocate(sizeof(struct array_header) + r * sizeof(double), char); a->ndims = 1; a->nrows = r; a->ncols = 1; v = (Vector) (a + 1);#define CHECK#ifdef CHECK if(HEADER(v) != (struct array_header *) a || NDIMS(v) != 1 || NROWS(v) != r || NCOLS(v) != 1) { exit_error("NewVector error: v=%x H: %x,%x D:%d,%d R:%d,%d C:%d,%d\n", v, HEADER(v), a, NDIMS(v), 1, NROWS(v), r, NCOLS(v), 1); }#endif return v;}MatrixNewMatrix(r, c)int r, c;{ register struct array_header *a = (struct array_header *) allocate(sizeof(struct array_header) + r * sizeof(double *), char); register int i; register Matrix m; m = (Matrix) (a + 1); for(i = 0; i < r; i++) m[i] = allocate(c, double); a->ndims = 2; a->nrows = r; a->ncols = c; return m;}voidFreeVector(v)Vector v;{ free(HEADER(v));}voidFreeMatrix(m)Matrix m;{ register int i; for(i = 0; i < NROWS(m); i++) free(m[i]); free(HEADER(m));}VectorVectorCopy(v)register Vector v;{ register Vector r = NewVector(NROWS(v)); register int i; for(i = 0; i < NROWS(v); i++) r[i] = v[i]; return r;}MatrixMatrixCopy(m)register Matrix m;{ register Matrix r = NewMatrix(NROWS(m), NCOLS(m)); register int i, j; for(i = 0; i < NROWS(m); i++) for(j = 0; j < NROWS(m); j++) r[i][j] = m[i][j]; return r;}/* Null vector and matrixes */voidZeroVector(v)Vector v;{ register int i; for(i = 0; i < NROWS(v); i++) v[i] = 0.0;}voidZeroMatrix(m)Matrix m;{ register int i, j; for(i = 0; i < NROWS(m); i++) for(j = 0; j < NCOLS(m); j++) m[i][j] = 0.0;}voidFillMatrix(m, fill)Matrix m;double fill;{ register int i, j; for(i = 0; i < NROWS(m); i++) for(j = 0; j < NCOLS(m); j++) m[i][j] = fill;}doubleInnerProduct(v1, v2)register Vector v1, v2;{ double result = 0; register int n = NROWS(v1); if(n != NROWS(v2)) { exit_error("InnerProduct %d x %d ", n, NROWS(v2)); } while(--n >= 0) result += *v1++ * *v2++; return result;}voidMatrixMultiply(m1, m2, prod)register Matrix m1, m2, prod;{ register int i, j, k; double sum; if(NCOLS(m1) != NROWS(m2)) { error("MatrixMultiply: Can't multiply %dx%d and %dx%d matrices", NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2)); return; } if(NROWS(prod) != NROWS(m1) || NCOLS(prod) != NCOLS(m2)) { error("MatrixMultiply: %dx%d times %dx%d does not give %dx%d product", NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2), NROWS(prod), NCOLS(prod)); return; } for(i = 0; i < NROWS(m1); i++) for(j = 0; j < NCOLS(m2); j++) { sum = 0; for(k = 0; k < NCOLS(m1); k++) sum += m1[i][k] * m2[k][j]; prod[i][j] = sum; }}/*Compute result = v'm where v is a column vector (r x 1) m is a matrix (r x c) result is a column vector (c x 1)*/voidVectorTimesMatrix(v, m, prod)Vector v;Matrix m;Vector prod;{ register int i, j; if(NROWS(v) != NROWS(m)) { error("VectorTimesMatrix: Can't multiply %d vector by %dx%d", NROWS(v), NROWS(m), NCOLS(m)); return; } if(NROWS(prod) != NCOLS(m)) { error("VectorTimesMatrix: %d vector times %dx%d mat does not fit in %d product" , NROWS(v), NROWS(m), NCOLS(m), NROWS(prod)); return; } for(j = 0; j < NCOLS(m); j++) { prod[j] = 0; for(i = 0; i < NROWS(m); i++) prod[j] += v[i] * m[i][j]; }} voidScalarTimesVector(s, v, product)double s;register Vector v, product;{ register int n = NROWS(v); if(NROWS(v) != NROWS(product)) { error("ScalarTimesVector: result wrong size (%d!=%d)", NROWS(v), NROWS(product)); return; } while(--n >= 0) *product++ = s * *v++;}voidScalarTimesMatrix(s, m, product)double s;register Matrix m, product;{ register int i, j; if(NROWS(m) != NROWS(product) || NCOLS(m) != NCOLS(product)) { error("ScalarTimesMatrix: result wrong size (%d!=%d)or(%d!=%d)", NROWS(m), NROWS(product), NCOLS(m), NCOLS(product)); return; } for(i = 0; i < NROWS(m); i++) for(j = 0; j < NCOLS(m); j++) product[i][j] = s * m[i][j];}/* Compute v'mv */doubleQuadraticForm(v, m)register Vector v;register Matrix m;{ register int i, j, n; double result = 0; n = NROWS(v); if(n != NROWS(m) || n != NCOLS(m)) { exit_error("QuadraticForm: bad matrix size (%dx%d not %dx%d)", NROWS(m), NCOLS(m), n, n); } for(i = 0; i < n; i++) for(j = 0; j < n; j++) {#ifdef PIQ_DEBUG printf("%g*%g*%g [%g] %s ", m[i][j],v[i],v[j], m[i][j] * v[i] * v[j], i==n-1&&j==n-1? "=" : "+");#endif result += m[i][j] * v[i] * v[j]; } return result;}/* Matrix inversion using full pivoting. * The standard Gauss-Jordan method is used. * The return value is the determinant. * The input matrix may be the same as the result matrix * * det = InvertMatrix(inputmatrix, resultmatrix); * * HISTORY * 26-Feb-82 David Smith (drs) at Carnegie-Mellon University * Written. * Sun Mar 20 19:36:16 EST 1988 - converted to this form by Dean Rubine * */int DebugInvertMatrix = 0;#define PERMBUFSIZE 200 /* Max mat size */#define _abs(x) ((x)>=0 ? (x) : -(x))doubleInvertMatrix(ym, rm)Matrix ym, rm;{ register int i, j, k; double det, biga, recip_biga, hold; int l[PERMBUFSIZE], m[PERMBUFSIZE]; register int n; if(NROWS(ym) != NCOLS(ym)) { exit_error("InvertMatrix: not square"); } n = NROWS(ym); if(n != NROWS(rm) || n != NCOLS(rm)) { exit_error("InvertMatrix: result wrong size"); } /* Copy ym to rm */ if(ym != rm) for(i = 0; i < n; i++) for(j = 0; j < n; j++) rm[i][j] = ym[i][j]; /*if(DebugInvertMatrix) PrintMatrix(rm, "Inverting (det=%g)\n", det);*/ /* Allocate permutation vectors for l and m, with the same origin as the matrix. */ if (n >= PERMBUFSIZE) { exit_error("InvertMatrix: PERMBUFSIZE"); } det = 1.0; for (k = 0; k < n; k++) { l[k] = k; m[k] = k; biga = rm[k][k]; /* Find the biggest element in the submatrix */ for (i = k; i < n; i++) for (j = k; j < n; j++) if (_abs(rm[i][j]) > _abs(biga)) { biga = rm[i][j]; l[k] = i; m[k] = j; } if(DebugInvertMatrix) if(biga == 0.0) PrintMatrix(m, "found zero biga = %g\n", biga); /* Interchange rows */ i = l[k]; if (i > k) for (j = 0; j < n; j++) { hold = -rm[k][j]; rm[k][j] = rm[i][j]; rm[i][j] = hold; } /* Interchange columns */ j = m[k]; if (j > k) for (i = 0; i < n; i++) { hold = -rm[i][k]; rm[i][k] = rm[i][j]; rm[i][j] = hold; } /* Divide column by minus pivot (value of pivot element is contained in biga). */ if (biga == 0.0) { return 0.0; } if(DebugInvertMatrix) printf("biga = %g\n", biga); recip_biga = 1/biga; for (i = 0; i < n; i++) if (i != k) rm[i][k] *= -recip_biga; /* Reduce matrix */ for (i = 0; i < n; i++) if (i != k) { hold = rm[i][k]; for (j = 0; j < n; j++) if (j != k) rm[i][j] += hold * rm[k][j]; } /* Divide row by pivot */ for (j = 0; j < n; j++) if (j != k) rm[k][j] *= recip_biga; det *= biga; /* Product of pivots */ if(DebugInvertMatrix) printf("det = %g\n", det); rm[k][k] = recip_biga; } /* K loop */ /* Final row & column interchanges */ for (k = n - 1; k >= 0; k--) { i = l[k]; if (i > k) for (j = 0; j < n; j++) { hold = rm[j][k]; rm[j][k] = -rm[j][i]; rm[j][i] = hold; } j = m[k]; if (j > k) for (i = 0; i < n; i++) { hold = rm[k][i]; rm[k][i] = -rm[j][i]; rm[j][i] = hold; } } if(DebugInvertMatrix) printf("returning, det = %g\n", det); return det;}#include "bitvector.h"VectorSliceVector(v, rowmask)Vector v;BitVector rowmask;{ register int i, ri; Vector r = NewVector(bitcount(NROWS(v), rowmask)); for(i = ri = 0; i < NROWS(v); i++) if(IS_SET(i, rowmask) ) r[ri++] = v[i]; return r;}MatrixSliceMatrix(m, rowmask, colmask)Matrix m;BitVector rowmask, colmask;{ register int i, ri, j, rj; Matrix r; r = NewMatrix(bitcount(NROWS(m), rowmask), bitcount(NCOLS(m), colmask)); for(i = ri = 0; i < NROWS(m); i++) if(IS_SET(i, rowmask) ) { for(j = rj = 0; j < NCOLS(m); j++) if(IS_SET(j, colmask)) r[ri][rj++] = m[i][j]; ri++; } return r;}MatrixDeSliceMatrix(m, fill, rowmask, colmask, r)Matrix m;double fill;BitVector rowmask, colmask;Matrix r;{ register int i, ri, j, rj; FillMatrix(r, fill); for(i = ri = 0; i < NROWS(r); i++) { if(IS_SET(i, rowmask) ) { for(j = rj = 0; j < NCOLS(r); j++) if(IS_SET(j, colmask)) r[i][j] = m[ri][rj++]; ri++; } } return r;}voidOutputVector(f, v)FILE *f;register Vector v;{ register int i; fprintf(f, " V %d ", NROWS(v)); for(i = 0; i < NROWS(v); i++) fprintf(f, " %g", v[i]); fprintf(f, "\n");}VectorInputVector(f)FILE *f;{ register Vector v; register int i; char check[4]; int nrows; if(fscanf(f, "%1s %d", check, &nrows) != 2) { exit_error("InputVector fscanf 1"); } if(check[0] != 'V') { exit_error("InputVector check"); } v = NewVector(nrows); for(i = 0; i < nrows; i++) { if(fscanf(f, "%lf", &v[i]) != 1) { exit_error("InputVector fscanf 2"); } } return v;}voidOutputMatrix(f, m)FILE* f;register Matrix m;{ register int i, j; fprintf(f, " M %d %d\n", NROWS(m), NCOLS(m)); for(i = 0; i < NROWS(m); i++) { for(j = 0; j < NCOLS(m); j++) fprintf(f, " %g", m[i][j]); fprintf(f, "\n"); }}MatrixInputMatrix(f)FILE *f;{ register Matrix m; register int i, j; char check[4]; int nrows, ncols; if(fscanf(f, "%1s %d %d", check, &nrows, &ncols) != 3) { exit_error("InputMatrix fscanf 1"); } if(check[0] != 'M') { exit_error("InputMatrix check"); } m = NewMatrix(nrows, ncols); for(i = 0; i < nrows; i++) for(j = 0; j < ncols; j++) { if(fscanf(f, "%lf", &m[i][j]) != 1) { exit_error("InputMatrix fscanf 2"); } } return m;}doubleInvertSingularMatrix(m, inv)Matrix m, inv;{ register int i, j, k; BitVector mask; Matrix sm; double det, maxdet; int mi = -1, mj = -1, mk = -1; maxdet = 0.0; for(i = 0; i < NROWS(m); i++) { printf("r&c%d, ", i); SET_BIT_VECTOR(mask); BIT_CLEAR(i, mask); sm = SliceMatrix(m, mask, mask); det = InvertMatrix(sm, sm); if(det == 0.0) printf("det still 0\n"); else { printf("det = %g\n", det); } if(_abs(det) > _abs(maxdet)) maxdet = det, mi = i; FreeMatrix(sm); } printf("\n"); printf("maxdet=%g when row %d left out\n", maxdet, mi); if(fabs(maxdet) > 1.0e-6) { goto found; } maxdet = 0.0; for(i = 0; i < NROWS(m); i++) { for(j = i+1; j < NROWS(m); j++) { /* printf("leaving out row&col %d&%d, ", i, j); */ SET_BIT_VECTOR(mask); BIT_CLEAR(i, mask); BIT_CLEAR(j, mask); sm = SliceMatrix(m, mask, mask); det = InvertMatrix(sm, sm); /* if(det == 0.0) printf("det still 0\n"); else { printf("det = %g\n", det); } */ if(abs(det) > abs(maxdet)) maxdet = det, mi = i, mj = j; FreeMatrix(sm); } } printf("maxdet=%g when rows %d,%d left out\n", maxdet, mi, mj); if(_abs(maxdet) > 1.0e-6) { goto found; } maxdet = 0.0; for(i = 0; i < NROWS(m); i++) { for(j = i+1; j < NROWS(m); j++) { for(k = j+1; k < NROWS(m); k++) { /* printf("leaving out row&col %d,%d&%d, ", i, j, k); */ SET_BIT_VECTOR(mask); BIT_CLEAR(i, mask); BIT_CLEAR(j, mask); BIT_CLEAR(k, mask); sm = SliceMatrix(m, mask, mask); det = InvertMatrix(sm, sm); /* if(det == 0.0) printf("det still 0\n"); else { printf("det = %g\n", det); } */ if(_abs(det) > _abs(maxdet)) maxdet = det, mi = i, mj = j, mk = k; FreeMatrix(sm); } } } printf("maxdet=%g when rows %d,%d&%d left out\n", maxdet, mi, mj, mk); if(mk == -1) return 0.0;found: SET_BIT_VECTOR(mask); if(mi >= 0) BIT_CLEAR(mi, mask); if(mj >= 0) BIT_CLEAR(mj, mask); if(mk >= 0) BIT_CLEAR(mk, mask); sm = SliceMatrix(m, mask, mask); det = InvertMatrix(sm, sm); DeSliceMatrix(sm, 0.0, mask, mask, inv); FreeMatrix(sm); PrintMatrix(inv, "desliced:\n"); return det;}/* You can fairly confidently ignore the compiler warnings after here */voidPrintVector(v, s,a1,a2,a3,a4,a5,a6,a7,a8)register Vector v;char *s; int a1,a2,a3,a4,a5,a6,a7,a8;{ register int i; printf(s,a1,a2,a3,a4,a5,a6,a7,a8); for(i = 0; i < NROWS(v); i++) printf(" %8.4f", v[i]); printf("\n");}voidPrintMatrix(m, s,a1,a2,a3,a4,a5,a6,a7,a8)register Matrix m;char *s; int a1,a2,a3,a4,a5,a6,a7,a8;{ register int i, j; printf(s,a1,a2,a3,a4,a5,a6,a7,a8); for(i = 0; i < NROWS(m); i++) { for(j = 0; j < NCOLS(m); j++) printf(" %8.4f", m[i][j]); printf("\n"); }}voidPrintArray(a, s,a1,a2,a3,a4,a5,a6,a7,a8)Array a;char *s; int a1,a2,a3,a4,a5,a6,a7,a8;{ switch(NDIMS(a)) { case 1: PrintVector((Vector) a, s,a1,a2,a3,a4,a5,a6,a7,a8); break; case 2: PrintMatrix((Matrix) a, s,a1,a2,a3,a4,a5,a6,a7,a8); break; default: error("PrintArray"); }}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -