/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include <cmath>
		#include <cfloat>
		#include <cstring>
		#include "core/geometry_kdtree.h"
		#include "core/geometry.h"
		#include "scene3d/mobject.h"
		#include "scene3d/mlight.h"
		#include "scene3d/mcamera.h"
		#include "timing/benchmark.h"
		#include "log/log.h"

		using namespace GS;
		using namespace GS::Core;


GeometryKDTree::KDTreeNode::KDTreeNode(){m_KDTREE_NODE_ID_POLY = NULL; m_KDTREE_NODE_ID_VTX = NULL; memset(m_KDTREE_NODE_ID_ROPE, -1, sizeof(int)*6);};

#define KDTREE_MAX_DEPTH			20
#define KDTREE_MAX_POLY_PER_NODE	5

#define X 0
#define Y 1
#define Z 2

#define CROSS(dest,v1,v2) \
	dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
	dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
	dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; 

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2) \
	dest[0]=v1[0]-v2[0]; \
	dest[1]=v1[1]-v2[1]; \
	dest[2]=v1[2]-v2[2]; 

#define FINDMINMAX(x0,x1,x2,Min,Max) \
	Min = Max = x0;   \
	if(x1<Min) Min=x1;\
	if(x1>Max) Max=x1;\
	if(x2<Min) Min=x2;\
	if(x2>Max) Max=x2;


//-----------------------------------------------------------------------------------
int GeometryKDTree::planeBoxOverlap(float normal[3], float vert[3], float maxbox[3])	
//-----------------------------------------------------------------------------------
{
	int q;
	float vmin[3],vmax[3],v;

	for(q=X;q<=Z;q++)
	{
		v=vert[q];				

		if(normal[q]>0.0f)
		{
			vmin[q]=-maxbox[q] - v;	
			vmax[q]= maxbox[q] - v;	
		}
		else
		{
			vmin[q]= maxbox[q] - v;	
			vmax[q]=-maxbox[q] - v;
		}
	}

	if(DOT(normal,vmin)>0.0f) 
		return 0;	

	if(DOT(normal,vmax)>=0.0f) 
		return 1;	

	return 0;

}

/*======================== X-tests ========================*/

#define AXISTEST_X01(a, b, fa, fb)			   \
	p0 = a*v0[Y] - b*v0[Z];			       	   \
	p2 = a*v2[Y] - b*v2[Z];			       	   \
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
	rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
	if(min>rad || max<-rad) return 0;

#define AXISTEST_X2(a, b, fa, fb)			   \
	p0 = a*v0[Y] - b*v0[Z];			           \
	p1 = a*v1[Y] - b*v1[Z];			       	   \
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
	if(min>rad || max<-rad) return 0;

/*======================== Y-tests ========================*/

#define AXISTEST_Y02(a, b, fa, fb)			   \
	p0 = -a*v0[X] + b*v0[Z];		      	   \
	p2 = -a*v2[X] + b*v2[Z];	       	       	   \
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
	rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
	if(min>rad || max<-rad) return 0;

#define AXISTEST_Y1(a, b, fa, fb)			   \
	p0 = -a*v0[X] + b*v0[Z];		      	   \
	p1 = -a*v1[X] + b*v1[Z];	     	       	   \
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
	if(min>rad || max<-rad) return 0;

/*======================== Z-tests ========================*/
#define AXISTEST_Z12(a, b, fa, fb)			   \
	p1 = a*v1[X] - b*v1[Y];			           \
	p2 = a*v2[X] - b*v2[Y];			       	   \
	if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
	rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
	if(min>rad || max<-rad) return 0;

#define AXISTEST_Z0(a, b, fa, fb)			   \
	p0 = a*v0[X] - b*v0[Y];				   \
	p1 = a*v1[X] - b*v1[Y];			           \
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
	if(min>rad || max<-rad) return 0;

//-------------------------------------------------------------------------------------------------------------------------
int GeometryKDTree::triBoxOverlap(float boxcenter[3],float boxhalfsize[3],float Verts1[3],float Verts2[3],float Verts3[3])
//-------------------------------------------------------------------------------------------------------------------------

{	/*    use separating axis theorem to test overlap between triangle and box */
	/*    need to test for overlap in these directions: */
	/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
	/*       we do not even need to test these) */
	/*    2) normal of the triangle */
	/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
	/*       this gives 3x3=9 more tests */

	float v0[3],v1[3],v2[3];

	//   float axis[3];
	float min,max,p0,p1,p2,rad,fex,fey,fez;		// -NJMP- "d" local variable removed
	float normal[3],e0[3],e1[3],e2[3];


	/* This is the fastest branch on Sun */
	/* move everything so that the boxcenter is in (0,0,0) */
	SUB(v0,Verts1,boxcenter);
	SUB(v1,Verts2,boxcenter);
	SUB(v2,Verts3,boxcenter);

	/* compute triangle edges */
	SUB(e0,v1,v0);      /* tri edge 0 */
	SUB(e1,v2,v1);      /* tri edge 1 */
	SUB(e2,v0,v2);      /* tri edge 2 */

	/* Bullet 3:  */

	/*  test the 9 tests first (this was faster) */
	fex = fabsf(e0[X]);
	fey = fabsf(e0[Y]);
	fez = fabsf(e0[Z]);

	AXISTEST_X01(e0[Z], e0[Y], fez, fey);
	AXISTEST_Y02(e0[Z], e0[X], fez, fex);
	AXISTEST_Z12(e0[Y], e0[X], fey, fex);

	fex = fabsf(e1[X]);
	fey = fabsf(e1[Y]);
	fez = fabsf(e1[Z]);

	AXISTEST_X01(e1[Z], e1[Y], fez, fey);
	AXISTEST_Y02(e1[Z], e1[X], fez, fex);
	AXISTEST_Z0(e1[Y], e1[X], fey, fex);

	fex = fabsf(e2[X]);
	fey = fabsf(e2[Y]);
	fez = fabsf(e2[Z]);

	AXISTEST_X2(e2[Z], e2[Y], fez, fey);
	AXISTEST_Y1(e2[Z], e2[X], fez, fex);
	AXISTEST_Z12(e2[Y], e2[X], fey, fex);

	/* Bullet 1: */
	/*  first test overlap in the {x,y,z}-directions */
	/*  find min, max of the triangle each direction, and test for overlap in */
	/*  that direction -- this is equivalent to testing a minimal AABB around */
	/*  the triangle against the AABB */

	/* test in X-direction */
	FINDMINMAX(v0[X],v1[X],v2[X],min,max);

	if(min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0;

	/* test in Y-direction */
	FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);

	if(min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0;

	/* test in Z-direction */
	FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);

	if(min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0;

	/* Bullet 2: */
	/*  test if the box intersects the plane of the triangle */
	/*  compute plane equation of triangle: normal*x+d=0 */

	CROSS(normal,e0,e1);

	if(!planeBoxOverlap(normal,v0,boxhalfsize)) return 0;	
		
	return 1;   /* box and triangle overlaps */
}

//---------------------------------------------------------------------------------------------
int GeometryKDTree::GetKDtreeSideAABB(float* _AABB, Vector4 &_EntryPoint, int &_LastEntrySide)
//---------------------------------------------------------------------------------------------
{
	int l_AvoidSide = _LastEntrySide;
	if(_LastEntrySide != -1)
	{
		if(_LastEntrySide%2)
			l_AvoidSide = _LastEntrySide-1;
		else
			l_AvoidSide = _LastEntrySide+1;
	}

	// check the nearest plane for the nearest rope
	int l_SideChoose = -1;
	float l_Dist = 3.402823466e+38F;

	float l_CheckDist = fabs(_AABB[KDTREE_SIDE_LEFT] - _EntryPoint.x);
	if(l_CheckDist < l_Dist && l_AvoidSide != KDTREE_SIDE_LEFT)
	{
		l_SideChoose = KDTREE_SIDE_LEFT;
		l_Dist = l_CheckDist;
	}

	l_CheckDist = fabs(_AABB[KDTREE_SIDE_RIGHT] - _EntryPoint.x);
	if(l_CheckDist < l_Dist && l_AvoidSide != KDTREE_SIDE_RIGHT)
	{
		l_SideChoose = KDTREE_SIDE_RIGHT;
		l_Dist = l_CheckDist;
	}

	l_CheckDist = fabs(_AABB[KDTREE_SIDE_BOTTOM] - _EntryPoint.y);
	if(l_CheckDist < l_Dist && l_AvoidSide != KDTREE_SIDE_BOTTOM)
	{
		l_SideChoose = KDTREE_SIDE_BOTTOM;
		l_Dist = l_CheckDist;
	}

	l_CheckDist = fabs(_AABB[KDTREE_SIDE_TOP] - _EntryPoint.y);
	if(l_CheckDist < l_Dist && l_AvoidSide != KDTREE_SIDE_TOP)
	{
		l_SideChoose = KDTREE_SIDE_TOP;
		l_Dist = l_CheckDist;
	}

	l_CheckDist = fabs(_AABB[KDTREE_SIDE_BACK] - _EntryPoint.z);
	if(l_CheckDist < l_Dist && l_AvoidSide != KDTREE_SIDE_BACK)
	{
		l_SideChoose = KDTREE_SIDE_BACK;
		l_Dist = l_CheckDist;
	}

	l_CheckDist = fabs(_AABB[KDTREE_SIDE_FRONT] - _EntryPoint.z);
	if(l_CheckDist < l_Dist && l_AvoidSide != KDTREE_SIDE_FRONT)
	{
		l_SideChoose = KDTREE_SIDE_FRONT;
		l_Dist = l_CheckDist;
	}

	_LastEntrySide = l_SideChoose;

	return l_SideChoose;
}

//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
bool GeometryKDTree::IntersectTriangle(const Vector4 &s, const Vector4 &d, float* Vtx1, float* Vtx2, float* Vtx3, Vector4* _Edge1, Vector4* _Edge2, float &l_Dist, float &u, float &v, bool& _Backface)
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
{
	const float EPSILON = 0.000001f;
	l_Dist = 3.402823466e+38F;

	//test if inside the plane

	/* find vectors for two edges sharing vert0 */
	//nVector l_edge1(Vtx2[0] - Vtx1[0], Vtx2[1] - Vtx1[1], Vtx2[2] - Vtx1[2]);
	//nVector l_edge2(Vtx3[0] - Vtx1[0], Vtx3[1] - Vtx1[1], Vtx3[2] - Vtx1[2]);

	/* begin calculating determinant - also used to calculate U parameter */
	//nVector l_pvec = d.Cross( *_Edge2); 
	Vector4 l_pvec(d.y * _Edge2->z - d.z * _Edge2->y, d.z * _Edge2->x - d.x * _Edge2->z, d.x * _Edge2->y - d.y * _Edge2->x);

	/* if determinant is near zero, ray lies in plane of triangle */
	float l_det = _Edge1->Dot( l_pvec);

	/* the non-culling branch */
	if (l_det <= -EPSILON || l_det >= EPSILON)
	{
		float l_inv_det = 1.0f / l_det;

		/* calculate distance from vert0 to ray origin */
		Vector4 l_tvec(s.x - Vtx1[0], s.y - Vtx1[1], s.z - Vtx1[2]);

		/* calculate U parameter and test bounds */
		float _u = l_tvec.Dot( l_pvec) * l_inv_det;
		if (_u >= 0.0f && _u <= 1.0f)
		{
			/* prepare to test V parameter */
			Vector4 l_qvec = l_tvec.Cross(  *_Edge1);

			/* calculate V parameter and test bounds */
			float _v = d.Dot( l_qvec) * l_inv_det;
			if (_v >= 0.0f && _u + _v <= 1.0f)
			{						
				/* calculate t, ray intersects triangle */
				l_Dist = _Edge2->Dot( l_qvec) * l_inv_det;
				u = _u;
				v = _v;
				
				if (l_det <= -EPSILON )
					_Backface = true;

				return true;
			}
		}
	}
	return false;
}

//----------------------------------------------------------------------------------------------------------------
bool GeometryKDTree::AABBIntersectRay(float* _AABB, const Vector4 &o, const Vector4 &d, float &tmin, float &tmax)
//----------------------------------------------------------------------------------------------------------------
{
	tmin = 0;
	tmax = 3.402823466e+38F;

	for	(uint n = 0; n < 3; ++n)
		if	(Math::EqualZero(d[n]))
		{
			if	((o[n] < _AABB[n*2]) || (o[n] > _AABB[n*2+1]))
				return false;
		}
		else
		{
			float	ood = 1.f / d[n];
			float	t0 = (_AABB[n*2] - o[n]) * ood;
			float	t1 = (_AABB[n*2+1] - o[n]) * ood;

			if	(t0 > t1)
			{	float swp = t1; t1 = t0; t0 = swp;	}

			tmin = tmin < t0 ? t0 : tmin;
			tmax = tmax < t1 ? tmax : t1;

			if	(tmin > tmax)
				return false;			
		}
		return true;
}

//---------------------------------------------------
int	GeometryKDTree::InsideKdTreeNode(const Vector4 &s, bool CheckInside)
//---------------------------------------------------
{
	int l_CurrentNode = 0;

	while(!m_NodeTree[l_CurrentNode].m_KDTREE_NODE_IS_LEAF)
	{
		switch (m_NodeTree[l_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT)
		{
		case KDTREE_X_AXIS:
			if(s.x > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
				l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
			else
				l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
			break;
		case KDTREE_Y_AXIS:
			if(s.y > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
				l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
			else
				l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
			break;
		case KDTREE_Z_AXIS:
			if(s.z > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
				l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
			else
				l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
			break;					
		}		
	}

	if(!CheckInside ||
	   (m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB[0] <= s.x && s.x <= m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB[1] && 
	   m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB[2] <= s.y && s.y <= m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB[3] && 
	   m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB[4] <= s.z && s.z <= m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB[5] ))
		return l_CurrentNode;
	else
		return -1;
}

//---------------------------------------------------------------------------------------------------------------
void GeometryKDTree::RaytraceGeometry(GeometryTrace &trace, const Vector4 &s, const Vector4 &d, float dist_max)
//---------------------------------------------------------------------------------------------------------------
{
	trace.has_i = false;

//	ray_count++;
	trace.tri_test = 1;

	if(m_count_bih <= 0)
		return;
 
 	float l_DistEntry, l_DistExit;
 
 	int l_CurrentNode = 0;

	// test the ray pass in this kdtree
	if(!AABBIntersectRay(m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB, s, d, l_DistEntry, l_DistExit))
			return;

	// set the max distance
	if(dist_max != -1.0f && dist_max < l_DistExit)
		l_DistExit = dist_max;

	l_DistEntry *= 0.9999f;

	int l_LastEntrySide = -1;
	Vector4 l_PointEntry;
	float l_NearestDist = 3.402823466e+38F;

 	while(l_DistEntry < l_DistExit)
 	{
 		l_PointEntry = s + d*l_DistEntry;
 
 		while(!m_NodeTree[l_CurrentNode].m_KDTREE_NODE_IS_LEAF)
 		{
 			switch (m_NodeTree[l_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT)
 			{
 			case KDTREE_X_AXIS:
				{
					float l_X = l_PointEntry.x ;
					if(l_X == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
// 					{	
// 						float l_TempDistEntry, l_TempDistExit;
// 						if(AABBIntersectRay(m_NodeTree[m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT].m_KDTREE_NODE_AABB, s, d, l_TempDistEntry, l_TempDistExit))		
// 						{
// 							float l_TempDistEntry2, l_TempDistExit2;
// 							if(AABBIntersectRay(m_NodeTree[m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT].m_KDTREE_NODE_AABB, s, d, l_TempDistEntry2, l_TempDistExit2))		
// 							{
// 								if(l_DistEntry < l_TempDistEntry2 )
// 									l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
// 								else
// 									l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
// 							}
// 							else
// 								l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
// 						}
// 						else
// 							l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
// 					}
						l_X += d.x*0.001f;
					//else
					if(l_X > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
						l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
					else
						l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
				}
 				break;
			case KDTREE_Y_AXIS:
				{
					float l_Y = l_PointEntry.y ;
					if(l_Y == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
						l_Y += d.y*0.001f;
					
					if(l_Y > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
						l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
					else
						l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
				}
 				break;
			case KDTREE_Z_AXIS:
				{
					float l_Z = l_PointEntry.z;
					if(l_Z == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
						l_Z += d.z*0.001f;
					
					if(l_Z > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
						l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
					else
						l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
				}
 				break;					
 			}
		}
 
		float l_CurrentDistEntry, l_CurrentDistExit;
		if(AABBIntersectRay(m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB, s, d, l_CurrentDistEntry, l_CurrentDistExit))		
		{
			// it's a leaf continue by check the intersect triangle
			int* l_TempPntVtx = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_VTX;
			int l_CountPoly = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_COUNT_POLY;
			for(int i=0; i< l_CountPoly; ++i)
			{
				float l_Dist = 0, u, v;
				bool l_Backface = false;
				if(IntersectTriangle(s, d, m_Vtx+(*(l_TempPntVtx))*4, m_Vtx+(*(l_TempPntVtx+1))*4, m_Vtx+(*(l_TempPntVtx+2))*4, &m_OptimizeEdgePoly[m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_POLY[i]*2], &m_OptimizeEdgePoly[m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_POLY[i]*2+1], l_Dist, u, v ,l_Backface) == 1)
				{// intersection with this triangle , change the end entry
					// the dist have to be between entry and exit
					if(l_Dist >= 0 && l_Dist < l_NearestDist && l_CurrentDistEntry <= l_Dist*1.0001f && l_Dist* 0.9999f  <= l_CurrentDistExit)
					{
						l_NearestDist = l_Dist;

						int ip = m_RealIdPoly[m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_POLY[i]];
						if(geometry->pol_normal.GetCount() > ip && geometry->pol.GetCount() > ip)
						{
							float dnv = trace.d.Dot(geometry->pol_normal[ip]);
							Material *m = material_table[geometry->pol[ip].material];

							if(!l_Backface || (dnv >= 0.f && m && m->renderword & Material::Render_DoubleSided))
							{
								l_DistExit = l_Dist;

								trace.g = geometry;
								trace.m = m;
								trace.bi = pol_index[ip];
								trace.ip = ip;

								trace.backface = false;

								trace.s = s;
								trace.d = d;

								trace.has_i = true;
								trace.i_t = l_Dist;

								trace.it = 0;

								if(geometry->pol[ip].vtx_count == 4 && ((uint)*(l_TempPntVtx+1)) == geometry->pol[ip].binding[3])
								{
									trace.u = 1-u;
									trace.v = 1-v;
								}
								else
								{
									trace.u = u;
									trace.v = v;
								}
								trace.w =  1.0f - trace.u - trace.v;

								trace.backface = l_Backface;
							}		
						}
					}			
				}
				l_TempPntVtx +=3;
			}
		}

		// found a hit so your job is done here ! for now...
		// wrong idea, if a big triangle is in a node before another little triangle at the good place but in a cell later.
		if(trace.has_i)
			return;

		l_DistEntry = l_CurrentDistExit*1.0000001f;

	//	l_CurrentNode = 0;

  		// get the rope
  		l_PointEntry = s + d*l_CurrentDistExit;
  		int l_SideChoosen = GetKDtreeSideAABB(m_NodeTree[l_CurrentNode].m_KDTREE_NODE_AABB, l_PointEntry, l_LastEntrySide);
  
 		if(l_SideChoosen == -1)	// if no node, no intersection
 			return;
 		else
 			l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_ROPE[l_SideChoosen];
 
 		// if no rope
 		if(l_CurrentNode < 0)
 			return;
	}	
}

//------------------------------------------------------------------
void GeometryKDTree::CreateRope(int &_CurrentNode, int *_RopeArray)
//------------------------------------------------------------------
{
 	if(m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF )
 	{
 		// finish the depth

		int * l_TempRope = m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_ROPE;

		memcpy(l_TempRope, _RopeArray, sizeof(int)*6);
 	}
 	else
 	{
		// copy the aabb to the child and change copy  the right id into the left aabb and inverse
		float* l_AABB = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB;

 		// do an optimization
		for (int i=0; i<6; ++i)
		{
			if(_RopeArray[i] != -1)
			{
				while(!m_NodeTree[_RopeArray[i]].m_KDTREE_NODE_IS_LEAF)
				{					
					char l_TempTypeSplitAxisRope = m_NodeTree[_RopeArray[i]].m_KDTREE_NODE_TYPE_SPLIT;

					// if the type axis is the same as the current node
					if((int)(i*0.5f) == l_TempTypeSplitAxisRope)
					{
						// give the id node depend of the axis, if the rope is from the right, stay on the right
						if(i%2)
							_RopeArray[i] = m_NodeTree[_RopeArray[i]].m_KDTREE_NODE_ID_CHILD_LEFT;
						else
							_RopeArray[i] = m_NodeTree[_RopeArray[i]].m_KDTREE_NODE_ID_CHILD_RIGHT;
					}
					else
					{
						// check the nearest rope to this plane, and stop if there is 2 nearest plane
						float l_ValueSplitRope = m_NodeTree[_RopeArray[i]].m_KDTREE_NODE_VALUE_SPLIT;
						if(l_AABB[l_TempTypeSplitAxisRope*2] > l_ValueSplitRope)
							_RopeArray[i] = m_NodeTree[_RopeArray[i]].m_KDTREE_NODE_ID_CHILD_RIGHT;
						else
							if(l_AABB[l_TempTypeSplitAxisRope*2+1] < l_ValueSplitRope)
								_RopeArray[i] = m_NodeTree[_RopeArray[i]].m_KDTREE_NODE_ID_CHILD_LEFT;
							else
								break;	//the split axis is just in the middle of the aabb so don't clip more
					}
				}
			}
		}

		char l_TempTypeSplitAxis = m_NodeTree[_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT;

		int l_IdChildLeft = m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;	
		int l_IdChildRight = m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;	

		int * l_TempLeftChildRope = m_NodeTree[l_IdChildLeft].m_KDTREE_NODE_ID_ROPE;
		int * l_TempRightChildRope = m_NodeTree[l_IdChildRight].m_KDTREE_NODE_ID_ROPE;

		memcpy(l_TempLeftChildRope, _RopeArray, sizeof(int)*6);
		memcpy(l_TempRightChildRope, _RopeArray, sizeof(int)*6);
 
 		l_TempLeftChildRope[l_TempTypeSplitAxis*2+1] = l_IdChildRight;
 		l_TempRightChildRope[l_TempTypeSplitAxis*2] = l_IdChildLeft;
 
 		CreateRope(l_IdChildLeft, l_TempLeftChildRope);
 		CreateRope(l_IdChildRight, l_TempRightChildRope);
 	}
}

//-------------------------------------------------------------------
void GeometryKDTree::IncreaseSizeNodeKdtreeBuffer(int _IncreaseSize)
//-------------------------------------------------------------------
{
	KDTreeNode* l_tempCopy = new KDTreeNode[m_SizeTree + _IncreaseSize];
	memcpy(l_tempCopy, m_NodeTree, sizeof(KDTreeNode)*m_SizeTree);

	for(int i=0; i<m_SizeTree; ++i)
	{
		if(m_NodeTree[i].m_KDTREE_NODE_ID_POLY)
		{
			l_tempCopy[i].m_KDTREE_NODE_ID_POLY = new int[m_NodeTree[i].m_KDTREE_NODE_COUNT_POLY];
			memcpy( l_tempCopy[i].m_KDTREE_NODE_ID_POLY, m_NodeTree[i].m_KDTREE_NODE_ID_POLY, sizeof(int)*m_NodeTree[i].m_KDTREE_NODE_COUNT_POLY);

			l_tempCopy[i].m_KDTREE_NODE_ID_VTX = new int[m_NodeTree[i].m_KDTREE_NODE_COUNT_POLY*3];		
			memcpy( l_tempCopy[i].m_KDTREE_NODE_ID_VTX, m_NodeTree[i].m_KDTREE_NODE_ID_VTX, sizeof(int)*m_NodeTree[i].m_KDTREE_NODE_COUNT_POLY*3);
		}
	}

	m_SizeTree += _IncreaseSize;

	delete []m_NodeTree;
	m_NodeTree = l_tempCopy;
}

//----------------------------------------------------------------------------------------------------------------------------------------------------------------
void GeometryKDTree::CreateNodeKdtree(int &_CurrentNode, int *_IdPoly, int *_IdVtx, int _CountPoly, float *_Vtx, int _CountVtx, int _CurrentDepth)
//----------------------------------------------------------------------------------------------------------------------------------------------------------------
{
	// check if there is a minimum of place for 2 child 
	if(m_SizeTree < _CurrentNode + 3)
	{
		IncreaseSizeNodeKdtreeBuffer(1000);
	}

	// set the id of the node
	m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID = _CurrentNode;

	//get the aabb
	float * l_TempAABB =  m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB;

	// check if  it's the moment to create the leaf
	if(_CountPoly < KDTREE_MAX_POLY_PER_NODE || _CurrentDepth >= KDTREE_MAX_DEPTH
		|| fabs(l_TempAABB[KDTREE_SIDE_LEFT] - l_TempAABB[KDTREE_SIDE_RIGHT]) < 0.1f
		|| fabs(l_TempAABB[KDTREE_SIDE_BOTTOM] - l_TempAABB[KDTREE_SIDE_TOP]) < 0.1f
		|| fabs(l_TempAABB[KDTREE_SIDE_BACK] - l_TempAABB[KDTREE_SIDE_FRONT]) < 0.1f)
	{
		// check if there is a minimum of place for all the poly
		if(m_SizeTree < (_CurrentNode + _CountPoly*3 + _CountPoly))
		{
			IncreaseSizeNodeKdtreeBuffer(10000 + _CountPoly*3 + _CountPoly);
		}

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF = true;

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT = -1;	
		m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT = -1;	

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_COUNT_POLY = _CountPoly;

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_POLY = new int[_CountPoly];
		memcpy( m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_POLY, _IdPoly, sizeof(int)*_CountPoly);

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_VTX = new int[_CountPoly*3];		
		memcpy( m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_VTX, _IdVtx, sizeof(int)*_CountPoly*3);

		// set the new id to set back
		++_CurrentNode;
	}
	else
	{
		m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF  = false;

		float l_TempValueSplit = 0.0f;
		char l_TempTypeSplitAxis = m_NodeTree[_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT;

		int l_CountAveragePoly = 0; 
		int *l_TempIdPnt = _IdVtx;

		float l_ExtremeRight = -3.402823466e+38F;
		float l_ExtremeLeft = 3.402823466e+38F;
		for(int i=0; i<_CountPoly; ++i)
		{
			if(l_ExtremeRight < _Vtx[(*l_TempIdPnt)*4+l_TempTypeSplitAxis])
				l_ExtremeRight = _Vtx[(*l_TempIdPnt)*4+l_TempTypeSplitAxis];
			if(l_ExtremeRight < _Vtx[(*(l_TempIdPnt+1))*4+l_TempTypeSplitAxis])
				l_ExtremeRight = _Vtx[(*(l_TempIdPnt+1))*4+l_TempTypeSplitAxis];
			if(l_ExtremeRight < _Vtx[(*(l_TempIdPnt+2))*4+l_TempTypeSplitAxis])
				l_ExtremeRight = _Vtx[(*(l_TempIdPnt+2))*4+l_TempTypeSplitAxis];

			if(l_ExtremeLeft > _Vtx[(*l_TempIdPnt)*4+l_TempTypeSplitAxis])
				l_ExtremeLeft = _Vtx[(*l_TempIdPnt)*4+l_TempTypeSplitAxis];
			if(l_ExtremeLeft > _Vtx[(*(l_TempIdPnt+1))*4+l_TempTypeSplitAxis])
				l_ExtremeLeft = _Vtx[(*(l_TempIdPnt+1))*4+l_TempTypeSplitAxis];
			if(l_ExtremeLeft > _Vtx[(*(l_TempIdPnt+2))*4+l_TempTypeSplitAxis])
				l_ExtremeLeft = _Vtx[(*(l_TempIdPnt+2))*4+l_TempTypeSplitAxis];

			l_TempValueSplit += (	_Vtx[(*l_TempIdPnt)*4+l_TempTypeSplitAxis] +
									_Vtx[(*(l_TempIdPnt+1))*4+l_TempTypeSplitAxis] +
									_Vtx[(*(l_TempIdPnt+2))*4+l_TempTypeSplitAxis])
									;
			++l_CountAveragePoly;

			l_TempIdPnt += 3;
		}

		if(l_CountAveragePoly)
			l_TempValueSplit /= (l_CountAveragePoly*3.0f);

		// check if the extreme is better split
		float l_MiddleAABB = (l_TempAABB[l_TempTypeSplitAxis*2] + l_TempAABB[l_TempTypeSplitAxis*2+1])*0.5f;

 		if(l_ExtremeRight < l_MiddleAABB && l_ExtremeRight > l_TempAABB[l_TempTypeSplitAxis*2])
 			l_TempValueSplit = l_ExtremeRight;
		else
 		if(l_ExtremeLeft > l_MiddleAABB && l_ExtremeLeft < l_TempAABB[l_TempTypeSplitAxis*2+1])
 			l_TempValueSplit = l_ExtremeLeft;
		
 		if(l_TempValueSplit <= l_TempAABB[l_TempTypeSplitAxis*2] ||
 			l_TempValueSplit >= l_TempAABB[l_TempTypeSplitAxis*2+1])
			l_TempValueSplit = l_MiddleAABB;

// 		if(l_TempValueSplit < _CurrentNode->m_AABB[_CurrentNode->m_TypeSplitAxis*2] || l_TempValueSplit > _CurrentNode->m_AABB[_CurrentNode->m_TypeSplitAxis*2+1] )
// 			int yo = 0;

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT = l_TempValueSplit;
		
		// create the 2 childs
		//set new axis
		int l_NewAxis = l_TempTypeSplitAxis+1;
		if(l_NewAxis >= 3)
			l_NewAxis = 0;

		// create the 2 child list
		int l_IdInBigArray;

		// left node
		{
			int l_NewIdChildLeft = _CurrentNode + 1;
			m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT = l_NewIdChildLeft;			

			m_NodeTree[l_NewIdChildLeft].m_KDTREE_NODE_TYPE_SPLIT = (char)(l_NewAxis);

			// set the new aabb
			float * l_TempAABBLeftChild = m_NodeTree[l_NewIdChildLeft].m_KDTREE_NODE_AABB;
		
			l_TempAABB =  m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB;
			memcpy(l_TempAABBLeftChild, l_TempAABB, sizeof(float)*6);
			l_TempAABBLeftChild[l_TempTypeSplitAxis*2+1] = l_TempValueSplit;

			int *l_IdLeftList = new int [_CountPoly*3];
			int *l_IdLeftPolyList = new int [_CountPoly];
			int l_IdLeftCount = 0;

			float l_LeftBoxCenter[3];
			l_LeftBoxCenter[0] = (l_TempAABBLeftChild[KDTREE_SIDE_RIGHT] + l_TempAABBLeftChild[KDTREE_SIDE_LEFT])*0.5f;
			l_LeftBoxCenter[1] = (l_TempAABBLeftChild[KDTREE_SIDE_TOP] + l_TempAABBLeftChild[KDTREE_SIDE_BOTTOM])*0.5f;
			l_LeftBoxCenter[2] = (l_TempAABBLeftChild[KDTREE_SIDE_FRONT] + l_TempAABBLeftChild[KDTREE_SIDE_BACK])*0.5f;

			float l_LeftHalfSize[3];
			l_LeftHalfSize[0] = fabs(l_TempAABBLeftChild[KDTREE_SIDE_RIGHT] - l_LeftBoxCenter[0])*1.001f;
			l_LeftHalfSize[1] = fabs(l_TempAABBLeftChild[KDTREE_SIDE_TOP] - l_LeftBoxCenter[1])*1.001f;
			l_LeftHalfSize[2] = fabs(l_TempAABBLeftChild[KDTREE_SIDE_FRONT] - l_LeftBoxCenter[2])*1.001f;

			l_TempIdPnt = _IdVtx;
			for(int i=0; i<_CountPoly; ++i)
			{
				if(triBoxOverlap(l_LeftBoxCenter, l_LeftHalfSize,	_Vtx+(*l_TempIdPnt)*4,
																	_Vtx+(*(l_TempIdPnt+1))*4,
																	_Vtx+(*(l_TempIdPnt+2))*4))				
				{
					l_IdLeftList[l_IdLeftCount*3] = (*l_TempIdPnt);
					l_IdLeftList[l_IdLeftCount*3+1] = *(l_TempIdPnt+1);
					l_IdLeftList[l_IdLeftCount*3+2] = *(l_TempIdPnt+2);

					l_IdLeftPolyList[l_IdLeftCount] = _IdPoly[i];

					++l_IdLeftCount;
				}

				l_TempIdPnt += 3;
			}

			// copy the strict minimum, not good, because it's fragment memory, but it's just for the creation
			{
				int* l_tempCopy = new int[l_IdLeftCount*3];
				memcpy(l_tempCopy, l_IdLeftList, sizeof(int)*l_IdLeftCount*3);

				delete []l_IdLeftList;
				l_IdLeftList = l_tempCopy;
			}
			{
				int* l_tempCopy = new int[l_IdLeftCount];
				memcpy(l_tempCopy, l_IdLeftPolyList, sizeof(int)*l_IdLeftCount);

				delete []l_IdLeftPolyList;
				l_IdLeftPolyList = l_tempCopy;
			}

			CreateNodeKdtree(l_NewIdChildLeft, l_IdLeftPolyList, l_IdLeftList, l_IdLeftCount, _Vtx, _CountVtx, _CurrentDepth+1);

			l_IdInBigArray = l_NewIdChildLeft;

			delete []l_IdLeftList;
			delete []l_IdLeftPolyList;
		}

		// right node
		{
			if(m_SizeTree < l_IdInBigArray + 3)
				IncreaseSizeNodeKdtreeBuffer(10000);

			int l_NewIdChildRight = l_IdInBigArray;
			m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT = l_NewIdChildRight;			

			m_NodeTree[l_NewIdChildRight].m_KDTREE_NODE_TYPE_SPLIT = (char)(l_NewAxis);

			// set the new aabb
			float * l_TempAABBRightChild = m_NodeTree[l_NewIdChildRight].m_KDTREE_NODE_AABB;
			
			l_TempAABB =  m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB;
			memcpy(l_TempAABBRightChild, l_TempAABB, sizeof(float)*6);
			l_TempAABBRightChild[l_TempTypeSplitAxis*2] = l_TempValueSplit;

			int *l_IdRightList = new int [_CountPoly*3];
			int *l_IdRightPolyList = new int [_CountPoly];
			int l_IdRightCount = 0;

			float l_RightBoxCenter[3];
			l_RightBoxCenter[0] = (l_TempAABBRightChild[KDTREE_SIDE_RIGHT] + l_TempAABBRightChild[KDTREE_SIDE_LEFT])*0.5f;
			l_RightBoxCenter[1] = (l_TempAABBRightChild[KDTREE_SIDE_TOP] + l_TempAABBRightChild[KDTREE_SIDE_BOTTOM])*0.5f;
			l_RightBoxCenter[2] = (l_TempAABBRightChild[KDTREE_SIDE_FRONT] + l_TempAABBRightChild[KDTREE_SIDE_BACK])*0.5f;

			float l_RightHalfSize[3];
			l_RightHalfSize[0] = fabs(l_TempAABBRightChild[KDTREE_SIDE_RIGHT] - l_RightBoxCenter[0])*1.001f;
			l_RightHalfSize[1] = fabs(l_TempAABBRightChild[KDTREE_SIDE_TOP] - l_RightBoxCenter[1])*1.001f;
			l_RightHalfSize[2] = fabs(l_TempAABBRightChild[KDTREE_SIDE_FRONT] - l_RightBoxCenter[2])*1.001f;

			l_TempIdPnt = _IdVtx;
			for(int i=0; i<_CountPoly; ++i)
			{
				if(triBoxOverlap(l_RightBoxCenter, l_RightHalfSize,_Vtx +(*l_TempIdPnt)*4,
																	_Vtx+(*(l_TempIdPnt+1))*4,
																	_Vtx+(*(l_TempIdPnt+2))*4))	
				{
					l_IdRightList[l_IdRightCount*3] = (*l_TempIdPnt);
					l_IdRightList[l_IdRightCount*3+1] = *(l_TempIdPnt+1);
					l_IdRightList[l_IdRightCount*3+2] = *(l_TempIdPnt+2);

					l_IdRightPolyList[l_IdRightCount] = _IdPoly[i];

					++l_IdRightCount;
				}

				l_TempIdPnt += 3;
			}

			// copy the strict minimum, not good, because it's fragment memory, but it's just for the creation
			{
				int* l_tempCopy = new int[l_IdRightCount*3];
				memcpy(l_tempCopy, l_IdRightList, sizeof(int)*l_IdRightCount*3);

				delete []l_IdRightList;
				l_IdRightList = l_tempCopy;
			}
			{
				int* l_tempCopy = new int[l_IdRightCount];
				memcpy(l_tempCopy, l_IdRightPolyList, sizeof(int)*l_IdRightCount);

				delete []l_IdRightPolyList;
				l_IdRightPolyList = l_tempCopy;
			}

			CreateNodeKdtree(l_NewIdChildRight, l_IdRightPolyList, l_IdRightList, l_IdRightCount, _Vtx, _CountVtx, _CurrentDepth+1);

			//set the new id for the next node in the stack
			_CurrentNode = l_NewIdChildRight;

			delete []l_IdRightList;
			delete []l_IdRightPolyList;
		}
	}
}

//--------------------------------------------------------------------------------
bool GeometryKDTree::BuildFromGeometry(ResourceFactory &gf, Geometry *g)
//--------------------------------------------------------------------------------
{
	Free();

	__LOG__ << "Building KD-Tree...\n";
	Benchmark build_bench(true);

	geometry = g;
	geometry->ComputePolygonIndex(pol_index);

	int l_CountVtx = 0;
	int *l_IdVtx = NULL;
	m_Vtx = NULL;

	if	(!g->material_table.GetCount() || !g->pol.GetCount())
		return false;

	material_table.Allocate(g->material_table.GetCount());
	for	(uint n = 0; n < material_table.GetCount(); ++n)
		material_table[n] = gf.LoadMaterial(g->material_table[n].name);

	// create the optimize edge for the intersect ray
	m_OptimizeEdgePoly = new Vector4[g->pol.GetCount() * 2*2];

	// to keep the real id of the poly, in case of no triangle poly
	m_RealIdPoly = new int[g->pol.GetCount()*2];

	// Transfer topology.
	m_count_bih = 0;
	l_IdVtx = new int[g->pol.GetCount() * 3*2];
	int	*l_TPointer = l_IdVtx;
	for	(uint n = 0; n < g->pol.GetCount(); ++n)
	{
		//nVector l_edge1(Vtx2[0] - Vtx1[0], Vtx2[1] - Vtx1[1], Vtx2[2] - Vtx1[2]);
		//nVector l_edge2(Vtx3[0] - Vtx1[0], Vtx3[1] - Vtx1[1], Vtx3[2] - Vtx1[2]);

		Polygon	&pol = g->pol[n];
		if(pol.vtx_count == 3)
		{
			m_RealIdPoly[m_count_bih] = n;

			l_TPointer[m_count_bih*3] = pol.binding[0];
			l_TPointer[m_count_bih*3+1] = pol.binding[1];
			l_TPointer[m_count_bih*3+2] = pol.binding[2];

			m_OptimizeEdgePoly[m_count_bih*2] = g->vtx[pol.binding[1]] - g->vtx[pol.binding[0]];
			m_OptimizeEdgePoly[m_count_bih*2+1] = g->vtx[pol.binding[2]] - g->vtx[pol.binding[0]];

			++m_count_bih;
		}
		else
		if(pol.vtx_count == 4)
		{
			m_RealIdPoly[m_count_bih] = n;
			l_TPointer[m_count_bih*3] = pol.binding[0];
			l_TPointer[m_count_bih*3+1] = pol.binding[1];
			l_TPointer[m_count_bih*3+2] = pol.binding[2];

			m_OptimizeEdgePoly[m_count_bih*2] = g->vtx[pol.binding[1]] - g->vtx[pol.binding[0]];
			m_OptimizeEdgePoly[m_count_bih*2+1] = g->vtx[pol.binding[2]] - g->vtx[pol.binding[0]];

			++m_count_bih;

			m_RealIdPoly[m_count_bih] = n;
			l_TPointer[m_count_bih*3] = pol.binding[2];
			l_TPointer[m_count_bih*3+1] = pol.binding[3];
			l_TPointer[m_count_bih*3+2] = pol.binding[0];

			m_OptimizeEdgePoly[m_count_bih*2] = g->vtx[pol.binding[3]] - g->vtx[pol.binding[2]];
			m_OptimizeEdgePoly[m_count_bih*2+1] = g->vtx[pol.binding[0]] - g->vtx[pol.binding[2]];

			++m_count_bih;
		}
	}

	// keep the good amount of memory
	{
		int* l_TempIdVtx = new int[m_count_bih * 3];
		memcpy(l_TempIdVtx, l_IdVtx, sizeof(int)*m_count_bih * 3);
		delete []l_IdVtx;
		l_IdVtx = l_TempIdVtx;
	}

	{
		Vector4* l_TempOptimizeEdgePoly = new Vector4[m_count_bih*2];
		memcpy(l_TempOptimizeEdgePoly, m_OptimizeEdgePoly, sizeof(Vector4)*m_count_bih * 2);
		delete []m_OptimizeEdgePoly;
		m_OptimizeEdgePoly = l_TempOptimizeEdgePoly;
	}
	{
		int* l_TempRealIdPoly = new int[m_count_bih];
		memcpy(l_TempRealIdPoly, m_RealIdPoly, sizeof(int)*m_count_bih);
		delete []m_RealIdPoly;
		m_RealIdPoly = l_TempRealIdPoly;
	}

	// Transfer vertice.
	m_Vtx = new float[g->vtx.GetCount()*4];
	for	(uint n = 0; n < g->vtx.GetCount(); ++n)
	{
		Vector4		l_Temp2Vtx = g->vtx[n];
		m_Vtx[n*4] = l_Temp2Vtx.x;
		m_Vtx[n*4+1] = l_Temp2Vtx.y;
		m_Vtx[n*4+2] = l_Temp2Vtx.z;
		m_Vtx[n*4+3] = l_Temp2Vtx.w;
	}

	l_CountVtx = g->vtx.GetCount();

	//very not powerful kdtree construction

	m_SizeTree = m_count_bih*4;
	m_NodeTree = new KDTreeNode[m_SizeTree];

	int m_CurrentNode = 0;

	m_NodeTree[m_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT = KDTREE_X_AXIS;

	float * l_TempAABB = m_NodeTree[m_CurrentNode].m_KDTREE_NODE_AABB;

	l_TempAABB[KDTREE_SIDE_LEFT] = 3.402823466e+38F;
	l_TempAABB[KDTREE_SIDE_BOTTOM] = 3.402823466e+38F;
	l_TempAABB[KDTREE_SIDE_BACK] = 3.402823466e+38F;
	l_TempAABB[KDTREE_SIDE_RIGHT] = -3.402823466e+38F;
	l_TempAABB[KDTREE_SIDE_TOP] = -3.402823466e+38F;
	l_TempAABB[KDTREE_SIDE_FRONT] = -3.402823466e+38F;

	float* l_TempVtx = m_Vtx;
	for(int i=0; i<l_CountVtx; ++i)
	{
		if((*l_TempVtx) < l_TempAABB[KDTREE_SIDE_LEFT])
			l_TempAABB[KDTREE_SIDE_LEFT] = (*l_TempVtx);
		if((*l_TempVtx) > l_TempAABB[KDTREE_SIDE_RIGHT])
			l_TempAABB[KDTREE_SIDE_RIGHT] = (*l_TempVtx);

		++l_TempVtx;
		if((*l_TempVtx) < l_TempAABB[KDTREE_SIDE_BOTTOM])
			l_TempAABB[KDTREE_SIDE_BOTTOM] = (*l_TempVtx);
		if((*l_TempVtx) > l_TempAABB[KDTREE_SIDE_TOP])
			l_TempAABB[KDTREE_SIDE_TOP] = (*l_TempVtx);

		++l_TempVtx;
		if((*l_TempVtx) < l_TempAABB[KDTREE_SIDE_BACK])
			l_TempAABB[KDTREE_SIDE_BACK] = (*l_TempVtx);
		if((*l_TempVtx) >l_TempAABB[KDTREE_SIDE_FRONT])
			l_TempAABB[KDTREE_SIDE_FRONT] = (*l_TempVtx);

		++l_TempVtx;
		++l_TempVtx;
	}


	// to build the kdtree: id of the poly
	int* l_IdPoly = new int[m_count_bih];
	for(int i=0; i<m_count_bih; ++i)
		l_IdPoly[i] = i;

	CreateNodeKdtree(m_CurrentNode, l_IdPoly, l_IdVtx, m_count_bih, m_Vtx, l_CountVtx, 0);

	int l_RopeArray[6];
	memset(l_RopeArray, -1, sizeof(int)*6);

	// Create the rope to have quick access
	m_CurrentNode = 0;
	CreateRope(m_CurrentNode, l_RopeArray);

	_safe_delete_array(l_IdVtx);
	_safe_delete_array(l_IdPoly);		

	build_bench.Stop();
	__LOG__ << "Done in " << build_bench.GetMs() << "ms.\n";
	return true;
}

//-----------------------------------------
void				GeometryKDTree::Free()
//-----------------------------------------
{
	geometry = NULL;
	_safe_delete_array(m_NodeTree);
	_safe_delete_array(m_Vtx);
	_safe_delete_array(m_OptimizeEdgePoly);
	_safe_delete_array(m_RealIdPoly);
	m_count_bih = 0;
}

GeometryKDTree::GeometryKDTree()
{
	geometry = NULL;
	m_NodeTree = NULL;
	m_Vtx = NULL;
	m_OptimizeEdgePoly = NULL;
	m_count_bih = 0;
	m_RealIdPoly = NULL;
}