/***********************************************************************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");	}}