matrix.c

snake算法

/****************************************************************************
 File Name  : matrix.c
 Purpose    : provides advanced matrix manipulation routines for IMAGE object
 Release    : Version 1.0
 Date	    : Aug 31,1995

GSNAKE API is jointly developed by the Information Technology Institute (ITI), Singapore, and the School of Applied Science, Nanyang Technological
University (NTU), Singapore. 

These software programs are available to the user without any license or royalty fees. Permission is hereby granted to use, copy, modify, and distribute this software and its documentation for any purpose. ITI and NTU gives no warranty, express, implied, or statuary for the software and/or documentation provided, including, without limitation, waranty of merchantibility and warranty of fitness for a particular purpose. The software provided hereunder is on an "as is"  basis, and ITI and NTU has no obligation to provide maintenance, support, updates, enhancements, or modifications.

GSNAKE API is available for any UNIX compatible system. User feedback, bugs, or software and manual suggestions should be sent via electronic mail to one of the following, who may or may not act on them as he/she desires :

		[e-mail]
		[e-mail]
***************************************************************************/

#include "gsnake.h"

void MATRIX::dump(char *header)
{
	if(header) printf("\n<%s>\n", header) ;
	IMAGE::print() ;
}

MATRIX::init(short _row, short _col, char identity)
{
	reset() ;
	if( !(data = (float *) _iMemAlloc(_row*_col*sizeof(float))))
		return MEMORYERROR ;
	
	row = _row ;
	col = _col ;
	memset(data, 0, row*col*sizeof(float));
	
	if(identity) {
	    	register short i;
	    	for(i=0; i<row; i++)
	    		put(i, i, 1);
	}
	return NOERROR ;
}


MATRIX * MATRIX::transpose(void)
{
	MATRIX *out = new MATRIX ;
	
	if( out->init(col, row) != NOERROR )
		return NULL ;

	register short i, j ;
	for(i=0; i<row; i++)
	   for(j=0; j<col; j++)
	   	out->put(j, i, get(i, j)) ;
	return out ;
}


MATRIX * MATRIX::operator+ (MATRIX& mtx)
{
	MATRIX *out = new MATRIX;

	if(mtx.getRow() != row || mtx.getCol() != col)
		return NULL ;
	
	if( out->init(row, col) != NOERROR ) 
		return NULL;

	register short i, j ;
	for(i=0; i<row; i++)
	   for(j=0; j<col; j++)
	   	out->put(i, j, get(i, j) + mtx.get(i, j) ) ;
	return out ;
}

MATRIX * MATRIX::operator- (MATRIX& mtx)
{
	MATRIX *out = new MATRIX;

	if(mtx.getRow() != row || mtx.getCol() != col)
		return NULL ;
	
	if( out->init(row, col) != NOERROR ) 
		return NULL ;
	
	register short i, j ;
	for(i=0; i<row; i++)
	   for(j=0; j<col; j++)
	   	out->put(i, j, get(i, j) - mtx.get(i, j) ) ;
	return out ;
}

MATRIX * MATRIX::operator* (MATRIX& mtx)
{
	MATRIX *out = new MATRIX ;

	if(col != mtx.getRow() )
		return NULL ;
	
	if( out->init(row, mtx.getCol()) != NOERROR )
		return NULL ;
	
	register short i, j, k ;
	for(i=0; i<row; i++)
	   for(j=0; j<out->getCol(); j++) {
	   	float sum = 0 ;
	   	for(k=0; k<col; k++)
	   		sum += get(i, k) * mtx.get(k, j) ;
	   	out->put(i, j, sum) ;
	   }
	return out ;
	
}

MATRIX::swapRow(short i, short j) 
{
	if( i != CAP(0, i, row-1) || j != CAP(0, j, row-1) )
		return PARAMERROR ;
	
	register short k  ;
	float temp ;
	for(k=0; k<col; k++) {
		temp = get(i, k) ;
		put(i, k, get(j, k) ) ;
		put(j, k, temp) ;
	}
	return NOERROR ;
}

MATRIX * MATRIX::inverse (void)
{
	MATRIX *out = new MATRIX ;
	MATRIX *org = new MATRIX ;
	register short i, j, k;
	
	if( !row || row != col) {
	  fprintf(stderr, "ERROR <MATRIX::inverse()> : Not a Square Matrix\n");
	  return NULL ;
	}
		
	if( out->init(row, col, 1) != NOERROR ||
	    org->init(row, col) != NOERROR ) {
	  fprintf(stderr, "ERROR <MATRIX::inverse()> : Memory Error\n");
	  return NULL ;
	}
	
	for(i=0; i<row; i++)	/* duplicate original matrix */
		for(j=0; j<col; j++)
			org->put(i, j, get(i, j) );
					
	/* start from first column to the next */
	int pivotal ;
	for(i=0; i<row; i++) {
	    float div = org->get(i, i) ;
	    if( fabs(div) < 1e-10) { /* pivotal element too small */
	       for(j=i+1; j<row; j++) 
	           if( fabs(div = org->get(j, i)) > 1e-10 ) {
	           	org->swapRow(i, j) ;
	           	out->swapRow(i, j) ;
	           	break ;
	           }
	       if( j == row) {
	             fprintf(stderr, 
	         	"ERROR <MATRIX::inverse()> : Singular Matrix\n");
	       	     return NULL ;
	       }
	    }

	    for(j=0; j<col; j++) {  /* divide entire row by pivotal element */
	        org->put(i, j, org->get(i, j)/div) ;
	        out->put(i, j, out->get(i, j)/div) ;
	    }
	    
	    for(k=0; k<row; k++) { /* row operation now */
	        if( k == i ) continue ;
	        
	        float mult = org->get(k, i) ;
	        if( fabs(mult) < 1e-10) continue ;
	        for(j=0; j<col; j++) {
	        	org->put(k, j, org->get(k, j) - mult*org->get(i, j)) ;
	        	out->put(k, j, out->get(k, j) - mult*out->get(i, j));
	        }
	    }
	}
	
	delete org ;
	
	return out ;
}