#include "stdafx.h"
#include <malloc.h>
#include <math.h>
#include "interfunc.h"
/**********************************************************************************/
//内部函数
//
//
/**********************************************************************************/
// 求协方差矩阵M(1)的逆
void M1_1(float **m,int L,float **returnM)
{
//根据X(1)向量的长度L,可以确定M(1),求得R(1)
int i,j,k,n;
long *r,*c;
float *temp,Pivot;
bool NoCompare;
int row;
row=L;
r=(long *)calloc(row,sizeof(long));
c=(long *)calloc(row,sizeof(long));
temp=(float *)calloc(row,sizeof(float));
for(i=0;i<row;i++)
{
c[i]=0;
r[i]=0;
for(j=0;j<row;j++)
{
returnM[i][j]=0;
}
}
for(i=0;i<row;i++)
returnM[i][i]=1;
for(i=0;i<row;i++)
{
Pivot=0;
NoCompare=false;
for(j=0;j<row;j++)
{
for(n=0;n<i;n++)
{
NoCompare=(j==c[n]);
if(NoCompare)
break;
}
if(NoCompare==false)
{
for(k=0;k<row;k++)
{
for(n=1;n<i;n++)
{
NoCompare=(k==r[n]);
if(NoCompare)
break;
}
if(NoCompare==false)
{
if(fabs(m[k][j])>=fabs(Pivot))
{
Pivot=fabs(m[k][j]);
r[i]=k;
c[i]=j;
}
}
}
}
}
Pivot=m[r[i]][c[i]];
if((Pivot!=1)&&(Pivot!=0))
{
for(k=0;k<row;k++)
{
m[r[i]][k]=m[r[i]][k]/Pivot;
returnM[r[i]][k]=returnM[r[i]][k]/Pivot;
}
for(j=0;j<row;j++)
{
Pivot=m[j][c[i]];
if((j!=r[i])&&(Pivot!=0))
{
for(k=0;k<row;k++)
{
m[j][k]=m[j][k]-m[r[i]][k]*Pivot;
returnM[j][k]=returnM[j][k]-returnM[r[i]][k]*Pivot;
}
}
}
}
}
for(i=0;i<row-1;i++)
{
if(r[i]!=c[i])
{
for(j=0;j<row;j++)
{
temp[j]=returnM[r[i]][j];
returnM[r[i]][j]=returnM[c[i]][j];
returnM[c[i]][j]=temp[j];
}
}
for(j=i+1;j<row;j++)
{
if(r[j]==c[i])
{
r[j]=r[i];
break;
}
r[i]=c[i];
}
}
free(r);
free(c);
free(temp);
}
/**********************************************************************************/
//填写向量X(i)的每个分量值(空间) 第i行 第j列 X向量 X的尺寸（一维）
void GetX_i(int **inImage,int iHei,int iWid,int i,int j,float *XVector,int L_X)
{
int ii,jj;
int m,n,p,temp_i,temp_j;
m=(int)(sqrt(L_X));//搜索框的边长
n=(m-1)/2;//搜索框的半边长
if((i>=n)&&(i<iHei-n)&&(j>=n)&&(j<iWid-n))
{
i=i-n;
j=j-n;
p=0;
for(ii=0;ii<m;ii++)
{
for(jj=0;jj<m;jj++)
{
XVector[p++]=inImage[i+ii][j+jj];
}
}
}
else
{
i=i-n;
j=j-n;
p=0;
for(ii=0;ii<m;ii++)
{
temp_i=i+ii;
if(i+ii<0)
temp_i=-1-i-ii;
else
if(i+ii>=iHei)
temp_i=iHei-(i+ii-iHei+1);
for(jj=0;jj<m;jj++)
{
temp_j=j+jj;
if(j+jj<0)
temp_j=-1-j-jj;
else
if(j+jj>iWid)
temp_j=iWid-(j+jj-iWid+1);
XVector[p++]=inImage[temp_i][temp_j];
}
}
}
}
/**********************************************************************************/
//填写向量X(i)的每个分量值（时间） 第i行 第j列 X向量 X的尺寸（一维）
void GetX_i_T(unsigned char ***inImage,int i,int j,float *XVector,int L_X)
{
int n;
for(n=0;n<L_X;n++)
{
XVector[n]=inImage[n][i][j];
}
}
void GetX_i_T2(float ***inImage,int i,int j,float *XVector,int L_X)
{
int n;
for(n=0;n<L_X;n++)
{
XVector[n]=inImage[n][i][j];
}
}
/**********************************************************************************/
//迭代
void Iterative9(float *X_1,int L_X,float **R_k,float **R_k1)
{
int i,j;
float temp;
float temp1[9],temp2[9],temp3[9][9];
for(i=0;i<L_X;i++)
{
temp1[i]=0;
for(j=0;j<L_X;j++)
{
temp1[i]+=R_k[i][j]*X_1[j];
}
}
for(i=0;i<L_X;i++)
{
temp2[i]=0;
for(j=0;j<L_X;j++)
{
temp2[i]+=X_1[j]*R_k[j][i];
}
}
for(i=0;i<L_X;i++)
{
for(j=0;j<L_X;j++)
{
temp3[i][j]=temp1[i]*temp2[j];
}
}
temp=1;
for(i=0;i<L_X;i++)
{
temp+=X_1[i]*temp2[i];
}
for(i=0;i<L_X;i++)
{
for(j=0;j<L_X;j++)
{
R_k1[i][j]=R_k[i][j]-temp3[i][j]/temp;
R_k[i][j]=R_k1[i][j];//-temp3[i][j]/temp;
}
}
}
/**********************************************************************************/
//迭代
void Iterative5(float *X_1,int L_X,float **R_k,float **R_k1)
{
int i,j;
float temp;
float temp1[5],temp2[5],temp3[5][5];
for(i=0;i<L_X;i++)
{
temp1[i]=0;
for(j=0;j<L_X;j++)
{
temp1[i]+=R_k[i][j]*X_1[j];
}
}
for(i=0;i<L_X;i++)
{
temp2[i]=0;
for(j=0;j<L_X;j++)
{
temp2[i]+=X_1[j]*R_k[j][i];
}
}
for(i=0;i<L_X;i++)
{
for(j=0;j<L_X;j++)
{
temp3[i][j]=temp1[i]*temp2[j];
}
}
temp=1;
for(i=0;i<L_X;i++)
{
temp+=X_1[i]*temp2[i];
}
for(i=0;i<L_X;i++)
{
for(j=0;j<L_X;j++)
{
R_k1[i][j]=R_k[i][j]-temp3[i][j]/temp;
R_k[i][j]=R_k1[i][j];
}
}
}
/**********************************************************************************/
//迭代
void Iterative7(float *X_1,int L_X,float **R_k,float **R_k1)
{
int i,j;
float temp;
float temp1[7],temp2[7],temp3[7][7];
for(i=0;i<L_X;i++)
{
temp1[i]=0;
for(j=0;j<L_X;j++)
{
temp1[i]+=R_k[i][j]*X_1[j];
}
}
for(i=0;i<L_X;i++)
{
temp2[i]=0;
for(j=0;j<L_X;j++)
{
temp2[i]+=X_1[j]*R_k[j][i];
}
}
for(i=0;i<L_X;i++)
{
for(j=0;j<L_X;j++)
{
temp3[i][j]=temp1[i]*temp2[j];
}
}
temp=1;
for(i=0;i<L_X;i++)
{
temp+=X_1[i]*temp2[i];
}
for(i=0;i<L_X;i++)
{
for(j=0;j<L_X;j++)
{
R_k1[i][j]=R_k[i][j]-temp3[i][j]/temp;
}
}
}
/**********************************************************************************/
//交换 remit R_K1->R_k
void remit(float **R_k1,float **R_k,int L_X)
{
int i,j;
for(i=0;i<L_X;i++)
{
for(j=0;j<L_X;j++)
{
R_k[i][j]=R_k1[i][j];
}
}
}
/**********************************************************************************/
void add_M(float **R_k1,float **R_k,int L_X)
{
int i,j;
for(i=0;i<L_X;i++)
for(j=0;j<L_X;j++)
R_k1[i][j]=R_k1[i][j]+R_k[i][j];
}
/**********************************************************************************/
void div_M(float **m,int div,int L_X)
{
int i,j;
for(i=0;i<L_X;i++)
for(j=0;j<L_X;j++)
m[i][j]=m[i][j]/div;
}
/**********************************************************************************/
void MMT(float *X_1,float **R_k,int L_X)
{
int i,j;
for(i=0;i<L_X;i++)
for(j=0;j<=i;j++)
R_k[i][j]=X_1[i]*X_1[j];
for(i=0;i<L_X;i++)
for(j=i+1;j<L_X;j++)
R_k[i][j]=R_k[j][i];
}
/**********************************************************************************/
//计算行列式
float hls33(float *W)
{
float value;
value=W[0]*W[4]*W[8]+W[1]*W[5]*W[6]+W[2]*W[3]*W[7]-W[2]*W[4]*W[6]-W[0]*W[5]*W[7]-W[1]*W[3]*W[8];
return(value);
}
