matrix.cpp

矩阵常见算法

#include "stdafx.h"
#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include <math.h>
#include "matrix.h"
void nrerror(char *error_text)
{
	fprintf(stderr,"Numerical Recipes run-time error...\n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"...now exiting to system...\n");
	exit(1);
}

double *dvector(int nl, int nh)
{
	double *v;

	v = (double *) calloc((unsigned) (nh-nl+1), sizeof(double));
	if (!v) nrerror("allocation failure in dvector()");
	return v-nl;
}

float *vector(int nl, int nh)
{
	float *v;

	v = (float *) calloc((unsigned) (nh-nl+1), sizeof(float));
	if (!v) nrerror("allocation failure in dvector()");
	return v-nl;
}

void free_dvector(double *v, int nl, int nh)
{
	free((char*) (v+nl));
}

void free_vector(float *v, int nl, int nh)
{
	free((char*) (v+nl));
}

float **matrix(int nrl,int nrh,int ncl,int nch)
{
	int i;
	float **m;

	m=(float **) malloc((unsigned) (nrh-nrl+1)*sizeof(float*));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m -= nrl;

	for(i=nrl;i<=nrh;i++) {
		m[i]=(float *) malloc((unsigned) (nch-ncl+1)*sizeof(float));
		if (!m[i]) nrerror("allocation failure 2 in matrix()");
		m[i] -= ncl;
	}
	return m;
}

void free_matrix(float **m,int nrl,int nrh,int ncl,int nch)
{
	int i;

	for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
	free((char*) (m+nrl));
}

double **dmatrix(int nrl, int nrh, int ncl, int nch)
{
	int i;
	double **m;

	m = (double **) calloc((unsigned) (nrh-nrl+1), sizeof(double*));
	if (!m) nrerror("allocation failure 1 in dmatrix()");
	m -= nrl;

	for(i=nrl;i<=nrh;i++) {
		m[i] = (double *) calloc((unsigned) (nch-ncl+1), sizeof(double));
		if (!m[i]) nrerror("allocation failure 2 in dmatrix()");
		m[i] -= ncl;
	}
	return m;
}

void free_dmatrix(double **m, int nrl, int nrh, int ncl, int nch)
{
	int i;

	for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
	free((char*) (m+nrl));
}


double log2(double a)
{
	return log10(a)/log10(2.0);
}

void sort(double *Y, int *I, double *A, int length)
{
	int i, j;
	double max, *tmp;

	tmp = (double *) calloc(length, sizeof(double));

	for (i=0;i<length;i++) 
		tmp[i] = A[i];

	max = tmp[0];
	for (i=1;i<length;i++) {
		if (tmp[i] > max) 
			max = tmp[i];
	}

	max = fabs(10*max);

	for (i=0;i<length;i++) {
		Y[i] = tmp[0];
		I[i] = 0;
		for (j=1;j<length;j++) {
			if (tmp[j] < Y[i]) {
				Y[i] = tmp[j];
				I[i] = j;
			}
		}

		tmp[I[i]] = max;
	}

	free(tmp);
}

void minimun(double *Y, int *I, double *A, int length)
{
	int i, index;
	double min;

	min = A[0];
	index = 0;
	for (i=1;i<length;i++) 
		if (A[i] < min) {
			min = A[i];
			index = i;
		}

	*Y = min;
	*I = index;
}

void Mat_Abs(Matrix *A)
{
	int h, w;

	for (h=0;h<A->height;h++)
		for (w=0;w<A->width;w++) {
			if (A->data[h][w] < 0)
			    A->data[h][w] = -1.0*(A->data[h][w]);
		}
}

void Mat_Mean(double *mean, Matrix *A)
{
	int h, w;
	double tmp;

	tmp = 0.0;
	for (h=0;h<A->height;h++) {
		for (w=0;w<A->width;w++) {
			tmp += A->data[h][w];
		}
	}

	*mean = tmp/(double) (A->height*A->width);
}

void Mat_Vector(Matrix *A, float *a)
{
	int h, w;

	for (h=0;h<A->height;h++)
		for (w=0;w<A->width;w++)
			a[h*A->width+w] = (float) A->data[h][w];
}

void Mat_Shift(Matrix *A, Matrix *B, int side)
{
	int h, w;

	for (h=side;h<B->height;h++)
		for (w=side;w<B->width;w++)
			A->data[h-side][w-side] = B->data[h][w];
		
	for (h=side;h<B->height;h++)
		for (w=0;w<side;w++)
			A->data[h-side][B->width-side+w] = B->data[h][w];

	for (h=0;h<side;h++)
		for (w=side;w<B->width;w++)
			A->data[B->height-side+h][w-side] = B->data[h][w];

	for (h=0;h<side;h++)
		for (w=0;w<side;w++)
			A->data[B->height-side+h][B->width-side+w] = B->data[h][w];
}

void Mat_Zeros(Matrix *A)
{
	int h, w;

	for (h=0;h<A->height;h++)
		for (w=0;w<A->width;w++)
			A->data[h][w] = 0;
}

void Mat_Zeros_uc(uc_Matrix *A)
{
	int h, w;

	for (h=0;h<A->height;h++)
		for (w=0;w<A->width;w++)
			A->data[h][w] = 0;
}

void Mat_Zeros_i(i_Matrix *A)
{
	int h, w;

	for (h=0;h<A->height;h++)
		for (w=0;w<A->width;w++)
			A->data[h][w] = 0;
}


void CreateMatrix(Matrix **M, int hei, int wid)
{
	int h;

	Matrix *tmp;

	tmp = (Matrix *) calloc(1, sizeof(Matrix)); 
	tmp->data = (double **) calloc(hei, sizeof(double *));

	if (!(tmp->data)) {
		nrerror("allocation failure in CreateMatrix()");
		exit(1);
	}

	for (h=0; h<hei; h++) {
		tmp->data[h] = (double *) calloc(wid, sizeof(double));
		if (!(tmp->data[h])) {
			nrerror("allocation failure in CreateMatrix()");
			exit(1);
		}
	}

	tmp->height = hei;
	tmp->width = wid;
	
	*M = tmp;
}


void FreeMatrix(Matrix *M)
{
	int h, hei = M->height;

	for (h=0; h<hei; h++) {
	     free(M->data[h]);
	}
	free(M->data);
	free(M);
}

void Create_i_Matrix(i_Matrix **M, int hei, int wid)
{
	int h;

	i_Matrix *tmp;

	tmp = (i_Matrix *) calloc(1, sizeof(i_Matrix));
	tmp->data = (int **) calloc(hei, sizeof(int *));
	if (!(tmp->data)) {
		nrerror("allocation failure in Create_i_Matrix()");
		exit(1);
	}

	for (h=0; h<hei; h++) {
		tmp->data[h] = (int *) calloc(wid, sizeof(int));
		if (!(tmp->data[h])) {
			nrerror("allocation failure in Create_i_Matrix()");
			exit(1);
		}
	}

	tmp->height = hei;
	tmp->width = wid;
	
	*M = tmp;
}

void Free_i_Matrix(i_Matrix *M)
{
	int h;

	for (h=0; h<M->height; h++) 
	     free(M->data[h]);
	free(M->data);
	free(M);
}

void Create_uc_Matrix(uc_Matrix **M, int hei, int wid)
{
	int h;

	uc_Matrix *tmp;

	tmp = (uc_Matrix *) calloc(1, sizeof(uc_Matrix));
	tmp->data = (unsigned char **) calloc(hei, sizeof(unsigned char *));
	if (!(tmp->data)) {
		nrerror("allocation failure in Create_uc_Matrix()");
		exit(1);
	}

	for (h=0; h<hei; h++) {
		tmp->data[h] = (unsigned char *) calloc(wid, sizeof(unsigned char));
		if (!(tmp->data[h])) {
			nrerror("allocation failure in Create_uc_Matrix()");
			exit(1);
		}
	}

	tmp->height = hei;
	tmp->width = wid;
	
	*M = tmp;
}

void Free_uc_Matrix(uc_Matrix *M)
{
	int h;

	for (h=0; h<M->height; h++) 
	     free(M->data[h]);
	free(M->data);
	free(M);
}


void Mat_FFT2(Matrix *Output_real, Matrix *Output_imag, Matrix *Input_real, Matrix *Input_imag)
{
	int xs, ys, i, j;
	double **R, **I, **Fr, **Fi;

	xs = Input_real->height;
	ys = Input_real->width;

        R  = dmatrix(1,xs,1,ys);
        I  = dmatrix(1,xs,1,ys);
        Fr = dmatrix(1,xs,1,ys);
        Fi = dmatrix(1,xs,1,ys);
			
        for (i=1;i<=Input_real->height;i++) 
            for (j=1;j<=Input_real->width;j++) {
                R[i][j] = Input_real->data[i-1][j-1];
                I[i][j] = Input_imag->data[i-1][j-1];
            }

        four2(Fr, Fi, R, I, xs, ys, 1);         /* 2-D FFT */