KdTreeを汎用化して使おうかなと思って作成した。

KdTreeMulti.cpp

#include"Vector.h"

#include<vector>

using namespace std;

/*

  KdTreeMulti 

		kdTreeを汎用的に実行できるクラスを実行

		add 追加
		search 探索

		getPos 仮想的にクラスで使う座標を取得

	（例）
		class kdTreeVector :public KdTreeMulti<Vector*>
		{
		public:
			kdTreeVector(Vector* v)
			{
				element = v;
			}

			Vector getPos()
			{
				return *element;
			}

			void Release()
			{
				delete(element);
				KdTreeMulti::Release();
			}
		};


		int main()
		{
			kdTreeVector *v = new kdTreeVector(new Vector(0,0,0));
			v->Initialize(400);

			for(int i=0;i<20;i++)
			{
				v->Add( new kdTreeVector(new Vector( rand()%100 , rand()%100 , rand()%100 ) ) );
			}

			vector<Vector**> qn;

			v->Search( &qn , Vector() , 200 );

			v->SearchEllipse( &qn , Vector(0,1,0) , Vector() , 200 );

			v->Release();
		}

*/

//kd木での分割点
#define NCUT -1
#define XCUT 0
#define YCUT 1
#define ZCUT 2

template<class T>
class KdTreeMulti
{
protected:
	//情報
	T element;
	//空間のサイズ
	float m_width;

	//次の値
	KdTreeMulti<T> *low;
	KdTreeMulti<T> *high;	

	//分割位置
	int cuts;
	//表示用のステップ
	int step;
	//半径
	double searchAlpha;

protected:

	//座標の取得
	virtual Vector getPos() = 0;

	//分割軸の遷移
	int NextCuts(int cuts)
	{
		switch(cuts)
		{
			case XCUT:cuts = YCUT;break;
			case YCUT:cuts = ZCUT;break;
			case ZCUT:cuts = XCUT;break;
		}
		return cuts;
	}

protected:

	KdTreeMulti()
	{
		low = 0;
		high = 0;
		cuts = XCUT;
		step=0;
		searchAlpha=0;
	}

public:

	//初期化
	virtual void Initialize(float width)
	{
		m_width = width;
	}

	//追加
	void Add( KdTreeMulti<T> *add , int cs = XCUT )
	{
		//追加した奴は初期化されていないので
		add->Initialize(m_width);

		Vector pos = getPos();

		float node_value;
		float add_value;
		//X
		if( cs == XCUT ){ node_value = pos.x; add_value = add->getPos().x; }
		//Y
		else if( cs == YCUT ){ node_value = pos.y; add_value = add->getPos().y; }
		//Z
		else if( cs == ZCUT ){ node_value = pos.z; add_value = add->getPos().z; }
		
		//基準の代入
		cuts = cs;
		
		//基準より大きいとき
		if( node_value < add_value )
		{
			//あったらそこの位置に
			if( high == 0 )high = add;
			//なかったら次の階層
			else high->Add( add , NextCuts(cs) );
		}
		//基準以下のとき
		else
		{
			if( low == 0 )low = add;
			else low->Add( add , NextCuts(cs) );
		}	
	}
	//取得
	void Search( vector<T*> *qn , Vector p , float r )
	{
		Vector pos = getPos();

		//円の小さい値と大きい値
		float c_low = 0;
		float c_high = 0;
		float n_value = 0;

		if( cuts == XCUT )		{c_low = p.x-r;c_high = p.x+r;n_value = pos.x;}
		else if( cuts == YCUT )	{c_low = p.y-r;c_high = p.y+r;n_value = pos.y;}
		else if( cuts == ZCUT )	{c_low = p.z-r;c_high = p.z+r;n_value = pos.z;}

		//マルだったら値の描画
		float s = (p-pos).dot(p-pos);
		float rr = r*r;
		if( s < rr )
		{
			//通常の半径を指定する
			searchAlpha = sqrt(s);
			qn->push_back( &element );
		}
		
		//次の点の決め方
		if( c_low < n_value || c_high < n_value )
			if(low!=0)low->Search( qn,p,r );
		if( c_low > n_value || c_high > n_value )
			if(high!=0)high->Search( qn,p,r );
	}
	void SearchEllipse(  vector<T*> *qn , Vector n ,  Vector p , float r )
	{
		Vector pos = getPos();

		//円の小さい値と大きい値
		float c_low = 0;
		float c_high = 0;
		float n_value = 0;

		if( cuts == XCUT )		{c_low = p.x-r;c_high = p.x+r;n_value = pos.x;}
		else if( cuts == YCUT )	{c_low = p.y-r;c_high = p.y+r;n_value = pos.y;}
		else if( cuts == ZCUT )	{c_low = p.z-r;c_high = p.z+r;n_value = pos.z;}

		//マルだったら値の描画
		Vector y = n * ( n.dot( pos-p ) );
		float x = (( pos-p )-y).length();

		//楕円の範囲内だったら
		float s = 1-(x*x)/(r*r);
		if( s > 0 )
		{
			//yの期待値を求める
			float ey = 1*sqrt( s );

			//楕円の範囲内だったら
			if( ey > y.length() )
			{
				//通常の半径を指定する
				searchAlpha = x;
				qn->push_back( &element );
			}
		}
		
		//次の点の決め方
		if( c_low < n_value || c_high < n_value )
			if(low!=0)low->SearchEllipse( qn,n,p,r );
		if( c_low > n_value || c_high > n_value )
			if(high!=0)high->SearchEllipse( qn,n,p,r );
	}
	//開放(ポインタ使っていたらここで消すしかない)
	virtual void Release()
	{
		if( low != 0 )low->Release();
		if( high != 0 )high->Release();
		delete(this);
	}

};