bsplineinterpolateobj.cpp

B样条插值

#include "stdafx.h"
#include "BSplineInterpolateObj.h"



CBSplineInterpolateObj::CBSplineInterpolateObj()
{
	m1 =0;
	n1= 0;
	PhiIJ = NULL;
	ScatteredPoint = NULL;
	ScatteredPointNumber = 0;
}
CBSplineInterpolateObj::~CBSplineInterpolateObj()
{
	m1 = 0;
	n1 = 0;
	if (PhiIJ) {
		delete []PhiIJ;
	}
	if (ScatteredPoint) {
		delete []ScatteredPoint;
	}
	ScatteredPointNumber = 0;
}
//=================控制点网格化函数,需要传入网格步长=============================//
//        入口：*ScatteredPnt(散乱点集数组),PntNum(散乱点集数组个数),Step(网格步长)
//        出口：M1(控制点网格φ:(m1+3)*(n1+3)),N1(控制点网格φ:(m1+3)*(n1+3)),
//        返回：1/0，成功/失败
//===============================================================================//
long CBSplineInterpolateObj::ControlPntTransform(G3D_Dot *ScatteredPnt,long PntNum,float XStep,float YStep, long &M1,long &N1)
{
	VECTOR_FLOAT ScateredPnt_X;
	VECTOR_FLOAT ScateredPnt_Y;
	int i =0;
	float temp;
	float StepX;
	float StepY;
	//存入向量，求出xy区域的最值
	for(i = 0;i<PntNum;i++)
	{
		temp = ScatteredPnt[i].x;
		ScateredPnt_X.push_back(temp);
		temp = ScatteredPnt[i].y;
		ScateredPnt_Y.push_back(temp);
	}
	sort(ScateredPnt_X.begin(),ScateredPnt_X.end());
	sort(ScateredPnt_Y.begin(),ScateredPnt_Y.end());
	
	StepX = (ScateredPnt_X[PntNum - 1])/(XStep);
	StepY = (ScateredPnt_Y[PntNum - 1])/(YStep);
	m1 = int(StepX) + 1;
	n1 = int(StepY) + 1;
	M1 = m1;
	N1 = n1;
	ScatteredPointNumber = PntNum;
	ScatteredPoint = new G3D_Dot[ScatteredPointNumber];
	for(i = 0;i<PntNum;i++)
	{
		ScatteredPoint[i].x = ScatteredPnt[i].x;
		ScatteredPoint[i].y = ScatteredPnt[i].y;
		ScatteredPoint[i].z = ScatteredPnt[i].z;
	}

	if (m1<=0||n1<=0||(ScatteredPointNumber == 0)||ScatteredPoint == NULL ) {
		return 0;
	}
	return 1;
}



//===========计算均匀单方向上的三次B样条基函数(B0(s),B1(s),B2(s),B3(s))==========//
//        入口：S(变量值)
//        出口：*BSplineValue(四个值的数组)
//        返回：1/0，成功/失败
//===============================================================================//
long CBSplineInterpolateObj::GetBasicBSplineFuction(float S,float *BSplineValue)
{
	if (!BSplineValue) {
		return  0;
	}
    BSplineValue[0] = (1.0f/6) * (1 + 3 * S - 3 * S * S + S * S * S);
	BSplineValue[1] = (1.0f/6) * (3 - 6 * S + 4 * S * S * S);
	BSplineValue[2] = (1.0f/6) * (-3 + 3 * S + 3 * S *S + S * S * S);
	BSplineValue[3] = 1;
	return 1;
}



//===========基本函数：计算Wkl函数值=============================================//
//        入口：k(0～3),l(0～3),s(Bk(s)),t(Bl(t))
//        出口：W(Wkl函数值)
//        返回：1/0，成功/失败
//===============================================================================//
long CBSplineInterpolateObj::CalculateW(int k,int l,float s,float t,float &W)
{
	float tempBValue1;
	float tempBValue2;
	if ((k == 0) && (l == 0)) {//k = 0,l = 0
		tempBValue1 = (1.0f/6) * (1 - 3 * s + 3 * s * s - s * s * s);
        tempBValue2 = (1.0f/6) * (1 - 3 * t + 3 * t * t - t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	if ((k == 1) && (l == 1)) {//k = 1,l =1;
		tempBValue1 = (1.0f/6) * (4 - 6 * s * s + 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (4 - 6 * t * t + 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 2) && (l == 2)) {//k = 2; l =2;
		tempBValue1 = (1.0f/6) * (1 + 3 * s + 3 * s * s - 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (1 + 3 * t + 3 * t * t - 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	if ((k == 3) && (l == 3)) {//k = 3, l =3;
		tempBValue1 = (1.0f/6) * (s * s * s);
        tempBValue2 = (1.0f/6) * (t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	if ((k == 0) && (l == 1)) {// k =0,l = 1;
		tempBValue1 = (1.0f/6) * (1 - 3 * s + 3 * s * s - s * s * s);
        tempBValue2 = (1.0f/6) * (4 - 6 * t * t + 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	if ((k == 0) && (l == 2)) {//k = 0;l = 2;
		tempBValue1 = (1.0f/6) * (1 - 3 * s + 3 * s * s - s * s * s);
        tempBValue2 = (1.0f/6) * (1 + 3 * t + 3 * t * t - 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 0) && (l == 3)) {// k = 0;l =3;
		tempBValue1 = (1.0f/6) * (1 - 3 * s + 3 * s * s - s * s * s);
        tempBValue2 = (1.0f/6) * (t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 1) && (l == 0)) {// k = 1, l = 0;
		tempBValue1 = (1.0f/6) * (4 - 6 * s * s + 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (1 - 3 * t + 3 * t * t - t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	if ((k ==1) && (l == 2)) {// k = 1,l = 2;
		tempBValue1 = (1.0f/6) * (4 - 6 * s * s + 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (1 + 3 * t + 3 * t * t - 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 1) && (l == 3)) {// k = 1, l = 3;
		tempBValue1 = (1.0f/6) * (4 - 6 * s * s + 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 2) && (l == 0)) {// k = 2, l = 0;
		tempBValue1 = (1.0f/6) * (1 + 3 * s + 3 * s * s - 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (1 - 3 * t + 3 * t * t - t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	if ((k == 2) && (l == 1)) {// k = 2, l = 1;
		tempBValue1 = (1.0f/6) * (1 + 3 * s + 3 * s * s - 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (4 - 6 * t * t + 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 2) && (l == 3)) {// k = 2, l = 3;
		tempBValue1 = (1.0f/6) * (1 + 3 * s + 3 * s * s - 3 * s * s * s);
        tempBValue2 = (1.0f/6) * (t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 3) && (l == 0)) {//k = 3, l =0;
		tempBValue1 = (1.0f/6) * (s * s * s);
        tempBValue2 = (1.0f/6) * (1 - 3 * t + 3 * t * t - t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
    if ((k == 3) && (l == 1)) {//k = 3, l =1;
		tempBValue1 = (1.0f/6) * (s * s * s);
        tempBValue2 = (1.0f/6) * (4 - 6 * t * t + 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	if ((k == 3) && (l == 2)) {//k = 3, l =2;
		tempBValue1 = (1.0f/6) * (s * s * s);
        tempBValue2 = (1.0f/6) * (1 + 3 * t + 3 * t * t - 3 * t * t * t);
		W = tempBValue1 * tempBValue2;
		return 1;
	}
	return 0 ;
}

//=====================计算控制点的φij值(先计算φc值)===========================//
//        入口：*ScatteredPnt(散乱点集数组),PntNum(散乱点集数组个数),
//              M1(0<=x<m1),N1(0<=y<n1)
//              Step(步长)
//        出口：*Phiij
//        返回：1/0，成功/失败
//===============================================================================//
long CBSplineInterpolateObj::GetPhiIJValue(G3D_Dot *ScatteredPnt,long PntNum,long M1, long N1,float XStep,float YStep,float *Phiij)
{
     if (!PhiIJ) {
		return 0;
	}
	VECTOR_FLOAT ScateredPnt_X;
	VECTOR_FLOAT ScateredPnt_Y;
    VECTOR_FLOAT ScateredPnt_Z;
    VECTOR_FLOAT Wab;
	VECTOR_FLOAT::iterator it;
	VECTOR_FLOAT TempWc;
	
    int i =0;
	int j =0;
	int n = 0;
	int k = 0;
	int l = 0;
	int m = 0;
	int p = 0;
	int Num = 0;
	int tempm = 0;
	int tempn = 0;
	float temp = 0;
	float FTemp = 0;
	float s = 0;
	float t = 0;
	float wc = 0;
	float numerator = 0;//分子
	float denominator  = 0;//分母
	float wab = 0;
	float PhiCTemp = 0;
	float Tempnumerator = 0;
	float Tempdenominator = 0;
	//矩形域上的控制点网格
    typedef struct ControlGrid {
		int M;
		int N;//(m1+3)*(n1+3)网格
		int Num;//邻近点集数目
		VECTOR_FLOAT IncludePnt;//邻近点数组(x，y，z三个一组，共3*Num个)
    }ControlGrid;
	ControlGrid * MN = new ControlGrid[sizeof(ControlGrid) * (m1+3) * (n1+3)];
	VECTOR_FLOAT PhiC;//中间变量φc
    //矩形域网格赋值
	for(i = 0;i<M1+3;i++)
	{
		for(j = 0;j<N1+3;j++)			
		{
            MN[i*(N1+3) + j].M = i - 1;      //矩形域
			MN[i*(N1+3) + j].N = j - 1;
			MN[i*(N1+3) + j].Num = 0;
		}
	}
	//散乱数据点值分别存入向量
	for(i = 0;i<PntNum;i++)
	{
		temp = ScatteredPnt[i].x;
		ScateredPnt_X.push_back(temp);
		temp = ScatteredPnt[i].y;
		ScateredPnt_Y.push_back(temp);
		temp = ScatteredPnt[i].z;
		ScateredPnt_Z.push_back(temp);
	}
	//邻近点集搜索,从-1开始到m+1,从-1开始到n+1
	for(i = 0;i<M1+3;i++)
	{
		for(j = 0;j<N1+3;j++)
		{
			tempm =  MN[i*(N1+3) + j].M;
			tempn =  MN[i*(N1+3) + j].N;
			for(k = 0;k<PntNum;k++)   
			{//找到4 × 4格子内的邻近点
				if(((ScatteredPnt[k].x - (tempm - 2)*XStep)>=0)&&((ScatteredPnt[k].x - (tempm + 2)*XStep)<0)
					&&((ScatteredPnt[k].y - (tempn -2)*YStep)>=0)&&((ScatteredPnt[k].y - (tempn + 2)*YStep)<0)) {
                    temp = ScatteredPnt[k].x;
					MN[i*(N1+3) + j].IncludePnt.push_back(temp);
					temp = ScatteredPnt[k].y;
					MN[i*(N1+3) + j].IncludePnt.push_back(temp);
					temp = ScatteredPnt[k].z;
					MN[i*(N1+3) + j].IncludePnt.push_back(temp);
					MN[i*(N1+3) + j].Num += 1;
				} 
			}
		}
	}
	CBSplineInterpolateObj tempObj;
	//开始计算φc，对于每一个点，必须从其邻近点集里面计算,最后给φij赋值
	for( i = 0;i<M1+3;i++)
	{
		for( j = 0;j<N1+3;j++)
		{
            Num = MN[i*(N1+3) + j].Num;
			if(Num == 0)
				Phiij[i * (N1+3) + j] = 0;
			else
			if(Num  == 1) {//一个邻近点时候直接将φc赋值给φij
                temp = MN[i*(N1+3) + j].IncludePnt[0]; 
				FTemp = MN[i*(N1+3) + j].IncludePnt[1];
				s = temp - int(temp);
				t = FTemp - int(FTemp);
				//计算分子,调用基本函数
				temp = temp / XStep;
				FTemp = FTemp / YStep;
				k = MN[i*(N1+3) +j].M + 1  - int(temp);
				l = MN[i*(N1+3) +j].N + 1  - int(FTemp);
                //计算Wc
                tempObj.CalculateW(k,l,s,t,wc);
                numerator = wc * MN[i * (N1+3) + j].IncludePnt[2];
				//计算分母
                for(m = 0;m<4;m++)
				{
					for(p = 0;p<4;p++)
					{
						tempObj.CalculateW(m,p,s,t,wab);
						wab = wab * wab;
						Wab.push_back(wab);
					}				
				}
				for(it = Wab.begin();it != Wab.end();it++)
				{
                    denominator += *it;
				}
				PhiCTemp = numerator / denominator;
				Phiij[i * (N1+3)+ j] = PhiCTemp;
				Wab.clear();
				numerator = 0;
				denominator = 0;
			}
			else
			if(Num>1)
			{//多个点用最小二乘法求解φij
			 for(n = 0;n<Num;n++)
			 {//每一个点φc
                 temp = MN[i*(N1+3) + j].IncludePnt[3*n + 0];
				 FTemp = MN[i*(N1+3)+ j].IncludePnt[3*n + 1];
				 s = temp - int(temp);
				 t = FTemp - int(FTemp);
				 //计算分子,调用基本函数
				 temp = temp / XStep;
				 FTemp = FTemp / YStep;
				 k = MN[i*(N1+3) +j].M + 1  - int(temp);
				 l = MN[i*(N1+3) +j].N + 1  - int(FTemp);
				 //计算Wc
                 tempObj.CalculateW(k,l,s,t,wc);
				 TempWc.push_back(wc);
				 //计算分子
				 numerator = wc * MN[i * (N1+3) + j].IncludePnt[3*n + 2];
				 //计算分母
				 for( m = 0 ;m<4;m++)
				 {
					 for(p = 0 ;p<4;p++)
					 {
						 tempObj.CalculateW(m,p,s,t,wab);
						 wab = wab * wab;
						 Wab.push_back(wab);
					 }
				 }
				 for(it = Wab.begin();it != Wab.end();it++)
				 {
					 denominator += *it;
				 }
				 PhiCTemp = numerator / denominator;
				 PhiC.push_back(PhiCTemp);
				 Wab.clear();
                 numerator = 0;
				 denominator = 0;
			 }
			 //最小二乘法求φij
			 temp = 0;//清零作叠加
			 FTemp = 0;
			 PhiCTemp = 0;
			 for(n = 0;n<Num;n++)
			 {
				 temp += TempWc[n] * TempWc[n] * PhiC[n];
				 FTemp += TempWc[n] * TempWc[n];
			 }
			 PhiCTemp = (temp) / (FTemp);
			 Phiij[i * (N1+3) + j] = PhiCTemp; 
			}
		}
	}	
    if (MN) {
		delete []MN;
		MN = NULL;
    }
    PhiIJ = new float[(m1 +3)*(n1 +3)];
	for(i = 0;i<m1+3;i++)
	{
		for(j = 0;j<n1+3;j++)
		{
			PhiIJ[i * (n1+3) + j] = Phiij[i * (n1+3) + j];
		}
	}
	if (PhiIJ==NULL) {
		return 0;
	}
     return 1;
}
long CBSplineInterpolateObj::GetInterpolateValue(G3D_Dot * ScatteredPnt,
												 long PntNum,long M1,long N1,float XStep,float YStep,
												 double &Ix,double &Iy,double &Iz)
{
	if (!ScatteredPnt) {
		return 0;
	}
	int I;//下标i
	int J;//下标j
	int k;
	int l;
    float s;
	float t;
	float temp;
	float T = 0;
	float w;
	VECTOR_FLOAT PhiC;//中间变量φc
	VECTOR_FLOAT FxyTemp;
	VECTOR_FLOAT ::iterator it;

	if (PhiIJ == NULL) {
		return 0;
	}
	CBSplineInterpolateObj Obj;
	//求插值
	I = int(Ix / XStep) - 1 + 1;//插值点位置
	J = int(Iy / YStep) - 1 + 1;//插值点位置
	s = Ix - int(Ix);
	t = Iy - int(Iy);
	for(k = 0;k<4;k++)
	{
		for(l = 0;l<4;l++)
		{
            Obj.CalculateW(k,l,s,t,w);
			temp = w;
			temp = temp * PhiIJ[(I + k) * (n1 + 3) + (J + l)];
			FxyTemp.push_back(temp);
		}
	}
	for(it = FxyTemp.begin();it!=FxyTemp.end();it++)
	{
		T += *it;//求和
	}
	//导出f(x,y),即Iz
    Iz = T;
	/*
	for(p = 0;p<IntPntNum;p++)
	{	
	//*	
	I = int((InterpolatePnt[p].x) / Step)  - 1 + 1 ;//插值点位置
	J = int((InterpolatePnt[p].y) / Step)  - 1 + 1 ;//插值点位置
	s = InterpolatePnt[p].x - int(InterpolatePnt[p].x);
	t = InterpolatePnt[p].y - int(InterpolatePnt[p].y);
	
	  for(k = 0;k<4;k++)
	  {
	  for(l = 0;l<4;l++)
	  {
	  CalculateW(k,l,s,t,w);
	  temp = w;
	  temp = temp * PhiIJ[(I + k) * (n1+3) + (J + l)];
	  FxyTemp.push_back(temp);
	  }
	  }
	  for(it = FxyTemp.begin();it!=FxyTemp.end();it++)
	  {
	  T += *it;//求和
	  }
	  Fxy.push_back(T);
	  FxyTemp.clear();
	  T = 0;
	//*/
	return 1;
}