#include <../source/TAL/objbase.h>

/*
 * Helper macro for computing 2D array indices
 */
#define REINITIALIZE_MATRIX_INDEX(i,mat) \
	(mat)->cell[0]=(mat)->data; \
	for((i)=1;(i)<(mat)->rows;(i)++) \
		(mat)->cell[(i)]=(mat)->cell[(i)-1]+(mat)->cols;

/*
 * Retuns memAlloc()'ed block
 */
matrix_p V_GLOBAL
matAlloc(u32_t rows,u32_t cols)
{
	matrix_t *m;
	u32_t i,size;
	if(!rows || !cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matAlloc(%d,%d): incorrect parameter(s).\n",
			rows,cols);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	size=
		sizeof(matrix_t)+
		rows*sizeof(vec_t*)+
		rows*cols*sizeof(vec_t);
	m = (matrix_t*)memAlloc(size);
	if(!m)
		return NULL;
	m->rows=rows;
	m->cols=cols;
	m->cell=(vec_t**)(m+1);
	m->data=(vec_t*)(m->cell+rows);
	REINITIALIZE_MATRIX_INDEX(i,m);
	return m;
}

void V_GLOBAL
matCopy(matrix_t *dest,matrix_t *src)
{
	u32_t i;
	if(!dest || !src)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCopy(%X,%X): incorrect parameter(s).\n",
			dest,src);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(src->rows!=dest->rows || src->cols != dest->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"matCopy(): matrices with different size (%ux%u and %ux%u).\n",
			dest->rows,dest->cols,src->rows,src->cols);
#endif	/* CHECKED_BUILD */
		return;
	}
	REINITIALIZE_MATRIX_INDEX(i,dest);
	for(i=0;i<src->rows;i++)
		memCopy32(dest->cell[i],src->cell[i],(src->cols*sizeof(vec_t))>>2);
}

/*
 * Retuns memAlloc()'ed block
 */
matrix_p V_GLOBAL
matDuplicate(matrix_t *ref)
{
	u32_t i;
	matrix_t *d;
	if(!ref)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matDuplicate(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	d = matAlloc(ref->rows,ref->cols);
	if(!d)
		return NULL;
	/*
	 * We can't copy all block because pointers in 'ref' may be swapped
	 */
	for(i=0;i<ref->rows;i++)
		memCopy32(d->cell[i],ref->cell[i],(ref->cols*sizeof(vec_t))>>2);
	return d;
}

void	V_GLOBAL
matCopyRow(vector_t *dst,matrix_t *m,u32_t row)
{
	if(!dst || !m || row >= m->rows || dst->size != m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCopyRow(%X,%X,%d): incorrect parameter(s).\n",
			dst,m,row);
#endif	/* CHECKED_BUILD */
		return;
	}
	memCopy32(dst->elements, m->cell[row], (m->cols * sizeof(vec_t)) >> 2);
}

void	V_GLOBAL
matCopyCol(vector_t *dst,matrix_t *m,u32_t col)
{
	u32_t r;
	if(!dst || !m || col >= m->cols || dst->size != m->rows)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCopyCol(%X,%X,%d): incorrect parameter(s).\n",
			dst,m,col);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(r = 0;r < m->rows;r++)
		dst->elements[r] = m->cell[r][col];
}

vector_p	V_GLOBAL
matDuplicateRow(matrix_t *m,u32_t row)
{
	vector_p dst;
	dst = vecAlloc(m->cols);
	if(dst)
		matCopyRow(dst,m,row);
	return dst;
}

vector_p	V_GLOBAL
matDuplicateCol(matrix_t *m,u32_t col)
{
	vector_t *dst;
	dst = vecAlloc(m->rows);
	if(dst)
		matCopyCol(dst,m,col);
	return dst;
}

void	V_GLOBAL
matSetRect(matrix_p data,
	u32_t left,u32_t top,u32_t right,u32_t bottom,
	vec_t value)
{
	CALLER_NAME(matSetRect)
	u32_t r, c;
	vec_t *row;
	u32_t row_len, row_bytes;
	if(!data)
	{
		ERROR_GENERIC(error_incorrect_parameters);
		return;
	}
	/* clip */
	if(right >= data->cols)
		right = data->cols - 1;
	if(bottom >= data->rows)
		bottom = data->rows - 1;
	if(left > right || top > bottom)
		return;
	row_len = 1 + right - left;
	row_bytes = row_len * sizeof(vec_t);
	if(row_bytes >= 1024 /* more than 1 kb in each row */)
	{
		vec_p storage = (vec_p)memAlloc(row_bytes);
		for(c = 0;c < row_len;c++)
			storage[c] = value;
		if(row_bytes & 3) /* it will _never_ be true, though */
		{
			for(r = top;r <= bottom;r++)
				memCopy(data->cell[r] + left,storage,row_bytes);
		}
		else
		{
			row_bytes = row_bytes >> 2;
			for(r = top;r <= bottom;r++)
				memCopy32(data->cell[r] + left,storage,row_bytes);
		}
		memFree(storage);
		return;
	}
	for(r = top;r <= bottom;r++)
	{
		row = data->cell[r];
		for(c = left;c <= right;c++)
			row[c] = value;
	}
}

void	V_GLOBAL
matUnit(matrix_t *m)
{
	u32_t i,min;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matUnit(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	matZero(m);
	min = (m->rows > m->cols) ? m->cols : m->rows;
	for(i=0;i<min;i++)
		m->cell[i][i]=1.0;
}

void	V_GLOBAL
matAdd(matrix_t *dest,matrix_t *m1,matrix_t *m2)
{
	u32_t i,j;
	if(!dest || !m1 || !m2)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matAdd(%X,%X,%X): incorrect parameter(s).\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(m1->rows!=m2->rows || m1->cols!=m2->cols ||
		dest->rows!=m1->rows || dest->cols!=m1->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matAdd(%X,%X,%X): matrices with different sizes.\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<dest->rows;i++)
		for(j=0;j<dest->cols;j++)
			dest->cell[i][j]=m1->cell[i][j]+m2->cell[i][j];
}

void	V_GLOBAL
matSub(matrix_t *dest,matrix_t *m1,matrix_t *m2)
{
	u32_t i,j;
	if(!dest || !m1 || !m2)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matSub(%X,%X,%X): incorrect parameter(s).\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(m1->rows!=m2->rows || m1->cols!=m2->cols ||
		dest->rows!=m1->rows || dest->cols!=m1->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matSub(%X,%X,%X): matrices with different sizes.\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<dest->rows;i++)
		for(j=0;j<dest->cols;j++)
			dest->cell[i][j]=m1->cell[i][j]-m2->cell[i][j];
}

void	V_GLOBAL
matNegate(matrix_t *dest,matrix_t *m)
{
	u32_t i,j;
	if(!dest || !m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matNegate(%X,%X): incorrect parameter(s).\n",
			dest,m);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->rows!=m->rows || dest->cols!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matNegate(%X,%X): matrices with different sizes.\n",
			dest,m);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<dest->rows;i++)
		for(j=0;j<dest->cols;j++)
			dest->cell[i][j]=-m->cell[i][j];
}

vboolean	V_GLOBAL
matCompare(matrix_t *m1,matrix_t *m2)
{
	u32_t i,j;
	if(!m1 || !m2)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCompare(%X,%X): incorrect parameter(s).\n",
			m1,m2);
#endif	/* CHECKED_BUILD */
		return false;
	}
	if(m1->rows!=m2->rows || m1->cols!=m2->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCompare(%X,%X): matrices with different sizes.\n",
			m1,m2);
#endif	/* CHECKED_BUILD */
		return false;
	}
	for(i=0;i<m1->rows;i++)
		for(j=0;j<m1->cols;j++)
		{
			vec_t diff = m1->cell[i][j] - m2->cell[i][j];
			if(diff < -FLOAT_ZERO_EPSILON || diff > FLOAT_ZERO_EPSILON)
				return false;
		}
	return true;
}

void	V_GLOBAL
matMin(matrix_t *m,u32_t *row,u32_t *col)
{
	u32_t i,j,r,c;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matMin(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	r=c=0;
	for(i=0;i<m->rows;i++)
		for(j=0;j<m->cols;j++)
			if(m->cell[i][j]<m->cell[r][c]) 
			{
				r=i;
				c=j;
			}
	if(row)
		*row=r;
	if(col)
		*col=c;
}

void	V_GLOBAL
matMax(matrix_t *m,u32_t *row,u32_t *col)
{
	u32_t i,j,r,c;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matMax(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	r=c=0;
	for(i=0;i<m->rows;i++)
		for(j=0;j<m->cols;j++)
			if(m->cell[i][j]>m->cell[r][c]) 
			{
				r=i;
				c=j;
			}
	if(row)
		*row=r;
	if(col)
		*col=c;
}

void	V_GLOBAL
matScale(matrix_t *dest,matrix_t *m,vec_t x)
{
	u32_t r,c;
	if(!dest || !m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matScale(%X,%X,%g): incorrect parameter(s).\n",
			dest,m,x);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->rows!=m->rows || dest->cols!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matScale(%X,%X,%g): matrices with different sizes.\n",
			dest,m,x);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(r=0;r<dest->rows;r++)
		for(c=0;c<dest->cols;c++)
			dest->cell[r][c]=m->cell[r][c]*x;
}

void	V_GLOBAL
matVecMul(vector_t *dest,matrix_t *m,vector_t *v)
{
	u32_t i,j;
	if(!dest || !m || !v)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matVecMul(%X,%X,%X): incorrect parameter(s).\n",
			dest,m,v);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->size!=m->rows || m->cols!=v->size)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matVecMul(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<m->rows;i++)
	{
		dest->elements[i]=0.0;
		for(j=0;j<m->cols;j++)
			dest->elements[i]+=v->elements[j] * m->cell[i][j];
	}
}

void	V_GLOBAL
matMul(matrix_t *dest,matrix_t *ml,matrix_t *mr)
{
	u32_t M,N,P;
	u32_t i,j,k;
	vec_t s;
	if(!dest || !ml || !mr)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matMul(%X,%X,%X): incorrect parameter(s).\n",
			dest,ml,mr);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(ml->cols!=mr->rows || dest->rows!=ml->rows || dest->cols!=mr->cols)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matMul(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	M=ml->rows;
	N=ml->cols;
	P=mr->cols;
	for(i=0;i<M;i++)
	{
		for(j=0;j<P;j++)
		{
			s=0;
			for(k=0;k<N;k++)
				s+=ml->cell[i][k]*mr->cell[k][j];
			dest->cell[i][j]=s;
		}
	}
}

void	V_GLOBAL
matTranspose(matrix_t *dest,matrix_t *m)
{
	u32_t r,c;
	if(!dest || !m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matTranspose(%X,%X): incorrect parameter(s).\n",
			dest,m);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->rows!=m->cols || dest->cols!=m->rows)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matTranspose(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	for(r=0;r<dest->rows;r++)
		for(c=0;c<dest->cols;c++)
			dest->cell[r][c]=m->cell[c][r];
}

void	V_GLOBAL
matTransposeIP(matrix_p m)
{
	vec_t temp;
	u32_t r, c, rlimit, climit;
	if(m->rows != m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matTransposeIP(%ux%u): "
			"must be square matrix.\n",m->rows,m->cols);
#endif	/* CHECKED_BUILD */
		return;
	}
	/* go through upper triangle */
	rlimit = m->rows-1;
	climit = m->cols;
	for(r = 0;r < rlimit;r++)
		for(c = r+1;c < climit;c++)
		{
			temp = m->cell[r][c];
			m->cell[r][c] = m->cell[c][r];
			m->cell[c][r] = temp;
		}
}

V_LOCAL vboolean
mat_nonempty_rect(u32_t left,u32_t top,u32_t *right,u32_t *bottom,
	u32_t max_right,u32_t max_bottom)
{
	if(*right > max_right)
		*right = max_right;
	if(*bottom > max_bottom)
		*bottom = max_bottom;
	if(left > *right)
		return false;
	if(top > *bottom)
		return false;
	return true;
}

V_LOCAL void
mat_copy_elements_pwr2(matrix_p dest,matrix_p source,
	s32_t dest_col,s32_t dest_row,
	u32_t src_left,u32_t src_top,u32_t src_right,u32_t src_bottom)
{
	u32_t sr, sc, dc;
	u32_t dest_row_mask = dest->rows - 1;
	u32_t dest_col_mask = dest->cols - 1;
	vec_p dst, src;
	for(sr = src_top;sr <= src_bottom;sr++)
	{
		dc = dest_col;
		dst = dest->cell[dest_row & dest_row_mask];
		src = source->cell[sr];
		for(sc = src_left;sc <= src_right;sc++)
			dst[dc++ & dest_col_mask] = src[sc];
		dest_row++;
	}
}

V_LOCAL	void
mat_copy_elements_generic(matrix_p dest,matrix_p source,
	s32_t dest_col,s32_t dest_row,
	u32_t src_left,u32_t src_top,u32_t src_right,u32_t src_bottom)
{
	u32_t sr, sc;
	u32_t dc;
	u32_t dest_rows = dest->rows;
	u32_t dest_cols = dest->cols;
	vec_p dst, src;
	if(dest_col < 0)
		dest_col += ((-dest_col % dest->cols) + 1) * dest->cols;
	if(dest_row < 0)
		dest_row += ((-dest_row % dest->rows) + 1) * dest->rows;
	dc = dest_col;
	for(sr = src_top;sr <= src_bottom;sr++)
	{
		dc = dest_col;
		dst = dest->cell[dest_row % dest_rows];
		src = source->cell[sr];
		for(sc = src_left;sc <= src_right;sc++)
			dst[dc++ % dest_cols] = src[sc];
		dest_row++;
	}
}

void	V_GLOBAL
matCopyElements(matrix_p dest,matrix_p source,
	s32_t dest_col,s32_t dest_row,
	u32_t src_left,u32_t src_top,u32_t src_right,u32_t src_bottom)
{
	CALLER_NAME(matCopyElements)
	if(!dest || !source || !dest->cols || !dest->rows ||
		!source->cols || !source->rows)
	{
		ERROR_GENERIC(error_incorrect_parameters);
		return;
	}
	if(!mat_nonempty_rect(src_left,src_top,&src_right,&src_bottom,
			source->cols-1,source->rows-1))
		return;
	if(IS_POWER_TWO(dest->cols) && IS_POWER_TWO(dest->rows))
		mat_copy_elements_pwr2(dest,source,dest_col,dest_row,
			src_left,src_top,src_right,src_bottom);
	else
		mat_copy_elements_generic(dest,source,dest_col,dest_row,
			src_left,src_top,src_right,src_bottom);
}

vec_t	V_GLOBAL
matDeterm(matrix_t *m,vboolean keepon)
{
	u32_t i,j,k,ind,N;
	vec_t lead, elem, max, res;
	vec_t *row;
	vec_t **Rows;
	matrix_t *m1;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matDeterm(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return 0;
	}
	if(m->rows!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matDeterm(): operand must be square matrix.\n");
#endif	/* CHECKED_BUILD */
		return 0;
	}
	if(keepon)
		m1=matDuplicate(m);
	else
		m1=m;
	Rows=m1->cell;
	N=m1->rows;
	res=1;
	for(k=0;k<N;k++)
	{
		/* find pivot element under diagonal in column #k */
		ind=k;
		max = v_abs(Rows[k][k]);
		for(i=k+1;i<N;i++)
		{
			elem = v_abs(Rows[i][k]);
			if(max<elem)
			{
				max=elem;
				ind=i;
			}
		}
		if(IsZero(max))
		{
#ifdef		CHECKED_BUILD
			conlPrintf(LOG_ERROR,"matDeterm(%d by %d): (near) singular matrix, "
				"couldn't find pivot for column %d.\n",m->rows,m->rows,k);
#endif		/* CHECKED_BUILD */
			memFree(m1);
			return (vec_t)0;
		}
		/* swap rows */
		if(k!=ind)
		{
			row=Rows[ind];
			Rows[ind]=Rows[k];
			Rows[k]=row;
			res=-res;
		}
		/* next iteration of Haussian elimination method */
		lead=Rows[k][k];
		res*=lead;
		for(j=k+1;j<N;j++)
			Rows[k][j]/=lead;
		for(i=k+1;i<N;i++)
			for(j=k+1;j<N;j++)
				Rows[i][j]-=Rows[i][k]*Rows[k][j];
	}
	memFree(m1);
	return res;
}

vector_p	V_GLOBAL
matSolve(matrix_t *a,vector_t *_b,vboolean keepon)
{
	u32_t i,j,k,ind, N;
	vec_t elem,max,b;
	vec_t *row, *B;
	vec_t **Rows;
	matrix_p a1;
	vector_p b1;
	if(!a || !_b)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matSolve(%X,%X,%d): incorrect parameter(s).\n",
			a,_b,keepon);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(a->rows!=a->cols || a->rows!=_b->size)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matSolve(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(keepon)
	{
		a1=matDuplicate(a);
		b1=vecDuplicate(_b);
	}
	else
	{
		a1=a;
		b1=_b;
	}
	Rows=a1->cell;
	B=b1->elements;
	N=a1->rows;
	for(k=0;k<N;k++)
	{
		ind=k;
		max = v_abs(Rows[ind][k]);
		for(i=ind+1;i<N;i++)
		{
			elem = v_abs(Rows[i][k]);
			if(elem>max)
			{
				ind=i;
				max=elem;
			}
		}
		if(IsZero(max))
		{
#ifdef		CHECKED_BUILD
			conlPrintf(LOG_ERROR,"matSolve(%d by %d): singular matrix, "
				"couldn't find pivot for column %d.\n",a->rows,a->rows,k);
#endif		/* CHECKED_BUILD */
			memFree(a1);
			memFree(b1);
			return NULL;
		}
		if(ind!=k)
		{
			row=Rows[ind];
			Rows[ind]=Rows[k];
			Rows[k]=row;
			b=B[ind];
			B[ind]=B[k];
			B[k]=b;
		}
		elem = Rows[k][k];
		for(j=k+1;j<N;j++)
			Rows[k][j]/=elem;
		B[k]/=elem;
		for(i=0;i<k;i++)
		{
			for(j=k+1;j<N;j++)
				Rows[i][j]-=Rows[i][k]*Rows[k][j];
			B[i]-=Rows[i][k]*B[k];
		}
		for(i=k+1;i<N;i++)
		{
			for(j=k+1;j<N;j++)
				Rows[i][j]-=Rows[i][k]*Rows[k][j];
			B[i]-=Rows[i][k]*B[k];
		}
	}
	memFree(a1);
	return b1;
}

matrix_p V_GLOBAL
matInverse(matrix_t *m,vboolean keepon)
{
	u32_t i,j,k,ind, N;
	vec_t elem,max;
	vec_t *row;
	vec_t **RowsS, **RowsD;
	matrix_t *temp;
	matrix_t *dupe;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matInverse(NULL,%d): incorrect parameter.\n",
			keepon);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(m->rows!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matInverse(): operand must be square matrix.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	temp = matAlloc(m->rows,m->cols);
	if(!temp)
		return NULL;
	matUnit(temp);
	if(keepon)
	{
		dupe = matDuplicate(m);
		if(!dupe)
		{
			memFree(temp);
			return NULL;
		}
	}
	else
		dupe = m;
	RowsS = dupe->cell;
	RowsD = temp->cell;
	N = dupe->rows;
	for(k=0;k<N;k++)
	{
		ind=k;
		max = v_abs(RowsS[ind][k]);
		for(i=ind+1;i<N;i++)
		{
			elem = v_abs(RowsS[i][k]);
			if(elem>max)
			{
				ind=i;
				max=elem;
			}
		}
		if(IsZero(max))
		{
#ifdef		CHECKED_BUILD
			conlPrintf(LOG_ERROR,"matInverse(%d by %d): (near) singular matrix, "
				"couldn't find pivot for column %d.\n",m->rows,m->rows,k);
#endif		/* CHECKED_BUILD */
			memFree(dupe);
			memFree(temp);
			return NULL;
		}
		if(ind!=k)
		{
			row=RowsS[ind];
			RowsS[ind]=RowsS[k];
			RowsS[k]=row;
			row=RowsD[ind];
			RowsD[ind]=RowsD[k];
			RowsD[k]=row;
		}
		elem = RowsS[k][k];
		for(j=k+1;j<N;j++)
			RowsS[k][j]/=elem;
		for(j=0;j<N;j++)
			RowsD[k][j]/=elem;
		for(i=0;i<k;i++)
		{
			for(j=k+1;j<N;j++)
				RowsS[i][j]-=RowsS[i][k]*RowsS[k][j];
			for(j=0;j<N;j++)
				RowsD[i][j]-=RowsS[i][k]*RowsD[k][j];
		}
		for(i=k+1;i<N;i++)
		{
			for(j=k+1;j<N;j++)
				RowsS[i][j]-=RowsS[i][k]*RowsS[k][j];
			for(j=0;j<N;j++)
				RowsD[i][j]-=RowsS[i][k]*RowsD[k][j];
		}
	}
	memFree(dupe);
	dupe = matDuplicate(temp);
	memFree(temp);
	return dupe;
}

#ifdef	MATRIX_NO_MACRO

void	V_GLOBAL
matZero(matrix_t *m)
{
	V_ASSERT(m);
	memZero32(m->data,m->rows*m->cols*(sizeof(vec_t)>>2));
}  

u32_t		V_GLOBAL
matRowCount(matrix_t *m)
{
	V_ASSERT(m);
	return m->rows;
}

u32_t		V_GLOBAL
matColCount(matrix_t *m)
{
	V_ASSERT(m);
	return m->cols;
}

#endif	/* MATRIX_NO_MACRO */
