/***********************************************************************
|
/***********************************************************************
|
|
|
matrix.c - simple matrix operations
|
matrix.c - simple matrix operations
|
|
|
Copyright (C) 1991 Dean Rubine
|
Copyright (C) 1991 Dean Rubine
|
|
|
This program is free software; you can redistribute it and/or modify
|
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
|
it under the terms of the GNU General Public License. See ../COPYING for
|
the full agreement.
|
the full agreement.
|
|
|
**********************************************************************/
|
**********************************************************************/
|
|
|
/*
|
/*
|
Simple matrix operations
|
Simple matrix operations
|
Why I am writing this stuff over is beyond me
|
Why I am writing this stuff over is beyond me
|
*/
|
*/
|
|
|
#undef PIQ_DEBUG
|
#undef PIQ_DEBUG
|
|
|
#include <stdio.h>
|
#include <stdio.h>
|
#include <stdlib.h>
|
#include <stdlib.h>
|
#include <math.h>
|
#include <math.h>
|
#include "util.h"
|
#include "util.h"
|
#include "matrix.h"
|
#include "matrix.h"
|
|
|
|
|
typedef struct array_header *Array;
|
typedef struct array_header *Array;
|
|
|
#define EPSILON (1.0e-10) /* zero range */
|
#define EPSILON (1.0e-10) /* zero range */
|
|
|
/*
|
/*
|
Allocation functions
|
Allocation functions
|
*/
|
*/
|
|
|
|
|
Vector
|
Vector
|
NewVector(r)
|
NewVector(r)
|
int r;
|
int r;
|
{
|
{
|
register struct array_header *a;
|
register struct array_header *a;
|
register Vector v;
|
register Vector v;
|
|
|
a = (struct array_header *)
|
a = (struct array_header *)
|
allocate(sizeof(struct array_header) + r * sizeof(double), char);
|
allocate(sizeof(struct array_header) + r * sizeof(double), char);
|
a->ndims = 1;
|
a->ndims = 1;
|
a->nrows = r;
|
a->nrows = r;
|
a->ncols = 1;
|
a->ncols = 1;
|
v = (Vector) (a + 1);
|
v = (Vector) (a + 1);
|
|
|
#define CHECK
|
#define CHECK
|
#ifdef CHECK
|
#ifdef CHECK
|
if(HEADER(v) != (struct array_header *) a ||
|
if(HEADER(v) != (struct array_header *) a ||
|
NDIMS(v) != 1 || NROWS(v) != r || NCOLS(v) != 1) {
|
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);
|
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
|
#endif
|
|
|
return v;
|
return v;
|
}
|
}
|
|
|
Matrix
|
Matrix
|
NewMatrix(r, c)
|
NewMatrix(r, c)
|
int r, c;
|
int r, c;
|
{
|
{
|
register struct array_header *a = (struct array_header *)
|
register struct array_header *a = (struct array_header *)
|
allocate(sizeof(struct array_header) + r * sizeof(double *), char);
|
allocate(sizeof(struct array_header) + r * sizeof(double *), char);
|
register int i;
|
register int i;
|
register Matrix m;
|
register Matrix m;
|
|
|
m = (Matrix) (a + 1);
|
m = (Matrix) (a + 1);
|
for(i = 0; i < r; i++)
|
for(i = 0; i < r; i++)
|
m[i] = allocate(c, double);
|
m[i] = allocate(c, double);
|
a->ndims = 2;
|
a->ndims = 2;
|
a->nrows = r;
|
a->nrows = r;
|
a->ncols = c;
|
a->ncols = c;
|
return m;
|
return m;
|
}
|
}
|
|
|
void
|
void
|
FreeVector(v)
|
FreeVector(v)
|
Vector v;
|
Vector v;
|
{
|
{
|
free(HEADER(v));
|
free(HEADER(v));
|
}
|
}
|
|
|
void
|
void
|
FreeMatrix(m)
|
FreeMatrix(m)
|
Matrix m;
|
Matrix m;
|
{
|
{
|
register int i;
|
register int i;
|
|
|
for(i = 0; i < NROWS(m); i++)
|
for(i = 0; i < NROWS(m); i++)
|
free(m[i]);
|
free(m[i]);
|
free(HEADER(m));
|
free(HEADER(m));
|
}
|
}
|
|
|
Vector
|
Vector
|
VectorCopy(v)
|
VectorCopy(v)
|
register Vector v;
|
register Vector v;
|
{
|
{
|
register Vector r = NewVector(NROWS(v));
|
register Vector r = NewVector(NROWS(v));
|
register int i;
|
register int i;
|
|
|
for(i = 0; i < NROWS(v); i++)
|
for(i = 0; i < NROWS(v); i++)
|
r[i] = v[i];
|
r[i] = v[i];
|
return r;
|
return r;
|
}
|
}
|
|
|
Matrix
|
Matrix
|
MatrixCopy(m)
|
MatrixCopy(m)
|
register Matrix m;
|
register Matrix m;
|
{
|
{
|
register Matrix r = NewMatrix(NROWS(m), NCOLS(m));
|
register Matrix r = NewMatrix(NROWS(m), NCOLS(m));
|
register int i, j;
|
register int i, j;
|
|
|
for(i = 0; i < NROWS(m); i++)
|
for(i = 0; i < NROWS(m); i++)
|
for(j = 0; j < NROWS(m); j++)
|
for(j = 0; j < NROWS(m); j++)
|
r[i][j] = m[i][j];
|
r[i][j] = m[i][j];
|
return r;
|
return r;
|
}
|
}
|
|
|
/* Null vector and matrixes */
|
/* Null vector and matrixes */
|
|
|
|
|
void
|
void
|
ZeroVector(v)
|
ZeroVector(v)
|
Vector v;
|
Vector v;
|
{
|
{
|
register int i;
|
register int i;
|
for(i = 0; i < NROWS(v); i++) v[i] = 0.0;
|
for(i = 0; i < NROWS(v); i++) v[i] = 0.0;
|
}
|
}
|
|
|
|
|
void
|
void
|
ZeroMatrix(m)
|
ZeroMatrix(m)
|
Matrix m;
|
Matrix m;
|
{
|
{
|
register int i, j;
|
register int i, j;
|
for(i = 0; i < NROWS(m); i++)
|
for(i = 0; i < NROWS(m); i++)
|
for(j = 0; j < NCOLS(m); j++)
|
for(j = 0; j < NCOLS(m); j++)
|
m[i][j] = 0.0;
|
m[i][j] = 0.0;
|
}
|
}
|
|
|
void
|
void
|
FillMatrix(m, fill)
|
FillMatrix(m, fill)
|
Matrix m;
|
Matrix m;
|
double fill;
|
double fill;
|
{
|
{
|
register int i, j;
|
register int i, j;
|
for(i = 0; i < NROWS(m); i++)
|
for(i = 0; i < NROWS(m); i++)
|
for(j = 0; j < NCOLS(m); j++)
|
for(j = 0; j < NCOLS(m); j++)
|
m[i][j] = fill;
|
m[i][j] = fill;
|
}
|
}
|
|
|
double
|
double
|
InnerProduct(v1, v2)
|
InnerProduct(v1, v2)
|
register Vector v1, v2;
|
register Vector v1, v2;
|
{
|
{
|
double result = 0;
|
double result = 0;
|
register int n = NROWS(v1);
|
register int n = NROWS(v1);
|
if(n != NROWS(v2)) {
|
if(n != NROWS(v2)) {
|
exit_error("InnerProduct %d x %d ", n, NROWS(v2));
|
exit_error("InnerProduct %d x %d ", n, NROWS(v2));
|
}
|
}
|
while(--n >= 0)
|
while(--n >= 0)
|
result += *v1++ * *v2++;
|
result += *v1++ * *v2++;
|
return result;
|
return result;
|
}
|
}
|
|
|
void
|
void
|
MatrixMultiply(m1, m2, prod)
|
MatrixMultiply(m1, m2, prod)
|
register Matrix m1, m2, prod;
|
register Matrix m1, m2, prod;
|
{
|
{
|
register int i, j, k;
|
register int i, j, k;
|
double sum;
|
double sum;
|
|
|
if(NCOLS(m1) != NROWS(m2)) {
|
if(NCOLS(m1) != NROWS(m2)) {
|
error("MatrixMultiply: Can't multiply %dx%d and %dx%d matrices",
|
error("MatrixMultiply: Can't multiply %dx%d and %dx%d matrices",
|
NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2));
|
NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2));
|
return;
|
return;
|
}
|
}
|
if(NROWS(prod) != NROWS(m1) || NCOLS(prod) != NCOLS(m2)) {
|
if(NROWS(prod) != NROWS(m1) || NCOLS(prod) != NCOLS(m2)) {
|
error("MatrixMultiply: %dx%d times %dx%d does not give %dx%d product",
|
error("MatrixMultiply: %dx%d times %dx%d does not give %dx%d product",
|
NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2),
|
NROWS(m1), NCOLS(m1), NROWS(m2), NCOLS(m2),
|
NROWS(prod), NCOLS(prod));
|
NROWS(prod), NCOLS(prod));
|
return;
|
return;
|
}
|
}
|
|
|
for(i = 0; i < NROWS(m1); i++)
|
for(i = 0; i < NROWS(m1); i++)
|
for(j = 0; j < NCOLS(m2); j++) {
|
for(j = 0; j < NCOLS(m2); j++) {
|
sum = 0;
|
sum = 0;
|
for(k = 0; k < NCOLS(m1); k++)
|
for(k = 0; k < NCOLS(m1); k++)
|
sum += m1[i][k] * m2[k][j];
|
sum += m1[i][k] * m2[k][j];
|
prod[i][j] = sum;
|
prod[i][j] = sum;
|
}
|
}
|
}
|
}
|
|
|
/*
|
/*
|
Compute result = v'm where
|
Compute result = v'm where
|
v is a column vector (r x 1)
|
v is a column vector (r x 1)
|
m is a matrix (r x c)
|
m is a matrix (r x c)
|
result is a column vector (c x 1)
|
result is a column vector (c x 1)
|
*/
|
*/
|
|
|
void
|
void
|
VectorTimesMatrix(v, m, prod)
|
VectorTimesMatrix(v, m, prod)
|
Vector v;
|
Vector v;
|
Matrix m;
|
Matrix m;
|
Vector prod;
|
Vector prod;
|
{
|
{
|
register int i, j;
|
register int i, j;
|
|
|
if(NROWS(v) != NROWS(m)) {
|
if(NROWS(v) != NROWS(m)) {
|
error("VectorTimesMatrix: Can't multiply %d vector by %dx%d",
|
error("VectorTimesMatrix: Can't multiply %d vector by %dx%d",
|
NROWS(v), NROWS(m), NCOLS(m));
|
NROWS(v), NROWS(m), NCOLS(m));
|
return;
|
return;
|
}
|
}
|
if(NROWS(prod) != NCOLS(m)) {
|
if(NROWS(prod) != NCOLS(m)) {
|
error("VectorTimesMatrix: %d vector times %dx%d mat does not fit in %d product" ,
|
error("VectorTimesMatrix: %d vector times %dx%d mat does not fit in %d product" ,
|
NROWS(v), NROWS(m), NCOLS(m), NROWS(prod));
|
NROWS(v), NROWS(m), NCOLS(m), NROWS(prod));
|
return;
|
return;
|
}
|
}
|
|
|
for(j = 0; j < NCOLS(m); j++) {
|
for(j = 0; j < NCOLS(m); j++) {
|
prod[j] = 0;
|
prod[j] = 0;
|
for(i = 0; i < NROWS(m); i++)
|
for(i = 0; i < NROWS(m); i++)
|
prod[j] += v[i] * m[i][j];
|
prod[j] += v[i] * m[i][j];
|
}
|
}
|
}
|
}
|
|
|
void
|
void
|
ScalarTimesVector(s, v, product)
|
ScalarTimesVector(s, v, product)
|
double s;
|
double s;
|
register Vector v, product;
|
register Vector v, product;
|
{
|
{
|
register int n = NROWS(v);
|
register int n = NROWS(v);
|
|
|
if(NROWS(v) != NROWS(product)) {
|
if(NROWS(v) != NROWS(product)) {
|
error("ScalarTimesVector: result wrong size (%d!=%d)",
|
error("ScalarTimesVector: result wrong size (%d!=%d)",
|
NROWS(v), NROWS(product));
|
NROWS(v), NROWS(product));
|
return;
|
return;
|
}
|
}
|
|
|
while(--n >= 0)
|
while(--n >= 0)
|
*product++ = s * *v++;
|
*product++ = s * *v++;
|
}
|
}
|
|
|
void
|
void
|
ScalarTimesMatrix(s, m, product)
|
ScalarTimesMatrix(s, m, product)
|
double s;
|
double s;
|
register Matrix m, product;
|
register Matrix m, product;
|
{
|
{
|
register int i, j;
|
register int i, j;
|
|
|
if(NROWS(m) != NROWS(product) ||
|
if(NROWS(m) != NROWS(product) ||
|
NCOLS(m) != NCOLS(product)) {
|
NCOLS(m) != NCOLS(product)) {
|
error("ScalarTimesMatrix: result wrong size (%d!=%d)or(%d!=%d)",
|
error("ScalarTimesMatrix: result wrong size (%d!=%d)or(%d!=%d)",
|
NROWS(m), NROWS(product),
|
NROWS(m), NROWS(product),
|
NCOLS(m), NCOLS(product));
|
NCOLS(m), NCOLS(product));
|
return;
|
return;
|
}
|
}
|
|
|
for(i = 0; i < NROWS(m); i++)
|
for(i = 0; i < NROWS(m); i++)
|
for(j = 0; j < NCOLS(m); j++)
|
for(j = 0; j < NCOLS(m); j++)
|
product[i][j] = s * m[i][j];
|
product[i][j] = s * m[i][j];
|
}
|
}
|
|
|
/*
|
/*
|
Compute v'mv
|
Compute v'mv
|
*/
|
*/
|
|
|
double
|
double
|
QuadraticForm(v, m)
|
QuadraticForm(v, m)
|
register Vector v;
|
register Vector v;
|
register Matrix m;
|
register Matrix m;
|
{
|
{
|
register int i, j, n;
|
register int i, j, n;
|
double result = 0;
|
double result = 0;
|
|
|
n = NROWS(v);
|
n = NROWS(v);
|
|
|
if(n != NROWS(m) || n != NCOLS(m)) {
|
if(n != NROWS(m) || n != NCOLS(m)) {
|
exit_error("QuadraticForm: bad matrix size (%dx%d not %dx%d)",
|
exit_error("QuadraticForm: bad matrix size (%dx%d not %dx%d)",
|
NROWS(m), NCOLS(m), n, n);
|
NROWS(m), NCOLS(m), n, n);
|
}
|
}
|
for(i = 0; i < n; i++)
|
for(i = 0; i < n; i++)
|
for(j = 0; j < n; j++) {
|
for(j = 0; j < n; j++) {
|
|
|
#ifdef PIQ_DEBUG
|
#ifdef PIQ_DEBUG
|
printf("%g*%g*%g [%g] %s ",
|
printf("%g*%g*%g [%g] %s ",
|
m[i][j],v[i],v[j],
|
m[i][j],v[i],v[j],
|
m[i][j] * v[i] * v[j],
|
m[i][j] * v[i] * v[j],
|
i==n-1&&j==n-1? "=" : "+");
|
i==n-1&&j==n-1? "=" : "+");
|
#endif
|
#endif
|
|
|
result += m[i][j] * v[i] * v[j];
|
result += m[i][j] * v[i] * v[j];
|
}
|
}
|
return result;
|
return result;
|
}
|
}
|
|
|
/* Matrix inversion using full pivoting.
|
/* Matrix inversion using full pivoting.
|
* The standard Gauss-Jordan method is used.
|
* The standard Gauss-Jordan method is used.
|
* The return value is the determinant.
|
* The return value is the determinant.
|
* The input matrix may be the same as the result matrix
|
* The input matrix may be the same as the result matrix
|
*
|
*
|
* det = InvertMatrix(inputmatrix, resultmatrix);
|
* det = InvertMatrix(inputmatrix, resultmatrix);
|
*
|
*
|
* HISTORY
|
* HISTORY
|
* 26-Feb-82 David Smith (drs) at Carnegie-Mellon University
|
* 26-Feb-82 David Smith (drs) at Carnegie-Mellon University
|
* Written.
|
* Written.
|
* Sun Mar 20 19:36:16 EST 1988 - converted to this form by Dean Rubine
|
* Sun Mar 20 19:36:16 EST 1988 - converted to this form by Dean Rubine
|
*
|
*
|
*/
|
*/
|
|
|
int DebugInvertMatrix = 0;
|
int DebugInvertMatrix = 0;
|
|
|
#define PERMBUFSIZE 200 /* Max mat size */
|
#define PERMBUFSIZE 200 /* Max mat size */
|
|
|
#define _abs(x) ((x)>=0 ? (x) : -(x))
|
#define _abs(x) ((x)>=0 ? (x) : -(x))
|
|
|
double
|
double
|
InvertMatrix(ym, rm)
|
InvertMatrix(ym, rm)
|
Matrix ym, rm;
|
Matrix ym, rm;
|
{
|
{
|
register int i, j, k;
|
register int i, j, k;
|
double det, biga, recip_biga, hold;
|
double det, biga, recip_biga, hold;
|
int l[PERMBUFSIZE], m[PERMBUFSIZE];
|
int l[PERMBUFSIZE], m[PERMBUFSIZE];
|
register int n;
|
register int n;
|
|
|
if(NROWS(ym) != NCOLS(ym)) {
|
if(NROWS(ym) != NCOLS(ym)) {
|
exit_error("InvertMatrix: not square");
|
exit_error("InvertMatrix: not square");
|
}
|
}
|
|
|
n = NROWS(ym);
|
n = NROWS(ym);
|
|
|
if(n != NROWS(rm) || n != NCOLS(rm)) {
|
if(n != NROWS(rm) || n != NCOLS(rm)) {
|
exit_error("InvertMatrix: result wrong size");
|
exit_error("InvertMatrix: result wrong size");
|
}
|
}
|
|
|
/* Copy ym to rm */
|
/* Copy ym to rm */
|
|
|
if(ym != rm)
|
if(ym != rm)
|
for(i = 0; i < n; i++)
|
for(i = 0; i < n; i++)
|
for(j = 0; j < n; j++)
|
for(j = 0; j < n; j++)
|
rm[i][j] = ym[i][j];
|
rm[i][j] = ym[i][j];
|
|
|
/*if(DebugInvertMatrix) PrintMatrix(rm, "Inverting (det=%g)\n", det);*/
|
/*if(DebugInvertMatrix) PrintMatrix(rm, "Inverting (det=%g)\n", det);*/
|
|
|
/* Allocate permutation vectors for l and m, with the same origin
|
/* Allocate permutation vectors for l and m, with the same origin
|
as the matrix. */
|
as the matrix. */
|
|
|
if (n >= PERMBUFSIZE) {
|
if (n >= PERMBUFSIZE) {
|
exit_error("InvertMatrix: PERMBUFSIZE");
|
exit_error("InvertMatrix: PERMBUFSIZE");
|
}
|
}
|
|
|
det = 1.0;
|
det = 1.0;
|
for (k = 0; k < n; k++) {
|
for (k = 0; k < n; k++) {
|
l[k] = k; m[k] = k;
|
l[k] = k; m[k] = k;
|
biga = rm[k][k];
|
biga = rm[k][k];
|
|
|
/* Find the biggest element in the submatrix */
|
/* Find the biggest element in the submatrix */
|
for (i = k; i < n; i++)
|
for (i = k; i < n; i++)
|
for (j = k; j < n; j++)
|
for (j = k; j < n; j++)
|
if (_abs(rm[i][j]) > _abs(biga)) {
|
if (_abs(rm[i][j]) > _abs(biga)) {
|
biga = rm[i][j];
|
biga = rm[i][j];
|
l[k] = i;
|
l[k] = i;
|
m[k] = j;
|
m[k] = j;
|
}
|
}
|
|
|
if(DebugInvertMatrix)
|
if(DebugInvertMatrix)
|
if(biga == 0.0)
|
if(biga == 0.0)
|
PrintMatrix(m, "found zero biga = %g\n", biga);
|
PrintMatrix(m, "found zero biga = %g\n", biga);
|
|
|
/* Interchange rows */
|
/* Interchange rows */
|
i = l[k];
|
i = l[k];
|
if (i > k)
|
if (i > k)
|
for (j = 0; j < n; j++) {
|
for (j = 0; j < n; j++) {
|
hold = -rm[k][j];
|
hold = -rm[k][j];
|
rm[k][j] = rm[i][j];
|
rm[k][j] = rm[i][j];
|
rm[i][j] = hold;
|
rm[i][j] = hold;
|
}
|
}
|
|
|
/* Interchange columns */
|
/* Interchange columns */
|
j = m[k];
|
j = m[k];
|
if (j > k)
|
if (j > k)
|
for (i = 0; i < n; i++) {
|
for (i = 0; i < n; i++) {
|
hold = -rm[i][k];
|
hold = -rm[i][k];
|
rm[i][k] = rm[i][j];
|
rm[i][k] = rm[i][j];
|
rm[i][j] = hold;
|
rm[i][j] = hold;
|
}
|
}
|
|
|
/* Divide column by minus pivot
|
/* Divide column by minus pivot
|
(value of pivot element is contained in biga). */
|
(value of pivot element is contained in biga). */
|
if (biga == 0.0) {
|
if (biga == 0.0) {
|
return 0.0;
|
return 0.0;
|
}
|
}
|
|
|
if(DebugInvertMatrix) printf("biga = %g\n", biga);
|
if(DebugInvertMatrix) printf("biga = %g\n", biga);
|
recip_biga = 1/biga;
|
recip_biga = 1/biga;
|
for (i = 0; i < n; i++)
|
for (i = 0; i < n; i++)
|
if (i != k)
|
if (i != k)
|
rm[i][k] *= -recip_biga;
|
rm[i][k] *= -recip_biga;
|
|
|
/* Reduce matrix */
|
/* Reduce matrix */
|
for (i = 0; i < n; i++)
|
for (i = 0; i < n; i++)
|
if (i != k) {
|
if (i != k) {
|
hold = rm[i][k];
|
hold = rm[i][k];
|
for (j = 0; j < n; j++)
|
for (j = 0; j < n; j++)
|
if (j != k)
|
if (j != k)
|
rm[i][j] += hold * rm[k][j];
|
rm[i][j] += hold * rm[k][j];
|
}
|
}
|
|
|
/* Divide row by pivot */
|
/* Divide row by pivot */
|
for (j = 0; j < n; j++)
|
for (j = 0; j < n; j++)
|
if (j != k)
|
if (j != k)
|
rm[k][j] *= recip_biga;
|
rm[k][j] *= recip_biga;
|
|
|
det *= biga; /* Product of pivots */
|
det *= biga; /* Product of pivots */
|
if(DebugInvertMatrix) printf("det = %g\n", det);
|
if(DebugInvertMatrix) printf("det = %g\n", det);
|
rm[k][k] = recip_biga;
|
rm[k][k] = recip_biga;
|
|
|
} /* K loop */
|
} /* K loop */
|
|
|
/* Final row & column interchanges */
|
/* Final row & column interchanges */
|
for (k = n - 1; k >= 0; k--) {
|
for (k = n - 1; k >= 0; k--) {
|
i = l[k];
|
i = l[k];
|
if (i > k)
|
if (i > k)
|
for (j = 0; j < n; j++) {
|
for (j = 0; j < n; j++) {
|
hold = rm[j][k];
|
hold = rm[j][k];
|
rm[j][k] = -rm[j][i];
|
rm[j][k] = -rm[j][i];
|
rm[j][i] = hold;
|
rm[j][i] = hold;
|
}
|
}
|
j = m[k];
|
j = m[k];
|
if (j > k)
|
if (j > k)
|
for (i = 0; i < n; i++) {
|
for (i = 0; i < n; i++) {
|
hold = rm[k][i];
|
hold = rm[k][i];
|
rm[k][i] = -rm[j][i];
|
rm[k][i] = -rm[j][i];
|
rm[j][i] = hold;
|
rm[j][i] = hold;
|
}
|
}
|
}
|
}
|
|
|
if(DebugInvertMatrix) printf("returning, det = %g\n", det);
|
if(DebugInvertMatrix) printf("returning, det = %g\n", det);
|
|
|
return det;
|
return det;
|
}
|
}
|
|
|
|
|
#include "bitvector.h"
|
#include "bitvector.h"
|
|
|
Vector
|
Vector
|
SliceVector(v, rowmask)
|
SliceVector(v, rowmask)
|
Vector v;
|
Vector v;
|
BitVector rowmask;
|
BitVector rowmask;
|
{
|
{
|
register int i, ri;
|
register int i, ri;
|
|
|
Vector r = NewVector(bitcount(NROWS(v), rowmask));
|
Vector r = NewVector(bitcount(NROWS(v), rowmask));
|
for(i = ri = 0; i < NROWS(v); i++)
|
for(i = ri = 0; i < NROWS(v); i++)
|
if(IS_SET(i, rowmask) )
|
if(IS_SET(i, rowmask) )
|
r[ri++] = v[i];
|
r[ri++] = v[i];
|
return r;
|
return r;
|
}
|
}
|
|
|
Matrix
|
Matrix
|
SliceMatrix(m, rowmask, colmask)
|
SliceMatrix(m, rowmask, colmask)
|
Matrix m;
|
Matrix m;
|
BitVector rowmask, colmask;
|
BitVector rowmask, colmask;
|
{
|
{
|
register int i, ri, j, rj;
|
register int i, ri, j, rj;
|
|
|
Matrix r;
|
Matrix r;
|
|
|
r = NewMatrix(bitcount(NROWS(m), rowmask),
|
r = NewMatrix(bitcount(NROWS(m), rowmask),
|
bitcount(NCOLS(m), colmask));
|
bitcount(NCOLS(m), colmask));
|
for(i = ri = 0; i < NROWS(m); i++)
|
for(i = ri = 0; i < NROWS(m); i++)
|
if(IS_SET(i, rowmask) ) {
|
if(IS_SET(i, rowmask) ) {
|
for(j = rj = 0; j < NCOLS(m); j++)
|
for(j = rj = 0; j < NCOLS(m); j++)
|
if(IS_SET(j, colmask))
|
if(IS_SET(j, colmask))
|
r[ri][rj++] = m[i][j];
|
r[ri][rj++] = m[i][j];
|
ri++;
|
ri++;
|
}
|
}
|
|
|
return r;
|
return r;
|
}
|
}
|
|
|
Matrix
|
Matrix
|
DeSliceMatrix(m, fill, rowmask, colmask, r)
|
DeSliceMatrix(m, fill, rowmask, colmask, r)
|
Matrix m;
|
Matrix m;
|
double fill;
|
double fill;
|
BitVector rowmask, colmask;
|
BitVector rowmask, colmask;
|
Matrix r;
|
Matrix r;
|
{
|
{
|
register int i, ri, j, rj;
|
register int i, ri, j, rj;
|
|
|
FillMatrix(r, fill);
|
FillMatrix(r, fill);
|
|
|
for(i = ri = 0; i < NROWS(r); i++) {
|
for(i = ri = 0; i < NROWS(r); i++) {
|
if(IS_SET(i, rowmask) ) {
|
if(IS_SET(i, rowmask) ) {
|
for(j = rj = 0; j < NCOLS(r); j++)
|
for(j = rj = 0; j < NCOLS(r); j++)
|
if(IS_SET(j, colmask))
|
if(IS_SET(j, colmask))
|
r[i][j] = m[ri][rj++];
|
r[i][j] = m[ri][rj++];
|
ri++;
|
ri++;
|
}
|
}
|
}
|
}
|
|
|
return r;
|
return r;
|
}
|
}
|
|
|
void
|
void
|
OutputVector(f, v)
|
OutputVector(f, v)
|
FILE *f;
|
FILE *f;
|
register Vector v;
|
register Vector v;
|
{
|
{
|
register int i;
|
register int i;
|
fprintf(f, " V %d ", NROWS(v));
|
fprintf(f, " V %d ", NROWS(v));
|
for(i = 0; i < NROWS(v); i++)
|
for(i = 0; i < NROWS(v); i++)
|
fprintf(f, " %g", v[i]);
|
fprintf(f, " %g", v[i]);
|
fprintf(f, "\n");
|
fprintf(f, "\n");
|
}
|
}
|
|
|
Vector
|
Vector
|
InputVector(f)
|
InputVector(f)
|
FILE *f;
|
FILE *f;
|
{
|
{
|
register Vector v;
|
register Vector v;
|
register int i;
|
register int i;
|
char check[4];
|
char check[4];
|
int nrows;
|
int nrows;
|
|
|
if(fscanf(f, "%1s %d", check, &nrows) != 2) {
|
if(fscanf(f, "%1s %d", check, &nrows) != 2) {
|
exit_error("InputVector fscanf 1");
|
exit_error("InputVector fscanf 1");
|
}
|
}
|
if(check[0] != 'V') {
|
if(check[0] != 'V') {
|
exit_error("InputVector check");
|
exit_error("InputVector check");
|
}
|
}
|
v = NewVector(nrows);
|
v = NewVector(nrows);
|
for(i = 0; i < nrows; i++) {
|
for(i = 0; i < nrows; i++) {
|
if(fscanf(f, "%lf", &v[i]) != 1) {
|
if(fscanf(f, "%lf", &v[i]) != 1) {
|
exit_error("InputVector fscanf 2");
|
exit_error("InputVector fscanf 2");
|
}
|
}
|
}
|
}
|
return v;
|
return v;
|
}
|
}
|
|
|
void
|
void
|
OutputMatrix(f, m)
|
OutputMatrix(f, m)
|
FILE* f;
|
FILE* f;
|
register Matrix m;
|
register Matrix m;
|
{
|
{
|
register int i, j;
|
register int i, j;
|
fprintf(f, " M %d %d\n", NROWS(m), NCOLS(m));
|
fprintf(f, " M %d %d\n", NROWS(m), NCOLS(m));
|
for(i = 0; i < NROWS(m); i++) {
|
for(i = 0; i < NROWS(m); i++) {
|
for(j = 0; j < NCOLS(m); j++)
|
for(j = 0; j < NCOLS(m); j++)
|
fprintf(f, " %g", m[i][j]);
|
fprintf(f, " %g", m[i][j]);
|
fprintf(f, "\n");
|
fprintf(f, "\n");
|
}
|
}
|
}
|
}
|
|
|
Matrix
|
Matrix
|
InputMatrix(f)
|
InputMatrix(f)
|
FILE *f;
|
FILE *f;
|
{
|
{
|
register Matrix m;
|
register Matrix m;
|
register int i, j;
|
register int i, j;
|
char check[4];
|
char check[4];
|
int nrows, ncols;
|
int nrows, ncols;
|
|
|
if(fscanf(f, "%1s %d %d", check, &nrows, &ncols) != 3) {
|
if(fscanf(f, "%1s %d %d", check, &nrows, &ncols) != 3) {
|
exit_error("InputMatrix fscanf 1");
|
exit_error("InputMatrix fscanf 1");
|
}
|
}
|
if(check[0] != 'M') {
|
if(check[0] != 'M') {
|
exit_error("InputMatrix check");
|
exit_error("InputMatrix check");
|
}
|
}
|
m = NewMatrix(nrows, ncols);
|
m = NewMatrix(nrows, ncols);
|
for(i = 0; i < nrows; i++)
|
for(i = 0; i < nrows; i++)
|
for(j = 0; j < ncols; j++) {
|
for(j = 0; j < ncols; j++) {
|
if(fscanf(f, "%lf", &m[i][j]) != 1) {
|
if(fscanf(f, "%lf", &m[i][j]) != 1) {
|
exit_error("InputMatrix fscanf 2");
|
exit_error("InputMatrix fscanf 2");
|
}
|
}
|
}
|
}
|
|
|
return m;
|
return m;
|
}
|
}
|
|
|
double
|
double
|
InvertSingularMatrix(m, inv)
|
InvertSingularMatrix(m, inv)
|
Matrix m, inv;
|
Matrix m, inv;
|
{
|
{
|
register int i, j, k;
|
register int i, j, k;
|
BitVector mask;
|
BitVector mask;
|
Matrix sm;
|
Matrix sm;
|
double det, maxdet;
|
double det, maxdet;
|
int mi = -1, mj = -1, mk = -1;
|
int mi = -1, mj = -1, mk = -1;
|
|
|
maxdet = 0.0;
|
maxdet = 0.0;
|
for(i = 0; i < NROWS(m); i++) {
|
for(i = 0; i < NROWS(m); i++) {
|
printf("r&c%d, ", i);
|
printf("r&c%d, ", i);
|
SET_BIT_VECTOR(mask);
|
SET_BIT_VECTOR(mask);
|
BIT_CLEAR(i, mask);
|
BIT_CLEAR(i, mask);
|
sm = SliceMatrix(m, mask, mask);
|
sm = SliceMatrix(m, mask, mask);
|
det = InvertMatrix(sm, sm);
|
det = InvertMatrix(sm, sm);
|
if(det == 0.0)
|
if(det == 0.0)
|
printf("det still 0\n");
|
printf("det still 0\n");
|
else {
|
else {
|
printf("det = %g\n", det);
|
printf("det = %g\n", det);
|
}
|
}
|
if(_abs(det) > _abs(maxdet))
|
if(_abs(det) > _abs(maxdet))
|
maxdet = det, mi = i;
|
maxdet = det, mi = i;
|
FreeMatrix(sm);
|
FreeMatrix(sm);
|
}
|
}
|
printf("\n");
|
printf("\n");
|
|
|
printf("maxdet=%g when row %d left out\n", maxdet, mi);
|
printf("maxdet=%g when row %d left out\n", maxdet, mi);
|
if(fabs(maxdet) > 1.0e-6) {
|
if(fabs(maxdet) > 1.0e-6) {
|
goto found;
|
goto found;
|
}
|
}
|
|
|
maxdet = 0.0;
|
maxdet = 0.0;
|
for(i = 0; i < NROWS(m); i++) {
|
for(i = 0; i < NROWS(m); i++) {
|
for(j = i+1; j < NROWS(m); j++) {
|
for(j = i+1; j < NROWS(m); j++) {
|
/* printf("leaving out row&col %d&%d, ", i, j); */
|
/* printf("leaving out row&col %d&%d, ", i, j); */
|
SET_BIT_VECTOR(mask);
|
SET_BIT_VECTOR(mask);
|
BIT_CLEAR(i, mask);
|
BIT_CLEAR(i, mask);
|
BIT_CLEAR(j, mask);
|
BIT_CLEAR(j, mask);
|
sm = SliceMatrix(m, mask, mask);
|
sm = SliceMatrix(m, mask, mask);
|
det = InvertMatrix(sm, sm);
|
det = InvertMatrix(sm, sm);
|
/*
|
/*
|
if(det == 0.0)
|
if(det == 0.0)
|
printf("det still 0\n");
|
printf("det still 0\n");
|
else {
|
else {
|
printf("det = %g\n", det);
|
printf("det = %g\n", det);
|
}
|
}
|
*/
|
*/
|
if(abs(det) > abs(maxdet))
|
if(abs(det) > abs(maxdet))
|
maxdet = det, mi = i, mj = j;
|
maxdet = det, mi = i, mj = j;
|
FreeMatrix(sm);
|
FreeMatrix(sm);
|
}
|
}
|
}
|
}
|
|
|
printf("maxdet=%g when rows %d,%d left out\n", maxdet, mi, mj);
|
printf("maxdet=%g when rows %d,%d left out\n", maxdet, mi, mj);
|
if(_abs(maxdet) > 1.0e-6) {
|
if(_abs(maxdet) > 1.0e-6) {
|
goto found;
|
goto found;
|
}
|
}
|
|
|
maxdet = 0.0;
|
maxdet = 0.0;
|
for(i = 0; i < NROWS(m); i++) {
|
for(i = 0; i < NROWS(m); i++) {
|
for(j = i+1; j < NROWS(m); j++) {
|
for(j = i+1; j < NROWS(m); j++) {
|
for(k = j+1; k < NROWS(m); k++) {
|
for(k = j+1; k < NROWS(m); k++) {
|
/* printf("leaving out row&col %d,%d&%d, ", i, j, k); */
|
/* printf("leaving out row&col %d,%d&%d, ", i, j, k); */
|
SET_BIT_VECTOR(mask);
|
SET_BIT_VECTOR(mask);
|
BIT_CLEAR(i, mask);
|
BIT_CLEAR(i, mask);
|
BIT_CLEAR(j, mask);
|
BIT_CLEAR(j, mask);
|
BIT_CLEAR(k, mask);
|
BIT_CLEAR(k, mask);
|
sm = SliceMatrix(m, mask, mask);
|
sm = SliceMatrix(m, mask, mask);
|
det = InvertMatrix(sm, sm);
|
det = InvertMatrix(sm, sm);
|
/*
|
/*
|
if(det == 0.0)
|
if(det == 0.0)
|
printf("det still 0\n");
|
printf("det still 0\n");
|
else {
|
else {
|
printf("det = %g\n", det);
|
printf("det = %g\n", det);
|
}
|
}
|
*/
|
*/
|
if(_abs(det) > _abs(maxdet))
|
if(_abs(det) > _abs(maxdet))
|
maxdet = det, mi = i, mj = j, mk = k;
|
maxdet = det, mi = i, mj = j, mk = k;
|
FreeMatrix(sm);
|
FreeMatrix(sm);
|
}
|
}
|
}
|
}
|
}
|
}
|
printf("maxdet=%g when rows %d,%d&%d left out\n", maxdet, mi, mj, mk);
|
printf("maxdet=%g when rows %d,%d&%d left out\n", maxdet, mi, mj, mk);
|
if(mk == -1)
|
if(mk == -1)
|
return 0.0;
|
return 0.0;
|
|
|
found:
|
found:
|
|
|
SET_BIT_VECTOR(mask);
|
SET_BIT_VECTOR(mask);
|
if(mi >= 0) BIT_CLEAR(mi, mask);
|
if(mi >= 0) BIT_CLEAR(mi, mask);
|
if(mj >= 0) BIT_CLEAR(mj, mask);
|
if(mj >= 0) BIT_CLEAR(mj, mask);
|
if(mk >= 0) BIT_CLEAR(mk, mask);
|
if(mk >= 0) BIT_CLEAR(mk, mask);
|
sm = SliceMatrix(m, mask, mask);
|
sm = SliceMatrix(m, mask, mask);
|
det = InvertMatrix(sm, sm);
|
det = InvertMatrix(sm, sm);
|
DeSliceMatrix(sm, 0.0, mask, mask, inv);
|
DeSliceMatrix(sm, 0.0, mask, mask, inv);
|
FreeMatrix(sm);
|
FreeMatrix(sm);
|
PrintMatrix(inv, "desliced:\n");
|
PrintMatrix(inv, "desliced:\n");
|
return det;
|
return det;
|
}
|
}
|
|
|
/* You can fairly confidently ignore the compiler warnings after here */
|
/* You can fairly confidently ignore the compiler warnings after here */
|
|
|
void
|
void
|
PrintVector(v, s,a1,a2,a3,a4,a5,a6,a7,a8)
|
PrintVector(v, s,a1,a2,a3,a4,a5,a6,a7,a8)
|
register Vector v;
|
register Vector v;
|
char *s; int a1,a2,a3,a4,a5,a6,a7,a8;
|
char *s; int a1,a2,a3,a4,a5,a6,a7,a8;
|
{
|
{
|
register int i;
|
register int i;
|
printf(s,a1,a2,a3,a4,a5,a6,a7,a8);
|
printf(s,a1,a2,a3,a4,a5,a6,a7,a8);
|
|
|
for(i = 0; i < NROWS(v); i++) printf(" %8.4f", v[i]);
|
for(i = 0; i < NROWS(v); i++) printf(" %8.4f", v[i]);
|
printf("\n");
|
printf("\n");
|
}
|
}
|
|
|
void
|
void
|
PrintMatrix(m, s,a1,a2,a3,a4,a5,a6,a7,a8)
|
PrintMatrix(m, s,a1,a2,a3,a4,a5,a6,a7,a8)
|
register Matrix m;
|
register Matrix m;
|
char *s; int a1,a2,a3,a4,a5,a6,a7,a8;
|
char *s; int a1,a2,a3,a4,a5,a6,a7,a8;
|
{
|
{
|
register int i, j;
|
register int i, j;
|
printf(s,a1,a2,a3,a4,a5,a6,a7,a8);
|
printf(s,a1,a2,a3,a4,a5,a6,a7,a8);
|
for(i = 0; i < NROWS(m); i++) {
|
for(i = 0; i < NROWS(m); i++) {
|
for(j = 0; j < NCOLS(m); j++)
|
for(j = 0; j < NCOLS(m); j++)
|
printf(" %8.4f", m[i][j]);
|
printf(" %8.4f", m[i][j]);
|
printf("\n");
|
printf("\n");
|
}
|
}
|
}
|
}
|
|
|
void
|
void
|
PrintArray(a, s,a1,a2,a3,a4,a5,a6,a7,a8)
|
PrintArray(a, s,a1,a2,a3,a4,a5,a6,a7,a8)
|
Array a;
|
Array a;
|
char *s; int a1,a2,a3,a4,a5,a6,a7,a8;
|
char *s; int a1,a2,a3,a4,a5,a6,a7,a8;
|
{
|
{
|
switch(NDIMS(a)) {
|
switch(NDIMS(a)) {
|
case 1: PrintVector((Vector) a, s,a1,a2,a3,a4,a5,a6,a7,a8); break;
|
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;
|
case 2: PrintMatrix((Matrix) a, s,a1,a2,a3,a4,a5,a6,a7,a8); break;
|
default: error("PrintArray");
|
default: error("PrintArray");
|
}
|
}
|
}
|
}
|
|
|
|
|
