server/algorithm/FittingPosition.cpp

#include "stdafx.h"
#include <math.h> 
#include "FittingPosition.h"

double Em[REF_POSITION_NUM][REF_POSITION_NUM];

double Fitting::sum(double * dNumarry,int n)  //计算方程组的增广矩阵时需要
{  
	double *dTemp= new double[n];  

	double dSum=0;  

	dTemp=dNumarry;  
	for (int i=0;i<n;i++)   
	{  
		dSum+=dTemp[i];  
	}  

	return dSum;  
}  

double Fitting::Sum(std::vector<double> vt_num,int n)
{
	double total = 0;  
	for (int i = 0; i < n; i++)  
	{  
		total+=vt_num[i];  
	}  
	return total;  
}

double Fitting::MutilSum(double* dX,double *dY,int n)  //计算方程组的增广矩阵时需要
{  
	double * dXTemp= new double [n];  
	double * dYTemp= new double [n];  
	double dMultiSum=0;  

	dXTemp=dX;  
	dYTemp=dY;  

	for (int i=0;i<n;i++)  
	{  
		dMultiSum += dX[i]*dY[i];  
	}  

	return dMultiSum;  

}  

double Fitting::MutilSum(std::vector<double> vx,std::vector<double> vy,int n)
{
	double dMultiSum=0;  
	for (int i = 0; i < n; i++)  
	{  
		dMultiSum+=vx[i]*vy[i];  
	}  
	return dMultiSum; 
}

double Fitting::RelatePow(double *dx,int n,int ex)  //计算方程组的增广矩阵时需要
{  
	double * dTemp =new double[n];  
	double ReSum=0;  
	dTemp=dx;  
	for (int j=0;j<n;j++)   
	{  
		ReSum+=pow(dTemp[j],ex);  
	}  
	return ReSum;  
}  

double Fitting::RelatePow(std::vector<double> vx, int n, int ex)
{
	double ReSum=0;  
	for (int i=0; i<n; i++)  
	{  
		ReSum+=pow(vx[i],ex);  
	}  
	return ReSum;
}

double Fitting::RelateMutiXY(double *dx,double*dy,int n,int ex)  //计算方程组的增广矩阵时需要
{  
	double * dXTemp= new double [n];  
	double * dYTemp= new double [n];  
	double dReMultiSum=0;  

	dXTemp=dx;  
	dYTemp=dy;  

	for (int i=0;i<n;i++)   
	{  
		dReMultiSum+=pow(dXTemp[i],ex)*dYTemp[i];  
	}  
	return dReMultiSum;  
}  

double Fitting::RelateMutiXY(std::vector<double> vx, std::vector<double> vy, int n, int ex)
{
	double dReMultiSum=0;  
	for (int i = 0; i < n; i++)  
	{  
		dReMultiSum+=pow(vx[i],ex)*vy[i];  
	}  
	return dReMultiSum; 
}

//计算方程组的增广矩阵  
void Fitting::EMatrix(double *dx,double*dy,int n,int ex,double coefficient[] )  
{  
	double * dXTemp= new double [n];  
	double * dYTemp= new double [n];  
	dXTemp=dx;  
	dYTemp=dy;  

	for(int i=1;i<=ex;i++)  
	{  

		for(int j=1;j<=ex;j++)  
		{  
			Em[i][j]=RelatePow(dXTemp,n,i+j-2);  
		}  
		Em[i][ex+1]=RelateMutiXY(dXTemp,dYTemp,n,i-1);  
	}  
	Em[1][1]=n;  
	CalEquation(ex,coefficient);  

}  

void Fitting::EMatrix(std::vector<double> vx, std::vector<double> vy, int n, int ex, double coefficient[])
{
	for (int i=1; i<=ex; i++)  
	{  
		for (int j=1; j<=ex; j++)  
		{  
			Em[i][j]=RelatePow(vx,n,i+j-2);  
		}  
		Em[i][ex+1]=RelateMutiXY(vx,vy,n,i-1);  
	}  
	Em[1][1]=n;  
	CalEquation(ex,coefficient);  
}

void Fitting::CalEquation(int exp,double coefficient[] )   //求解方程，计算方程组的增广矩阵时需要
{  
	for(int k=1;k<exp;k++) //消元过程  
	{  
		for(int i=k+1;i<exp+1;i++)  
		{  
			double p1=0;  

			if(Em[k][k]!=0)  
				p1=Em[i][k]/Em[k][k];  

			for(int j=k;j<exp+2;j++)   
				Em[i][j]=Em[i][j]-Em[k][j]*p1;  
		}  
	}  

	coefficient[exp]=Em[exp][exp+1]/Em[exp][exp];  
	for(int l=exp-1;l>=1;l--)   //回代求解  
		coefficient[l]=(Em[l][exp+1]-F(coefficient,l+1,exp))/Em[l][l];  
}  

double Fitting::F(double c[],int l,int m)//供CalEquation函数调用  
{  
	double sum=0;  
	for(int i=l;i<=m;i++)  
		sum+=Em[l-1][i]*c[i];  
	return sum;   
} 

double Fitting::F(std::vector<double> vc,int l,int m)
{
	double sum = 0;  
	for(int i = l;i <= m;i++)  
		sum+=Em[l-1][i]*vc[i];

	return sum; 

}

double Fitting::polyval(double* a,double x){ //根据ct计算位置
	int i;
	double y = 0.0;
	for(i=1;i<4;i++){
		y+=a[i]*pow(x,i-1);
	}
	return y;
}

double Fitting::polyval(std::vector<double> vt,double v)
{
	double y = 0.0;
	for(int i = 1;i < 4;i++){
		y+=vt[i]*pow(v,i-1);
	}

	return y;
}

double Fitting::VAR(double v[REF_POSITION_NUM]){  //求方差
	double sum=0,s=0,e;
	int i;
	for(i=0;i<REF_POSITION_NUM;i++)  
		sum+=v[i];
	e=sum/REF_POSITION_NUM;    
	for(i=0;i<REF_POSITION_NUM;i++){ 
		if(i!=REF_POSITION_NUM-1)
			s+=(v[i]-e)*(v[i]-e);
	}
	s=s/REF_POSITION_NUM;
	return s;
}

double Fitting::VAR(std::vector<double> vt)
{
	double sum = 0,s = 0,e;
	int n = REF_POSITION_NUM;
	int i;
	for(i = 0;i < n;i++)  
		sum+=vt[i];
	e = sum/n;    
	for(i = 0;i < n;i++){ 
		if(i!=n-1)
			s+=(vt[i]-e)*(vt[i]-e);
	}
	s = s/n;

	return s;
}