建立3D点的KD树，速度很快

建立3D点的kd树，速度很快，可以自己设置搜索距离和搜索点数

#include “3dKDtree.h“

kdTree::kdTree（const int nodes）
{
	storedKDNodes = 0;
	maxNumOfNodes = nodes;

	kdNodes = new kdNode[maxNumOfNodes + 1];
	if（!kdNodes）
	{
		cout<<“初始化kd树时内存溢出！“<		exit（-1）;
	}

	boundrayMin[0] = boundrayMin[1] = boundrayMin[2] = 1e8f;
	boundrayMax[0] = boundrayMax[1] = boundrayMax[2] = -1e8f;
}

kdTree::~kdTree（）
{
	delete [] kdNodes;
}

void kdTree::treeBalance（）
{
	if（storedKDNodes > 1）
	{
		kdNode **pa1 = new kdNode*[storedKDNodes + 1];  //组织好树后的指针
		kdNode **pa2 = new kdNode*[storedKDNodes + 1];  //原始元素的指针

		for（int i =0; i <= storedKDNodes; i++）
			pa2[i] = &kdNodes[i];

		balancePartition（pa1 pa2 1 1 storedKDNodes）;
		delete []pa2;

		//重新排列树
		//__w64 int d j = 1;                  // According to the warning given when ‘int ‘ is used
		int d j = 1; //j位置元素已经转移走
		int foo = 1;  //fooNodes存储的元素的最初位置
		kdNode fooNodes = kdNodes[j];

		for（int i = 1; i <= storedKDNodes; i++）
		{
			d = pa1[j] - kdNodes;
			pa1[j] = NULL;

			if（d != foo）
				kdNodes[j] = kdNodes[d];
			else
			{
				kdNodes[j] = fooNodes;

				if（i < storedKDNodes）
				{
					for（; foo <= storedKDNodes; foo++）
						if（NULL != pa1[foo]）
							break;

					fooNodes = kdNodes[foo];
					j = foo;
				}
				continue;
			}
			j = d;
		}
		delete []pa1;
	}
	halfStoredKDNodes = storedKDNodes/2 - 1;
}

void kdTree::locateNodes（nNearestNodes * const nNNconst int index）const
{
	const kdNode *p = &kdNodes[index];
	double dist1;

	if（index < halfStoredKDNodes）
	{
		dist1 = nNN->pos[p->plane] - p->pos[p->plane];

		if（0.0 < dist1）
		{
			locateNodes（nNN 2 * index + 1）;
			if（nNN->dist2[0] > dist1 * dist1）
				locateNodes（nNN 2 * index）;
		}
		else
		{
			locateNodes（nNN 2 * index）;
			if（nNN->dist2[0] > dist1 * dist1）
				locateNodes（nNN 2 * index + 1）;
		}//if
	}//if

	// 计算距离
	dist1 = p->pos[0] - nNN->pos[0];
	double dist2 = dist1 * dist1;
	dist1 = p->pos[1] - nNN->pos[1];
	dist2 += dist1 * dist1;
	dist1 = p->pos[2] - nNN->pos[2];
	dist2 += dist1 * dist1;

	if（nNN->dist2[0] > dist2）
	{
		if（nNN->found < nNN->max）
		{
			nNN->found++;
			nNN->dist2[nNN->found] = dist2;
			nNN->index[nNN->found] = p;
		}
		else
		{
			int j parent;
			if（0 == nNN->got_Heap）//建立大顶堆
			{
				double dst2;
				const kdNode *nd;
				int halfFound = nNN->found >> 1;

				for（int k = halfFound; k >= 1; k--）
				{
					parent = k;
					nd = nNN->index[k];
					dst2 = nNN->dist2[k];

					while（parent <= halfFound）
					{
						j = parent + parent;

						if（j < nNN->found && nNN->dist2[j] < nNN->dist2[j + 1]）
							j ++;

						if（dst2 >= nNN->dist2[j]）
							break;

						nNN->dist2[parent] = nNN->dist2[j];
						nNN->index[parent] = nNN->index[j];

						parent = j;
					}//while
					nNN->dist2[parent] = dst2;
					nNN->index[parent] = nd;
				}//for
				nNN->got_Heap = 1;
			}//if

			//插入
			parent = 1;
			//if（）
			j

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       5652  2009-09-16 21:03  3D_KDTree\3D_KDTree\3dKDtree.cpp

     文件       3161  2009-09-16 20:52  3D_KDTree\3D_KDTree\3dKDtree.h

     文件       4104  2009-09-16 17:04  3D_KDTree\3D_KDTree\3D_KDTree.vcproj

     文件       1427  2009-09-17 17:12  3D_KDTree\3D_KDTree\3D_KDTree.vcproj.LH-G1QDTFEC081H.gejuan.user

     文件      46192  2009-09-16 21:04  3D_KDTree\3D_KDTree\Debug\3dKDtree.obj

     文件        405  2009-09-16 17:06  3D_KDTree\3D_KDTree\Debug\3D_KDTree.exe.embed.manifest

     文件        472  2009-09-16 17:06  3D_KDTree\3D_KDTree\Debug\3D_KDTree.exe.embed.manifest.res

     文件        387  2009-09-16 21:06  3D_KDTree\3D_KDTree\Debug\3D_KDTree.exe.intermediate.manifest

     文件       6714  2009-09-16 21:06  3D_KDTree\3D_KDTree\Debug\BuildLog.htm

     文件         63  2009-09-16 21:06  3D_KDTree\3D_KDTree\Debug\mt.dep

     文件      65281  2009-09-16 21:06  3D_KDTree\3D_KDTree\Debug\testMain.obj

     文件     453632  2009-09-16 21:06  3D_KDTree\3D_KDTree\Debug\vc80.idb

     文件     233472  2009-09-16 21:06  3D_KDTree\3D_KDTree\Debug\vc80.pdb

     文件       1111  2009-09-16 21:09  3D_KDTree\3D_KDTree\testMain.cpp

     文件   10234880  2009-09-17 17:12  3D_KDTree\3D_KDTree.ncb

     文件        892  2009-09-16 09:37  3D_KDTree\3D_KDTree.sln

    ..A..H.     11264  2009-09-17 17:12  3D_KDTree\3D_KDTree.suo

     文件      53248  2009-09-16 21:06  3D_KDTree\debug\3D_KDTree.exe

     文件     473492  2009-09-16 21:06  3D_KDTree\debug\3D_KDTree.ilk

     文件     576512  2009-09-16 21:06  3D_KDTree\debug\3D_KDTree.pdb

     文件    2377570  2009-09-16 21:39  3D_KDTree\KD_Tree.docx

     目录          0  2009-09-16 21:07  3D_KDTree\3D_KDTree\Debug

     目录          0  2009-09-16 21:09  3D_KDTree\3D_KDTree

     目录          0  2009-09-16 21:07  3D_KDTree\debug

     目录          0  2009-09-16 21:39  3D_KDTree

----------- ---------  ---------- -----  ----

             14549931                    25