matfunc.c

早期freebsd实现

	offset = 0;
	for (i = 0; i < dim; i++) {
		if (indices->v_type != V_NUM)
			math_error("Non-numeric index for matrix");
		q = indices->v_num;
		if (qisfrac(q))
			math_error("Non-integral index for matrix");
		index = qtoi(q);
		if (zisbig(q->num) || (index < mp->m_min[i]) || (index > mp->m_max[i]))
			math_error("Index out of bounds for matrix");
		offset *= (mp->m_max[i] - mp->m_min[i] + 1);
		offset += (index - mp->m_min[i]);
		indices++;
	}
	return mp->m_table + offset;
}


/*
 * Search a matrix for the specified value, starting with the specified index.
 * Returns the index of the found value, or -1 if the value was not found.
 */
long
matsearch(m, vp, index)
	MATRIX *m;
	VALUE *vp;
	long index;
{
	register VALUE *val;

	if (index < 0)
		index = 0;
	val = &m->m_table[index];
	while (index < m->m_size) {
		if (!comparevalue(vp, val))
			return index;
		index++;
		val++;
	}
	return -1;
}


/*
 * Search a matrix backwards for the specified value, starting with the
 * specified index.  Returns the index of the found value, or -1 if the
 * value was not found.
 */
long
matrsearch(m, vp, index)
	MATRIX *m;
	VALUE *vp;
	long index;
{
	register VALUE *val;

	if (index >= m->m_size)
		index = m->m_size - 1;
	val = &m->m_table[index];
	while (index >= 0) {
		if (!comparevalue(vp, val))
			return index;
		index--;
		val--;
	}
	return -1;
}


/*
 * Fill all of the elements of a matrix with one of two specified values.
 * All entries are filled with the first specified value, except that if
 * the matrix is square and the second value pointer is non-NULL, then
 * all diagonal entries are filled with the second value.  This routine
 * affects the supplied matrix directly, and doesn't return a copy.
 */
void
matfill(m, v1, v2)
	MATRIX *m;		/* matrix to be filled */
	VALUE *v1;		/* value to fill most of matrix with */
	VALUE *v2;		/* value for diagonal entries (or NULL) */
{
	register VALUE *val;
	long row, col;
	long rows;
	long index;

	if (v2 && ((m->m_dim != 2) ||
		((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))))
			math_error("Filling diagonals of non-square matrix");
	val = m->m_table;
	for (index = m->m_size; index > 0; index--)
		freevalue(val++);
	val = m->m_table;
	if (v2 == NULL) {
		for (index = m->m_size; index > 0; index--)
			copyvalue(v1, val++);
		return;
	}
	rows = m->m_max[0] - m->m_min[0] + 1;
	for (row = 0; row < rows; row++) {
		for (col = 0; col < rows; col++) {
			copyvalue(((row != col) ? v1 : v2), val++);
		}
	}
}


/*
 * Set a copy of a square matrix to the identity matrix.
 */
static MATRIX *
matident(m)
	MATRIX *m;
{
	register VALUE *val;	/* current value */
	long row, col;		/* current row and column */
	long rows;		/* number of rows */
	MATRIX *res;		/* resulting matrix */

	if (m->m_dim != 2)
		math_error("Matrix dimension must be two for setting to identity");
	if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
		math_error("Matrix must be square for setting to identity");
	res = matalloc(m->m_size);
	*res = *m;
	val = res->m_table;
	rows = (res->m_max[0] - res->m_min[0] + 1);
	for (row = 0; row < rows; row++) {
		for (col = 0; col < rows; col++) {
			val->v_type = V_NUM;
			val->v_num = ((row == col) ? qlink(&_qone_) : qlink(&_qzero_));
			val++;
		}
	}
	return res;
}


/*
 * Calculate the inverse of a matrix if it exists.
 * This is done by using transformations on the supplied matrix to convert
 * it to the identity matrix, and simultaneously applying the same set of
 * transformations to the identity matrix.
 */
MATRIX *
matinv(m)
	MATRIX *m;
{
	MATRIX *res;		/* matrix to become the inverse */
	long rows;		/* number of rows */
	long cur;		/* current row being worked on */
	long row, col;		/* temp row and column values */
	VALUE *val;		/* current value in matrix*/
	VALUE mulval;		/* value to multiply rows by */
	VALUE tmpval;		/* temporary value */

	if (m->m_dim != 2)
		math_error("Matrix dimension must be two for inverse");
	if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
		math_error("Inverting non-square matrix");
	/*
	 * Begin by creating the identity matrix with the same attributes.
	 */
	res = matalloc(m->m_size);
	*res = *m;
	rows = (m->m_max[0] - m->m_min[0] + 1);
	val = res->m_table;
	for (row = 0; row < rows; row++) {
		for (col = 0; col < rows; col++) {
			if (row == col)
				val->v_num = qlink(&_qone_);
			else
				val->v_num = qlink(&_qzero_);
			val->v_type = V_NUM;
			val++;
		}
	}
	/*
	 * Now loop over each row, and eliminate all entries in the
	 * corresponding column by using row operations.  Do the same
	 * operations on the resulting matrix.  Copy the original matrix
	 * so that we don't destroy it.
	 */
	m = matcopy(m);
	for (cur = 0; cur < rows; cur++) {
		/*
		 * Find the first nonzero value in the rest of the column
		 * downwards from [cur,cur].  If there is no such value, then
		 * the matrix is not invertible.  If the first nonzero entry
		 * is not the current row, then swap the two rows to make the
		 * current one nonzero.
		 */
		row = cur;
		val = &m->m_table[(row * rows) + row];
		while (testvalue(val) == 0) {
			if (++row >= rows) {
				matfree(m);
				matfree(res);
				math_error("Matrix is not invertible");
			}
			val += rows;
		}
		invertvalue(val, &mulval);
		if (row != cur) {
			matswaprow(m, row, cur);
			matswaprow(res, row, cur);
		}
		/*
		 * Now for every other nonzero entry in the current column, subtract
		 * the appropriate multiple of the current row to force that entry
		 * to become zero.
		 */
		val = &m->m_table[cur];
		/* ignore Saber-C warning about bad pointer val */
		for (row = 0; row < rows; row++, val += rows) {
			if ((row == cur) || (testvalue(val) == 0))
				continue;
			mulvalue(val, &mulval, &tmpval);
			matsubrow(m, row, cur, &tmpval);
			matsubrow(res, row, cur, &tmpval);
			freevalue(&tmpval);
		}
		freevalue(&mulval);
	}
	/*
	 * Now the original matrix has nonzero entries only on its main diagonal.
	 * Scale the rows of the result matrix by the inverse of those entries.
	 */
	val = m->m_table;
	for (row = 0; row < rows; row++) {
		if ((val->v_type != V_NUM) || !qisone(val->v_num)) {
			invertvalue(val, &mulval);
			matmulrow(res, row, &mulval);
			freevalue(&mulval);
		}
		/* ignore Saber-C warning about bad pointer val */
		val += (rows + 1);
	}
	matfree(m);
	return res;
}


/*
 * Calculate the determinant of a square matrix.
 * This is done using row operations to create an upper-diagonal matrix.
 */
VALUE
matdet(m)
	MATRIX *m;
{
	long rows;		/* number of rows */
	long cur;		/* current row being worked on */
	long row;		/* temp row values */
	int neg;		/* whether to negate determinant */
	VALUE *val;		/* current value */
	VALUE mulval, tmpval;	/* other values */

	if (m->m_dim != 2)
		math_error("Matrix dimension must be two for determinant");
	if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
		math_error("Non-square matrix for determinant");
	/*
	 * Loop over each row, and eliminate all lower entries in the
	 * corresponding column by using row operations.  Copy the original
	 * matrix so that we don't destroy it.
	 */
	neg = 0;
	m = matcopy(m);
	rows = (m->m_max[0] - m->m_min[0] + 1);
	for (cur = 0; cur < rows; cur++) {
		/*
		 * Find the first nonzero value in the rest of the column
		 * downwards from [cur,cur].  If there is no such value, then
		 * the determinant is zero.  If the first nonzero entry is not
		 * the current row, then swap the two rows to make the current
		 * one nonzero, and remember that the determinant changes sign.
		 */
		row = cur;
		val = &m->m_table[(row * rows) + row];
		while (testvalue(val) == 0) {
			if (++row >= rows) {
				matfree(m);
				mulval.v_type = V_NUM;
				mulval.v_num = qlink(&_qzero_);
				return mulval;
			}
			val += rows;
		}
		invertvalue(val, &mulval);
		if (row != cur) {
			matswaprow(m, row, cur);
			neg = !neg;
		}
		/*
		 * Now for every other nonzero entry lower down in the current column,
		 * subtract the appropriate multiple of the current row to force that
		 * entry to become zero.
		 */
		row = cur + 1;
		/* ignore Saber-C warning about bad pointer into val */
		val = &m->m_table[(row * rows) + cur];
		/* ignore Saber-C warning about bad pointer into val */
		for (; row < rows; row++, val += rows) {
			if (testvalue(val) == 0)
				continue;
			mulvalue(val, &mulval, &tmpval);
			matsubrow(m, row, cur, &tmpval);
			freevalue(&tmpval);
		}
		freevalue(&mulval);
	}
	/*
	 * Now the matrix is upper-diagonal, and the determinant is the
	 * product of the main diagonal entries, and is possibly negated.
	 */
	val = m->m_table;
	mulval.v_type = V_NUM;
	mulval.v_num = qlink(&_qone_);
	for (row = 0; row < rows; row++) {
		mulvalue(&mulval, val, &tmpval);
		freevalue(&mulval);
		mulval = tmpval;
		/* ignore Saber-C warning about bad pointer into val */
		val += (rows + 1);
	}
	matfree(m);
	if (neg) {
		negvalue(&mulval, &tmpval);
		freevalue(&mulval);
		return tmpval;
	}
	return mulval;
}


/*
 * Local utility routine to swap two rows of a square matrix.
 * No checks are made to verify the legality of the arguments.
 */
static void
matswaprow(m, r1, r2)
	MATRIX *m;
	long r1, r2;
{
	register VALUE *v1, *v2;
	register long rows;
	VALUE tmp;

	if (r1 == r2)
		return;
	rows = (m->m_max[0] - m->m_min[0] + 1);
	v1 = &m->m_table[r1 * rows];
	v2 = &m->m_table[r2 * rows];
	while (rows-- > 0) {
		tmp = *v1;
		*v1 = *v2;
		*v2 = tmp;
		v1++;
		v2++;
	}
}


/*
 * Local utility routine to subtract a multiple of one row to another one.
 * The row to be changed is oprow, the row to be subtracted is baserow.
 * No checks are made to verify the legality of the arguments.
 */
static void
matsubrow(m, oprow, baserow, mulval)
	MATRIX *m;
	long oprow, baserow;
	VALUE *mulval;
{
	register VALUE *vop, *vbase;
	register long entries;
	VALUE tmp1, tmp2;

	entries = (m->m_max[0] - m->m_min[0] + 1);
	vop = &m->m_table[oprow * entries];
	vbase = &m->m_table[baserow * entries];
	while (entries-- > 0) {
		mulvalue(vbase, mulval, &tmp1);
		subvalue(vop, &tmp1, &tmp2);
		freevalue(&tmp1);
		freevalue(vop);
		*vop = tmp2;
		vop++;
		vbase++;
	}
}


/*
 * Local utility routine to multiply a row by a specified number.
 * No checks are made to verify the legality of the arguments.
 */
static void
matmulrow(m, row, mulval)
	MATRIX *m;
	long row;
	VALUE *mulval;
{
	register VALUE *val;
	register long rows;
	VALUE tmp;

	rows = (m->m_max[0] - m->m_min[0] + 1);
	val = &m->m_table[row * rows];
	while (rows-- > 0) {
		mulvalue(val, mulval, &tmp);
		freevalue(val);
		*val = tmp;
		val++;
	}
}


/*
 * Make a full copy of a matrix.
 */
MATRIX *
matcopy(m)
	MATRIX *m;
{
	MATRIX *res;
	register VALUE *v1, *v2;
	register long i;

	res = matalloc(m->m_size);
	*res = *m;
	v1 = m->m_table;
	v2 = res->m_table;
	i = m->m_size;
	while (i-- > 0) {
		if (v1->v_type == V_NUM) {
			v2->v_type = V_NUM;
			v2->v_num = qlink(v1->v_num);
		} else
			copyvalue(v1, v2);
		v1++;
		v2++;
	}
	return res;
}


/*
 * Allocate a matrix with the specified number of elements.
 */
MATRIX *
matalloc(size)
	long size;
{
	MATRIX *m;

	m = (MATRIX *) malloc(matsize(size));
	if (m == NULL)
		math_error("Cannot get memory to allocate matrix of size %d", size);
	m->m_size = size;
	return m;
}


/*
 * Free a matrix, along with all of its element values.
 */
void
matfree(m)
	MATRIX *m;
{
	register VALUE *vp;
	register long i;

	vp = m->m_table;
	i = m->m_size;
	while (i-- > 0) {
		if (vp->v_type == V_NUM) {
			vp->v_type = V_NULL;
			qfree(vp->v_num);
		} else
			freevalue(vp);
		vp++;
	}
	free(m);
}


/*
 * Test whether a matrix has any nonzero values.
 * Returns TRUE if so.
 */
BOOL
mattest(m)
	MATRIX *m;
{
	register VALUE *vp;
	register long i;

	vp = m->m_table;
	i = m->m_size;
	while (i-- > 0) {
		if ((vp->v_type != V_NUM) || (!qiszero(vp->v_num)))
			return TRUE;
		vp++;
	}
	return FALSE;
}


/*
 * Test whether or not two matrices are equal.
 * Equality is determined by the shape and values of the matrices,
 * but not by their index bounds.  Returns TRUE if they differ.
 */
BOOL
matcmp(m1, m2)
	MATRIX *m1, *m2;
{
	VALUE *v1, *v2;
	long i;

	if (m1 == m2)
		return FALSE;
	if ((m1->m_dim != m2->m_dim) || (m1->m_size != m2->m_size))
		return TRUE;
	for (i = 0; i < m1->m_dim; i++) {
		if ((m1->m_max[i] - m1->m_min[i]) != (m2->m_max[i] - m2->m_min[i]))
		return TRUE;
	}
	v1 = m1->m_table;
	v2 = m2->m_table;
	i = m1->m_size;
	while (i-- > 0) {
		if (comparevalue(v1++, v2++))
			return TRUE;
	}
	return FALSE;
}


#if 0
/*
 * Test whether or not a matrix is the identity matrix.
 * Returns TRUE if so.
 */
BOOL
matisident(m)
	MATRIX *m;
{
	register VALUE *val;	/* current value */
	long row, col;		/* row and column numbers */

	if ((m->m_dim != 2) ||
		((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1])))
			return FALSE;
	val = m->m_table;
	for (row = 0; row < m->m_size; row++) {
		for (col = 0; col < m->m_size; col++) {
			if (val->v_type != V_NUM)
				return FALSE;
			if (row == col) {
				if (!qisone(val->v_num))
					return FALSE;
			} else {
				if (!qiszero(val->v_num))
					return FALSE;
			}
			val++;
		}
	}
	return TRUE;
}
#endif


/*
 * Return a trivial hash value for a matrix.
 */
HASH
mathash(m)
	MATRIX *m;
{
	HASH hash;
	long fullsize;
	long skip;
	int i;
	VALUE *vp;

	hash = m->m_dim * 500009;
	fullsize = 1;
	for (i = m->m_dim - 1; i >= 0; i--) {
		hash = hash * 500029 + m->m_max[i];
		fullsize *= (m->m_max[i] - m->m_min[i] + 1);
	}
	hash = hash * 500041 + fullsize;
	vp = m->m_table;
	for (i = 0; ((i < fullsize) && (i < 16)); i++)
		hash = hash * 500057 + hashvalue(vp++);
	i = 16;
	vp = &m->m_table[16];
	skip = (fullsize / 11) + 1;
	while (i < fullsize) {
		hash = hash * 500069 + hashvalue(vp);
		i += skip;
		vp += skip;
	}
	return hash;
}


/*
 * Print a matrix and possibly few of its elements.
 * The argument supplied specifies how many elements to allow printing.
 * If any elements are printed, they are printed in short form.
 */
void
matprint(m, max_print)
	MATRIX *m;
	long max_print;
{
	VALUE *vp;
	long fullsize, count, index, num;
	int dim, i;
	char *msg;
	long sizes[MAXDIM];

	dim = m->m_dim;
	fullsize = 1;
	for (i = dim - 1; i >= 0; i--) {
		sizes[i] = fullsize;
		fullsize *= (m->m_max[i] - m->m_min[i] + 1);
	}
	msg = ((max_print > 0) ? "\nmat [" : "mat [");
	for (i = 0; i < dim; i++) {
		if (m->m_min[i])
			math_fmt("%s%ld:%ld", msg, m->m_min[i], m->m_max[i]);
		else
			math_fmt("%s%ld", msg, m->m_max[i] + 1);
		msg = ",";
	}
	if (max_print > fullsize)
		max_print = fullsize;
	vp = m->m_table;
	count = 0;
	for (index = 0; index < fullsize; index++) {
		if ((vp->v_type != V_NUM) || !qiszero(vp->v_num))
			count++;
		vp++;
	}
	math_fmt("] (%ld element%s, %ld nonzero)",
		fullsize, (fullsize == 1) ? "" : "s", count);
	if (max_print <= 0)
		return;

	/*
	 * Now print the first few elements of the matrix in short
	 * and unambigous format.
	 */
	math_str(":\n");
	vp = m->m_table;
	for (index = 0; index < max_print; index++) {
		msg = "  [";
		num = index;
		for (i = 0; i < dim; i++) {
			math_fmt("%s%ld", msg, m->m_min[i] + (num / sizes[i]));
			num %= sizes[i];
			msg = ",";
		}
		math_str("] = ");
		printvalue(vp++, PRINT_SHORT | PRINT_UNAMBIG);
		math_str("\n");
	}
	if (max_print < fullsize)
		math_str("  ...\n");
}

/* END CODE */