matrix_tool.cpp

扩展的直序扩频系统

// This file contains some useful calculate function for matrix.

// Programmer: Unknown some senior students.
#include "Common.h"

//the Multiply algorithm of two Matrix
void ComMatrix_Mul(IN complex *p1,IN complex *p2,IN int M, IN int N, IN int K, OUT complex *p3)
// p1 is a M X N input matrix;
// p2 is a N X K input matrix;
// p3 is a output matrix which equals p1*p2;
{
	int i,j,l,u,v,w;
    float p,q,s;
	float *ar,*ai,*br,*bi,*cr,*ci;
	
	ar=(float *) calloc(M*N,sizeof(float));
	ai=(float *) calloc(M*N,sizeof(float));
	br=(float *) calloc(N*K,sizeof(float));
	bi=(float *) calloc(N*K,sizeof(float));
	cr=(float *) calloc(M*K,sizeof(float));
	ci=(float *) calloc(M*K,sizeof(float));
	
	for (i=0;i<M*N;i++)
	{
		ar[i]=p1[i].real;
		ai[i]=p1[i].imag;
	}
	
	for (i=0;i<N*K;i++)
	{
		br[i]=p2[i].real;
		bi[i]=p2[i].imag;
	}
	
    for (i=0; i<=M-1; i++)
		for (j=0; j<=K-1; j++)
		{ u=i*K+j;
          *(cr+u)=0.0; *(ci+u)=0.0;
          for (l=0; l<=N-1; l++)
		  { v=i*N+l; w=l*K+j;
	    	p=(*(ar+v))*(*(br+w));
	    	q=(*(ai+v))*(*(bi+w));
		    s=((*(ar+v))+(*(ai+v)))*((*(br+w))+(*(bi+w)));
    		*(cr+u)=*(cr+u)+p-q;
    		*(ci+u)=*(ci+u)+s-p-q;
		
    		(*(p3+i*K+j)).real=*(cr+u);
    		(*(p3+i*K+j)).imag=*(ci+u);
		  }
		}
		
		free(ar);
		free(ai);
		free(br);
		free(bi);
		free(cr);
		free(ci);
		
		return;
}


//calculate the inverse of Matrix 
void ComMatrix_Inv( IN complex *p1, IN int N, OUT complex *p2)
{
// p1 is a N X N input matrix;
// p2 is the N X N output matrix which equals inverse of p1;

	complex *p33=p1;
	float *ar,*ai;
	int *is,*js,i,j,k,l,u,v,w;
	float p,q,s,t,d,b;

	ar=(float *)calloc(N*N,sizeof(float));
	ai=(float *)calloc(N*N,sizeof(float));
	
	for (i=0;i<N*N;i++)
	{
		*(ar+i)=(*(p33+i)).real;
		*(ai+i)=(*(p33+i)).imag;
	}
 
    is=(int *)malloc(N*sizeof(int));
    js=(int *)malloc(N*sizeof(int));
    for (k=0; k<=N-1; k++)
	{ 
		d=0.0;
        for (i=k; i<=N-1; i++)
        for (j=k; j<=N-1; j++)
		{ 
			u=i*N+j;
            p=(*(ar+u))*(*(ar+u))+(*(ai+u))*(*(ai+u));
            if (p>d) 
			{ 
				d=p; 
				is[k]=i; 
				js[k]=j;
			}
		}
        if (d+1.0==1.0)
          { 
			free(is); free(js); 
			printf("err**not inv\n");	
			return ;
		  }
        if (is[k]!=k)
          for (j=0; j<=N-1; j++)
            { u=k*N+j; v=is[k]*N+j;
              t=*(ar+u); *(ar+u)=*(ar+v); *(ar+v)=t;
              t=*(ai+u); *(ai+u)=*(ai+v); *(ai+v)=t;
            }
        if (js[k]!=k)
          for (i=0; i<=N-1; i++)
            { u=i*N+k; v=i*N+js[k];
              t=*(ar+u); *(ar+u)=*(ar+v); *(ar+v)=t;
              t=*(ai+u); *(ai+u)=*(ai+v); *(ai+v)=t;
            }
        l=k*N+k;
        *(ar+l)=*(ar+l)/d; *(ai+l)=-*(ai+l)/d;
        for (j=0; j<=N-1; j++)
          if (j!=k)
            { u=k*N+j;
              p=(*(ar+u))*(*(ar+l)); q=(*(ai+u))*(*(ai+l));
              s=(*(ar+u)+(*(ai+u)))*(*(ar+l)+(*(ai+l)));
              *(ar+u)=p-q; *(ai+u)=s-p-q;
            }
        for (i=0; i<=N-1; i++)
          if (i!=k)
            { v=i*N+k;
              for (j=0; j<=N-1; j++)
                if (j!=k)
                  { u=k*N+j;  w=i*N+j;
                    p=(*(ar+u))*(*(ar+v)); q=(*(ai+u))*(*(ai+v));
                    s=(*(ar+u)+(*(ai+u)))*(*(ar+v)+(*(ai+v)));
                    t=p-q; b=s-p-q;
                    *(ar+w)=*(ar+w)-t;
                    *(ai+w)=*(ai+w)-b;
                  }
            }
        for (i=0; i<=N-1; i++)
          if (i!=k)
            { u=i*N+k;
              p=(*(ar+u))*(*(ar+l)); q=(*(ai+u))*(*(ai+l));
              s=(*(ar+u)+(*(ai+u)))*(*(ar+l)+(*(ai+l)));
              *(ar+u)=q-p; *(ai+u)=p+q-s;
            }
      }
    for (k=N-1; k>=0; k--)
      { if (js[k]!=k)
          for (j=0; j<=N-1; j++)
            { u=k*N+j; v=js[k]*N+j;
              t=*(ar+u); *(ar+u)=*(ar+v); *(ar+v)=t;
              t=*(ai+u); *(ai+u)=*(ai+v); *(ai+v)=t;
            }


        if (is[k]!=k)
          for (i=0; i<=N-1; i++)
            { u=i*N+k; v=i*N+is[k];
              t=*(ar+u); *(ar+u)=*(ar+v); *(ar+v)=t;
              t=*(ai+u); *(ai+u)=*(ai+v); *(ai+v)=t;
            }
      }

	
	for (i=0;i<N*N;i++)
	{
		(*(p2+i)).real=*(ar+i);
		(*(p2+i)).imag=*(ai+i);
	}

    free(is); 
	free(js);
	free(ar);
	free(ai);
}

// calculate Matrix's Complex conjugate 
void ComMatrix_H(IN complex *p1, IN int M, IN int N, OUT complex *p2)
{
// p1 is a M X N input matrix;
// p2 is a N X M output matrix which equals p1(H);
	int i,j;
	
	for (i=0;i<M;i++)
		for (j=0;j<N;j++)
		{
			(*(p2+j*M+i)).real=(*(p1+i*N+j)).real;
			(*(p2+j*M+i)).imag=-(*(p1+i*N+j)).imag;
		}
}

void ComMatrix_T(IN complex *p1, IN int M, IN int N, OUT complex *p2)
{
	// p1 is a M X N input matrix;
	// p2 is a N X M output matrix which equals p1(H);
	int i,j;
	
	for (i=0;i<M;i++)
		for (j=0;j<N;j++)
		{
			(*(p2+j*M+i)).real=(*(p1+i*N+j)).real;
			(*(p2+j*M+i)).imag=(*(p1+i*N+j)).imag;
		}
}

//calculate R Matrix Multiply
void R_Matrix_Mul(IN float *p1, IN float *p2,
				   IN int M, IN int N, IN int K,
				   OUT float *p3)
// p1 is a M X N input matrix;
// p2 is a N X K input matrix;
// p3 is a output matrix which equals p1*p2;
{
	int i,j,l,u,v,w;
    float p,q,s;
	float *ar,*ai,*br,*bi,*cr,*ci;
	
	ar=(float *) calloc(M*N,sizeof(float));
	ai=(float *) calloc(M*N,sizeof(float));
	br=(float *) calloc(N*K,sizeof(float));
	bi=(float *) calloc(N*K,sizeof(float));
	cr=(float *) calloc(M*K,sizeof(float));
	ci=(float *) calloc(M*K,sizeof(float));
	
	for (i=0;i<M*N;i++)
	{
		ar[i]=p1[i];
		ai[i]=0.f;
	}
	
	for (i=0;i<N*K;i++)
	{
		br[i]=p2[i];
		bi[i]=0.f;
	}
	
    for (i=0; i<=M-1; i++)
		for (j=0; j<=K-1; j++)
		{ u=i*K+j;
        *(cr+u)=0.0; *(ci+u)=0.0;
        for (l=0; l<=N-1; l++)
		{ v=i*N+l; w=l*K+j;
		p=(*(ar+v))*(*(br+w));
		q=(*(ai+v))*(*(bi+w));
		s=((*(ar+v))+(*(ai+v)))*((*(br+w))+(*(bi+w)));
		*(cr+u)=*(cr+u)+p-q;
		*(ci+u)=*(ci+u)+s-p-q;
		
		(*(p3+i*K+j))=*(cr+u);
//		(*(p3+i*K+j)).im=*(ci+u);
		}
		}
		
		free(ar);
		free(ai);
		free(br);
		free(bi);
		free(cr);
		free(ci);
		
		return;
}