matfunc.c

早期freebsd实现

/*
 * Copyright (c) 1994 David I. Bell
 * Permission is granted to use, distribute, or modify this source,
 * provided that this copyright notice remains intact.
 *
 * Extended precision rational arithmetic matrix functions.
 * Matrices can contain arbitrary types of elements.
 */

#include "value.h"


static void matswaprow MATH_PROTO((MATRIX *m, long r1, long r2));
static void matsubrow MATH_PROTO((MATRIX *m, long oprow, long baserow,
	VALUE *mulval));
static void matmulrow MATH_PROTO((MATRIX *m, long row, VALUE *mulval));
static MATRIX *matident MATH_PROTO((MATRIX *m));



/*
 * Add two compatible matrices.
 */
MATRIX *
matadd(m1, m2)
	MATRIX *m1, *m2;
{
	int dim;

	long min1, min2, max1, max2, index;
	VALUE *v1, *v2, *vres;
	MATRIX *res;
	MATRIX tmp;

	if (m1->m_dim != m2->m_dim)
		math_error("Incompatible matrix dimensions for add");
	tmp.m_dim = m1->m_dim;
	tmp.m_size = m1->m_size;
	for (dim = 0; dim < m1->m_dim; dim++) {
		min1 = m1->m_min[dim];
		max1 = m1->m_max[dim];
		min2 = m2->m_min[dim];
		max2 = m2->m_max[dim];
		if ((min1 && min2 && (min1 != min2)) || ((max1-min1) != (max2-min2)))
			math_error("Incompatible matrix bounds for add");
		tmp.m_min[dim] = (min1 ? min1 : min2);
		tmp.m_max[dim] = tmp.m_min[dim] + (max1 - min1);
	}
	res = matalloc(m1->m_size);
	*res = tmp;
	v1 = m1->m_table;
	v2 = m2->m_table;
	vres = res->m_table;
	for (index = m1->m_size; index > 0; index--)
		addvalue(v1++, v2++, vres++);
	return res;
}


/*
 * Subtract two compatible matrices.
 */
MATRIX *
matsub(m1, m2)
	MATRIX *m1, *m2;
{
	int dim;
	long min1, min2, max1, max2, index;
	VALUE *v1, *v2, *vres;
	MATRIX *res;
	MATRIX tmp;

	if (m1->m_dim != m2->m_dim)
		math_error("Incompatible matrix dimensions for sub");
	tmp.m_dim = m1->m_dim;
	tmp.m_size = m1->m_size;
	for (dim = 0; dim < m1->m_dim; dim++) {
		min1 = m1->m_min[dim];
		max1 = m1->m_max[dim];
		min2 = m2->m_min[dim];
		max2 = m2->m_max[dim];
		if ((min1 && min2 && (min1 != min2)) || ((max1-min1) != (max2-min2)))
			math_error("Incompatible matrix bounds for sub");
		tmp.m_min[dim] = (min1 ? min1 : min2);
		tmp.m_max[dim] = tmp.m_min[dim] + (max1 - min1);
	}
	res = matalloc(m1->m_size);
	*res = tmp;
	v1 = m1->m_table;
	v2 = m2->m_table;
	vres = res->m_table;
	for (index = m1->m_size; index > 0; index--)
		subvalue(v1++, v2++, vres++);
	return res;
}


/*
 * Produce the negative of a matrix.
 */
MATRIX *
matneg(m)
	MATRIX *m;
{
	register VALUE *val, *vres;
	long index;
	MATRIX *res;

	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		negvalue(val++, vres++);
	return res;
}


/*
 * Multiply two compatible matrices.
 */
MATRIX *
matmul(m1, m2)
	MATRIX *m1, *m2;
{
	register MATRIX *res;
	long i1, i2, max1, max2, index, maxindex;
	VALUE *v1, *v2;
	VALUE sum, tmp1, tmp2;

	if ((m1->m_dim != 2) || (m2->m_dim != 2))
		math_error("Matrix dimension must be two for mul");
	if ((m1->m_max[1] - m1->m_min[1]) != (m2->m_max[0] - m2->m_min[0]))
		math_error("Incompatible bounds for matrix mul");
	max1 = (m1->m_max[0] - m1->m_min[0] + 1);
	max2 = (m2->m_max[1] - m2->m_min[1] + 1);
	maxindex = (m1->m_max[1] - m1->m_min[1] + 1);
	res = matalloc(max1 * max2);
	res->m_dim = 2;
	res->m_min[0] = m1->m_min[0];
	res->m_max[0] = m1->m_max[0];
	res->m_min[1] = m2->m_min[1];
	res->m_max[1] = m2->m_max[1];
	for (i1 = 0; i1 < max1; i1++) {
		for (i2 = 0; i2 < max2; i2++) {
			sum.v_num = qlink(&_qzero_);
			sum.v_type = V_NUM;
			v1 = &m1->m_table[i1 * maxindex];
			v2 = &m2->m_table[i2];
			for (index = 0; index < maxindex; index++) {
				mulvalue(v1, v2, &tmp1);
				addvalue(&sum, &tmp1, &tmp2);
				freevalue(&tmp1);
				freevalue(&sum);
				sum = tmp2;
				v1++;
				v2 += max2;
			}
			index = (i1 * max2) + i2;
			res->m_table[index] = sum;
		}
	}
	return res;
}


/*
 * Square a matrix.
 */
MATRIX *
matsquare(m)
	MATRIX *m;
{
	register MATRIX *res;
	long i1, i2, max, index;
	VALUE *v1, *v2;
	VALUE sum, tmp1, tmp2;

	if (m->m_dim != 2)
		math_error("Matrix dimension must be two for square");
	if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
		math_error("Squaring non-square matrix");
	max = (m->m_max[0] - m->m_min[0] + 1);
	res = matalloc(max * max);
	res->m_dim = 2;
	res->m_min[0] = m->m_min[0];
	res->m_max[0] = m->m_max[0];
	res->m_min[1] = m->m_min[1];
	res->m_max[1] = m->m_max[1];
	for (i1 = 0; i1 < max; i1++) {
		for (i2 = 0; i2 < max; i2++) {
			sum.v_num = qlink(&_qzero_);
			sum.v_type = V_NUM;
			v1 = &m->m_table[i1 * max];
			v2 = &m->m_table[i2];
			for (index = 0; index < max; index++) {
				mulvalue(v1, v2, &tmp1);
				addvalue(&sum, &tmp1, &tmp2);
				freevalue(&tmp1);
				freevalue(&sum);
				sum = tmp2;
				v1++;
				v2 += max;
			}
			index = (i1 * max) + i2;
			res->m_table[index] = sum;
		}
	}
	return res;
}


/*
 * Compute the result of raising a square matrix to an integer power.
 * Negative powers mean the positive power of the inverse.
 * Note: This calculation could someday be improved for large powers
 * by using the characteristic polynomial of the matrix.
 */
MATRIX *
matpowi(m, q)
	MATRIX *m;		/* matrix to be raised */
	NUMBER *q;		/* power to raise it to */
{
	MATRIX *res, *tmp;
	long power;		/* power to raise to */
	unsigned long bit;	/* current bit value */

	if (m->m_dim != 2)
		math_error("Matrix dimension must be two for power");
	if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
		math_error("Raising non-square matrix to a power");
	if (qisfrac(q))
		math_error("Raising matrix to non-integral power");
	if (zisbig(q->num))
		math_error("Raising matrix to very large power");
	power = (zistiny(q->num) ? z1tol(q->num) : z2tol(q->num));
	if (qisneg(q))
		power = -power;
	/*
	 * Handle some low powers specially
	 */
	if ((power <= 4) && (power >= -2)) {
		switch ((int) power) {
			case 0:
				return matident(m);
			case 1:
				return matcopy(m);
			case -1:
				return matinv(m);
			case 2:
				return matsquare(m);
			case -2:
				tmp = matinv(m);
				res = matsquare(tmp);
				matfree(tmp);
				return res;
			case 3:
				tmp = matsquare(m);
				res = matmul(m, tmp);
				matfree(tmp);
				return res;
			case 4:
				tmp = matsquare(m);
				res = matsquare(tmp);
				matfree(tmp);
				return res;
		}
	}
	if (power < 0) {
		m = matinv(m);
		power = -power;
	}
	/*
	 * Compute the power by squaring and multiplying.
	 * This uses the left to right method of power raising.
	 */
	bit = TOPFULL;
	while ((bit & power) == 0)
		bit >>= 1L;
	bit >>= 1L;
	res = matsquare(m);
	if (bit & power) {
		tmp = matmul(res, m);
		matfree(res);
		res = tmp;
	}
	bit >>= 1L;
	while (bit) {
		tmp = matsquare(res);
		matfree(res);
		res = tmp;
		if (bit & power) {
			tmp = matmul(res, m);
			matfree(res);
			res = tmp;
		}
		bit >>= 1L;
	}
	if (qisneg(q))
		matfree(m);
	return res;
}


/*
 * Calculate the cross product of two one dimensional matrices each
 * with three components.
 *	m3 = matcross(m1, m2);
 */
MATRIX *
matcross(m1, m2)
	MATRIX *m1, *m2;
{
	MATRIX *res;
	VALUE *v1, *v2, *vr;
	VALUE tmp1, tmp2;

	if ((m1->m_dim != 1) || (m2->m_dim != 1))
		math_error("Matrix not 1d for cross product");
	if ((m1->m_size != 3) || (m2->m_size != 3))
		math_error("Matrix not size 3 for cross product");
	res = matalloc(3L);
	res->m_dim = 1;
	res->m_min[0] = 0;
	res->m_max[0] = 2;
	v1 = m1->m_table;
	v2 = m2->m_table;
	vr = res->m_table;
	mulvalue(v1 + 1, v2 + 2, &tmp1);
	mulvalue(v1 + 2, v2 + 1, &tmp2);
	subvalue(&tmp1, &tmp2, vr + 0);
	freevalue(&tmp1);
	freevalue(&tmp2);
	mulvalue(v1 + 2, v2 + 0, &tmp1);
	mulvalue(v1 + 0, v2 + 2, &tmp2);
	subvalue(&tmp1, &tmp2, vr + 1);
	freevalue(&tmp1);
	freevalue(&tmp2);
	mulvalue(v1 + 0, v2 + 1, &tmp1);
	mulvalue(v1 + 1, v2 + 0, &tmp2);
	subvalue(&tmp1, &tmp2, vr + 2);
	freevalue(&tmp1);
	freevalue(&tmp2);
	return res;
}


/*
 * Return the dot product of two matrices.
 *	result = matdot(m1, m2);
 */
VALUE
matdot(m1, m2)
	MATRIX *m1, *m2;
{
	VALUE *v1, *v2;
	VALUE result, tmp1, tmp2;
	long len;

	if ((m1->m_dim != 1) || (m2->m_dim != 1))
		math_error("Matrix not 1d for dot product");
	if (m1->m_size != m2->m_size)
		math_error("Incompatible matrix sizes for dot product");
	v1 = m1->m_table;
	v2 = m2->m_table;
	mulvalue(v1, v2, &result);
	len = m1->m_size;
	while (--len > 0) {
		mulvalue(++v1, ++v2, &tmp1);
		addvalue(&result, &tmp1, &tmp2);
		freevalue(&tmp1);
		freevalue(&result);
		result = tmp2;
	}
	return result;
}


/*
 * Scale the elements of a matrix by a specified power of two.
 */
MATRIX *
matscale(m, n)
	MATRIX *m;		/* matrix to be scaled */
	long n;
{
	register VALUE *val, *vres;
	VALUE num;
	long index;
	MATRIX *res;		/* resulting matrix */

	if (n == 0)
		return matcopy(m);
	num.v_type = V_NUM;
	num.v_num = itoq(n);
	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		scalevalue(val++, &num, vres++);
	qfree(num.v_num);
	return res;
}


/*
 * Shift the elements of a matrix by the specified number of bits.
 * Positive shift means leftwards, negative shift rightwards.
 */
MATRIX *
matshift(m, n)
	MATRIX *m;		/* matrix to be scaled */
	long n;
{
	register VALUE *val, *vres;
	VALUE num;
	long index;
	MATRIX *res;		/* resulting matrix */

	if (n == 0)
		return matcopy(m);
	num.v_type = V_NUM;
	num.v_num = itoq(n);
	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		shiftvalue(val++, &num, FALSE, vres++);
	qfree(num.v_num);
	return res;
}


/*
 * Multiply the elements of a matrix by a specified value.
 */
MATRIX *
matmulval(m, vp)
	MATRIX *m;		/* matrix to be multiplied */
	VALUE *vp;		/* value to multiply by */
{
	register VALUE *val, *vres;
	long index;
	MATRIX *res;

	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		mulvalue(val++, vp, vres++);
	return res;
}


/*
 * Divide the elements of a matrix by a specified value, keeping
 * only the integer quotient.
 */
MATRIX *
matquoval(m, vp)
	MATRIX *m;		/* matrix to be divided */
	VALUE *vp;		/* value to divide by */
{
	register VALUE *val, *vres;
	long index;
	MATRIX *res;

	if ((vp->v_type == V_NUM) && qiszero(vp->v_num))
		math_error("Division by zero");
	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		quovalue(val++, vp, vres++);
	return res;
}


/*
 * Divide the elements of a matrix by a specified value, keeping
 * only the remainder of the division.
 */
MATRIX *
matmodval(m, vp)
	MATRIX *m;		/* matrix to be divided */
	VALUE *vp;		/* value to divide by */
{
	register VALUE *val, *vres;
	long index;
	MATRIX *res;

	if ((vp->v_type == V_NUM) && qiszero(vp->v_num))
		math_error("Division by zero");
	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		modvalue(val++, vp, vres++);
	return res;
}


MATRIX *
mattrans(m)
	MATRIX *m;			/* matrix to be transposed */
{
	register VALUE *v1, *v2;	/* current values */
	long rows, cols;		/* rows and columns in new matrix */
	long row, col;			/* current row and column */
	MATRIX *res;

	if (m->m_dim != 2)
		math_error("Matrix dimension must be two for transpose");
	res = matalloc(m->m_size);
	res->m_dim = 2;
	res->m_min[0] = m->m_min[1];
	res->m_max[0] = m->m_max[1];
	res->m_min[1] = m->m_min[0];
	res->m_max[1] = m->m_max[0];
	rows = (m->m_max[1] - m->m_min[1] + 1);
	cols = (m->m_max[0] - m->m_min[0] + 1);
	v1 = res->m_table;
	for (row = 0; row < rows; row++) {
		v2 = &m->m_table[row];
		for (col = 0; col < cols; col++) {
			copyvalue(v2, v1);
			v1++;
			v2 += rows;
		}
	}
	return res;
}


/*
 * Produce a matrix with values all of which are conjugated.
 */
MATRIX *
matconj(m)
	MATRIX *m;
{
	register VALUE *val, *vres;
	long index;
	MATRIX *res;

	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		conjvalue(val++, vres++);
	return res;
}


/*
 * Produce a matrix with values all of which have been rounded to the
 * specified number of decimal places.
 */
MATRIX *
matround(m, places)
	MATRIX *m;
	long places;
{
	register VALUE *val, *vres;
	VALUE tmp;
	long index;
	MATRIX *res;

	if (places < 0)
		math_error("Negative number of places for matround");
	res = matalloc(m->m_size);
	*res = *m;
	tmp.v_type = V_INT;
	tmp.v_int = places;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		roundvalue(val++, &tmp, vres++);
	return res;
}


/*
 * Produce a matrix with values all of which have been rounded to the
 * specified number of binary places.
 */
MATRIX *
matbround(m, places)
	MATRIX *m;
	long places;
{
	register VALUE *val, *vres;
	VALUE tmp;
	long index;
	MATRIX *res;

	if (places < 0)
		math_error("Negative number of places for matbround");
	res = matalloc(m->m_size);
	*res = *m;
	tmp.v_type = V_INT;
	tmp.v_int = places;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		broundvalue(val++, &tmp, vres++);
	return res;
}


/*
 * Produce a matrix with values all of which have been truncated to integers.
 */
MATRIX *
matint(m)
	MATRIX *m;
{
	register VALUE *val, *vres;
	long index;
	MATRIX *res;

	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		intvalue(val++, vres++);
	return res;
}


/*
 * Produce a matrix with values all of which have only the fraction part left.
 */
MATRIX *
matfrac(m)
	MATRIX *m;
{
	register VALUE *val, *vres;
	long index;
	MATRIX *res;

	res = matalloc(m->m_size);
	*res = *m;
	val = m->m_table;
	vres = res->m_table;
	for (index = m->m_size; index > 0; index--)
		fracvalue(val++, vres++);
	return res;
}


/*
 * Index a matrix normally by the specified set of index values.
 * Returns the address of the matrix element if it is valid, or generates
 * an error if the index values are out of range.  The create flag is TRUE
 * if the element is to be written, but this is ignored here.
 */
/*ARGSUSED*/
VALUE *
matindex(mp, create, dim, indices)
	MATRIX *mp;
	BOOL create;
	long dim;		/* dimension of the indexing */
	VALUE *indices;		/* table of values being indexed by */
{
	NUMBER *q;		/* index value */
	long index;		/* index value as an integer */
	long offset;		/* current offset into array */
	int i;			/* loop counter */

	if ((dim <= 0) || (dim > MAXDIM))
		math_error("Bad dimension %ld for matrix", dim);
	if (mp->m_dim != dim)
		math_error("Indexing a %ldd matrix as a %ldd matrix", mp->m_dim, dim);