www.pudn.com > faces.rar > matrix.c

/*** 
 **     libface - Library of face recognition and supporting algorithms 
        Copyright (c) 2003 Stefan Farthofer 
 
	This file is part of libface, which is 
 
        free software; you can redistribute it and/or modify 
        it under the terms of the GNU General Public License as published by 
        the Free Software Foundation; either version 2 of the License, or 
        (at your option) any later version. 
 
        This program is distributed in the hope that it will be useful, 
        but WITHOUT ANY WARRANTY; without even the implied warranty of 
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
        GNU General Public License for more details. 
 
        You should have received a copy of the GNU General Public License 
        along with this program; if not, write to the Free Software 
        Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
 
	For further information seek us at http://sourceforge.net/projects/openbio/ 
**	or write an email to dimitri.pissarenko@gmx.net or farthofer@chello.at. 
***/ 
 
/* we need error codes so include frbase.h */ 
#include  
#include  
#include  
#include "frbase.h" 
#include "matrix.h" 
 
void _matTransposeD(float* dst, float* src, unsigned int rowsSrc, unsigned int colsSrc) { 
	unsigned int i,j; 
 
	for (i=0; i < rowsSrc; i++) 
		for (j=0; j < colsSrc; j++) 
			dst[j*rowsSrc+i] = src[i*colsSrc+j]; 
} 
 
void _matTransposeQ(float* mat, unsigned int n) { 
	unsigned int i,j; 
	float tmp; 
 
	for(i=0; i < n; i++) {  
		for(j=i+1; j < n; j++) { 
			tmp = mat[i*n+j]; 
			mat[i*n+j] = mat[j*n+i]; 
			mat[j*n+i] = tmp; 
		} 
	} 
} 
 
/* computes m x n * n x p matrix multiply */ 
void _matMultiplyD(float* dst, float* a, float* b, unsigned int m, unsigned int n, unsigned int p) { 
	unsigned int i, j, k; 
 
	for (i=0; i < m; i++) { 
		for (j=0; j < p; j++) { 
			dst[i*p+j] = 0; 
			for (k=0; k < n; k++) dst[i*p+j] += a[i*n+k] * b[k*p+j]; 
		} 
	} 
} 
 
/* subtract same-sized matrizes */  
void _matSubtractD(float* dst, float* minuend, float* subtrahend, unsigned int m, unsigned int n) { 
	for(n *= m, m = 0; m < n; m++) 
		dst[m] = minuend[m] - subtrahend[m]; 
} 
 
/* add same-sized matrizes */  
void _matAdd(float* dst, float* b, unsigned int m, unsigned int n) { 
	for(n *= m, m = 0; m < n; m++) 
		dst[m] = dst[m] + b[m]; 
} 
 
 
/* computes eigenvalues and vectors of a symmetric matrix  
 * columns of matVectors contain the eigenvectors 
 * 
 * a - matrix 
 * n - dimension of matrix 
 * d - array for eigenvvalues 
 * v - array for eigenvectors (each vector occupies a column) 
 */ 
int _matEigenSD(float* a, int n, float* d, float* v) { 
  int j,iq,ip,ip_times_n,i ; 
  float tresh,theta,tau,t,sm,s,h,g,c,*b,*z; 
  int nrotint, *nrot = &nrotint; 
   
  b = (float *)malloc(n*sizeof(float)); 
  if (b == NULL) return FR_ERR_NOMEM; 
  z = (float *)malloc(n*sizeof(float)); 
  if (z == NULL) { free(b); return FR_ERR_NOMEM; } 
 
   
  for(ip_times_n=0, ip=0; ip 3 && g < EPS) 
		a[ip_times_n + iq] = 0.0 ; 
 
	      else if(fabs(a[ip_times_n+iq]) > tresh)  
		  { 
		  h=d[iq]-d[ip]; 
		  if(g < EPS) 
		    t = (fabs(a[ip_times_n+iq]) > EPS) ? (a[ip_times_n+iq])/h : 0.0f ;  
		  else 
		      {  
		      theta=(fabs(h) < EPS) ? 0.0f : 0.5f*h/(a[ip_times_n+iq]); 
		      t=1.0f/((float)fabs(theta)+(float)sqrt(1.0f+theta*theta)); 
		      if(theta < 0.0f) 
			t = -t ;  
		      }  
		  c=1.0f/(float)sqrt(1.0f+t*t); 
		  s=t*c; 
		  tau=s/(1.0f+c); 
		   
		  h=t*a[ip_times_n+iq]; 
		  z[ip] -= h; 
		  z[iq] += h; 
		  d[ip] -= h; 
		  d[iq] += h; 
		  a[ip_times_n+iq]=0.0; 
		   
		  for(j=0;j