URL https://opencores.org/ocsvn/or1k/or1k/trunk

Subversion Repositories or1k

[/] [or1k/] [trunk/] [mw/] [src/] [demos/] [nxscribble/] [matrix.c] - Blame information for rev 1773

Go to most recent revision | Details | Compare with Previous | View Log


/***********************************************************************
 
matrix.c - simple matrix operations
 
Copyright (C) 1991 Dean Rubine
 
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License. See ../COPYING for
the 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
*/
 
 
Vector
NewVector(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;
}
 
Matrix
NewMatrix(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;
}
 
void
FreeVector(v)
Vector v;
{
        free(HEADER(v));
}
 
void
FreeMatrix(m)
Matrix m;
{
        register int i;
 
        for(i = 0; i < NROWS(m); i++)
                free(m[i]);
        free(HEADER(m));
}
 
Vector
VectorCopy(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;
}
 
Matrix
MatrixCopy(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 */
 
 
void
ZeroVector(v)
Vector v;
{
        register int i;
        for(i = 0; i < NROWS(v); i++) v[i] = 0.0;
}
 
 
void
ZeroMatrix(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;
}
 
void
FillMatrix(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;
}
 
double
InnerProduct(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;
}
 
void
MatrixMultiply(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)
*/
 
void
VectorTimesMatrix(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];
        }
}
 
void
ScalarTimesVector(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++;
}
 
void
ScalarTimesMatrix(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
 */
 
double
QuadraticForm(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))
 
double
InvertMatrix(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"
 
Vector
SliceVector(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;
}
 
Matrix
SliceMatrix(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;
}
 
Matrix
DeSliceMatrix(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;
}
 
void
OutputVector(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");
}
 
Vector
InputVector(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;
}
 
void
OutputMatrix(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");
        }
}
 
Matrix
InputMatrix(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;
}
 
double
InvertSingularMatrix(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 */
 
void
PrintVector(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");
}
 
void
PrintMatrix(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");
        }
}
 
void
PrintArray(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");
        }
}
 

Browse

Tools

Subversion Repositories or1k

[/] [or1k/] [trunk/] [mw/] [src/] [demos/] [nxscribble/] [matrix.c] - Blame information for rev 1773

Line No.	Rev	Author	Line
1	673	markom	`/***********************************************************************`
2
3			`matrix.c - simple matrix operations`
4
5			`Copyright (C) 1991 Dean Rubine`
6
7			`This program is free software; you can redistribute it and/or modify`
8			`it under the terms of the GNU General Public License. See ../COPYING for`
9			`the full agreement.`
10
11			`**********************************************************************/`
12
13			`/*`
14			`Simple matrix operations`
15			`Why I am writing this stuff over is beyond me`
16			`*/`
17
18			`#undef PIQ_DEBUG`
19
20			`#include <stdio.h>`
21			`#include <stdlib.h>`
22			`#include <math.h>`
23			`#include "util.h"`
24			`#include "matrix.h"`
25
26
27			`typedef struct array_header *Array;`
28
29			`#define EPSILON (1.0e-10) /* zero range */`
30
31			`/*`
32			`Allocation functions`
33			`*/`
34
35
36			`Vector`
37			`NewVector(r)`
38			`int r;`
39			`{`
40			`register struct array_header *a;`
41			`register Vector v;`
42
43			`a = (struct array_header *)`
44			`allocate(sizeof(struct array_header) + r * sizeof(double), char);`
45			`a->ndims = 1;`
46			`a->nrows = r;`
47			`a->ncols = 1;`
48			`v = (Vector) (a + 1);`
49
50			`#define CHECK`
51			`#ifdef CHECK`
52			`if(HEADER(v) != (struct array_header *) a \|\|`
53			`NDIMS(v) != 1 \|\| NROWS(v) != r \|\| NCOLS(v) != 1) {`
54			`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);`
55			`}`
56			`#endif`
57
58			`return v;`
59			`}`
60
61			`Matrix`
62			`NewMatrix(r, c)`
63			`int r, c;`
64			`{`
65			`register struct array_header a = (struct array_header )`
66			`allocate(sizeof(struct array_header) + r * sizeof(double *), char);`
67			`register int i;`
68			`register Matrix m;`
69
70			`m = (Matrix) (a + 1);`
71			`for(i = 0; i < r; i++)`
72			`m[i] = allocate(c, double);`
73			`a->ndims = 2;`
74			`a->nrows = r;`
75			`a->ncols = c;`
76			`return m;`
77			`}`
78
79			`void`
80			`FreeVector(v)`
81			`Vector v;`
82			`{`
83			`free(HEADER(v));`
84			`}`
85
86			`void`
87			`FreeMatrix(m)`
88			`Matrix m;`
89			`{`
90			`register int i;`
91
92			`for(i = 0; i < NROWS(m); i++)`
93			`free(m[i]);`
94			`free(HEADER(m));`
95			`}`
96
97			`Vector`
98			`VectorCopy(v)`
99			`register Vector v;`
100			`{`
101			`register Vector r = NewVector(NROWS(v));`
102			`register int i;`
103
104			`for(i = 0; i < NROWS(v); i++)`
105			`r[i] = v[i];`
106			`return r;`
107			`}`
108
109			`Matrix`
110			`MatrixCopy(m)`
111			`register Matrix m;`
112			`{`
113			`register Matrix r = NewMatrix(NROWS(m), NCOLS(m));`
114			`register int i, j;`
115
116			`for(i = 0; i < NROWS(m); i++)`
117			`for(j = 0; j < NROWS(m); j++)`
118			`r[i][j] = m[i][j];`
119			`return r;`
120			`}`
121
122			`/* Null vector and matrixes */`
123
124
125			`void`
126			`ZeroVector(v)`
127			`Vector v;`
128			`{`
129			`register int i;`
130			`for(i = 0; i < NROWS(v); i++) v[i] = 0.0;`
131			`}`
132
133
134			`void`
135			`ZeroMatrix(m)`
136			`Matrix m;`
137			`{`
138			`register int i, j;`
139			`for(i = 0; i < NROWS(m); i++)`
140			`for(j = 0; j < NCOLS(m); j++)`
141			`m[i][j] = 0.0;`
142			`}`
143
144			`void`
145			`FillMatrix(m, fill)`
146			`Matrix m;`
147			`double fill;`
148			`{`
149			`register int i, j;`
150			`for(i = 0; i < NROWS(m); i++)`
151			`for(j = 0; j < NCOLS(m); j++)`
152			`m[i][j] = fill;`
153			`}`
154
155			`double`
156			`InnerProduct(v1, v2)`
157			`register Vector v1, v2;`
158			`{`
159			`double result = 0;`
160			`register int n = NROWS(v1);`
161			`if(n != NROWS(v2)) {`
162			`exit_error("InnerProduct %d x %d ", n, NROWS(v2));`
163			`}`
164			`while(--n >= 0)`
165			`result += v1++ *v2++;`
166			`return result;`
167			`}`
168
169			`void`
170			`MatrixMultiply(m1, m2, prod)`
171			`register Matrix m1, m2, prod;`
172			`{`
173			`register int i, j, k;`
174			`double sum;`
175
176			`if(NCOLS(m1) != NROWS(m2)) {`
177			`error("MatrixMultiply: Can't multiply %dx%d and %dx%d matrices",`
178			`NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2));`
179			`return;`
180			`}`
181			`if(NROWS(prod) != NROWS(m1) \|\| NCOLS(prod) != NCOLS(m2)) {`
182			`error("MatrixMultiply: %dx%d times %dx%d does not give %dx%d product",`
183			`NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2),`
184			`NROWS(prod), NCOLS(prod));`
185			`return;`
186			`}`
187
188			`for(i = 0; i < NROWS(m1); i++)`
189			`for(j = 0; j < NCOLS(m2); j++) {`
190			`sum = 0;`
191			`for(k = 0; k < NCOLS(m1); k++)`
192			`sum += m1[i][k] * m2[k][j];`
193			`prod[i][j] = sum;`
194			`}`
195			`}`
196
197			`/*`
198			`Compute result = v'm where`
199			`v is a column vector (r x 1)`
200			`m is a matrix (r x c)`
201			`result is a column vector (c x 1)`
202			`*/`
203
204			`void`
205			`VectorTimesMatrix(v, m, prod)`
206			`Vector v;`
207			`Matrix m;`
208			`Vector prod;`
209			`{`
210			`register int i, j;`
211
212			`if(NROWS(v) != NROWS(m)) {`
213			`error("VectorTimesMatrix: Can't multiply %d vector by %dx%d",`
214			`NROWS(v), NROWS(m), NCOLS(m));`
215			`return;`
216			`}`
217			`if(NROWS(prod) != NCOLS(m)) {`
218			`error("VectorTimesMatrix: %d vector times %dx%d mat does not fit in %d product" ,`
219			`NROWS(v), NROWS(m), NCOLS(m), NROWS(prod));`
220			`return;`
221			`}`
222
223			`for(j = 0; j < NCOLS(m); j++) {`
224			`prod[j] = 0;`
225			`for(i = 0; i < NROWS(m); i++)`
226			`prod[j] += v[i] * m[i][j];`
227			`}`
228			`}`
229
230			`void`
231			`ScalarTimesVector(s, v, product)`
232			`double s;`
233			`register Vector v, product;`
234			`{`
235			`register int n = NROWS(v);`
236
237			`if(NROWS(v) != NROWS(product)) {`
238			`error("ScalarTimesVector: result wrong size (%d!=%d)",`
239			`NROWS(v), NROWS(product));`
240			`return;`
241			`}`
242
243			`while(--n >= 0)`
244			`product++ = s *v++;`
245			`}`
246
247			`void`
248			`ScalarTimesMatrix(s, m, product)`
249			`double s;`
250			`register Matrix m, product;`
251			`{`
252			`register int i, j;`
253
254			`if(NROWS(m) != NROWS(product) \|\|`
255			`NCOLS(m) != NCOLS(product)) {`
256			`error("ScalarTimesMatrix: result wrong size (%d!=%d)or(%d!=%d)",`
257			`NROWS(m), NROWS(product),`
258			`NCOLS(m), NCOLS(product));`
259			`return;`
260			`}`
261
262			`for(i = 0; i < NROWS(m); i++)`
263			`for(j = 0; j < NCOLS(m); j++)`
264			`product[i][j] = s * m[i][j];`
265			`}`
266
267			`/*`
268			`Compute v'mv`
269			`*/`
270
271			`double`
272			`QuadraticForm(v, m)`
273			`register Vector v;`
274			`register Matrix m;`
275			`{`
276			`register int i, j, n;`
277			`double result = 0;`
278
279			`n = NROWS(v);`
280
281			`if(n != NROWS(m) \|\| n != NCOLS(m)) {`
282			`exit_error("QuadraticForm: bad matrix size (%dx%d not %dx%d)",`
283			`NROWS(m), NCOLS(m), n, n);`
284			`}`
285			`for(i = 0; i < n; i++)`
286			`for(j = 0; j < n; j++) {`
287
288			`#ifdef PIQ_DEBUG`
289			`printf("%g%g%g [%g] %s ",`
290			`m[i][j],v[i],v[j],`
291			`m[i][j] * v[i] * v[j],`
292			`i==n-1&&j==n-1? "=" : "+");`
293			`#endif`
294
295			`result += m[i][j] * v[i] * v[j];`
296			`}`
297			`return result;`
298			`}`
299
300			`/* Matrix inversion using full pivoting.`
301			`* The standard Gauss-Jordan method is used.`
302			`* The return value is the determinant.`
303			`* The input matrix may be the same as the result matrix`
304			`*`
305			`* det = InvertMatrix(inputmatrix, resultmatrix);`
306			`*`
307			`* HISTORY`
308			`* 26-Feb-82 David Smith (drs) at Carnegie-Mellon University`
309			`* Written.`
310			`* Sun Mar 20 19:36:16 EST 1988 - converted to this form by Dean Rubine`
311			`*`
312			`*/`
313
314			`int DebugInvertMatrix = 0;`
315
316			`#define PERMBUFSIZE 200 /* Max mat size */`
317
318			`#define _abs(x) ((x)>=0 ? (x) : -(x))`
319
320			`double`
321			`InvertMatrix(ym, rm)`
322			`Matrix ym, rm;`
323			`{`
324			`register int i, j, k;`
325			`double det, biga, recip_biga, hold;`
326			`int l[PERMBUFSIZE], m[PERMBUFSIZE];`
327			`register int n;`
328
329			`if(NROWS(ym) != NCOLS(ym)) {`
330			`exit_error("InvertMatrix: not square");`
331			`}`
332
333			`n = NROWS(ym);`
334
335			`if(n != NROWS(rm) \|\| n != NCOLS(rm)) {`
336			`exit_error("InvertMatrix: result wrong size");`
337			`}`
338
339			`/* Copy ym to rm */`
340
341			`if(ym != rm)`
342			`for(i = 0; i < n; i++)`
343			`for(j = 0; j < n; j++)`
344			`rm[i][j] = ym[i][j];`
345
346			`/if(DebugInvertMatrix) PrintMatrix(rm, "Inverting (det=%g)\n", det);/`
347
348			`/* Allocate permutation vectors for l and m, with the same origin`
349			`as the matrix. */`
350
351			`if (n >= PERMBUFSIZE) {`
352			`exit_error("InvertMatrix: PERMBUFSIZE");`
353			`}`
354
355			`det = 1.0;`
356			`for (k = 0; k < n; k++) {`
357			`l[k] = k; m[k] = k;`
358			`biga = rm[k][k];`
359
360			`/* Find the biggest element in the submatrix */`
361			`for (i = k; i < n; i++)`
362			`for (j = k; j < n; j++)`
363			`if (_abs(rm[i][j]) > _abs(biga)) {`
364			`biga = rm[i][j];`
365			`l[k] = i;`
366			`m[k] = j;`
367			`}`
368
369			`if(DebugInvertMatrix)`
370			`if(biga == 0.0)`
371			`PrintMatrix(m, "found zero biga = %g\n", biga);`
372
373			`/* Interchange rows */`
374			`i = l[k];`
375			`if (i > k)`
376			`for (j = 0; j < n; j++) {`
377			`hold = -rm[k][j];`
378			`rm[k][j] = rm[i][j];`
379			`rm[i][j] = hold;`
380			`}`
381
382			`/* Interchange columns */`
383			`j = m[k];`
384			`if (j > k)`
385			`for (i = 0; i < n; i++) {`
386			`hold = -rm[i][k];`
387			`rm[i][k] = rm[i][j];`
388			`rm[i][j] = hold;`
389			`}`
390
391			`/* Divide column by minus pivot`
392			`(value of pivot element is contained in biga). */`
393			`if (biga == 0.0) {`
394			`return 0.0;`
395			`}`
396
397			`if(DebugInvertMatrix) printf("biga = %g\n", biga);`
398			`recip_biga = 1/biga;`
399			`for (i = 0; i < n; i++)`
400			`if (i != k)`
401			`rm[i][k] *= -recip_biga;`
402
403			`/* Reduce matrix */`
404			`for (i = 0; i < n; i++)`
405			`if (i != k) {`
406			`hold = rm[i][k];`
407			`for (j = 0; j < n; j++)`
408			`if (j != k)`
409			`rm[i][j] += hold * rm[k][j];`
410			`}`
411
412			`/* Divide row by pivot */`
413			`for (j = 0; j < n; j++)`
414			`if (j != k)`
415			`rm[k][j] *= recip_biga;`
416
417			`det = biga; / Product of pivots */`
418			`if(DebugInvertMatrix) printf("det = %g\n", det);`
419			`rm[k][k] = recip_biga;`
420
421			`} /* K loop */`
422
423			`/* Final row & column interchanges */`
424			`for (k = n - 1; k >= 0; k--) {`
425			`i = l[k];`
426			`if (i > k)`
427			`for (j = 0; j < n; j++) {`
428			`hold = rm[j][k];`
429			`rm[j][k] = -rm[j][i];`
430			`rm[j][i] = hold;`
431			`}`
432			`j = m[k];`
433			`if (j > k)`
434			`for (i = 0; i < n; i++) {`
435			`hold = rm[k][i];`
436			`rm[k][i] = -rm[j][i];`
437			`rm[j][i] = hold;`
438			`}`
439			`}`
440
441			`if(DebugInvertMatrix) printf("returning, det = %g\n", det);`
442
443			`return det;`
444			`}`
445
446
447			`#include "bitvector.h"`
448
449			`Vector`
450			`SliceVector(v, rowmask)`
451			`Vector v;`
452			`BitVector rowmask;`
453			`{`
454			`register int i, ri;`
455
456			`Vector r = NewVector(bitcount(NROWS(v), rowmask));`
457			`for(i = ri = 0; i < NROWS(v); i++)`
458			`if(IS_SET(i, rowmask) )`
459			`r[ri++] = v[i];`
460			`return r;`
461			`}`
462
463			`Matrix`
464			`SliceMatrix(m, rowmask, colmask)`
465			`Matrix m;`
466			`BitVector rowmask, colmask;`
467			`{`
468			`register int i, ri, j, rj;`
469
470			`Matrix r;`
471
472			`r = NewMatrix(bitcount(NROWS(m), rowmask),`
473			`bitcount(NCOLS(m), colmask));`
474			`for(i = ri = 0; i < NROWS(m); i++)`
475			`if(IS_SET(i, rowmask) ) {`
476			`for(j = rj = 0; j < NCOLS(m); j++)`
477			`if(IS_SET(j, colmask))`
478			`r[ri][rj++] = m[i][j];`
479			`ri++;`
480			`}`
481
482			`return r;`
483			`}`
484
485			`Matrix`
486			`DeSliceMatrix(m, fill, rowmask, colmask, r)`
487			`Matrix m;`
488			`double fill;`
489			`BitVector rowmask, colmask;`
490			`Matrix r;`
491			`{`
492			`register int i, ri, j, rj;`
493
494			`FillMatrix(r, fill);`
495
496			`for(i = ri = 0; i < NROWS(r); i++) {`
497			`if(IS_SET(i, rowmask) ) {`
498			`for(j = rj = 0; j < NCOLS(r); j++)`
499			`if(IS_SET(j, colmask))`
500			`r[i][j] = m[ri][rj++];`
501			`ri++;`
502			`}`
503			`}`
504
505			`return r;`
506			`}`
507
508			`void`
509			`OutputVector(f, v)`
510			`FILE *f;`
511			`register Vector v;`
512			`{`
513			`register int i;`
514			`fprintf(f, " V %d ", NROWS(v));`
515			`for(i = 0; i < NROWS(v); i++)`
516			`fprintf(f, " %g", v[i]);`
517			`fprintf(f, "\n");`
518			`}`
519
520			`Vector`
521			`InputVector(f)`
522			`FILE *f;`
523			`{`
524			`register Vector v;`
525			`register int i;`
526			`char check[4];`
527			`int nrows;`
528
529			`if(fscanf(f, "%1s %d", check, &nrows) != 2) {`
530			`exit_error("InputVector fscanf 1");`
531			`}`
532			`if(check[0] != 'V') {`
533			`exit_error("InputVector check");`
534			`}`
535			`v = NewVector(nrows);`
536			`for(i = 0; i < nrows; i++) {`
537			`if(fscanf(f, "%lf", &v[i]) != 1) {`
538			`exit_error("InputVector fscanf 2");`
539			`}`
540			`}`
541			`return v;`
542			`}`
543
544			`void`
545			`OutputMatrix(f, m)`
546			`FILE* f;`
547			`register Matrix m;`
548			`{`
549			`register int i, j;`
550			`fprintf(f, " M %d %d\n", NROWS(m), NCOLS(m));`
551			`for(i = 0; i < NROWS(m); i++) {`
552			`for(j = 0; j < NCOLS(m); j++)`
553			`fprintf(f, " %g", m[i][j]);`
554			`fprintf(f, "\n");`
555			`}`
556			`}`
557
558			`Matrix`
559			`InputMatrix(f)`
560			`FILE *f;`
561			`{`
562			`register Matrix m;`
563			`register int i, j;`
564			`char check[4];`
565			`int nrows, ncols;`
566
567			`if(fscanf(f, "%1s %d %d", check, &nrows, &ncols) != 3) {`
568			`exit_error("InputMatrix fscanf 1");`
569			`}`
570			`if(check[0] != 'M') {`
571			`exit_error("InputMatrix check");`
572			`}`
573			`m = NewMatrix(nrows, ncols);`
574			`for(i = 0; i < nrows; i++)`
575			`for(j = 0; j < ncols; j++) {`
576			`if(fscanf(f, "%lf", &m[i][j]) != 1) {`
577			`exit_error("InputMatrix fscanf 2");`
578			`}`
579			`}`
580
581			`return m;`
582			`}`
583
584			`double`
585			`InvertSingularMatrix(m, inv)`
586			`Matrix m, inv;`
587			`{`
588			`register int i, j, k;`
589			`BitVector mask;`
590			`Matrix sm;`
591			`double det, maxdet;`
592			`int mi = -1, mj = -1, mk = -1;`
593
594			`maxdet = 0.0;`
595			`for(i = 0; i < NROWS(m); i++) {`
596			`printf("r&c%d, ", i);`
597			`SET_BIT_VECTOR(mask);`
598			`BIT_CLEAR(i, mask);`
599			`sm = SliceMatrix(m, mask, mask);`
600			`det = InvertMatrix(sm, sm);`
601			`if(det == 0.0)`
602			`printf("det still 0\n");`
603			`else {`
604			`printf("det = %g\n", det);`
605			`}`
606			`if(_abs(det) > _abs(maxdet))`
607			`maxdet = det, mi = i;`
608			`FreeMatrix(sm);`
609			`}`
610			`printf("\n");`
611
612			`printf("maxdet=%g when row %d left out\n", maxdet, mi);`
613			`if(fabs(maxdet) > 1.0e-6) {`
614			`goto found;`
615			`}`
616
617			`maxdet = 0.0;`
618			`for(i = 0; i < NROWS(m); i++) {`
619			`for(j = i+1; j < NROWS(m); j++) {`
620			`/* printf("leaving out row&col %d&%d, ", i, j); */`
621			`SET_BIT_VECTOR(mask);`
622			`BIT_CLEAR(i, mask);`
623			`BIT_CLEAR(j, mask);`
624			`sm = SliceMatrix(m, mask, mask);`
625			`det = InvertMatrix(sm, sm);`
626			`/*`
627			`if(det == 0.0)`
628			`printf("det still 0\n");`
629			`else {`
630			`printf("det = %g\n", det);`
631			`}`
632			`*/`
633			`if(abs(det) > abs(maxdet))`
634			`maxdet = det, mi = i, mj = j;`
635			`FreeMatrix(sm);`
636			`}`
637			`}`
638
639			`printf("maxdet=%g when rows %d,%d left out\n", maxdet, mi, mj);`
640			`if(_abs(maxdet) > 1.0e-6) {`
641			`goto found;`
642			`}`
643
644			`maxdet = 0.0;`
645			`for(i = 0; i < NROWS(m); i++) {`
646			`for(j = i+1; j < NROWS(m); j++) {`
647			`for(k = j+1; k < NROWS(m); k++) {`
648			`/* printf("leaving out row&col %d,%d&%d, ", i, j, k); */`
649			`SET_BIT_VECTOR(mask);`
650			`BIT_CLEAR(i, mask);`
651			`BIT_CLEAR(j, mask);`
652			`BIT_CLEAR(k, mask);`
653			`sm = SliceMatrix(m, mask, mask);`
654			`det = InvertMatrix(sm, sm);`
655			`/*`
656			`if(det == 0.0)`
657			`printf("det still 0\n");`
658			`else {`
659			`printf("det = %g\n", det);`
660			`}`
661			`*/`
662			`if(_abs(det) > _abs(maxdet))`
663			`maxdet = det, mi = i, mj = j, mk = k;`
664			`FreeMatrix(sm);`
665			`}`
666			`}`
667			`}`
668			`printf("maxdet=%g when rows %d,%d&%d left out\n", maxdet, mi, mj, mk);`
669			`if(mk == -1)`
670			`return 0.0;`
671
672			`found:`
673
674			`SET_BIT_VECTOR(mask);`
675			`if(mi >= 0) BIT_CLEAR(mi, mask);`
676			`if(mj >= 0) BIT_CLEAR(mj, mask);`
677			`if(mk >= 0) BIT_CLEAR(mk, mask);`
678			`sm = SliceMatrix(m, mask, mask);`
679			`det = InvertMatrix(sm, sm);`
680			`DeSliceMatrix(sm, 0.0, mask, mask, inv);`
681			`FreeMatrix(sm);`
682			`PrintMatrix(inv, "desliced:\n");`
683			`return det;`
684			`}`
685
686			`/* You can fairly confidently ignore the compiler warnings after here */`
687
688			`void`
689			`PrintVector(v, s,a1,a2,a3,a4,a5,a6,a7,a8)`
690			`register Vector v;`
691			`char *s; int a1,a2,a3,a4,a5,a6,a7,a8;`
692			`{`
693			`register int i;`
694			`printf(s,a1,a2,a3,a4,a5,a6,a7,a8);`
695
696			`for(i = 0; i < NROWS(v); i++) printf(" %8.4f", v[i]);`
697			`printf("\n");`
698			`}`
699
700			`void`
701			`PrintMatrix(m, s,a1,a2,a3,a4,a5,a6,a7,a8)`
702			`register Matrix m;`
703			`char *s; int a1,a2,a3,a4,a5,a6,a7,a8;`
704			`{`
705			`register int i, j;`
706			`printf(s,a1,a2,a3,a4,a5,a6,a7,a8);`
707			`for(i = 0; i < NROWS(m); i++) {`
708			`for(j = 0; j < NCOLS(m); j++)`
709			`printf(" %8.4f", m[i][j]);`
710			`printf("\n");`
711			`}`
712			`}`
713
714			`void`
715			`PrintArray(a, s,a1,a2,a3,a4,a5,a6,a7,a8)`
716			`Array a;`
717			`char *s; int a1,a2,a3,a4,a5,a6,a7,a8;`
718			`{`
719			`switch(NDIMS(a)) {`
720			`case 1: PrintVector((Vector) a, s,a1,a2,a3,a4,a5,a6,a7,a8); break;`
721			`case 2: PrintMatrix((Matrix) a, s,a1,a2,a3,a4,a5,a6,a7,a8); break;`
722			`default: error("PrintArray");`
723			`}`
724			`}`
725