kdtree.cpp

处理网格数据点的KDTREE数据结构程序

#include "KDTree.h"
#include <stack> 

//创建树 p是坐标数组，low为数组起始下标，high为末下标，k指示起始排序方向，默认为0（x方向）
kdt KDTree::CreateSubTree(osg::Vec3Array *p,int low,int high,int k=0)
{
    //show（）是创建一棵树的主要创建函数
	osg::Vec3Array* pa =new osg::Vec3Array;
	pa->insert(pa->end(),p->begin(),p->end());
	show(pa,low,high,k);
	//返回根节点
	return root;
};

//查找树中是否包含坐标（x,y）的节点，t为开始查找的节点
//f为查找失败后复制给叶子节点的值，默认为NULL，p为保存当前搜索节点的父节点,real表明search函数在创建时调用还是查找时调用
bool KDTree::SearchKDT(kdt t,float x,float y,int real,kdt f=NULL,kdt *p=NULL)
	{
		if(!t) {*p=f;return false;} //若搜索节点为空，则将f赋值给其父节点
		else if(sqrt(pow(t->p._v[0]-x,2)+pow(t->p._v[1]-y,2))<=0.28) //若找到该值，则将该节点赋值给p，输出
		{   if(real==0)
		    {
			*p=t;
			//std::cout<<"查找到此点：";
			//std::cout<<"("<<x<<","<<y<<","<<t->p._v[2]<<")"<<std::endl;
			return true;
		    }
		    else
			{
                *p=t;
				return true;
		     }
		}
		else if(t->key==0&&(fabs(t->p._v[0]-x)<0.1||t->p._v[0]<x)) SearchKDT(t->r,x,y,real,t,p); //若当前搜索方向为x，比较x大小，左递归
		else if(t->key==0&&t->p._v[0]>x) SearchKDT(t->l,x,y,real,t,p);//若当前搜索方向为x，比较x大小，右递归
		else if(t->key==1&&(fabs(t->p._v[1]-y)<0.1||t->p._v[1]<y)) SearchKDT(t->r,x,y,real,t,p); //若当前搜索方向为y，比较y大小，右递归
		else if(t->key==1&&t->p._v[1]>y) SearchKDT(t->l,x,y,real,t,p); //若当前搜索方向为y，比较y大小，左递归
		
		
	};

//向树中插入vec3类型的值e，t为起始节点
int KDTree::InsertKDT(kdt *t,osg::Vec3 e)
	{
		kdt p=NULL;//p，s为中间变量
		kdt s=NULL;
		if(!SearchKDT(*t,e._v[0],e._v[1],1,NULL,&p))//搜索该值，若找到就放弃不插入，否则插入
		{
			/*kkk++;
			std::cout<<"kkk="<<kkk<<std::endl;*/
		s=(kdt)malloc(sizeof(KDTreeNode));//分配新节点空间
		s->p._v[0]=e._v[0];//赋值x,y,z
		s->p._v[1]=e._v[1];
		s->p._v[2]=e._v[2];
		s->l=NULL;//新插入节点的左右子树赋值为空
		s->r=NULL;

		if(!p) s->key=0;//若该节点的父节点不存在，则key赋值0（起始搜索方向），否则为父节点的key值加1取余2，的0或1
		else s->key=(p->key+1)%2;


		if(!p) *t=s;//若父节点不存在，意味该树未创建，新节点作为当前节点
		else if(p->key==0&&(fabs(e.x()-p->p.x())<0.1||e.x()>p->p.x())) p->r=s; //若父节点搜索方向为x，比较x大小，作为右子节点
		else if(p->key==0&&e.x()<p->p.x()) p->l=s; //若父节点搜索方向为x，比较x大小，作为左子节点
		else if(p->key==1&&(fabs(e.y()-p->p.y())<0.1||e.y()>p->p.y())) p->r=s; //若父节点搜索方向为y，比较y大小，作为右子节点
		else if(p->key==1&&e.y()<p->p.y()) p->l=s; //若父节点搜索方向为y，比较y大小，作为左子节点
		
		return true;
		}
		else 
			return false;
	};

//创建树的主要函数，p是坐标数组，low为数组起始下标，high为末下标，k指示起始排序方向，默认为0（x方向）
int KDTree::show(osg::Vec3Array *pa,int low,int high,int k)
{
	if(k%2==0)//若搜索方向标志为偶数，则排序x方向否则y方向
    quick_sortX(pa,low,high);
	else
    quick_sortY(pa,low,high);

	if(low<high)//low<high递归结束标识
	{
		//插入排序后的中间值
		InsertKDT(&root,pa->at(int((low+high)/2)));
	}
	else
		return 0;
	show(pa,low,int((low+high)/2)-1,++k);//左右部分递归
	show(pa,int((low+high)/2)+1,high,++k);
};

//前序遍历输出
osg::Vec3Array* KDTree::PreOrderTraverse(kdt t)
{
	if(!t)
		return 0;

	temparray->push_back(t->p);
	//temparray->insert(temparray->end(),t->p);
	//std::cout<<"("<<t->p.x()<<","<<t->p.y()<<","<<t->p.z()<<")"<<std:: endl;

	PreOrderTraverse(t->l);
	PreOrderTraverse(t->r);
	return temparray;

};


//交换指针p1,p2指向的值
void KDTree::exchange(osg::Vec3 *p1,osg::Vec3 *p2)
{
	osg::Vec3 temp;
	temp._v[0]=0;
	temp._v[1]=0;
	temp._v[2]=0;

	temp._v[0]=(*p1)._v[0];
    (*p1)._v[0]=(*p2)._v[0];
    (*p2)._v[0]=temp._v[0];

	temp._v[1]=(*p1)._v[1];
    (*p1)._v[1]=(*p2)._v[1];
    (*p2)._v[1]=temp._v[1];

	temp._v[2]=(*p1)._v[2];
    (*p1)._v[2]=(*p2)._v[2];
    (*p2)._v[2]=temp._v[2];


};


//以下为快速排序算法
//pa为指向A[]的数组，p,r为下标，对A[p..r]进行就地重排，以A[r]为主元
//划分为小于主元和大于主元的两部分，返回主元的下标q
//例如：A[1..6]={18,8,16,6,9,10}结果：A[1..6]={8,6,9,10,18,16} q=4
//(元素顺序可能与结果不一致，但小于10的元素在10前面，大于10的元素在10后面)


//int KDTree::partitionX(osg::Vec3Array *pa,int p,int r)
//{
//	int x=pa->at(r)._v[0]; //主元
//    int i=p-1,j;  //i表示小于主元的最后一个元素
//    for(j=p;j<r;j++)
//    {
//        if(pa->at(j)._v[0]<x)
//        {
//            i++;
//            exchange(&(pa->at(i)),&(pa->at(j)));
//        }
//    }
//    exchange(&(pa->at(r)),&(pa->at(i+1)));
//    return i+1;
//};
//
//int KDTree::partitionY(osg::Vec3Array *pa,int p,int r)
//{
//	int y=pa->at(r)._v[1]; //主元
//    int i=p-1,j;  //i表示小于主元的最后一个元素
//    for(j=p;j<r;j++)
//    {
//        if(pa->at(j)._v[1]<y)
//        {
//            i++;
//            exchange(&(pa->at(i)),&(pa->at(j)));
//        }
//    }
//    exchange(&(pa->at(r)),&(pa->at(i+1)));
//    return i+1;
//};
//
//void KDTree::quick_sortX(osg::Vec3Array *pa,int p,int r)
//{
//    if(p<r)
//    {
//        int q=partitionX(pa,p,r);
//        quick_sortX(pa,p,q-1);
//        quick_sortX(pa,q+1,r);
//    }
//};
//
//void KDTree::quick_sortY(osg::Vec3Array *pa,int p,int r)
//{
//    if(p<r)
//    {
//        int q=partitionY(pa,p,r);
//        quick_sortY(pa,p,q-1);
//        quick_sortY(pa,q+1,r);
//    }
//};

int KDTree::partitionX(osg::Vec3Array* pData, int low, int high) 
{ 
	osg::Vec3 key = pData->at(low); 
       while(low < high) 
       { 
		   while(low < high && pData->at(high)._v[0]>key._v[0]) 
                  high--; 
		   exchange(&(pData->at(low)), &(pData->at(high))); 
		   while(low < high && pData->at(low)._v[0]<=key._v[0]) 
                     low++; 
              exchange(&(pData->at(low)), &(pData->at(high))); 
       } 
       return low; 
};
void KDTree::quick_sortX(osg::Vec3Array* pData, int low, int high) 
{ 
std::stack<int> st; 
       int tmp; 
       if(low < high) 
       { 
              int pivot = partitionX(pData, low, high); 
              st.push(low); 
              st.push(pivot - 1); 
              st.push(pivot + 1); 
              st.push(high); 
              while(!st.empty()) 
              {    high = st.top(); 
                    st.pop(); 
                     low = st.top(); 
                     st.pop(); 
                     if(low < high) 
                     { 
                            pivot = partitionX(pData, low, high); 
                            tmp = pivot - 1; 
                            if(low < tmp) 
                            {     st.push(low); 
                                   st.push(tmp); 
                            } 
                            tmp = pivot + 1; 
                            if(tmp < high) 
                            { 
                                   st.push(tmp); 
                                   st.push(high); 
                            }
                     }
              }
       }
};
int KDTree::partitionY(osg::Vec3Array* pData, int low, int high) 
{ 
	osg::Vec3 key = pData->at(low); 
	 while(low < high) 
       { 
		   while(low < high && pData->at(high)._v[1] > key._v[1]) 
                  high--; 
		   exchange(&(pData->at(low)), &(pData->at(high))); 
		   while(low < high && pData->at(low)._v[1] <= key._v[1]) 
                     low++; 
              exchange(&(pData->at(low)), &(pData->at(high))); 
       } 
       return low; 
};
void KDTree::quick_sortY(osg::Vec3Array* pData, int low, int high) 
{ 
	std::stack<int> st; 
       int tmp; 
       if(low < high) 
       { 
              int pivot = partitionY(pData, low, high); 
              st.push(low); 
              st.push(pivot - 1); 
              st.push(pivot + 1); 
              st.push(high); 
              while(!st.empty()) 
              {    high = st.top(); 
                    st.pop(); 
                     low = st.top(); 
                     st.pop(); 
                     if(low < high) 
                     { 
                            pivot = partitionY(pData, low, high); 
                            tmp = pivot - 1; 
                            if(low < tmp) 
                            {     st.push(low); 
                                   st.push(tmp); 
                            } 
                            tmp = pivot + 1; 
                            if(tmp < high) 
                            {
								st.push(tmp);
								st.push(high);
							}
					 }
			  }
	   }
};